Source-linked AI summary
Fast Contact-Implicit Model-Predictive Control
Simon Le Cleac'h, Taylor Howell, Shuo Yang, Chi-Yen Lee, John Zhang, Arun Bishop, Mac Schwager, Zachary Manchester
TL;DR
Reliable general-purpose control that reasons about contact events across robotic systems remains elusive; this paper presents a bi-level contact-implicit MPC approach. The controller generates new contact sequences online and demonstrates real-time, robust control across simulated systems and quadruped hardware.
Problem
Reliable general-purpose control that reasons about contact events across a wide range of robotic systems remains elusive, while many existing policies are system-specific.
Source: §I · p. 1, A. Model Predictive Control · p. 2Method
The paper uses bi-level MPC with time-varying complementarity-based contact dynamics to plan contact changes and track trajectories.
Source: §I · p. 1Results
Takeaways & Limitations
CI-MPC robustly tracks reference trajectories through contact despite disturbances, model mismatch, and uncertain environments.
Source: §VI · p. 10For general locomotion, classic convex MPC is significantly more robust in practice because it includes an online foothold-selection strategy.
Source: A. Limitations · p. 11
Abstract
from arXiv · showhide
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 utilizing a bi-level planning formulation with lower-level contact dynamics formulated as time-varying linear complementarity problems (LCPs) computed using strategic Taylor approximations about a reference trajectory. These dynamics enable the upper-level planning problem to reason about contact timing and forces, and generate entirely new contact-mode sequences online. To achieve reliable and fast numerical convergence, we devise a structure-exploiting interior-point solver for these LCP contact dynamics and a custom trajectory optimizer for the tracking problem. We demonstrate real-time solution rates for CI-MPC and the ability to generate and track non-periodic behaviours in hardware experiments on a quadrupedal robot. We also show that the controller is robust to model mismatch and can respond to disturbances by discovering and exploiting new contact modes across a variety of robotic systems in simulation, including a pushbot, planar hopper, planar quadruped, and planar biped.
I. INTRODUCTION · II. RELATED WORK · A. Model Predictive Control
CI-MPC uses approximate contact dynamics and specialized optimization in a bi-level MPC framework to plan contact changes and track trajectories. The paper situates this approach against system-specific MPC methods and reports results across simulated robots and quadruped hardware.
I. INTRODUCTION
- Reliable general-purpose control remains elusive for contact-rich robots without system-specific model simplifications, gait heuristics, or extensive tuning.The introduction contrasts this gap with successful but varied approaches, including trajectory optimization, smooth-contact methods, feedback linearization, and MPC.Source: §I · p. 1
- CI-MPC combines fast differentiable rigid-body contact dynamics, reference-trajectory approximations, and specialized optimization in a bi-level MPC framework.The upper-level trajectory optimizer reasons about contact changes using lower-level time-varying LCPs.Source: §I · pp. 1–2
- The method retains complementarity constraints while linearizing dynamics, signed distances, and friction cones, and uses interior-point solving and implicit differentiation for contact dynamics.Fast linear algebra is used for the upper-level trajectory optimization.Source: §I · pp. 1–2
- CI-MPC generated new contact sequences online and tracked trajectories despite model mismatch and large disturbances in simulations and on Unitree Go1 hardware.The simulated systems included a pushbot, planar hopper, quadruped, and biped.Source: §I · p. 2
II. RELATED WORK
- The related-work section reviews MPC for systems that make and break contact and complementarity-based contact dynamics.Source: §II · p. 2
A. Model Predictive Control
- Raibert’s hopping-robot approach decouples body-motion planning from leg-motion and torque control.The high-level controller plans body motions while the low-level controller generates the forces and torques.Source: A. Model Predictive Control · p. 2
- Humanoid MPC advances include Atlas navigation and parkour using centroidal dynamics and full kinematics, and simulated acrobatics enabled by hardware-controller integration.Source: A. Model Predictive Control · p. 2
- Quadruped advances include lightweight legs matched to controller approximations, real-time whole-body control, and agile locomotion combining force-based MPC with whole-body control [22].The paper identifies high system specificity as a major limitation of prior methods and says CI-MPC generalizes across systems while achieving comparable or better performance.Source: A. Model Predictive Control · p. 2
B. Complementarity-Based Contact Dynamics · III. BACKGROUND · A. Model Predictive Control
The background describes MPC as repeatedly optimizing a horizon-based plan for feedback, often through linearized dynamics and quadratic costs, and reviews LCP contact dynamics for simulation and planning. It also explains how contact models and optimization methods affect differentiability and numerical behavior.
B. Complementarity-Based Contact Dynamics
- LCP time-stepping enforces impact and friction constraints, with solutions computed by pivoting methods such as Lemke’s algorithm or by interior-point methods [23, 24, 25].Implementations include PATH and the Bullet and DART physics engines.Source: B. Complementarity-Based Contact… · p. 2
- Interior-point methods relax complementarity during iterations, enabling smooth implicitly differentiated gradients, whereas pivoting methods yield subgradients at nondifferentiable points [25, 29, 30].Other gradient approaches use auto-differentiation tools or select subgradients by analyzing the LCP solution [31].Source: B. Complementarity-Based Contact… · p. 2
- LCP contact dynamics support planning through collocation constraints that allow mode sequences to be chosen during optimization and rollout-based gradient optimization [31].Later collocation work added higher-order integrators and an exact ℓ1-penalty for complementarity constraints.Source: B. Complementarity-Based Contact… · p. 2
- MuJoCo’s soft-contact model trades physical realism for fast, reliable performance; other alternatives use a strictly convex quadratic program or relax LCP complementarity.The quadratic-program approach computes gradients by finite differences and is computationally less efficient; relaxing complementarity can improve numerical properties in some scenarios.Source: B. Complementarity-Based Contact… · pp. 2–3
III. BACKGROUND
- The background section covers MPC, complementarity-based contact dynamics, interior-point methods, and implicit differentiation.Source: §III · p. 3
A. Model Predictive Control
- MPC optimizes a finite-horizon objective subject to system dynamics, then repeatedly replans from the evolved state so high-rate control provides feedback.Applying controls from time-varying linearized dynamics and quadratic costs to the nonlinear system is effective, especially for reference tracking.Source: A. Model Predictive Control · p. 3
- To reduce online cost and unreliability, linear MPC tracks an offline reference using linearized dynamics and a quadratic objective; without added constraints, this is the LQR problem solved by Riccati recursion.The proxy approximates dynamics and costs about the reference trajectory [s6.p4#997ae3, s6.p5#9cf358, s6.p6#938aa4, s6.p7#518f26, s6.p8#75806e].Source: A. Model Predictive Control · p. 3
B. Complementarity-Based Contact Dynamics
The section formulates contact dynamics as a complementarity problem and partially linearizes the dynamics to obtain a linear complementarity problem (LCP). Its model includes contact forces, friction, and mappings between generalized motion, contact forces, and control inputs.
B. Complementarity-Based Contact Dynamics
- The model includes mass and dynamics bias, contact and input Jacobians, friction forces constrained by a linearized friction cone, and a signed-distance function.It describes a single contact point and generalizes to systems with multiple contacts.Source: B. Complementarity-Based Contact… · p. 3
- Semiimplicit Euler discretization, complementarity constraints, and maximum-dissipation friction terms encode contact switching, impact, and friction.Source: B. Complementarity-Based Contact… · p. 3
- Partial linearization of the dynamics produces an LCP for contact dynamics.The formulation evaluates terms at the next configuration; N is the derivative of signed distance with respect to configuration.Source: B. Complementarity-Based Contact… · p. 4
C. Interior-Point Method
The interior-point method solves LCPs by minimizing a residual while gradually driving a relaxed complementarity constraint toward zero.
C. Interior-Point Method
- Newton or quasi-Newton steps reduce a residual encoding robot mechanics, state, and environment properties, while backtracking preserves strict inequalities and each reduced-parameter subproblem warm-starts from the current solution.The process repeats until the central-path parameter, or complementary slackness, falls below a desired tolerance.Source: C. Interior-Point Method · p. 4
- Their predictor-corrector algorithm [45] modifies the Newton direction, typically halves convergence iterations, adapts the central-path parameter to prevent premature ill-conditioning, and improves reliability over prior relaxed-complementarity work.The authors report significantly improved convergence behavior in practice.Source: C. Interior-Point Method · p. 4
D. Implicit Differentiation · IV. CONTACT-IMPLICIT MODEL PREDICTIVE CONTROL
CI-MPC tracks systems that make and break contact using selectively approximated time-varying LCP dynamics, with a fast solver, smooth gradients, bi-level trajectory optimization, and a contact-height heuristic for uncertain terrain.
D. Implicit Differentiation
- Implicit differentiation computes gradients of contact dynamics by differentiating through the feasibility optimization problem.Source: D. Implicit Differentiation · p. 4
- At a stationary solution, the implicit-function theorem [29] yields sensitivity to problem data through a first-order expansion and a solve for the solution perturbation.Source: D. Implicit Differentiation · p. 4
- The differentiable interior-point method computes gradients at intermediate contact-dynamics results when the central-path parameter κ_grad is nonzero.Additional details on differentiating through intermediate results are cited in [30].Source: D. Implicit Differentiation · p. 4
IV. CONTACT-IMPLICIT MODEL PREDICTIVE CONTROL
- CI-MPC combines selectively approximated time-varying LCP contact dynamics with a fast solver for tracking systems that make and break contact.The dynamics are approximated about a reference trajectory.Source: §IV · p. 4
- The bi-level planning formulation uses these dynamics for direct trajectory optimization.The section also proposes a contact-height heuristic for environments with uncertain terrain.Source: §IV · p. 4
- A contact-height heuristic is proposed to help CI-MPC work well in environments with uncertain terrain.Source: §IV · p. 4
A. Time-Varying Contact Dynamics · B. Fast Contact Dynamics
The method uses reference-trajectory approximations to formulate time-varying LCP contact dynamics, then exploits their structure to accelerate online solution. The dynamics support planning over contact timing and forces by returning the next configuration and optionally current contact forces.
A. Time-Varying Contact Dynamics
- The LCP formulation uses nonnegative complementarity variables and slack variables to express its constraints for the interior-point solver.Its decision variables include configuration, contact-force variables, and slacks.Source: A. Time-Varying Contact Dynamics · p. 5
- Reference-trajectory approximations produce precomputed linear-system matrices for time-varying LCP dynamics, which return the next configuration and optionally current contact forces.The LCP is formulated for an interior-point method, with slack variables introduced for convenience.Source: A. Time-Varying Contact Dynamics · p. 5
B. Fast Contact Dynamics
- Because solving the interior-point method’s linear system is the most expensive step, the solver exploits sparsity and prefactorizes the mostly constant Jacobian structure offline.The Jacobian is partitioned by configuration, contact, and slack variables, and sparsity in its third row yields a condensed system.Source: B. Fast Contact Dynamics · p. 5
- The online policy uses a reference trajectory and fast contact dynamics generated offline, then repeatedly computes control and advances the system dynamics.The algorithm sets a planning horizon before the online loop.Source: B. Fast Contact Dynamics · p. 5
- The solver computes most of the condensed system offline, applies QR decomposition to its Schur complement, and obtains the remaining search direction with cheap vector operations.E^-1, GE^-1, and GE^-1F are also precomputed offline.Source: B. Fast Contact Dynamics · p. 5
- The structure-exploiting solver gives a 15× speed-up over LAPACK LU for LCP dynamics and a 2.5× speed-up for CI-MPC.Its online complexity is O(8c^3d^3).Source: B. Fast Contact Dynamics · p. 5
C. Gradients
Contact-dynamics gradients are obtained by differentiating through the LCP, with smooth gradients from intermediate interior-point results providing information through contact events.
C. Gradients
- Gradients differentiate the contact LCP with respect to θ = (q−−, q−, u), optionally including timestep, friction coefficients, masses, and inertia.Source: C. Gradients · p. 6
- Nonzero central-path values κ from intermediate interior-point iterates yield smooth gradients through contact events, unlike often-unhelpful subgradients at κ≈0.Larger κ produces smoother gradients; smaller κ more closely approximates a true subgradient at nondifferentiable points.Source: C. Gradients · p. 6
D. Planning
CI-MPC uses a bi-level planning formulation: direct trajectory optimization at the upper level is constrained by lower-level LCP dynamics. The planning objective and solver exploit sparse structure for efficient computation.
D. Planning
- At 25 Hz, CI-MPC stayed within the 0.04 s budget while MIQP exceeded real-time, and both policies regulated PushBot around equilibrium.The comparison reports mean and standard deviations for optimization times, but the supplied table passage does not provide the values.Source: Table I · p. 6
- The upper-level problem directly optimizes configurations and controls over horizon H, while lower-level LCP problems enforce dynamics with the first state fixed.The LCP constraints couple states across adjacent time steps.Source: D. Planning · p. 6
- A convex quadratic objective penalizes velocities using finite differences; the two-configuration state representation limits cost coupling to one time step, yielding a block-diagonal Hessian precomputed offline.Source: D. Planning · p. 6
- The KKT system uses a Gauss-Newton constraint-Hessian approximation and a sparse LDLT solver; experiments use QDLDL for its efficient implementation.Source: D. Planning · p. 7
E. Contact-Height Heuristic
The contact-height heuristic maintains an estimate for each contact and updates it when contact is detected, enabling adaptation to unknown terrain heights with little added computation. It was effective and reliable across unknown terrain in the tested systems.
E. Contact-Height Heuristic
- The policy maintains a height estimate for each contact and uses a modified signed-distance function to adapt to unknown terrain heights.When contact is detected, the estimate is updated; simulation uses an impact-force threshold, while practice uses force sensors to detect contact.Source: E. Contact-Height Heuristic · p. 7
- The heuristic was effective and reliable across unknown terrain in the tested systems, adding only c addition operations to compute r without changing the dynamics structure.It does not affect the structure of (23) and adds computation when evaluating the fast contact dynamics (22).Source: E. Contact-Height Heuristic · p. 7
F. Algorithm … A. Simulation
CI-MPC plans contact-rich behavior online, with results showing real-time operation, adaptation to new contacts, and robustness across simulated and hardware systems. The simulations demonstrate its use on multiple robots and terrains.
F. Algorithm
- The offline stage designs a reference trajectory and time-varying LCPs; online, CI-MPC replans over a specified horizon and applies the optimized control.The references are designed using contact-implicit trajectory optimization.Source: F. Algorithm · p. 7
G. Heuristics
- For real-time planning, CI-MPC returns the best solution after a fixed iteration budget, warm-starts from the reference or previous solution, and solves LCPs at κ ≈ 1e-4.In simulation verification, nonlinear contact dynamics are instead solved to convergence at κ = 1e-6.Source: G. Heuristics · p. 7
A. Simulation
- Simulations solve nonlinear contact complementarity to convergence and use dynamics sampled 5-10× faster than the reference; 100 perturbed initial conditions test hopper and quadruped robustness.The hopper recovers from significant orientation offsets, while the quadruped handles large drops.Source: A. Simulation · pp. 8–9
- After an impulse, PushBot discovers wall contact to stabilize or return to its nominal position; CI-MPC runs online unlike MIQP, whose complexity grows exponentially with contact modes.Warm-starting the MIQP may make it more amenable to online optimization.Source: A. Simulation · p. 8
- The hopper adapts across terrain heights of 0-24 cm and tracks a staircase and front flip, which the Raibert heuristic cannot directly execute as a non-periodic gait.The comparison covers flat, sinusoidal, and piecewise-linear terrains as well as the parkour stunt.Source: Table II · p. 7, A. Simulation · p. 8
- The planar quadruped tracks its nominal gait over varied terrain despite an unmodeled 3-kg payload, and the same policy transfers across quadruped scenarios without retuning.The payload represents 25% of nominal mass; the flat-ground reference gait adapts online to unmodeled terrain variation.Source: A. Simulation · p. 8, Figure 5 · p. 8, Figure 4 · p. 8
- Using the same policy without additional tuning, the biped walks on flat, sinusoidal, and piecewise-linear terrain and handles inclines up to ten degrees, versus Pratt’s reported five degrees.The comparison is with Pratt’s state-machine and proportional-derivative-controller policy.Source: A. Simulation · p. 9, Figure 6 · p. 9
B. Hardware
Hardware experiments on a Unitree Go1 quadruped demonstrate real-time CI-MPC tracking across periodic and non-periodic behaviors, including robustness to external disturbances. A simplified contact-implicit model enables the policy to generate new footstep sequences online.
Point-foot model
- The simplified 36-state point-foot model reduces online dynamics and derivative computation while allowing new footstep sequences to be generated online.It neglects leg dynamics and models three-dimensional forces applied to each foot; unlike fixed-contact-sequence quadratic-programming policies, CI-MPC reasons about new contact sequences.Source: B. Hardware · p. 9
Experimental setup
- A pre-tuned low-level controller tracks policy-specified foot forces, while CI-MPC, state estimation, and low-level control run on an Intel i9-12900KS computer.The controller and Kalman-filter state estimator run at 1000 Hz, with state estimates based on joint encoders, an onboard IMU, and motion capture.Source: B. Hardware · p. 9
Hardware behaviors
- CI-MPC ran at an average 100 Hz while tracking trotting, wall-standing, and sequential right-then-left step-placement behaviors on a Unitree Go1.The trotting example was designed to withstand large external disturbances; the wall behavior transitions from four feet on the ground to standing against the wall on two feet.Source: B. Hardware · p. 9
VI. CONCLUSION · A. Limitations · B. Future Work
CI-MPC uses fast differentiable contact dynamics for robust trajectory tracking through contact, while its conclusion identifies limitations in approximations, contact modeling, and reference generation alongside directions for future work.
A. Limitations
- Strategic contact-dynamics approximations enable offline gradient computations and partial matrix factorizations; tested planning horizons were typically 0.1–0.25 seconds, but longer horizons and highly dynamic hardware behaviors remain uncertain.Source: A. Limitations · p. 10
- CI-MPC computes smooth gradients using κgrad ≈1e-4 while simulating hard contact at κsim = 1e-6; planning uses softened contact, with sub-millimeter interpenetration, raising questions about hard-contact simulation.Source: A. Limitations · p. 10
- Contact-implicit trajectory optimization supplies references but converges unreliably; proposed alternatives include specialized solvers [54], one-step rollout methods, and learning-based approaches [5].Source: A. Limitations · pp. 10–11
- Classic convex MPC is more robust for general locomotion because it adds Raibert foothold selection; adding that strategy and using multithreaded C/C++ could improve CI-MPC’s speed and reliability.Source: A. Limitations · p. 11
VI. CONCLUSION
- CI-MPC presents fast differentiable contact dynamics for MPC and robustly tracks reference trajectories through contact despite disturbances, model mismatch, and uncertain environments.Source: §VI · p. 10, B. Future Work · p. 11
B. Future Work
- Future work proposes second-order friction-cone approximations for dynamic sliding-contact behaviors and extending CI-MPC from locomotion to manipulation, potentially with quasi-static models.Source: B. Future Work · p. 11
- The simplified quadruped planning model could extend to point-contact models for bipeds, humanoids, and potentially dexterous hands.Source: B. Future Work · p. 11
- A library of CI-MPC template behaviors could support more diverse online behavior, with a high-level agent composing templates in task-and-motion planning for complex long-horizon plans.Source: B. Future Work · p. 11