Inverse Dynamics Trajectory Optimization for Contact-Implicit Model Predictive Control

TL;DR

IDTO reframes contact-rich trajectory optimization as a nonlinear least-squares problem using generalized positions as the sole decision variables, enabling real-time CI-MPC for complex robots. A compliant contact model with regularized friction, combined with a Gauss-Newton trust-region solver and a sparse Hessian, yields fast iterations and robustness to local minima. The method is validated across simulation and hardware on spinner, a quadruped, a dexterous hand, and bi-manual manipulation, achieving real-time MPC rates up to hundreds of Hz in some cases. The work provides an open-source IDTO solver, discusses practical limitations, and outlines avenues for improving analytical derivatives, contact discovery, and integration with higher-level planning.

Abstract

Robots must make and break contact with the environment to perform useful tasks, but planning and control through contact remains a formidable challenge. In this work, we achieve real-time contact-implicit model predictive control with a surprisingly simple method: inverse dynamics trajectory optimization. While trajectory optimization with inverse dynamics is not new, we introduce a series of incremental innovations that collectively enable fast model predictive control on a variety of challenging manipulation and locomotion tasks. We implement these innovations in an open-source solver and present simulation examples to support the effectiveness of the proposed approach. Additionally, we demonstrate contact-implicit model predictive control on hardware at over 100 Hz for a 20-degree-of-freedom bi-manual manipulation task. Video and code are available at https://idto.github.io.

Figures (16)

• Figure 1: IDTO enables real-time CI-MPC on a variety of challenging manipulation and locomotion tasks. Contact sequences, locations, and timings are all determined automatically by the solver over a 1-2 second horizon.
• Figure 2: Visualization of our compliant contact model \ref{['eq:compliance_model']}-\ref{['eq:regularized_friction_model']} for various parameter values. This model is differentiable everywhere, but introduces force-at-a-distance (\ref{['fig:stiffness']}) and drift-during-stiction (\ref{['fig:friction']}) artifacts.
• Figure 3: Convergence plot for the Allegro hand example with and without scaling.
• Figure 4: Visual (left) and collision (right) geometries for the bi-manual manipulation task. We eliminate sharp corners by modeling the box with inscribed spheres. Additionally, we only include arm collision geometries near the end-effectors. IDTO is able to overcome the resulting modeling error in both simulations (using box collisions) and hardware experiments.
• Figure 5: Convergence with quadratic penalty and Lagrange multipliers for each of the four examples.

Theorems & Definitions (3)

• Remark 1
• Remark 2
• Remark 3