cs.ROApr 11, 2026

GPU-Accelerated Continuous-Time Successive Convexification for Contact-Implicit Legged Locomotion

Authors: Samuel C. Buckner, Purnanand Elango

Organizations: Department of Aeronautics and Astronautics, University of Washington, Seattle, WA 98195. · Mitsubishi Electric Research Laboratories, Cambridge, MA 02139.

Read PDF
Ask AI
Cite

Copy citation

DOINot available for this paper

Abstract

Contact-implicit trajectory optimization (CITO) enables the automatic discovery of contact sequences, but most methods rely on fine time discretization to capture all contact events accurately, which increases problem size and runtime while tying solution quality to grid resolution. We extend the recently proposed sequential convex programming (SCP) approach for trajectory optimization, continuous-time successive convexification (ct-SCvx), to CITO by introducing integral cross-complementarity constraints, which eliminate the risk of missing contact events between discretization nodes while preserving the flexibility of contact mode changes. The resulting framework, contact-implicit successive convexification (ci-SCvx), models full multibody dynamics in maximal coordinates, including stick-slip friction and partially elastic impacts. To handle complementarity constraints, we embed a backtracking homotopy scheme within SCP for reliable convergence. We implement this framework in a stand-alone Python software, leveraging JAX for GPU acceleration and a custom canonical-form parser for the convex subproblems of SCP to avoid the overhead of general-purpose modeling tools such as CVXPY. We demonstrate ci-SCvx on diverse legged-locomotion tasks. In particular, we validate the approach in MuJoCo with the Gymnasium HalfCheetah model against the MuJoCo MPC baseline, showing that a tracking simulation with the optimized torque profiles from ci-SCvx produces physically consistent trajectories with lesser energy consumption. We also show that the resulting software achieves faster solve times than existing state-of-the-art SCP implementations by over an order of magnitude, thereby demonstrating a practically important contribution to scalable real-time trajectory optimization.

Figures & tables

Figure 1: Overview of the proposed framework, including problem formulation, integral cross-complementarity and the CI -SC VX algorithm.
Figure 2: Monoped locomotion solution on a flat surface, produced by CI -SC VX .
Figure 3: Biped locomotion solution on a sinusoidal surface, produced by CI -SC VX .
Figure 4
Figure 5: Timing results comparing the proposed CI -SC VX -C USTOM implementation against the baseline CI -SC VX -B ASE .
More in this topic

Trajectory Optimization

Trajectory optimization computes motion or control sequences that minimize costs while satisfying system dynamics, safety limits, and task constraints.

See all 199 papers

Explore similar work

CardsList
May 9, 2026

IMPACT: An Implicit Active-Set Augmented Lagrangian for Fast Contact-Implicit Trajectory Optimization

Contact-implicit trajectory optimization (CITO) has attracted growing attention as a unified framework for planning and control in contact-rich robotic tasks. Recent approaches have demonstrated promising results in manipulation and locomotion without requiring a prescribed contact-mode schedule. It is well known that the underlying mathematical programs with complementarity constraints (MPCCs) remain numerically ill-conditioned, and systematic, scalable solution strategies for CITO remain an active area of research. More efficient and principled solvers that can handle contact constraints are therefore essential to broaden the applicability of CITO. In this work, we develop an augmented-Lagrangian approach to CITO for solving MPCC-based CITO with stationarity guarantees. The method can be interpreted as identifying the implicit contact-mode branches on the fly during the trajectory optimization (TO) iterations; we call this approach IMPACT (IMPlicit contact ACtive-set Trajectory optimization). We provide an efficient C++ implementation tailored to trajectory-optimization workloads and evaluate it on the open-source CITO and contact-implicit model predictive control (CI-MPC) benchmarks. On CITO, IMPACT achieves 2.9x-70x speedups over strong baselines (geometric mean 13.8x). On CI-MPC, we show improved control quality for contact-rich trajectories on dexterous manipulation tasks in simulation. Finally, we demonstrate the proposed method on real robotic hardware on a T-shaped object pushing task.

Jiayun Li, Dejian Gong, Georgia Chalvatzaki

1PEARL Lab, Dept. of Computer Science, TU Darmstadt, Germany · 2Hessian.AI, Darmstadt, Germany · 3Robotics Institute Germany (RIG)

Constrained Motion PlanningTrajectory OptimizationContact-Rich Robotic ManipulationAugmented Lagrangian MethodConstrained OptimizationOptimal Control
AskPDF
Aug 12, 2026

ContactIPM: A Structure-Exploiting Interior-Point Solver for Contact-Implicit Trajectory Optimization

Contact-implicit trajectory optimization avoids prescribing contact sequences, but yields mathematical programs with complementarity constraints (MPCCs) whose degeneracy challenges conventional primal--dual solvers. Existing contact-specific methods improve robustness to this degeneracy but do not leverage a stagewise optimal-control factorization and primal--dual consistency, while structure-exploiting optimal-control solvers are not designed for complementarity constraints. We show that these capabilities can be combined in a single primal--dual method. ContactIPM identifies complementary inequality pairs, embeds them through a barrier-coupled elastic interior relaxation, eliminates slack and dual variables stagewise, and solves the reduced Newton system using a Riccati recursion. A fixed multi-phase MPCC recovery schedule provides four continuation and restart attempts from naive initializations, while termination is gated by the unrelaxed physical complementarity residual. We compare ContactIPM with two contact-specific MPCC solvers, CRISP and IMPACT, using matched benchmark conditions and common post-solve acceptance criteria. On four fixed CRISP benchmark cases, ContactIPM is 2.172.17--8.87×8.87\times faster over 20 paired timing repetitions per case and achieves higher success on the Push Box and Push-T robustness suites. Against IMPACT, ContactIPM is 2.96×2.96\times faster on Push T and 4.91×4.91\times faster on Cart Transport, but 4.46×4.46\times slower on Push Box. In 50 closed-loop Push Box rollouts spanning model mismatch, measurement noise, initial-pose errors, and state resets,

Yucheng Chen

Primal-Dual OptimizationTrajectory OptimizationConstrained Optimization
AskPDF
Oct 14, 2026

LEAP: Gradient Flow Augmented Lagrangian Trajectory Optimization for Contact Constrained Robots

Generating dynamically feasible motions for legged and manipulating robots requires solving a large, nonlinear, non-convex optimal control problem (OCP) in which contact wrenches must satisfy friction-cone constraints throughout the motion, not only at isolated instants. Existing solvers are sensitive to the initial guess and scale poorly with the number of joints. This paper introduces LEAP, which transcribes the OCP with pseudospectral collocation and, instead of a Newton or SQP solve, integrates a primal--dual gradient flow of a Powell--Hestenes--Rockafellar augmented Lagrangian to compute a solution. Because the flow is stiff, this paper designs a linearly implicit integrator with a Gauss--Newton metric, momentum, and a merit-based step controller that keeps the augmented Lagrangian monotone. Across locomotion benchmarks on five robot platforms, from planar hoppers to 32-joint humanoids, LEAP is the only method to solve every tested problem, achieves substantial speedups over Crocoddyl and Aligator on several benchmarks, and reaches trackable references at smaller compute budgets. The resulting trajectories are validated by tracking them in a high-fidelity simulator and on hardware. We will make the code public upon final acceptance of this paper.

César E. Ramos Chuquiure, Ryan Tsai, Yutong Zhou +8

Department of Robotics, University of Michigan, Ann Arbor, MI, USA. · Computer Science and Engineering, University of Michigan, Ann Arbor, MI, USA. · Department of Electrical Engineering and Computer Science, University of Michigan, Ann Arbor, MI, USA.

Hypergradient OptimizationTrajectory OptimizationRobotic OptimizationAugmented Lagrangian MethodRobot Locomotion
AskPDF