IMPACT: An Implicit Active-Set Augmented Lagrangian for Fast Contact-Implicit Trajectory Optimization

2026-05-09

AI Summary

This work addresses the numerical ill-conditioning and lack of efficient, scalable solvers in contact-implicit trajectory optimization (CITO) caused by complementarity constraints. We propose a novel approach that integrates an augmented Lagrangian method with an implicit active-set strategy to dynamically identify contact mode branches during trajectory optimization iterations. This is the first method to enable online handling of complementarity constraints while guaranteeing convergence to stationary points. Our custom C++ solver achieves speedups of 2.9–70× (13.8× geometric mean) over strong baselines on standard CITO benchmarks, significantly enhancing dexterous manipulation performance in contact-implicit model predictive control (CI-MPC). The approach is successfully validated on a real robotic system performing a T-shaped object pushing task.

Abstract

Contact-implicit trajectory optimization (CITO) has attracted growing attention as a unified framework for planning and control in contact-rich robotic tasks. Recent approaches have demonstrated promising results in manipulation and locomotion without requiring a prescribed contact-mode schedule. It is well known that the underlying mathematical programs with complementarity constraints (MPCCs) remain numerically ill-conditioned, and systematic, scalable solution strategies for CITO remain an active area of research. More efficient and principled solvers that can handle contact constraints are therefore essential to broaden the applicability of CITO. In this work, we develop an augmented-Lagrangian approach to CITO for solving MPCC-based CITO with stationarity guarantees. The method can be interpreted as identifying the implicit contact-mode branches on the fly during the trajectory optimization (TO) iterations; we call this approach IMPACT (IMPlicit contact ACtive-set Trajectory optimization). We provide an efficient C++ implementation tailored to trajectory-optimization workloads and evaluate it on the open-source CITO and contact-implicit model predictive control (CI-MPC) benchmarks. On CITO, IMPACT achieves 2.9x-70x speedups over strong baselines (geometric mean 13.8x). On CI-MPC, we show improved control quality for contact-rich trajectories on dexterous manipulation tasks in simulation. Finally, we demonstrate the proposed method on real robotic hardware on a T-shaped object pushing task.

Problem

• contact-implicit trajectory optimization
• mathematical programs with complementarity constraints
• numerical ill-conditioning
• scalable solvers
• trajectory optimization

Innovation

• contact-implicit trajectory optimization
• augmented Lagrangian
• complementarity constraints
• implicit active-set
• trajectory optimization