CHOMP: Covariant Hamiltonian Optimization for Motion Planning - Robotics Institute Carnegie Mellon University