Direct Trajectory Optimization of Rigid Body Dynamical Systems Through Contact

Michael Posa and Russ Tedrake

Direct methods for trajectory optimization are widely used for planning locally optimal trajectories of robotic systems. Most state-of-the-art techniques treat the discontinuous dynamics of contact as discrete modes and restrict the search for a complete path to a specified sequence through these modes. Here we present a novel method for trajectory planning through contact that eliminates the requirement for an a priori mode ordering. Motivated by the formulation of multi-contact dynamics as a Linear Complementarity Problem (LCP) for forward simulation, the proposed algorithm leverages Sequential Quadratic Programming (SQP) to naturally resolve contact constraint forces while simultaneously optimizing a trajectory and satisfying nonlinear complementarity constraints. The method scales well to high dimensional systems with large numbers of possible modes. We demonstrate the approach using three increasingly complex systems: rotating a pinned object with a finger, planar walking with the Spring Flamingo robot, and high speed bipedal running on the FastRunner platform.

@inproceedings{Posa2012,
  address = {Cambridge, MA},
  author = {Posa, Michael and Tedrake, Russ},
  booktitle = {The Workshop on the Algorithmic Foundations of Robotics (WAFR)},
  month = jun,
  pages = {527--542},
  title = {{Direct Trajectory Optimization of Rigid Body Dynamical Systems Through Contact}},
  year = {2012},
  youtube = {pH1pDXnCBx4},
  url = {https://link.springer.com/chapter/10.1007/978-3-642-36279-8_32}
}