The Covariant Hamiltonian Optimization and Motion Planning (CHOMP) algorithm has found many recent applications in robotics research, such as legged locomotion and mobile manipulation. Although integrating kinematic constraints into CHOMP has been investigated, prior work in this area has proven to be slow for trajectories with a large number of constraints. In this paper, we present Multigrid CHOMP with Local Smoothing, an algorithm which improves the runtime of CHOMP under constraints,

more »

... significantly reducing optimality. The effectiveness of this algorithm is demonstrated on two simulated problems, and on a physical HUBO+ humanoid robot, in the context of door opening. We demonstrate order-of-magnitude or higher speedups over the original constrained CHOMP algorithm, while achieving within 2% of the performance of the original algorithm on the underlying objective function.