Authors: César E. Ramos Chuquiure, Ryan Tsai, Yutong Zhou, Zihang Huang, Vansh Thakur, Akshunn Trivedi, Zachary Lin, Pearl Lin, Ruikai Yang, Challen Enninful Adu, Ram Vasudevan
Organizations: 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.
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.
Figures & tables
Quantity
Error metric
Threshold
Base position
Euclidean position error
0.050 m
Base orientation
Max. absolute wrapped RPY error
0.10 rad
Joint position
Max. absolute actuated-joint error
0.20 rad
Foot position
Max. Euclidean foot error
0.040 m
Base linear speed
Linear velocity norm
0.10 m/s
Joint speed
Max. absolute actuated-joint velocity
0.10 rad/s
TABLE I: Terminal goal thresholds shared by all robots and methods. Each metric must be strictly below its threshold at the final time of the rollout.
Solve time [s]
Hopper
Go2
H1
G1
Talos
nj
2
12
19
29
32
Uneven terrain
LEAP
20.9
79.8
13693
1218
4096
Crocoddyl
143
78.1
14400†
1425
×
Aligator
1201
487
14400†
2121
×
Fence jumping
TABLE II: Optimization time of the reference trajectories on both benchmarks. Each entry is the solve time in seconds of the reference whose simulated rollout satisfies the terminal-goal criteria of Table I . × marks a method whose rollout does not, and † a run that reached the budget without meeting the feasibility test. Bold marks the fastest successful method for each problem, times above 100 s are rounded to the second and method colors match Figure 2 . nj is the actuated joint count.
maxiei/eˉi
Robot
Method
Uneven terrain
Fence jumping
Hopper (2)
LEAP
0.12
0.89
Crocoddyl
0.14
147
Aligator
0.17
14
Go2 (12)
LEAP
0.23
0.038
Crocoddyl
0.32
0.31
TABLE III: Terminal goal accuracy of the simulated rollouts on both benchmarks. Each entry is maxiei/eˉi , the largest of the six errors of Table I divided by its threshold evaluated at the terminal time. A value below one means the rollout reaches the goal. Bold marks the smallest value for each problem, and method colors match Table II and Figure 2 . Parentheses give the actuated joint count, listed as terrain/fence for the Hopper, which the two tasks pose differently. It can be seen that for 9/10 cases LEAP attains the smaller terminal error.
Fig. 1: Simulated tracking of LEAP trajectories for both tasks, in side view: walking over uneven terrain (left) and fence jumping (right). Obstacles are red, and robot configurations are colored by elapsed time within that robot’s plan, from the initial pose in solid gray to the final pose in solid green. The stepping stones are placed at distinct heights and positions making the gait non-periodic, and self-collision and collision with the fence are included in the optimization formulation.
Fig. 2: This figure shows terminal goal error maxiei/eˉi (Table I ) of the rollout versus compute budget. The dashed line at one is the success criterion. At each budget value we simulate the solution with the smallest node violation recorded up to that point. A dot marks where the run passed the solver’s convergence test or stalled, and the dotted tail carries its final iterate, the reference of Tables II and III , to larger budgets. LEAP (blue), Aligator (orange), Crocoddyl (pink). Counting the budget from which the retained reference stays trackable, LEAP is earliest on seven of ten problems.
Quantity
RMS error
Peak absolute error
Final absolute error
Joint position [rad]
0.0682
0.4455
0.1546
Base roll [rad]
0.0249
0.0572
0.0435
Base pitch [rad]
0.0426
0.1093
0.0670
Base yaw [rad]
0.0113
0.0309
0.0107
Base x [m]
0.0424
0.0917
0.0309
Base y [m]
0.0347
0.0769
0.0093
TABLE IV: Tracking errors for hardware execution of the hopscotch trajectory.
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)
This paper presents dynamics-relaxed model predictive control (DR-MPC), a novel MPC formulation for legged locomotion, and a tailored interior-point method (IPM) solver. The formulation combines online optimization feasibility by construction with a contact-aware input parameterization. DR-MPC moves the dynamics equality and affine input constraints into quadratic penalties and retains only nonempty box constraints. The resulting box-constrained quadratic program (QP) has a block-arrow Hessian that enables the state and affine-output directions to be eliminated through a Schur complement. The solver factors only the reduced control system after swing-force elimination and contact-aligned move blocking. For the evaluated implementations using the same DR-MPC formulation, our method achieves median end-to-end MPC speedups of 16.0× over HPIPM and 4.4× over OSQP, with comparable locomotion performance in simulation. DR-MPC achieves a median onboard MPC end-to-end time of 4.4 ms and is validated on a Unitree Go1 quadruped. Open-source code will be made available after publication.
Run Wang, Alapati Tuerxun, Shuo Liu +4
Department of Automation, Tsinghua University, Beijing, China · Department of Mechanical Engineering, Boston University, Brookline, MA, USA · School of Electrical and Electronic Engineering, Nanyang Technological University, Singapore +1
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.
Samuel C. Buckner, Purnanand Elango
Department of Aeronautics and Astronautics, University of Washington, Seattle, WA 98195. · Mitsubishi Electric Research Laboratories, Cambridge, MA 02139.