Contact-Implicit Trajectory Optimization (CITO)

• CITO is a framework that generates dynamically feasible trajectories by implicitly modeling contact forces, friction, and make-break events in robotic systems.
• It uses complementarity constraints, collocation methods, and Bézier parameterizations to enforce dynamic consistency and friction cone constraints.
• The approach enables automatic discovery of contact sequences for complex tasks like legged locomotion, validated by improved tracking accuracy and reduced errors.

Contact-Implicit Trajectory Optimization (CITO) is a unified mathematical and algorithmic framework for generating dynamically feasible trajectories for robotic systems performing motions involving intermittent contact with their environment. CITO captures both the continuous robot dynamics and the non-smooth, hybrid aspects of contact interactions by formulating an optimal control problem that implicitly models contact forces, friction, and switching (make–break events) as part of a large-scale nonlinear program, typically employing complementarity or smooth surrogate constraints to enforce contact physics without prescribing a contact sequence. This approach enables the discovery of complex whole-body behaviors, such as locomotion or manipulation with contact switches, directly from trajectory optimization.

1. Mathematical Foundations and Problem Formulation

The core CITO problem is posed as a nonlinear program (NLP) or, more generally, as a Mathematical Program with Complementarity Constraints (MPCC), where the decision variables include the robot’s states, controls, and contact force magnitudes at each time step. The dynamics for a floating-base or articulated robot are encoded via discrete-time updates (e.g., trapezoidal, variational, or orthogonal collocation), and the variational principle or rigid-body equations produce constraints such as

$$M(q)\,\ddot q + C(q, \dot q) = S^T \tau + J(q)^T \lambda,$$

where $q$ are generalized positions, $M$ is the mass matrix, $C$ collects Coriolis, centrifugal, gravitational terms, $S^T$ maps torques, $J(q)$ is the contact Jacobian, and $\lambda$ is the contact force vector (Chen et al., 2021, Doshi et al., 2019, Sleiman et al., 2021, Kim et al., 28 Oct 2025).

The central challenge arises from enforcing unilateral contact and friction physics:

• Non-penetration: $\phi(q) \ge 0$, where $\phi(q)$ is the signed distance to contact.
• Unilateral force: $\lambda_n \ge 0$ (no pulling contact).
• Complementarity: $q$0 (force iff contact).
• Friction cone: $q$1.

These constraints are encoded either as hard equalities in the MPCC, slack-penalized inequalities, or by smooth surrogate models (e.g., soft “virtual” springs or analytically eliminated via minimal models) (Chatzinikolaidis et al., 2020, Onol et al., 2018, Onol et al., 2020, Onol et al., 2018).

Typical CITO problem formulations may also include trajectory tracking, control effort, or force regularization in the objective, and enforce kinematic or boundary constraints as required by the task (Chen et al., 2021, Kim et al., 28 Oct 2025).

2. Discretization, Trajectory Parameterization, and Contact Modeling

Time discretization is standard in CITO and underpins the numerical treatment:

• Collocation methods (trapezoidal, Radau, orthogonal): States and controls are parameterized at discretization “knots,” and integration consistency is enforced through collocation equations. Higher-order collocation delivers improved accuracy, particularly vital for contacts with sharp transitions (Patel et al., 2018, Turski et al., 2023, Kim et al., 28 Oct 2025).
• Bézier polynomials: Used as a high-order basis for parameterizing both foot trajectories and contact forces, enabling explicit enforcement of friction cones and dynamic constraints throughout the phase (Kim et al., 28 Oct 2025).
• Multiple-shooting: Segmental discretization with auxiliary variables at boundaries can accelerate convergence and provide improved conditioning for long horizons (Sleiman et al., 2021).

Contact modeling in CITO varies:

• Rigid/complementarity-based: Non-smooth complementarity constraints for hard contact (Chen et al., 2021, Patel et al., 2018, Kim et al., 28 Oct 2025).
• Smooth/relaxed models: Virtual smooth force models (eg., exponentials of distance; “variable smooth contact models”) to regularize non-differentiability for gradient-based optimization, often used in combination with penalty/annealing strategies (Onol et al., 2018, Onol et al., 2018, Onol et al., 2020).
• Analytically explicit models: Closed-form contact force expressions parameterized by compliance/stiffness/friction, eliminating auxiliary force variables and reducing decision space, with friction handled implicitly (Chatzinikolaidis et al., 2020).

3. Phase Decomposition, Contact Sequence Discovery, and Dynamics Consistency

A hallmark of CITO is the implicit discovery of contact schedules:

• Implicit sequencing: Contacts are made/broken as a result of optimization dynamics and constraints, rather than pre-specified.
• Phase-based decomposition: For locomotion, problem structure may be decomposed by stance/swing phases and further subdivided (e.g., each leg alternates stance and swing), with durations optimized and constraints linking boundary conditions (Kim et al., 28 Oct 2025, Turski et al., 2023).
• Translational dynamics superposition: The translational dynamics are decoupled and integrated analytically for each contact phase, leveraging Bézier polynomials to ensure consistency throughout the entire trajectory (Kim et al., 28 Oct 2025).
• Friction cone enforcement: The convex hull property of Bézier parameterizations guarantees compliance with friction cone constraints throughout the trajectory by requiring all Bézier control points to reside within the cone (Kim et al., 28 Oct 2025).

4. Solver Architectures and Algorithmic Features

CITO frameworks leverage modern nonlinear programming tools and custom solver strategies for tractability and scalability:

• Sparse NLP solvers: IPOPT, SNOPT, or qpSWIFT are typically employed to solve large-scale, structured problems with thousands of variables and constraints (Kim et al., 28 Oct 2025, Patel et al., 2018, Turski et al., 2023).
• SQP (Sequential Quadratic Programming): Used for problems where analytic gradients and Hessians (Gauss-Newton) can be efficiently computed (Kim et al., 28 Oct 2025).
• Direct multiple-shooting: Decomposes the problem into smaller segments with continuity enforced at boundaries, facilitating improved performance on long horizons (Sleiman et al., 2021, Kim et al., 28 Oct 2025).
• Complementarity relaxation: Slack variables and soft penalty terms are introduced to soften hard complementarity, and are annealed over the course of the solve or stage-wise in continuation schemes (Turski et al., 2023, Kim et al., 28 Oct 2025).

5. Experimental Results and Validation

The paradigm has been applied to a variety of complex contact-rich robotic problems:

• Legged locomotion: Generation of dynamically consistent quadruped gaits (flat terrain, stairs, obstacles) with negligible violation of translational and friction cone constraints. The approach reduces angular-dynamics inconsistencies by a factor of four compared to baseline methods (TOWR) and achieves better tracking errors under model predictive control execution (Kim et al., 28 Oct 2025).
• Motion generation for various gaits: Can synthesize a diversity of gait patterns (e.g., walking, running) for quadrupeds without prior specification, discovering appropriate footstep timing and force profiles (Kim et al., 28 Oct 2025, Turski et al., 2023).
• Computation times: Mean solve times for quadrupedal motion planning are on the order of 7–8 s (comparable to or slightly slower than trajectory optimization frameworks with fixed contact schedules, but with the benefit of dynamic consistency and contact-sequence flexibility) (Kim et al., 28 Oct 2025).

Quantitative outcomes demonstrate near-zero violation of hard physical constraints and successful motion generation across diverse terrain and gait requirements.

6. Extensions, Advantages, and Limitations

CITO frameworks with direct contact-point decomposition and Bézier parameterization provide a range of benefits:

• Dynamically consistent translation: Analytical enforcement of translational dynamics avoids the approximation errors and segment inconsistency of shooting/collocation-only approaches (Kim et al., 28 Oct 2025).
• Global frictional consistency: The continuous satisfaction of friction cones over time via control-point constraints eliminates infeasibility present in discretization-only approaches.
• Contact sequence flexibility: No need for a priori contact schedule enumeration; applicable to arbitrary environments and complex contact strategies (Kim et al., 28 Oct 2025, Turski et al., 2023).
• Scalability: Efficient handling of hundreds of variables and constraints; mean solve times suitable for motion planning but not (yet) for real-time control on high-DOF robots.

However, limitations remain:

• Computational expense: Although improved over naive collocation, CITO remains more computationally expensive than trajectory optimization with fixed contact schedules.
• Local minima and initialization: The nonconvexity of the overall NLP may lead to inferior solutions if poorly initialized.
• Model idealizations: The method assumes rigid contact, single-rigid-body dynamics approximation for translation, and piecewise-constant force/trajectory segments.

7. Context within the Trajectory Optimization Landscape

The approach in (Kim et al., 28 Oct 2025) represents an evolution of contact-implicit trajectory optimization toward greater physical fidelity and dynamic reliability in robotics:

• Contrast with direct collocation and mode-scheduling: Unlike mode-scheduled approaches, CITO does not require pre-specified contact/planning phases, enabling greater autonomy in behavior discovery (Turski et al., 2023, Patel et al., 2018).
• Contrast with smooth/relaxed contact models: Rather than trading accuracy for smoothness, the described contact-point decomposition and Bézier-parameterized compliance enforce hard physical constraints, yielding trajectories with physical realism suitable for real hardware.
• Integration with state-of-the-art solvers: The framework leverages multiple-shooting, sequential quadratic programming, and extensive use of analytic gradients for efficiency.

This class of contact-implicit trajectory optimization makes it possible to synthesize dynamically consistent, physically plausible motion plans for complex legged robots across a wide spectrum of contact-rich tasks, marking a significant step forward in the robustness and generality of motion planning for high-DOF systems (Kim et al., 28 Oct 2025).