1

Fast Contact-Implicit
Model-Predictive Control
Simon Le Cleac’h1∗ , Taylor A. Howell1∗ , Mac Schwager2 , and Zachary Manchester3

Abstract—We present a general approach for controlling
robotic systems that make and break contact with their environments. Contact-implicit model-predictive control (CI-MPC)
generalizes linear MPC to contact-rich settings by relying onlinear complementarity problems (LCP) computed using strategic
Taylor approximations about a reference trajectory and retaining
non-smooth impact and friction dynamics, allowing the policy to
not only reason about contact forces and timing, but also generate
entirely new contact mode sequences online. To achieve reliable
and fast numerical convergence, we devise a structure-exploiting,interior-point solver for the LCP contact dynamics and a custom
trajectory optimizer for trajectory-tracking MPC problems. We
demonstrate CI-MPC at real-time rates in simulation, andshow that it is robust to model mismatch and can respond to
disturbances by discovering and exploiting new contact modes
across a variety of robotic systems, including a pushbot, hopper,
and planar quadruped and biped.
Index Terms—Model-Predictive Control, Legged Robots, Contact Modeling, Optimization and Optimal Control.

I. I NTRODUCTIONONTROLLING systems that make and break contact
with their environments is one of the grand challenges
in robotics. Numerous approaches have been employed forcontrolling such systems, ranging from hybrid-zero dynamics[13, 1, 10], to complementarity controllers [2], to neuralnetwork policies [4, 5], and model-predictive control (MPC)[14, 11]. There have also been numerous successes deploying
such approaches on complex systems in recent years: direct
trajectory optimization and LQR on Atlas [8], smooth-contact
models and differential dynamic programming on HRP-2 [12,7], zero-moment point and feedback linearization on ASIMO[6], and MPC with simplified dynamics models on Cheetah [3]and ANYmal [9]. However, reliable general-purpose control
techniques that can reason about contact events and can beapplied across a wide range of robotic systems without requiring application-specific model simplifications, gait-generation
heuristics, or extensive parameter tuning remain elusive.
In this work, we focus on the problem of local tracking control for systems that experience contact interactions with theirC

1 Simon Le Cleac’h and Taylor A. Howell are with the Department ofMechanical Engineering, Stanford University, Stanford, CA 94305, USA.
{simonlc, thowell}@stanford.edu
2 Mac Schwager is with the Department of Aeronautics andAstronautics, Stanford University, Stanford, CA 94305, USA.
schwager@stanford.edu
3 Zachary Manchester is with The Robotics Institute, Carnegie Mellon
University, Pittsburgh, PA 15213, USA. zacm@cmu.edu(Corresponding author: Taylor A. Howell)∗ These authors contributed equally to this work.

Fig. 1: Monte Carlo simulations of initial conditions for systems tracking a reference trajectory. 100 initial configurations
are randomly sampled for a hopper (top) and quadruped(bottom). Perturbations from the reference initial configuration
include large translations, tilts, and joint-angle offsets. The
policy successfully converges to the reference gait for all initialconditions on both systems.

environments. Our approach combines a differentiable “hardcontact” rigid-body dynamics formulation with strategic linearizations, exploitation of the trajectory-optimization problem
structure, and specialized numerical optimization techniques.
The result is a model-predictive-control algorithm that caneffectively reason about contact changes in the presence oflarge disturbances while remaining fast enough for real-time
execution on modest computing hardware.
We formulate dynamics with contact as a complementarity problem that simultaneously satisfies impact and friction

2constraints. By employing a interior-point method to optimize
this problem, we naturally and reliably converge from “soft”
to “hard” contact as the central-path parameter is decreased
to zero. At a solution point, the implicit-function theorem isthen utilized to efficiently compute dynamics derivatives foruse in the policy. To enable real-time performance for modelpredictive control, we pre-compute linearizations of the system’s dynamics, signed-distance functions, and friction cones
about a reference trajectory, while explicitly retaining complementarity constraints that encode contact switching behavior,
resulting in a sequence of lower-level time-varying linearcomplementarity problems (LCP). An upper-level trajectoryoptimization problem is then optimized using an efficient
structure-exploiting solver. We refer to this algorithm as
contact-implicit model-predictive control (CI-MPC).
Finally, we demonstrate that our CI-MPC policy can generate new contact sequences online and reliably track reference
trajectories while subject to significant model mismatch andlarge disturbances for a number of qualitatively different
robotic systems, including a pushbot, hopper, and planar
quadruped and biped.
Our specific contributions are:
• A contact-dynamics formulation that can be reliably evaluated and efficiently differentiated with a custom interiorpoint solver• Fast, structure-exploiting solvers for the contact-dynamics
and trajectory-optimization problems• A model-predictive-control framework for robotic systems with contact dynamics• A collection of simulation examples demonstrating theperformance of the CI-MPC algorithm on a variety ofrobotic systems across a range of highly dynamic balancing and locomotion tasks
A pre-print of our paper is available onarXiv:
arxiv.org/abs/2107.05616.
An
open-source
implementationofthealgorithm,
ContactImplicitMPC.jl, along with a set ofexperiments on various dynamical systems are available at:github.com/dojo-sim/ContactImplicitMPC.jl.
ACKNOWLEDGMENTS
This work was supported in part by Frontier Robotics,
Innovative Research Excellence, Honda R&D Co., Ltd, ONR
award N00014-18-1-2830, NSF NRI award 1830402, andDARPA YFA award D18AP00064. Toyota Research Institute(“TRI”) provided funds to assist the authors with their research
but this article solely reflects the opinions and conclusions ofits authors and not TRI or any other Toyota entity.
R EFERENCES[1] A. D. Ames, K. Galloway, K. Sreenath, and J. W. Grizzle. Rapidly
exponentially stabilizing control Lyapunov functions and hybrid zerodynamics. IEEE Transactions on Automatic Control, 59(4):876–891,
2014.
[2] A. Aydinoglu, V. M. Preciado, and M. Posa. Contact-aware controller
design for complementarity systems. In 2020 IEEE International
Conference on Robotics and Automation (ICRA), pages 1525–1531.
IEEE, 2020.

Fig. 2: Biped walking from left to right across flat (top),sinusoidal (middle), and piecewise linear (bottom) terrain
using the same policy.

[3] G. Bledt. Regularized predictive control framework for robust dynamic
legged locomotion. PhD thesis, Massachusetts Institute of Technology,
2020.
[4] N. Heess, D. TB, S. Sriram, J. Lemmon, J. Merel, G. Wayne, Y. Tassa,T. Erez, Z. Wang, S. M. Eslami, et al. Emergence of locomotion
behaviours in rich environments. arXiv preprint arXiv:1707.02286,2017.
[5] E. Heiden, D. Millard, E. Coumans, Y. Sheng, and G. S. Sukhatme.
Neuralsim: Augmenting differentiable simulators with neural networks.
arXiv preprint arXiv:2011.04217, 2020.
[6] K. Hirai, M. Hirose, Y. Haikawa, and T. Takenaka. The developmentof Honda humanoid robot. In Proceedings. 1998 IEEE InternationalConference on Robotics and Automation, volume 2, pages 1321–1326.
IEEE, 1998.
[7] J. Koenemann, A. Del Prete, Y. Tassa, E. Todorov, O. Stasse, M. Bennewitz, and N. Mansard. Whole-body model-predictive control applied
to the HRP-2 humanoid. In 2015 IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS), pages 3346–3351. IEEE, 2015.
[8] S. Kuindersma, R. Deits, M. Fallon, A. Valenzuela, H. Dai, F. Permenter,
T. Koolen, P. Marion, and R. Tedrake. Optimization-based locomotion
planning, estimation, and control design for the Atlas humanoid robot.
Autonomous Robots, 40(3):429–455, 2016.
[9] J. Lee, J. Hwangbo, L. Wellhausen, V. Koltun, and M. Hutter. Learning
quadrupedal locomotion over challenging terrain. Science Robotics, 5(47), 2020.
[10] Z. Li, X. Cheng, X. B. Peng, P. Abbeel, S. Levine, G. Berseth,and K. Sreenath. Reinforcement learning for robust parameterized
locomotion control of bipedal robots. arXiv preprint arXiv:2103.14295,2021.
[11] J.-P. Sleiman, F. Farshidian, M. V. Minniti, and M. Hutter. A unified
mpc framework for whole-body dynamic locomotion and manipulation.
IEEE Robotics and Automation Letters, 6(3):4688–4695, 2021.
[12] E. Todorov, T. Erez, and Y. Tassa. MuJoCo: A physics engine for modelbased control. In 2012 IEEE/RSJ International Conference on Intelligent
Robots and Systems, pages 5026–5033. IEEE, 2012.
[13] E. R. Westervelt, J. W. Grizzle, and D. E. Koditschek. Hybrid zerodynamics of planar biped walkers. IEEE Transactions on AutomaticControl, 48(1):42–56, 2003.
[14] A. W. Winkler, C. D. Bellicoso, M. Hutter, and J. Buchli. Gait andTrajectory Optimization for Legged Systems Through Phase-Based EndEffector Parameterization. IEEE Robotics and Automation Letters, 3(3):1560–1567, July 2018.