Motion Planning/Planning Pipeline

In this section, you will learn how to use the planning pipeline. The planning pipeline makes it easy to use the motion planners and planning request adapters by taking care of setup and serializing these components together.

Setup
Setting up the planning pipeline (Code API)is really simple. All it requires as input is:
 * An instance of a robot model
 * The name of the parameter on the ROS parameter server that specifies the motion planner to use
 * The name of the parameter on the ROS parameter server that specifies the list of planning adapters to use

Here's the code that sets up an instance of the planning pipeline.

Specifying the motion planner
The motion planner is specified in the planning_plugin on the ROS parameter server. Here, we intend to use the planners in OMPL so the line that we need in our launch file is the following: Note that this param must be defined inside the private namespace of the node in which the planning pipeline is instantiated. For an example, see the launch file for this tutorial.

Specifying the planning request adapters
The planning request adapters are all specified as a set of space separated names (strings). An example is given below where we specify two adapters: (1) the "AddTimeParameterization" adapter operates on a planned trajectory and adds a time parameterization to it and (b) the "FixStartStateBounds" adapter operates on the start state of the motion planning request and fixes any joint values that might be slightly out of bounds. Another example of this specification can be found in the launch file for this tutorial.

Planning
The rest of this tutorial is almost exactly similar to the tutorial on using the planning interface directly, so we will not repeat it here except to point out the differences and additional features.

Call the Planning Pipeline
Now, we are ready to call the planning pipeline. In contrast to the example where you used the planning interface directly, this call to the planning pipeline will result in a set of operations: (1) Motion Planning (2) Applying the set of planning request adapters. Note also that the call to the planning pipeline has a slightly different API than the call directly to the motion planner.

Test a Planning Request Adapter
In this example, we are going to explicitly test one of the planning request adapters: the "FixStartStateBounds" adapter. We will deliberately set one of the joints in the right arm of the PR2 to be slightly outside its joint limits and let the adapter fix it. Here's sample code that will set the "r_shoulder_pan_joint" to be slightly outside its lower limit.

Now, we'll call the planning pipeline with a pose goal. The pipeline should automatically set the start state back to the joint limit, allowing the motion planner to successfully generate a plan. Of course, you don't want to allow this large a deviation to always be valid - a parameter for the "FixStartStateBounds" adapter specifies how much the start state can be outside bounds for it to be fixed.

A list of the default planning request adapters provided by MoveIt! is available on the Planning Request Adapters page.

The Entire Code
The entire code can be seen here in the moveit_pr2 github project.

Compiling the code
Follow the instructions for compiling code from source.

The launch file
The entire launch file is here on github. All the code in this tutorial can be compiled and run from the pr2_moveit_tutorials package that you have as part of your MoveIt! setup.

Running the code

 * Roslaunch the launch file to run the code directly from pr2_moveit_tutorials:

Results in Rviz
In Rviz, you should be able to see the motion plans for four different phases of planning: (1) Move from the default position to a pose goal (2) Move to a joint goal (3) Move back to the original pose goal even though we deliberately set the start state to be outside the physical bounds for one joint of the right arm. If you are unsure of how to visualize plans in Rviz, make sure to read the Rviz Motion Planning Plugin Tutorial.

Additional Reading

 * Planner Base Class
 * PlanningScene Code API
 * Planning Request Adapter Base Class and Default Adapters

Links

 * Back to Motion Planning