A Direct-Indirect Hybridization Approach toControl-Limited DDP

Carlos Mastalli∗ Josep Marti-Saumell† Wolfgang Merkt∗

Joan Sola† Nicolas Mansard‡ Sethu Vijayakumar∗∗School of Informatics, University of Edinburgh, Edinburgh, UK

†Institut de Robotica i Informatica Industrial, Universitat Politecnica de Catalunya, Barcelona, Spain‡Gepetto Team, LAAS-CNRS, Toulouse, France

Email: carlos.mastalli@ed.ac.uk.

Abstract—Optimal control is a widely used tool for syn-thesizing motions and controls for user-defined tasks underphysical constraints. A common approach is to formulate itusing direct multiple-shooting and then to use off-the-shelfnonlinear programming solvers that can easily handle arbitraryconstraints on the controls and states. However, these methodsare not fast enough for many robotics applications such as real-time humanoid motor control. Exploiting the sparse structureof optimal control problem, such as in Differential DynamicProgramming (DDP), has proven to significantly boost thecomputational efficiency, and recent works have been focusedon handling arbitrary constraints. Despite that, DDP has beenassociated with poor numerical convergence, particularly whenconsidering long time horizons. One of the main reasons is dueto system instabilities and poor warm-starting (only controls).This paper presents control-limited Feasibility-driven DDP (Box-FDDP), a solver that incorporates a direct-indirect hybridizationof the control-limited DDP algorithm. Concretely, the forwardand backward passes handle feasibility and control limits. Weshowcase the impact and importance of our method on a set ofchallenging optimal control problems against the Box-DDP andsquashing-function approach.

I. INTRODUCTION

A. Motivation

Optimal control is a powerful tool to synthesize motions andcontrols through task goals (cost / optimality) and constraints(e.g., system dynamics, interaction constraints, etc.). We canformulate it through direct methods [5], which discretize overboth state and controls as optimization variables, and then usegeneral-purpose Nonlinear Programming (NLP) solvers suchas SNOPT [15], KNITRO [7], and IPOPT [29]. However, themain disadvantage of this approach is that it requires verylarge matrix factorizations, which limits its application domainto control on reduced models (e.g. [30, 25, 10]) or motionplanning (e.g. [9, 20, 1, 31]).

Recent results on fast nonlinear Model Predictive Control(nMPC) based on DDP (e.g. [26, 18, 23, 12]) have onceagain attracted attention to indirect methods which only dis-cretize over the controls, in particular with its Gauss-Newton(GN) approximation called iterative Linear-Quadratic Regula-tor (iLQR) [19]. These methods impose and exploit a sparsestructure of the problem by applying “Bellman’s principle ofoptimality” and successively solving the smaller sub-problems.This leads to fast and cheap computation due to very small

Fig. 1. Box-FDDP: a solver that incorporates a direct-indirect hybridizationof the control-limited DDP algorithm. Challenging maneuvers computed byBox-FDDP: monkey bar and jumping tasks. The lines describe the trajectoryperformed by the hands and the feet.

matrix factorizations and effective data cache accesses. On thecontrary, the main limitation compared with direct methods isthe ability to efficiently encode hard constraints.

B. Related work

There are important recent achievements in both direct-indirect hybridization [14, 21] and handling input limits [27],as well as nonlinear constraints [32, 17], that are rooted indynamic programming. For instance, Giftthaler et al. [14]introduced a lifted1 version of the Riccati equations that allowsus to warm-start both state and control trajectories. In turn,Mastalli et al. [21] proposed a modification of the forwardpass that numerically matches the gap contraction expectedby a direct multiple-shooting method with only equality con-straints. Unfortunately, both methods do not handle inequalityconstraints such as control limits. However, their hybrid ap-proach makes these works numerically more robust to poorinitialization as well as initialization from state trajectories.With hybrid approach, we refer to artificially include defect

1This name is coined by [2], and we refer to gaps or defects producedbetween multiple shooting nodes.

constraints (i.e. gaps on the dynamics) and the state trajectoryas decision variables.

Tassa et al. [27] focused on handling control limits duringthe computation of the backward pass in a DDP-like method(Box-DDP). They proposed to include control bounds duringthe minimization of the control Hamiltonian (i.e. the Q terms).Later, Xie et al. [32] included general inequality constraintsinto Tassa’s method (CDDP). Despite that, Xie’s method sacri-fices the computational effort by including a second QuadraticProgramming (QP) program in the forward pass, even thoughit remains faster than solving the same problem using a directformulation (with SNOPT). To reduce the computational cost,Howell et al. [17] formulate the inequality constraints usingan Augmented Lagrangian method (ALTRO), which also usesan active-set projection for solution polishing. In this work,we bridge the gap between direct-indirect hybridization withcontrol-limits inequalities. Other inequalities, e.g. on the state,are handled through quadratic penalization as explained below.

C. Contribution

In this paper, we propose enhancements to the Box-DDPalgorithm [27]. These modifications make our method morerobust (a) to face the feasibility problem, and (b) to discovergood solutions despite poor initial guesses. Our algorithm iscalled control-limited Feasibility-driven Differential DynamicProgramming (FDDP) (in short Box-FDDP). It comprises twomodes: feasibility-driven and control-bounded modes. Box-FDDP combines a hybridization of the multiple-shooting withDDP to leverage feasibility under control limits. In our results,we highlight the impact and importance of these changesover a wide range of different optimal control problems: fromdouble pendulum to humanoid locomotion (Fig. 1).

The rest of the paper is organized as follows. In Section II,we quickly describe the optimal control formulation and thenrecall the DDP algorithm with control limits. Section IIIdescribes our algorithm called Box-FDDP. Results that supportthe impact and importance of our proposed changes areprovided for several optimal control problems in Section IV,and Section V summarizes the work conclusions.

II. PRELIMINARIES

In this section, we first introduce our formulation of themulti-phase optimal control problem (Section II-A). We useholonomic constraints, similarly to [16, 6], together withfriction-cone inequalities to model contacts. Our main dif-ference is that we efficiently compute the derivatives of thecontact forces as explained in Section II-A1. Later, we give ashort introduction to the Box-DDP algorithm (Section II-B),necessary for our proposed algorithm called Box-FDDP; for acomplete description see [22, 26].

A. Optimal control formulation

Given a predefined contact sequence and timings, we formu-late a hybrid optimal control problem that exploits the sparsity

of the contact dynamics and its derivatives2:

minxs,us

lN (xN ) +

N−1∑k=0

∫ tk+∆tk

tk

lk(xk,uk,λk)dt

s.t. qk+1 = qk ⊕∫ tk+∆tk

tk

vk dt, (integrator)

vk+1 = vk +

∫ tk+∆tk

tk

vk dt,[vk−λk

]=

[M J>cJc 0

]−1 [τ b−a0

], (contact dynamics)

RλC(k) ≤ r, (friction-cone)

log (pG(k)(qk)−1oMfG(k)) = 0, (contact placement)

x ≤ xk ≤ x, (state bounds)u ≤ uk ≤ u, (control bounds)

(1)where the state x = (q,v) ∈ X lies in a differential manifold(with nx dimension) formed by the configuration point q andits tangent vector v, the control u ∈ Rnu is formed by theinput torque commands, C(k) and G(k) define respectivelythe set of active and gain contacts given the time index k,M ∈ Rnx×nx is the joint-space inertia matrix, Jc is the contactJacobian, λ ∈ Rnf is the contact force, a0 ∈ Rnf is thedesired acceleration in the constraint space, τ b is the force-bias vector, and (R, r) describe the linearized friction cone.Note that we could alternatively use impulse dynamics duringthe contact transition phases as described in [21].

Regarding the cost functions lk(·), we use a naıve refer-ence of the center of mass and swing motions to drive thesolution towards a desired behavior. To improve numericalstability, especially important for redundant tasks, we applyregularization on the contact dynamics, centroidal momentum,state, and control. For efficiency purposes, we use the GNapproximation to compute the Hessians of the cost function(e.g. lxx = lx

T lx).The contact dynamics [28] are defined by (a) the stack

of contact Jacobian expressed in the local frame Jc (full-rank matrix) and defined by C(k) indices, (b) the desiredacceleration in the constraint space a0 ∈ Rnf (with Baumgartestabilization [3]), (c) the joint-space inertia matrix M ∈Rnx×nx , and (d) the force-bias vector τ b = Su− b ∈ Rnx

that accounts for control u, the Coriolis and gravitational effectb, and S selects the actuated joint coordinates. We regularizethe Schur complement during the LDU decomposition neededfor the matrix inversion of the contact dynamics. Additionally,during the contact-gain phases [13], we substitute the contactdynamics by impulses of the form:[

M J>cJc 0

] [v+

−Λ

]=

[Mv−

−eJcv−], (2)

where Λ is the contact impulse and, v− and v+ are the dis-continuous changes in the generalized velocity (i.e., velocity

2We exploit the sparse structure of M and Jc during the matrix-matrixmultiplications needed for the factorization.

before and after impact, respectively), and e ∈ [0, 1] is therestitution coefficient that considers compression / expansion.

The friction-cone, contact placement, and state-bounds con-straints are enforced through quadratic penalization, whichin practice has good numerical convergence compared withthe log-barrier method. On the contrary, control bounds arehandled as explained in Section III. The linearized friction-cone model (R, r) is computed from the surface normal n,friction coefficient µ, and minimum / maximum normal forces(λ, λ). As the contact placement constraint lies on a SE(3),we explicitly express it in its tangent space using the log(·)operator.

1) Contact forces and their derivatives: As we expresscontact forces as accelerations in the contact constraint sub-space, we analytically compute the derivatives of the contact-forces for imposing the friction-cone constraints. If we applythe chain rule, the first-order Taylor approximation of thecontact forces is defined as:

δλ =

λx︷ ︸︸ ︷[∂λ∂τ

∂λ∂a0

] [ ∂τ∂x∂a0

∂x

]δx +

λu︷ ︸︸ ︷[∂λ∂τ

∂λ∂a0

] [ ∂τ∂u∂a0

∂u

]δu, (3)

where ∂λ∂τ = −M−1JcM

−1 and ∂λ∂a0

= −M−1 are thecontact-force Jacobians with respect to inverse dynamics andconstrained-acceleration kinematics functions, respectively,∂τ∂x , ∂τ

∂u are the derivatives of Recursive Newton-Euler Algo-rithm (RNEA), and ∂a0

∂x , ∂a0

∂u are the kinematics derivatives ofthe frame acceleration computed as in [8]. A similar procedureis used to compute the derivatives of the impulse dynamicsdescribed in Eq. (2).

B. Differential dynamic programming with control-limits

As proposed by [26], the control-limited DDP locally ap-proximates the optimal flow (i.e. the Value function) as

Vk(δxk) = minδuk

lk(δxk, δuk) + Vk+1(fk(δxk, δuk)),

s.t. u ≤ uk + δuk ≤ u ,(4)

which breaks the constrained Optimal Control (OC) probleminto a sequence of simpler sub-problems; u, u are the lowerand upper bounds of the control, respectively. Then, a localsearch direction is computed through a Linear Quadratic (LQ)approximation of the Value function:

δu∗k(δxk) = (5)

arg minδuk

H(δxk,δuk,Vk,k)︷ ︸︸ ︷1

2

1δxkδuk

T 0 QTxk

QTuk

QxkQxxk

Qxuk

QukQT

xukQuuk

1δxkδuk

,s.t. u ≤ uk + δuk ≤ u, (6)

where the Qk terms represent the LQ approximation of theHamiltonian function H(·), and the derivatives of Value func-tion Vk = (Vxk

, Vxxk) take the role of the costate variables.

From the derivatives of the cost and dynamics functionslk(·), fk(·), the Hamiltonian is calculated around a guess

(xik,uik) at each i-th iteration [22]. Solving Eq. (5) provides

the feed-forward term kk and the feedback gain Kk at eachdiscretization point k.

1) Control-bounded direction: Due to the mathematicalsimplicity of the control bounds, a subspace minimizationapproach allows the active set to change rapidly [24]. Thesubspace is defined by the search direction projected onto thefeasible box. To adopt this strategy into DDP algorithm, Tassaet al. [27] proposed to break the problem into feed-forwardand feedback sub-problems, where the feed-forward problemis defined as

kk = arg minδuk

1

2δuTkQuuk

δuk + QTukδuk,

s.t. u ≤ uk + δuk ≤ u,(7)

and it is solved by iteratively identifying the active set andthen moving along the free subspace of the Newton step(i.e. Projected-Newton QP [4]). Instead, the feedback gain iscomputed along the free subspace of the Hessian, i.e.

Kk = −Q−1uu,fk

Quxk, (8)

where Q−1uu,fk

is the control Hessian of the free subspace,and it is computed from feed-forward sub-problem. Note thatthe Box-QP computes the Newton direction along the freesubspace.

With this feedback gain, the changes in the nominal tra-jectory are projected onto the feasible box. Additionally, theBox-QP algorithm requires a feasible warm-start δu0

k, and if ithas the same active set, then the solution is calculated withina single iteration3.

III. CONTROL-LIMITED FDDP

The Box-FDDP comprises two modes: feasibility-drivenand control-bounded modes, that might be chosen in a giveniteration (Algorithm 1). The feasibility-driven mode uses aDDP hybridization of the multiple-shooting formulation tocompute the search direction and step length (lines 7 and 15).Instead, the control-bounded mode projects the search direc-tion onto the feasible control region whenever the dynam-ics constraint is feasible (line 10). Additionally, the appliedcontrol is always projected onto its feasible box (line 13),causing dynamic-infeasible iterations to reach the control box.Technical descriptions of both modes are elaborated in Sec-tions III-A and III-B.

A. Search direction of Box-FDDP

In direct multiple-shooting, the nonlinearities of the dy-namics are distributed over the entire horizon, instead ofbeing accumulated as in single shooting [11]. In DDP, thefeedback gain helps to distribute the dynamics nonlinearitiesas well. However, it does not resemble the Hamilton-Jacobi-Bellman (HJB) equation applied to a direct multiple-shootingformulation as described below.

3The computational cost of a single iteration is similar to performing aCholesky decomposition.

Algorithm 1: Control-limited FDDP (Box-FDDP)

1 compute LQ approximation of the cost and dynamics2 if infeasible iterate then3 compute the gaps, Eq. (9)

4 for k ← N − 1 to 0 do5 update the feasibility-driven Hamiltonian, Eq. (12)6 if infeasible iterate then7 compute feasibility-driven direction, Eq. (14)8 else9 clamp Box-QP warm-start, Eq. (15)

10 compute control-bounded direction, Eq. (7)-(8)

11 for α ∈{

1, 12 , · · · ,

12n

}do

12 for k ← 0 to N do13 project control onto the feasible box, Eq. (16)14 if infeasible iterate or α 6= 1 then15 update the gaps, Eq. (17)16 else17 close the gaps,

fk = 0 ∀k ∈ {0, · · · , N − 1}18 perform step, Eq. (19)

19 compute the expected improvement, Eq. (20)20 if success step then21 break

1) Computing the gaps: Given a current iterate (xs,us),we compute the gaps by performing a nonlinear rollout, i.e.

fk+1 = f(xk,uk)− xk+1, (9)

where f(xk,uk) is the rollout state at interval k+1, and xk+1

is the next shooting state.In the standard Box-DDP, an initial forward pass is per-

formed in order to close the gaps. Instead, our Box-FDDPcomputes the gaps once at each iteration (line 3), and thenuses them to find the search direction (Section III-A3) and tocompute the expected improvement (Section III-B4).

2) Hamiltonian of direct multiple-shooting formulation:Without loss of generality, we use the GN approximation [19]to write the Hamiltonian function as

H(·) =1

2

[1

δxk+1

]T [0 V Txk+1

Vxk+1Vxxk+1

] [1

δxk+1

]

+1

2

1δxkδuk

T 0 lTxklTuk

lxklxxk

lxuk

luklTxuk

luuk

1δxkδuk

, (10)

where lx, lu and lxx, lxu, luu are the gradient and Hessian ofthe cost function, respectively, δxk+1 = fxk

δxk + fukδuk is

the linearized dynamics, and fx, fu are its Jacobians. However,in a direct multiple-shooting setting, we have a drift in thelinearized dynamics

δxk+1 = fxkδxk + fukδuk + fk+1 (11)

due to the gaps fk+1 produced between multiple-shoots. Then,according to the Pontryagin’s Maximum Principle (PMP), theRiccati recursion needs to be adapted as follows:

Qxk= lxk

+ fTxkV +xk+1

,

Quk= luk

+ fTukV +xk+1

,

Qxxk= lxxk

+ fTxkVxxk+1

fxk, (12)

Qxuk= lxuk

+ fTxkVxxk+1

fuk,

Quuk= luuk

+ fTukVxxk+1

fuk,

where

V +xk+1

= Vxk+1+ Vxxk+1

fk+1 (13)

is the Jacobian of the Value function after the deflectionproduced by fk+1, and the Hessian of the Value functionremains unchanged. Indeed, this is possible since DDP ap-proximates the Value function to a LQ model. Note that asimilar derivation is proposed by [14].

3) Feasibility-driven direction: During dynamic-infeasibleiterates (line 7), we compute a control-free direction4:

kk = −Q−1uuk

Quk,

Kk = −Q−1uuk

Quxk.

(14)

The reason is due to the fact that we cannot quantify the effectof the gaps on the control bounds, which are needed to solvethe feed-forward sub-problem Eq. (7). Note that our approachis equivalent to opening the control bounds during dynamic-infeasible iterates.

4) Control-bounded direction: We warm-start the Box-QPusing the feed-forward term kk computed in the previousiteration. However, in case of a previous infeasible iteration,kk might fall outside the feasible box (i.e. u − δuk ≤ kk ≤u − δuk). This is in contrast to the standard Box-DDP, inwhich a feasible warm-start u0

s needs to be provided, and then,the iterates always remain feasible.

To handle infeasible iterates, we propose to clamp the warm-start of the Box-QP (line 9) as

JkkKu−δuk= min (max (kk, u− δuk),u− δuk), (15)

where u− δuk and u− δuk are the upper and lower boundsof the feed-forward sub-problem, Eq. (7), respectively.

5) Regularization: Each time that the computation of thefeed-forward sub-problem fails, Eq. (7), we increment theregularization over Quu and re-start the computation of thedirection. On the other hand, each time that the algorithmaccepts a big step5, then we decrease the regularization. Withthis, we provide major robustness to the algorithm since itmoves from Newton direction to steepest-descent, or viceversa.

4In this work, with control-free direction, we also refer to feasibility-drivendirection, i.e., the direction ignoring the control constraints.

5Steps with α ≥ α0, where α0 is an user-defined threshold.

B. Step-length of Box-FDDP

As far as we know, Tassa et al. [26] proposed only to modifythe search direction using a Box-QP. However, it is importantto pay attention to the rollout as well, during which we find astep length that minimizes the cost [24]. A similar motivationcan be found in methods such as [32, 17], where a standardline-search procedure is used around a local model.

1) Projecting the rollout towards the feasible box: Wepropose to project the control onto the feasible box in thenonlinear rollout (line 13), i.e.

uk ← min (max (uk, u),u), (16)

where uk is the control policy computed from the searchdirection; for more details see Section III-B3. Our method doesnot require to solve another QP problem [32] or to project thelinear search direction given the gaps on the dynamics [17].

2) Updating the gaps: In related work [21] is analyzed thebehavior of the gaps during the numerical optimization, i.e. byiteratively solving the Karush-Kuhn-Tucker (KKT) problem ofa direct multiple-shooting algorithm. Their conclusion is thatthe gaps will be either partially closed by a factor of

fk ← (1− α)fk, (17)

where α is the accepted step-length found by the line-searchprocedure (line 11-21), or completely closed in case of a full-step (α = 1).

3) Nonlinear step: With a nonlinear rollout6 (line 18),we avoid the linear prediction error of the dynamics that istypically handled by a merit function in off-the-shelf NLPsolvers. Therefore, the prediction of gaps after applying anα-step are:

f i+1k+1 = f ik+1 − α(δxk+1 − fxkδxk − fukδuk)

= (1− α)(f(xk,uk)− xk+1), (18)

and if we keep the gap-contraction rate of Eq. (17), then weobtain

xk = f(xk−1, uk−1)− (1− α)fk−1,

uk = uk + αkk + Kk(xk − xk), (19)

where kk and Kk are the feed-forward term and feedbackgains computed by Eq. (14) or Eq. (7)-(8), and the initialcondition of the rollout is defined as x0 = x0−(1−α)f0. Thisis in contrast to the standard Box-DDP, in which the gaps arealways closed.

4) Expected improvement: It is critical to properly evaluatethe success of a trial step. Given the current gaps on thedynamics fk, the Box-FDDP computes the expected improve-ment of a computed search direction as

∆J(α) = ∆1α+1

2∆2α

2, (20)

6In this work, rollout is sometimes referred as nonlinear step.

with

∆1 =

N∑k=0

k>k Quk+ f>k (Vxk

− Vxxkxk),

∆2 =

N∑k=0

k>k Quukkk + f>k (2Vxxk

xk − Vxxkfk). (21)

Note that J is the total of cost of a given state-controltrajectory (xs, us).

We obtain this expression by computing the cost froma linear rollout of the current control policy as describedin Eq. (19). Finally, we also accept ascend directions sincewe use the Goldstein condition to check for the trial step.

IV. RESULTS

Box-FDDP outperforms Box-DDP on a wide number ofoptimal control problems which are described in Section IV-A.In Section IV-B, we provide a comparison that shows theadvantages of the proposed modifications. Later, we analyzethe gap contraction and how it is connected with the dynamicnonlinearities (Section IV-B2). Finally, we show that the earlycontrol saturation of the Box-DDP has disadvantages (Sec-tion IV-C).

A. Optimal control problems

We compare the performance of our solver against Box-DDP [27] for a range of different OC problems: an under-actuated double pendulum, a quadcopter navigating througha narrow passage and looping, various gaits in legged loco-motion, aggressive jumps and unstable hopping, and whole-body manipulation and balance. All the studied cases highlightthe benefits of our proposed method: Box-FDDP. To make afair comparison, we use the same initial regularization value(10−9) and stopping criteria. Fig. 2 shows snapshots of motioncomputed for some of these problems, for more details see theaccompanying video7.

1) Double pendulum (pend): The goal is to swing from thestable to the unstable equilibrium points, i.e. from down-wardto up-ward positions, respectively. To increase the problemcomplexity, the double pendulum (with weight of ≈ 4.5 N)has a single actuated joint with small range of control (from−5 to 5 N, largely insufficient for a static execution of thetrajectory). The time horizon is 1 s with 100 nodes.

2) Quadcopter: We consider three tasks for the IRIS quad-copter: reaching goal (quad), looping maneuver (loop), andtraversing a narrow passage (narrow). We use different way-points to describe the task, where each way-point specifiesthe desired pose and velocity. The way-points are describedthrough cost functions in the robot placement and velocity.The vehicle pose is described as SE(3) element, which allowsus to consider any kind of motion such as looping maneuvers.Control inputs are considered to be the thrust produced by thepropellers, which can vary within a range from 0.1 to 10.3 Neach. The solution is computed from a cold-start of the solver.

7https://www.dropbox.com/s/oyrfqijbrajgj0i

(a)

(b)

(c)

(d)

(e)

Fig. 2. Snapshots of generated robot maneuvers using Box-FDDP. (a) traversing a narrow passage with a quadcopter (quad); (b) aggressive jumping of30 cm that reaches ANYmal limits (jump); (c) Talos balancing on a single leg (taichi); (d) ANYmal hopping with two legs (hop); (e) Talos climbing in amonkey bar.

3) Aggressive jump, unstable hopping, and various gaits:We use the ANYmal quadruped robot to generate a wide rangeof motions — jumping, hopping, walking, trotting, pacing,and bounding. We deliberately reduce the torque and velocitylimits to 32 N m and 7.5 rad/s, respectively. The joint velocitylimits make it particularly hard to solve the jumping task(jump). Finally, the unstable hopping task (hop) is describedwith a long horizon: 5.8 s with 580 nodes. It includes 10 hopsin total with a phase that switches the legs. We warm-start thesolver with the default posture and quasi-static torques8.

4) Whole-body manipulation and balance: We considerthree problems for the Talos humanoid robot: whole-bodymanipulation (man), hand control while balancing in singleleg (taichi), and a monkey bar task (bar). For the monkeybar task, we increase by 10 times the joint torque limits ofthe arms9. Additionally, we consider joint position limits in

8We use the reference posture with zero velocity to compute the quasi-statictorques, i.e. the torques due to the effect of gravity.

9Talos’ arms are not strong enough to support its own weight.

each scenario. Both taichi and monkey bar tasks are dividedin three phases; for the taichi task: manipulation, standing onone foot, and balancing; for the monkey bar task: grasping thebar, climbing up, and landing on ground. Note that we do notinclude friction cone constraints in the grasping bar phase.

B. Advantages of the feasibility mode

To understand the benefits of the feasibility-mode, we ana-lyze the resulting total cost and number of iterations for both:Box-FDDP and Box-DDP. As shown in Fig. 3, Box-FDDP’ssolutions (*-feas) have lower total cost and are computed withfewer iterations when compared to Box-DDP. We summarizethe results of each benchmark problem in Table I.

1) Greater globalization strategy: The feasibility-drivenmode becomes crucial to solve the double pendulum (pend),monkey bar task (bar), aggressive jumping (jump), and unsta-ble hopping (hop) problems, in which the Box-DDP fails tofind a solution (Table I). This mode helps to find a feasiblesequence of controls despite the poor initialization warm-start.Indeed, infeasible iterations can be seen as a globalization

10 17

10 14

10 11

10 8

10 5

10 2pendpend-feasquadquad-feasmanman-feas

100 101 102

iterations (log-scale)

10 13

10 11

10 9

10 7

10 5

10 3

10 1

taichitaichi-feasjumpjump-feashophop-feas

norm

alize

d co

st (l

og-s

cale

)

Fig. 3. Cost and convergence comparison for different optimal control prob-lems. The Box-FDDP outperforms the Box-DDP in all the cases: (top) doublependulum (pend), quadcopter navigation (quad), and whole-body manipula-tion (man); and (bottom) whole-body balance (taichi), quadrupedal jumping(jump), quadrupedal hopping (hop). Box-FDDP (*-feas) solves the problemwith fewer iterations and lower cost than Box-DDP. Furthermore, Box-DDP fails to solve the hardest problems: i.e. double pendulum, quadrupedaljumping, and hopping. Our algorithm shows better globalization strategy, i.e.it is less sensitive to poor initialization compared with Box-DDP.

TABLE INUMBER OF ITERATIONS, TOTAL COST, AND SOLUTION SUCCESS RATE.

Box-DDP Box-FDDP (feas)

Problems Iter. Cost Sol. Iter. Cost Sol.

pend 424 0.223 7 31 0.0273 3quad-goal 23 0.0764 3 18 0.0072 3quad-loop 133 6.7211 3 56 0.6444 3quad-narrow 70 1.9492 3 35 0.4577 3man 88 4.6193 3 80 4.6193 3taichi 148 6.8184 3 141 6.8184 3jump 646 7.21× 104 7 454 1.81× 104 3hop 18 1.13× 106 7 205 2.23× 104 3bar 27 927.7 7 358 23.316 3

3 solver finds a solution, 7 solver does not find a solution.

strategy that ensure convergence from remote initial pointsas they balance objective and feasibility. We encountered thatthis trade-off could also improve the solution. For instance,early clamping of control commands (produced by Box-DDP)generates unnecessary loops during the quadcopter navigation.

2) Gap contraction and nonlinearities: Fig. 4 shows thegap contraction for each benchmark problem. We observethat the gap contraction rate is highly influenced by thenonlinearities of the system’s dynamics. When compared tothe dynamics, the nonlinearities of the task have a smallereffect (e.g. jump vs hop). Note that the gap contraction speedfollows the order: humanoid, quadruped, double pendulum,and quadcopter.

Propagation errors due to the dynamics linearization have an

100 101 102

iterations (log-scale)

0.0

0.2

0.4

0.6

0.8

1.0

norm

alize

d ga

ps L

2-no

rm

pendquadmantaichijumphop

Fig. 4. Gap contraction of Box-FDDP for different optimal control problems.For all the cases, the gaps are open for the first several iterations. Thegap contraction rate varies according to the accepted step-length. Smallercontraction rates, during the first iterations, appear in very nonlinear problems(taichi, man, hop, and jump), because of the larger error of the search direction.

30

20

10

0

10

20

30

join

t tor

que

(Nm

)

HAAHFEKFE

0.0 0.2 0.4 0.6 0.8 1.0time (s)

7.5

5.0

2.5

0.0

2.5

5.0

7.5

join

t vel

ocity

(rad

/s)

HAAHFEKFE

Fig. 5. Joint torques and velocity for the ANYmal jumping maneuver. (top)Generated torques of the LF joints and its limits (32 Nm); (bottom) Generatedvelocities of the LF joint and its limits (7.5 rad/s). The red region describesthe flight phase. Note that HAA, HFE, and KFE are the abduction/adduction,hip flexion/extension and knee flexion/extension joints, respectively.

important influence on the algorithm progress, mainly becauseDDP-based methods maintain a local quadratic approximationof the Value function. In other words, the prediction of theexpected improvement is more accurate for systems with lessnonlinearities and, as a result, the algorithm tends to acceptbigger steps that result in higher gap reductions.

While the gaps are open, our algorithm is in feasibility-driven mode. During this phase, the cost reduction is smallerthan in the control-bounded mode, in particular for verynonlinear systems (see Fig. 3 and 4). However, once thegaps are closed, a higher cost reductions often appear in verynonlinear systems.

3) Highly-dynamic maneuvers: Our algorithm can solve awide range of motions: from unstable and consecutive hops

to aggressive and constrained jumps. In Fig. 5, we show thejoint torques and velocities of a single leg for the ANYmal’sjumping task (depicted in Fig. 2-b). The motion consists ofthree phases: jumping (0 - 300 ms), flying (300 - 700 ms),and landing (700 - 1000 ms). We reduced the real joint limitsof the ANYmal robot: from 40 to 32 N m (torque limits)and from 15 to 7.5 rad/s (velocity limits). Thus, generating a30 cm jump becomes a very challenging task. The constraintviolations on the state limits appear due to the fact that we usequadratic penalization to enforce them. Nonetheless, we onlyencountered these violations in very constrained problems.

For the walking, trotting, pacing, and bounding gaits (re-porting in the accompanying video), the Box-FDDP convergesapproximately with the same number of iterations achieved bythe DDP solver (i.e. unconstrained case).

C. Box-FDDP, Box-DDP, and squashing approach in nonlin-ear problems

We compare the performance of Box-FDDP, Box-DDPand DDP with a squashing function for three scenarios withthe IRIS quadcopter: reaching goal (goal), looping maneuver(loop), and traversing a narrow passage (narrow). We use asigmoidal element-wise squashing function of the form:

si(ui) =1

2

(ui +

√β2 + (ui − ui)2

)+

1

2

(ui −

√β2 + (ui − ui)2

),

in which the sigmoid is approximated through two smooth-abs functions, β defines its smoothness, and ui, ui are theelement-wise lower and upper control bounds, respectively.We introduce this squashing function on the system controlsas: xk+1 = f(xk, s(uk)). We use β = 2 for all the experimentspresented in this work.

Fig. 6 shows that Box-FDDP converges faster than theother approaches. Furthermore, the motions computed by Box-FDDP are more intuitive and with the lowest cost as reportedin Table I and in accompanying video. We also observe thatthe squashing approach converges sooner compared to Box-DDP for the looping task. The main reason is due to the earlysaturation of the controls performed by Box-DDP.

In Fig. 7, we show the cost evolution for 10 different initialconditions of the reaching goal task10. Infeasible iterations, inBox-FDDP, produce a very low cost in the first iterations. Thesquashing approach is the most sensitive to initial conditions.However, on average, it produces slightly better solutions thanBox-DDP. This is in contrast to the reported results in [27],where the performance was analyzed only for LQ optimalcontrol problem.

V. CONCLUSION

In this paper, we proposed a direct-indirect hybridization ofthe control-limited DDP algorithm (Box-FDDP). Our methoddiscovers good solutions despite poor initial guesses thanks

10Target and initial configurations are (3, 0, 1) and (-0.3 ± 0.6, 0, 0) m,respectively.

100 101 102

iterations (log-scale)

10 13

10 10

10 7

10 4

10 1

102

norm

alize

d co

st (l

og-s

cale

)

looploop-squashloop-feasnarrownarrow-squashnarrow-feas

Fig. 6. Cost and convergence comparison for different quadcopter maneuvers:looping (loop) and narrow passage traversing (narrow). Box-FDDP (*-feas)outperforms both Box-DDP and DDP with squashing function (*-squash).

100 10110 1

100

101

102

103

cost

(log

-sca

le)

Box-DDP

100 101

Squashing DDP (squash)

100 101

Box-FDDP (feas)

iterations (log-scale)Fig. 7. Costs associated for 10 different initial conditions of reaching goaltask. Box-FDDP converges earlier and with lower total cost than Box-DDPand DDP with squashing function. The performance of the squashing functionapproach exhibits a high dependency on the initial condition.

to infeasible iterations, which resembles a direct multiple-shooting approach. A vast range of optimal control problemsdemonstrate the benefits of the proposed method. Future workwill focus on general inequalities constraints and model pre-dictive control. Our implementation of Box-FDDP includingall examples will be available soon.

REFERENCES

[1] B. Aceituno-Cabezas, C. Mastalli, H. Dai, M. Focchi,A. Radulescu, D. G. Caldwell, J. Cappelletto, J. C.Grieco, G. Fernandez-Lopez, and C. Semini. Simulta-neous Contact, Gait and Motion Planning for RobustMulti-Legged Locomotion via Mixed-Integer ConvexOptimization. IEEE Robotics and Automation Letters(RA-L), 2017.

[2] J. Albersmeyer and M. Diehl. The Lifted NewtonMethod and Its Application in Optimization. SIAM J.on Optimization, 2010.

[3] J. Baumgarte. Stabilization of constraints and integralsof motion in dynamical systems. Computer Methods inApplied Mechanics and Engineering, 1972.

[4] D. P. Bertsekas. Projected newton methods for optimiza-tion problems with simple constraints. SIAM Journal onControl and Optimization, 1982.

[5] J. T. Betts. Practical Methods for Optimal Control andEstimation Using Nonlinear Programming. CambridgeUniversity Press, USA, 2nd edition, 2009.

[6] R. Budhiraja, J. Carpentier, C. Mastalli, and N. Mansard.Differential Dynamic Programming for Multi-PhaseRigid Contact Dynamics. In IEEE-RAS InternationalConference on Humanoid Robots (Humanoids), 2018.

[7] R. H. Byrd, J. Nocedal, and R. A. Waltz. KNITRO: Anintegrated package for nonlinear optimization. In LargeScale Nonlinear Optimization, 35–59, 2006, 2006.

[8] J. Carpentier and N. Mansard. Analytical Derivatives ofRigid Body Dynamics Algorithms. In Robotics: Scienceand Systems (RSS), 2018.

[9] J. Carpentier, S. Tonneau, M. Naveau, O. Stasse, andN. Mansard. A versatile and efficient pattern generatorfor generalized legged locomotion. In IEEE InternationalConference on Robotics and Automation (ICRA), 2016.

[10] J. Di Carlo, P. M. Wensing, B. Katz, G. Bledt, andS. Kim. Dynamic Locomotion in the MIT Cheetah 3Through Convex Model-Predictive Control. In IEEE/RSJInternational Conference on Intelligent Robots and Sys-tems (IROS), 2018.

[11] M. Diehl, H. G. Bock, H. Diedam, and P.-B. Wieber.Fast Direct Multiple Shooting Algorithms for OptimalRobot Control. In Fast Motions in Biomechanics andRobotics: Optimization and Feedback Control. SpringerBerlin Heidelberg, 2006.

[12] F. Farshidian, E. Jelavic, A. Satapathy, M. Giftthaler,and J. Buchli. Real-time motion planning of leggedrobots: A model predictive control approach. In IEEE-RAS International Conference on Humanoid Robotics(Humanoids), 2017.

[13] R. Featherstone. Rigid Body Dynamics Algorithms.Springer-Verlag, Berlin, Heidelberg, 2007.

[14] M. Giftthaler, M. Neunert, M. Stauble, J. Buchli, andM. Diehl. A Family of Iterative Gauss-Newton ShootingMethods for Nonlinear Optimal Control. In IEEE/RSJInternational Conference on Intelligent Robots and Sys-tems (IROS), 2018.

[15] P. E. Gill, W. Murray, and M. A. Saunders. SNOPT: AnSQP Algorithm for Large-Scale Constrained Optimiza-tion. SIAM Rev., 2005.

[16] A. Hereid and A. D. Ames. FROST*: Fast robotoptimization and simulation toolkit. In IEEE/RSJ Inter-national Conference on Intelligent Robots and Systems(IROS), 2017.

[17] T. A. Howell, B.E. Jackson, and Z. Manchester. ALTRO:A Fast Solver for Constrained Trajectory Optimization.In IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS), 2019.

[18] J. Koenemann, A. Del Prete, Y. Tassa, E. Todorov,O. Stasse, M. Bennewitz, and N. Mansard. Whole-body model-predictive control applied to the HRP-2humanoid. In IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS), 2015.

[19] W. Li and E. Todorov. Iterative Linear Quadratic Regula-tor Design for Nonlinear Biological Movement Systems.In ICINCO, 2004.

[20] C. Mastalli, M. Focchi, I. Havoutis, A. Radulescu,

S. Calinon, J. Buchli, D. G. Caldwell, and C. Sem-ini. Trajectory and Foothold Optimization using Low-Dimensional Models for Rough Terrain Locomotion. InIEEE International Conference on Robotics and Automa-tion (ICRA), 2017.

[21] C. Mastalli, R. Budhiraja, W. Merkt, G. Saurel, B. Ham-moud, M. Naveau, J. Carpentier, L. Righetti, S. Vijayaku-mar, and N. Mansard. Crocoddyl: An Efficient andVersatile Framework for Multi-Contact Optimal Control.In IEEE International Conference on Robotics and Au-tomation (ICRA), 2020.

[22] D. Mayne. A second-order gradient method for deter-mining optimal trajectories of non-linear discrete-timesystems. International Journal of Control, 1966.

[23] M. Neunert, M. Stauble, M. Giftthaler, C. D. Bellicoso,J. Carius, C. Gehring, M. Hutter, and J. Buchli. Whole-Body Nonlinear Model Predictive Control Through Con-tacts for Quadrupeds. IEEE Robotics and AutomationLetters (RA-L), 2018.

[24] J. Nocedal and S. J. Wright. Numerical Optimization.Springer, New York, NY, USA, second edition, 2006.

[25] D. Pardo, L. Moller, M. Neunert, A. W. Winkler, andJ. Buchli. Evaluating Direct Transcription and NonlinearOptimization Methods for Robot Motion Planning. IEEERobotics and Automation Letters (RA-L), 2016.

[26] Y. Tassa, T. Erez, and E. Todorov. Synthesis and stabi-lization of complex behaviors through online trajectoryoptimization. In IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS), 2012.

[27] Y. Tassa, N. Mansard, and E. Todorov. Control-LimitedDifferential Dynamic Programming. In IEEE Interna-tional Conference on Robotics and Automation (ICRA),2014.

[28] Firdaus Udwadia and Robert Kalaba. A New Perspectiveon Constrained Motion. Proceedings of the Royal SocietyA: Mathematical, Physical and Engineering Sciences,1992.

[29] A. Wachter and L. T. Biegler. On the implementationof an interior-point filter line-search algorithm for large-scale nonlinear programming. Mathematical Program-ming, 2006.

[30] P.-B. Wieber. Trajectory Free Linear Model PredictiveControl for Stable Walking in the Presence of StrongPerturbations. In IEEE-RAS International Conference onHumanoid Robots (Humanoids), 2006.

[31] A. W. Winkler, D. C Bellicoso, M. Hutter, and J. Buchli.Gait and Trajectory Optimization for Legged Sys-tems through Phase-based End-Effector Parameteriza-tion. IEEE Robotics and Automation Letters (RA-L),2018.

[32] Z. Xie, C. K. Liu, and K. Hauser. Differential dy-namic programming with nonlinear constraints. In IEEEInternational Conference on Robotics and Automation(ICRA), 2017.