pr2_arm_motion_planners: sbpl_arm_planner
Package Summary
- Author: Benjamin Cohen/bcohen@seas.upenn.edu
- License: BSD
- Repository: wg-ros-pkg (https://code.ros.org/svn/wg-ros-pkg)
Overview
This package contains a new search-based motion planning algorithm for robotic manipulators. The algorithm will appear in the upcoming conference proceedings of ICRA 2010. It is titled "Search-based Planning for Manipulation with Motion Primitives" by Benjamin Cohen, Sachin Chitta and Maxim Likhachev.
In addition to the actual planner, this package implements the motion planner interface that the move_arm package requires. The planner node provides a service called '/sbpl_planning/plan_path' that accepts the GetMotionPlan request message.
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.
For tutorials on the arm navigation stack, please check out the arm navigation tutorials. To use the SBPL arm planner, simply launch the pr2_arm_navigation_planning/launch/sbpl_planning_xxxx_arm.launch file instead of any of the other planners.
ROS API
API Stability
- ROS API is UNREVIEWED and UNSTABLE
- C++ API is UNREVIEWED and UNSTABLE
Shortcomings
- 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.
- This planner has currently been configured and tested only for the PR2 right arm. Testing and release for other robots is on the roadmap.
ROS Topics
Subscribed Topics
collision_map_occ (mapping_msgs/CollisionMap)
- A 3D occupancy grid of the surrounding environment
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.
- 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_name (string, default: ARA)
- The type of A* type planner to search the graph with. (options: 'ARA','RStar')
- Despite the value of the epsilon (bounds on the sub-optimality of the solution), return the first solution found. If set to false, it will keep decreasing the epsilon to possibly improve the path until the allowed time runs out.
- 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.
- 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.
- Print the path to the terminal.
R* Planner
~number_random_successors (integer, default: 10)
- The number of high level successors randomly generated for each node in the high level graph.
- The distance in meters from the current node's pose to generate the random successors.
- For debugging only. This is the seed used to initialize srand(). If set to -1, the current time is used. Otherwise, the user can replay a certain scenario by setting the seed to the previous value.
Collision Checking
~robot_description (string, default: robot_description)
- The name of the ROS parameter that contains the URDF robot description of the robot.
- The name of the topic that the collision map is being published on.
- The name of the collision link group in the URDF that will be used for kinematic calculations and collision checking.
- The name of the reference frame used during planning. Note: At the moment, the collision map must be received in this frame as well.
- If set to true, collision checking will be performed on an occupancy grid with 2cm resolution rather than 1cm resolution. It provides a pretty large performance boost.
Visualizations
~visualize_heuristic (bool, default: false)
- Visualize the suggested 3D path from the start position to the goal pose in rviz.
- Visualize the poses of the states expanded during the search in rviz.
Non-ROS Parameters
Here is an example config/params.cfg. I'll explain what each param means below.
environment_size(meters): 1.6 2.2 2.0
environment_origin(meters): -0.6 -1.5 -0.3
resolution(meters): 0.01
planner_epsilon: 20
use_dijkstra_heuristic: 1
sum_heuristics: 0
use_smoothing: 0
smoothing_weight: 0.00001
plan_to_6d_pose_constraint: 1
verbose: 0
use_uniform_obstacle_cost: 0
use_rstar_planner: 0
rstar_successor_distance: 0.15
use_low_resolution_collision_checking: 1
collision_checking_resolution(meters): 0.02
short_distance_mprims_threshold(meters): 0.20
solve_for_ik_threshold(distance): 0.10
use_multiresolution_motion_primitives: 0
two_calls_to_orientation_planner: 0
distance_from_goal_to_use_orientation_heuristic(meters): 0.0001
Motion_Primitives(degrees): 11 7 7
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
0 0 0 0 2 0 0
0 0 0 0 0 2 0
0 0 0 0 0 0 2
Parameter Definitions
environment_size(meters): (double double double, default: 1.6 2.2 2.0)
- Set the size of the environment in meters -> {size of X_dim} {size of Y_dim} {size of Z_dim}
- The location of cell (0,0,0) in the occupancy grid, in the robot's environment.
- The resolution of the occupancy grid used for planning and for collision checking if 'low_resolution_collision_checking' is disabled.
- The initial epsilon used by the graph search, if time permits the epsilon will be decremented in search for a more optimal solution. (higher value leads in a faster search, but a possibly larger sub-optimality bound on the solution cost).
- Set the type of heuristic used to guide the search towards the position constraint. If 1 is set, a 3D-Dijkstra distance will be computed as the heuristic. If it's set to 0, then the euclidean distance will be used.
- Add the h_xyz + h_rpy together to result in an inadmissable heuristic that *may* lead to a faster search (but may not). ***h_rpy is not being used right now (7/10/2010)***
- Enable a smoothing cost to be used during the search. It adds another dimension to the statespace and thus slows down the search, however, it may prevent the need for a post-processing smoothing step to the trajectory. (Read more in the paper)
- The weight applied to the smoothing cost. A higher weight will result in smoother paths but possibly slower planning times. It only matters if smoothing is enabled.
Roadmap
See the stack roadmap for more details.