Overview

This package contains a new search-based motion planning algorithm for manipulation.

A detailed explanation of how the planner works can be found in the following papers.

Benjamin Cohen, Gokul Subramanian, Sachin Chitta, and Maxim Likhachev, "Planning for Manipulation with Adaptive Motion Primitives, " Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2011.

Benjamin Cohen, Sachin Chitta, and Maxim Likhachev, "Search-based Planning for Manipulation with Motion Primitives, " Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2010.

One of the notable differences between this planner and the other popular planning algorithms for manipulation is that the sbpl_arm_planner plans to a desired goal pose constraint without solving for an IK solution first. The joint configuration at the desired goal pose will be determined by the cost function used by the planner.

How to Use It

This planner is intended to be used with the sbpl_arm_planner_node interface.

Videos

The following video shows the PR2 manipulating an 80cm stick through a window that is 86cm wide.

The following video is somewhat repetitive. It's intended to show you that the planner is robust in cluttered environments. First the PR2 plans a couple of paths to navigate the gripper between shelves of the cabinet. Then, The PR2 navigates the gripper above and below a tabletop with obstacles on it.

ROS API

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 robot agnostic.
• This planner cannot deal with path constraints.
• This planner cannot deal with goal joint constraints (our main focus is in planning to goal pose constraints).
• 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.

ROS Topics

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.

Motion Primitives

The following text file format is how the motion primitives are inputted into the planner. In the future, we plan on moving to a yaml format so that we can put the primitives on the rosparam server.

Motion_Primitives(degrees): {# of prims} {# of angles per prim} {# of short distance prims}
x x x x x x x
...

The default primitive file can be found in config/pr2.mprim and looks like this:

Motion_Primitives(degrees): 8 7 4
4 0 0 0 0 0 0
0 4 0 0 0 0 0
0 0 4 0 0 0 0
0 0 0 4 0 0 0
2 0 0 0 0 0 0
0 2 0 0 0 0 0
0 0 2 0 0 0 0
0 0 0 2 0 0 0

Roadmap

See the stack roadmap for more details.