MoveIt 2 Planning Pipeline Refactoring
Introduction & Background
“Motion planning is the problem of finding a robot motion from a start state to a goal state that avoids obstacles in the environment and satisfies other constraints” (Lynch, K. M., & Park, F. C. (2017)). Such a planning problem can be encoded as constraints and optimization objectives. In many cases, the solution is a trajectory (a timed sequence of robot states) that a robot can execute. The classical approach to solving motion planning problems uses a sequence of algorithms, for example, Path planning -> Path Optimization -> Trajectory Generation.
A generic implementation of such a planning pipeline is one of the core components of MoveIt. Data structures to encode planning problems (MotionPlanRequest) and solutions (MotionPlanResponse) are provided, and it is possible to create custom pipelines by choosing from various algorithms in MoveIt. These algorithms are implemented as plugins so that you can specify a planning pipeline at runtime and make it easy to integrate your implementations. Since no planning algorithm is a jack of all trades, it is important to be able to define and swap pipelines. Take for example, a bin picking application, where a planning pipeline with a Cartesian planner is useful to plan grasp and retreat motions and a planning pipeline with a sampling-based planner could be used to quickly find collision free motions to get the picked object to the drop position.
However, the planning pipeline implementation in MoveIt had some shortcomings:
- Order of algorithm calls was determined by the algorithm implementations, which made it hard to comprehend and prone to errors.
- It was not possible to chain planner algorithms.
- No clear separation and naming of pre-processing, planning and post-processing algorithms.
This is why we decided to refactor the planning pipeline to address the problems above (tracked by moveit2#2408). This blog post describes these changes in detail.
Refactoring Steps
Our goal was to refactor the current pipeline implementation (caricatural control flow shown in Fig. 1)
Fig. 1: Control flow of the old planning pipeline implementation
into a clean and tidy structure as shown in Fig. 2
Fig. 2: Refactored planning pipeline implementation
1. Cleanup and Simplify!
First, the existing implementation was cleaned up and simplified. This included:
- Separate post-processing, planning, and post-processing algorithms
- PlanRequestAdapter class implementations can only modify the MotionPlanRequest.
- Post-processing algorithms inherit now from a new PlanResponseAdapter class and can only modify the MotionPlanResponse. Furthermore, they cannot be used as the first element in the chain.
- Planning algorithms only inherit from the planning interface class, and the minimal pipeline requires at least one planning algorithm.
- Clean-up call order
- User specifies vectors of PlanRequestAdapters, PlannerPlugins, and PlanResponseAdapters for a pipeline and the algorithms in these vectors get called sequentially in the defined order.
- Move functionality from the Pipeline class into new plugins
- In the pipeline class, several checks on the result trajectory were performed. These checks have been moved into a new post-processing plugin: ValidateSolution.
- The pipeline class sent messages to RVIZ for visualization. This functionality has also been moved into a separate post-processing plugin: DisplayMotionPath.
2.Test coverage and introspection
Second, we improved the testing and introspection capabilities of the planning pipeline.
- Unit Tests for the planning pipeline class have been added 🤓.
- Intermediate results between stages of the planning pipeline can now be published if the progress_topic parameter is set.
3.Enable planner chaining
The final step was to enable chaining planner algorithms.
- A planning pipeline can load multiple planner plugins
- You can now create seed trajectories for optimizing planner plugins with other planner plugins! For example, use OMPL’s RRTConnect to create a solution and optimize it with STOMP.
- STOMP and CHOMP planner plugins can now use the reference trajectory in the MotionPlanResponse if available. While warm-starting optimization-based planning algorithms is not a new feature, it is now possible to do so by chaining planner plugins instead of writing an additional smoothing plugin. The chaining approach enables leaner and more flexible planning pipeline configurations.
- Removes duplicate code
- Before: Two implementations of STOMP/CHOMP plugins existed: A PlannerPlugin and a RequestAdapterPlugin.
- After: STOMP and CHOMP are only PlannerPlugin types.
Exemplary Setup
With these refactorings, it is now easier than ever to build a custom chain of planning algorithms with MoveIt. An example configuration, like in MoveItPro’s UR base config, looks like this:
request_adapters:
- default_planning_request_adapters/ResolveConstraintFrames
- default_planning_request_adapters/ValidateWorkspaceBounds
- default_planning_request_adapters/CheckStartStateBounds
- default_planning_request_adapters/CheckStartStateCollision
planning_plugins:
- ompl_interface/OMPLPlanner
response_adapters:
- default_planning_response_adapters/AddTimeOptimalParameterization
- default_planning_response_adapters/ValidateSolution
- default_planning_response_adapters/DisplayMotionPath
A resulting motion planning solutions can be seen in Fig. 3.:
Fig. 3: Example motions planned with a planning pipeline similar to the example configuration in MoveItPro.
PlanningRequestAdapters
CheckForStackedConstraints
A planning request adapter that checks if the motion plan request contains a conflicting path or goal constraints. If that is the case, a warning is created, but the planning process is not interrupted.
CheckStartStateBounds
This adapter validates if the start state is within the joint limits specified in the URDF.
CheckStartStateCollision
Checks if the start state of the planning problem is in a collision.
ResolveConstraintFrames
Transforms all the constrained frames to frames that are part of the robot model. This might be necessary because the frames of an attached object are not necessarily known to a planner.
ValidateWorkspaceBounds
If not specified otherwise, this adapter adds workspace bounds to the planning problem.
PlannerPlugins
OMPLPlanner
Planner plugin uses sampling-based planners from the Open Motion Planning Library.
STOMPPlanner
Stochastic Trajectory Optimization for Motion Planning is a planning algorithm that plans and/or optimizes paths with a probabilistic optimization approach (more info).
CHOMPPlanner
Covariant Hamiltonian optimization for motion planning is a gradient-based path planner/ optimizer (more info).
PILZIndustrialMotionPlanner
Generates industrial Cartesian trajectories (LIN, PTP, CIRC). You can find more information on Cartesian planning in another blog post on this website.
PlanningResponseAdapters
ValidateSolution
Checks the validity (collision-free, feasible, constraint satisfaction) of the path created by the planners.
AddTimeOptimalParameterization
Generates a time-optimal trajectory based on a given path based on an algorithm described by Agarwal et al..
AddRuckigTrajectorySmoothing
Uses the Ruckig algorithm to adapt a trajectory to be jerk-constrained and time-optimal.
DisplayMotionPath
Publishes the computed path to RVIZ for visualization.
Conclusion
MoveIt 2 now supports more customizable planning pipelines to support the varying planning needs of unstructured environments. We are looking forward to seeing your planner or adapter plugin implementations as contributions to the MoveIt ecosystem! For more production environment features, check out our MoveIt Pro offering.