Covariant Hamiltonian Optimization for Motion Planning

Toward high-dimensional imitation learning:

Reducing motion planning to trajectory optimization.

Nathan Ratliff, Matthew Zucker, J. Andrew Bagnell, Siddhartha Srinivasa

Covariant Hamiltonian optimization for motion planning (CHOMP) is a novel 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 dynamical quantities such as joint velocities and accelerations. It rapidly converges to a smooth collision-free trajectory that can be executed efficiently on the robot. Additionally, by characterizing motion planning as optimization, we can again utilize the MMP framework for training this high-dimensional planner.

We implemented CHOMP on both a six degree-of-freedom WAM arm developed by Barrett Technology (top row of images) and a twelve degree-of-freedom quadrupedal robot developed by Boston Dynamics (bottom row of images). The WAM arm shown the movie is housed at Intel Research, Pittsburgh as part of the Personal Robotics project. Although CHOMP is not a complete planner, it successfully found smooth collision-free trajectories for the robot arm on 99 out of 105 planning problems sampled from the household environment shown above. By detecting when the algorithm first finds a collision free trajectory and stopping CHOMP after a fixed number of iterations thereafter, we achieved running times of approximately 2.5 seconds on average across these trials.

On the quadrupedal robot, we implemented a coordinate-descent variant of CHOMP in which we repeatedly optimized the swing-leg alone, fixing all other joints, and then optimized the body position, fixing all swing-leg joints. This trajectory optimization procedure is actively used on the current implementation of the CMU LittleDog system for the DARPA sponsored Learning for Locomotion project. The left portion of the LittleDog image sequence above shows the quadrupedal robot crossing various terrains used by the government team for testing. On the right half, we show a schematic diagram of the initial infeasible trajectory (middle right) alongside the final feasible collision-free trajectory (middle left). Note that the algorithm shifted the entire body away from the Jersey barrier to engage a more dynamic range of motion in the swing-leg.