<figure><img src="image_1.png" /></figure>

HAL
open science

# A Constrained Iterative LQR Solver for the Trajectory Optimization Framework Horizon

Arturo Laurenzi, Francesco Ruscelli, Nikos Tsagarakis

<figure><img src="image_2.png" /></figure>

## To cite this version:

Arturo Laurenzi, Francesco Ruscelli, Nikos Tsagarakis. A Constrained Iterative LQR Solver for the Trajectory Optimization Framework Horizon. Istituto Italiano di Tecnologia. 2025. hal-04999530

HAL Id: hal-04999530

https://hal.science/hal-04999530v1

Submitted on 20 Mar 2025

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_3.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# A Constrained Iterative LQR Solver for the Trajectory Optimization Framework Horizon

Arturo Laurenzi, Francesco Ruscelli, and Nikos G. Tsagarakis.

**Abstract**— This work introduces the implementation of the equality-constrained Iterative Linear-Quadratic Regulator (eILQR) solver used inside the recently published trajectory optimization (TO) framework for robotic application, Horizon. Differently from most constrained ILQR implementations that use an iterative penalty approach such as Augmented Lagrangian, we directly apply Newton's method to the KKT equations of the TO problem at hand, and show how to compute Newton steps with linear complexity w.r.t. the horizon length. Our approach generates estimates of Lagrangian multipliers corresponding to all constraints, allowing to use an exact $ℓ_1$ merit function inside a line-search based globalization strategy. We conclude our work with an extensive validation campaign involving many complex robotic platforms, such as our wheeled-legged quadruped robot CENTAURO.

## I. INTRODUCTION AND RELATED WORKS

Trajectory optimization (TO) has recently attracted the interest of the robotics community thanks to its ability to generate complex, whole-body coordinated motions from an intuitive, high-level task description specified in terms of costs and constraints, also resulting in a plurality of available software tools, such as Crocoddyl [1], OCS2 [2], Drake [3]. In this paper, we focus on the equality-constrained, discrete time formulation as follows,

$$ \min_{x_0:N, u_{0:N}-1} \sum_{k=0}^{N-1} \ell_k(x_k, u_k) + \ell_N(x_N) \quad (1a) $$

$$ \text{s.t.} \quad x_{k+1} = F(x_k, u_k) \quad (1b) $$

$$ h_k(x_k, u_k) = 0, \quad h_N(x_N) = 0; \quad (1c) $$

this can be easily obtained from a continuous-time description via, e.g., a multiple shooting (MS) method [4, Sec. 3.4], which notably results in a sparse NLP that can be warm-started with a dynamically-inconsistent initial guess. Indeed, while dense formulations scale poorly (i.e., with cubic complexity) with the horizon length, sparse formulations are able to exploit matrix structure through complex heuristic strategies [5], [6] that aim at preserving sparsity through matrix factorizations. Such algorithms are applicable to generic NLP problems, but are likely to offer sub-optimal performance for any specific problem at hand. Indeed, TO problems in the form (1), exhibit a special structure that is due to the so-called Markov property, i.e. (i) cost and constraints are separable w.r.t. time, and (ii) the state vector at a next time $k+1$ can be expressed as a sole function of the state and input at the present time $k$. This key property is classically exploited by the well-known Iterative Linear

<figure><img src="image_4.png" /><figcaption>Fig. 1. The Centauro [10] robot climbing off a 30 cm platform thanks to the proposed eILQR solver. Visualization of optimized trajectory (left) and real robot execution (right).</figcaption></figure>

Quadratic Regulator (ILQR) solver, which essentially applies Bellman's principle of optimality [7] to a linear-quadratic model of cost function and dynamics, in order to split the whole problem (1) into $N$ smaller sub-problems<fnref n="1" />, therefore achieving linear complexity w.r.t. the horizon length $N$. This procedure results in the well-known Riccati backward recursion, or backward pass. As during the backward pass the system's state is unknown, it must be treated as a free parameter; therefore, ILQR must solve a sequence of parametric sub-problems, each of which yields a linear policy, as opposed of a single numeric value; it has also been demonstrated [9] how the ILQR policy can depend on any parameter affecting the TO problem, such as a time-varying references that are typical of trajectory tracking scenarios. Because of its parametric nature, extensions of ILQR including constraints have proven to be more challenging to devise. For instance, [1], [11] inserts contact constraints expressed at the acceleration level inside the dynamics, while introducing additional tuning gains to avoid drifting. Other approaches such as [12], [13] approximate the active set heuristically to avoid dependence upon the unknown state. Projection-based approaches such as [2], [14], [15] can cope with equality path constraints (limited to unit relative degree<fnref n="2" /> in the case of [2], [14]), but fail to handle terminal constraints, therefore ruling out relevant applications, e.g. pick-and-place tasks, where the constraint dimension exceeds the number of available controls. Approaches based on Augmented Lagrangian have

Authors are with the Humanoid and Human-Centred Mechatronics (HHCM) Lab, Istituto Italiano di Tecnologia (IIT), Genova. Please send correspondence to {name.surname}@iit.it

<footnote n="1">¹A multiple-shooting variant of ILQR has been proposed in [8].</footnote>

<footnote n="2">²The relative degree of a constraint $h(x)$ is defined as its minimum time derivative order $r$ that explicitly depends on the input, i.e. $\frac{\partial}{\partial u} \frac{\partial h}{\partial r} \neq 0$.</footnote>
been proposed [16], [17], which however can suffer from
poor numerical conditioning due the required large values of
their penalty parameter, depending on tuning. Exact strate-
gies for general eILQR were proposed e.g. in [18], which
however includes a cubic complexity constraint handling
routine. Recently, the methods of [19], [20] proposed a
solution to eLQR that enjoys linear complexity w.r.t. N,
limiting however to quadratic costs and linear constraints,
and neglecting the computation of Lagrange multipliers.

In this work, we present the implementation of an eILQR
algorithm for our recently introduced TO framework, Hori-
zon [21]. Our main contributions are the following:

* We present a derivation of eILQR that does not rely on Bellman's principle of optimality, and instead applies Newton's method to the first-order necessary condition of optimality for (1).

* Thanks to the proposed derivation, we compute values of the Lagrange multipliers, and show that they prove to be useful for globalization purposes, as well as to incorporate second-order information.

* Finally, we validate our eILQR algorithm with many TO instances involving quadruped and humanoid robots, ranging from offline-computed dynamic maneuvers, to online receding-horizon optimization. To the best of the authors' knowledge, this work provides the first comprehensive validation of eILQR in the general case of non-quadratic cost and non-linear constraints.

The remainder of this work is organized into two main
parts: Section II goes through the mathematical details of
the proposed eILQR formulation, including a discussion on
potential sources of ill-conditioning, as well as on global-
ization issues that arise when dealing with non-linear, non-
convex TO problems; Section III discusses the software im-
plementation of our method, and presents validation results
that are very relevant to the field of legged robotics. Finally,
Section IV concludes the paper by summarizing the achieved
results, and proposing a few future research directions.

II. PROPOSED METHOD

First and foremost, let us define the transcribed TO prob-
lem (1) more formally. The cost to be optimized (1a) is a
sum of instantaneous costs $\ell_k : \mathbb{R}^n \times \mathbb{R}^m \to \mathbb{R}$, where $n$ is
the dimension of the state vector $x_k$, and $m$ is the dimension
of the control vector $u_k$. The terminal cost $\ell_N : \mathbb{R}^n \to \mathbb{R}$ is
defined as a pure function of the terminal state. We restrict
the TO problem to state trajectories $x_{0:N} = \{x_0, \dots, x_N\}$
and input trajectories $u_{0:N-1} = \{u_0, \dots, u_{N-1}\}$ that fulfill the
given discrete-time dynamics equation (1b). Furthermore,
we seek trajectories that fulfill the equality constraints (1c),
where $h_k : \mathbb{R}^n \times \mathbb{R}^m \to \mathbb{R}^{p_k}$, and $h_N : \mathbb{R}^n \to \mathbb{R}^{p_N}$. Finally, let
$X \in \mathbb{R}^{n \times}$ be the vector of all optimization variables, i.e. all
states and controls, $J(X) : \mathbb{R}^{n \times} \to \mathbb{R}$ the overall cost function,
and $C(X) : \mathbb{R}^{n \times} \to \mathbb{R}^{nC}$ the overall constraint vector of (1).

A. KKT conditions for the TO problem

Since the TO (1) is an ordinary equality-constrained NLP,
we can attempt to solve it by writing down the first-

order necessary condition for the optimality of a candidate
solution, i.e. the Karush-Kuhn-Tucker (KKT) conditions. To
this aim let $\lambda_k \in \mathbb{R}^n$ and $\mu_k \in \mathbb{R}^{p_k}$, for $k \in \{0, \dots, N\}$,
be the vectors of Lagrange multipliers corresponding to
the dynamics constraint (1b)$^3$, and equality constraint (1c),
respectively. The Lagrangian function for problem (1) is
therefore defined as

$$
\begin{align}
L(x_{0:N}, u_{0:N-1}, \lambda_{0:N-1}, \mu_{0:N}) = & \sum_{k=0}^{N-1} \ell_k(x_k, u_k) + \mu_k^T h_k(x_k, u_k) + \lambda_k^T (F_k(x_k, u_k) - x_{k+1}) + \tag{2} \\
& + \ell_N(x_N) + \mu_N^T h_N(x_N). \nonumber
\end{align}
$$

The KKT conditions for (2) read as follows:

$$
\frac{\partial L}{\partial x_k} = \nabla_x \ell_k + C_k^T \mu_k - \lambda_{k-1} + A_k^T \lambda_k = 0 \quad (3a)
$$

$$
\frac{\partial L}{\partial u_k} = \nabla_u \ell_k + D_k^T \mu_k + B_k^T \lambda_k = 0 \quad (3b)
$$

$$
\frac{\partial L}{\partial \lambda_k} = F_k(x_k, u_k) - x_{k+1} = 0 \quad (3c)
$$

$$
\frac{\partial L}{\partial \mu_k} = h_k(x_k, u_k) = 0, \quad (3d)
$$

for $k \in \{0, \dots, N-1\}$, whereas for $k = N$ we have

$$
\frac{\partial L}{\partial x_N} = \nabla_x \ell_N + C_N^T \mu_N - \lambda_{N-1} = 0 \quad (4a)
$$

$$
\frac{\partial L}{\partial \mu_k} = h_N(x_N) = 0. \qquad (4b)
$$

In (3) and (4) $\nabla_x \ell_k \in \mathbb{R}^n$ and $\nabla_u \ell_k \in \mathbb{R}^m$ are the gradients
of the intermediate costs w.r.t. state and input, respectively;
$A_k \in \mathbb{R}^{p_k \times n}$ and $B_k \in \mathbb{R}^{p_k \times m}$ are the Jacobian matrices of the
dynamic constraint (1b) w.r.t. state and input, respectively;
$C_k \in \mathbb{R}^{p_k \times n}$ and $D_k \in \mathbb{R}^{p_k \times m}$ are the Jacobian matrices of the
equality constraint (1c) w.r.t. state and input, respectively;
note that the dependency of all quantities upon the generic
KKT candidate $(x_k, u_k)$ had been omitted for the sake of
brevity.

B. Linearized KKT conditions

In general (3) and (4) represent a system of non-linear
equations, whose (possibly not unique) solution is a KKT
point, i.e. a candidate to be a local minimizer of (1). Starting
from an initial guess in terms of both state, input trajectories
and Lagrange multipliers, we can apply Newton’s method to
iteratively compute increments of such quantities, i.e. δx_k,
δu_k, δλ_k, and δμ_k, that fulfill the linearized KKT system
that follows:

$$
Q_k \delta x_k + P_k^T \delta u_k + C_k^T \delta \mu_k - \delta \lambda_{k-1} + A_k^T \delta \lambda_k = -q_k \quad (5a)
$$

$$
P_k \delta x_k + R_k \delta u_k + D_k^T \delta \mu_k + B_k^T \delta \lambda_k = -r_k \quad (5b)
$$

$$
A_k \delta x_k + B_k \delta u_k - \delta x_{k+1} = -d_k \quad (5c)
$$

$$
C_k \delta x_k + D_k \delta u_k = -c_k \quad (5d)
$$

for $k \in \{0, \dots, N-1\}$, whereas for $k = N$ we have

$$
Q_N \delta x_N + C_N^T \delta \mu_N - \delta \lambda_{N-1} = -q_N \quad (6a)
$$

$$
C_N \delta x_N = -c_N. \qquad (6b)
$$

<footnote>$^3\lambda_k$ is also known as the co-state vector.</footnote>
In (5) the following symbol definitions hold:

$$Q_k = \nabla_{xx} \ell_k + \nabla_{xx} h_k \cdot \mu_k + \nabla_{xx} F_k \cdot \lambda_k \quad (7a)$$

$$P_k = \nabla_{ux} \ell_k + \nabla_{ux} h_k \cdot \mu_k + \nabla_{ux} F_k \cdot \lambda_k \quad (7b)$$

$$R_k = \nabla_{uu} \ell_k + \nabla_{uu} h_k \cdot \mu_k + \nabla_{uu} F_k \cdot \lambda_k; \quad (7c)$$

furthermore, $q_k \in \mathbb{R}^n$, $r_k \in \mathbb{R}^m$, $d_k \in \mathbb{R}^n$, and $c_k \in \mathbb{R}^{p_k}$ are the KKT residuals given by equations (3a)-(3d), evaluated at the current linearization point. At the terminal node $k = N$ we have instead

$$Q_N = \nabla_{xx} \ell_N + \nabla_{xx} h_N \cdot \mu_N, \quad (8)$$

with $q_N \in \mathbb{R}^n$ and $c_N \in \mathbb{R}^{p_N}$ being given by the KKT residuals (4). Finally, in order for (3a) and (5a) to be valid at $k = 0$, we define the multipliers $\lambda_{-1}$ and $\delta\lambda_{-1}$ to be identically zero.

### C. Newton step computation: backward pass

Note that the linearized KKT block (5) at the generic node $k$ cannot be directly solved due to the dynamics-induced coupling with the previous and next nodes, as seen in (5a) and (5c). On the other hand, solving the overall KKT system obtained by stacking (5) for $k \in \{0, \dots, N-1\}$ would violate the linear complexity requirement w.r.t. the horizon length $N$. We therefore take a Riccati approach, and use the linearized dynamics (5c) to propagate both cost and constraints backward in time, starting from the final node, and solving at each step for one single control input increment $\delta u_k$. We shall hypothesize that a relation of the form (5) holds for any $k+1 \in \{0, \dots, N\}$, i.e.

$$S_{k+1} \delta x_{k+1} + V_{k+1}^T \delta v_{k+1} - \delta \lambda_k = -s_{k+1} \quad (9a)$$

$$V_{k+1} \delta x_{k+1} = -v_{k+1}, \quad (9b)$$

for some appropriate choice of a symmetric positive-definite matrix $S_{k+1} \in \mathbb{R}^{n \times n}$, $V_{k+1} \in \mathbb{R}^{q_{k+1} \times n}$, $s_{k+1} \in \mathbb{R}^n$, and $v_{k+1} \in \mathbb{R}^{q_{k+1}}$; as it will become more clear towards the end of this section, (9b) accumulates all constraints that have not been satisfied by future control inputs $\delta u_i$, for $i > k$, together with their corresponding multipliers $\delta v_{k+1}$. Clearly, for (9) to hold at $k+1 = N$, it must match (6), where $Q_N$ is therefore required to be positive-definite. To show that (9) holds at node $k$, too, let us substitute $\delta x_{k+1}$ from (5c) into (9) to obtain the following expressions:

$$\delta \lambda_k = S_{k+1} (A_k \delta x_k + B_k \delta u_k + d_k) + V_{k+1}^T \delta v_{k+1} + s_{k+1} \quad (10a)$$

$$V_{k+1} (A_k \delta x_k + B_k \delta u_k + d_k) = v_{k+1}. \quad (10b)$$

Using (10a) inside (5b), and concatenating (10b) with (5d) produces the following “reduced” KKT system

$$H_{uu,k} \delta u_k + \tilde{D}_k^T \delta \tilde{u}_k = -h_{u,k} - H_{ux,k} \delta x_k \quad (11a)$$

$$\tilde{D}_k \delta u_k = -\tilde{c}_k - \tilde{C}_k \delta x_k, \quad (11b)$$

where a tilde symbol has been used to denote extended constraint matrices and multipliers. These terms account for

both intermediate constraints (5d), and unsatisfied future constraints (9b). The following symbol definitions hold for (11):

$$H_{uu,k} = R_k + B_k^T S_{k+1} B_k, \quad (12a)$$

$$H_{xx,k} = Q_k + A_k^T S_{k+1} A_k \quad (12b)$$

$$H_{ux,k} = P_k + B_k^T S_{k+1} A_k \quad (12c)$$

$$h_{u,k} = r_k + B_k^T (S_{k+1} d_k + s_{k+1}) \quad (12d)$$

$$h_{x,k} = q_k + A_k^T (S_{k+1} d_k + s_{k+1}) \quad (12e)$$

$$\tilde{D}_k = \begin{bmatrix} D_k \\ V_{k+1} B_k \end{bmatrix} \in \mathbb{R}^{\tilde{p}_k \times m} \quad (12f)$$

$$\tilde{C}_k = \begin{bmatrix} C_k \\ V_{k+1} A_k \end{bmatrix} \in \mathbb{R}^{\tilde{p}_k \times n} \quad (12g)$$

$$\tilde{c}_k = \begin{bmatrix} c_k \\ v_{k+1} + V_{k+1} d_k \end{bmatrix} \in \mathbb{R}^{\tilde{p}_k} \quad (12h)$$

$$\delta \tilde{\mu}_k = \begin{bmatrix} \delta \mu_k \\ \delta v_{k+1} \end{bmatrix} \in \mathbb{R}^{\tilde{p}_k}, \quad (12i)$$

where $\tilde{p}_k = p_k + q_{k+1}$. After these steps an expression that only involves quantities at node $k$ has been obtained; (11) is a square, symmetric linear system in the control input increment $\delta u_k$ and in the vector of extended Lagrange multiplier increments $\delta \tilde{\mu}_k$, whereas the state increment $\delta x_k$ plays the role of a free parameter. Yet, (11) cannot be solved directly, as the extended input-constraint matrix $\tilde{D}_k$ is in general rank-deficient. Intuitively this reflects the fact that, for any given state increment $\delta x_k$, a constraint at time $k$ can not be fulfilled by the sole selection of an appropriate control $\delta u_k$: proper conditions must be satisfied by previous controls $\delta u_i$ for $i < k$, too.

### D. Constraint back-propagation

To compute $\delta u_k$ from (11), we (i) identify the part of the constraint that is feasible at time $k$, and (ii) define the remaining part to be the unsatisfied constraint at time $k$ in (9b). To this aim, let $\rho_k$ denote the rank of $\tilde{D}_k$, and consider the QR decomposition with column pivoting of $\tilde{D}_k$:

$$\tilde{D}_k = \tilde{Q} \tilde{R} \tilde{P}^T, \quad (13)$$

where $\tilde{Q} \in \mathbb{R}^{\tilde{p}_k \times \tilde{p}_k}$ is an orthonormal matrix; the matrix $\tilde{R} \in \mathbb{R}^{\tilde{p}_k \times m}$ has an upper-triangular structure, with only the upper $\rho_k$ rows containing non-zero values, whereas the lower $q_k = \tilde{p}_k - \rho_k$ rows are identically null:

$$\tilde{R} = \begin{bmatrix} \tilde{R}_{11} & \tilde{R}_{12} \\ 0 & 0 \end{bmatrix}, \quad (14)$$

with $\tilde{R}_{11} \in \mathbb{R}^{\rho_k \times \rho_k}$ upper-triangular and invertible; finally, $\tilde{P} \in \mathbb{R}^{m \times m}$ is a permutation matrix. By partitioning $\tilde{Q}$ as

$$\tilde{Q} = [\tilde{Q}_1 \quad \tilde{Q}_2], \quad \tilde{Q}_1 \in \mathbb{R}^{\tilde{p}_k \times \rho_k}, \quad \tilde{Q}_2 \in \mathbb{R}^{\tilde{p}_k \times q_k}, \quad (15)$$

we can separate the rank-deficient part of the input-constraint matrix $\tilde{D}_k$. It is simple to check that the following properties hold. First, the matrix $Y_k \in \mathbb{R}^{\rho_k \times m}$ is full-rank, where

$$Y_k = \tilde{Q}_1^T \tilde{D}_k; \quad (16)$$
indeed, using (14), one has

$$
\begin{align}
Y_k &= \tilde{Q}_1^T [\tilde{Q}_1 \quad \tilde{Q}_2] \tilde{R} \tilde{P}^T = \nonumber \\
    &= [I_{\rho_k} \quad 0_{\rho_k \times q_k}] \tilde{R} \tilde{P}^T = \nonumber \\
    &= [\tilde{R}_{11} \quad \tilde{R}_{12}] \tilde{P}^T, \tag{17}
\end{align}
$$

which is full-rank due to the $\tilde{R}_{11}$ block. Second, the matrix $Y_k^I \in \mathbb{R}^{q_k \times m}$ is identically zero, i.e.

$$
Y_k^I = \tilde{Q}_2^T \tilde{D}_k = 0_{q_k \times m}, \quad (18)
$$

where analogous steps as in (17) have been taken. Therefore,
rotating the constraint (11b) with $\tilde{Q}^T$ separates its feasible
components (i.e., the upper $\rho_k$ rows) from the infeasible ones
(i.e., the lower $q_k$ rows), yielding the expression

$$
H_{uu,k} \delta u_k + \tilde{D}_k^T \delta \tilde{\mu}_k = -h_{u,k} - H_{ux,k} \delta x_k \quad (19a)
$$

$$
Y_k \delta u_k = -w_k - W_k \delta x_k \quad (19b)
$$

$$
Y_k^I \delta u_k = 0 = -v_k - V_k \delta x_k, \quad (19c)
$$

where additional constraint matrices have been defined as
follows:

$$
\begin{align}
W_k &= \tilde{Q}_1^T \tilde{C}_k && w_k = \tilde{Q}_1^T \tilde{c}_k \nonumber \\
V_k &= \tilde{Q}_2^T \tilde{C}_k && v_k = \tilde{Q}_2^T \tilde{c}_k \tag{20}
\end{align}
$$

At time $k$, we remove (19c) from the reduced KKT system, and mark it as “to be satisfied” by the preceding control increment $\delta u_{k-1}$.

To remove from (19) the Lagrangian multipliers corre-
sponding to the infeasible constraint component, we again
exploit the QR decomposition (14), and write $\delta \tilde{\mu}_k$ from its
rotated components, i.e.

$$
\delta \tilde{\mu}_k = \tilde{Q}_1 \delta \eta_k + \tilde{Q}_2 \delta v_k, \quad (21)
$$

where $\delta\eta_k \in \mathbb{R}^{\rho_k}$ and $\delta v_k \in \mathbb{R}^{q_k}$ can be regarded as the
Lagrangian multiplier increments for the feasible and infea-
sible constraint components, respectively. Substituting (21)
into (19a) yields

$$
H_{uu,k} \delta u_k + Y_k^T \delta \eta_k = -h_{u,k} - H_{ux,k} \delta x_k \quad (22a)
$$

$$
Y_k \delta u_k = -w_k - W_k \delta x_k \quad (22b)
$$

where, due to (18), the infeasible constraint multiplier $\delta v_k$
has disappeared. The reduced KKT system (22) can finally be
solved for both the control input increment, and the Lagrange
multiplier increment of the feasible constraint component. As
expected, the result is expressed in terms of a linear policy
w.r.t. the unknown state increment, i.e.

$$
\begin{bmatrix} \delta u_k \\ \delta \eta_k \end{bmatrix} = - \underbrace{\begin{bmatrix} H_{uu,k} & Y_k^T \\ Y_k & 0 \end{bmatrix}^{-1}}_{K_k} \left( \begin{bmatrix} h_{u,k} \\ w_k \end{bmatrix} + \begin{bmatrix} H_{ux,k} \\ W_k \end{bmatrix} \right) \delta x_k, \quad (23)
$$

that we can compactly write as

$$
\delta u_k = l_u^k + L_u^k \delta x_k \quad (24a)
$$

$$
\delta \eta_k = l_{\eta,k} + L_{\eta,k} \delta x_k, \quad (24b)
$$

for

$$
\begin{bmatrix} l_{u,k} & L_{u,k} \\ l_{\eta,k} & L_{\eta,k} \end{bmatrix} = -K_k^{-1} \begin{bmatrix} h_{u,k} & H_{ux,k} \\ w_k & W_k \end{bmatrix}. \quad (25)
$$

The required recursion for (9) is completed by considering
the last remaining equation for time k, i.e. (5a). Substitut-
ing (10a), (21), and (24) yields

$$
\begin{align}
S_k' \delta x_k + C_k^T \delta \mu_k + A_k^T V_{k+1}^T \delta v_{k+1} - \delta \lambda_{k-1} &= \nonumber \\
&= S_k' \delta x_k + \tilde{C}_k^T \delta \tilde{\mu}_k - \delta \lambda_{k-1} = \nonumber \\
&= S_k' \delta x_k + \tilde{C}_k^T (\tilde{Q}_1 \delta \eta_k + \tilde{Q}_2 \delta v_k) - \delta \lambda_{k-1} = \nonumber \\
&= S_k \delta x_k + V_k^T \delta v_k - \delta \lambda_{k-1} = -s_k, \tag{26}
\end{align}
$$

which together with the following symbol definitions

$$
S_k = H_{xx,k} + H_{ux,k}^T L_{u,k} + W_k^T L_{\eta,k} \quad (27a)
$$

$$
s_k = h_{x,k} + H_{ux,k}^T l_{u,k} + W_k^T l_{\eta,k}, \quad (27b)
$$

as well as (19c), form the required relation (9) at node k.
Note that - due to (25) - (27a) can be equivalently expressed
as

$$
S_k = H_{xx,k} - \begin{bmatrix} H_{ux,k} \\ W_k \end{bmatrix}^T K_k^{-1} \begin{bmatrix} H_{ux,k} \\ W_k \end{bmatrix}, \quad (28)
$$

where $K_k \in \mathbb{R}^{(m+\rho_k) \times (m+\rho_k)}$ is the symmetric KKT matrix of (23), therefore highlighting the symmetry of $S_k$, as required.

E. Newton step computation: initial state optimization

Although most of the existing literature focuses on the
case where the initial state $x_0$ is fixed, the ILQR algorithm
poses no such restriction. Indeed, optimizing over the initial
state finds applications in the field of offline trajectory
optimization, where the initial system state is an actual
design variable (refer to Section III-A.3 for a proof-of-
concept example); notice also how in the context of equality-
constrained ILQR, a fixed initial state is obtained with the
simple linear constraint $x_0 = \bar{x}_0$.

Extending ILQR to initial state optimization is done by
considering (9) evaluated at the initial node, i.e.

$$
S_0 \delta x_0 + V_0^T \delta v_0 = -s_0 \quad (29a)
$$

$$
V_0 \delta x_0 = -v_0, \quad (29b)
$$

where the definition of $\delta\lambda_{-1} = 0$ has been used. Solving for the initial state increment $\delta x_0$ and constraint multiplier $\delta v_0$ completes the backward pass, and allows to initialize the forward recursion, as explained in the next subsection.

F. Newton step computation: forward pass

The Newton step computation process is completed by
providing recursive expressions for the state, dynamics mul-
tiplier, and constraint increments, starting from the known
quantities $\delta x_0$ and $\delta v_0$, as given by (29). Concerning the
state increment trajectory, a forward recursion is obtained
by directly applying the linearized KKT equation (5c), where
the control increment is computed by the linear policy (24).
To propagate the Lagrangian multiplier estimates, we first
compute the extended multiplier increment $\delta \tilde{\mu}_k$ from (21),
where $\delta v_k$ is known, and $\delta \eta_k$ is computed from the linear
policy (24). It is now possible to retrieve the Lagrangian mul-
tiplier increment $\delta \mu_k$ from (12i), together with the unsatisfied
constraint multiplier $\delta v_{k+1}$, which allows to compute co-state increments $\delta \lambda_k$ from (10), and to continue the forward pass. The equality-constrained ILQR forward recursion is summarized in the following set of equations:

$$ \delta x_{k+1} = (A_k + B_k L_{u,k}) \delta x_k + B_k l_{u,k} + d_k \quad (30a) $$

$$ \delta \tilde{\mu}_k = \begin{bmatrix} \delta \mu_k \\ \delta v_{k+1} \end{bmatrix} = \tilde{Q}_1 (l_{\eta,k} + L_{\eta,k} \delta x_k) + \tilde{Q}_2 \delta v_k \quad (30b) $$

$$ \delta \lambda_k = S_{k+1} \delta x_{k+1} + V_{k+1}^T \delta v_{k+1} + s_{k+1}, \quad (30c) $$

for $k \in \{0, \dots, N-1\}$, whereas the final constraint multiplier is simply given by $\delta \mu_N = \delta v_N$.

## G. Discussion

In contrast to most literature, we have derived the equality-constrained ILQR recursions from a KKT argument, without resorting to Bellman's principle of optimality. Our implementation is therefore mathematically equivalent to applying an SQP approach to the transcribed TO problem (1). In contrast to [19], [20], we have derived recursions for the Lagrangian multipliers, which allow to include second-order information about dynamics and constraint into the Newton's step, according to (7). Indeed, the presence of equality constraints does affect co-state multipliers - as seen in (10) - and therefore the second-order terms in (7). In this regard, it is worth noticing how classical DDP incorporates this information differently, i.e. by approximating the co-state vector as $\lambda_k \approx s_{k+1}$ (see [12, eq. (4c)-(4d)]), and therefore deviating from the Newton step. Estimates of Lagrangian multipliers contain valuable information on how constraints affect the optimal cost. Beside computation of second-order constraint information, they can be exploited to include inequality constraints via active-set strategies, where the multiplier value is essential in understanding which constraints should be removed from the current active set. Furthermore, they can be used inside a line-search based globalization strategy, as explained in Section II-H.

## H. Globalization

While the full Newton step $\delta X = (\delta x_{0:N}, \delta u_{0:N-1})$ enjoys local quadratic convergence close to a solution, it can perform poorly when the linear model of the KKT system is not accurate enough. Furthermore, applying Newton's method to the KKT system can result in convergence towards a KKT point that is a local maximizer of the considered cost. Convergence to a local minimum can be promoted by enforcing the decrease of a suitable merit function via e.g. of the line-search procedure described in [22, Ch. 3], which scales down the computed step $\delta X$ by a factor $\alpha$ until sufficient improvement is achieved. As discussed in [22], a merit function for a constrained problem should balance two conflicting objectives, i.e. feasibility on the one hand, and optimality on the other hand; furthermore, it is desirable for the chosen merit function to be exact, i.e. to possess a minimum point at any local solution of (1). One such function [22, Ch. 15.4] is the following exact $\ell_1$ merit $m$:

$$ m(X) = J(X) + \gamma \|C(X)\|_1, \quad (31) $$

where $J$ and $C$ are the total cost and constraint vector for (1), provided that the coefficient $\gamma > 0$ exceeds the maximum Lagrange multiplier at the solution, i.e.

$$ \gamma > \max\{\|\lambda_{0:N-1}^*\|_{\infty}, \|\mu_{0:N}^*\|_{\infty}\}. \quad (32) $$

As too large values of $\gamma$ are known to cause numerical issues, we conveniently exploit the computed multiplier estimates (30b), (30c) to compute a reasonable weight value.

In practice, the line-search procedure can fail at finding a large enough step size $\alpha > \alpha_{\min}$; we therefore add a regularization term around the current state trajectory, i.e. we set $Q_k \leftarrow Q_k + \epsilon_{\text{reg}}I$ for all $k$, increasing $\epsilon_{\text{reg}}$ by a constant factor whenever a failure happens, and reducing it upon success.

## III. IMPLEMENTATION AND VALIDATION

The proposed eILQR algorithm is implemented in C++, using Eigen3 for dense linear algebra, and made available via Python bindings to the recently published Horizon framework [21]. Within Horizon, we rely on CasADi [23] to act as a modeling language, as well as to compute all required derivatives with support to code generation, too. Multibody kinematics and dynamics are computed by the Pinocchio [24] library, including collision computations via HPP-FCL. We have found that the proposed architecture greatly simplifies prototyping of new costs and constraints as they can be directly written down with a natural syntax and without compilation steps, while the framework takes care of derivative generation.

As our use case is robotics-driven, we validate eILQR on a vast collection of robotic examples, where it is employed as an offline TO solver as well as in an online, receding-horizon fashion according to a real-time iteration (RTI) scheme. The adopted dynamic model is a kino-dynamic one, similar to [25], [26], where the state is composed of the robot configuration $q$ and its velocity $v$, whereas the accelerations $\dot{v}$ and contact forces $F$ are taken as inputs; the resulting continuous-time dynamics is then integrated with a 4th-order Runge-Kutta scheme.

In all TO problems equality constraints play a dominant role, as they are adopted (i) to enforce the robot's centroidal dynamics (i.e., physical consistency between state and input), (ii) to model stance phase by imposing zero contact velocity, (iii) to impose a given clearance trajectory to swing feet, and (iv) to constrain (a sub-set of) the final configuration to a desired goal state, which is the main driver behind all the optimized motions. Costs are only used to regularize the solution, whereas unilateral contact forces, friction cones, and joint limits are modeled with soft barriers.

### A. Validation suite

A brief description of our test suite follows; due to space limitations, the reader is invited to refer to the supplementary material (video also available at https://youtu.be/NGIJ9iaalRw) to get a better grasp about the optimized motions.
<figure><img src="image_5.png" /><figcaption>Fig. 2. Optimized trajectory requiring our humanoid robot COMAN+ to perform a dynamic 1.0m forward jump.</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>Fig. 3. History of constraint violations as the eILQR solver converges to a solution. The solver termination condition is set to $\epsilon_{feas} = 10^{-6}$.</figcaption></figure>

<figure><img src="image_7.png" /><figcaption>Fig. 4. Adaptive step size $\alpha$ and regularization $\epsilon_{reg}$ histories.</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>Fig. 5. Histogram of required CPU time to solve a single backward pass iteration (i.e., for a single node), as well as to perform the constraint backpropagation routine. Experiment IDs correspond to the legend of Figure 3.</figcaption></figure>

1) *Biped walk (BW)*: Optimization of a fixed number of steps to make our humanoid robot COMAN+ reach a given final configuration.

2) *Centauro walk (CW)*: Optimization of a fixed number of steps to make our quadruped Centauro climb off a 30 cm platform (see Figure 1), by assigning target footholds to a sub-set of its contacts via equality constraints. Since the robot upper body causes a significant forward shift to its CoM, this kind of motion requires careful handling of stability, joint limits, and leg over-stretching.

3) *Non-holonomic base (MB)*: A non-holonomic mobile manipulator is required to perform a pick-and-place motion between two fixed poses, modeled with equality constraints. An offline optimization is carried out both with fixed and variable initial state.

4) *Dynamic motions suite (DM)*: In this set of TO problems we adopt Spot® from Boston Dynamics as archetypal agile quadruped, and require it to perform different jumping motions. These are obtained from just (i) specification of a contact schedule, and (ii) final robot configuration constraint.

5) *Third-order jump (J3)*: To further highlight the effectiveness of eILQR when high relative degree constraints are employed, we augment our kino-dynamic model with an additional integrator, so that our control inputs become the jerk $\ddot{v}$ and the force derivative $\dot{F}$. We optimize two jumping motions with Spot® (with and without front-flip), and one with our humanoid robot COMAN+ (see Figure 2).

## B. Discussion

As seen e.g. in Figure 3, eILQR can solve all problems within a modest number of iterations despite being seeded with a naive, constant initial guess. The proposed merit function and regularization strategy (Figure 4) play therefore

a crucial role in monitoring progress and result in a globally convergent algorithm. Finally, the computational impact of the constraint propagation routine of Section II-D on the backward pass is rather moderate, as seen in Figure 5.

## IV. CONCLUSIONS

This paper has introduced the implementation of the equality-constrained ILQR (eILQR) solver used inside the recently published Horizon framework [21] for trajectory optimization (TO). Instead of relying on Bellman's principle of optimality, we directly applied Newton's method to the KKT conditions for the transcribed TO problem, and shown how to solve for the Newton step with linear complexity w.r.t. the horizon length. Furthermore, we have presented forward-backward recursions for the Lagrange multipliers, which are critical in incorporating second-order constraint information, as well as to define a well-behaved merit function for globalization purposes. Compared to competing approaches based on Augmented Lagrangian techniques, the proposed eILQR can solve linear-quadratic problems in a single iteration, without the need to iteratively increase a penalty parameter, leading to an improved numerical conditioning. Finally, to the best of the authors' knowledge, this work is the first to provide a quantitative and extensive validation of eILQR outside the linear-quadratic domain, by applying it to complex, non-linear and non-convex TO problems that arise in the field of multi-legged robotics.

## ACKNOWLEDGMENTS

The research leading to these results has received funding from the European Union's Horizon 2020 research and innovation programme under grant agreements No. 101016007 (CONCERT) and No. 871237 (SOPHIA).
REFERENCES

[1] C. Mastalli, R. Budhiraja, W. Merkt, G. Saurel, B. Hammoud, M. Naveau, J. Carpentier, L. Righetti, S. Vijayakumar, and N. Mansard, “Crocoddyl: An efficient and versatile framework for multi-contact optimal control,” in *2020 IEEE International Conference on Robotics and Automation (ICRA)*, pp. 2536–2542, IEEE, 2020.

[2] F. Farshidian, M. Neunert, A. W. Winkler, G. Rey, and J. Buchli, “An efficient optimal planning and control framework for quadrupedal locomotion,” in *2017 IEEE International Conference on Robotics and Automation (ICRA)*, pp. 93–100, IEEE, 2017.

[3] R. Tedrake and the Drake Development Team, “Drake: Model-based design and verification for robotics,” 2019.

[4] J. T. Betts, *Practical Methods for Optimal Control and Estimation Using Nonlinear Programming, Second Edition*. Society for Industrial and Applied Mathematics, second ed., 2010.

[5] G. Karypis and V. Kumar, “METIS—A Software Package for Partitioning Unstructured Graphs, Partitioning Meshes and Computing Fill-Reducing Ordering of Sparse Matrices,” 01 1997.

[6] T. A. Davis, J. R. Gilbert, S. I. Larimore, and E. G. Ng, “Algorithm 836: COLAMD, a Column Approximate Minimum Degree Ordering Algorithm,” *ACM Trans. Math. Softw.*, vol. 30, p. 377–380, sep 2004.

[7] R. Bellman, “Dynamic programming,” *Science*, vol. 153, no. 3731, pp. 34–37, 1966.

[8] M. Giftthaler, M. Neunert, M. Stauble, J. Buchli, and M. Diehl, “A family of iterative gauss-newton shooting methods for nonlinear optimal control,” pp. 1–9, 10 2018.

[9] E.>Dantec, M. Taïx, and N. Mansard, “First order approximation of model predictive control solutions for high frequency feedback,” *IEEE Robotics and Automation Letters*, vol. 7, no. 2, pp. 4448–4455, 2022.

[10] N. Kashiri, L. Baccelliere, L. Muratore, A. Laurenzi, Z. Ren, E. M. Hoffman, M. Kamedula, G. F. Rigano, J. Malzahn, S. Cordasco, et al., “Centauro: A hybrid locomotion and high power resilient manipulation platform,” *IEEE Robotics and Automation Letters*, vol. 4, no. 2, pp. 1595–1602, 2019.

[11] C. Mastalli, W. Merkt, G. Xin, J. Shim, M. Mistry, I. Havoutis, and S. Vijayakumar, “Agile maneuvers in legged robots: a predictive control approach,” *arXiv preprint arXiv:2203.07554*, 2022.

[12] Y. Tassa, N. Mansard, and E. Todorov, “Control-limited differential dynamic programming,” in *2014 IEEE International Conference on Robotics and Automation (ICRA)*, pp. 1168–1175, 2014.

[13] Z. Xie, C. K. Liu, and K. Hauser, “Differential dynamic programming with nonlinear constraints,” in *2017 IEEE International Conference on Robotics and Automation (ICRA)*, pp. 695–702, 2017.

[14] M. Giftthaler and J. Buchli, “A projection approach to equality constrained iterative linear quadratic optimal control,” in *2017 IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids)*, pp. 61–66, 2017.

[15] S. Ko and R. R. Bitmead, “Optimal control of linear systems with state equality constraints,” *IFAC Proceedings Volumes*, vol. 38, no. 1, pp. 407–412, 2005.

[16] S. E. Kazdadi, J. Carpentier, and J. Ponce, “Equality constrained differential dynamic programming,” in *2021 IEEE International Conference on Robotics and Automation (ICRA)*, pp. 8053–8059, 2021.

[17] T. A. Howell, B. E. Jackson, and Z. Manchester, “ALTRO: A Fast Solver for Constrained Trajectory Optimization,” in *2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, pp. 7674–7679, 2019.

[18] A. Sideris and L. A. Rodriguez, “A riccati approach to equality constrained linear quadratic optimal control,” in *Proceedings of the 2010 American Control Conference*, pp. 5167–5172, 2010.

[19] F. Laine and C. Tomlin, “Efficient Computation of Feedback Control for Equality-Constrained LQR,” in *2019 International Conference on Robotics and Automation (ICRA)*, pp. 6748–6754, 2019.

[20] S. Yang, G. Chen, Y. Zhang, F. Dellaert, and H. Choset, “Equality constrained linear optimal control with factor graphs,” *2021 IEEE International Conference on Robotics and Automation (ICRA)*, pp. 9717–9723, 2021.

[21] F. Ruscelli, A. Laurenzi, N. G. Tsagarakis, and E. Mingo Hoffman, “Horizon: A trajectory optimization framework for robotic systems,” *Frontiers in Robotics and AI*, vol. 9, 2022.

[22] J. Nocedal and S. J. Wright, *Numerical Optimization*. New York, NY, USA: Springer, 2e ed., 2006.

[23] J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “CasADi – A software framework for nonlinear optimization and optimal control,” *Mathematical Programming Computation*, vol. 11, no. 1, pp. 1–36, 2019.

[24] J. Carpentier, G. Saurel, G. Buondonno, J. Mirabel, F. Lamiraux, O. Stasse, and N. Mansard, “The pinocchio c++ library – a fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives,” in *IEEE International Symposium on System Integrations (SII)*, 2019.

[25] J.-P. Sleiman, F. Farshidian, M. V. Minniti, and M. Hutter, “A unified mpc framework for whole-body dynamic locomotion and manipulation,” *IEEE Robotics and Automation Letters*, vol. 6, no. 3, pp. 4688–4695, 2021.

[26] H. Ferrolho, V. Ivan, W. Merkt, I. Havoutis, and S. Vijayakumar, “Inverse dynamics vs. forward dynamics in direct transcription formulations for trajectory optimization,” in *2021 IEEE International Conference on Robotics and Automation (ICRA)*, pp. 12752–12758, 2021.