About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Overview

This package contains the ROS interface for the sbpl_arm_planner package.

How to Use It

This package is intended to be used as the planner's interface to the arm_navigation stack. If you already know how to make planning requests to move_arm, then you are almost ready to go.

To request motion plans of the sbpl_arm_planner instead of your current planner you need to:

• Launch the proper launch file for the arm you intend to use. The launch folder contains two launch files that you can choose from to use with the PR2:
  • launch/sbpl_planning_left_arm.launch
  • launch/sbpl_planning_right_arm.launch

• In your MoveArmGoal request message to move_arm,there are two changes that must be made:

   // don't run IK - send pose constraint as-is to the planner
   my_msg.disable_ik = true;
  
   // sbpl planner service name
   my_msg.planner_service_name = std::string("sbpl_planning/plan_path");

If you don't already know how to send planning requests to move_arm, I suggest you check out the useful arm navigation tutorials.

ROS API

Subscribed Topics

collision_map_occ (mapping_msgs/CollisionMap)

• A 3D occupancy grid of the surrounding environment

collision_object (mapping_msgs/CollisionObject)

• Objects in the environment that are known. They get added to the collision map.

attached_collision_object (mapping_msgs/AttachedCollisionObject)

• Objects that were added to the robot model - usually objects that the robot grasped. They get added to the robot's collision model.

Published Topics

visualization_marker (visualization_msgs/Marker)

• The goal pose and if enabled, the pose(position and orientation) of each node in the graph expanded during the search.

visualization_marker_array (visualization_msgs/MarkerArray)

• The position of each node in the graph expanded during the search. If enabled, the path suggested by the heuristic function from the start position to the goal can be displayed.

Services Offered

/sbpl_planning/plan_path (motion_planning_msgs/GetMotionPlan)

• This service accepts planning requests and returns trajectories. Note: In its current form the planner will only attempt to plan a path for a goal position constraint. Direct requests to the planner with pose constraints will be handled correctly but pose constraint requests to move_arm need to have the 'disable_ik' flag set to true to be forwarded to the planner service correctly.

ROS Parameters

Planner

General

~/planner/search_mode (bool, default: true)

• Return first solution found.

~/planner/epsilon (double, default: 100.0)

• The initial epsilon to be used by the planner. Epsilon is the bounds on the suboptimality of the solution.

~/planner/verbose (bool, default: false)

• If true, a lot of descriptive information is printed out before the search that describes the arm configuration, motion primitives, planner settings. A tolerable amount of data is printed out during the search.

~/planner/use_dijkstra_heuristic (bool, default: true)

• For the heuristic function, use the 3D distance to the goal calculated by Dijkstra's algorithm. If set to false, the Euclidean distance is used.

~/planner/use_research_heuristic (bool, default: true)

• For the heuristic function, use the 3D distance from the elbow position to any of the possible elbow poses when the end effector is at the goal pose.

~/planner/use_uniform_obstacle_cost (bool, default: false)

• If true, it doesn't include a "distance to the nearest obstacle" cost in the cost function. If false, it tries to stay away from obstacles (may take longer to plan).

~/planner/use_multiresolution_motion_primitives (bool, default: true)

• If true, use one set of primitives when at least n meters from the goal, and a different set (usually with smaller motions) when closer to the goal. If false, use the same for the entire search.

~/planner/arm_description_file (string, default: )

• Path to the arm description text file.

~/planner/motion_primitive_file (string, default: )

• Path to the motion primitive text file.

Research

~/planner/research/solve_with_ik_threshold (double, default: 0.10)

• The distance (meters) from the goal to start trying to solve for the goal pose using IK. Note: IK is pretty fast to compute but checking for collisions along the interpolated path to the goal from the current node's configuration takes time and so IK should not be called too far from the goal in a cluttered environment.

~/planner/research/sum_heuristics (bool, default: false)

• If true, sum the end effector heuristic and elbow heuristic. If false, then take the maximum between the two.

~/planner/research/short_distance_mprims_threshold (double, default: 0.2)

• The distance in meters from the goal pose, when the search should switch to the short distance motion primitives.

Debug

~/debug/print_out_path (bool, default: true)

• Print the path to the terminal.

Robot Model

~robot_description (string, default: robot_description)

• The name of the ROS parameter that contains the URDF robot description of the robot.

~/robot/num_joints (int, default: 7)

• The number of joints that are being planned for.

~/robot/arm_name (string, default: right_arm)

• The name of the collision link group in the URDF that will be used for kinematic calculations and collision checking.

~planning_frame (string, default: base_link)

• The name of the reference frame used during planning. Note: At the moment, the collision map must be received in this frame as well.

~fk_service_name (string, default: pr2_right_arm_kinematics/get_fk)

• The name of the forward kinematics service.

~ik_service_name (string, default: pr2_right_arm_kinematics/get_ik)

• The name of the inverse kinematics service.

Collision Space

~/collision_space/reference_frame (string, default: base_link)

• The reference frame used for the collision map.

~/collision_space/resolution (double, default: 0.02)

• The resolution of the occupancy grid.

~/collision_space/collision_map_topic (string, default: collision_map_occ)

• The topic of type mapping_msgs/CollisionMap, that the collision_map is being published on.

~/collision_space/occupancy_grid/origin_x (double, default: -0.6)

• x-coordinate of the origin of occupancy grid (cell 0,0,0) in meters.

~/collision_space/occupancy_grid/origin_y (double, default: -1.25)

• y-coordinate of the origin of occupancy grid (cell 0,0,0) in meters.

~/collision_space/occupancy_grid/origin_z (double, default: -0.05)

• z-coordinate of the origin of occupancy grid (cell 0,0,0) in meters.

~/collision_space/occupancy_grid/size_x (double, default: 1.6)

• size in meters of the occupancy grid in the x dimension.

~/collision_space/occupancy_grid/size_y (double, default: 1.6)

• size in meters of the occupancy grid in the y dimension.

~/collision_space/occupancy_grid/size_z (double, default: 1.4)

• size in meters of the occupancy grid in the z dimension.

Visualizations

~/visualizations/heuristic (bool, default: false)

• Visualize the suggested 3D path from the start position to the goal pose in rviz.

~/visualizations/goal (bool, default: true)

• Visualize the goal pose as a sphere with an arrow.

~/visualizations/expanded_states (bool, default: false)

• Visualize the poses of the states expanded during the search in rviz.

~/visualizations/trajectory (bool, default: false)

• Visualize the found trajectory (straight from the planner).

~/visualizations/trajectory_throttle (double, default: 5)

• Throttle the number of waypoints visualized (1: don't throttle).

~/visualizations/collision_model_trajectory (bool, default: false)

• Visualize the spherical collision model of the arm along the trajectory.

API Stability

• ROS API is UNREVIEWED and UNSTABLE
• C++ API is UNREVIEWED and UNSTABLE

Shortcomings

• This planner has been developed for the PR2 only. Some changes would have to be made to make it work for another robot.
• This planner cannot currently deal with path constraints.
• This planner cannot currently deal with goal joint constraints (our main focus is in planning to goal pose constraints and letting the cost function decide on the goal arm configuration).
• This planner cannot deal with goal region constraints besides a cube.
• Until the grasping pipeline can handle pose constraints without using IK - this planner cannot be used with the grasping pipeline.

Troubleshooting

The planner keeps failing what is going wrong?

• The planner tries to come up with the best possible solution within the allotted planning time. You should try raising the "allowed_planning_time" field in the MoveArmGoal message.

    my_msg.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

• The planner uses a cylindrical collision model of the arm. If your goal is set too close to an obstacle, the planner may not be able to find a solution that's collision free.
• The planner may be struggling to compute a path to the exact desired pose constraint that you set. Try increasing the tolerance on the position constraint to at least 1cm in all dimensions.