Contact-Implicit Trajectory Optimization

• Contact-implicit trajectory optimization is a framework that embeds contact events and forces into the robot dynamics for automatic discovery of contact sequences.
• It employs both hard complementarity constraints and smooth contact models, combined with advanced numerical algorithms to ensure physical realism and scalability.
• Applications span legged locomotion, dexterous manipulation, and multi-agent collaboration, demonstrating enhanced control and real-time performance.

Contact-implicit trajectory optimization (CITO) is a framework for automatically synthesizing trajectories for robotic systems that must interact dynamically with their environments through contact, without requiring a priori specification of the sequence, locations, or timing of contact events. CITO formulates trajectory optimization problems such that contact forces, contact events, and robot controls are all treated as decision variables within a single mathematical program, with physical constraints imposed via complementarity—a mathematical formalism that enforces mutually exclusive alternatives, such as “either contact force is zero or the bodies are not in contact.” This approach facilitates the discovery and optimization of complex contact-rich behaviors in high-dimensional systems, such as legged locomotion, whole-arm and dexterous manipulation, and multi-object collaborative tasks. Recent advances have addressed challenges of smoothness, scalability, and physical realism through sophisticated contact models, algorithmic solvers, and numerical transcription schemes (Kurtz et al., 2022, Kurtz et al., 2023, Salagame et al., 4 Aug 2025, Zhang et al., 2024, Shorinwa et al., 2024, Chatzinikolaidis et al., 2020, Buckner et al., 11 Apr 2026, Howell et al., 2022, Li et al., 9 May 2026, Zhang et al., 2023, Chatzinikolaidis et al., 2021, Onol et al., 2020, Onol et al., 2018, Turski et al., 2023, Sleiman et al., 2021, Onol et al., 2018, Wang et al., 2022, Patel et al., 2018, Doshi et al., 2019, Chen et al., 2021).

1. Mathematical Formulation and Contact Models

In CITO, the state, control, and contact decision variables are embedded into a direct transcription of the robot’s dynamics. For a general articulated robot:

$$M(q) \dot v + C(q,v) v + g(q) = S^T u + \sum_c J_c(q)^T f_c$$

where $q, v, u$ are the robot's generalized positions, velocities, and actuator torques, $J_c$ is the contact Jacobian, and $f_c$ is the contact wrench at patch $c$. Discretized dynamics yield $x_{k+1} = f(x_k, u_k)$, where all contact impulses and torques are treated algebraically (Kurtz et al., 2022).

Contact models in CITO fall into two principal categories:

• Hard contact/complementarity models explicitly impose non-penetration ($\phi(q)\geq 0$), unilaterality ($f_n\geq 0$), and complementarity ($\phi(q)f_n=0$), generalized to friction via cone constraints and additional complementarity for stick-slip transitions (Sleiman et al., 2021, Salagame et al., 4 Aug 2025, Patel et al., 2018).
• Smooth or regularized models approximate complementarity with smooth mappings, such as pressure-field models (e.g., hydroelastic contact $p(X) = E_h \delta(X) + D_h \dot\delta(X)$ (Kurtz et al., 2022)), smoothed compliance laws $q, v, u$0 (Kurtz et al., 2023), or variable smooth contact models where contact “stiffnesses” become decision variables (Onol et al., 2018, Onol et al., 2020, Onol et al., 2018).

These models allow CITO to represent both the discrete and continuous aspects of contact mechanics within a continuous optimization problem, yielding a mathematical program with complementarity constraints (MPCC) or a relaxed smooth nonlinear program.

2. Computational Algorithms and Solvers

Solving CITO problems requires specialized numerical strategies due to their nonconvexity, nonsmoothness, and scale. Central algorithmic elements include:

• Direct transcription or multiple-shooting discretizes the state, control, and—in some schemes—contact variables, for optimization over a fixed time grid (Sleiman et al., 2021, Salagame et al., 4 Aug 2025, Kurtz et al., 2022).
• Implicit and explicit Differential Dynamic Programming (iLQR, DDP) variants have been extended to include contact via pressure-field models and implicit sensitivity analysis for efficient second-order updates, leveraging closed-form or QP-based contact solves (Kurtz et al., 2022, Chatzinikolaidis et al., 2021, Onol et al., 2018).
• Successive Convexification (SCvx/SCP) approaches iteratively linearize nonlinear constraints and cost, solving a sequence of convex programs subject to trust-regions and slack-penalization, often embedding smoothed contact models (Buckner et al., 11 Apr 2026, Onol et al., 2018).
• Augmented Lagrangian and ADMM methods (e.g., IMPACT, CALIPSO, DisCo) retain hard or relaxed complementarity by alternating between primal and dual updates, facilitating robust convergence for MPCCs and enabling distributed or bi-level optimization (Li et al., 9 May 2026, Howell et al., 2022, Shorinwa et al., 2024).
• Infinite-programming/exchange frameworks, such as STOCS, dynamically instantiate only a small set of candidate contacts at each time step, enabling scalability to high-fidelity 3D geometries (Zhang et al., 2024, Zhang et al., 2023).

Recent implementations exploit GPU acceleration and just-in-time (JAX) differentiation for real-time or large-scale tasks (Buckner et al., 11 Apr 2026, Kurtz et al., 2023). Solvers target reliable computation of sensitivities (for bi-level learning (Howell et al., 2022)), distributed collaborative robots (Shorinwa et al., 2024), and hardware realism via validated manipulation and locomotion (Kurtz et al., 2022, Kurtz et al., 2023, Shorinwa et al., 2024, Sleiman et al., 2021, Doshi et al., 2019).

3. Physical Fidelity and Scalability

Physical realism in CITO depends crucially on the contact model’s capacity to represent compliance, dissipation, and friction phenomena:

• Hydroelastic contact models compute differentiable pressure fields over overlap surfaces, yielding $q, v, u$1 with regularized friction, resulting in smooth (C¹) dynamics through contact transitions (Kurtz et al., 2022).
• Analytic compliance models parameterize stiffness and damping, enabling a continuous spectrum between rigid (large $q, v, u$2), soft (small $q, v, u$3), and slippery (small $q, v, u$4), and afford closed-form solutions that eliminate complementarity constraints entirely (Chatzinikolaidis et al., 2020).
• Integral cross-complementarity constraints (ci-SCvx) in continuous-time formulations guarantee that contact events are not missed between mesh points, enforcing complementarity in an integral sense and enabling low grid resolution without loss of event accuracy (Buckner et al., 11 Apr 2026).
• Multi-stage and staged optimization frameworks (e.g., Staged Contact Optimization) combine reduced-order contact-implicit trajectory generation (centroidal CIO) to discover contact sequences, then enforce fixed contacts in robust hybrid trajectory optimization for full-order states (Turski et al., 2023).

Scalability has advanced via active-set selection (IMPACT), infinite-programming approaches (STOCS), and aggressive numerical acceleration on modern hardware, enabling high-fidelity whole-body planning and distributed multi-robot coordination (Zhang et al., 2024, Shorinwa et al., 2024, Li et al., 9 May 2026, Buckner et al., 11 Apr 2026).

4. Benchmark Problems and Applications

CITO methods have been demonstrated on a broad spectrum of robotic tasks:

• Legged locomotion: automatic gait discovery for quadrupeds (Mini Cheetah, ANYmal) in trotting, running, and jumping, with accurate timing and force profiles under varying ground properties (Kurtz et al., 2022, Kurtz et al., 2023, Chatzinikolaidis et al., 2020, Buckner et al., 11 Apr 2026, Patel et al., 2018, Doshi et al., 2019, Turski et al., 2023).
• Whole-arm and dexterous manipulation: contact-rich manipulation with Kinova and Allegro arms, including whole-arm lifting, rolling, and sliding, with transfer to hardware (Kurtz et al., 2022, Kurtz et al., 2023, Zhang et al., 2024, Wang et al., 2022, Chen et al., 2021).
• Snake robot undulation: optimal kinematics and stick-slip transitions in reduced-order and full-body models, matching high-fidelity simulation and experimental hardware within 10–15% (Salagame et al., 4 Aug 2025).
• Distributed multi-agent collaboration: DisCo demonstrates team-based manipulation, modular locomotion, and team sports, with distributed optimization yielding 2.5×–5× faster computation and 3× higher success rates versus centralized baselines (Shorinwa et al., 2024).
• Dynamic object manipulation and non-prehensile pushing: CITO computes dynamic push, spin, and lift strategies with accurate impact timing and frictional transitions, validated in both simulation and hardware (Sleiman et al., 2021, Wang et al., 2022).

Benchmarks consistently demonstrate automatic contact-sequence discovery, multi-phase contact transitions (rolling/sliding), and robustness to variations in model parameterization and mesh resolution.

5. Numerical Performance, Limitations, and Design Insights

Key insights and best practices for CITO include:

• Parameter tuning: Stiffness/dissipation in soft/contact models ($q, v, u$5) trades off realism and numerical conditioning. Moderately compliant values often promote smooth gradients and solver convergence, but overly soft values induce spurious tunneling (Kurtz et al., 2022, Chatzinikolaidis et al., 2020).
• Time discretization: Fine time steps ($q, v, u$6) accurately capture contact dynamics, while larger steps risk missing events or failing tampered solvers (Kurtz et al., 2022, Buckner et al., 11 Apr 2026).
• Solver iteration behavior: Modern iLQR and SCvx-based CITO often achieve convergence in 5–40 iterations per problem window, but computation is dominated by automatic differentiation of dynamics through contact. Efficient analytic derivatives and structure-exploiting linear algebra are critical for scalability (Kurtz et al., 2022, Buckner et al., 11 Apr 2026, Kurtz et al., 2023, Li et al., 9 May 2026).
• Active-set and warm-start: Implicit active-set identification (IMPACT) and warm-starting from prior solves/warm contact patterns improve mode selection stability and optimization runtime (Li et al., 9 May 2026, Onol et al., 2020, Zhang et al., 2024).
• Scalability and contact complexity: Methods that dynamically select or grow an active set of contacts (e.g., STOCS with oracle-based selection) enable practical optimization for objects represented by tens of thousands of mesh vertices (Zhang et al., 2024).

Limitations persist in local minima robustness, the treatment of sharp contact geometries, solver convergence under high parameter stiffness, and real-time rates for systems exceeding 10–20 DOFs, though aggressive parallelization and code generation have yielded >10× speedups in recent literature (Buckner et al., 11 Apr 2026).

6. Directions for Future Research

Open research areas in CITO span:

• Analytic and structure-exploiting derivatives for faster gradient computation through high-fidelity contact (Buckner et al., 11 Apr 2026, Kurtz et al., 2023).
• Global planning/meta-optimization: Integration with sampling-based, graph-based, or hierarchical planners for multi-modal and multi-contact tasks (Zhang et al., 2023, Chen et al., 2021).
• Closed-loop and real-time control: Embedding CITO as inner loops for model-predictive control (MPC) and reinforcement learning-based outer loops (Kurtz et al., 2023, Howell et al., 2022, Li et al., 9 May 2026).
• Uncertainty and robust optimization: Handling model parameter uncertainty, sensor noise, and friction variability (Zhang et al., 2024, Salagame et al., 4 Aug 2025).
• Hardware validation and sim-to-real transfer: Continued emphasis on aligning simulated contact models with hardware through model identification and validation experiments (Kurtz et al., 2022, Kurtz et al., 2023, Shorinwa et al., 2024, Doshi et al., 2019).

Contact-implicit trajectory optimization continues to advance rapidly, providing a foundation for autonomous synthesis of contact-rich behaviors in diverse domains from mobile manipulation to multi-robot collaboration, and is increasingly supported by robust numerical algorithms and open-source software (Kurtz et al., 2022, Kurtz et al., 2023, Buckner et al., 11 Apr 2026, Howell et al., 2022, Zhang et al., 2024).