Contact-Implicit Trajectory Optimization

Unlike traditional methods that use discrete contact modes (e.g., 'stick' or 'slip'), contact-implicit optimization introduces continuous complementarity constraints. The solver optimizes over contact forces as continuous variables, with the constraint that a force can only be non-zero if the corresponding distance is zero (i.e., bodies are in contact). This allows the discovery of contact timings and modes (making/breaking, sticking/sliding) as part of the solution, rather than requiring them as inputs.

The core mathematical formulation enforces the Signorini condition for non-penetration and the Coulomb friction model. These are expressed as complementarity constraints:

• Non-penetration: The gap distance must be non-negative, the normal force must be non-negative, and their product must be zero (complementary).
• Friction Cone: The tangential friction force must lie inside the Coulomb friction cone. This creates a mathematical program with complementarity constraints (MPCC), a challenging but expressive non-linear optimization problem.

This method is critical for planning non-prehensile manipulation (e.g., pushing, pivoting, tumbling) and complex in-hand manipulation where the sequence of finger contacts is not obvious. Example tasks include:

• Regrasping an object by rolling it in the hand.
• Using environmental contacts, like a table edge, to reorient an object.
• Planning dynamic manipulation where the object is not firmly grasped but controlled through intermittent impacts and sliding.

Contact-explicit optimization requires the engineer to pre-specify a contact sequence—a timeline dictating which links contact which surfaces and in what mode (e.g., stick or slide). This is brittle and often intractable for complex tasks. In contrast, contact-implicit optimization:

• Reduces engineering burden by not requiring a priori contact mode specification.
• Discovers hybrid dynamics (transitions between contact states) automatically.
• Is more suitable for exploratory motion planning where the optimal contact strategy is unknown.

Solving MPCCs is computationally intensive. Modern approaches use:

• Direct transcription: Discretizing the continuous-time problem into a large, sparse non-linear program (NLP).
• Relaxation techniques: Smoothing the complementarity constraints (e.g., with Fischer-Burmeister or slack variables) to improve solver convergence.
• Specialized NLP solvers: Tools like SNOPT or IPOPT are often used to handle the large-scale, constrained optimization problems that result from the transcription. Computation times can range from seconds to minutes for a single trajectory.

The optimized trajectory, including the predicted contact forces, serves as a feedforward plan for a whole-body controller. A downstream model predictive control (MPC) or impedance control layer tracks the planned motion while robustly handling small deviations in contact timing or force. This separation of planning (contact-implicit optimization) and execution (reactive control) is a common architecture for deploying these plans on physical hardware.

Trajectory optimization is the broader mathematical process of computing a sequence of robot states and control inputs that minimizes a cost function (e.g., energy, time) while satisfying dynamic constraints and task goals. Contact-implicit trajectory optimization is a specialized variant where the contact sequence itself is an optimization variable, not a predefined input. This allows the solver to discover optimal contact timings and locations, which is essential for non-prehensile manipulation like pushing or pivoting.

Model predictive control is a real-time, receding-horizon control strategy. At each time step, it solves a finite-horizon trajectory optimization problem using a dynamic model of the system, applies the first control input, and then re-solves at the next step with new sensor feedback. Contact-implicit MPC integrates contact-implicit planning into this loop, enabling robots to reactively discover and exploit new contact modes (e.g., sliding, rolling) in dynamic, uncertain environments. This is critical for maintaining stability during manipulation.

Non-prehensile manipulation involves moving objects without a firm, enclosing grasp. Techniques include pushing, pivoting, tumbling, and batting. These tasks are inherently contact-rich and require planning where contacts are made and broken strategically. Contact-implicit trajectory optimization is the principal algorithmic approach for planning such behaviors, as it can automatically determine the optimal sequence of intermittent contacts to achieve a goal, such as reorienting a box on a table using repeated pushes.

Complementarity constraints are the mathematical formalism that enables contact-implicit planning. They encode the physical laws of contact: a contact force can only be applied when there is no gap (non-penetration), and a force is zero when there is a gap (no adhesion). Formally, this is expressed as 0 ≤ force ⊥ gap ≥ 0. The optimization solver must satisfy these disjunctive constraints alongside dynamics, making the problem a Mathematical Program with Complementarity Constraints (MPCC). This is the core challenge that distinguishes it from simpler trajectory optimization.

The sim-to-real gap is the performance drop when a policy trained in simulation is deployed on a physical robot. Contact-implicit trajectory optimization is often solved in high-fidelity physics simulators (like MuJoCo, Drake, or Isaac Sim) that model contact dynamics. Successfully transferring these plans requires techniques like domain randomization (varying simulation parameters like friction) and robust state estimation to mitigate discrepancies in contact modeling, sensor noise, and actuator dynamics between simulation and reality.

In-hand manipulation is the fine-grained control of an object within a robotic hand's grasp using finger gaits and rolling contacts. This is a premier application for contact-implicit trajectory optimization. The solver can plan complex finger gait sequences—determining when to break and make fingertip contacts, and how to roll the object—to achieve a desired reorientation without dropping it. This requires reasoning about force closure stability throughout the planned trajectory of intermittent contacts.