密歇根大学LEAP：从2关节跳跃机器人到32关节人形，接触运动规划10个基准全部求解，Crocoddyl与Aligator分别为7个和8个

Abstract

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.

I Introduction

Robots interact with the world through contact: a humanoid walks by planting and lifting its feet, a manipulator pushes, grasps and releases objects, and a quadruped climbs by loading and unloading its limbs. Planning such motions means choosing joint torques and contact forces that respect the robot’s dynamics, its actuation limits, and the physics of contact, which requires the contact wrench to stay inside a friction cone at every instant. Trajectory optimization formalizes this as an Optimal Control Problem (OCP) over hybrid dynamics: minimize a cost, subject to the equations of motion in each contact mode, and the constraints above.

Solving this OCP is difficult for three reasons. The dynamics are nonlinear and change discontinuously across contact modes. The constraints, in particular the friction cones and the dynamics themselves, are nonlinear and non-convex, and any solver must decide how to enforce them and where in time. And the resulting problem is non-convex in the decision variables, so the outcome depends on the initial guess, and a good one is often unavailable for a new behavior or a new robot. We refer to these as the dynamics, constraint, and initialization challenges.

State-of-the-art solvers exploit the temporal structure of the dynamics to make each iteration cheap. Differential Dynamic Programming (DDP) methods such as Crocoddyl [17] and Aligator [11] combine quadratic approximations of the value function with multiple shooting, and direct methods such as acados [24, 26] transcribe the OCP into a nonlinear program (NLP) with banded structure. In both families each iteration is a Newton-type step, and this is what makes the constraint and initialization challenges persist. A Newton step is only meaningful near a solution, so these solvers rely on a good warm start and on line-search or trust-region globalization whose parameters must be tuned per problem. Constraints are handled by penalties, by projection onto a box, or by an augmented Lagrangian inside the Riccati recursion, and they are enforced only at discrete knots, so the friction cone can be violated between them. In Section V illustrates that, on the contact-constrained problems, these solvers scale poorly with the number of joints and their performance depends strongly on hyper-parameters.

Gradient flows (GF) offer a different way to handle the initialization challenge. Instead of taking Newton steps, the trajectory is deformed continuously in a pseudo-time until it becomes stationary for an action functional. Every step of the flow is a descent step that is well defined anywhere, not only near a solution, so the method tolerates poor initial guesses and admits continuation naturally. [15] and [14] introduced the Affine Geometric Heat Flow (AGHF), whose steady state solves the OCP, and [6], [16] and [7] extended it to free boundary conditions and to low-dimensional legged systems. [1] and [2] improved the numerics to reach higher-dimensional systems, and [13]extends the idea to extended Lagrangians. What these methods gain on initialization they give back on the other two challenges. Constraints enter as quadratic penalties, so they are never satisfied exactly, and raising the penalty to reduce the violation makes the flow stiffer. The stiff flow is then integrated with general-purpose solvers whose error control is unaware of the optimization structure, so the dynamics challenge reappears as a numerical one.

This paper introduces LEAP, a gradient-flow trajectory optimizer for robots in contact under a prescribed contact sequence, together with a numerical scheme designed for that flow. It keeps the robustness to initialization of gradient flows while resolving the aforementioned constraint and stiffness problems. Our contributions are four-fold: First, we utilize a Powell–Hestenes–Rockafellar (PHR) augmented Lagrangian formulation and derive a two-time-scale primal–dual flow to enable exact constraint handling (Section III). Second, we describe how to extend this approach to hybrid dynamical systems and illustrate how to ensure that wrench feasibility is satisfied for all timein Lemma 2. Third, we develop a linearly implicit solver to ensure that the augmented Lagrangian monotonically decreases along the numerical solution to the primal-dual flow (Section IV). Finally, we compare LEAP against Crocoddyl and Aligator on solve time and terminal tracking accuracy under a time budget across two tasks and five robots with two to thirty-two joints with solution quality judged by a whole-body tracking controller in a high-fidelity simulator (Section V). A hopscotch trajectory is executed on a Unitree Go2 quadruped (Section VI).

II Preliminaries

This section introduces the concepts necessary to construct the gradient-flow based trajectory optimization algorithm LEAP.

II-A Robot dynamics with a prescribed contact sequence

Consider a robot with configuration q(t)\in\mathbb{R}^{n_{q}}, generalized velocity v(t)\in\mathbb{R}^{n_{v}} related by \dot{q}(t)=N(q(t))v(t), and state x(t)=(q(t),v(t))\in\mathbb{R}^{n_{x}} on the horizon I:=[0,T]. Let E be the set of possible contacts. A contact \ell\in E is described by a smooth holonomic residual \phi_{\ell}:\mathbb{R}^{n_{q}}\to\mathbb{R}^{d_{\ell}}, and is active while \phi_{\ell}(q(t))=0. A mode is a set of simultaneously active contacts, which we identify with the subset j\subseteq E. Its stacked residual \Phi_{j}:=(\phi_{\ell})_{\ell\in j} and contact Jacobian J_{c,j}(q) are related by \dot{\Phi}_{j}=J_{c,j}(q)\,v. Within mode j the state obeys the differential algebraic equation

\displaystyle\hskip-7.11317ptM(q(t))\dot{v}(t)+C(q(t),v(t)) \\ \displaystyle=Bu(t)+J_{c,j}^{T}(q(t))\,\lambda_{j}(t), \\ \displaystyle\Phi_{j}(q(t)) \\ \displaystyle=0,

where M is the mass matrix, C collects Coriolis and gravity terms, B is the actuation matrix, u(t)\in U is the control input and \lambda_{j}(t) is the stacked contact wrench of the contacts in j.

We consider motions with a prescribed mode sequence (j_{0},\ldots,j_{M-1}) and prescribed switching times t_{0}=0<t_{1}<\cdots<t_{M-1}<t_{M}=T, so that mode j_{i} is active on I_{i}:=[t_{i},t_{i+1}]. At a switching time t_{i} the configuration is continuous, and the velocity is mapped by a reset v(t_{i}^{+})=R_{j_{i-1},j_{i}}(q(t_{i}),v(t_{i}^{-})), which we model as a Newton impact [20] when a contact becomes active and as the identity when a contact is released. This is a hybrid system in the sense of Burden et al. [3] whose discrete transitions are fixed in advance, so no guard conditions are needed.

II-B Contact Sequence Robot OCP

Problem 1.

Given the mode sequence and switching times above, the Contact Sequence Robot OCP is

\displaystyle\underset{x,u,\{\lambda_{i}\}}{\inf}\, \\ \displaystyle\sum_{i=0}^{M-1}\int_{t_{i}}^{t_{i+1}}l_{i}(x(t),u(t),\lambda_{i}(t))~dt \\ \displaystyle\mathrm{s.t.}\quad \\ \displaystyle g_{i,k}(x(t),u(t),\lambda_{i}(t))\leq 0\quad\forall k\in\mathcal{I}_{i},\ \forall t\in I_{i}, \\ \displaystyle g_{i,k}(x(t),u(t),\lambda_{i}(t))=0\quad\forall k\in\mathcal{E}_{i},\ \forall t\in I_{i},

for i\in\{0,\ldots,M-1\}, where \lambda_{i} abbreviates \lambda_{j_{i}}.

The equality constraints \mathcal{E}_{i} contain the dynamics (1), the reset map at t_{i}, and the boundary conditions; the inequality constraints \mathcal{I}_{i} contain the friction cone on \lambda_{i} and bounds on x and u. All constraints must hold for every t, not only at a finite set of times.

II-C Polynomial representations

To transcribe Problem 1 we represent each trajectory on an interval, rescaled to [-1,1], by a polynomial of degree p in one of two forms [22]. The first is a Lagrange interpolant through the Chebyshev–Gauss–Lobatto (CGL) nodes

\displaystyle\tau_{n}:=\cos\left(\frac{\pi n}{p}\right),\quad n\in\{0,\ldots,p\},

parameterized by its nodal values f(\tau_{n}). The second is a Bézier curve

\displaystyle f(\tau)=\sum_{b=0}^{p}B_{b}^{p}(\tau)\,\bar{f}_{b},

parameterized by its control points \bar{f}_{b}, where B_{b}^{p} are the Bernstein basis polynomials on [-1,1]. Either way a trajectory becomes a finite vector of coefficients. The Bézier form has a property that the nodal form lacks, and which we use to enforce wrench constraints for all time.

Lemma 2 (Bézier control points certify convex constraints).

Let \mathcal{W}\subseteq\mathbb{R}^{n} be convex and let \lambda be a Bézier curve with control points \bar{\lambda}_{0},\ldots,\bar{\lambda}_{p}. If \bar{\lambda}_{b}\in\mathcal{W} for all b, then \lambda(\tau)\in\mathcal{W} for all \tau\in[-1,1].

Proof.

The Bernstein basis is nonnegative and sums to one, so \lambda(\tau) is a convex combination of the control points. ∎

III Transcribing the OCP into a Gradient Flow

This section transcribes Problem 1 into a nonlinear program and derives the pseudo-time Gradient Flow (GF) that LEAP integrates to solve it.

III-A Transcription

On the mode interval I_{i}=[t_{i},t_{i+1}], we introduce the reference time

\tau=\frac{2(t-t_{i})}{t_{i+1}-t_{i}}\in[-1,1],

and represent each decision function on I_{i} by a polynomial in \tau using the forms of Section II-C. The state and control are Lagrange interpolants on the CGL nodes (3), so their coefficients are the nodal values q(\tau_{n}),v(\tau_{n}),u(\tau_{n}), and time derivatives are obtained by the CGL differentiation matrix [22]. The contact wrench is a Bézier curve (4) with control points \bar{\lambda}_{i,0},\ldots,\bar{\lambda}_{i,p}. Each field can use a different degree. We call one such polynomial segment an element and the collection of elements the mesh; initially there is one element per mode interval, and the h-refinement of Section IV-E may later split an element in two. Let z_{i} collect the coefficients of mode i and z=(z_{0},\ldots,z_{M-1})\in\mathbb{R}^{n_{z}} all decision variables.

The cost (2a) is approximated by Clenshaw–Curtis quadrature on the CGL nodes. The constraints are transcribed in two ways. The dynamics (1), the bounds on x and u, and any other constraint involving the state or control are collocated at the CGL nodes. The friction cone, which is convex in \lambda_{i}, is imposed on the Bézier control points \bar{\lambda}_{i,b} rather than at the nodes, so by Lemma 2 it holds for every \tau\in[-1,1] and not only at the collocation nodes. The reset map at t_{i} becomes an algebraic constraint linking the last node of element i-1 to the first node of element i, q being continuous and v mapped by R_{j_{i-1},j_{i}}, and the boundary conditions are imposed on the first and last nodes of the mesh. Constraints collocated at the nodes may be violated between them; Section IV describes how to control this error through h-refinement and penalties.

Collecting the rows, we obtain the nonlinear program

Problem 3 (Transcribed OCP).

\displaystyle\min_{z\in\mathbb{R}^{n_{z}}}\quad \\ \displaystyle J_{d}(z) \\ \displaystyle\mathrm{s.t.}\quad \\ \displaystyle g_{i}(z)=0,\quad i\in\mathcal{E}_{d}, \\ \displaystyle g_{i}(z)\leq 0,\quad i\in\mathcal{I}_{d},

where \mathcal{E}_{d} collects the collocated dynamics, the reset maps and the boundary conditions, \mathcal{I}_{d} collects the friction-cone rows on the control points and the collocated bounds, and m_{d}:=|\mathcal{E}_{d}|+|\mathcal{I}_{d}|.

III-B Gradient Flow

We now define a flow in a pseudo-time s whose equilibria are the KKT points of Problem 3, built on the Augmented Lagrangian of Powell, Hestenes and Rockafellar (PHR) [10, 19, 21].

Definition 4 (PHR augmented Lagrangian).

For each constraint i\in\mathcal{E}_{d}\cup\mathcal{I}_{d}, let \gamma_{i} be its multiplier and \mu_{i}>0 its penalty. The effective residual and effective multiplier are

\displaystyle c_{i}^{\mathrm{eff}}(z,\gamma) \\ \displaystyle:=\begin{cases}g_{i}(z),&i\in\mathcal{E}_{d},\\
\max\!\left(g_{i}(z),-\tfrac{\gamma_{i}}{\mu_{i}}\right),&i\in\mathcal{I}_{d},\end{cases} \\ \displaystyle w_{i}(z,\gamma) \\ \displaystyle:=\gamma_{i}+\mu_{i}\,c_{i}^{\mathrm{eff}}(z,\gamma),

respectively, and the PHR augmented Lagrangian is

\displaystyle L_{A}(z,\gamma;\mu):=J_{d}(z)+\sum_{i=1}^{m_{d}}\frac{w_{i}(z,\gamma)^{2}-\gamma_{i}^{2}}{2\mu_{i}}.

For an equality constraint the ith summand equals \gamma_{i}g_{i}+\frac{\mu_{i}}{2}g_{i}^{2}, and for an inequality constraint w_{i}=[\gamma_{i}+\mu_{i}g_{i}]_{+}, so L_{A} is continuously differentiable with

\displaystyle\nabla_{z}L_{A} \\ \displaystyle=\nabla J_{d}(z)+\sum_{i=1}^{m_{d}}w_{i}(z,\gamma)\,\nabla g_{i}(z), \\ \displaystyle\nabla_{\gamma}L_{A} \\ \displaystyle=c^{\mathrm{eff}}(z,\gamma).

Definition 5 (Two-time-scale flow).

Let z:[0,s_{\max}]\to\mathbb{R}^{n_{z}} be a curve in pseudo-time s, and partition [0,s_{\max}] into windows [s_{k},s_{k+1}). On each window z follows the primal flow at frozen multipliers \gamma=\gamma(s_{k}),

\displaystyle\dot{z}(s)=-\nabla_{z}L_{A}(z(s),\gamma;\mu),

and at s_{k+1} the multipliers jump by the damped update

\displaystyle\gamma^{+}:=\gamma+\tau_{\gamma}\,\mu\odot c^{\mathrm{eff}}(z(s_{k+1}),\gamma),\qquad\tau_{\gamma}\in(0,1].

Proposition 6 (Equilibria and monotonicity).

Fix \mu\in\mathbb{R}^{m_{d}}_{>0}.

1. If \nabla_{z}L_{A}(z,\gamma;\mu)=0 and \gamma^{+}=\gamma, then (z,\gamma) is a KKT point of Problem 3.
2. Along any solution of (14) on a window, \displaystyle\frac{d}{ds}L_{A}(z(s),\gamma;\mu)=-\|\nabla_{z}L_{A}(z(s),\gamma;\mu)\|^{2}\leq 0, (16) with equality if and only if \nabla_{z}L_{A}=0, so L_{A}(\cdot,\gamma;\mu) is non-increasing on every window and the equilibria of (14) are the stationary points of \min_{z}L_{A}(z,\gamma;\mu).

Proof.

(i) Since \tau_{\gamma}\mu_{i}>0, \gamma^{+}=\gamma gives c^{\mathrm{eff}}=0. For i\in\mathcal{E}_{d} this is g_{i}=0; for i\in\mathcal{I}_{d}, \max(g_{i},-\gamma_{i}/\mu_{i})=0 gives g_{i}\leq 0, \gamma_{i}\geq 0 and \gamma_{i}g_{i}=0. Then w_{i}=\gamma_{i} for all i, and (12) turns \nabla_{z}L_{A}=0 into \nabla J_{d}+\sum_{i}\gamma_{i}\nabla g_{i}=0, which completes the KKT conditions. (ii) Apply the chain rule along (14): \frac{d}{ds}L_{A}=\nabla_{z}L_{A}^{T}\dot{z}. ∎

The multiplier update (15) ascends L_{A}, so monotonicity is a property of the primal flow between multiplier updates and not of the composed map \gamma\mapsto\gamma^{+}. This is the property the integrator of Section IV is designed to preserve.

IV Numerical scheme

Section III reduced trajectory optimization to integrating the flow (14). Doing so with an off-the-shelf integrator fails for three reasons. First, the flow is stiff when the penalties are large [9]. Second, its Jacobian jumps whenever an inequality constraint enters or leaves the active set \mathcal{A}(z,\gamma):=\mathcal{E}_{d}\cup\{i\in\mathcal{I}_{d}:w_{i}(z,\gamma)>0\}, which violates the Lipschitz assumption behind the error control of implicit integrators. Third, the point of the flow is the monotonicity of Proposition 6(ii), and a discretization that does not preserve it forfeits the robustness that motivated the approach. Section IV-A builds a step that handles stiffness and the jumps, Section IV-B controls that step so that monotonicity survives discretization, Section IV-C discretizes the outer time scale of the multipliers, Section IV-D decides when the discrete solution is good enough, and Section IV-E handles initialization and mesh adaptation. Algorithm 1 shows how these components fit together.

IV-A Linearly implicit step

Stiffness calls for an implicit method, but a fully implicit step of (14) requires solving a nonlinear system at every step, and the exact Jacobian \nabla^{2}_{zz}L_{A} contains second derivatives of the dynamics, which are expensive, and the rank-one terms \mu_{i}\nabla g_{i}\nabla g_{i}^{T} that appear and disappear with the active set. We therefore take a linearly implicit step: one linear solve per step, against a Jacobian model that is cheap, positive semidefinite, and already contains the terms the exact Jacobian is about to gain. A step in the rest of the section means one such update.

Definition 7 (Gauss–Newton model on an anticipatory active set).

For a margin \varepsilon_{\mathcal{A}}\geq 0, let \mathcal{A}_{\varepsilon}(z,\gamma):=\mathcal{E}_{d}\cup\{i\in\mathcal{I}_{d}:\gamma_{i}+\mu_{i}(g_{i}(z)+\varepsilon_{\mathcal{A}})>0\}\supseteq\mathcal{A}(z,\gamma) and define

\displaystyle H(z,\gamma):=H_{0}(z)+J_{\mathcal{A}_{\varepsilon}}(z)^{T}\operatorname{diag}(\mu_{\mathcal{A}_{\varepsilon}})J_{\mathcal{A}_{\varepsilon}}(z),

where H_{0}(z)\succeq 0 is the Gauss–Newton Hessian of J_{d} and J_{\mathcal{A}_{\varepsilon}} stacks the gradients \nabla g_{i}^{T} for i\in\mathcal{A}_{\varepsilon}.

H is the Gauss–Newton approximation of \nabla^{2}_{zz}L_{A}, obtained from (12) by dropping the curvature terms w_{i}\nabla^{2}g_{i}. The margin \varepsilon_{\mathcal{A}} enlarges the active set by the constraints within \varepsilon_{\mathcal{A}} of switching, so that the model already carries the rank-one term the exact Jacobian will gain within the step.

Definition 8 (Linearly implicit step).

Given a linearization point v, frozen multipliers \gamma, a step h>0 and \theta\in[\tfrac{1}{2},1], the \theta-method applied to (14) with Jacobian model -H(v,\gamma) is

\displaystyle\big(I+h\theta H(v,\gamma)\big)\,\Delta z=-h\nabla_{z}L_{A}(v,\gamma),

followed by z^{+}=v+\alpha\Delta z with \alpha\in(0,1] chosen in Section IV-B. LEAP uses \theta=1.

Dividing (18) by h\theta shows that \Delta z solves (H+\tfrac{1}{h\theta}I)\Delta z=-\tfrac{1}{\theta}\nabla_{z}L_{A}: a Levenberg–Marquardt step with damping 1/(h\theta), which is a gradient step as h\to 0 and the Gauss–Newton step as h\to\infty. The pseudo-time step therefore doubles as a trust-region radius, and the question of how large a step to take becomes the question of how far to trust the model H. The next lemma provides the quantity that answers it.

Lemma 9 (Descent and predicted decrease).

For every h>0 and \theta\in[\tfrac{1}{2},1], the step \Delta z of (18) exists, is unique, and satisfies \nabla_{z}L_{A}^{T}\Delta z<0 whenever \nabla_{z}L_{A}\neq 0. The quadratic model m(\alpha):=L_{A}(v)+\alpha\nabla_{z}L_{A}^{T}\Delta z+\tfrac{\alpha^{2}}{2}\Delta z^{T}H\Delta z predicts the decrease

\displaystyle\mathrm{pred}(\alpha):=m(0)-m(\alpha)>0.

Proof.

H is a sum of Gram matrices, so I+h\theta H\succeq I is invertible and \nabla_{z}L_{A}^{T}\Delta z=-h\nabla_{z}L_{A}^{T}(I+h\theta H)^{-1}\nabla_{z}L_{A}<0. From (18), \Delta z^{T}H\Delta z=-(h\nabla_{z}L_{A}^{T}\Delta z+\|\Delta z\|^{2})/(h\theta); substituting into m(\alpha) gives \mathrm{pred}(\alpha)=\alpha(1-\frac{\alpha}{2\theta})(-\nabla_{z}L_{A}^{T}\Delta z)+\frac{\alpha^{2}\|\Delta z\|^{2}}{2h\theta}, which is positive since \alpha\leq 1\leq 2\theta. ∎

IV-B Acceptance and step control

A stiff integrator chooses h from a local truncation-error estimate, which presumes a smooth vector field and misjudges the step whenever the active set changes. We instead choose h the way an optimizer would: a step is accepted only if L_{A} decreases, which makes monotonicity a property of the discrete iterates by construction, and h is grown or shrunk according to how well the model of Lemma 9 predicted that decrease. Both tests use only values of L_{A}, so neither sees the Jacobian jumps.

The linearization point v_{k} is either the last accepted iterate z_{k} or an extrapolation of z_{k} and z_{k-1}, which is discarded whenever L_{A}(v_{k},\gamma)>L_{A}(z_{k},\gamma), so that L_{A}(v_{k},\gamma)\leq L_{A}(z_{k},\gamma) in either case. The trial z^{+}(\alpha)=v_{k}+\alpha\Delta z is accepted, and becomes z_{k+1}, if and only if L_{A}(z^{+}(\alpha),\gamma)<L_{A}(v_{k},\gamma), starting at \alpha=1 and backtracking on rejection; an accepted step advances pseudo-time by \alpha h. Chaining the two inequalities gives the discrete counterpart of Proposition 6(ii):

Lemma 10 (Monotone accepted sequence).

The accepted iterates satisfy L_{A}(z_{k+1},\gamma)<L_{A}(z_{k},\gamma) for every k, with or without look-ahead.

The step h is controlled by the ratio \varrho:=\big(L_{A}(v)-L_{A}(z^{+}(\alpha))\big)/\mathrm{pred}(\alpha) of actual to predicted decrease, as in a trust-region method [18, Ch. 4]:

\displaystyle h\leftarrow\begin{cases}\min(g_{+}h,h_{\max}),&\varrho>\eta_{2}\text{ and }\alpha=1,\\
\max(g_{-}h,h_{\min}),&\varrho<\eta_{1},\\
h/2,&\alpha<1,\\
h,&\text{otherwise,}\end{cases}

with 0<\eta_{1}<\eta_{2}<1 and g_{+}>1>g_{-}>0. A large \varrho means the Gauss–Newton model is accurate and h grows toward the Gauss–Newton step; a small \varrho shrinks it toward a gradient step. The step-size controller is thus deciding how far to trust H, not how accurately to follow the flow in pseudo-time, which is why it can be insensitive to the jumps that defeat the truncation-error estimators of stiff integrators [9].

IV-C Windows, multipliers and penalties

The flow of Definition 5 holds \gamma fixed while z descends and updates \gamma only once the inner problem is near stationarity. We call the run of accepted steps between two multiplier updates a window. A window ends when its accumulated pseudo-time reaches \Delta s, or earlier when \|\nabla_{z}L_{A}\| falls below \epsilon_{g} or the decrease of L_{A} stalls. At the window boundary the feasibility criterion of Section IV-D is tested; if it fails, the multipliers are updated by (15) with \tau_{\gamma}=\tfrac{1}{2}, and any penalty whose constraints are not converging is increased.

The penalty update acts on groups of constraints rather than on all of them at once. Let \nu_{i}:=|g_{i}| for i\in\mathcal{E}_{d} and \nu_{i}:=[g_{i}]_{+} for i\in\mathcal{I}_{d} denote the violation of constraint i. The rows of one named constraint of Problem 1 form a term t with index set \mathcal{R}_{t}, violation \mathrm{cv}_{t}:=\max_{i\in\mathcal{R}_{t}}\nu_{i} and a shared penalty \mu_{t}, and a term whose violation has not contracted by \kappa_{\mu}\in(0,1) since the previous boundary has its penalty increased:

\displaystyle\mathrm{cv}_{t}>\kappa_{\mu}\,\mathrm{cv}_{t}^{\mathrm{prev}}\implies\mu_{t}\leftarrow\min(\varrho_{\mu}\mu_{t},\mu_{\max}),

where \varrho_{\mu}>1. The reason for per-term penalties is a degeneracy of the uniform choice. Suppose the window has converged, \nabla_{z}L_{A}=0, and apply (15) at that z. By (12) the gradient becomes \tau_{\gamma}\sum_{i}\mu_{i}\,c_{i}^{\mathrm{eff}}\nabla g_{i}. If \mu_{i}\equiv\mu this is \tau_{\gamma}\mu times the gradient of the infeasibility \tfrac{1}{2}\sum_{i}(c_{i}^{\mathrm{eff}})^{2}, so at an infeasible stationary point of the infeasibility the update does nothing and raising \mu does not help. Distinct \mu_{t} weight the sum differently and generically move the iterate off such a point.

IV-D Feasibility criterion

The transcription of Section III-A enforces the path constraints of Problem 1 at the CGL nodes. Deciding when the discrete solution is good enough therefore involves two errors of different origin: the violation at the nodes, which the flow drives down, and the violation between nodes, which only refining the mesh can reduce. The criterion measures both, so that the scheme can tell whether to keep flowing or to refine.

Definition 11 (Node gate, fine-grid violation and criterion).

Let

\displaystyle G(z) \\ \displaystyle:=\max_{i}\nu_{i}(z), \\ \displaystyle F(z) \\ \displaystyle:=\max_{e}F_{e}(z), \\ \displaystyle F_{e}(z) \\ \displaystyle:=\max_{t_{k}\in[t_{e},t_{e+1}]}\,\max_{r}\,\nu_{r}(t_{k}),

where t_{k}=k\Delta t is a fine grid of physical times and r runs over the path constraints of element e evaluated on its polynomials. G is the node gate, F the fine-grid violation, and the criterion is G(z)\leq\mathrm{tol} and F(z)\leq\mathrm{tol}.

A level is the sequence of windows run on one mesh. Each time a window ends, the criterion is evaluated. If it holds, the level ends and the solve is complete. If G\leq\mathrm{tol} but F>\mathrm{tol}, the nodes have converged and the remaining violation lies between them, which the flow cannot reduce; the level ends and the outer loop refines the mesh (Section IV-E). Otherwise the flow continues into the next window.

IV-E Outer loop

Two things remain outside the inner iteration. It converges to a stationary point of \min_{z}L_{A} in whose basin it starts, so a poor initial guess must first be brought into a useful basin; and it can only reduce the nodal violation G, so when a level ends with F>\mathrm{tol} the mesh must change.

Feasibility flow

Before the objective and the multipliers enter, the flow (14) is run on the special case of L_{A} with \gamma=0, a common penalty \omega and J_{d} dropped, so that L_{A} reduces to \Phi(z):=\tfrac{\omega}{2}\sum_{i}\nu_{i}(z)^{2}. The step (18) is then the Levenberg–Marquardt iteration for \min_{z}\Phi(z) [18, Ch. 10], and this stage removes the bulk of the infeasibility of the initial guess, which in our experiments is a linear interpolation between boundary poses.

h-refinement

When a level ends with F>\mathrm{tol}, each element receives the indicator \eta_{e}:=\max(\text{nodal violation on }e,\ F_{e}), and Dörfler marking [5] bisects the elements with the largest indicators at the time of their worst violation [4]. The children keep the parent’s degrees, and the iterate is transferred to the new mesh by re-interpolating the nodal fields at the children’s CGL nodes and splitting each Bézier wrench by de Casteljau subdivision [8]. Interpolation at p+1 nodes reproduces a degree-p polynomial exactly, and de Casteljau subdivision returns the same curve on each half with control points that are convex combinations of the parent’s, so Lemma 2 applies to the children. Because the constraint set changes, the multipliers are reset and the penalties reseeded; the refined iterate is nearly feasible, so the flow restarts with a large step. The solve ends when the criterion holds, when n_{\mathrm{lev}} refinements are exhausted, or when refinement stops reducing G, which indicates a wrong basin rather than a discretization error.

V Simulation experiments

This section compares LEAP against the state of the art methods Crocoddyl [17] and Aligator [11]. All methods solve instances of Problem 1 for different robot morphologies and under different constraints. Subsection V-A describes the experimental setup that will allow us to fairly compare the different methods. Subsection V-B compares optimization times and the success of the resulting trajectories under simulated tracking. Subsection V-C illustrates how each method fares when given a time budget to solve the OCP.

V-A Experimental setup

To fairly compare the different methods, we define a set of benchmark problems and criteria to evaluate the solution. All experiments are run in a machine with a 2x AMD EPYC 7742 64-Core Processor and for each experiment each method is allocated 12 physical cores.

V-A1 Benchmark Problems

The results presented here comprise ten benchmark problems across two tasks: walking over uneven terrain and jumping over a fence (Figure 1). Each task is evaluated on five robot platforms: a planar hopper(n_{j}=2), Unitree’s Go2 quadruped (n_{j}=12), H1 biped (n_{j}=19), G1 humanoid (n_{j}=29), and PAL Robotics’ Talos humanoid (n_{j}=32). All methods solve the same optimization problem to the extend their formulations allow: same cost function, constraint and initial guess.

V-A2 Solution quality criteria

Each optimization method produces optimal trajectories for the states and torques at the collocation nodes. Small constraint residuals at the collocation nodes do not ensure that a tracking controller can execute the resulting motion and reach the prescribed terminal state. We therefore evaluate the references through closed-loop simulation and assess terminal goal accuracy separately from optimization feasibility. A whole-body inverse-dynamics controller tracks each reference in MuJoCo, using the same controller configuration for all optimization methods on a given task and robot. We refer to the state obtained by numerically integrating the robot dynamics under the controller’s torques and the simulator’s contact forces as the simulated rollout.

For the solve-time comparisons, success is determined by the rollout’s error relative to the problem’s prescribed terminal rest pose and foot placements. The position errors and residual speeds must be strictly below the six thresholds in Table I, with identical numerical thresholds for every robot and method.

V-B Scalability to convergence

We compare LEAP, Crocoddyl and Aligator on the Fence and Uneven terrain benchmark. Figure 1 shows the rollout solution produced by LEAP for all the scenarios and all the robots. The goal of this subsection is to compare the solve time of the different methods for solutions whose rollout satisfy the success criteria.

Table II reports the optimization time of the reference trajectories whose simulated rollouts satisfy the terminal tracking criteria in Table I, with the five robot platforms across the columns and the two tasks down the rows. Crosses indicate that the corresponding rollout does not satisfy these criteria. Each entry is the solver time at which a method produced its final iterate within a 14400 s budget. That iterate is the reference tracked in simulation. All three methods stop early once the iterate satisfies the shared feasibility test (maximum node and fine-grid constraint violation below 10^{-3}) or their own convergence test. Across the ten displayed cases, LEAP satisfies the tracking criteria in all ten, compared with seven for Crocoddyl and eight for Aligator.

Table III summarizes each rollout by its largest normalized terminal error in the terminal state. LEAP attains the smallest normalized error on nine of the ten problems. The H1 fence-jumping experiment is the only instance where a comparison method obtains better terminal accuracy.

V-C Scalability under a time budget

We next examine how the quality of the simulated rollout improves with the available computation time for the different trajectory optimizers. For each compute budget we retain, the recorded candidate with the smallest node constraint violation available, track it in simulation with the controller of Section V-A, and report the normalized terminal error \max_{i}e_{i}/\bar{e}_{i} of Table III. Figure 2 plots this error against the budget, with uneven terrain in the left column and fence jumping in the right column. A curve below the dashed line at one means that the reference available at that budget is tracked to the terminal goal. Counting the budget from which the retained reference stays trackable to the end of the run, LEAP reaches that point first on seven of the ten problems.

VI Hardware Experiments

In this section we evaluate whether a hopscotch trajectory generated by LEAP can be executed on a physical quadruped using a tracking controller. The task combines forward jumps, alternating diagonal support, and a widened four-foot stance, as illustrated in Figure .

VI-A Hardware Setup

Experiments use a Unitree Go2 EDU quadruped with 12 actuated joints and a low-level torque-control interface. Robot measurements and control commands are exchanged through the Unitree SDK 2 [23]. Joint positions and the base orientation are obtained from onboard measurements. A Vicon motion-capture system records the position of the floating base in the torso.

VI-B Controller and System Identification

Execution in hardware follows the reference-generation and feedback-tracking approach used in Section V. To reduce the model mismatch we employ system identification [27, 25] and the tracking controller is trained with domain randomization to accommodate remaining discrepancies between simulation and hardware. The controller is inspired by Deep Tracking Control [12] and observes joint angles, floating-base position and orientation, and base angular velocity. LEAP supplies the reference motion, while the controller provides feedback corrections during execution.

VI-C Results

Table IV summarizes the errors for the execution shown in Figure . This execution demonstrates that a LEAP reference can guide the robot through the hopscotch sequence with centimeter-scale RMS position errors.

VII Conclusion

This paper presented LEAP, a gradient-flow method for trajectory optimization of robots with prescribed contact sequences. The method combines pseudospectral collocation with a two-time-scale PHR augmented-Lagrangian flow whose equilibria satisfy the KKT conditions of the transcribed problem. Bézier wrench parameterizations certify convex contact-wrench constraints throughout each interval when their control points are feasible. A linearly implicit Gauss–Newton integrator preserves descent at fixed multipliers and penalties, 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 and centimeter-scale RMS position errors. We will release all code upon final acceptance of this paper

References

1. [1] C. E. Adu, C. E. R. Chuquiure, B. Zhang, and R. Vasudevan (2025) Bring the heat: rapid trajectory optimization with pseudospectral techniques and the affine geometric heat flow equation. IEEE Robotics and Automation Letters 10 (4), pp. 4148–4155. External Links: Document
2. [2] C. E. Adu, C. E. R. Chuquiure, Y. Zhou, P. Lin, R. Yang, B. Zhang, S. Singh, and R. Vasudevan (2025) Phasing through the flames: rapid motion planning with the aghf pde for arbitrary objective functions and constraints. External Links: 2505.01589, Link
3. [3] S. A. Burden, H. Gonzalez, R. Vasudevan, R. Bajcsy, and S. S. Sastry (2015) Metrization and simulation of controlled hybrid systems. IEEE Transactions on Automatic Control 60 (9), pp. 2307–2320. External Links: Document
4. [4] C. L. Darby, W. W. Hager, and A. V. Rao (2011) An hp-adaptive pseudospectral method for solving optimal control problems. Optimal Control Applications and Methods 32 (4), pp. 476–502.
5. [5] W. Dörfler (1996) A convergent adaptive algorithm for Poisson’s equation. SIAM Journal on Numerical Analysis 33 (3), pp. 1106–1124.
6. [6] Y. Fan, S. Liu, and M. Belabbas (2020) Mid-air motion planning of robot using heat flow method with state constraints. Mechatronics 66, pp. 102323. External Links: ISSN 0957-4158, Document, Link
7. [7] Y. Fan, S. Liu, and M. Belabbas (2021) Geometric heat flow method for legged locomotion planning. IEEE Control Systems Letters 5 (3), pp. 941–946. External Links: Document
8. [8] G. Farin (2002) Curves and surfaces for CAGD: a practical guide. 5 edition, Morgan Kaufmann, San Francisco, CA.
9. [9] E. Hairer and G. Wanner (1996) Solving ordinary differential equations II: stiff and differential-algebraic problems. 2 edition, Springer, Berlin.
10. [10] M. R. Hestenes (1969) Multiplier and gradient methods. Journal of Optimization Theory and Applications 4 (5), pp. 303–320.
11. [11] W. Jallet, A. Bambade, E. Arlaud, S. El-Kazdadi, N. Mansard, and J. Carpentier (2025) ProxDDP: proximal constrained trajectory optimization. IEEE Transactions on Robotics 41 (), pp. 2605–2624. External Links: Document
12. [12] F. Jenelten, J. He, F. Farshidian, and M. Hutter (2024) DTC: deep tracking control. Science Robotics 9 (86), pp. eadh5401. External Links: Document, Link, https://www.science.org/doi/pdf/10.1126/scirobotics.adh5401
13. [13] S. Kim and H. Park (2025) EL-aghf: extended lagrangian affine geometric heat flow. External Links: 2505.24751, Link
14. [14] S. Liu and M. A. Belabbas (2019) A homotopy method for motion planning. arXiv preprint arXiv:1901.10094.
15. [15] S. Liu, Y. Fan, and M. Belabbas (2019) Affine geometric heat flow and motion planning for dynamic systems. IFAC-PapersOnLine 52 (16), pp. 168–173. Note: 11th IFAC Symposium on Nonlinear Control Systems NOLCOS 2019 External Links: ISSN 2405-8963, Document, Link
16. [16] S. Liu, Y. Fan, and M. Belabbas (2020) Geometric motion planning for affine control systems with indefinite boundary conditions and free terminal time. arXiv preprint arXiv:2001.04540.
17. [17] C. Mastalli, W. Merkt, J. Marti-Saumell, H. Ferrolho, J. Solà, N. Mansard, and S. Vijayakumar (2022) A feasibility-driven approach to control-limited ddp. Autonomous Robots 46 (8), pp. 985–1005. External Links: Document, ISBN 1573-7527, Link
18. [18] J. Nocedal and S. J. Wright (2006) Numerical optimization. 2 edition, Springer, New York.
19. [19] M. J. D. Powell (1969) A method for nonlinear constraints in minimization problems. In Optimization, R. Fletcher (Ed.), pp. 283–298.
20. [20] (2017) Robot dynamics lecture notes. Note: Lecture notes, ETH Zürich
21. [21] R. T. Rockafellar (1973) The multiplier method of Hestenes and Powell applied to convex programming. Journal of Optimization Theory and Applications 12 (6), pp. 555–562.
22. [22] L. N. Trefethen (2000) Spectral methods in MATLAB. SIAM, Philadelphia, PA.
23. [23] Unitree Robotics unitree_sdk2: unitree robot sdk version 2. Note: https://github.com/unitreerobotics/unitree_sdk2GitHub repository, accessed Sep. 16, 2026
24. [24] R. Verschueren, G. Frison, D. Kouzoupis, J. Frey, N. v. Duijkeren, A. Zanelli, B. Novoselnik, T. Albin, R. Quirynen, and M. Diehl (2022) Acados—a modular open-source framework for fast embedded optimal control. Mathematical Programming Computation 14 (1), pp. 147–183. External Links: Document, ISBN 1867-2957, Link
25. [25] B. Zhang, D. Haugk, and R. Vasudevan (2024) System identification for constrained robots. arXiv preprint arXiv:2408.08830.
26. [26] B. Zhang and R. Vasudevan (2024) Rapid and robust trajectory optimization for humanoids. External Links: 2409.00303, Link
27. [27] B. Zhang, Z. Zhou, and R. Vasudevan (2025) Provably-safe, online system identification. arXiv preprint arXiv:2504.21486.