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

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.

• The paper introduces IMPACT, a safeguarded augmented Lagrangian solver that enforces original complementarity constraints for robust contact-implicit trajectory optimization.
• It employs a structured block coordinate descent for efficient contact-mode discovery, achieving up to 70× speedup over baseline methods.
• IMPACT is validated in simulation and hardware, demonstrating improved control quality, reduced effort, and smoother operation in multi-contact robotic tasks.

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

Overview

The paper introduces IMPACT, a solver architecture for contact-implicit trajectory optimization (CITO) leveraging a safeguarded augmented Lagrangian (AuLa) method for mathematical programs with complementarity constraints (MPCCs). IMPACT targets numerically robust, efficient optimization in multi-contact, hybrid-dynamics robotic tasks by avoiding the typical smoothing or relaxation of contact complementarity. Instead, it implements a structured block coordinate descent (BCD) that enforces original nonsmooth constraints and facilitates efficient contact-mode discovery during trajectory optimization. The method is validated both in simulation and real hardware, with substantial computational speedups and improved control quality in challenging dexterous manipulation and pushing problems.

Figure 1: IMPACT demonstrations in simulation and hardware. Top: Allegro Hand reorients a rubber duck in simulation. Bottom: Panda robot pushes a T-shaped block to a target pose in real-robot experiments; the red curve shows the block's coordinate origin trajectory.

Contact-Implicit Trajectory Optimization and MPCCs

CITO unifies motion planning and control across contact-rich tasks without a prescribed schedule for contact modes, overcoming the hybrid discrete-continuous structure inherent to manipulation and locomotion. Trajectories are optimized subject to contact constraints encoded as complementarity—a requirement that typically leads to MPCCs, which are computationally challenging due to degeneracy and ill-conditioning near feasible contact-mode boundaries. Standard NLP solvers are brittle for MPCCs, suffering from convergence sensitivity and unbounded multipliers [scheel2000mathematical, nurkanovic2023solving].

Numerical treatments such as smoothing, relaxation, or penalty-based reformulation improve convergence but often degrade fidelity to rigid contact dynamics or introduce heuristic parameter dependencies. The IMPACT framework circumvents these pitfalls by preserving original complementarity via explicit enforcement in subproblems structured within a safeguarded AuLa outer loop.

Figure 2: Comparison of complementarity-handling methods. Top row: Scholtes relaxation and squared-penalty method. Bottom row: IMPACT, showing the primal update and the slack update. Slack variables regularize the next primal update, and slack is projected onto the complementarity set.

Methodology: Safeguarded AuLa and Structured BCD

IMPACT reformulates CITO/CI-MPC as an MPCC over generalized trajectories. The complementarity forms are split using slack variables and enforced explicitly, allowing robust AuLa treatments on the smooth equality and inequality constraints. The outer-loop (AuLa) uses safeguarded multiplier and penalty updates, preventing pathological multiplier blow-up and providing stationarity guarantees under standard MPCC assumptions [guo2022new, jia2023augmented].

The inner solver uses a two-block coordinate descent:

• Trajectory variables $X$ update: Gauss-Newton optimization (with globalization via Armijo line search) on the least-squares objective augmented with penalty and multiplier terms.
• Slack variables $(Y, Z)$ update: Closed-form minimization over the union of convex cones $0 \leq Y \perp Z \geq 0$, driven by the current multiplier and penalty settings. This is not merely feasibility-driven projection but includes multiplier-induced shifts (cf. ADMM), yielding stronger progress per iteration.

IMPACT’s design ensures attainment of $\epsilon$-stationarity of the KKT residual for AuLa subproblems within finitely many inner iterations, under weak regularity and sufficient decrease assumptions for Gauss-Newton updates.

Performance Evaluation and Empirical Results

IMPACT is benchmarked on two classes of problems: long-horizon CITO (Push Box, Cart Transport, Push-T) and high-dimensional CI-MPC for Allegro-Hand manipulation over 17 objects.

Long-Horizon CITO

IMPACT is compared against strong baselines: Scholtes relaxation, squared penalty, and CRISP. Problem scales span up to 2404 decision variables, 2150 complementarity pairs, and 1200 constraints.

Figure 3: IMPACT planning demos on three CITO tasks. Start and goal boxes, contact force arrows, and object trajectories are annotated for visual clarity.

IMPACT achieves $2.9\times$–$70\times$ speedup (geometric mean $13.8\times$) over baselines, with competitive tracking error and $100\%$ success rates under identical termination criteria. Compared to CRISP, IMPACT is $16.8\times$, $25.0\times$, and $(Y, Z)$0 faster on Box, T, and Cart. All-zero initialization is used to avoid warm-start biases. Wall-clock time is not strictly proportional to iteration count due to varying inner-subproblem conditioning across methods.

Real-Hardware Demonstration

IMPACT is deployed for the Push-T task on a Panda robot, achieving successful execution in all trials by replanning as necessary in response to trajectory deviations, demonstrating robustness beyond simulation.

Allegro-Hand CI-MPC Benchmark

For dexterous multi-contact in-hand manipulation, IMPACT solves real-time CI-MPC with up to 20 simultaneous contacts, matching the cfree pipeline's success rates while delivering lower control variance, improved smoothness, and reduced effort across 17 diverse objects (Table aggregate metrics):

• Variance: $(Y, Z)$1 (IMPACT) vs $(Y, Z)$2 (cfree)
• Smoothness: $(Y, Z)$3 (IMPACT) vs $(Y, Z)$4 (cfree)
• Effort: $(Y, Z)$5 (IMPACT) vs $(Y, Z)$6 (cfree)

IMPACT operates at $(Y, Z)$7 Hz, which is below cfree’s $(Y, Z)$8–$(Y, Z)$9 Hz but is sufficient for experimental manipulation. Contact-mode stability can be sporadically affected under tight compute budgets; augmenting stabilization via trust-region or hysteresis mechanisms is an active direction.

Figure 4: Allegro-Hand CI-MPC benchmark results on 17 objects, comparing IMPACT and cfree methods across success and control-quality metrics.

Numerical Validation and Practical Stopping Rules

IMPACT’s empirical stationarity using stagnation stopping matches theoretical KKT residual predictions, with log-log correlation $0 \leq Y \perp Z \geq 0$0 between objective decrease and residual norm. Practical objective stagnation thresholds reliably ensure $0 \leq Y \perp Z \geq 0$1-stationarity.

Figure 5: Empirical validation of the stagnation-based stopping criterion on Push-T. Strong correlation between $0 \leq Y \perp Z \geq 0$2-block residual and $0 \leq Y \perp Z \geq 0$3 across 15,139 iterations.

Limitations

Contact-mode stability under limited computation is a known challenge; IMPACT can exhibit transient contact-set switching before contacts are firmly established, particularly in high-dimensional CI-MPC regimes. Planning remains local and may require contact-encouraging cost terms, which induce sensitivity to hyperparameters, notably for slender geometries like the stick object. These limitations motivate integration of offline guidance (RL) and global geometric encodings to reduce reliance on hand-crafted costs.

Conclusion

IMPACT establishes a robust, theoretically justified framework for fast contact-implicit trajectory optimization by explicit enforcement of complementarity in a safeguarded augmented Lagrangian scheme. It achieves substantial computational gains and improved control quality in both simulation and real-robot manipulation, maintaining stationarity guarantees for MPCCs. The architecture is scalable from offline planning to closed-loop MPC, and its performance implications extend to multi-contact dexterous manipulation and locomotion optimization tasks. Future directions include enhanced contact-mode stabilization, integration of global guidance signals, and application to broader classes of contact-rich robotic control.

Figure 6: Rollout trajectory from $0 \leq Y \perp Z \geq 0$4 to $0 \leq Y \perp Z \geq 0$5 for the Pushing Box task.

Figure 7: Rollout from $0 \leq Y \perp Z \geq 0$6 to $0 \leq Y \perp Z \geq 0$7 for the Push T task.

Figure 8: Rollout sequence for Cart Transporter from $0 \leq Y \perp Z \geq 0$8 to $0 \leq Y \perp Z \geq 0$9.

Figure 9: Allegro in-hand re-orientation result on the Airplane object.

Figure 10: Allegro in-hand re-orientation result on the Camera object.

Figure 11: Allegro in-hand re-orientation result on the Foam brick.

Figure 12: Allegro in-hand re-orientation result on the Teapot.

• "IMPACT: An Implicit Active-Set Augmented Lagrangian for Fast Contact-Implicit Trajectory Optimization" (2605.09127)
• "Safeguarded Augmented Lagrangian Method for MPCCs" [guo2022new]
• "Complementarity-Free Multi-Contact Modeling and Optimization for Dexterous Manipulation" [jin2024complementarity]
• "On the Surprising Robustness of Sequential Convex Optimization for Contact-Implicit Motion Planning" [li2025surprising]
• "Mathematical Programs with Complementarity Constraints: Stationarity, Optimality, and Sensitivity" [scheel2000mathematical]
• "Contact models in robotics: A comparative analysis" [le2024contact]