💻 computer science

ContactIPM: A Structure-Exploiting Interior-Point Solver for Contact-Implicit Trajectory Optimization

ContactIPM is a novel structure-exploiting interior-point solver that combines stagewise optimal-control factorization with primal-dual consistency to efficiently and robustly solve contact-implicit trajectory optimization problems, demonstrating significant speedups and improved success rates over existing contact-specific solvers.

Original authors: Yucheng Chen

Published 2026-08-13

📖 3 min read☕ Coffee break read

CC BY 4.0

Original authors: Yucheng Chen

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine a robot trying to push a heavy box across a room. To a human, this seems simple: grab, push, slide, stop. But to a robot's computer brain, it's a nightmare of math. The robot has to decide exactly when its hand touches the box, how hard to push, and when to let go, all while figuring out how to move its own joints. This is called "contact-implicit trajectory optimization." Think of it as asking a GPS to not just find a route, but to decide whether to drive on the road, walk on the sidewalk, or fly over a fence, all while calculating the perfect speed for every single second.

The tricky part is the "contact." In math, touching something is a weird condition called "complementarity." It's like a rule that says, "You can either be touching the box (force > 0) or not touching it (gap > 0), but you can never be doing both at the same time." This creates a mathematical dead-end where standard computer solvers get confused and stall, like a car trying to drive on a road that suddenly disappears. For a long time, scientists had to choose between two bad options: use a super-smart solver that gets stuck on the "touching" rules, or use a simpler solver that ignores the structure of the problem and takes forever to compute.

Enter ContactIPM, a new tool created by Yucheng Chen that acts like a master mechanic for these robot problems. Instead of choosing between speed and smarts, ContactIPM combines them. It treats the "touching" rules not as a hard, unbreakable wall, but as a stretchy, elastic band. This allows the computer to gently nudge the robot's plan through the tricky moments where it's deciding to touch or not touch, without getting stuck. Once the plan is found, it snaps back to the real, physical rules to make sure the robot actually works.

The paper shows that this approach is a game-changer. When tested against other top-tier robot solvers, ContactIPM was often 2 to 8 times faster. In one specific test involving a robot pushing a box, it solved the problem in just 2.08 milliseconds on average, even when the robot was confused by noise or errors. Most impressively, in a series of 50 difficult, real-world-like simulations where the robot had to recover from mistakes, ContactIPM succeeded in all 50 cases, whereas other methods sometimes gave up or failed to find a valid path. It proves that you don't have to sacrifice speed to get the robot to handle the messy, sticky reality of touching things.

Technical Summary: ContactIPM

Problem Statement

Contact-implicit trajectory optimization (CITO) seeks to optimize robot states, inputs, contact forces, and contact modes simultaneously without prescribing a contact sequence. This flexibility introduces Mathematical Programs with Complementarity Constraints (MPCCs), where unilateral contact and friction are modeled via complementarity conditions (e.g., $0 \le a \perp b \ge 0$ ).

These problems present a fundamental numerical challenge: at feasible MPCC points, the gradients of active nonnegativity and product constraints are structurally dependent, causing standard constraint qualifications (like LICQ) to fail. This degeneracy renders conventional primal–dual nonlinear programming solvers unreliable, often resulting in singular or ill-conditioned KKT systems.

Existing approaches address this robustness issue by abandoning the fully coupled primal–dual Newton step. Methods like CRISP optimize only the primal trajectory through convex subproblems, while IMPACT uses an augmented-Lagrangian outer loop with block-coordinate descent. Conversely, structure-exploiting optimal control solvers (e.g., acados) utilize Riccati recursions for efficiency but often fail to generate useful contact when exact complementarity is imposed without special treatment. There remains a gap between MPCC-specific robustness and the efficiency of structure-exploiting optimal control (OCP) solvers.

Methodology: ContactIPM

ContactIPM proposes a unified primal–dual interior-point method that retains the block-banded OCP structure required for Riccati recursion while handling complementarity constraints robustly.

1. Barrier-Coupled Elastic Interior Relaxation

Instead of using penalty weights or augmented Lagrangians, ContactIPM introduces a stage-local elastic interior relaxation for every marked complementarity pair $(a, b)$ :

Slack Variables: Positive side slacks ( $s_a, s_b$ ) and a product slack ( $s_c$ ) are introduced.
Elastic Lift: The complementarity condition is transformed into a system of equalities:
$g_a + s_a = 0, \quad g_b + s_b = 0, \quad g_a g_b - \theta(\mu) + s_c = 0$
where $\theta(\mu) = \gamma_{mpcc}\mu$ ties the complementarity tightening directly to the barrier parameter $\mu$ .
Regularity: This construction ensures the lifted constraint Jacobian contains an identity block with respect to the slacks. Consequently, the relaxed problem satisfies the Linear Independence Constraint Qualification (LICQ) at every interior point, guaranteeing unique multipliers and a regular primal–dual system.
No Independent Penalty: Unlike penalty methods, the tightening of complementarity is controlled by the same barrier parameter $\mu$ that defines the central path, eliminating the need for independently tuned penalty coefficients.

2. Stagewise Primal–Dual Elimination and Riccati Recursion

The solver maintains primal–dual consistency, updating the trajectory, dynamics multipliers, inequality multipliers, and slacks as components of a single perturbed KKT system.

Local Elimination: Because complementarity pairs and their elastic variables are local to individual stages, the solver eliminates the local slack ( $\Delta s_k$ ) and dual ( $\Delta \lambda_k$ ) variables stagewise.
Reduced System: This elimination modifies only the local stage Hessian ( $\tilde{H}_k$ ) and gradient ( $\tilde{q}_k$ ). The dynamics remain the sole coupling between adjacent stages, preserving the block-banded structure.
Efficient Solve: The reduced system is solved using a backward Riccati recursion followed by forward substitution, achieving linear complexity $O(N)$ with respect to the horizon length.

3. Multi-Phase MPCC Recovery

To handle the multiple stationary branches associated with different contact modes, ContactIPM employs a fixed recovery schedule involving:

Continuation: Gradually tightening the barrier parameter while attempting to stay in the current contact-mode basin.
Restart: If continuation fails (e.g., due to loss of conditioning), the solver restores the original primal guess and restarts with a larger barrier parameter and Gauss–Newton curvature to explore different basins.
Termination: The solver terminates only when the unrelaxed physical complementarity residual ( $r_{phys} = \max |a_k b_k|$ ) falls below a specified tolerance, ensuring the final trajectory satisfies the physical contact model, not just the relaxed formulation.

4. Scaling and Globalization

The method applies horizon-consistent diagonal coordinate scaling derived from stage derivative magnitudes. Globalization is achieved via a filter line search that balances objective reduction and constraint violation, with a fraction-to-boundary rule to maintain positive slacks.

Key Contributions

Unified Formulation: A stagewise formulation converting marked complementarity pairs into barrier-coupled elastic product rows within a nonlinear dynamics, cost, and inequality framework.
MPCC-Aware Primal–Dual Newton Method: A method that preserves the coupled perturbed KKT structure while utilizing local slack/dual elimination and Riccati recursion. It includes trajectory-scale preconditioning and a multi-phase recovery policy.
Reproducible Evaluation: A comprehensive experimental suite comparing ContactIPM against CRISP, IMPACT, and an OCP-structured acados baseline (with exact complementarity), including 50 closed-loop rollouts under various disturbances.

Experimental Results

Performance vs. CRISP

Speed: On four fixed benchmark cases (Cartpole, Push Box, Transport, Push T), ContactIPM is 2.17× to 8.87× faster than CRISP over 20 paired timing repetitions.
Robustness: ContactIPM matches CRISP on Cartpole and Transport but succeeds in 5 additional Push Box cases (24/25 vs. 19/25). On the Push T suite, ContactIPM succeeds in all 50 cases, whereas CRISP succeeds in only 27/50 when strict physical complementarity is enforced.

Performance vs. IMPACT

Speed: ContactIPM is 2.96× faster on Push T and 4.91× faster on Cart Transport. However, IMPACT is 4.46× faster on Push Box.
Robustness: ContactIPM succeeds in 150/150 cases across three 50-case suites (Push Box, Push T, Cart Transport), compared to IMPACT's 147/150.

Comparison with OCP-Structured Baseline (acados)

When acados is configured with exact complementarity (no elastic relaxation), it frequently fails to generate useful contact. For example, in the Push Box benchmark, acados passes physical feasibility checks in all 25 cases but 0/25 reach the terminal goal, often terminating at a zero-contact trajectory.
Across 165 contact-rich cases (CRISP Transport, CRISP Push T, IMPACT Push T, and IMPACT Cart Transport), acados produced only 1 accepted trajectory, while ContactIPM succeeded in 158. This demonstrates that exploiting OCP structure alone is insufficient to overcome MPCC degeneracy.

Closed-Loop Validation

In 50 closed-loop Push Box rollouts involving model mismatch, noise, and state resets, ContactIPM succeeded in all 50 cases.
Latency: The median solve time was 2.08 ms, with 99.37% of solves meeting a 100 ms control deadline.
Feasibility: The closed-loop feedback maintained primal feasibility and physical complementarity, with maximum physical complementarity products remaining below $1.01 \times 10^{-6}$ .

Significance and Claims

The paper claims that MPCC robustness, primal–dual consistency, and optimal-control structure can coexist in a single solver. ContactIPM demonstrates that the separation of complementarity handling from the primal–dual Newton step (as seen in CRISP and IMPACT) is not an unavoidable cost of contact complementarity. By combining an elastic interior relaxation with stagewise elimination and Riccati recursion, the solver achieves both the numerical robustness required for contact-implicit problems and the computational efficiency of structure-exploiting optimal control. The results suggest that specialized MPCC treatment is necessary even when using advanced OCP linear algebra techniques.