Motion Planning/C++ API

In this section, you will learn how to use the C++ interface to the motion planners. The C++ interface is primarily provided through a plugin interface, allowing new planners to be easily integrated. All the code in this tutorial is available in the pr2_moveit_tutorials package and you can run it from there.

Setup
The interface to motion planning requires (1) setting up the plugin interface to the motion planners and (2) having a planning scene that the motion planners can plan in.

Setup the Planner Plugin interface
The plugin interface can be setup using the following lines which allow you to specify a plugin name on the ROS parameter server and load the corresponding planner.

Setup the Planning Scene
You can setup the planning scene in a manner similar to what you did in the corresponding C++ tutorial.

MotionPlanRequest: Setting up a Pose Request
We will first setup a pose request for the end-effector of the right arm of the PR2. Luckily, there are several convenience functions that make this process really easy.

Define a tolerance
You will have to specify a tolerance for how close the motion planner should get to the desired pose (both in position and orientation).

Create the Request
A convenience function makes this really easy. Just specify the name of the group that's being planned for, create the pose goal (as a constraint) and push it back onto the set of goal constraints for the planner.

Allowed Planning Time: Optional (with default)
You can also set the allowed planning time (in seconds). If you don't set the time in the request, it will default to 1.0 second.

Planning Volume
The planning volume specifies the bounding volume within which the robot's motion will be bounded. It is required for mobile base motion to limit the volume that planning will be performed over. It is generally not required if you are doing planning for a fixed manipulator.

Call the Planner
Now, we are ready to call the planner:

Visualize the Result
Note that the motion plan response is not a ROS message but we can use a convenience function to convert it into the corresponding ROS message format.

Modify the Start State for Motion Planning
The motion planner uses the current robot state in the PlanningScene to determine the start state for motion planning. We will modify this state and set it to the last state in the plan that we just computed. You can thus set the robot in the PlanningScene to any state you would like.

MotionPlanRequest: Setting up a Joint Goal
You can directly specify a joint goal for the robot to achieve. Again we will use a JointStateGroup to easily specify the desired joint goal for the right arm of the PR2 robot as a constraint.

User-defined Constraints
You can also impose user-defined constraints on the planning using a callback in the PlanningScene class. See the User-defined Constraints Section in the Planning Scene tutorials for more information on how to do this.

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 (4) Move to a new pose goal. 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 API
 * PlanningScene Code API

Links

 * Back to Motion Planning