Planning Request Adapters

Motivation
At the ROS API level, the representation of a planning request is a moveit_msgs::MotionPlanRequest message. This message includes everything move_group needs to compute a motion plan.

It is sometimes desirable however to intervene and make changes to this planning request right before it gets sent to a motion planner. For example, a motion planner that can only handle JointConstraints but not Position + Orientation constraints could benefit from converting the Position + Orientation constraints into a set of JointConstraints (using IK, for example). Another example is that often joint positions reported by encoders appear to be just slightly out of bounds (e.g., even as small as 1e-6, due to use of single precision in certain parts of the system). It is then important to correct this problem before the request is sent to a motion planner, so that the robot's initial state does not appear to be outside bounds (and thus invalid).

In addition to processing the request, making changes to the result of the planning request could also be needed. For example, applying a time parametrization using maximum velocities and maximum accelerations to the solution computed by a planner that reports a sequence of waypoints.

Problems such as mentioned above can be solved using 'planning request adapters'.

Planning Request Adapters
A planning request adapter is simply a plugin that satisfies the [/doxygen/classplanning__request__adapter_1_1PlanningRequestAdapter.html PlanningRequstAdapter interface]. The implementation of such a plugin needs only to provide the adaptAndPlan function. This function receives as argument a function pointer to an object that can run the motion planner given a request, the actual planning request, and is also responsible for filling the planning result. A dummy request adapter that does nothing could simply call the function pointer provided on the input/output arguments received. The simplest way to implement new adapters is perhaps to look at some of the existing implementations, described below.

Default Planning Request Adapters
MoveIt! provides a set of default motion planning adapters that each perform a very specific function.

FixStartStateBounds
The fix start state bounds adapter fixes the start state to be within the joint limits specified in the URDF. The need for this adapter arises in situations where the joint limits for the physical robot are not properly configured. The robot may then end up in a configuration where one or more of its joints is slightly outside its joint limits. In this case, the motion planner is unable to plan since it will think that the starting state is outside joint limits. The "FixStartStateBounds" planning request adapter will "fix" the start state by moving it to the joint limit. However, this is obviously not the right solution every time - e.g. where the joint is really outside its joint limits by a large amount. A parameter for the adapter specifies how much the joint can be outside its limits for it to be "fixable".

FixWorkspaceBounds
The fix workspace bounds adapter will specify a default workspace for planning: a cube of size 10 m x 10 m x 10 m. This workspace will only be specified if the planning request to the planner does not have these fields filled in.

FixStartStateCollision
The fix start state collision adapter will attempt to sample a new collision-free configuration near a specified configuration (in collision) by perturbing the joint values by a small amount. The amount that it will perturb the values by is specified by a "jiggle_factor" parameter that controls the perturbation as a percentage of the total range of motion for the joint. The other parameter for this adapter specifies how many random collisions the adapter will try to sample before giving up.

FixStartStatePathConstraints
This adapter is applied when the start state for a motion plan does not obey the specified path constraints. It will attempt to plan a path between the current configuration of the robot to a new location where the path constraint is obeyed. The new location will serve as the start state for planning.

AddTimeParameterization
The motion planners will typically generate "kinematic paths", i.e., paths that do not obey any velocity or acceleration constraints and are not time parameterized. This adapter will "time parameterize" the motion plans by applying velocity and acceleration constraints.

Using Planning Request Adapters
Planning request adapters are automatically loaded and applied in sequence on a planning request, as part of a planning pipeline. The [/doxygen/classplanning__pipeline_1_1PlanningPipeline.html PlanningPipeline library] loads the planning plugin as well as a set of request adapters. Which request adapters are loaded is determined by the constructor arguments passed to the library. By default, the "planning_adapters" ROS parameter is loaded and a list of plugin names is expected (separated by space). Using this ROS parameter determines how the move_group node operates. Here is an example of the launch file for the OMPL planing pipeline for the PR2.

The order of plugins specified matters. This determines how the chain of plugins is formed. For example, if the planner plugin is P and the request adapters are A B C, then this will translate to running the following code:
 * preprocess request by A
 * preprocess request by B
 * preprocess request by C
 * run the actual planning plugin P
 * postprocess the response by C
 * postprocess the response by B
 * postprocess the response by A