Using CHOMP Planner
Covariant Hamiltonian Optimization for Motion Planning (CHOMP) is a gradient-based trajectory optimization procedure that makes many everyday motion planning problems both simple and trainable (Ratliff et al., 2009c). While most high-dimensional motion planners separate trajectory generation into distinct planning and optimization stages, this algorithm capitalizes on covariant gradient and functional gradient approaches to the optimization stage to design a motion planning algorithm based entirely on trajectory optimization. Given an infeasible naive trajectory, CHOMP reacts to the surrounding environment to quickly pull the trajectory out of collision while simultaneously optimizing dynamic quantities such as joint velocities and accelerations. It rapidly converges to a smooth collision-free trajectory that can be executed efficiently on the robot. More info
If you haven’t already done so, make sure you’ve completed the steps in Getting Started.
You should also have gone through the steps in Visualization with MoveIt RViz Plugin
To use CHOMP with your robot you must have a MoveIt configuration package for your robot. For example, if you have a Panda robot, it’s called
panda_moveit_config and can be found here. These are typically configured using the MoveIt Setup Assistant.
Using CHOMP with Your Robot
Note: If you plan to use the
panda_moveit_config package from the ros-planning/moveit_resources repository, these steps are already done for you and you can skip this section. Otherwise, to add the configuration for your robot you must:
Create a chomp_demo.launch.py file in the launch directory for your MoveIt config package.
Modify all references to Panda in
chomp_demo.launch.pyto point to your custom configuration instead
Ensure you have included a chomp_planning.yaml file in the config directory of your MoveIt config package.
chomp_planning.yamlin your favorite editor and change
animate_endeffector_segment: "panda_rightfinger"to the appropriate link for your robot. Feel free to modify any parameters you think may better suit your needs.
Running the Demo
If you have the
panda_moveit_config from the ros-planning/moveit_resources repository along with
moveit2_tutorials you can run the demo using:
ros2 launch moveit2_tutorials chomp_demo.launch.py rviz_tutorial:=True
Note: For convenience we have provided an RViz configuration you may use, but setting
False or simply omitting it will allow you to set up RViz according to your personal preferences.
Adding Obstacles to the Scene
To add obstacles to the scene, we can use this node to create a scene with obstacles.
To run the CHOMP planner with obstacles, open a second shell. In the first shell (if you closed the one from from the previous step) start RViz and wait for everything to finish loading:
ros2 launch moveit2_tutorials chomp_demo.launch.py rviz_tutorial:=True
In the second shell, run the command:
ros2 run moveit2_tutorials collision_scene_example
Next, in RViz, select CHOMP in the MotionPlanning panel under the Context tab. Set the desired start and goal states by moving the end-effector around with the marker and then click on the Plan button under the Planning tab in the MotionPlanning panel to start planning. The planner will now attempt to find a feasible solution between the given start and end position.
Modifying the parameters for CHOMP
CHOMP has some optimization parameters associated with it. These can be modified for the given environment/robot you are working with and is normally present in the chomp_planning.yaml file in the config folder of the robot you are working with. If this file does not exist for your robot, you can create it and set the parameter values as you want. The following may provide some insight on what the parameters in
chomp_planning.yaml are used for:
planning_time_limit: Maximum amount of time the optimizer can take to find a solution before terminating.
max_iterations: Maximum number of iterations that the planner can take to find a good solution during optimization.
max_iterations_after_collision_free: Maximum number of iterations to be performed after a collision-free path is found.
smoothness_cost_weight: Weight of smoothness in the final cost function for CHOMP to optimize.
obstacle_cost_weight: Weight given to obstacles for the final cost CHOMP optimizes over. e.g., 0.0 would have obstacles to be ignored, 1.0 would be a hard constraint.
learning_rate: The rate used by the optimizer to find the local / global minima while reducing the total cost.
smoothness_cost_velocity, smoothness_cost_acceleration, smoothness_cost_jerk: Variables associated with the cost in velocity, acceleration and jerk.
ridge_factor: Noise added to the diagonal of the total quadratic cost matrix in the objective function. Addition of small noise (e.g., 0.001) allows CHOMP to avoid obstacles at the cost of smoothness in trajectory.
use_pseudo_inverse: Enables pseudo inverse calculations when
pseudo_inverse_ridge_factor: Set the ridge factor if pseudo inverse is enabled.
joint_update_limit: Update limit for the robot joints.
collision_clearance: Minimum distance from obstacles needed to avoid collision.
collision_threshold: The cost threshold that that must be maintained to avoid collisions.
use_stochastic_descent: Use stochastic descent while optimizing the cost when set to
true. In stochastic descent, a random point from the trajectory is used, rather than all the trajectory points. This is faster and guaranteed to converge, but it may take more iterations in the worst case.
true, CHOMP will tweak certain parameters in an attempt to find a solution when one does not exist with the default parameters specified in the
max_recovery_attempts: Maximum times that CHOMP is run with a varied set of parameters after the first attempt with the default parameters fails.
trajectory_initializaiton_method: The type of trajectory initialization given to CHOMP, which can be
fillTrajectory. The first three options refer to the interpolation methods used for trajectory initialization between start and goal states.
fillTrajectoryprovides an option of initializing the trajectory with a path computed from an existing motion planner like OMPL.
Choosing parameters for CHOMP requires some intuition that is informed by the planning environment. For instance, the default parameters for CHOMP work well in environments without obstacles; however, in environments with many obstacles the default parameters will likely cause CHOMP to get stuck in local minima. By tweaking parameters, we can improve the quality of plans generated by CHOMP.
Some of the unused/commented parameters are hmc_stochasticity, hmc_annealing_factor, hmc_discretization, use_hamiltonian_montecarlo, animate_endeffector, animate_endeffector_segment, animate_path, random_jump_amount, add_randomness.
Difference between plans obtained by CHOMP and OMPL
Optimizing planners optimize a cost function that may sometimes lead to surprising results: moving through a thin obstacle might be lower cost than a long, winding trajectory that avoids all collisions. In this section we make a distinction between paths obtained from CHOMP and contrast it to those obtained from OMPL.
OMPL is a open source library for sampling based / randomized motion planning algorithms. Sampling based algorithms are probabilistically complete: a solution would be eventually found if one exists, however, non-existence of a solution cannot be reported. These algorithms are efficient and usually find a solution quickly. OMPL does not contain any code related to collision checking or visualization, as the designers of OMPL did not want to tie it to a particular collision checker or visualization front end. The library is designed so it can be easily integrated into systems that provide the additional components. MoveIt integrates directly with OMPL and uses the motion planners from OMPL as its default set of planners. The planners in OMPL are abstract; i.e. OMPL has no concept of a robot. Instead, MoveIt configures OMPL and provides the back-end for OMPL to work with problems in robotics.
CHOMP: While most high-dimensional motion planners separate trajectory generation into distinct planning and optimization stages, CHOMP capitalizes on covariant gradient and functional gradient approaches to the optimization stage to design a motion planning algorithm based entirely on trajectory optimization. Given an infeasible naive trajectory, CHOMP reacts to the surrounding environment to quickly pull the trajectory out of collision while simultaneously optimizing dynamic quantities such as joint velocities and accelerations. It rapidly converges to a smooth, collision-free trajectory that can be executed efficiently on the robot. A covariant update rule ensures that CHOMP quickly converges to a locally optimal trajectory.
For scenes containing obstacles, CHOMP often generates paths which do not prefer smooth trajectories by addition of some noise (ridge_factor) in the cost function for the dynamic quantities of the robot (like acceleration, velocity). CHOMP is able to avoid obstacles in most cases, but it can fail if it gets stuck in local minima due to a bad initial guess for the trajectory. OMPL can be used to generate collision-free seed trajectories for CHOMP to mitigate this issue.