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

The gist 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<ref:2610.20936#pg2>.

Problem Formulation and Transcription

The Contact Sequence Robot OCP is defined as minimizing a cost subject to dynamics, mode constraints, and contact constraints across a prescribed sequence of modes<ref:2610.20936#pg4>. To transcribe this problem, the trajectory is represented by polynomials of degree p in one of two forms, the first being a Lagrange interpolant through the Chebyshev–Gauss–Lobatto (CGL) nodes<ref:2610.20936#pg6>. The cost function is approximated by Clenshaw–Curtis quadrature on the CGL nodes<ref:2610.20936#pg6>. The constraints are transcribed in two ways, with the friction cone being imposed on the Bezier control points rather than at the nodes<ref:2610.20936#pg6>. This results in a nonlinear program Problem 3 (Transcribed OCP) with variables z and constraints gi(z)=0 and gi(z)≤0<ref:2610.20936#pg8>.

Gradient Flow Methodology

LEAP integrates a primal–dual gradient flow of a Powell–Hestenes–Rockafellar augmented Lagrangian to compute a solution<ref:2610.20936#pg9>. This flow is designed to handle the initialization challenge by deforming the trajectory continuously in pseudo-time until it becomes stationary for an action functional. The method utilizes a two-time-scale flow where the primal flow follows z˙(s) = −∇zLA(z(s), γ; µ) and the multipliers jump by the damped update γ+:= γ + τγ µ ⊙ c eff(z(sk+1), γ). This structure is maintained by Lemma 10 (Monotone accepted sequence), which ensures that the accepted iterates satisfy LA(zk+1, γ) < LA(zk, γ) for every k.

Numerical Scheme for Flow Integration

The flow of Definition 5 is integrated using a linearly implicit solver to ensure that the augmented Lagrangian monotonically decreases along the numerical solution to the primal-dual flow. This step involves solving a linearized system defined by H(z, γ), which is a Gauss–Newton approximation of the exact Jacobian. The step update is given by I + hθH(v, γ)∆z = −h∇zLA(v, γ), where ∆z solves (H + 1/hθ I)∆z = −1/θ∇zLA. The step-size controller decides how far to trust H based on the ratio ϱ:= LA(v) − LA(z+(α))/pred(α).

Feasibility and Refinement

The process involves an outer loop that manages feasibility and mesh adaptation. The feasibility criterion is defined by G(z) ≤ tol and F(z) ≤ tol, where G is the node gate and F is the fine-grid violation. If G ≤ tol but F > tol, the level ends and the outer loop refines the mesh using Dorfler marking to bisect elements with the largest indicators. The outer loop also includes an initial feasibility flow where LA reduces to Φ(z):= ω squared P i ν i(z) squared, which is solved via a Levenberg–Marquardt iteration.

Performance and Validation

LEAP has been compared against Crocoddyl and Aligator on ten benchmark problems across five robot platforms. LEAP meets the terminal tracking criteria in all ten simulated rollouts, compared with seven for Crocoddyl and eight for Aligator. Furthermore, a hopscotch execution on the Unitree Go2 quadruped demonstrates hardware tracking of an optimized reference with a joint-position RMS error of 0.0682 rad. The method achieves substantial solve-time reductions and lower normalized terminal error over a range of compute budgets.

Hardware Execution

The hopscotch trajectory is executed on a Unitree Go2 quadruped using a tracking controller trained with domain randomization to accommodate remaining discrepancies between simulation and hardware. This execution shows that a LEAP reference can guide the robot through the hopscotch sequence with centimeter-scale RMS position errors. The final absolute error for the base position is 0.0309 m, which is below the threshold of 0.1 m/s.

Conclusion

LEAP combines pseudospectral collocation with a two-time-scale PHR augmented-Lagrangian flow whose equilibria satisfy the KKT conditions of the transcribed problem. It preserves descent at fixed multipliers and penalties through a linearly implicit Gauss–Newton integrator, while feasibility initialization and adaptive mesh refinement address initial infeasibility and violations between collocation nodes. Across ten benchmarks on five robot platforms, LEAP meets the terminal tracking criteria in all ten simulated rollouts, compared with seven for Crocoddyl and eight for Aligator. It achieves substantial solve-time reductions on several benchmarks and lower normalized terminal error over a range of compute budgets. A hopscotch execution on the Unitree Go2 further demonstrates hardware tracking of an optimized reference, with a joint-position RMS error of 0.0682 rad.

1. The LEAP method can solve large, nonlinear, non-convex optimal control problems (OCPs) for dynamically feasible motions in robots with contact constraints by integrating a primal–dual gradient flow of a Powell–Hestenes–Rockafellar augmented Lagrangian. This allows the system to handle the dynamics, constraint, and initialization challenges that plague existing solvers.

2. The improved AI system can generate optimal hopscotch trajectories for various robot platforms, from planar hoppers to 32-joint humanoids (like the Unitree Go2), achieving substantial speedups over Crocoddyl and Aligator on several benchmarks while reaching trackable references at smaller compute budgets.

3. The trajectory optimization can be executed with high fidelity on physical hardware; for instance, a hopscotch execution on a Unitree Go2 quadruped showed a joint position RMS error of 0.0682 rad and centimeter-scale RMS position errors, guided by an optimized reference motion.

4. The system incorporates an adaptive mesh refinement strategy based on the criterion: G(z) ≤ tol and F(z) ≤ tol, where G is the node gate (maximum nodal violation) and F is the fine-grid violation. This ensures that even when nodes have converged, the remaining error between them is addressed by refining the mesh.

5. The solver employs a linearly implicit Gauss–Newton integrator with a step controller based on Lemma 10, ensuring that "LA(zk+1, γ) < LA(zk, γ) for every k, which guarantees that the accepted iterates satisfy the descent property necessary for convergence while being insensitive to the jumps" in the active set.

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.