A direct method for trajectory optimization of rigid bodies through contact

Direct methods for trajectory optimization are widely used for planning locally optimal trajectories of robotic systems. Many critical tasks, such as locomotion and manipulation, often involve impacting the ground or objects in the environment. Most state-of-the-art techniques treat the discontinuous dynamics that result from impacts 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 of rigid-body systems that contact their environment through inelastic impacts and Coulomb friction. This method eliminates the requirement for a priori mode ordering. Motivated by the formulation of multi-contact dynamics as a Linear Complementarity Problem for forward simulation, the proposed algorithm poses the optimization problem as a Mathematical Program with Complementarity Constraints. We leverage Sequential Quadratic Programming to naturally resolve contact constraint forces while simultaneously optimizing a trajectory that satisfies the complementarity constraints. The method scales well to high-dimensional systems with large numbers of possible modes. We demonstrate the approach on four increasingly complex systems: rotating a pinned object with a finger, simple grasping and manipulation, planar walking with the Spring Flamingo robot, and high-speed bipedal running on the FastRunner platform.

[1]  David Q. Mayne,et al.  Differential dynamic programming , 1972, The Mathematical Gazette.

[2]  Anil V. Rao,et al.  Practical Methods for Optimal Control Using Nonlinear Programming , 1987 .

[3]  C. Hargraves,et al.  DIRECT TRAJECTORY OPTIMIZATION USING NONLINEAR PROGRAMMING AND COLLOCATION , 1987 .

[4]  A. Fischer A special newton-type optimization method , 1992 .

[5]  J. Trinkle,et al.  On Dynamic Multi‐Rigid‐Body Contact Problems with Coulomb Friction , 1995 .

[6]  Olvi L. Mangasarian,et al.  A class of smoothing functions for nonlinear and mixed complementarity problems , 1996, Comput. Optim. Appl..

[7]  D. Stewart,et al.  AN IMPLICIT TIME-STEPPING SCHEME FOR RIGID BODY DYNAMICS WITH INELASTIC COLLISIONS AND COULOMB FRICTION , 1996 .

[8]  Bethany L. Nicholson,et al.  Mathematical Programs with Equilibrium Constraints , 2021, Pyomo — Optimization Modeling in Python.

[9]  M. Anitescu,et al.  Formulating Dynamic Multi-Rigid-Body Contact Problems with Friction as Solvable Linear Complementarity Problems , 1997 .

[10]  Jerry E. Pratt,et al.  Intuitive control of a planar bipedal walking robot , 1998, Proceedings. 1998 IEEE International Conference on Robotics and Automation (Cat. No.98CH36146).

[11]  J. Betts Survey of Numerical Methods for Trajectory Optimization , 1998 .

[12]  Masao Fukushima,et al.  A Globally Convergent Sequential Quadratic Programming Algorithm for Mathematical Programs with Linear Complementarity Constraints , 1998, Comput. Optim. Appl..

[13]  B. Brogliato Nonsmooth Mechanics: Models, Dynamics and Control , 1999 .

[14]  Jerry E. Pratt,et al.  Exploiting inherent robustness and natural dynamics in the control of bipedal walking robots , 2000 .

[15]  David E. Stewart,et al.  Rigid-Body Dynamics with Friction and Impact , 2000, SIAM Rev..

[16]  Michael A. Saunders,et al.  SNOPT: An SQP Algorithm for Large-Scale Constrained Optimization , 2002, SIAM J. Optim..

[17]  Henrik I. Christensen,et al.  Implementation of multi-rigid-body dynamics within a robotic grasping simulator , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[18]  C. Lemaréchal,et al.  On the Equivalence Between Complementarity Systems, Projected Systems and Unilateral Differential Inclusions , 2004 .

[19]  Andrew Howard,et al.  Design and use paradigms for Gazebo, an open-source multi-robot simulator , 2004, 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE Cat. No.04CH37566).

[20]  Michael A. Saunders,et al.  SNOPT: An SQP Algorithm for Large-Scale Constrained Optimization , 2005, SIAM Rev..

[21]  Mihai Anitescu,et al.  On Using the Elastic Mode in Nonlinear Programming Approaches to Mathematical Programs with Complementarity Constraints , 2005, SIAM J. Optim..

[22]  Russ Tedrake,et al.  Efficient Bipedal Robots Based on Passive-Dynamic Walkers , 2005, Science.

[23]  Sven Leyffer,et al.  Local Convergence of SQP Methods for Mathematical Programs with Equilibrium Constraints , 2006, SIAM J. Optim..

[24]  Manoj Srinivasan,et al.  Computer optimization of a minimal biped model discovers walking and running , 2006, Nature.

[25]  C. Glocker,et al.  Trajectory optimization of mechanical hybrid systems using SUMT , 2006, 9th IEEE International Workshop on Advanced Motion Control, 2006..

[26]  E. Westervelt,et al.  Feedback Control of Dynamic Bipedal Robot Locomotion , 2007 .

[27]  Christoph Glocker,et al.  A Combined Continuation and Penalty Method for the Determination of Optimal Hybrid Mechanical Trajectories , 2007 .

[28]  Katie Byl,et al.  Approximate optimal control of the compass gait on rough terrain , 2008, 2008 IEEE International Conference on Robotics and Automation.

[29]  B. Brogliato,et al.  Numerical Methods for Nonsmooth Dynamical Systems: Applications in Mechanics and Electronics , 2008 .

[30]  Vincent Acary,et al.  Numerical Methods for Nonsmooth Dynamical Systems: Applications in Mechanics and Electronics , 2008 .

[31]  Zoran Popovic,et al.  Optimal gait and form for animal locomotion , 2009, ACM Trans. Graph..

[32]  Yuval Tassa,et al.  Stochastic Complementarity for Local Control of Discontinuous Dynamics , 2010, Robotics: Science and Systems.

[33]  K. Mombaur,et al.  Modeling and Optimal Control of Human-Like Running , 2010, IEEE/ASME Transactions on Mechatronics.

[34]  Binh Nguyen,et al.  Sources of Error in a Simulation of Rigid Parts on a Vibrating Rigid Plate , 2010 .

[35]  Binh Nguyen,et al.  Modeling non-convex configuration space using linear complementarity problems , 2010, 2010 IEEE International Conference on Robotics and Automation.

[36]  Ian R. Manchester,et al.  Bounding on rough terrain with the LittleDog robot , 2011, Int. J. Robotics Res..

[37]  Pierre-Brice Wieber,et al.  Author manuscript, published in "IEEE/RSJ International Conference on Intelligent Robots and Systems (2011)" A sparse model predictive control formulation for walking motion generation , 2011 .

[38]  Jerry E. Pratt,et al.  FastRunner: A fast, efficient and robust bipedal robot. Concept and planar simulation , 2012, 2012 IEEE International Conference on Robotics and Automation.

[39]  Emanuel Todorov,et al.  Trajectory optimization for domains with contacts using inverse dynamics , 2012, 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[40]  Yuval Tassa,et al.  Synthesis and stabilization of complex behaviors through online trajectory optimization , 2012, 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[41]  Zoran Popovic,et al.  Discovery of complex behaviors through contact-invariant optimization , 2012, ACM Trans. Graph..

[42]  Russ Tedrake,et al.  Direct Trajectory Optimization of Rigid Body Dynamical Systems through Contact , 2012, WAFR.

[43]  Zoran Popovic,et al.  Contact-invariant optimization for hand manipulation , 2012, SCA '12.

[44]  TedrakeRuss,et al.  Corrigendum: A direct method for trajectory optimization of rigid bodies through contact , 2014 .