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

Venue: RSS 2026 (Sydney, Jul 13–17) · Session: Modeling and Optimization · paper #163 Authors: Jiayun Li, Dejian Gong, Georgia Chalvatzaki arXiv: 2605.09127 · program page

Summary compiled from the arXiv paper (v3); all numbers quoted from the paper. Trend context: RSS 2026 survey.

Top: an Allegro Hand reorients a rubber duck in simulation (initial → manipulation → final) under contact-implicit MPC. Bottom: a Panda robot pushes a T-shaped block to a target pose on real hardware; the red curve traces the block origin's trajectory during the push.

Problem

Contact-implicit trajectory optimization (CITO) avoids prescribing a contact schedule, but the underlying mathematical programs with complementarity constraints (MPCCs) violate standard constraint qualifications (LICQ/MFCQ fail, multipliers can be unbounded), making off-the-shelf NLP solvers numerically brittle and slow — the main obstacle to using CITO for planning and control in contact-rich tasks.

Method

IMPACT (TU Darmstadt PEARL Lab) is an augmented-Lagrangian method that keeps the original nonsmooth complementarity constraints and identifies the implicit active contact set on the fly during optimization, with stationarity guarantees (a computable KKT residual and ε-stationarity attainability result, Theorems in Secs. IV-V). The inner augmented-Lagrangian subproblems are solved by a structured block coordinate descent (BCD) that exploits variable-wise complementarity, with backtracking line search and componentwise clipping; the whole pipeline is implemented in C++ with CasADi symbolic differentiation, tailored to TO/MPC workloads.

Results

On the CRISP CITO benchmark (Push Box, Cart Transport, Push T; horizons T = 50-300, 50 random goals each, all-zero initialization): IMPACT is 16.8×/25.0×/34.0× faster than CRISP (geometric mean 24.3×) and 5.7×/2.9×/25.0× faster than the penalty method, spanning an overall 2.9×-70× speedup range (geometric mean 13.8×) at competitive tracking error and 100% success where some baselines fail (penalty method: 90% on Push T, 98% on Push Box). On the MuJoCo Allegro-Hand CI-MPC benchmark (17 objects), IMPACT achieves success comparable to the smoothed cfree baseline (cfree avg 91.2 ± 4.2%) while improving control-quality metrics (variance/smoothness/effort), running ~9.5 Hz vs cfree's ~50-54 Hz on the same PC. A hardware Push-T demonstration on a Panda closes the loop from solver to real robot.

Significance

Makes rigid-complementarity CITO an order of magnitude faster without smoothing away the contact structure — model-based infrastructure that complements learning-heavy approaches to contact-rich manipulation in Review-Dexterous-Manipulation.

← Back to RSS 2026 survey · RSS-2026-Papers · Home