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

conference_logo CRISP has been accepted by Robotics: Science and Systems (RSS) 2025!

1Robotics and Autonomous Systems Thrust, The Hong Kong University of Science and Technology
2School of Engineering and Applied Sciences, Harvard University
CRISP Overview
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 T tasks.

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 focuses only 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 l1 penalty. We (i) prove 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 initializations. In fact, CRISP solves several contact-implicit problems with an all-zero initialization.

Trajectories Animation

Shallow trajectories represent initial guesses.

Real-World Push T (2x)