On the Surprising Robustness of Sequential Convex Optimization for Contact-Implicit Motion Planning

TL;DR

This work introduces CRISP, a primal-only sequential convex programming solver for contact-implicit motion planning with nonlinear complementarity constraints. By employing a weighted $\ell_1$ merit and trust-region convex subproblems, CRISP avoids primal–dual difficulties arising from violated constraint qualifications and offers convergence guarantees to merit stationary points. The authors provide a high-performance C++ implementation with automatic differentiation, benchmark CRISP against state-of-the-art solvers on six contact-rich tasks, and demonstrate robust performance from naive initializations, including all-zero starts. The approach is validated both in simulation and real-world MPC-like deployment, and the codebase is open-source, enabling broader adoption in robotics optimization.

Abstract

Contact-implicit motion planning-embedding contact sequencing as implicit complementarity constraints-holds the promise of leveraging continuous optimization to discover new contact patterns online. Nevertheless, the resulting optimization, being an instance of Mathematical Programming with Complementary Constraints, fails the classical constraint qualifications that are crucial for the convergence of popular numerical solvers. We present robust contact-implicit motion planning with sequential convex programming (CRISP), a solver that departs from the usual primal-dual algorithmic framework but instead only focuses on the primal problem. CRISP solves a convex quadratic program with an adaptive trust region radius at each iteration, and its convergence is evaluated by a merit function using weighted penalty. We (i) provide sufficient conditions on CRISP's convergence to first-order stationary points of the merit function; (ii) release a high-performance C++ implementation of CRISP with a generic nonlinear programming interface; and (iii) demonstrate CRISP's surprising robustness in solving contact-implicit planning with naive initialization. In fact, CRISP solves several contact-implicit problems with all-zero initialization.

Figures (9)

• Figure 1: CRISP computes entirely new contact sequences from naive and even all-zero initializations. For (a), (b), (c), and (d), the left side shows the initial trajectories and the right side displays the optimized trajectory from CRISP. For (e) the hopper problem, the initial guess is a free-fall motion released from the origin. The color gradient represents the progression of time (from blue to yellow). For (f), we implement the policy derived from CRISP in a Model Predictive Control (MPC) framework for real-world push tasks. Detailed descriptions of these tasks are provided in §\ref{['sec:exp']}.
• Figure 2: Geometric intuition of MPCC through Example example:toy.
• Figure 3: Depiction of the $\ell_1$ penalty merit function for Example example:toy with different penalty parameters $\mu$.
• Figure 4: Schematic overview of the contact-implicit motion planning tasks considered in the experiments. Each task poses unique challenges in contact sequencing, force distribution, and modeling of the multi-modal dynamics.
• Figure 5: Box plots of benchmark metrics on three tasks: cartpole with soft walls, push box, transport, and push T. The plots complement tab:comparison to show the distribution of (a) objective value, (b) tracking error, and (c) constraint violation across multiple initial states and initial guesses. PROXNLP was unable to solve the Push T problem within a tractable amount of time due to its larger problem size, so its data is not applicable in the figure.

Theorems & Definitions (12)

• Example 1: Geometric Intuition of MPCC
• Example 2: Merit Function for the Toy Problem
• Remark 3: Merit Function
• Remark 4: Elastic Mode
• Definition 5: Directional Derivative
• Definition 6: Stationary Point
• Theorem 8: Convergence
• proof
• Proposition 9: Local Optimality
• proof