Wilson Jallet

Antoine Bambade

Etienne Arlaud

Sarah El-Kazdadi

Nicolas Mansard

Justin Carpentier

TRANSACTIONS ON ROBOTICS 1 PROXDDP: Proximal Constrained Trajectory Optimization

Keywords: Optimization and Optimal Control, Legged Robots, Model-Predictive Control

Trajectory optimization (TO) has proven, over the last decade, to be a versatile and effective framework for robot control. Several numerical solvers have been demonstrated to be fast enough to allow recomputing full-dynamics trajectories for various systems at control time, enabling model predictive control (MPC) of complex robots. These first implementations of MPC in robotics predominantly utilize some differential dynamic programming (DDP) variant for its computational speed and ease of use in constraint-free settings. Nevertheless, many scenarios in robotics call for adding hard constraints in TO problems (e.g., torque limits, obstacle avoidance), which existing solvers, based on DDP, often struggle to handle. Effectively addressing path constraints still poses optimization challenges (e.g., numerical stability, efficiency, accuracy of constraint satisfaction) that we propose to solve by combining advances in numerical optimization with the foundational efficiency of DDP. In this article, we leverage proximal methods for constrained optimization and introduce a DDP-like method to achieve fast, constrained trajectory optimization with an efficient warm-starting strategy particularly suited for MPC applications. Compared to earlier solvers, our approach effectively manages hard constraints without warm-start limitations and exhibits commendable convergence accuracy. Additionally, we leverage the computational efficiency of DDP, enabling real-time resolution of complex problems such as whole-body quadruped locomotion. We provide a complete implementation as part of an open-source and flexible C++ trajectory optimization library called ALIGATOR. These algorithmic contributions are validated through several trajectory planning scenarios from the robotics literature and the real-time whole-body MPC of a quadruped robot.

I. INTRODUCTION

T RAJECTORY OPTIMIZATION is an efficient and generic approach for controlling complex dynamical systems such as robots. It is a principled framework for describing desired behaviors and generating motion. A workhorse in modern robotics, it has become a crucial ingredient in both kinodynamic planning and model predictive control (MPC) over the past decade, enabled by the increasing performance of computer chips and algorithmic enhancements alleviating previous computational bottlenecks. Notably, recent progress in both software and hardware has enabled the real-time computation of numerical quantities commonly involved in trajectory optimization (e.g., rigid-body kinematics, dynamics and their derivatives [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF]- [START_REF] Carpentier | Analytical derivatives of rigid body dynamics algorithms[END_REF]). Optimal control problems (OCPs) are, by nature, infinitedimensional optimization problems, which are largely not solvable in closed form. However, they can be solved numerically. On the one hand, there are indirect methods for OCPs, based on deriving their optimality conditions [START_REF] Farshidian | An efficient optimal planning and control framework for quadrupedal locomotion[END_REF]. On the other hand, there are direct methods [START_REF] Diehl | Fast direct multiple shooting algorithms for optimal robot control[END_REF] which transcribe OCP problems into nonlinear programs (NLPs) of finite dimensions.

Direct methods, whichever the method of transcription, attempt resolution by utilizing a nonlinear programming approach, either leveraging general-purpose and off-the-shelf solvers such as IPOPT [START_REF] Wächter | On the implementation of an interiorpoint filter line-search algorithm for large-scale nonlinear programming[END_REF] or SNOPT [START_REF] Gill | Snopt: An sqp algorithm for large-scale constrained optimization[END_REF], or a more tailored solution. Several approaches are considered in the literature to solve them in practice. We will argue why, in the robotics community, differential dynamic programming-based solvers are seen as a promising research direction, notably for deploying receding horizon control schemes for real-time robot control. One transcription method is collocation, which approximates the OC problem using a finite-dimensional basis functions such as polynomials. Another transcription method is shooting methods. They use a discretization of the system dynamics through numerical integration, which is generic, efficient, and easy to implement. For these shooting methods, there exist structureexploiting solvers leveraging Riccati recursion [START_REF] De | Differential dynamic programming and newton's method[END_REF]- [START_REF] Giftthaler | A family of iterative gauss-newton shooting methods for nonlinear optimal control[END_REF], such as the differential dynamic programming (DDP) [START_REF] Mayne | A second-order gradient method for determining optimal trajectories of non-linear discrete-time systems[END_REF] algorithm. DDP is one of the earliest such methods and a reference in nonlinear trajectory optimization, known to have quadratic convergence [START_REF] Murray | Differential dynamic programming and newton's method for discrete optimal control problems[END_REF], and has several variants such as the iterative linear quadratic regulator (iLQR) [START_REF] Li | Iterative linear quadratic regulator design for nonlinear biological movement systems[END_REF], [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF] (which is in fact a simplified version of DDP). DDP methods are a popular choice in robotics as they are relatively easy to implement with performance in mind [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF], which is a crucial requirement in the field. They have enabled complex applications such as whole-body nonlinear MPC in robotics [START_REF] Dantec | Whole body model predictive control with a memory of motion: Experiments on a torque-controlled talos[END_REF], [START_REF] Assirelli | Wholebody mpc without foot references for the locomotion of an impedancecontrolled robot[END_REF].

However, standard Riccati recursion, and by extension DDP, is not meant for constrained problems. A classic approach to handle this in robotics is to use soft penalty methods [START_REF] Dantec | Whole body model predictive control with a memory of motion: Experiments on a torque-controlled talos[END_REF], [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF]. The issue with this approach is having to tune the penalty weights to avoid ill-conditioning issues while adequately enforcing the constraints and still getting the desired behavior in the system. After years of practice with advanced MPC in various experimental setups, we firmly believe that stricter enforcement of constraints is required to bring MPC on complex robots to move outside the labs. The central objective of this paper is to enrich DDP with a sound method for handling hard constraints.

Several attempts have been made in that direction. Prior work has extended the Riccati recursion to the equalityconstrained case [START_REF] Giftthaler | A projection approach to equality constrained iterative linear quadratic optimal control[END_REF]- [START_REF] Vanroye | A generalization of the riccati recursion for equality-constrained linear quadratic optimal control[END_REF] using factorizations such as LU or QR decompositions; these methods can be used for solving subproblems in DDP or sequential quadratic programming (SQP) schemes. Unfortunately, these approaches do not tackle inequality constraints and would need to be combined with another approach for this purpose. Furthermore, the factorizations at each stage of the algorithm require additional assumptions on the problem constraints (e.g. positive definite reduced Hessians at every stage), which an off-the-shelf sparse solver such as Cholmod [START_REF] Davis | Algorithm 849: A concise sparse cholesky factorization package[END_REF] would not need when applied to the entire problem. Projection approaches have also been used for boxinequality constraints such as torque limits [START_REF] Tassa | Control-limited differential dynamic programming[END_REF], [START_REF] Mastalli | A feasibility-driven approach to control-limited ddp[END_REF] by solving a quadratic program (QP) in a modified DDP backward pass. Other approaches for inequality constraints, such as interiorpoint methods, have been employed in HPIPM [START_REF] Frison | Hpipm: A high-performance quadratic programming framework for model predictive control[END_REF], [START_REF] Frison | Introducing the quadratically-constrained quadratic programming framework in hpipm[END_REF] and by Pavlov [START_REF] Pavlov | Interior point differential dynamic programming[END_REF] (the two former focusing on for QP-and QCQP-like optimal control problems). Although interior-point methods can obtain good precision in nonlinear programming settings [START_REF] Wächter | On the implementation of an interiorpoint filter line-search algorithm for large-scale nonlinear programming[END_REF], the approach is not always suitable for MPC applications due to having limited warm-starting capabilities [START_REF] Wang | Fast model predictive control using online optimization[END_REF]. Closely related are barrier methods [START_REF] Nocedal | Numerical Optimization[END_REF], which have been used in trajectory optimization by [START_REF] Hauser | A barrier function method for the optimization of trajectory functionals with constraints[END_REF], [START_REF] Feller | Relaxed logarithmic barrier function based model predictive control of linear systems[END_REF] and MPC by Grandia [START_REF] Grandia | Feedback mpc for torque-controlled legged robots[END_REF] using a "relaxed" barrier approach. However, these barrier approaches do not solve inequality constraints exactly: they penalize them more or less aggressively, leaving some margin of error in the optimality conditions or letting through infeasible trajectories, and like penalty methods, they can suffer from ill-conditioning. The aforementioned work still leaves us with the need for a TO solver to deal with generic constraints with less need for penalty tuning, more accurate constraint satisfaction, and better warm-starting capacities, especially for real-time robot control.

In nonlinear constrained optimization, methods based on the augmented Lagrangian (AL) [START_REF] Hestenes | Multiplier and gradient methods[END_REF], [START_REF] Rockafellar | The multiplier method of hestenes and powell applied to convex programming[END_REF] are a broad class of exact penalty methods that have been popular with implementations such LANCELOT [START_REF] Conn | Lancelot: A Fortran Package for Large-Scale Nonlinear Optimization (Release A)[END_REF]. Their application to trajectory optimization problems has been discussed [START_REF] Toussaint | A novel augmented lagrangian approach for inequalities and convergent any-time non-central updates[END_REF], [START_REF]A tutorial on newton methods for constrained trajectory optimization and relations to slam, gaussian process smoothing, optimal control, and probabilistic inference[END_REF], namely with methods such as ALTRO [START_REF] Howell | Altro: A fast solver for constrained trajectory optimization[END_REF], [START_REF] Jackson | Altro-c: A fast solver for conic model-predictive control[END_REF] for equality and inequality constraints. Sleiman et al. [START_REF] Sleiman | Constraint handling in continuous-time ddp-based model predictive control[END_REF] have also employed AL methods for integrating inequality constraints in continuous-time DDP, studying multiple smooth and non-smooth penalty choices. One difficulty with AL methods is that they can be numerically brittle when encountering ill-conditioned systems of equations, and hence, obtaining good convergence in AL methods has been an issue: [START_REF] Howell | Altro: A fast solver for constrained trajectory optimization[END_REF], [START_REF] Aoyama | Constrained differential dynamic programming revisited[END_REF] have to use a second, projection-based stage after AL to refine the solutions to convergence. Recently, [START_REF] Kazdadi | Equality constrained differential dynamic programming[END_REF] leveraged a penalty-scheduling [START_REF] Conn | A globally convergent augmented lagrangian algorithm for optimization with general constraints and simple bounds[END_REF] as well as a primal-dual system of equations similar to [START_REF] Hermans | Qpalm: A newton-type proximal augmented lagrangian method for quadratic programs[END_REF], [START_REF] Bambade | Prox-qp: Yet another quadratic programming solver for robotics and beyond[END_REF] to ensure more reliable convergence for these methods in the equality-constrained case. In our prior work [START_REF] Jallet | Implicit differential dynamic programming[END_REF], [START_REF] Jallet | Constrained differential dynamic programming: A primal-dual augmented lagrangian approach[END_REF], we have highlighted the link between such AL approaches and proximal methods in optimization, and we argue in this paper that they provide a strong candidate for a framework integrating AL with DDP-based solvers.

In this article, we propose a primal-dual proximal-point formulation for constrained trajectory optimization and derive an efficient structure-exploiting algorithm combining AL and DDP. In Section II, we first provide a background on nonlinear optimization and augmented Lagrangian methods, with an accent on primal-dual proximal methods. Section III recalls the principles behind trajectory optimization with DDP, which will be useful in formulating our contribution, the proximal DDP algorithm presented in Section IV. Our method is provided with its own C++ implementation, a novel trajectory optimization library named ALIGATOR, discussed in Section VI, with experiments showcasing it and the method's capabilities in Section VII.

This article is an extended version of our previous work on implicit differentiable dynamic programming [START_REF] Jallet | Implicit differential dynamic programming[END_REF] and constrained differentiable dynamic programming [START_REF] Jallet | Constrained differential dynamic programming: A primal-dual augmented lagrangian approach[END_REF]. Compared to our previous work, this article provides (i) a unified view on combining primal-dual proximal augmented Lagrangian techniques within DDP-based solvers, (ii) a more appropriate merit function inspired by [START_REF] Marchi | On a primal-dual newton proximal method for convex quadratic programs[END_REF] for better globalization, (iii) an open-source and highly efficient implementation of the proposed solution within a new C++ framework for trajectory optimization, as well as (iv) further additional experiments, especially some real-world MPC control applications of the proposed approach with real-time control of the SOLO quadrupedal robot.

II. BACKGROUND ON NONLINEAR OPTIMIZATION AND

AUGMENTED LAGRANGIAN METHODS This section provides a general overview of nonlinear optimization and augmented Lagrangian methods. These optimization techniques are at the core of the approach developed in this article. This approach is similar to prior work leveraging semismooth Newton methods for the augmented Lagrangian [START_REF] Birgin | Improving ultimate convergence of an augmented lagrangian method[END_REF] (for general NLPs) and [START_REF] Hermans | Qpalm: A newton-type proximal augmented lagrangian method for quadratic programs[END_REF], [START_REF] Bambade | Prox-qp: Yet another quadratic programming solver for robotics and beyond[END_REF], [START_REF] Marchi | On a primal-dual newton proximal method for convex quadratic programs[END_REF] (for QPs). Its novelty is the use of a primal-dual semi-smooth Newton method for inequality constraints in the augmented Lagrangian for general NLPs (used for QPs by [START_REF] Bambade | Prox-qp: Yet another quadratic programming solver for robotics and beyond[END_REF], [START_REF] Marchi | On a primal-dual newton proximal method for convex quadratic programs[END_REF]), along with the use of the parameter update rule of [START_REF] Conn | A globally convergent augmented lagrangian algorithm for optimization with general constraints and simple bounds[END_REF] in this setting. This specific variant of the augmented Lagrangian method is used as a basis for our main contribution in Section IV. A further detailed comparison with prior work is provided at the end of this section (Subsection II-E).

A. Nonlinear constrained optimization

Trajectory optimization problems may often be transcribed as nonlinear programming problems (NLPs). NLPs with both equality and inequality constraints correspond to optimization problems of the form:

min z∈Z f (z) (1a) s.t. g(z) = 0, (1b) h(z) ⩽ 0, (1c)
with z ∈ Z the optimization variable, where Z = R nz , f : Z → R is the twice-differentiable cost function and g : Z → R ne (resp. h : Z → R ni ) are the differentiable equality (resp. inequality) constraints.

The first-order necessary conditions of optimality are given by the so-called Karush-Kuhn-Tucker (KKT) conditions [START_REF] Nocedal | Numerical Optimization[END_REF], which is that for an optimal point z, there exist Lagrange multipliers (λ, ν) satisfying:

∇ z f (z) + ∂g ∂z ⊤ λ + ∂h ∂z ⊤ ν = 0, (2a)
g(z) = 0, (2b) h(z) ⩽ 0 ⊥ ν ⩾ 0. ( 2c
)
The complementarity conditions (2c), mean that for every i, h j (z) ⩽ 0, ν i ⩾ 0 and either one or the other is zero. When h j (z) = 0, ν j > 0 and the i th constraint is said to be active.

The nonlinear programming problem can be seen as that of finding a saddle-point [START_REF] Nocedal | Numerical Optimization[END_REF], [START_REF] Boyd | Convex Optimization[END_REF] of the so-called Lagrangian function:

L(z, λ, ν) = f (z) + λ ⊤ g(z) + ν ⊤ h(z) if ν ⩾ 0, -∞ otherwise. (3)

B. Augmented Lagrangian methods

Our approach is fundamentally based on augmented Lagrangian-type methods [START_REF] Hestenes | Multiplier and gradient methods[END_REF], [START_REF] Rockafellar | The multiplier method of hestenes and powell applied to convex programming[END_REF]. We consider the classical Powell-Hestenes-Rockafellar (PHR) augmented Lagrangian penalty function, which is defined as the value of the dual proximal point of the Lagrangian:

L µ (z; λ e , ν e ) = max (λ,ν) ν⩾0 L(z, λ, ν) -µ 2 ∥(λ, ν) -(λ e , ν e )∥ 2 2 (4)
where µ > 0 is the augmented Lagrangian penalty parameter. The maximizers (see Appendix A) in (λ, ν) are the following mappings, called first-order multiplier estimates

λ + (z) def = λ e + 1 µ g(z), (5a)
ν + (z) def = ν e + 1 µ h(z) + . ( 5b
)
Injecting the new updated multiplier values into the Lagrangian function leads to:

L µ (z; λ e , ν e ) = f (z) + 1 2µ ∥g(z) + µλ e ∥ 2 -µ 2 ∥λ e ∥ 2 + 1 2µ ∥[h(z) + µν e ] + ∥ 2 -µ 2 ∥ν e ∥ 2 . (6)
The gradient of the augmented Lagrangian with respect to the primal variable z reads:

∇ z L µ (z; λ e , ν e ) = ∇ z L(z, λ + (z), ν + (z)). (7)
The solution z * to ∇ z L µ = 0 and the multipliers (λ + (z * ), ν + (z * )) can be summarized as the following set of equations:

T µ (z, λ, ν; λ e , ν e ) =   ∇L(z, λ, ν) g(z) + µ(λ e -λ) [h(z) + µν e ] + -µν   = 0, (8)
where T µ is the proximal-KKT operator [START_REF] Hermans | Qpalm: A newton-type proximal augmented lagrangian method for quadratic programs[END_REF], [START_REF] Bambade | Prox-qp: Yet another quadratic programming solver for robotics and beyond[END_REF], [START_REF] Marchi | Augmented lagrangian methods as dynamical systems for constrained optimization[END_REF].

The augmented Lagrangian method. The prototypical augmented Lagrangian method or method of multipliers [START_REF] Hestenes | Multiplier and gradient methods[END_REF] consists of minimizing the augmented Lagrangian [START_REF] Wächter | On the implementation of an interiorpoint filter line-search algorithm for large-scale nonlinear programming[END_REF], updating the dual variables, and repeating until convergence. This process is outlined in the abstract Algorithm 1. Decreasing the penalty parameter µ k can accelerate convergence [START_REF] Nocedal | Numerical Optimization[END_REF]. Global convergence of the method in the convex case (under feasibility of the dual problem) was proven by Rockafellar [52, Theorem 4] using proximal-point arguments. It was also shown that the penalty parameter µ k can be kept bounded from below in this setting. Work on generalizing these theoretical results is still an active research topic [START_REF]Advances in convergence and scope of the proximal point algorithm[END_REF].

Algorithm 1: Prototypical ALM algorithm.

Data: objective f , constraint functions g, h, initial guess z 0 // Outer loop

1 for k = 0 to k max do 2 Find a minimizer z k+1 of L µ k (z; λ k , ν k ) // inner loop // first-order multiplier updates 3 λ k+1 = λ k + 1 µ k g(z k+1 ); 4 ν k+1 = [ν k + 1 µ k h(z k+1 )] + ; 5 if converged then 6 return (z k+1 , λ k+1 , ν k+1 ); 7
Choose µ k+1 > 0;

Stopping criteria. The algorithm is deemed to have converged when g(z k ) has converged to zero and there exists ν * such that ν k → ν * and the following equivalent conditions are satisfied:

(i) h(z * ) ⩽ 0 ⊥ ν * ⩾ 0 (complementarity) (ii) max(ν * , -h(z * )) = 0 (iii) ν * = ν * + 1 µ h(z * ) + .
These two sets of conditions imply that the multiplier sequence (λ k , ν k ) has converged (this stems from using the first-order update). In addition, since the multipliers converged, we have

∇L µ (z * ; λ * , ν * ) = ∇L(z * , λ * , ν * ) = 0. All put together, this implies that (z * , λ * , ν * ) is a KKT point.
The augmented Lagrangian method is an exact penalty method [29, chap. 17], and can also be interpreted as a sequence of shifted penalty methods as discussed in Appendix A. Outside of specific settings (such as equality-constrained QPs), closedform minimization of the augmented Lagrangian L µ (Alg. 1, Line 2) is impossible in practice. Thus, the algorithm has to rely on finding an approximate minimizer of:

min z L µ k (z; λ k , ν k ), (9)
corresponding to the "inner loop" of the prototypical ALM algorithm 1. This raises the question of how accurate this minimizer needs to be, and several approaches might be considered, such as inexact minimization strategies, as detailed below. Furthermore, for quick convergence, one needs to carefully adapt µ k according to the progress of the problem feasibility, which should be done in the outer loop, as done, for instance, in the bound-constrained Lagrangian update strategy [START_REF] Conn | A globally convergent augmented lagrangian algorithm for optimization with general constraints and simple bounds[END_REF], that we recall below.

C. Solving the inner loop via inexact minimization

A practical algorithm has to perform approximate minimization of L µ . The use of inexact minimizers in the AL method has been largely studied and proven in the optimization literature, leading to efficient solutions for solving general NLP problems [START_REF] Rockafellar | The multiplier method of hestenes and powell applied to convex programming[END_REF], [START_REF] Conn | A globally convergent augmented lagrangian algorithm for optimization with general constraints and simple bounds[END_REF], [START_REF]Monotone operators and the proximal point algorithm[END_REF]. In particular, the authors of [START_REF] Conn | A globally convergent augmented lagrangian algorithm for optimization with general constraints and simple bounds[END_REF] provide a practical method named bound-constrained Lagrangian (BCL) for controlling both the inexactness in the AL minimization and the penalty schedule, with convergence guarantees. The minimization is done within a scheduled tolerance ω k > 0 on a stopping criterion for the subproblem.

First-order methods for minimizing the augmented Lagrangian function have been investigated [START_REF] Birgin | Nonmonotone spectral projected gradient methods on convex sets[END_REF], as well as second-order methods in the equality-constrained case [START_REF] Conn | Lancelot: A Fortran Package for Large-Scale Nonlinear Optimization (Release A)[END_REF]. Recently, a bigger focus has been put on second-order methods for the inequality-constrained case without using slack variables [START_REF] Birgin | Improving ultimate convergence of an augmented lagrangian method[END_REF]. For instance, the QP solvers QPALM [START_REF] Hermans | Qpalm: A newton-type proximal augmented lagrangian method for quadratic programs[END_REF], PROXQP [START_REF] Bambade | Prox-qp: Yet another quadratic programming solver for robotics and beyond[END_REF], and QPDO [START_REF] Marchi | On a primal-dual newton proximal method for convex quadratic programs[END_REF] rely on semi-smooth Newton methods applied to the augmented Lagrangian function.

Linear system. The semi-smooth Newton method solves the following linear system

H µ k δz = -∇ z L µ k (z; λ k , ν k ), (10)
where H µ k is defined by

H µ k = H + 1 2µ k (A ⊤ A + B ⊤ I B I ) (11)
with

H = ∇ 2 z L(z, λ + k (z), ν + k (z))
is the Lagrangian Hessian1 , A = ∂g/∂z, B = ∂h/∂z are the constraint Jacobian matrices and B I is the set of rows of B corresponding to the active set of constraints I = I k (z), defined at point z ∈ Z as

I k (z) def = {1 ⩽ j ⩽ m | h j (z) + µ k ν k,j ⩾ 0}. ( 12
)
The linear system (10) is often poorly conditioned. It is suggested [START_REF] Nocedal | Numerical Optimization[END_REF], [START_REF] Kazdadi | Equality constrained differential dynamic programming[END_REF], [START_REF] Hermans | Qpalm: A newton-type proximal augmented lagrangian method for quadratic programs[END_REF], to rewrite it as a primal-dual system of normal equations:

  H A ⊤ B ⊤ I A -µ k I B I -µ k I     δz * *   = -   ∇L µ k 0 0   ( 13
)
where the second and third block unknowns are not used -only δz is used. Equations ( 13) and [START_REF] Neunert | Fast nonlinear model predictive control for unified trajectory optimization and tracking[END_REF] are equivalent, where the latter can be obtained from the former by Schur complement.

Furthermore [START_REF] Bambade | Prox-qp: Yet another quadratic programming solver for robotics and beyond[END_REF], equation ( 13) can be rewritten, given any (λ, ν), as an equivalent SQP iteration-like system  

H A ⊤ B ⊤ I A -µ k I B I -µ k I     δz δλ δν   = -   ∇f + A ⊤ λ + B ⊤ I ν µ k (λ + (z) -λ) µ k (ν + (z) -ν)   . (14)
The derivation of a primal-dual step akin to SQP methods highlights the possibility of incorporating the dual variables into the line-search merit function, enabling the search of optimal solutions for primal and dual variables simultaneously.

Given the search direction δz and α ∈ (0, 1], a candidate point z is given by

z ← z + αδz. ( 15
)
This is the classical scheme, where the appropriate stopping criterion would be the AL gradient

∥∇L µ k (z; λ k , ν k )∥ ∞ ⩽ ω k .
Primal-dual search. Yet, the equivalent system ( 14) suggests also including search directions in (λ, ν), as done by [START_REF] Marchi | On a primal-dual newton proximal method for convex quadratic programs[END_REF], [START_REF] Gill | A primal-dual augmented lagrangian[END_REF].

In each subproblem, the dual variables are to be initialized as

λ ← λ k , ν ← ν k .
Then, these variables are updated in SQP fashion using the dual step computed from ( 14)

λ ← λ + αδλ, ν ← ν + αδν, (16)
where α is the same step size over the dual variables as the one over primal variables, which we choose for simplicity.

Linesearch and the merit function. The globalization strategy we adopt is based on a linesearch procedure [29, chap. 3] using a merit function, a function for which the computed search direction is a descent direction. Several merit functions can be envisaged. Depending on the chosen scheme, one may use the augmented Lagrangian L µ k for linesearch over the primal variable z only. An alternative solution is the primal-dual augmented Lagrangian introduced by Gill and Robinson [START_REF] Gill | A primal-dual augmented lagrangian[END_REF] and recently extended to the inequality-constrained case by De Marchi [START_REF] Marchi | On a primal-dual newton proximal method for convex quadratic programs[END_REF] (we provide a derivation in Appendix B):

M µ (z, λ, ν; λ e , ν e ) = f (z) + 1 µ ∥g(z) + µ(λ -λ e /2)∥ 2 + µ 4 ∥λ∥ 2 + 1 µ ∥[h(z) + µ(ν -ν e /2)] + ∥ 2 + µ 4 ∥ν∥ 2 . ( 17
)
In this case, the linesearch is performed over the primal and dual variables (z, λ, ν).

The linesearch procedure checks for the Armijo condition: at a new proposal primal-dual point (z + , λ + , ν + ) given by a trial step size α ∈ (0, 1], we check if

M µ (z + , λ + , ν + ; λ e , ν e ) ⩽ M µ (z, λ, ν; λ e , ν e ) + c 1 α∇M ⊤ µ (δz, δλ, δν) (18)
where c 1 ∈ (0, 1/2) is a given hyperparameter; typical values in the literature [START_REF] Nocedal | Numerical Optimization[END_REF], [START_REF] Conn | A globally convergent augmented lagrangian algorithm for optimization with general constraints and simple bounds[END_REF] include {10 -2 , 10 -3 , 10 -4 }. In practice, our implementation of the linesearch procedure uses up to third-order polynomial interpolation as described in [29, ch. 3]. To solve the interpolation problem, we use a QR decomposition of the linear system instead of the explicit formula in [29, ch. 3], which we found less numerically stable and prone to numerical underflow.

Inner-loop stopping criterion. When using the primal-dual search, an appropriate metric on the primal-dual convergence is given by the following infinity norm:

r k (z, λ, ν) def =   ∇ z L(z, λ, ν) µ k (λ + k (z) -λ) µ k (ν + k (z) -ν)   ∞ . (19)
As stopping criteria, we consider (z, λ, ν) to be sufficiently optimal for the inner loop when r k (z, λ, ν) ⩽ ω k .

D. Outer-loop parameter updates and stopping criterion

Once the subproblem ( 9) is approximately solved, either the multiplier update ( 5) is applied, or the penalty parameter µ k is decreased to

µ k+1 = µ f µ k , depending on if the new constraint violation ∥(g, h -[h + µ k ν k ] -)∥ ∞ is within another tolerance η k > 0.
When one or the other algorithm branch is taken, the subproblem tolerances (ω k , η k ) are updated following a geometric rule.

Akin to [START_REF] Bambade | Prox-qp: Yet another quadratic programming solver for robotics and beyond[END_REF], our approach uses the penalty-scheduling BCL method of [START_REF] Conn | Lancelot: A Fortran Package for Large-Scale Nonlinear Optimization (Release A)[END_REF], [START_REF] Conn | A globally convergent augmented lagrangian algorithm for optimization with general constraints and simple bounds[END_REF], which, to the best of our knowledge, had not been used with alongside a primal-dual semi-smooth Newton for the augmented Lagrangian in the inequality constrained case. In contrast, the reference paper and solver for BCL [START_REF] Conn | Lancelot: A Fortran Package for Large-Scale Nonlinear Optimization (Release A)[END_REF], [START_REF] Conn | A globally convergent augmented lagrangian algorithm for optimization with general constraints and simple bounds[END_REF] consider non-negativity (z ⩾ 0) or box constraints (l ⩽ z ⩽ u) on the optimization variable z.

We use the constraint violation measure ∥g(z k+1 )∥ ∞ for equality constraints to assess overall convergence. For inequalities, this measure is ∥h(z k+1 ) -[h(z k+1 ) + µ k ν k ] -∥ ∞ , which fully accounts for the complementarity conditions (2c).

E. Summary of the approach and software implementation

The overall approach, including how the subproblem tolerances and penalty are updated, is summarized in Algorithm 2. We also provide an efficient C++ implementation of this primaldual proximal NLP solver, called PROXNLP 2 and detailed in Section VI, which notably operates on generic manifolds [START_REF]An Introduction to Optimization on Smooth Manifolds[END_REF], unlike existing off-the-shelf NLP solvers such as IPOPT [START_REF] Wächter | On the implementation of an interiorpoint filter line-search algorithm for large-scale nonlinear programming[END_REF] or SNOPT [START_REF] Gill | Snopt: An sqp algorithm for large-scale constrained optimization[END_REF].

III. BACKGROUND ON TRAJECTORY OPTIMIZATION AND DIFFERENTIAL DYNAMIC PROGRAMMING

We now move to extend the algorithm presented in Section II to resolve constrained optimal control problems. Before presenting this new approach in Section IV, we first review the fundamentals of trajectory optimization and the classic DDP algorithm it is based on.

Algorithm 2: AL method with inexact subproblem minimization.

Input: Initial guesses (z 0 , λ 0 , ν 0 ), µ f ∈ (0, 1), parameters (α ω , α η , β ω , β η ), target tolerance ϵ tol // Outer loop (ẑ, λ, ν) ← (ẑ, λ, ν)+α(δz, δλ, δν)// eq. ( 15)-( 16)

1 for k = 0 to k max do // initialize inner loop 2 (ẑ, λ, ν) ← (z k , λ k , ν k ); //
7 until r k (ẑ, λ, ν) ⩽ ω k // until eq. ( 19
); 8 Set z k+1 = ẑ; // Constraint violation measure; see subsection II-D 9 Set c k+1 = ∥(g(z k+1 ), max(h(z k+1 ), -µ k ν k ))∥ ∞ ; 10 if c k+1 ⩽ η k then // accept multipliers 11 λ k+1 = λ; 12 ν k+1 = ν; 13 µ k+1 = µ k ; 14 if max(r k , c k+1 ) ⩽ ϵ tol then 15 return (z k+1 , λ k+1 , ν k+1 );
// update tolerances following BCL (success)

16

ω k+1 = ω k µ αω k+1 ; 17 η k+1 = η k µ αη k+1 ; 18 else 19 λ k+1 = λ k ; 20 ν k+1 = ν k ; // decrease penalty 21 µ k+1 = µ f µ k ;
// update tolerances following BCL (failure)

22

ω k+1 = ω k µ βω k+1 ; 23 η k+1 = η k µ βη k+1 ; 24 ω k+1 = max(ω k+1 , 10 -2 ϵ tol )
A. Problem statement Continuous time formulation. The goal of nonlinear trajectory optimization is to approximately solve continuous-time optimal control problems of the form:

min x,u J(x, u) = T 0 ℓ t (x(t), u(t)) dt + ℓ T (x(T )) (20a) s.t. ẋ(t) = f t (x(t), u(t)), (20b) x(0) = x0 , (20c) h t (x(t), u(t)) ⩽ 0, (20d) h T (x(T )) ⩽ 0, (20e)
where x0 ∈ X is a given initial state, g t , g T , h t , and h T are constraint-defining functions, f is the time-dependant system dynamics, and ℓ t and ℓ T are the running and terminal cost functions, respectively. The objective J is called the cost functional.

x and u are the state and control trajectories respectively. Problem ( 20) is an infinite-dimensional problem that, in most cases, cannot be directly solved.

Discrete time formulation. The approximate resolution of the problem can be tackled using direct methods [START_REF] Diehl | Fast direct multiple shooting algorithms for optimal robot control[END_REF]. These methods rely on a direct transcription of problem [START_REF] Laine | Efficient computation of feedback control for equality-constrained lqr[END_REF], which is a discretization of the original problem as a finite-dimensional, nonlinear program (NLP):

min x,u J(x, u) def = N -1 t=0 ℓ t (x t , u t ) + ℓ T (x N ) (21a) s.t. f t (x t , u t , x t+1 ) = 0, (21b) x 0 = x, (21c) h t (x t , u t ) ⩽ 0, (21d) h N (x N ) ⩽ 0, (21e)
where t now denotes the discrete-time variable, a notation we will keep in the sequel. We also abuse notation by keeping the same notations f , h, ℓ, although in the discrete setting, they could be the result of numerical integration of ( 20) (e.g. using Euler or RK integration [START_REF] Bertsekas | Dynamic Programming and Optimal Control, ser. Athena Scientific optimization and computation series[END_REF]). This class of NLP is also called a discrete-time optimal control (DTOC) problem due to its inherent structure [START_REF] Diehl | Fast direct multiple shooting algorithms for optimal robot control[END_REF]. Here, we use the most generic discretization of the dynamics as an implicit integration scheme [START_REF] Jallet | Implicit differential dynamic programming[END_REF], which also covers explicit formulations that are more common in the DDP literature. This formulation encompasses path equality constraints as inequality constraints, although these are implemented separately in practice.

We introduce the following Lagrange multipliers: λ = (λ t ) t=0,...,N are the co-states (i.e. the multipliers for dynamics, with λ 0 corresponding to the initial condition x 0 = x), ν = (ν t ) t=0,...,N ⩾ 0 for the path constraints (with ν N corresponding to the terminal constraint (20c)). The Lagrangian of the problem reads

L(x, u, λ, ν) = J(x, u) + λ ⊤ 0 (x 0 -x) + N -1 t=0 λ ⊤ t+1 f t (x t , u t , x t+1 ) + ν ⊤ t h t (x t , u t ) + ν ⊤ N h N (x N ). (22)

B. KKT conditions and Hamiltonian

The necessary optimality conditions given by the so-called Lagrangian stationarity conditions [START_REF] Nocedal | Numerical Optimization[END_REF] read:

∇ x L ∇ u L = 0. ( 23
)
Expanding these conditions for each of the variables x t , u t leads to:

ℓ u + f ⊤ t,u λ t+1 + h ⊤ t,u ν t = 0 (24a) f ⊤ t,y λ t + ℓ x + f ⊤ t,x λ t+1 + h ⊤ t,x ν t = 0, (24b)
and

ℓ x + f ⊤ t,x λ 1 + h ⊤ 0,t ν 0 = 0 (24c) f ⊤ t,y λ N + ℓ N,x + h ⊤ x,N ν N = 0, (24d)
where we now use the shorthands f t,x = ∂f t /∂x, f t,u = ∂f t /∂u and so on. It is worth noticing that, in the case of explicit dynamics where f t,y = -I (where y denotes x t+1 ), we recover the equations of a discrete-time Pontryagin's principle of optimality. The feasibility and complementarity conditions read:

f t (x t , u t , x t+1 ) = 0 and 0 ⩽ ν t ⊥ -h t ⩾ 0. (25)
C. Differential dynamic programming Notation. In this section, for a vector z = (z 0 , . . . , z n ), z ⩽k denotes the prefix (z 0 , . . . , z k ) while z ⩾k denotes the suffix (z k , . . . , z n ).

Differential dynamic programming (DDP) [START_REF] Mayne | A second-order gradient method for determining optimal trajectories of non-linear discrete-time systems[END_REF], and its variants such as iLQR [START_REF] Li | Iterative linear quadratic regulator design for nonlinear biological movement systems[END_REF], [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF] are a class of secondorder gradient methods which rely on Bellman's principle of optimality to compute descent directions and efficiently exploit the sparsity induced by time.

In its classical formulation recalled below, DDP is only able to handle unconstrained instances of ( 21) of the form:

V 0 (x 0 ) def = min x,u J(x, u), s.t. x t+1 = f t (x t , u t ), t = 0, . . . , N -1, (26)
where x def = (x 0 , . . . , x N ) and u def = (u 0 , . . . , u N -1 ). The value function V t starting at time t is defined as

V t (x) def = min N -1 i=t ℓ i (x i , u i ) + ℓ T (x N ), (27)
in which the relationship x t+1 = f t (x t , u t ) still holds and x t = x. Bellman's principle of optimality states that each subproblem is linked with the solution of the next subproblem through the Bellman recursion:

V t (x) = min u ℓ t (x, u) + V t+1 (f t (x t , u t )) def = Q t (x, u) . ( 28
)
Once this backward recursion is solved from t = N -1 down to t = 0, the solution (x, u) can be reconstructed by a forward pass, as the initial state x 0 is known. In general, the recursion cannot be solved in closed form as a function of x, except for specific cases such as the linear-quadratic (LQ) problems. However, a local (secondorder) expansion of its minimum around x can be obtained if expansions of ℓ and V t+1 are available.

Backward pass. The Bellman equation can be rewritten as follows:

V

t (x) = min u Q t (x, u). ( 29
)
The idea of DDP is to solve for a quadratic model of Q t , leveraging the derivatives

Q a,t = ℓ a + f ⊤ t,a V x,t+1 , (30a)
Q ab,t = ℓ ab + V x,t+1 • f t,ab + f ⊤ a V xx,t+1 f t,b , (30b)
where the symbols a, b ∈ {x, u}. In this perspective, the iterative LQR by Tassa [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF] neglects the terms of the secondorder dynamic ∇V • f ab -which can be interpreted as a Gauss-Newton approximation of the action-value derivatives.

Minimizing the quadratic model involved in [START_REF] Nocedal | Numerical Optimization[END_REF] with respect to both x and u leads to the relation between the variations of δu t and δx t :

Q uu,t δu ⋆ t = -Q u,t -Q ux,t δx t . (31)
Inverting this equation relies on the assumption that the Hessian Q uu is positive-definite. In practice, some regularization is needed to enforce Q uu ≻ 0 as in [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF]. The resulting relationship is

δu ⋆ t = k t + K t δx t , (32)
where

k t = -Q -1 uu,t Q u,t , K t = -Q -1 uu,t Q ux,t . (33)
Forward pass. A nonlinear search is adopted in the classical DDP algorithm [START_REF] Mayne | A second-order gradient method for determining optimal trajectories of non-linear discrete-time systems[END_REF], [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF]. A candidate solution (x α , u α ) is defined by a rollout of the nonlinear dynamics with a linesearch parameter α ∈ (0, 1] on the control feedforward term:

u α t = αk t + K t (x α t -x t ) (34a) x α t+1 = f t (x α t , u α t ), t = 0, . . . N -1. (34b)
The line-search parameter α ∈ (0, 1] is chosen to satisfy an Armijo sufficient decrease condition [29, ch. 3] on the cost function, or some second-order version [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF], [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF].

D. Discussion and limitations of vanilla DDP

The strength of DDP is its efficiency, which is due to clever exploitation of the time sparsity structure of [START_REF] Vanroye | A generalization of the riccati recursion for equality-constrained linear quadratic optimal control[END_REF]. It is also relatively easy to implement. These two elements make this algorithm a popular choice for implementing MPC on complex robotic systems [START_REF] Farshidian | An efficient optimal planning and control framework for quadrupedal locomotion[END_REF], [START_REF] Dantec | Whole body model predictive control with a memory of motion: Experiments on a torque-controlled talos[END_REF], [START_REF] Koenemann | Whole-body model-predictive control applied to the hrp-2 humanoid[END_REF]. It has proven strong convergence properties [START_REF] De | Differential dynamic programming and newton's method[END_REF] and is available in modern software libraries such as CROCODDYL [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF] or OCS2 [START_REF] Farshidian | OCS2: An open source library for optimal control of switched systems[END_REF]. We recall that the weakness of the nominal algorithm is its inability to enforce strict constraints and the lack of a proper globalization strategy. Attempts have been made to use modified DDPs as a backbone for constrained solvers, using augmented Lagrangian methods, for instance [START_REF] Toussaint | A novel augmented lagrangian approach for inequalities and convergent any-time non-central updates[END_REF], [START_REF] Howell | Altro: A fast solver for constrained trajectory optimization[END_REF], [START_REF] Kazdadi | Equality constrained differential dynamic programming[END_REF]. Our proposal, in the next section, is to bring the ideas of the proximal framework we introduced in Section II together with DDP-like recursions to produce an efficient and numerically stable algorithm for constrained trajectory optimization that can run on real robots in real-time.

IV. PROXIMAL DIFFERENTIAL DYNAMIC PROGRAMMING

In this section, we detail the central contribution of this article, leveraging proximal and augmented Lagrangian techniques to incorporate the strengths of DDP whilst extending its range to more general constrained trajectory optimization problems.

A. Proximal reformulation of DTOCP

Similarly as in Section II, we propose to solve the DTOC [START_REF] Vanroye | A generalization of the riccati recursion for equality-constrained linear quadratic optimal control[END_REF] with an augmented Lagrangian scheme, where we iteratively minimize the AL function:

L µ k (x, u, λ k , ν k ) = J(x, u) + 1 2µ k ∥x 0 -x + µ k λ 0 ∥ 2 + N -1 t=0 1 2µ k ∥f t (x t , u t , x t+1 ) + µ k λ k,t+1 ∥ 2 + 1 2µ k ∥[h t (x t , u t ) + µ k ν k,t ] + ∥ 2 + 1 2µ k ∥[h N (x N ) + µ k ν k,N ] + ∥ 2 . ( 35
)
We now detail how the equations of Section II can be implemented efficiently using dynamic programming, following a backward-pass/forward-pass strategy just like DDP.

The first-order conditions can be restated by introducing the following Q-function:

Q t (x, u, y, λ, ν) = ℓ(x, u)+λ ⊤ f t (x, u, y)+ν ⊤ h t (x, u)+V t+1 (y), (36)
where the V t are the cost-to-go functions for the augmented Lagrangian. Dropping the subscript t, derivatives of Q t are given by:

Q x = ℓ x + f ⊤ x λ + h ⊤ x ν, (37a)
Q u = ℓ u + f ⊤ u λ + h ⊤ u λ, (37b)
Q y = V ′ x + f ⊤ y λ. (37c)
For completeness, we also write the second-order derivatives:

Q xx = ℓ xx + λ • f xx + ν • h xx , (37d)
Q xu = Q ⊤ ux = ℓ xu + λ • f xu + ν • h xu , (37e)
Q uu = ℓ uu + λ • f uu + ν • h uu , (37f) Q xy = Q ⊤ yx = λ • f xy + ν • h xy , (37g)
Q uy = Q ⊤ yu = λ • f uy + ν • h uy , (37h)
Q yy = V ′ xx + λ • f yy . (37i)
Using these notations and following the approach of Section II, the stationarity of the augmented Lagrangian (6) can be rewritten as nonsmooth, proximal KKT equations for t = 0, . . . , N -1:

T t,k (u, y, λ, ν; x) =     Q t,u Q t,y f t + µ k (λ k -λ) [h t + µ k ν k ] + -µ k ν     = 0 ( 38
)
where f t = f t (x t , u t , x t+1 ) and h t = h t (x t , u t ) are the constraint values. This operator T t,k is the dynamic programming equivalent to [START_REF] De | Differential dynamic programming and newton's method[END_REF].

In the sequel, we reintroduce the notation for the first-order multiplier estimates associated with the dynamics and the path constraints, similar to (5):

λ + k,t+1 = λ k,t+1 + 1 µ k f t (x t , u t , x t+1 ), ν + k,t = ν k,t + 1 µ k h t (x t , u t ) + . (39)

B. Backward pass

In the sequel, we denote by f t = f t (x t , u t , x t+1 ) the value of the dynamical constraint for t ⩽ 1, h t = h t (x t , u t ) the value of the inequality constraints, ft = f t + µ k (λ k,tλ t ) the shifted value of the dynamics constraint, and ht = [h t + µ k ν k,t ] + -µ k ν t the shifted inequality constraint.

In the spirit of DDP, we now show that we can compute a search direction efficiently through dynamic programming by solving a set of Riccati-type equations.

Terminal stage. In this proximal formulation, the terminal value function is

V N (x) = max ν⩾0 ℓ T (x) + ν ⊤ h N (x) - µ k 2 ∥ν -ν N,k ∥ 2 = ℓ T (x) + 1 2µ k ∥[h N (x) + µ k ν k,N ] + ∥ 2 - µ k 2 ∥ν k,N ∥ 2 .
(40) It is a continuously differentiable function in x, and has a generalized Hessian. The quadratic model of this value function involves its gradient,

∇ x V N = ∇ℓ T + 1 µ k h ⊤ x,N [h N + µ k ν k,N ] + = ∇ℓ T + h ⊤ x,N ν + k,N ,
and choosing an element of its generalized Hessian [START_REF] Clarke | Optimization and nonsmooth analysis[END_REF],

Vxx,N = ∇ 2 ℓ T + 1 µ k h ⊤ x,N P h x,N + 1 µ k ν + k,N • h xx,N . (41)
We choose P as a diagonal projection matrix onto the active set I. The second term can be rewritten h ⊤ x P h x = (h x ) ⊤ I (h x ) I , and the third term is often neglected in Gauss-Newton approximations.

Computing feedforward and feedback gains. For convenience, we will drop the time index t. At each step t, we look for a primal-dual search direction δw = (δu, δy, δλ, δν) by applying a semi-smooth Newton method [START_REF] Hintermuller | Semismooth newton methods and applications[END_REF] to T t,k (w). This leads to solving a linear system of equations

K µ     δu δy δλ δν     = -     Q u + Q ux δx Q y + Q yx δx µ k (λ + k -λ) + f x δx µ k (ν + k -ν t ) + (h x ) I δx     . ( 42
)
where K µ is the following matrix, an element of the generalized Jacobian ∂T t,k (w) of ( 38)

K µ =     Q uu Q uy f ⊤ u h ⊤ u Q yu Q yy f ⊤ y f u f y -µI (h u ) I -µI     . ( 43
)
As δx is unknown, we need to solve the sensitivities' system

K µ     k K c C ξ Ξ ζ Z     = -     Q u Q ux Q y Q yx µ k (λ + k -λ) f x µ k (ν + k -ν t ) (h x ) I     . ( 44
)
Obtaining a symmetric linear system. It is important to notice that K µ is not a symmetric matrix, disqualifying it from using symmetric linear solvers, e.g., Cholesky decomposition. However, the system can be transformed into a symmetric one by exploiting the fact that the last line implies that we can solve for the inactive multipliers satisfy (δν) I = -ν I and that h u = (h u ) I + (h u ) I . Now we define the symmetric matrix as

K µ def =     Q uu Q uy f ⊤ u (h u ) ⊤ I Q yu Q yy f ⊤ y f u f y -µI (h u ) I -µI     . (45)
The system (42) from above is now equivalent to

K µ     k K c C ξ Ξ ζ Z     = -     Q u + (h u ) ⊤ I ν Q ux Q y Q yx µ k (λ + k -λ) f x µ k (ν + k -ν) (h x ) I     (46)
As ν = ν I + ν I , the top-left block on the right-hand side reads

ℓ u + f ⊤ u λ + h ⊤ u ν I = ℓ u + f ⊤ u λ + (h u ) ⊤ I ν.
The right-hand side of [START_REF] Jallet | Implicit differential dynamic programming[END_REF] should not be used as a stopping criterion for the subproblem. It is merely a proxy to get a symmetric linear system. Inverting K µ is similar to inverting a regularization of Q uu in classical DDP. The control feedback gains in (33) also have an equivalent in [START_REF] Jallet | Implicit differential dynamic programming[END_REF]. Here, the proximal regularization in the dual blocks of (45) guarantees it is always well-conditioning, enhancing the method's convergence. Propagating the value function model. The gradient of the value function is given by

V x = Q x + Q ⊤ u K + Q ⊤ y C + f ⊤ Ξ + h ⊤ Z. ( 47a
)
since Q λ = f and Q ν = h. Its Hessian is

V xx = Q xx + Q xu K + Q xy C + f ⊤ x Ξ + (h x ) ⊤ I Z. ( 47b
) These equations can be derived by taking the optimality condition

∇ x V t = ∇ x Q t
and writing out its Taylor expansion, introducing the shorthand δw = (δu, δy, δλ, δν),

V x + V xx δx = Q x + Q xx δx + Q xw δw, (48)
and finally, replacing the expression of δw.

Initial stage. We suggest here to handle the initial condition x 0 = x using the same proximal scheme by considering the saddle point

min max V 0 (x 0 ) + λ ⊤ 0 (x 0 -x) - µ 2 ∥λ 0 -λ k,0 ∥ 2 (49)
The corresponding SQP step solves the system of equations

V xx0 I I -µI δx 0 δλ 0 = - V x0 + λ 0 µ k (λ k,0 -λ 0 ) + x 0 -x .
A benefit of our method is that it generalizes to other initial stage constraints, as discussed in Appendix C. We can also discard x 0 (and λ 0 ) from the decision variables of our problem and force x 0 = x in the algorithm's execution -this saves some computation but might be counter-productive in some scenarios such as warm-starting MPC schemes.

C. Forward pass

We discuss two ways of performing the forward pass: (i) a linear rollout computing the sequential quadratic programming (SQP) step, and (ii) a nonlinear rollout like DDP methods [START_REF] Mayne | A second-order gradient method for determining optimal trajectories of non-linear discrete-time systems[END_REF], [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF]. Linear rollout. A linear rollout reconstructs the SQP step (δx, δu, δλ) by using the feedback and feedforward gains computed in the backward pass: compute (δx 0 , δλ 0 ) by solving [START_REF] Hauser | A projection operator approach to the optimization of trajectory functionals[END_REF], and for every t = 0, . . . , N -1,

δu t = k t + K t δx t (50a) δx t+1 = c t + C t δx t (50b) δλ t+1 = ξ t + Ξ t δx t (50c)
This linear rollout was used in the first iLQR paper [START_REF] Li | Iterative linear quadratic regulator design for nonlinear biological movement systems[END_REF], but it is not used in DDP papers for robotics which follow [START_REF] Mayne | A second-order gradient method for determining optimal trajectories of non-linear discrete-time systems[END_REF], [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF].

Nonlinear rollout. Liao and Shoemaker [START_REF] Liao | Advantages of differential dynamic programming over newton's method for discrete-time optimal control problems[END_REF] argue that one of the strengths of DDP methods regarding their convergence regime is the use of the full nonlinear dynamics in the forward rollout instead of the linearized dynamics, which recover the SQP or Newton directions. This is also used in the estimation literature, e.g., in extended Kalman filters (EKFs), and was one of the enhancements to iLQR proposed by Tassa [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF], which brought iLQR closer to the original DDP algorithm.

A nonlinear rollout can be defined as follows, in the case of explicit system dynamics y = f ex (x, u): given a step size α ∈ (0, 1], the new trajectory (x α , u α , λ α ) is given by:

x α 0 def = x 0 + αδx 0 , (51a)
λ α 0 def = λ 0 + αδλ 0 , (51b)
u α t def = u t + αk t + K t (x α t -x t ), (51c)
λ α t+1 def = λ t+1 + αξ t+1 + Ξ t+1 (x α t -x t ), (51d)
x α t+1 def = f ex (x α t , u α t ) + µ k (λ α t+1 -λ t+1 ). (51e)
Nonlinear rollout with implicit dynamics. In the implicitdynamics case, we have to replace (51e) by solving the equation

f t (x α t , u α t , y) + µ k (λ α t+1 -λ t+1 ) = 0
in the unknown y. In practice, this can be done approximately using a root-finding algorithm such as the Newton-Raphson method, similar to Chatzinikolaidis et al. [START_REF] Chatzinikolaidis | Trajectory optimization of contact-rich motions using implicit differential dynamic programming[END_REF]. Having to do this in the case of implicit dynamics is a clear limitation but is necessary if the user wants to perform nonlinear rollouts.

From an algorithmic perspective, the only way we have seen of circumventing this is to use a linear rollout instead.

D. Convergence criterion

The convergence criterion for the solver is given by checking for the KKT conditions through both the dual residual

r d = ∥(∇ x L, ∇ u L)∥ ∞ , (52)
corresponding to [START_REF] Mastalli | A feasibility-driven approach to control-limited ddp[END_REF] and the primal residual (covering the complementarity conditions):

r p = ∥(f t , [h t + µ k ν k,t ] --h t ) t ∥ ∞ . (53)
Furthermore, the globalization strategy we adopt is through linesearch, using either the augmented Lagrangian [START_REF] Conn | Lancelot: A Fortran Package for Large-Scale Nonlinear Optimization (Release A)[END_REF] or the primal-dual function [START_REF] Assirelli | Wholebody mpc without foot references for the locomotion of an impedancecontrolled robot[END_REF].

E. The PROXDDP algorithm

The implementation of the PROXDDP method presented in this section corresponds to Algorithm 2, where the equations associated with the Newton step in the NLP are replaced by the dynamic programming-based approach outlined previously. A comparison of the decision variables and equations in both methods is given by table I.

F. Practical details

Choice of rollouts. In our experiments, we used the nonlinear rollout formulation of the forward pass when explicit dynamics were considered, adhering to the philosophy of classical DDP. Parallel work in the controls community [START_REF] Hauser | A projection operator approach to the optimization of trajectory functionals[END_REF]- [START_REF] Sforni | Gopronto: A feedback-based framework for nonlinear optimal control[END_REF] has taken a look at using a specific nonlinear dynamics-based projection of the Newton or SQP direction in trajectory-space (initially for continuous-time trajectories [START_REF] Hauser | A projection operator approach to the optimization of trajectory functionals[END_REF]). This literature suggests using the Riccati solution of the linearized system to define the projection operator. A link [START_REF] Bayer | A Projected SQP Method for Nonlinear Optimal Control with Quadratic Convergence[END_REF] has been suggested with the feasible SQP method introduced by [START_REF] Wright | A feasible trust-region sequential quadratic programming algorithm[END_REF], which was showcased in the latter paper in the context of MPC. Further research should be done on interpreting the nonlinear rollout in this setting and its real-world performance as applied to complex, nonlinear systems in robotics, which we leave to future work.

Multiple shooting.

Unlike what the definition of problem [START_REF] Vanroye | A generalization of the riccati recursion for equality-constrained linear quadratic optimal control[END_REF] suggests, where the unknown states x = (x t ) t are an explicit decision variable, DDP methods classically reduce it in practice to a single shooting formulation where the x t are dependent on u at every step of the algorithm. Single-shooting formulations have several drawbacks. One of them is their inability to properly use infeasible initial guess trajectories, which is troublesome for settings such as locomotion problems. Furthermore, they require solving for the dynamics in the forward pass, which might be costly or numerically harmful in the case of implicit integration schemes or DAEs. In contrast, multiple-shooting approaches help create a more tractable optimization landscape by adding more variables and allowing an amount of infeasibility at some problem nodes [START_REF] Diehl | Fast direct multiple shooting algorithms for optimal robot control[END_REF]. A combination of linear, SQP-type, and nonlinear, DDP-type rollouts also leads to a variant of multiple shooting, as explored in [START_REF] Giftthaler | A family of iterative gauss-newton shooting methods for nonlinear optimal control[END_REF]. Standard DDP can also be augmented into a multiple-shooting algorithm by introducing slack variables [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF], [START_REF] Kazdadi | Equality constrained differential dynamic programming[END_REF] for the dynamical constraints. In our method, the slack variable is linked to the gap in convergence in the dual (co-state) variables, and the link with the former approaches is mathematically explored in Appendix D.

Further extensions. In Appendix C, we provide extensions to the algorithm to account for other initial stage constraints than x 0 = x. We also show how to modify the algorithm to allow for individual scaling of each constraint, which we found useful to tune the convergence behavior of the solver in difficult scenarios.

V. DISCUSSION ON OTHER METHODS

The treatment of constraints in the literature is often dependent on the type of constraints, as inequality constraints are generally seen as more complex due to the nature of their optimality conditions. For equality constraints, some recent works [START_REF] Laine | Efficient computation of feedback control for equality-constrained lqr[END_REF], [START_REF] Vanroye | A generalization of the riccati recursion for equality-constrained linear quadratic optimal control[END_REF] adopt a nullspace-based method for the resolution of equality-constrained LQR subproblemsthe equivalent to the equality-constrained QP subproblem in sequential quadratic programming (SQP) methods [29, chap. 18]. Some recent other works advocate for using augmented Lagrangian (AL) approaches for equality constraints combined with DDP-like recursion, which has led to the development of several methods in recent years [START_REF] Howell | Altro: A fast solver for constrained trajectory optimization[END_REF], [START_REF] Aoyama | Constrained differential dynamic programming revisited[END_REF], [START_REF] Kazdadi | Equality constrained differential dynamic programming[END_REF], [START_REF] Jallet | Implicit differential dynamic programming[END_REF]. Augmented Lagrangian approaches for both equality and inequality constraints have been developed [START_REF] Toussaint | A novel augmented lagrangian approach for inequalities and convergent any-time non-central updates[END_REF], [START_REF] Howell | Altro: A fast solver for constrained trajectory optimization[END_REF], [START_REF] Aoyama | Constrained differential dynamic programming revisited[END_REF], [START_REF] Toussaint | Differentiable physics and stable modes for tool-use and manipulation planning[END_REF], including the authors' prior work [START_REF] Jallet | Constrained differential dynamic programming: A primal-dual augmented lagrangian approach[END_REF] of which this paper is an extension. In contrast to [START_REF] Toussaint | A novel augmented lagrangian approach for inequalities and convergent any-time non-central updates[END_REF], [START_REF] Howell | Altro: A fast solver for constrained trajectory optimization[END_REF], [START_REF] Aoyama | Constrained differential dynamic programming revisited[END_REF], our approach uses a primal-dual Newton step (46) instead of a purely primal step akin to [START_REF] Neunert | Fast nonlinear model predictive control for unified trajectory optimization and tracking[END_REF]. Furthermore, the AL methods in the former papers only produce a coarse solution, which is refined through SQP steps. In contrast, the primal-dual approach allows for convergence to higher accuracy (see [START_REF] Kazdadi | Equality constrained differential dynamic programming[END_REF]) due to better numerical conditioning of the primal-dual system [START_REF] Bambade | Prox-qp: Yet another quadratic programming solver for robotics and beyond[END_REF].

For inequality constraints, [START_REF] Frison | Hpipm: A high-performance quadratic programming framework for model predictive control[END_REF]- [START_REF] Pavlov | Interior point differential dynamic programming[END_REF], [START_REF] Vanroye | Fatrop : A fast constrained optimal control problem solver for robot trajectory optimization and control[END_REF], [START_REF] Howell | Calipso: A differentiable solver for trajectory optimization with conic and complementarity constraints[END_REF] employ interior-point methods which have been hugely successful in nonlinear programming through popular solvers such as IPOPT [START_REF] Wächter | On the implementation of an interiorpoint filter line-search algorithm for large-scale nonlinear programming[END_REF]. Interior-point methods solve a succession of log barrier-penalized problems that correspond to a relaxation of the KKT complementarity slackness. They can approach the real problem satisfactorily over multiple iterations and tightening of the relaxation. However, they have limited warm-starting capabilities from a previous problem's solution due to resetting the relaxation parameters when the underlying problem has shifted. Related methods include barrier function methods [START_REF] Nocedal | Numerical Optimization[END_REF] leveraged by [START_REF] Hauser | A barrier function method for the optimization of trajectory functionals with constraints[END_REF]- [START_REF] Grandia | Feedback mpc for torque-controlled legged robots[END_REF], [START_REF] Li | Hybrid systems differential dynamic programming for whole-body motion planning of legged robots[END_REF]. Among these, the relaxed barrier approach [START_REF] Feller | Relaxed logarithmic barrier function based model predictive control of linear systems[END_REF] allows use in the MPC context by switching to a quadratic barrier when the underlying inequality constraints are violated. These barrier methods suffer from different limitations: tuning of the barrier parameter, the trade-off with numerical conditioning, and proper satisfaction of the constraints (the relaxed barrier allows violation, and the log barrier does not allow constraints to saturate).

An overview of the different features and methods around alternative DDP/iLQR/Riccati-based approaches of state of the art is summarized in Table II.

VI. SOFTWARE IMPLEMENTATION DETAILS

Over the past few years, several open-source TO libraries have been developed in the robotics community: CONTROL TOOLBOX [START_REF] Giftthaler | A family of iterative gauss-newton shooting methods for nonlinear optimal control[END_REF], OCS2 [START_REF] Farshidian | OCS2: An open source library for optimal control of switched systems[END_REF], ALTRO [START_REF] Howell | Altro: A fast solver for constrained trajectory optimization[END_REF], CROCODDYL [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF], FATROP [START_REF] Vanroye | Fatrop : A fast constrained optimal control problem solver for robot trajectory optimization and control[END_REF]. We contribute to this open-source initiative by developing two new software libraries for nonlinear optimization and optimal control in robotics:

1) PROXNLP, which focuses on generic nonlinear programming on manifolds using the method of Section II; 2) ALIGATOR, a library for constrained trajectory optimization, following the approach of Section IV. These two libraries can be exploited in contexts beyond robotics (e.g., automatic control, biomechanics, etc.). These two new libraries are motivated by the need for efficiency and the lack of flexibility of existing frameworks to easily implement or benchmark new algorithms.

PROXNLP and ALIGATOR are written in C++ using the Eigen [START_REF] Guennebaud | Eigen v3[END_REF] linear algebra template library, which is leveraged for efficient computation with minimal dynamic memory allocation at runtime, enabling high-performance scenarios. More specifically, the ALIGATOR package includes the following features:

• an implementation of the ProxDDP algorithm described in this paper; • the support for problems formulated on Lie groups, which extends the framework that we have presented; • the support for the PINOCCHIO rigid-body dynamics library [START_REF] Carpentier | The pinocchio c++ library -a fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives[END_REF], which implements analytical derivatives [START_REF] Carpentier | Analytical derivatives of rigid body dynamics algorithms[END_REF], several standard Lie groups in robotics and the support of constrained-based models [START_REF] Carpentier | Proximal and sparse resolution of constrained dynamic equations[END_REF]; • an API frontends in both C++ and Python programming languages: these frontends are built using the same modeldata pattern as PINOCCHIO and CROCODDYL for easy caching of previous computations; • the compatibility with the CROCODDYL [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF] trajectory optimization library. ALIGATOR comes with its own implementation of the FDDP algorithm, initially introduced in [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF]. Both libraries are templated on the desired scalar type (by default, double precision floating point). In the future, we plan to add the following: support and integration with CasADi [START_REF] Andersson | CasADi -A software framework for nonlinear optimization and optimal control[END_REF] and CppAD is planned for automatic differentiation and code generation; support for higher precision floating point types using MPFR; multithreading support through OpenMP; a release on package managers such as APT, rospkg, Homebrew, Conda, and pip.

VII. EXPERIMENTS

In this section, we present a set of experiments, ranging from simple toy problems to complex trajectory optimization Vanilla DDP/Tassa's iLQR [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF] Single-shooting Closed-loop ✗ Linesearch FDDP [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF] Multiple-shooting 1Closed-loop ✗ Linesearch BOX-DDP [START_REF] Tassa | Control-limited differential dynamic programming[END_REF] Single-shooting Closed-loop Control limits (projection) Linesearch CROCODDYL INTRO 2 [START_REF] Mastalli | Inverse-dynamics mpc via nullspace resolution[END_REF] Multiple-shooting Closed-loop ✓(projection) Linesearch ALTRO [START_REF] Howell | Altro: A fast solver for constrained trajectory optimization[END_REF] Multiple-shooting 3Closed-loop ✓(A.L. + projection) Linesearch CONTROL TOOLBOX GNMS/ILQR-GNMS [START_REF] Giftthaler | A family of iterative gauss-newton shooting methods for nonlinear optimal control[END_REF] Multiple-shooting Linear/Closed-loop4 ✗ Linesearch5 FATROP [START_REF] Vanroye | Fatrop : A fast constrained optimal control problem solver for robot trajectory optimization and control[END_REF] Multiple-shooting Linear ✓(interior-point6 ) Filter 6 ALIGATOR PROXDDP (ours) Multiple-shooting Linear/Closed-loop ✓(A.L.) Linesearch

Constrained SLQ [START_REF] Farshidian | An efficient optimal planning and control framework for quadrupedal locomotion[END_REF] Single-shooting Closed-loop ✓(projection + barrier) Linesearch Interior-point method Multiple-shooting Linear ✓(interior-point) Linesearch OCS2 [START_REF] Farshidian | OCS2: An open source library for optimal control of switched systems[END_REF] SQP with HPIPM [START_REF] Frison | Hpipm: A high-performance quadratic programming framework for model predictive control[END_REF], [START_REF] Frison | Introducing the quadratically-constrained quadratic programming framework in hpipm[END_REF] Multiple-shooting Linear ✓(interior-point in QP) Linesearch ACADOS [START_REF] Verschueren | Towards a modular software package for embedded optimization[END_REF], [START_REF] Verschueren | Acados-a modular open-source framework for fast embedded optimal control[END_REF] SQP/SCQP with HPIPM Multiple-shooting Linear ✓(interior-point) Linesearch on robotic systems and model-predictive control (MPC) on a real quadruped robot. These experiments showcase the capabilities of our formulation and software contribution regarding flexibility and real-time abilities.

A. Toy examples

Constrained LQR. The linear-quadratic regulator (LQR) is a classical system from the optimal control literature and the most well-known system that can be solved in semi-closed form -in its simplest instance without stagewise constraints such as control limits. Our first example is to show that our solver can solve an LQR with a terminal constraint and additional control bounds -ū ⩽ u ⩽ ū in a reasonable number of iterations. The optimized state-control trajectory and convergence of primaldual residuals are shown in Fig. 3 for the terminal-constraint and control-bounded problem.

Pendulum. We add both torque limits with τ max = -τ min = 20 N.m -1 , a terminal constraint on the pendulum's tip, and no terminal cost ℓ T ≡ 0. The state-control trajectories are shown in Fig. 4a, and convergence is shown in Fig. 4b. The timestep is ∆t = 10 ms. Cartpole. A case for a very nonlinear, non-convex problem is that of the inverted pendulum on top of a cartpole. The discretization timestep is ∆t = 10 ms, and the time horizon is T = 5 s The constrained cartpole task is defined to have the terminal position of the end-effector p ee (x N ) be at p = (0, 1) ⊤ in the yz-plane, and torque limits -τ max ⩽ τ ⩽ τ max . See Fig. 5 for the resulting state-control and end-effector trajectories and convergence of the iterates. Acrobot and double pendulum. In the first setting, we impose a terminal constraint on the state. The optimized state-control trajectories are shown in Fig. 6, with a 3D rendering shown in Fig. 2. The state of these systems is x = ((cos θ 1 , sin θ 1 ), (cos θ 2 , sin θ 2 ), θ1 , θ2 ) ∈ SO(2)2 × R 2 . In the second setting, we impose control torque limits and relax the target reaching task into a quadratic cost: the resulting Here, the initial ALM parameter is chosen as µ 0 = 10 -6 , promoting rapid convergence if the subproblem can be solved accurately. trajectories are shown in Fig. 7. In both settings, the dynamics AL parameter is scaled by 10 -3 to promote faster convergence on the dynamics constraint. The integration time step is ∆t = 5 ms, and the integrator is second-order Runge-Kutta.

B. Ballistics with a manipulator

This second experiment demonstrates the formulation of a simple ballistics task on a manipulator, without the user specifying a launch velocity but by specifying a target position pT ∈ R 3 for the thrown object to reach. System model. The system is modelled using PINOCCHIO [START_REF] Carpentier | The pinocchio c++ library -a fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives[END_REF].

The chosen manipulator is a UR10 arm, and a projectile (a mug) is added to the robot's model tree with a free-flyer joint. The system's configuration vector q = (q ob , q a ) consists of the joint configuration q a ∈ R nq1 and projectile pose q ob ∈ SE(3).

We use a whole-body model for the system dynamics M (q)q + b(q, q) + J c (q

) ⊤ λ c = Bτ (54a) c(q) = 0. ( 54b
)
where M (q) is the joint space inertia matrix, τ ∈ R 6 is the input torque, c is the 6D constraint function between a frame attached to the end-effector and the projectile's frame, J c = ∂c(q)/∂q ∈ R 6×12 its Jacobian, and λ c ∈ R 6 is the constraint force between the end-effector and the projectile. These are underactuated dynamics with a selection matrix B which imposes that there is no actuation of the projectile. Since (54b) is a position-level constraint while the dynamics are solved for acceleration q, it is replaced with an accelerationlevel constraint:

c = J(q)q + γ = -K p c(q) -K d ċ, (55)
where (K p , K d ) are the Baumgarte stabilization gains [START_REF] Baumgarte | Stabilization of constraints and integrals of motion in dynamical systems[END_REF].

The constrained dynamics model is implemented and solved using the constrained forward dynamics algorithm of [START_REF] Carpentier | Proximal and sparse resolution of constrained dynamic equations[END_REF] implemented in PINOCCHIO.

After a set time t c ∈ [0, T ] in the time horizon, the projectile detaches from the end-effector and is launched. Thus, for t > t c , the constraint (54b) and contact force λ c are removed, the robot and projectile dynamics are decoupled, and the projectile is in a flight phase. Control regularization and initial guess. The gravitycompensating torque (with zero acceleration) for a manipulator without constraints is given by τ 0 = b a (q 0 , v 0 ). It can be computed using the recursive Newton-Euler algorithm (RNEA) [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF]. When a body is attached to the end-effector, this is not appropriate anymore, and the extra mass means applying the torque τ 0 will still create a nonzero acceleration. We can solve this problem by separating the dynamics with zero acceleration:

b a b ob + J c,a J c,ob ⊤ λ c = τ 0
where J c,a is the Jacobian with respect to q a . Once equipped with an appropriate τ 0 , we use it to construct a quasistatic initial guess, which is used to cold-start the solver, and also use it for the control regularization term in the cost function:

ℓ reg,u (u) = 1 2 ∥u -τ 0 ∥ 2 R .
The other cost function term is a regularizer on the manipulator joint velocities and, with a lesser weight, the joint configurations. As alluded to at the beginning of this subsection, there are no target position or even target launch velocity costs, and only a hard constraint on the terminal position 3D of the projectile. We expect the optimizer to find a feasible trajectory that satisfies this constraint in this setting. Additionally, we add limits on the manipulator input torque:

-τ max ⩽ u ⩽ τ max ,
where the value for τ max is the robot's maximum actuation.

In this example, we used a UR10 manipulator; the projectile is a mug. The optimized control trajectory is shown in Figure 8, with the joint velocities in 9. The graphs show very highbandwidth behavior in the controls, which is not reflective of what can be achieved on an actual system -modeling the control inputs using, e.g., a spline with a penalty on the ℓ 2 of the controls' time derivative could lead to better behavior. A video rendering of the solution found by the solver is provided in Figure 13 as well as the supplementary video accompanying this submission.

C. Motion planning on a quadrotor

We use the Hector quadrotor model 3 . Its configuration variable is its base pose q ∈ SE(3), and its dynamics are standard inertial dynamics actuated through the rotors. We build two example problems for the quadrotor system: (i) thrust control limits and (ii) obstacle avoidance. The red (resp. black) dashed line represents the velocity upper (resp. lower) limit. Here, the velocity limits were not enforced through constraints or even accounted for in the cost functions.

Thrust limits. In this variant, we add thrust limits to the quadrotor:

-τ max ⩽ τ ⩽ τ max .
The initial AL penalty parameter is set to µ 0 = 0.1. The converged controls and convergence regime in Fig. 10. Obstacles. In this variant, we want the quadrotor to reach an endpoint while "slaloming" between two obstacles represented as cylinders. We use a simple collision model between the cylinders and a bounding sphere around the quadrotor. To solve this problem, we had to switch to a linear rollout to keep the solver stable. We set the initial AL penalty parameter in both settings to µ 0 = 1. We obtain the obstacle-avoiding trajectory as rendered in Fig. 14. The controls and convergence regime are shown in Fig. 11. In this setting, the torque limits were not enforced using constraints. The corresponding rendered trajectory is shown in Fig. 14. The AL penalty for the obstacles is scaled down by a factor w obs = 0.1.

D. Whole-body planning of a jump on a quadruped

The system is also modeled using a whole-body model with bilateral contact constraints as in (54b):

M (q)q + b(q, q) + c∈Ct J c (q) ⊤ λ c = Bτ (56a) c(q) = 0, (56b)
where C t is the set of contact points (feet) at time t. The contact constraint is a 3-dimensional position constraint concerning points on the ground. The problem is modeled in three phases: (i) all four feet in contact, (ii) no contact (flight phase), and (iii) contact once again (landing phase). The phases are chosen to start at times t = 0, t = t 0 > 0, and t = t 1 > t 0 , respectively. In this problem, the constraint is also replaced by an acceleration constraint with Baumgarte gains c = J(q)q + γ = -K p c(q) -K d ċ.

Our desired behavior is to have zero constraint spatial acceleration and velocity and enforce that the altitude of the feet is z c (q) = 0. Thus, the xy-plane position-level gains in K p are set to zero. To have the planned foot motion hit the ground at t = t 1 , we add a position-level hard constraint: z c (q(t 1 )) = 0, which the proximal optimizer can account for.

The generated motion is illustrated in Figure 15. The convergence of the problem residuals as the solver progresses is shown in Fig. 12.

E. Model-predictive control on the SOLO-12 quadruped

To demonstrate the capabilities of our package when it comes to real-time whole-body MPC on the SOLO quadruped on uneven terrain, as illustrated on the screenshot sequences in Fig. 16 and in the companion video associated with this article. In this demonstration, we solve whole-body optimal control problems without pre-defining reference foot trajectories, as done in Assirelli et al. [START_REF] Assirelli | Wholebody mpc without foot references for the locomotion of an impedancecontrolled robot[END_REF]. The desired robot base linear and angular velocities are provided using a gamepad. Hardware. The Solo quadruped is connected over Ethernet to a 2019 Dell XPS 13 laptop running under Ubuntu Linux 20.04, patched with a real-time kernel. The whole-body MPC runs in a separate ROS [START_REF] Quigley | ROS: An Open-Source Robot Operating System[END_REF] node on an Apple Mac Studio M1 Ultra desktop computer, with communication between the two over LAN. Control framework. We have adapted the framework developed in [START_REF] Assirelli | Wholebody mpc without foot references for the locomotion of an impedancecontrolled robot[END_REF] to leverage the real-time computational efficiency of the PROXDDP algorithm implemented in ALIGATOR. This framework is composed of:

• a state estimation module using a complementary filter initially described in [START_REF] Léziart | Implementation of a reactive walking controller for the new openhardware quadruped solo-12[END_REF], which estimates the robot base position and linear velocity, • a lower-level, higher-frequency whole-body controller using the first Riccati gain and feedforward control, which outputs a desired torque:

τ (t) def = u 0 + K 0 (x(t) ⊖ x ocp (t)) (57)
where x ocp is an upsampling of the OCP solution x ocp using e.g. interpolation or integrating the system dynamics. In our experiments, the MPC loop frequency is ∆t mpc = 12 ms and runs over a prediction horizon of 0.5s. The MPC formulation accounts for robot position, velocity, and torque limits. The cost function is defined by:

ℓ(x, u) = ℓ fly (x) + ℓ reg (x, u) + ℓ g (x) + ℓ bv (x) + ℓ bu (u) + ℓ base (x; vbase ) (58)
All the cost function terms are summarized in Table III. The index j spans over the quadruped's four feet, z j is the altitude of the j-th foot, v j ∈ R 3 its linear velocity (and v j ∥ its component along the xy-plane), λ are the contact forces for all legs (in contact).

The low-level Riccati control ( 57) is sampled at a frequency of 1 kHz. Each MPC update consists of a single step of the trajectory optimization algorithm; in practice, it was solved on the Mac M1 hardware in less than 6 ms. The code associated with this MPC framework is publicly available 4 .

In Fig. 16 and in the companion video, we can observe that SOLO-12 exhibits smooth behavior and successfully walks on uneven terrain even though the terrain is assumed to be flat in the prediction horizon.

VIII. CONCLUSION

In this paper, we propose a unified formulation for solving constrained trajectory optimization problems leveraging a primal-dual augmented Lagrangian approach while exploiting the underlined temporal structure of OCPs. This results in two novel algorithms: (i) PROXNLP for generic nonlinear programming problems on manifolds, and (ii) PROXDDP specifically suited for constrained trajectory optimization. We have empirically demonstrated the efficiency of our solver in coping with several real-size optimal control problems for various classes of robots requiring hard constraint satisfaction. Used inside an MPC scheme to perform whole-body control on a quadrupedal robot, we have particularly demonstrated that our solver has no difficulty taking advantage of a warm-start. We are also releasing an efficient C++ implementation with Python bindings to allow reproduction of our experiments, on which we hope other teams will be able to build more complex behaviors on various robots. We hope this work will entice future developments in robust, real-time capable, constrained trajectory optimization solvers, especially their application to model-predictive control schemes.

In future work, we will consider delving deeper into a couple of subjects leveraging the proposed framework. First, we plan to extend these software packages, taking feedback from the community and extending its range of applications towards more real-world scenarios. We expect this to be a continuous work akin to what is being done with both PINOCCHIO and CROCODDYL. We expect ALIGATOR to become a catalyzer in robotics, facilitating the deployment of real-time receding horizon schemes, thus advancing and simplifying the generation of complex motions on modern robots. Second is further work on structure-exploiting linear algebra solvers, both for the subproblems [START_REF] Jallet | Implicit differential dynamic programming[END_REF] as well as parallelizing of the backward pass similar to [START_REF] Laine | Parallelizing lqr computation through endpointexplicit riccati recursion[END_REF], [START_REF] Jackson | A parallell linear system solver for optimal control[END_REF]. We expect this to accelerate the timing performance of our library significantly, making possible online whole-body model predictive control on more complex robotic systems such as humanoids and dexterous hands or facilitating the deployment of MPC solutions on embedded devices with limited computational resources. Per-constraint AL scaling. Different constraints in an optimization problem can have different scales regarding the values they take on the search space. For this reason, one may adopt different weights for each constraint. For simplicity, consider a weight w d for the dynamical constraint w i for the inequality constraints. Then, the AL parameter µ is replaced by w d µ and w i µ respectively. The KKT matrix and right-hand side of equation ( 43), as well as the linesearch penalties, are all adjusted accordingly.

D. Relationship between PROXDDP and multiple shooting

Similarly to [START_REF] Kazdadi | Equality constrained differential dynamic programming[END_REF], we can solve multiple shooting formulations with an equality constraint-capable DDP by using slack variables for the dynamics s = (s t ) t :

x t+1 = f (x t , u t ) -s t , 1 ⩽ i ⩽ N -1,
or equivalently f (x t , u t )x t+1 = s t , and add the constraint that s t = 0. The trajectory optimization problem becomes: min x,u,s J(x, u)

s.t. x t+1 = f (x t , u t ) -s t , s t = 0. (66)
To solve this using a DDP-like algorithm, let us introduce the following Hamiltonian function for this problem: Q t (x, u, s, λ) = ℓ t (x, u) + V t+1 (f (x, u)s) + λ ⊤ s. [START_REF] Saccon | Optimal control on lie groups: The projection operator approach[END_REF] Using the shorthands Q = Q t , V ′ = V t+1 , the first-order necessary conditions satisfied by the quantities (u ⋆ , s ⋆ , λ ⋆ ) at step i are:

0 = Q u = ℓ u + f ⊤ u V ′ x 0 = Q s = -V ′ x + λ ⋆ 0 = s ⋆ . ( 68
)
We extract from these optimality conditions (i) the standard stopping criterion from DDP, ℓ u + f ⊤ u V ′ x = 0, and (ii) a link between the co-state and value function gradient: λ ⋆ = -V ′

x . However, using an augmented Lagrangian formulation akin to [START_REF] Kazdadi | Equality constrained differential dynamic programming[END_REF], the optimality conditions in (u, s, λ) of the proximal subproblem are:

  ℓ u + f ⊤ u V ′ x -V ′ x + λ -s + µ k (λ k -λ)   = 0.
When the inner subproblem is exactly solved, this formulation leads to the slack variable being equal to the outer iteration multiplier gap in the PROXDDP algorithm:

s ⋆ = µ k (λ k -λ k+1 ),
where λ k+1 = λ ⋆ . Thus, we have that the primal-dual scheme induces a slack in the system dynamics.

Fig. 1 .

 1 Fig. 1. Solo-12 walking on an unmodelled slope using the whole-body MPC framework based on primal-dual augmented Lagrangian techniques.

Fig. 2 .

 2 Fig. 2. 3D rendering of the acrobot swing-up task with the terminal constraint. Rendered using the MeshCat visualizer.

  Optimized state and control trajectory. The third coordinate of the control inputs (in green) saturates the control bound in the entire solution save for the first stage.

  Primal-dual residuals of the problem at each iteration of the semi-smooth Newton method.

Fig. 3 .

 3 Fig. 3. LQR problem with terminal constraint and control bounds.Here, the initial ALM parameter is chosen as µ 0 = 10 -6 , promoting rapid convergence if the subproblem can be solved accurately.

  Optimized state and control trajectories. The system state converges to the target state x = 0, and the control inputs saturate. Convergence of the problem primal and dual residuals.

Fig. 4 .

 4 Fig. 4. Pendulum swing-up problem with terminal constraint and torque limits. The initial AL parameter is µ 0 = 1.

  Optimized state and control trajectories and trajectory of the cartpole's end-effector. The thick black lines are the control bounds, which saturate thrice along the time horizon.

  Primal-dual residuals along the optimization iterates.

Fig. 5 .Fig. 6 .

 56 Fig.5. Cartpole with both torque limits and a terminal constraint. We chose an initial parameter of µ 0 = 10 -5 . The torque limits were chosen to be τmax = 6 N.m -1 .

Fig. 7 .

 7 Fig. 7. Acrobot with control limits. Optimized control trajectory with control bounds. The initial AL parameter is µ 0 = 10 -3 .

Fig. 8 .

 8 Fig.8. Optimized control trajectory for the UR10 ballistic task. The red (resp. black) dashed line represents the torque upper (resp. lower) limit. The effort (torque) limits were imposed using a hard box constraint in the problem formulation.

Fig. 9 .

 9 Fig.9. Computed joint velocities for the ballistic task on the UR10. The red (resp. black) dashed line represents the velocity upper (resp. lower) limit. Here, the velocity limits were not enforced through constraints or even accounted for in the cost functions.

Fig. 10 .

 10 Fig.10. Optimized control trajectory and convergence of the quadrotor problem with thrust limits. The thrust limits often saturate in this setting due to the tight rightward turn needed to move to the next target. The AL penalty scale for the thrust limits is w t.lims = 10.

Fig. 11 .

 11 Fig.11. Optimized trajectory and convergence of the quadrotor problem with obstacles. In this setting, the torque limits were not enforced using constraints. The corresponding rendered trajectory is shown in Fig.14. The AL penalty for the obstacles is scaled down by a factor w obs = 0.1.

Fig. 12 .

 12 Fig. 12. Convergence of the primal-dual residuals for the SOLO-12 jumping task. The initial AL parameter is µ 0 = 10 -4 .

Fig. 13 .

 13 Fig. 13. Optimized motion for the UR10 ballistic task, rendered using the MeshCat visualizer.

Fig. 14 .

 14 Fig. 14. Trajectory of the quadrotor avoiding obstacles, rendered using the MeshCat visualizer. Above and below are two different viewpoints.

Fig. 15 .

 15 Fig. 15. 3D rendering of the SOLO-12 jumping task. Rendered using the MeshCat visualizer. The time step is ∆t = 20 ms and time horizon T = 1.2 s.

Fig. 16 .

 16 Fig. 16. SOLO-12 walking task. The time step is ∆t = 12 ms and time horizon T = 0.480 s. The slope and bumps of the terrain are unmodeled and act as perturbations.

2 wgwg = 10 3 2 w bu w bu = 10 4

 2324 Name & symbol Expression ParametersFly-high cost ℓ fly j e -γz j ∥v j ∥ ∥ 2 w fly γ = 50, w fly = 5.10 4 Ft. height bound ℓg j ∥ max(z j , z ref )∥ Spatial vel. tracking ℓ base ∥v base -vbase ∥ 2 w base w base = 8.10 5 Vel. bounds ℓ bv∥ max(min(v, v), v)∥ 2 w bv w bv = 10 × [0 6 ; 1 nv -6 ] Ctrl. bounds ℓ bu ∥ max(min(u, u), u)∥ Regularization ℓreg ∥xx ref ∥ 2 wx + ∥u∥ 2 wu + ∥λλ ref ∥ 2w λ contact forces wu = 10 4 , w λ = 10 2 , λ ref = (0, 0, mg ncontacts ) ⊤ Impact cost ℓ imp. j ∥z jz ref ∥w h + ∥v j ∥w i w h = w i = 10 4

TABLE II COMPARISON

 II OF DIFFERENT METHODS AND SOFTWARE LIBRARIES FOR SOLVING OCPS.

	Library	Solver	Dynamics handling	Forward pass	Constraints (approach)	Globalization
	-	Stagewise Newton [8]	Single-shooting	Open-loop	✗	Linesearch
	-	Li's iLQR [14]	Single-shooting	Open-loop	✗	Linesearch

TABLE III TERMS

 III OF THE COST FUNCTION FOR THE MPC EXPERIMENT.

In practice, we can use an approximation of the Lagrangian Hessian which does not use the second constraint derivatives -this corresponds to the Gauss-Newton approximation of the augmented Lagrangian Hessian.

https://github.com/Simple-Robotics/proxnlp

The dynamics gap is not taken as a slack variable or included in the linesearch through a merit function.

Multiple-shooting similar to FDDP. This is meant for inverse-dynamics formulations on multibody systems (it exploits the structure in these formulations).

Using a slack variable with a quadratic penalty.

Full GNMS uses the linearized dynamics in the forward sweep (for which open-loop and closed-loop coincide), and the iLQR variants use the nonlinear dynamics in closed-loop.

In the implementation; the paper only deals with full-step iterations.

Equality constraints solved in SQP fashion using equality-constrained LQR[START_REF] Vanroye | A generalization of the riccati recursion for equality-constrained linear quadratic optimal control[END_REF]. Filter implementation similar to IPOPT[START_REF] Wächter | On the implementation of an interiorpoint filter line-search algorithm for large-scale nonlinear programming[END_REF].

Its URDF is provided in the example-robot-data package: https://github. com/Gepetto/example-robot-data

https://gitlab.laas.fr/gepetto/quadruped-reactive-walking/-/tree/ wjallet/devel

APPENDIX

A. Details on the augmented Lagrangian method

Expression of the first-order estimate. Recall the initial definition of the augmented Lagrangian:

The stationarity conditions over λ read:

And the conditions over ν read:

where N R n + (ν) is the normal cone to R n + at ν: it being nonempty implies ν ⩾ 0. Rewriting this condition:

which leads to applying the projection on R n + :

Interpretation as a shifted-penalty method. The method of multipliers can also be seen as a standard ℓ 2 -penalty method with varying penalty weights (µ k ) k to solve a cascade of problems with shifted constraints of the form:

where (λ k ) k⩾0 , (ν k ) k⩾0 are a sequence of vectors, so that the (µ k λ k , µ k ν k ) can be interpreted as "slack variables". Applying the aforementioned penalty method leads to the sequence of unconstrained minimization problems:

, where

The objective f (z) + P k (z) is precisely the augmented Lagrangian function L µ k .

B. Primal-dual augmented Lagrangian functions

In the NLP case, we can derive a primal-dual merit function as in De Marchi [48] by starting with a slack variable s and the Gill-Robinson primal-dual function [START_REF] Gill | A primal-dual augmented lagrangian[END_REF] for the equality-constrained case. Let us consider the only-inequality constrained case for simplicity:

We introduce the slack variable s ⩽ 0 and add the constraint that g(x) = s. The primal-dual augmented Lagrangian for this problem is then:

(59) Minimizing over s ⩽ 0 leads to the minimum point

and minimum value

(61) Define the primal-dual multiplier update:

The gradients are

Note that the stationarity conditions in this primal-dual function at a point (x * , λ * ) imply directly λ * = λ • k (x * ) and ∇f (x * ) + J(x * ) ⊤ λ • k (x * ) = 0. Again, the equality-constrained case of [START_REF] Gill | A primal-dual augmented lagrangian[END_REF]

). The BCL-type update is given by:

C. Extensions for the PROXDDP algorithm

More general initial conditions. We can generalize to problems that are not initial value problems but where x 0 is itself a decision variable, with a generic constraint ω(x 0 ) = 0. For instance, where only part of the state space is known (e.g., position but not velocity). The corresponding SQP step leads to solving the following linear system:

where W 0 ≈ ∇ 2 x0 (V 0 + λ ⊤ 0 ω) (e.g. W = V xx0 ).