Frédéric Mallet
email: fmallet@sophia.inria.fr

Marie-Agnès Peraldi-Frati

Charles André
email: candre@sophia.inria.fr

Marte CCSL to execute East-ADL Timing Requirements

In the automotive domain, several loosely-coupled Architecture Description Languages (ADLs) compete to provide a set of abstract modeling and analysis services on top of the implementation code. In an effort to make all these languages, and more importantly their underlying models, interoperable, we use the UML Profile for MARTE as a pivot to define the semantics of these models.

In this paper, we particularly focus on East-ADL2. We discuss the benefits of having an integrated, MARTEcentered, approach. We give a formal semantics of East-ADL2 timing requirements. Relying on this semantics, several kinds of analysis are possible. Requirements become executable and simulations are run. A constraint solver is used to detect logical inconsistencies. Our proposal is illustrated on an Anti-lock Braking System (ABS).

Introduction

Architecture Description Languages (ADLs) are more and more accepted as means to manage the engineering information related to automotive electronics and deal with the increasing complexity of the automotive software [START_REF] Lönn | Automotive Architecture Description Language[END_REF]. Several loosely-related ADLs are competing in that area (AADL [START_REF]Architecture Analysis and Design Language[END_REF], EAST-ADL [START_REF] Cuenot | Managing complexity of automotive electronics using the East-ADL[END_REF], SYSML [START_REF] Weilkiens | Systems Engineering with SysML/UML: Modeling, Analysis, Design[END_REF][START_REF] Omg | Systems Modeling Language (SysML) Specification 1.1. Object Management Group[END_REF], AML [START_REF] Braun | A model-based approach for automotive software development[END_REF]). Some propose connections with the emerging standard AU-TOSAR (http://www.autosar.org), rely on it for the implementation and build on top some requirement and tracability facilities. Others provide analysis models and tools. However, the ever increasing cost of software in domains like automotive calls for a single framework that would bring together all tools and models and interprete them consistently. Building on the numerous existing, close to maturity, UML (Unified Modeling Language) tools seems like a good way to avoid building and maintaining separate domain-specific graphical editors when graphical aspects should be managed once and for all.

UML is becoming more and more popular for the design and modeling of software systems. Being a general-purpose language, it lacks key features to model software of the realtime and embedded (RTE) domain. The recently adopted UML Profile for Modeling and Analysis of Real-Time and Embedded systems (MARTE) [START_REF]UML Profile for MARTE, beta 2. Object Management Group[END_REF] aims at bringing in the missing features. The goal has never been for MARTE to become the unified (universal) language for modeling RTE systems but rather to be a pivot that brings interoperability between the existing languages and formalisms of the RTE domain. The belief is that one single language or model of computation would never be able to cover all aspects for RTE systems whatever good it is for a given specific aspect (schedulability, dependability, requirement . . . analyses).

In a continuous effort, we have been trying to show how to use MARTE and its time model [START_REF] André | Modeling time(s)[END_REF] to define the semantics of existing models of the RTE domain formally. Following some similar work on AADL [START_REF] André | Modeling of AADL data-communications with UML Marte[END_REF], this paper focuses on EAST-ADL and explains how to express the semantics of EAST-ADL timing requirements in MARTE. Based on this semantics, our constraint solver can detect inconsistencies and execute the timing requirements when there is no constraint violations. Giving a formal semantics to automotive-related models to make the specifications open for direct analysis resembles other work on AUTOSAR [START_REF] Richter | Defining a timing model for AUTOSAR -status and challenges[END_REF] and AADL [START_REF] Sokolsky | Schedulability analysis of AADL models[END_REF]. However, instead of defining another specific time model, we reuse MARTE time model in a consistent way so that different models and tools can be compared and can ultimately interoperate. Another direct benefit of using MARTE, a UML Profile, is that any UML-compliant tool can be used to edit, store, version and maintain all required models of systems under study, from requirements and system specification to implementation models.

We start with a brief overview of EAST-ADL capabilities in Section 2. Then Section 3 introduces MARTE time model. Follows our contribution in Section 4 where MARTE is used to define the semantics of EAST-ADL timing requirements. Finally, Section 5 discusses the benefits of our proposal and possible modeling improvements. An Antilock Braking System (ABS) is used as a running example.

Overview of East-ADL2

EAST-ADL (Electronic Architecture and Software Tools, Architecture Description Language) has been initially developed in the context of the East-EEA European project [START_REF] East | Definition of language for automotive embedded electronic architecture approach[END_REF]. To integrate proposals from the emerging standard AUTOSAR and from other requirement formalisms like SYSML, a new release called EAST-ADL2 [START_REF] Cuenot | Managing complexity of automotive electronics using the East-ADL[END_REF][START_REF]East-ADL2 specification[END_REF] has been proposed by the ATESST project.

Structural modeling in EAST-ADL covers both analysis and design levels. In this paper the focus is on the analysis level and especially on timing requirements.

Timing Requirements

EAST-ADL requirements extend SYSML requirements and express conditions that must be met by the system. They usually enrich the functional architecture with extrafunctional characteristics such as variability and temporal behavior. In this paper, we focus on the three kinds of timing requirements available in EAST-ADL:

1. DelayRequirement that constrains the delay "from" a set of entities "until" another set of entities. It specifies the temporal distance between the execution of the earliest "from" entity and the latest "until" entity;

2. RepetitionRate that defines the inter-arrival time of data on a port or the triggering period of an elementary ADLFunction;

3. Input/outputSynchronization that expresses a timing requirement on the input/output synchronization among the set of ports of an ADLFunction. It should be used to express the maximun temporal skew allowed between input or ouput events or data of an ADLFunction.

Timing requirements specialize the meta-class Timin-gRestriction, which defines bounds on system timing attributes. The timing restriction can be specified as a nominal value, with or without a jitter, and can have lower and upper bounds. The jitter is the maximal positive or negative variation from the nominal value. A bound is a real value associated with an implicit time unit (ms, s, . . . ).

Example

As an illustration, we consider an Anti-lock Braking System (ABS). This example and the associated timing requirements are taken from the ATESST report on EAST-ADL timing model [START_REF] Johansson | ATESST timing model[END_REF]. The ABS architecture consists of four sensors, four actuators and an indicator of the vehicle speed. The sensors ( i f l , i f r , i rl , i rr ) measure the rotation speed of the vehicle wheels. The actuators (o f l , o f r , o rr and o rl ) indicate the brake pressure to be applied on the wheels. The FunctionalArchitecture is composed of FunctionalDevices for sensors and actuators and an ADLFunctionType for the functional part of the ABS. An ADLOutFlowPort provides the vehicule speed (speed).

The execution of the ABS is triggered by R. Parameter L s measures the latency of sensor sampling. The values of the four sensors involved in the ABS must arrive on the input ADLFlowPorts within delay J ii (InputSynchronization). A similar OuputSynchronization delay J oo is represented on the output interface side. The L io represents the delay from the first event on the input set of the ABS until the last event occurrence on the output set. The sampling interval of the sensor is given by parameter H. These parameters are modeled by timing requirements characterized by timing values or intervals with jitters.

R k-1 R k Lsk-1 Liok-1 Joo,k Jii,k Joo,k-1 Jii,k-1 Lsk Liok H k-1

MARTE Time Model

MARTE defines a broadly expressive Time Model that provides for a generic timed interpretation of UML models. In this presentation we focus on clocks and clock constraints. MARTE time model is described in a previous paper [START_REF] André | Modeling time(s)[END_REF]. It defines time structure as a set of interdependent clocks. Interdependency results from relationships existing between instants i and j from different clocks: precedence (i ≺ j), coincidence (i ≡ j) or exclusion (i#j). In practice, such relationships are specified with the dedicated language CCSL. It is briefly presented in the next subsection.

CCSL

CCSL (Clock Constraint Specification Language) is a non normative language annexed to MARTE specification. It is a declarative language that specifies constraints imposed on the clocks (activation conditions) of a model. These constraints can be classified into four categories: synchronous, asynchronous, mixed, and non-functional.

Synchronous clock constraints rely on coincidence.

Subclocking is such a constraint: each instant of the subclock must coincide with one instant of the superclock. The mapping must be order-preserving. To model the ABS (see Section 4) we need two other synchronous constraints: dis-cretizedBy and isPeriodicOn. The former discretizes a dense clock. It is mainly used to derive a discrete chronometric clock from IdealClk. IdealClk is a dense chronometric clock, predefined in MARTE Time Library, and supposed to follow "physical time" faithfully. For instance:

Clock c 10 = IdealClk discretizedBy 0.0001 (1)
Eq. 1 specifies that c 10 is a discrete chronometric clock whose period is 0.0001 s, where s is the time unit associated with IdealClk, therefore c 10 is a 10 kHz clock.

Clock c 1 isPeriodicOn c 10 period 10

(2)

Eqs. 2 reads that there is a tick of c 1 every 10 th ticks of c 10 (i.e., c 1 is a 1 kHz clock).

Asynchronous clock constraints are based on precedence, which may appear in a strict (≺) or a non-strict ( ) form.

The clock constraint "a isFasterThan b" (symbolically denoted by a b) specifies that a is (non-strictly) faster than b, that is for all natural number k, the k th instant of a precedes or is coincident with the k th instant of b

(∀k ∈ N, a[k] b[k]). "b isSlowerThan a" is equivalent to "a isFasterThan b".
Alternation is a form of mutual precedence. "a alternatesWith b" (symbolically denoted by a ∼ b)

states that ∀k ∈ N , a[k] ≺ b[k] ≺ a[k + 1].
Mixed clock constraints combine coincidence and precedence.

Given 2 clocks a and b, "c = inf(a, b)" is the slowest clock faster than both a and b Non Functional Property constraints apply to chronometric clocks. While IdealClk is supposed to be perfect, an actual clock may have flaws. For instance, its period may not be strictly constant with respect to IdealClk. CCSL introduces special constraints to specify stability, drift, offset. . . of chronometric clocks. Examples of stability are given in Section 4.

(∀k ∈ N , c[k] ≡ if a[k] b[k] then a[k] else b[k]).
Stochastic parameters are available in CCSL. Nondetermism introduced by such parameters may reflect a hidden detail or a partial knowledge about the actual constraints. The uniform distribution is often used to represent a tolerance interval on a duration.

TimeSquare

TIMESQUARE is the software environment we propose to deal with MARTE time model and CCSL. TIMESQUARE is an Eclipse plugin that has four main functionalities: 1) interactive clock-related specifications, 2) clock constraint checking, 3) generation of a solution, 4) displaying and exploring waveforms.

A wizard, included in TIMESQUARE, facilitates clock definitions, clock constraint specifications, model element browsing, and parameter setting.

The second functionality checks constraint sanity and is called when the above mentioned wizard is not used.

The third functionality relies on a constraint solver that yields a satisfying execution trace or issues an error message in case of inconsistency. The traces are given as waveforms written in an IEEE standard textual format [START_REF]IEEE Standard for Verilog Hardware Description Language. Design Automation Standards Committee[END_REF] for dumpfiles (called VCD). The solver intensively uses Binary Decision Diagrams (BDD) to manipulate boolean equations induced by CCSL clock constraints.

Modeling East-ADL requirements in CCSL 4.1 Clocks and events

The term clock used in CCSL may be misleading and deserves to be further discussed. Every event on which we want to attach time constraints can be associated with a clock. Event is taken here in the very broad sense (as in UML) to denote something that happens (the start of an action, the receipt of a message, . . . ). In that case, CCSL clocks represent the set of instants at which the concerned event occurs. CCSL clock constraints are relations amongst instants of the related clocks.

Overall, there are two kinds of relations supported by EAST-ADL timing requirements. Either, a timing requirement specifies a temporal relation between two successive occurrences of a single event (i.e., two successive instants of the same clock), or between occurrences of different events. In the latter case, it is mostly between events that occur at the same rate and constraints relate the i th occurrence of one event and the i th occurrence of other events. Additionally, all requirements involve physical time but most also induce logical relations. A part of our contribution is to make explicit these logical relations by applying the adequate CCSL constraints. This section shows how CCSL can express the example described in Section 2.

Repetition rate

A RepetitionRate concerns successive occurrences of the same event (data arriving to or departing from a port, triggering of a function). In all cases, it consists in giving a nominal duration between two successive occurrences/instants of the same event/clock. When the duration is specified in terms of number of occurrences of another event (for instance, number of clock cycles), CCSL relation isPeriodicOn must be used. When the duration is given in seconds (time unit s), then relation discretizedBy must be used. The two relations can also be combined to give both logical and physical constraints. Eq. 2 (Section 3.1) builds a discrete, 1 kHz chronometric clock c 1 . Eq. 3 uses c 1 to specify a 5 ms repetition rate for function f , where f.start is a clock associated with the beginning of function f . This equation defines f.start as being a subclock of c 1 five times less frequent.

f.start isPeriodicOn c 1 period 5

(3) MARTE stereotype TimedProcessing enables the association of a clock with the start event of a given behavior. If a jitter is associated with the period, CCSL relation hasStability should be used: f.start hasStability 0.2. This stability property combined with Eq. 3 states that the jitter on the nominal 5 ms period is 1 ms, thus expressing the EAST-ADL repetition rate.

Delay requirements

A DelayRequirement constrains the delay between a set of inputs and a set of outputs. Input (resp. output) synchronizations are a specialization without outputs (resp. inputs). At each iteration, all inputs and outputs must occur. So for each iteration i, it is a matter of constraining the temporal distance between the i th occurrences of two events (occurring on inputs or outputs). These requirements concern two sets of events and imply a constraint between the earliest occurrence out of the first set (CCSL relation inf) and the latest occurrence out of the second set (CCSL relation sup). In our example, function abs has four inputs, one for each wheel. Eqs. 4-5 specify the inf and sup for the inputs. Analogous equations exist for the outputs.

Clock i inf = inf(i f l , i f r , i rl , i rr ); (4) Clock i sup = sup(i f l , i f r , i rl , i rr ); (5)
CCSL operator delayedFor builds from an initial clock a delayed clock for a given duration. Combining delayed-For with isFasterThan allows for specifying distances between two clock instants. Here again, distance are a priori expressed in number of ticks of a reference clock. For instance, Eq. 6 denotes a delay requirement and states that the maximum end-to-end latency of the function abs is 3 ms. It characterizes the requirement L io .

o sup (i inf delayedFor 3 on c 1 ) (6)
When the latency is not a nominal value, similar mechanisms as the one explained in the previous subsection can be exploited to define lower/upper bounds and jitters.

Causal relationships

An ADLFunctionType implies a data flow execution. The flow goes from sensors to actuators and is repeated forever. This causal flow can be modeled in CCSL with the precedence-based relation alternatesWith ∼ . For instance, equations below state that each release R must be followed by one and only one capture of each input, one execution of the function abs and one output emission and that all these actions must be performed before the next release. The last equation specifies that all inputs must occur before any output is emitted.

R ∼ i inf R ∼ i sup R ∼ abs R ∼ o inf R ∼ o sup i sup ∼ o inf

Discussion and results

The previous section has shown how to express the timing requirements of the ABS system in a declarative way using CCSL. This specification can be used as a golden model to validate existing realizations of the system. With TIMESQUARE, we may go a step further towards executable specifications. Starting with timing requirements expressed in CCSL, we build a refined model of the timed behavior of the ABS system. Refining a constraint may consist in fixing some parameter values, in accordance with the initial specification. Then, TIMESQUARE simulates the refined model, possibly revealing inconsistency in the specification.

The ABS can be modeled as a periodic data flow system, or, because of the jitters, an almost periodic one. MARTE clocks and clock constraints can easily capture such a data flow behavior. We use Timers, a specialization of the constraint delayedFor, for modeling latencies associated with communications and processings. For instance, i = R delayedFor L s on c 10 reads that R causes the availability of a sample (clock i) with a latency L s . Note that L s is a stochastic delay defined here by a uniform distribution Uniform(10..30). Since the delay is measured on clock c 10 (10 kHz), the above constraint states that 1 ms ≤ L s ≤ 3 ms, in accordance with the given timing specifications.

Clock i is not part of the ABS specification. We have introduced this auxiliary clock to respect faithfully Figure 1 borrowed from the ATESST example. Since there are 4 values to acquire, instead of one clock (i), we had to consider 4 clocks (i f l , i f r , i rl , i rr ) each of them being delayed from i for a random duration Uniform(0..5) on c 10 (i.e., 0 ≤ delay ≤ 0.5 ms). This gives rise to the input synchronization jitter J ii forced to be less than 0.5 ms.

The current version of TIMESQUARE generates a sequence of steps that satisfies the set of constraints. An inconsistent specification can lead to a deadlock: after a finite sequence of steps, the simulation gets at a point where all the clocks are disabled (not allowed to tick). With the values given in the specification, we have encountered such a deadlock caused by the violation of an alternation: R tries to occur while o sup has not occurred yet. R being the (indirect) cause of all other events, the simulation cannot proceed on. This diagnostic is facilitated by the generation of an anomaly report file that contains the history upto the deadlock and the state of all clocks at the faulty step. Raising the period of R from 7 ms to 8 ms allows o sup to occur in time and the simulation runs without abortion. Of course, this is not a proof of correctness. A simulation is conclusive only when it reveals a counter-example. Note that the initial specification allowed only 5 ms to period R, which was certainly insufficient and surely rejected by the simulator.

An inconsistent specification may also lead to a "livelock": a subset of clocks are mutually exclusive so that they cannot tick any more. This behavior is easily spotted on the simulation display by the persistent absence of pulses for these clocks. This is only a presumption of livelock, that should be confirmed or invalidated by inspection of the simulation trace.

Conclusion

The AUTOSAR community has screened the Time model proposed by the ATESST project and identified a number of delicate issues. Our approach meets these conclusions and can be considered as a proposal to solve some of the issues.

In this paper, we have expressed the semantics of EAST-ADL timing requirements using MARTE CCSL. More details about our approach are available in a research report [START_REF] Mallet | Marte CCSL and East-ADL2 timing requirements[END_REF]. Making the semantics formal allows for an automatic detection of logical inconsistencies. Then, requirements have been refined into an executable specification to run simulations and explore the dynamic properties of acceptable executions. Executable specifications certainly help detecting errors at early stages. Using a pivot language, like CCSL, allows for better interoperability and builds on the large number of UML-based modeling frameworks.

Even though the simulation can detect some errors in the specification, it should be combined with exhaustive formal analyses. For instance, livelock detection can be automated with model-checking approaches, provided that the state-space is finite. TIMESQUARE already generates the state-space at each step. For now, our default simulation policy consists in randomly selecting a minimal set of fireable clocks at each simulation step. Other policies must be implemented and combined with validation of properties.

Figure 1 .

 1 Figure 1. Timing model of the ABS

  Similarly, "d = sup(a, b)" is the fastest clock among all clocks slower than both a and b ∀k ∈ N, d[k] ≡ if a[k] b[k] then b[k] else a[k] . Most of the time, inf and sup clocks are neither a nor b. inf and sup are easily extended to sets of clocks.Another mixed clock constraint enforces delayed coincidences. "c = a delayedFor n on b" imposes c to tick synchronously with the n th tick of b following a tick of a. It is considered as a mixed constraint since a and b are not assumed to be synchronous.


Wilson Jallet

Antoine Bambade

Nicolas Mansard

Justin Carpentier

Constrained Differential Dynamic Programming: A primal-dual augmented Lagrangian approach

Trajectory optimization is an efficient approach for solving optimal control problems for complex robotic systems. It relies on two key components: first the transcription into a sparse nonlinear program, and second the corresponding solver to iteratively compute its solution. On one hand, differential dynamic programming (DDP) provides an efficient approach to transcribe the optimal control problem into a finite-dimensional problem while optimally exploiting the sparsity induced by time. On the other hand, augmented Lagrangian methods make it possible to formulate efficient algorithms with advanced constraint-satisfaction strategies. In this paper, we propose to combine these two approaches into an efficient optimal control algorithm accepting both equality and inequality constraints. Based on the augmented Lagrangian literature, we first derive a generic primal-dual augmented Lagrangian strategy for nonlinear problems with equality and inequality constraints. We then apply it to the dynamic programming principle to solve the value-greedy optimization problems inherent to the backward pass of DDP, which we combine with a dedicated globalization strategy, resulting in a Newton-like algorithm for solving constrained trajectory optimization problems. Contrary to previous attempts of formulating an augmented Lagrangian version of DDP, our approach exhibits adequate convergence properties without any switch in strategies. We empirically demonstrate its interest with several case-studies from the robotics literature.

I. INTRODUCTION

In this paper, we are interested in solving constrained continuous-time optimal control problems (OCP) of the form:

min x,u T 0 (t, x(t), u(t)) dt + T (x(T )) (1a)
s.t. f (t, x(t), u(t), ẋ(t)) = 0, t ∈ [0, T ) (1b)

x(0) = x0 (1c) h(t, x(t), u(t)) 0 (1d) h T (x(T )) 0, (1e)
where and T are the running and terminal costs respectively, (1b) accounts for the system dynamics written as a differential-algebraic equation (DAE) (and includes the ODE case ẋ = f (t, x(t), u(t))). We denote X and U the state and a LAAS-CNRS, 7 Avenue du Colonel Roche, F-31400 Toulouse, France b Inria, Département d'informatique de l'ENS, École normale supérieure, CNRS, PSL Research University, Paris, France c ENPC, France, * corresponding author: wjallet@laas.fr This work was supported in part by the HPC resources from GENCI-IDRIS (Grant AD011011342), the French government under management of Agence Nationale de la Recherche as part of the "Investissements d'avenir" program, reference ANR-19-P3IA-0001 (PRAIRIE 3IA Institute) and ANR-19-P3IA-000 (ANITI 3IA Institute), Louis Vuitton ENS Chair on Artificial Intelligence, and the European project MEMMO (Grant 780684). control spaces, T > 0 the time horizon, x0 ∈ X the initial condition, h(•) and h T (•) the path and terminal constraints.

For numerical resolution, the continuous-time OCP (1) must be transcribed into a finite-dimensional optimization problem (i.e., with a finite number of variables, which the continuous-time trajectories are not) [START_REF] Betts | Discretize then optimize[END_REF]. Several transcriptions are possible [START_REF] Hargraves | Direct trajectory optimization using nonlinear programming and collocation[END_REF], [START_REF] Diehl | Fast direct multiple shooting algorithms for optimal robot control[END_REF], [START_REF] Diehl | Efficient numerical methods for nonlinear MPC and moving horizon estimation[END_REF]. Differential Dynamic Programming (DDP) is a particular OC algorithm which implies a direct transcription known as single shooting [START_REF] Murray | Differential dynamic programming and Newton's method for discrete optimal control problems[END_REF]. Popularized in robotics in the late 2000s [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF], it has the advantage over other transcriptions of providing a simple formulation, optimally exploiting the sparsity of the resulting nonlinear programs while providing feedback gains at no extra cost. The corresponding transcription, extended to any constraints, reads:

min x,u N -1 k=0 k (x k , u k ) + N (x N ) (2a) s.t. f k (x k , u k , x k+1 ) = 0, k ∈ 0, N -1 (2b) x 0 = x0 (2c) h k (x k , u k ) 0 (2d) h N (x N ) 0, (2e)
where h k , h N , f k are appropriate functions discretizing the dynamics and path constraints depending on the given numerical discretization scheme employed. The k are approximations of the cost integrals t k+1 t k (t, x(t), u(t)) dt. We use the shorthands x def = (x 0 , . . . , x N ) and u def = (u 0 , . . . , u N -1 ) for the discretized state and control trajectories.

While the nominal DDP algorithm is not able to handle path constraints, implicit integrators or multiple-shooting stabilization, several improvements have been proposed over the years to equip it with these properties. In this paper, we focus on the handling of equality and inequality constraints, and we first review previous work focusing on it.

A first subcase of interest only considers OCP with control bounds, which can be handled by a projected quasi-Newton approach [START_REF] Tassa | Control-limited differential dynamic programming[END_REF]. Several other projection-based formulations have then been proposed to extend DDP [START_REF] Giftthaler | A projection approach to equality constrained iterative linear quadratic optimal control[END_REF], [START_REF] Xie | Differential dynamic programming with nonlinear constraints[END_REF], none of which have been shown to be robust enough to be widely adopted in robotics. To account fro inequality constraints, interior-point methods [START_REF] Pavlov | Interior Point Differential Dynamic Programming[END_REF], [START_REF] Singh | Optimizing Trajectories with Closed-Loop Dynamic SQP[END_REF] have also been recently investigated; however, these do not allow for easy warmstarting [START_REF] Diehl | Fast direct multiple shooting algorithms for optimal robot control[END_REF] which is unsuitable for online optimization and application to model-predictive control (MPC) [START_REF] Wang | Fast model predictive control using online optimization[END_REF], [START_REF] Rawlings | Model predictive control: theory, computation, and design[END_REF].

In the past few years, augmented Lagrangian approaches have emerged as a suitable solution for solving constrained trajectory optimization problems [START_REF] Toussaint | A novel augmented lagrangian approach for inequalities and convergent any-time non-central updates[END_REF]. As argued later in this paper, it offers many of the good properties that we need for trajectory optimization: super-linear convergence or even more quadratic convergence, stability, ability to warmstart, and so on. Yet the first attempt to write dedicated OCP solvers based on augmented Lagrangians exhibited poor convergence properties. Thereby, further refinement using a projection in a two-stage approach had to be introduced in the solver ALTRO [START_REF] Howell | ALTRO: A Fast Solver for Constrained Trajectory Optimization[END_REF]. The penalty function used in ALTRO was then recognized to be irregular and discarded in [START_REF] Aoyama | Constrained Differential Dynamic Programming Revisited[END_REF], which introduces a switch to an SQP formulation to converge to a higher precision.

A key idea that we exploit in this paper is to introduce the augmented Lagrangian formulation directly in the backward pass, to solve the value-greedy problems while directly considering the constraints, as initially proposed for multiphase constrained problems [START_REF] Lantoine | A Hybrid Differential Dynamic Programming Algorithm for Constrained Optimal Control Problems. Part 1: Theory[END_REF]. This enables us to obtain better numerical accuracy for equality-constrained problems, by stabilizing the backward pass using a primal-dual system of equations to compute the control and multipliers together [START_REF] Kazdadi | Equality Constrained Differential Dynamic Programming[END_REF], and a monotonic update of the penalty parameter derived from the bound-constrained Lagragian (BCL) [START_REF] Conn | A Globally Convergent Augmented Lagrangian Algorithm for Optimization with General Constraints and Simple Bounds[END_REF] strategy. Their method converges reliably to good numerical accuracy. We have recently extended this formulation to also account for the dynamics and other equality constraints using a primal-dual augmented Lagrangian, allowing for the inclusion of infeasible initialization and implicit integrators [START_REF] Jallet | Implicit Differential Dynamic Programming[END_REF].

In this paper, we introduce a complete augmented Lagrangian DDP algorithm for handling both equality and inequality constraints, and validate it on several real-size robotic scenarios. We first introduce in Sec. II a primaldual algorithm, rooted in the nonlinear programming literature [START_REF] Gill | A primal-dual augmented Lagrangian[END_REF], to handle generic nonlinear optimization problems (NLPs). We then adapt it to the specific case of OCPs of the form (2) in Sec. III. It results in an overall secondorder quasi-Newton-like algorithm with good convergence properties for solving constrained trajectory optimization problems. We finally benchmark our method in Sec. IV on various standard case studies from the robotics literature. A companion video is available 1 .

II. THE PRIMAL-DUAL AUGMENTED LAGRANGIAN METHOD FOR CONSTRAINED OPTIMIZATION

This section introduces our augmented Lagrangian approach to solve constrained nonlinear optimization problems (NLP) of the form:

min x∈R n f (x) s.t. c(x) = 0, h(x) 0, (3)
where c and h stands for equality and inequality constraints respectively. We then adapt this approach in Sec. III to the case of trajectory optimization. While many augmented Lagrangian approaches have been introduced in the optimization literature [START_REF] Nocedal | Numerical Optimization[END_REF], most of them rely on alternating between primal solving and dual updates. In this work, we propose 1 https://peertube.laas.fr/videos/watch/dfeca51c-c2cf-468a-b46a-86f808e9a561

instead to compute combined primal-dual steps by taking inspiration from the work of Gill and Robinson in [START_REF] Gill | A primal-dual augmented Lagrangian[END_REF], which we extend by also considering inequality constraints and by connecting it to the proximal method of multipliers (PMM) [START_REF] Rockafellar | Augmented Lagrangians and Applications of the Proximal Point Algorithm in Convex Programming[END_REF] that we use to for numerical robustness. We discuss these contributions in more detail at the end of this section.

A. Optimality conditions

The Lagrangian L associated with (3) is defined by:

L(x, λ, ν) = f (x) + λ c(x) + ν h(x), λ ∈ R ne , ν ∈ R ni + ( 4
) A saddle point of L is a solution of (3). This leads to the Karush-Kuhn-Tucker (KKT) necessary conditions [START_REF] Boyd | Convex optimization[END_REF] for ensuring a primal-dual point (x, λ, ν) to be optimal:

∇ x L(x, λ, ν) = 0, c(x) = 0 and h(x) 0, ν 0 and h(x) ν = 0. (KKT)
In practice, we search for a triplet (x, λ, ν) satisfying these optimality conditions (KKT) up to a certain level of predefined accuracy abs > 0, leading us to the following natural absolute stopping criterion:

∇ x L(x, λ, ν) ∞ abs , (c(x), [h(x)] + ) ∞ abs , (5)
where [z] + denotes the projection of z ∈ R nz on R nz + .

B. Equality constrained nonlinear programming problems

In this section, we provide a high-level overview on the primal-dual augmented Lagrangian (PDAL) method. It is closely related to the probably even more famous Method of Multipliers (MM) [START_REF] Bertsekas | Constrained optimization and Lagrange multiplier methods[END_REF]Chapter 2], which we review first in the context of purely equality-constrained NLPs.

Primal-dual augmented Lagrangian. The PDAL function L A µ [21, Section 3] is defined by augmenting the standard Lagrangian L (4) with two squared 2 penalties:

M A µ (x, λ; λ e ) def = L(x, λ e , 0) + 1 2µe c(x) 2 2 + 1 2µe c(x) + µ e (λ e -λ) 2 2 , (6)
where µ e > 0 is a scalar2 . The PDAL method then searches a sequence of iterates approximately minimizing (6) [START_REF] Robinson | Primal-dual methods for nonlinear optimization[END_REF]:

x l+1 , λ l+1 ≈ ω l min x,λ M A µ (x, λ; λ l ), (7)
where ≈ ω l stands for requiring (x l+1 , λ l+1 ) to be an ω l -approximate solution to subproblem [START_REF] Tassa | Control-limited differential dynamic programming[END_REF]. The approximation is controlled via the following condition:

r l (x l+1 , λ l+1 ) ∞ ω l , (8)
where r l accounts for the optimality conditions at iteration l: if r l (x, λ) ∞ abs then (x, λ) is a solution to (7) at precision abs . The formula for r l will be specified in the sequel. The first subproblems are solved coarsely, since a precise solution is not required when the multiplier estimates λ 0 are far from the optimal dual solution: this avoids unnecessary computation.

To enforce the generated sequence (x l , λ l ) to converge to a local solution of NLP problem (3), we must address two important aspects: (i) computing suitable iterates (x l+1 , λ l+1 ) satisfying (8) efficiently; (ii) choosing appropriate rules for scheduling ω l (ω l should decrease) and adequately increasing µ e (as shown by the theory, µ e should be increased over the iterations, but a too large value may drastically impact the overall numerical stability [START_REF] Nocedal | Numerical Optimization[END_REF]).

In the last two paragraphs of section II-B, the problem (i) of finding suitable approximations for the subproblems is handled in the next paragraph. Then we review in the last paragraph the BCL globalization strategy (originating from [START_REF] Conn | A Globally Convergent Augmented Lagrangian Algorithm for Optimization with General Constraints and Simple Bounds[END_REF]) for dealing with (ii).

Primal-dual Newton descent. The stationarity condition of ( 7) is :

∇M A µ (x, λ; λ l ) = 0. (9)
Iterates (x l+1 , λ l+1 ) satisfying ( 9) at precision ω l can be derived using a quasi-Newton descent [START_REF] Gill | A primal-dual augmented Lagrangian[END_REF], which iteration at t + 1 starting from (x 0 l , λ0 l ) = (x l , λ l ) reads:

H l + 1 µe J c J c -J c -J c µ e I δx δλ = - ∇ x L A µ (x t l , λt l ; λ l ) ∇ λ L A µ (x t l , λt l ; λ l ) , (10) with:
xt+1 l = xt l + δx, λt+1 l = λt l + δλ, (11)
and where H l is the Lagrangian Hessian ∇ 2 x L(x t l , 2π l (x t l )λt l , 0) or an approximation thereof, with π l (x) def = λ l + 1 µe c(x) (following the notation in [START_REF] Gill | A primal-dual augmented Lagrangian[END_REF]), and J c the constraint Jacobian matrix at xt l . There are two conflicting goals to balance in this iterative process. First, the smaller the value of µ e the faster the convergence, as 1 µe penalizes the constraints. Second, as µ e gets smaller, the conditioning of (H l + 1 µe J T c J c ) gets worse, thereby harming the numerical robustness of the approach, in particular when J c has a large condition number.

Fortunately, the linear system (10) can be rewritten in the following equivalent form:

H l J c J c -µ e I δx δλ = - ∇ x L(x t l , λt l , 0) c(x t l ) + µ e (λ l -λt l ), , (12)
which shows that the PDAL method is closely related to the method of multipliers (MM) [START_REF] Bertsekas | Constrained optimization and Lagrange multiplier methods[END_REF]Chapter 2]. Indeed, [START_REF] Wang | Fast model predictive control using online optimization[END_REF] implies that the sequence of iterates (x l , λ l ) approximate those of a proximal-point method applied to the dual of (4):

x l+1 , λ l+1 ≈ ω l min x max λ L(x, λ, 0) -µe 2 λ -λ l 2 2 . ( 13
)
Hence, µ e is the inverse of the step-size of the equivalent MM, and it directly calibrates the convergence speed of the approach (see [START_REF] Rockafellar | Augmented Lagrangians and Applications of the Proximal Point Algorithm in Convex Programming[END_REF]Section 4] for details). Moreover, this linear system involves a matrix that is always nonsingular thanks to the regularization terms -µe 2 λ -λ l 2 2 . In other words, the problem ( 12) is always well-defined in the iterative process. Such linear systems are also better conditioned than [START_REF] Pavlov | Interior Point Differential Dynamic Programming[END_REF] [22, Section 17.1]. Finally, [START_REF] Wang | Fast model predictive control using online optimization[END_REF] implies that the sequence (x t l , λt l ) t 0 converges to a pair (x * , λ * ) satisfying the following optimality conditions:

∇ x L(x * , λ * , 0) c(x * ) + µ e (λ l -λ * ) = 0. (14)
Hence, we choose the optimality criterion function r l to be:

r l (x, λ) def = ∇ x L(x, λ, 0) c(x) + µ e (λ l -λ) . ( 15
)
The globalization strategy. For fixing the hyper-parameters (tolerance on subproblems ω l , step-sizes µ e ), we rely on BCL (see [START_REF] Conn | A Globally Convergent Augmented Lagrangian Algorithm for Optimization with General Constraints and Simple Bounds[END_REF] and [START_REF] Nocedal | Numerical Optimization[END_REF]Algorithm 17.4]) which has been proved to perform well in advanced optimization packages such as LANCELOT [START_REF]LANCELOT: a Fortran package for large-scale nonlinear optimization (Release A)[END_REF] and also in robotics for solving constrained optimal control problems [START_REF] Aoyama | Constrained Differential Dynamic Programming Revisited[END_REF], [START_REF] Plancher | Constrained unscented dynamic programming[END_REF].

The main idea underlying BCL consists in updating the dual variables λ l from (13) only when the corresponding primal feasibility (denoted by η l hereafter) is small enough. More precisely, we use a second sequence of tolerances denoted by l (which we also tune within the BCL strategy) and update the dual variables only when η l+1 l , where η l+1 denotes the primal infeasibility as follows:

η l+1 def = c(x l+1 ) ∞ . (16)
It remains to explain how the BCL strategy chooses appropriate values for the hyper-parameters ω l , l and µ e . As for the update of the dual variables, it proceeds in two stages:

• If η l+1 < l : the primal feasibility is good enough, we thus keep the constraint penalization parameters as is. • Otherwise: the primal infeasibility is too large, we thus increase quadratic penalization terms on the constraints for the subsequent subproblem [START_REF] Tassa | Control-limited differential dynamic programming[END_REF]. Concerning the accuracy parameters ω l and l , the update rules are more technical and the motivation underlying those choices is to ensure global convergence: an exponentialdecay type update when primal feasibility is good enough, and see [START_REF] Conn | A Globally Convergent Augmented Lagrangian Algorithm for Optimization with General Constraints and Simple Bounds[END_REF]Lemma 4.1] for when the infeasibility is too large. The detailed strategy is summarized in Algorithm 1 for the general case (including inequalities).

C. Extension to inequality constrained nonlinear programs

As we will see, our approach developed for tackling equality constraints easily extends to the general case. Indeed, as we will see the PDAL function only changes in a subtle way for taking into account inequality constraints. As a result, it also impacts how the minimization procedure must be realized.

Generalized primal-dual merit function. In the general setup, the PDAL function can be framed in its equality constrained form introducing a slack variable z 0 satisfying the new equality constraint:

h(x) -z = 0. (17)
Hence, the generalized PDAL function reads:

L A µ (x, λ,ν, z; λ l , ν l ) def = L(x, λ l , ν l ) + 1 2µe c(x) 2 2 + 1 2µe c(x) + µ e (λ l -λ) 2 2 + 1 2µi h(x) -z 2 2 + 1 2µi h(x) -z + µ i ν l -µ i ν 2 2 + g(z). (18)
g is the (component-wise) indicator function related to z 0:

g(z) def = 0 if z i 0, i ∈ [1, n i ], +∞ otherwise.
The minimization of (18) w.r.t. x, λ, ν or z variables commutes. Considering the problem structure and following ideas from [START_REF] Marchi | On a primal-dual Newton proximal method for convex quadratic programs[END_REF], it can be shown that z and ν can be directly deduced as functions of x:

ẑ(x, ν l ), ν(x, ν l ) def = arg min z,ν L A µ (x, λ, ν, z; λ l , ν l ), ẑ(x, ν l ) = [h(x) + µ i ν l ] -, ν(x, ν l ) = 1 µi h(x) + ν l + . (19)
The minimization problem can thus be reduced to:

min x,λ,ν,z L A µ (x, λ, ν, z; λ l , ν l ) = min x,λ L A µ (x, λ, ν(x, ν l ), ẑ(x, ν l ); λ l , ν l ). (20)
Yet, we choose to maintain the ν variable relaxed in the minimization procedure [START_REF] Jallet | Implicit Differential Dynamic Programming[END_REF] as it enables us to preserve similar well conditioned linear systems and stopping criterion derived in [START_REF] Wang | Fast model predictive control using online optimization[END_REF] and [START_REF] Toussaint | A novel augmented lagrangian approach for inequalities and convergent any-time non-central updates[END_REF]. Consequently, the generalized merit function corresponds to:

M µ (x, λ, ν; λ l , ν l ) def = L A µ (x, λ, ν, ẑ(x, ν l ); λ l , ν l ) = f (x) + 1 2µe c(x) + µ e λ l 2 2 + 1 2µe c(x) + µ e (λ l -λ) 2 2 + 1 2µi [h(x) + µ i ν l ] + 2 2 + 1 2µi [h(x) + µ i ν l ] + -µ i ν 2 2 . ( 21
)
For ensuring better regularization w.r.t. the primal variable x, following the PMM [START_REF] Rockafellar | Augmented Lagrangians and Applications of the Proximal Point Algorithm in Convex Programming[END_REF], we finally consider the following generalized primal-dual merit function:

M µ,ρ (x, λ, ν; x l , λ l , ν l ) def = M µ (x, λ, ν; λ l , ν l )+ ρ 2 x-x l 2 2 , (22)
with ρ > 0 a proximal parameter.

Semi-smooth Newton step with line-search procedure. Contrary to the equality-constrained case, [START_REF] Nocedal | Numerical Optimization[END_REF] now corresponds to a semi-smooth function (due to the presence of the positive orthant projection operators [•] + ). It should thus be minimized using a semi-smooth quasi-Newton iterative procedure [33, Chapter 1], [START_REF] Nocedal | Numerical Optimization[END_REF]Chapter 6]. To ensure convergence, the procedure must have a line-search scheme 3 over the semi-smooth convex primal-dual merit function [START_REF] Nocedal | Numerical Optimization[END_REF] to find an adequate step size along the primal-dual Newton direction, following an approach similar to [START_REF] Wang | Fast model predictive control using online optimization[END_REF], while also 3 Different line-search schemes can be used to minimize the merit function ( 22) through a semi-smooth quasi-Newton iterative procedure.

considering the change of active inequality-constraints defined by A l (x):

A l (x) def = {j | (ν l + µ i h j (x)) 0}, (23)
where A l (x) is the shifted active-set of the l-th subproblem at point x. This definition of shifted active set differs from existing augmented Lagrangian-based optimal control methods in robotics which defines the active-set by the condition h j (x) 0, as done in [START_REF] Howell | ALTRO: A Fast Solver for Constrained Trajectory Optimization[END_REF], [START_REF] Li | Hybrid systems differential dynamic programming for whole-body motion planning of legged robots[END_REF].

Algorithm 1: PDAL Method for constrained optimization 1 Inputs:

• initial states: x 0 , λ 0 , ν 0 ,

• initial parameters: 0 , ω 0 , ρ, µ e , µ i > 0,

• hyper-parameters: µ f < 1, α bcl ∈ (0, 1), β bcl ∈ (0, 1), µ i , µ e > 0. while Stopping criterion (5) not satisfied do Compute ( x l+1 , λ l+1 , ν l+1 ) satisfying ( 26) using II-C ;

x l+1 = x l+1 ; if η l+1 < l then l+1 = l µ βbcl i ; ω l+1 = ω l µ i ; λ l+1 = 2 λ(x l+1 , λ l ) -λ l+1 ; ν l+1 = [2ν(x l+1 , ν l ) -ν l+1 ] + ; else µ i ← -max(µ i , µ f µ i ), µ e ← -max(µ e , µ f µ e ); l+1 = 0 µ αbcl i ; ω l+1 = ω 0 µ i ; λ l+1 = λ l ; ν l+1 = ν l ; end l ← l + 1;
end Output: A (x l , λ l , ν l ) satisfying the abs -approximation criterion (5) for problem (3).

D. Final algorithm

Once a local ω l primal-dual solution (x * , λ * , ν * ) minimizing ( 22) is found, for better numerical precision, we follow the Lagrange multiplier update rule introduced in [28, Section 4]:

λ l+1 = 2 λ(x * , λ l ) -λ * , ν l+1 = [2ν(x * , ν l ) -ν * ] + , (24)
where ν(x, ν l ) is defined from the derivation of our generalized merit function in [START_REF] Conn | A Globally Convergent Augmented Lagrangian Algorithm for Optimization with General Constraints and Simple Bounds[END_REF], and λ(x, λ l ) comes from the classic multiplier update rule (similar to ν(x, ν l ) without projections) [START_REF] Robinson | Primal-dual methods for nonlinear optimization[END_REF]. Hence, the measure of the convergence towards the optimality conditions captured by r l (15) can be more generally defined as follows:

r l (x, λ, ν) def =   ∇ x L(x, λ, ν) + ρ(x -x l ) µ e ( λ(x, λ l ) -λ) µ i (ν(x, ν l ) -ν)   . (25)
The generalized inner-loop exit condition thus reads:

r l (x, λ, ν) ∞ ω l . (26)
The primal feasibility also generalizes as:

η l+1 def = (c(x l+1 ), [h(x l+1 )] + ) ∞ . (27)
Algorithm 1 summarizes our approach for solving NLPs.

E. Key novelties of Algorithm 1

Algorithm 1 differs from [START_REF] Gill | A primal-dual augmented Lagrangian[END_REF] for two main aspects. First, we show in the equality-constrained case that the linear systems involved in the (quasi-)Newton steps are equivalent to a better-conditioned linear system originating from the proximal method of multipliers [START_REF] Rockafellar | Augmented Lagrangians and Applications of the Proximal Point Algorithm in Convex Programming[END_REF]. For this reason, we use this equivalent saddle-point system formulation and its associated stopping criterion to enforce the overall numerical stability of the approach. Second, we extend the PDAL function from [START_REF] Gill | A primal-dual augmented Lagrangian[END_REF] to account for inequality constraints by introducing a new merit function that does not require any slack variables.

The resulting algorithm is a generic NLP solver which is a contribution in itself, with direct application in optimization for robotics e.g. [START_REF] Brossette | Multicontact postures computation on manifolds[END_REF]. As our aim is to design a constrained OCP solver, we have left evaluation of this generic solver's performance for future work, and we directly jump to its adaptation to dynamic programming.

III. PRIMAL-DUAL AUGMENTED LAGRANGIAN FOR CONSTRAINED DIFFERENTIAL DYNAMIC PROGRAMMING

In this section, we extend the differential dynamic programming framework accounting for equality constraints [START_REF] Kazdadi | Equality Constrained Differential Dynamic Programming[END_REF] and implicit dynamics [START_REF] Jallet | Implicit Differential Dynamic Programming[END_REF] to the case of inequality constraints using the PDAL introduced in Sec. II.

A. Relaxation of the Bellman equation.

In [START_REF] Jallet | Implicit Differential Dynamic Programming[END_REF], we show that applying the MM to (2) leads to a relaxation of the Bellman equation, in the equalityconstrained case. We now extend this idea to the inequalityconstrained case. Indeed, the discrete-time problem (2) also satisfies a dynamic programming equation in the inequalityconstrained case. The value function for the subproblem at time k satisfies the Bellman relation:

V k (x) = min u,x k (x, u) + V k+1 (x ) s.t. f k (x, u, x ) = 0 and h k (x, u) 0. (28)
The optimality conditions for this Bellman equation involve a Lagrangian function of the form:

L k (x, u, x , λ, ν) = k (x, u) + V k+1 (x ) + λ f k (x, u, x ) + ν h k (x, u). ( 29
)
The last term in this Lagrangian, relating to the inequality constraint, will appear in the KKT conditions of the Bellman equation and influence the sensitivities with respect to x. Following the PDAL method from Sec. II, we can define an augmented primal-dual Q-function modelled after [START_REF] Gill | A primal-dual augmented Lagrangian[END_REF] which reads, considering multiplier estimates (λ l , ν l ), l ∈ N:

Q l µ,k (x, u, x , λ, ν) = k (x, u) + V k+1 (x ) + µe 2 ( 1 µe f k (x, u, x ) + λ l 2 + 1 µe f k (x, u, x ) + λ l -λ 2 ) + µi 2 ( [ν l + 1 µi h k (x, u)] + 2 + [ν l + 1 µi h k (x, u)] + -ν 2 ). ( 30
)
Then, the minimization in the Bellman equation is relaxed to the following augmented Lagrangian iteration:

V l k (x) = min u,x λ,ν Q l µ,k (x, u, x , λ, ν), (31)
with the boundary condition V l (x) = f (x). This dynamic programming equation can be seen as a relaxation of the classical Bellman equation ( 28) using the primal-dual merit function -including the dynamics f k (x, u, x ) in this relaxation leads to a multiple-shooting formulation. Indeed, assuming the multipliers estimates (λ l , ν l ) are optimal multipliers associated with [START_REF] Robinson | Primal-dual methods for nonlinear optimization[END_REF], then any minimizer (ū, x , λ, ν) of ( 31) also satisfies the optimality conditions for [START_REF] Robinson | Primal-dual methods for nonlinear optimization[END_REF].

B. Backward and forward passes

As outlined in the previous section, the semi-smooth (quasi-)Newton descent direction for Q l can be recovered from the system of equations:

K µ     δu δx δλ [δν] A     = -     Q u + Q ux δx Q x + Q x x δx f + f x δx + µ e (λ l -λ) [h + h x δx + µ i (ν l -ν)] A     , (32)
where:

K µ def =     Q uu Q ux f u [h u ] A Q x u Q x x f x [h x ] A f u f x -µ e I [h u ] A [h x ] A -µ i I     , (33)
is a regularized KKT matrix. It is similar to the matrix derived in [START_REF] Jallet | Implicit Differential Dynamic Programming[END_REF], with an additional block covering the active set of inequality constraints, denoted by [•] A . Subscripted symbols (e.g. f x , h u . . . ) denote partial derivatives. We switch convention from [START_REF] Jallet | Implicit Differential Dynamic Programming[END_REF], where µ is the reciprocal of the parameters (µ e , µ i ) we use here.

Since the previous state deviation δx is unknown but the r.h.s. of ( 32) is linear in that parameter, we can recover the solution from the sensitivities, which satisfy:

K µ     k K a A ξ Ξ ζ A Z A     = -     Q u Q ux Q x Q x x f + µ e (λ e -λ) f x [h + µ i (ν l -ν)] A [h x ] A     . ( 34
)
The step is recovered as:

δu = k + Kδx, δx = a + Aδx δλ = ξ + Ξδx, δν = -[ν] A c + ζ A + Z A δx. (35)
In practice, the system (34) is solved by an LDL Cholesky factorization of the KKT matrix K µ . Forward pass and linear rollout. Similarly to [START_REF] Jallet | Implicit Differential Dynamic Programming[END_REF], the primal-dual step is recovered by a linear rollout over [START_REF] Brossette | Multicontact postures computation on manifolds[END_REF]:

δu t = k t + K t δx t , δx t+1 = a t + A t δx t δλ t+1 = ξ t + Ξ t δx t , δν t+1 = -[ν t+1 ] A c + ζ A,t + Z A,t δx t . (36)
The initial step over (δx 0 , δλ 0 , δν 0 ) is associated with the value function, equality and inequality constraints at k = 0.

C. Convergence and globalization strategy

As discussed in Sec. II and following the approach proposed in [START_REF] Kazdadi | Equality Constrained Differential Dynamic Programming[END_REF], [START_REF] Jallet | Implicit Differential Dynamic Programming[END_REF], we use a BCL strategy as an outer loop to automatically update the parameters µ and ρ and the multipliers estimates (λ l , ν l ) according to the progress made on the primal and dual feasibility. We refer to [START_REF] Jallet | Implicit Differential Dynamic Programming[END_REF] to see how BCL is used within the constrained DDP. We also use a backtracking line-search procedure to compute a step length α > 0 at each iteration of the algorithm after the linear rollout. This line-search relies on the assumption that the direction δw = (δx, δu, δλ, δν) is a descent direction satisfying ∇M µ,ρ δw < 0. Denoting φ(α) = M µ,ρ (w + αδw; w l ), our Armijo backtracking procedure looks for the first k ∈ N such that φ(t k ) φ(0) + c 1 t k φ (0) and sets α = t k . To ensure the descent condition, we play on the proximal parameters ρ l > 0 in the outer BCL loop, and have a heuristic similar to [START_REF] Wächter | On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming[END_REF] and [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF] to control the inertias of the regularized KKT matrices [START_REF] Hintermüller | Semismooth newton methods and applications[END_REF], which is central to obtain good convergence behavior. Our stopping criteria is the same as in the constrained optimization framework outlined in Alg. 1.

IV. EXPERIMENTS

For experimental validation our approach, we extend the numerical optimal control framework of [START_REF] Jallet | Implicit Differential Dynamic Programming[END_REF], written in Python, which relies on 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] for providing analytical derivatives [START_REF] Carpentier | Analytical Derivatives of Rigid Body Dynamics Algorithms[END_REF] and NumPy [START_REF] Harris | Array programming with NumPy[END_REF] for linear algebra. Because it is in Python, we do not provide CPU timings against existing implementations.

A. Bound-constrained problems

Bound-constrained LQR. The first system we test is the simple linear-quadratic regulator (LQR) with bound constraints, of the form:

min x,u N -1 k=0 1 2 x k Qx k + 1 2 u k Ru k + 1 2 x N Q N x N s.t. x k+1 = Ax k + Bu k + c, k N -1 x 0 = x0 , -ū u k ū (37)
where Q, Q N and R are positive semi-definite matrices, ū ∈ R nu + are the control bounds. This problem is a convex quadratic program (QP), which can be solved with classical QP solvers. We test a few configurations for the problem parameters (A, c, ū), leading to the results in Fig. 2 and3. For bound-constrained LQR, the proposed method takes about ten iterations to converge to an optimal solution with precision = 10 -8 . Control-bounded LQR Fig. 3. The continuous time system is Ac = 0.4 2 -2 0.4 , c is the same, the target is located near a repulsive equilibrium. Even with the same bound ū = 0.4, the target is not reached. The obtained control is a bang-bang control.

Car parking, as proposed in [START_REF] Tassa | Control-limited differential dynamic programming[END_REF]IV.B.]. The car dynamics is defined by its state variables (x, y, θ, v), the goal is to steer the car to the state (0, 0, 0, 0). The control inputs are the front wheel acceleration a ∈ R and angle ω, with bounds |a| 10 m/s 2 , |ω| 0.5 s -1 . Figure 4 illustrates the resulting trajectory with comments. Following [START_REF] Tassa | Control-limited differential dynamic programming[END_REF], the system makes the distinction between the initial angles 3π 2 and -π 2 , which (interestingly for the interest of the benchmark) forces the solver to find more commutations. We used initial penalty parameters µ 0 = 100, ρ 0 = 10 -5 , and convergence threshold = 2.10 -4 (no convergence threshold was given in [START_REF] Tassa | Control-limited differential dynamic programming[END_REF]). The timestep is dt = 0.03 s and horizon T = 15 s. The problem converges to an optimal solution in 62 iterations. UR5 -throwing task. The goal of this task is to throw a ball at a target velocity v at a time half-way through the horizon T = 1 s (with a minimum velocity in the z direction). We constrain the elbow frame to be above ground, the endeffector stay within a box, along with joint velocity and torque limits. The dynamics are integrated using a second order Runge-Kutta scheme with timestep dt = 0.05 s. The state and control trajectories satisfy the bound constraints as depicted in in Fig. 5. The robot motion is illustrated in Fig. 1. The minimum target end-effector velocity is also satisfied.

B. Obstacle-avoidance

LQR with obstacles. We extend the exemple of the boundconstrained LQR [START_REF] Carpentier | The Pinocchio C++ library -A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives[END_REF] with path constraints that consist in avoiding obstacles. We consider avoiding the interiors of polyhedral sets of the form P

(j) = {x | C (j) x d (j) } which is the piecewise linear constraint max i (C (j) x -d (j) ) i 0. (38)
These constraints make the problem nonconvex and thus cannot be handled by standard convex solvers. Fig. 6 shows an example with both obstacles and control which saturate. UR10 -reach task with obstacles. The goal is for the end-effector p e (q) to reach a target p ∈ R 3 , expressed as a terminal cost f (x) = 1 2 p e (q) -p 2 Wee . We also impose waypoint constraints at t 0 , t 1 ∈ (0, T ), with time horizon T = 3 s. As obstacles, we choose simple vertical cylinders of radius r C and impose that they should not collide with spheres of radius r S centered around given frames p j (the end-effector and wrist links of the UR10). This condition is expressed using the distance from the sphere center p j to the cylinder axis: p j -proj cyl. axis (p j ) r S + r C . Control-bounded LQR / primal-dual errors Fig. 6. Top: The pink area is a rectangular obstacle defined by [START_REF] Carpentier | Analytical Derivatives of Rigid Body Dynamics Algorithms[END_REF].

The trajectory avoids the area (at the discretization nodes) and the controls saturate: both constraints are satisfied. Bottom: Evolution of the primal-dual residuals after each step (backward and forward pass). We obtain very fast convergence in a handful of steps.

illustrates the motion on the UR10 robot, and Fig. 7 controls and velocities.

V. CONCLUSION

In this work, we have introduced a new approach for solving generic NLPs with equality and inequality constraints. We propose combining the BCL globalization strategy [START_REF] Conn | A Globally Convergent Augmented Lagrangian Algorithm for Optimization with General Constraints and Simple Bounds[END_REF] with the minimization of a relaxed semi-smooth primal-dual Augmented Lagrangian function inspired by [START_REF] Gill | A primal-dual augmented Lagrangian[END_REF]. We then apply this approach to extend the framework of equalityconstrained [START_REF] Kazdadi | Equality Constrained Differential Dynamic Programming[END_REF] and dynamics-implicit [START_REF] Jallet | Implicit Differential Dynamic Programming[END_REF] DDP to the case of inequality constraints. It results in an overall second-order quasi-Newton-like algorithm for solving constrained DDP problems. We finally highlight the numerical efficiency of our method on various sets of standard case-studies from the robotic literature. These contributions pave the way towards more advanced numerical methods for dealing with complex optimization problems in robotics, with the ambition of significantly reducing the computational burden, increase the numerical robustness of the trajectory optimization methods while also lowering the need of manually tuning underlying hyper-parameters. As future work, we plan to implement our contributions in C++ within the Crocoddyl library [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF], to properly account for equality and inequality constraints in trajectory optimization.

Fig. 1 .

 1 Fig.1. Left. Throwing motion on UR10. The ball is the yellow, then green sphere. Right. UR10 reach task. The yellow spheres around the end-effector and wrist links do not collide with the purple cylinders, and the waypoints are reached at the specified times.

Fig. 2 .

 2 Fig.2. Rotational system: A approximates a continuous-time system with matrix Ac = 0 2 -2 0 , c = (0.3, -0.2), control bound ū = 0.4. In spite of the control bounds, which saturate, the target is reached.

1 Fig. 4 .

 14 Fig. 4. Solution of the car parking task. The starting state is (1, 1, 3π2 , 0). The turn control u 0 = ω often saturates, causing the policy to go backwards to turn further; due to the parametrization with angle θ, a lot more turning is required.

Fig. 5 .

 5 Fig. 5. Joint angles (upper left), joint velocities (middle left), controls (lower left), end-effector trajectory (upper right) and velocity (lower right) for the throwing motion on UR10. The ball trajectory is displayed with dashed green lines on the upper-right plots. Both the velocities, controls and end-effector position satisfy their respective bounds, displayed in red.The minimum target end-effector velocity is also satisfied.

Fig. 7 .

 7 Fig. 7. Controls and velocities for the UR10 reach task. Translucent red lines indicate control and velocity bounds. The velocities saturate only for a few axes in the middle and end of the trajectory (the arm bows down to avoid the obstacles, and lurches forward to reach the final waypoint).

Some authors associate a penalty parameter to each constraint. In this case, the penalty parameters µe is a matrix Σ λ[START_REF] Hermans | QPALM: A Newton-type Proximal Augmented Lagrangian Method for Quadratic Programs[END_REF],[START_REF]QPALM: A Proximal Augmented Lagrangian Method for Nonconvex Quadratic Programs[END_REF].