1

Pfeiffer et al. Efficient Lexicographic Optimization for Prioritized Robot
Control and Planning

Kai Pfeiffer.

# Efficient Lexicographic Optimization for Prioritized Robot Control and
Planning

Kai Pfeiffer  Abderrahmane Kheddar  Schaeffler Hub for Advanced Research,
School of Mechanical and Aerospace Engineering, Nanyang Technological
University, Singapore  Joint Robotics Laboratory (JRL) UMI3218/RL, CNRS/AIST,
Tsukuba, Japan  Interactive Digital Human, University of Montpellier, CNRS,
LIRMM, UMR5506, Montpellier, France<EMAIL_ADDRESS>Pfeiffer K
Kheddar A

###### Abstract

[Abstract]In this work, we present several tools for efficient sequential
hierarchical least-squares programming (S-HLSP) for lexicographical
optimization tailored to robot control and planning. As its main step, S-HLSP
relies on approximations of the original non-linear hierarchical least-squares
programming (NL-HLSP) to a hierarchical least-squares programming (HLSP) by
the hierarchical Newton’s method or the hierarchical Gauss-Newton algorithm.
We present a threshold adaptation strategy for appropriate switches between
the two. This ensures optimality of infeasible constraints, promotes numerical
stability when solving the HLSP’s and enhances optimality of lower priority
levels by avoiding regularized local minima. We introduce the solver
$\mathcal{N}$ADM2, an alternating direction method of multipliers for HLSP
based on nullspace projections of active constraints. The required basis of
nullspace of the active constraints is provided by a computationally efficient
turnback algorithm for system dynamics discretized by the Euler method. It is
based on an upper bound on the bandwidth of linearly independent column
subsets within the linearized constraint matrices. Importantly, an expensive
initial rank-revealing matrix factorization is unnecessary. We show how the
high sparsity of the basis in the fully-actuated case can be preserved in the
under-actuated case. $\mathcal{N}$ADM2 consistently shows faster computations
times than competing off-the-shelf solvers on NL-HLSP composed of test-
functions and whole-body trajectory optimization for fully-actuated and under-
actuated robotic systems. We demonstrate how the inherently lower accuracy
solutions of the alternating direction method of multipliers can be used to
warm-start the non-linear solver for efficient computation of high accuracy
solutions to non-linear hierarchical least-squares programs.

, . Efficient Lexicographic Optimization for Prioritized Robot Control and
Planning J Comput Phys. 2024;00(00):1–18.

###### keywords:

optimisation, robots, nonlinear programming, hierarchical systems, discrete
time systems, optimal control

††footnotetext: Abbreviations: HLSP, hierarchical least-squares programming;
NL-HLSP, non-linear hierarchical least-squares programming; S-HLSP, sequential
hierarchical least-squares programming.

##  1 Introduction

###  1.1 Context and contribution

Lexicographic multi-objective optimization (LMOO) is the hierarchical stacking
of $p$ optimization problems 1 ($\operatorname*{lexmin.}$: lexicographically
minimize)

$\displaystyle\operatorname*{lexmin.}_{x,v}\qquad$
$\displaystyle\|v_{\mathbb{C}_{1}}\|_{g},\dots,\|v_{\mathbb{C}_{p}}\|_{g}$
(LMOO) s.t $\displaystyle f_{\mathbb{C}_{\cup p}}(x)\leqq v_{\mathbb{C}_{\cup
p}}$

The symbol $\leqq$ summarily describes equality and inequality constraints in
the constraint set $\mathbb{C}$. The symbol $\cup$ represents the union of
constraint sets from levels 1 to $p$ as $\mathbb{C}_{\cup
p}\coloneqq\mathbb{C}_{1}\cup\dots\mathbb{C}_{p}$. The function
$f_{\mathbb{C}}(x)\in\mathbb{R}^{|\mathbb{C}|}$ in dependence of the variable
vector $x\in\mathbb{R}^{n}$ represents the constraint set $\mathbb{C}$. Such
problems are characterized by the optimal infeasibility (slacks $v$)
$\|v_{\mathbb{C}_{\cup l-1}}^{*}\|_{g}>0$ or optimality $v_{\mathbb{C}_{\cup
l-1}}^{*}=0$ of higher priority levels 1 to $l-1$, which must be preserved by
the lower priority levels $l$ to $p$. The infeasibility is thereby optimal /
minimal with respect to some norm $g\geq 1$. Our above formulation of LMOO is
a modification of the classical one as described in 2 to include (feasible and
infeasible) inequality constraints. A specific form of LMOO is non-linear
hierarchical least-squares programming (NL-HLSP) with $g=2$. NL-HLSP’s have
been commonly utilized in instantaneous robot feed-forward 3 and feed-back 4
control. This enables an intuitive control formulation as no weights between
different constraints need to be tuned. Furthermore, robot safety and physical
stability is enhanced as critical constraints of different importance are
strictly separated from control objectives like reaching tasks. Instantaneous
prioritized robot control can be solved by a real-time and anytime algorithm
modification of sequential hierarchical least-squares programming (S-HLSP) for
NL-HLSP 5. It utilizes the current hierarchical least-squares programming
(HLSP) approximation of the original NL-HLSP to deduce a new robot control
step. By utilizing trust-region constraints or regularization, the validity of
the approximation is maintained at the current robot state. In the recent work
6, S-HLSP has been leveraged for the resolution of NL-HLSP representing
optimal control or trajectory optimization problems. Here, not only one
instance, but a longer horizon of the robot control and state is considered.
This enables robots to achieve a wide range of motions by reasoning in
anticipating fashion about its physical and mechanical limits 7. In this work,
we propose a fast hierarchical least-squares programming (HLSP) solver based
on the alternating direction method of multipliers (ADMM). The proposed HLSP
solver is computationally more efficient than other solver methods when the
number of iterations is limited. Sparse nullspace projections are leveraged to
eliminate structured constraints like dynamics equations in optimal control
scenarios. The nullspace basis is based on an efficient implementation of the
turnback algorithm 8 with an upper bound on the bandwidth in case of multiple-
shooting transcription by Euler integration. At the same time, we are able to
handle multi-stage constraints like regularization of momentum evolution for
safe robot control. This is a distinguishing factor to recursive methods like
differential dynamic programming (DDP, 9). The efficiency of the proposed
methods is evaluated on non-linear test-functions and robot trajectory
planning.

###  1.2 Non-linear programming

Non-linear programming (NLP) is a broad classification of optimization
problems and includes non-linear and smooth convex and non-convex constraints
and objectives 10. It is a special form of LMOO with $p=2$, typically feasible
constraints $v_{1}=0$ and $|\mathbb{C}_{2}|=1$ such that the norm notation can
be omitted. Solution approaches typically involve the repeated approximation
of the original non-linear program to a simpler one at the current working
point. Inequality constraints are typically recast by including penalty terms
in the cost function. The primal-dual interior-point method is characterized
by a barrier functions with primal penalization towards the boundary of the
feasible region 11. Exact penalty functions like non-smooth indicator
functions have been investigated for example in the context of augmented
Lagrangian methods. Here, infeasibilities are infinitely penalized outside of
the primal feasible region, and not penalized otherwise 12. The augmented
Lagrangian can for example be used within the ADMM. It consists of alternating
updates of the primal and the dual 13. Another solution approach can be found
in sequential quadratic programming (SQP) 14. The original non-linear
optimality conditions are approximated to second order by Newton’s method. The
resulting quadratic program (QP) sub-problem is then iteratively solved for a
primal and dual sub-step. In all methods above, convergence relies on
globalization methods to direct the approximate sub-steps in terms of
infeasibility reduction and optimality. Filter methods with trust region
constraints are popular in SQP 15. The trust-region constraint maintains
validity of the QP sub-problems by limiting the step-size. On the other hand,
line search methods directly curtail the resulting step in order to fulfill
for example Armijo’s condition 16. Line search in combination with a filter
method has been proposed for the interior-point method 17.

###  1.3 Prioritized robot control and planning

NL-HLSP’s can exhibit sparsity patterns, for example resulting from discrete
optimal control problems. We consider discretization by direct transcription
methods, namely numerical integration by the Euler method. Due to stage-wise
variable dependency of the constraints, the resulting optimality conditions
exhibit block-diagonal structure. Exploiting this sparsity is critical in
order to maintain linear complexity in the control horizon length 18.
Differential dynamic programming methods 9 are a popular tool to leverage such
sparse problem formulations. They are very efficient due to low bandwidth only
dependent on the number of the input controls. Originally developed for
unconstrained systems, in recent years many developments have been proposed
for constrained ones. These include interior-point method 19 and augmented
Lagrangian 20 based approaches. Prioritized trajectory optimization has been
treated for example in 21 which solves a hierarchy of quadratic programs each
projected into the nullspace of the previous level. Sparsity of the
constraints is exploited by leveraging DDP. The approach in 22 explicitly
considers the active constraints in order to preserve the hierarchical
ordering. Using principles from time-delay systems, linear and robust controls
result from quadratic program solutions at each instance of a model-predictive
controller. Both approaches can only handle input and state limit constraints.
This is in contrast to LMOO as proposed in 23, which is based on prioritized
Pareto efficiency. However, sparsity is not exploited. In this work, we
propose a sparse solver for prioritized trajectory optimization cast as NL-
HLSP under multi-stage equality and inequality constraints. Constraints can
involve variables from several stages, for example regularization of the
momentum evolution for safe robot control. Such problems can be solved by off-
the-shelf sparse non-linear solvers 17. However, higher efficiency can be
achieved with more dedicated solvers like the aforementioned prioritized
solvers based on the reduced Hessian formulation 6. Here, the right choice of
nullspace basis reduces the number of variables (dense programming) or non-
zeros (sparse programming). This offsets the computational burden of computing
a basis of the nullspace. Several sparsity preserving bases of nullspace have
been proposed both in the communities of structural mechanics and control
theory. The authors in 24 described a sparsity preserving basis based on
identifying linearly independent sub-sets. These arise due to the limited
bandwidth of the blocks. Several improvements have been proposed. The work in
25 proposes sparsity enhancing improvements. A more efficient computation
leveraging columns updates of the linearly independent sub-matrices is
described in 6. A control theoretic approach has been developed for example in
26.

###  1.4 Overview

This article is structured as follows. First, we describe the current state-
of-the-art and our contributions in non-linear hierarchical least-squares
programming and prioritized non-linear optimal control (Sec. 2). We then
introduce a heuristic for adjusting the threshold for second-order information
(Sec. 3). This promotes numerical stability when solving the HLSP sub-problems
and avoids local regularized minima of lower priority levels. We develop the
HLSP solver $\mathcal{N}$ADM2 based on the ADMM, see Sec. 4. We present how to
tune parameters of the ADMM for algorithmic efficiency (Sec. 4.2) and make
some considerations towards warm-starting (Sec. 4.3) and the computation of
dual variables (Sec. 4.4). Next, we design an efficient implementation of the
turnback algorithm for the computation of nullspace basis of banded matrices.
Specifically, we consider dynamics discretized by Euler integration and derive
an upper bound on the bandwidth of the resulting nullspace basis (Sec. 5). We
show that the algorithm can be highly parallelized, which is a distinguishing
factor in comparison to recursive methods like the DDP (Sec. 5.7).

## Nomenclature

  * $l$

Current priority level

  * $p$

Overall number of priority levels, excluding the trust region constraint on
$l=0$

  * $n$

Number of variables

  * $r$

Rank of matrix

  * $n_{r}$

Number of remaining variables after nullspace projections

  * $m$

Number of constraints

  * $x\in\mathbb{R}^{n}$

Primal of NL-HLSP

  * $\Delta x\in\mathbb{R}^{n}$

Primal of HLSP

  * $\Delta z\in\mathbb{R}^{n_{r}}$

Primal of projected HLSP

  * $\Delta\hat{z}\in\mathbb{R}^{n_{r}}$

Auxiliary primal of projected HLSP

  * $f(x)\in\mathbb{R}^{m}$

Non-linear constraint function of variable vector $x\in\mathbb{R}^{n}$

  * $\mathbb{E}_{l}$

Set of $m_{\mathbb{E}}$ equality constraints (eq.) of level $l$

  * $\mathbb{I}_{l}$

Set of $m_{\mathbb{I}}$ inequality constraints (eq.) of level $l$

  * $\mathcal{I}_{l}$

Set of $m_{\mathcal{I}}$ inactive inequality constraints (ineq.) of level $l$

  * $\mathcal{A}_{l}$

Set of $m_{\mathcal{A}}$ active equality and inequality constraints of level
$l$

  * $\mathbb{E}_{\cup l}$ (or ${\mathcal{E}}_{\cup l}$)

Set union $\mathbb{E}_{\cup
l}\coloneqq\bigcup_{i=1}^{l}\mathbb{E}_{i}=\mathbb{E}_{1}\cup\cdots\cup\mathbb{E}_{l}$
with $m_{{\mathbb{E}}_{\cup l}}$ constraints

  * $A_{\mathbb{E}}\in\mathbb{R}^{m_{\mathbb{E}}\times n}$

Matrix representing a set $\mathbb{E}$ of $m_{\mathbb{E}}$ linear constraints

  * $b_{\mathbb{E}}\in\mathbb{R}^{m_{\mathbb{E}}}$

Vector representing a set $\mathbb{E}$ of $m_{\mathbb{E}}$ linear constraints

  * $\mathcal{N}(A_{\mathcal{A}_{l}})$

Operator to compute the nullspace basis $Z_{{\mathcal{A}}_{l}}$ and the rank
$r$ of a matrix $A_{\mathcal{A}_{l}}$

  * $Z_{{\mathcal{A}}_{l}}\in\mathbb{R}^{n\times n_{r}}$

Nullspace basis of matrix
$A_{\mathcal{A}_{l}}\in\mathbb{R}^{m_{\mathcal{A}_{l}}\times n}$ with rank
$r$, $n_{r}=n-r$ and $A_{\mathcal{A}_{l}}Z_{{\mathcal{A}}_{l}}=0$

  * $N_{l-1}\in\mathbb{R}^{n\times n_{r}}$

Accumulated nullspace basis $N_{\mathcal{A}_{\cup
l}}=Z_{{\mathcal{A}}_{1}}\dots Z_{{\mathcal{A}}_{l}}$

  * $\tilde{M}\in\mathbb{R}^{m\times n_{r}}$

Matrix $\tilde{M}=MN$ projected into the nullspace basis
$N\in\mathbb{R}^{n\times n_{r}}$ of a matrix $A\in\mathbb{R}^{m\times n}$ of
rank $r$; $n_{r}=n-r$ (variable elimination)

  * $v\in\mathbb{R}^{m}$

Slack variable of NL-HLSP

  * $\hat{v}\in\mathbb{R}^{m}$

HLSP equivalent of slack variable

  * $v^{*}\in\mathbb{R}^{m}$

Optimal slack variable

  * $\mathcal{L}$

Lagrangian

  * $K$

Gradient of Lagrangian $K\coloneqq\nabla\mathcal{L}$

  * $H_{l}$, $\hat{H}_{l}$

Hierarchical Lagrangian Hessian, positive definite equivalent

  * $\rho$, $\sigma$

ADMM step-size parameter

  * $\lambda\in\mathbb{R}^{{m}}$

Lagrange multiplier

  * $\upsilon$

Scaled Lagrange multiplier

  * $k$

Outer iteration of NL-HLSP solver

  * $\iota$

Inner iteration of HLSP solver

  * $\rho$

Trust region radius

  * $\nu$

Activation threshold of inequality constraints

  * $\chi$

Convergence threshold of S-HLSP

  * $\epsilon_{adaptive,l}$

Adaptive second-order information (SOI) threshold of level $l$

##  2 Problem definition and contributions

Non-linear Hierarchical Least-Squares Program (NL-HLSP) L.$1$
$f_{1}(x_{k})\leqq\leavevmode\nobreak\ v_{1}$
$\vdots$ L.$p$ $f_{p}(x_{k})\leqq\leavevmode\nobreak\ v_{p}$ Linearization of
each level $l=1,\dots,p$
• If
$\|\hat{v}_{l,k}^{*}\|_{2}\geq{\color[rgb]{0.35,0.35,1}\definecolor[named]{pgfstrokecolor}{rgb}{0.35,0.35,1}\epsilon_{adaptive,l}}$
(Sec. 3) – Hierarchical Newton’s method 5 – Hierarchical Quasi-Newton method
27 • Else – Hierarchical Gauss-Newton algorithm ($\hat{H}_{l}=0$) Hierarchical
Least-Squares Program (HLSP) L.$0$ $-\rho\leq\Delta x_{k}\leq\rho$
L.$1$ $A_{1,k}^{i}\Delta x_{k}+b_{1,k}^{i}\leqq\leavevmode\nobreak\
\hat{v}_{1,k}^{i}$
$\vdots$ L.$p$ $A_{p,k}^{i}\Delta x_{k}+b_{p,k}^{i}\leqq\leavevmode\nobreak\
\hat{v}_{p,k}^{i}$ $\Delta x_{k}$$\hat{v}_{{\mathcal{A}}_{\cup
p},k}^{*}$$\hat{\lambda}_{{\mathcal{A}}{\cup p},k}$$x_{0}$, $k=0$, $\rho_{\cup
p}=\rho_{0}$ As input for calculations of HLSP and HSF  Solve HLSP for $\Delta
x_{k}$: $\mathcal{N}$ADM2 (Sec. 4) based on Turnback algorithm for Euler
integrated dynamics (Sec. 5) Is $\|\Delta x_{k}\|_{2}<\chi$?
$x_{k+1},\rho_{l}\leftarrow$HSFl($x_{k}+\Delta x_{k},\rho_{l}$) 6
$v_{l}^{*}=v_{l}$, $l$++, if $l=p+1$ exit $k$++ No Yes

Figure 1: A symbolic overview of the sequential hierarchical least-squares
programming (S-HLSP) with trust region and hierarchical step-filter (HSF)
based on the SQP step-filter 15 to solve non-linear hierarchical least-squares
programmings (NL-HLSP) with $p$ levels. Our contributions, an adaptive
threshold for second-order information and the HLSP sub-problem solver
$\mathcal{N}$ADM2 in combination with an efficient turnback algorithm for
Euler integrated dynamics, are marked in blue.

###  2.1 Non-linear Hierarchical Least-Squares Programming

In this article, we consider non-linear hierarchical least-squares problems
(NL-HLSP) as preemptive transcription 28 of LMOO with $g=2$:

$\displaystyle\operatorname*{min.}_{{x},v_{\mathbb{E}_{l}},v_{\mathbb{I}_{l}}}\quad$
$\displaystyle\frac{1}{2}\left\|v_{\mathbb{E}_{l}}\right\|^{2}_{2}+\frac{1}{2}\left\|v_{\mathbb{I}_{l}}\right\|^{2}_{2}\qquad\quad\hskip
8.0ptl=1,\dots,p$ s.t. $\displaystyle
f_{\mathbb{E}_{l}}(x)=v_{\mathbb{E}_{l}}$ $\displaystyle
f_{\mathbb{I}_{l}}(x)\leq v_{\mathbb{I}_{l}}$ $\displaystyle
f_{{\mathcal{A}}_{\cup l-1}}(x)=v_{{\mathcal{A}}_{\cup l-1}}^{*}$
$\displaystyle f_{{\mathcal{I}}_{\cup l-1}}(x)\leq 0$ (NL-HLSP)

Each problem corresponding to the levels $l=1,\dots,p$ is solved in order. At
convergence of each level $l$ at the primal $x^{*}_{l}\in\mathbb{R}^{n}$, the
feasible $v_{l}^{*}\in\mathbb{R}^{m_{l}}=0$ or optimally infeasible points
$v_{l}^{*}\neq 0$ of the sets of equality and inequality constraints
$|{\mathbb{E}}_{l}|={m_{{\mathbb{E}}_{l}}}$ and
$|{\mathbb{I}}_{l}|={m_{{\mathbb{I}}_{l}}}$ is identified. The slack variables
$v_{{\mathcal{A}}_{\cup l-1}}^{*}$ are the optimal ones identified for the
higher priority levels $1$ to $l-1$. The active set ${\mathcal{A}}_{\cup l-1}$
contains all constraints that are active at convergence of levels $1$ to
$l-1$. The active set includes all equality constraints
${\mathbb{E}}_{\cup_{l-1}}$, and furthermore all violated / infeasible
($v^{*}>0$) or saturated ($v*=0$) inequality constraints of
${\mathbb{I}}_{l}$. In a similar vein, the inactive set ${\mathcal{I}}_{\cup
l-1}$ contains all the remaining feasible inequality constraints ($v*=0$) of
the sets ${\mathbb{I}}_{\cup l-1}$.

Contribution: we make some considerations towards resolving limitations of NL-
HLSP’s. Specifically, we demonstrate how we can identify local minima
associated with negative function values, see Sec. 6.2.

###  2.2 Sequential Hierarchical Least-Squares Programming

Sequential hierarchical least-squares programming (S-HLSP) is a method to
resolve NL-HLSP. Here, the NL-HLSP is linearized around the current working
point $x_{k}$ at every outer iteration $k$ to a HLSP by virtue of the
hierarchical Newton’s method 5. Hierarchical least-squares programs (HLSP) are
problems of the form

$\displaystyle\operatorname*{min.}_{\Delta
x,\hat{v}_{\mathbb{E}_{l}},\hat{v}_{\mathbb{I}_{l}}}$
$\displaystyle\qquad\frac{1}{2}\|\hat{v}_{\mathbb{E}_{l}}\|^{2}_{2}+\frac{1}{2}\|\hat{v}_{\mathbb{I}_{l}}\|^{2}_{2}\qquad
l=1,\dots,p$ s.t. $\displaystyle\qquad A_{\mathbb{E}_{l}}\Delta
x-b_{\mathbb{E}_{l}}=\hat{v}_{\mathbb{E}_{l}}$ $\displaystyle\qquad
A_{\mathbb{I}_{l}}\Delta x-b_{\mathbb{I}_{l}}\leq\hat{v}_{\mathbb{I}_{l}}$
$\displaystyle\qquad A_{{\mathcal{A}}_{\cup l-1}}\Delta
x-b_{{\mathcal{A}}_{\cup l-1}}=\hat{v}_{{\mathcal{A}}_{\cup l-1}}^{*}$
$\displaystyle\qquad A_{{\mathcal{I}}_{\cup l-1}}\Delta
x-b_{{\mathcal{I}}_{\cup l-1}}\leq 0$ (HLSP)

Variables $\hat{\cdot}$ are the linear equivalents to the non-linear ones of
the NL-HLSP. Notably, the problem constraints are linear. The constraint
matrices and vectors $A$ and $b$ represent this linearization (Jacobians and
Hessians) of non-linear constraints $f$.

Utilizing Fletcher’s filter method, the resulting primal steps from the HLSP
sub-problems are accepted or rejected, depending on sufficient progress in
terms of constraint infeasibility reduction and optimality. The HLSP sub-
problems are subject to a trust-region constraint in order to maintain the
validity of the approximation. The trust-region radius is increased or
decreased depending on step acceptance and rejection, respectively.
Linearization methods of the NL-HLSP to HLSP include the hierarchical Newton’s
method (using second order information (SOI) in form of the hierarchical
Hessian) or the hierarchical Gauss-Newton algorithm (no SOI) 5. A switch
between the two is decided upon the residual of the HLSP sub-problem.

Contribution: An overview of S-HLSP is given in Fig. 1. In this work, we
propose an adaptive thresholding strategy for SOI augmentation in the
hierarchical Newton’s method (Sec. 3). This promotes numerical stability when
solving the HLSP sub-problems and enhances solution optimality of lower
priority levels. Furthermore, an efficient solver for HLSP based on the ADMM
is presented (Sec. 4). As a first-order method, the solver primarily relies on
matrix-vector multiplications instead of matrix factorizations as for the IPM.
This solver is efficient in approximating a solution of moderate accuracy with
a limited number of iterations with respect to its IPM equivalent. The
approximate primal guess then can be used to warm-start a high accuracy solver
as we demonstrate in Sec. 6.2.

###  2.3 Prioritized trajectory optimization

A specific form of NL-HLSP’s are prioritized non-linear trajectory
optimization problems of the form

$\displaystyle\operatorname*{min.}_{x_{T},v_{\cup l,t}}$
$\displaystyle\quad\frac{1}{2}\left\|{v}_{\cup l,t}\right\|^{2}_{2}\qquad
l=1,\dots,p$ (PTO) s.t. $\displaystyle\quad f_{l}(x_{s_{t}:e_{t}})\hskip
3.0pt\leqq\hskip 3.0pt{v}_{l,t}\qquad t=0,\dots,T$
$\displaystyle\quad{f}_{\cup l-1}(x_{s_{t}:e_{t}})\hskip 3.0pt\leqq\hskip
3.0pt{v}_{\cup l-1,t}^{*}$

Here, ${f}_{\cup l-1}$ represents equality and inequality constraints of lower
priority levels $1$ to $l-1$, which is indicated by the symbol $\leqq$ (note
that in the case of inactive inequality constraints ${\mathcal{I}}_{\cup
l-1}$, we have ${v}_{\cup l-1,t}^{*}=0$). $T$ is the length of the control
horizon. The individual time steps $t=0,\dots,T$ are also referred to as
stages. Constraints only depend on specific variable segments / intervals
$x_{{s_{t}:e_{t}}}\coloneqq x\left[s_{t_{0}}:e_{t_{1}}\right]$. The indices
$s_{t_{0}}\leq e_{t_{1}}$ with $t_{0}\leq t_{1}$ are the start and end indices
of the segments in $x$ corresponding to time steps $t_{0}$ and $t_{1}$. The
constraint Jacobians $J$ therefore exhibit a banded structure as follows

$\displaystyle
J=\begin{bmatrix}\nabla_{x_{\left[0\right]}}f(x_{s_{0}:e_{1}})&\nabla_{x_{\left[1\right]}}f(x_{s_{0}:e_{1}})&\cdots&0\\\
0&\nabla_{x_{\left[1\right]}}f(x_{s_{1}:e_{2}})&\cdots&0\\\
\vdots&\vdots&\ddots&\vdots\\\
0&0&\cdots&\nabla_{x_{\left[T\right]}}f(x_{s_{T-1}:e_{T}})\end{bmatrix}$ (1)

$\left[t\right]$ indicates the interval $\left[s_{t}:e_{t}\right]$. This
optimal control problem structure handles multi-stage constraints (excluding
the dynamics), unlike DDP. Typically, we have some initial condition on parts
of the variable vector $x_{:}=x_{:,0}$ with constant $x_{:,0}$, which can be
seamlessly integrated as a high priority constraint.

Contribution: In this work, we present a sparse nullspace basis based on the
turnback algorithm for Euler integrated dynamics (Sec. 5). We provide an upper
bound on its bandwidth. This enables us to design an efficient turnback
algorithm which does not rely on a costly initial rank-revealing matrix
factorization. We demonstrate how the high degree of sparsity in the case of
full actuation can be transferred to the case of under-actuation

##  3 Hierarchical step-filter with adaptive threshold for second order
information

S-HLSP utilizes the hierarchical Newton’s method 5 (or the Quasi-Newton
equivalent 27) or the hierarchical Gauss-Newton algorithm to linearize NL-
HLSP. Switching between the two can be based on the principle that at
convergence, variables corresponding to infeasible constraints need to be
‘locked’ in the HLSP by a full rank Hessian in order to not disturb the
optimal infeasibility of the non-linear constraints. Furthermore, in robotics
constraint Jacobians in the HLSP are typically rank deficient at infeasible
points due to kinematic and algorithmic singularities 29. The Newton’s method
and its second-order information (SOI) then acts as a regularization and
enables a global solution to the HLSP. At the same time, deactivating SOI
promotes solution optimality. SOI is typically full-rank on the variables that
the corresponding constraints occupy. Therefore, these variables can not be
used any more for the resolution of lower priority levels. If SOI is
unnecessarily activated for feasible constraints, this results in less optimal
local minima for constraints on lower priority levels.

The switching method proposed in 27 adheres to the following strategy

$\displaystyle H_{l}$ $\displaystyle=J^{T}J+\text{SOI}_{l}\quad\text{ if
}\|\hat{v}_{l}\|_{2}\geq\epsilon\text{ (Newton's method)}$ (2) $\displaystyle
H_{l}$ $\displaystyle=J_{l}^{T}J_{l}\phantom{+SOIl}\quad\text{ otherwise
(Gauss-Newton algorithm) }$

SOI is defined as

$\text{SOI}_{l}\coloneqq\hat{H}_{l}=R_{l}^{T}R_{l}$ (3)

$\hat{H}_{l}$ represents some positive definite regularization (for example
Higham 30 or symmetric Schur regularization 31, Broyden-Fletcher-Goldfarb-
Shanno algorithm (BFGS) 32, weighted identity matrices 33, …) of the
hierarchical Lagrangian Hessian of a level $l$ 5. It involves SOI and
approximate Lagrange multipliers of the levels 1 to $l$. $\epsilon$ is a
constant threshold on the linear slacks $\hat{v}$ as an indicator for
constraint infeasibility. The linear slacks $\hat{v}$ capture well-posed /
compatible HLSP sub-problems and enable SOI deactivation even if an iterate
$x^{k}\neq x^{*}$. In contrast, the non-linear slack $v_{l}$ of feasible
constraints only vanishes at convergence $x^{*}$. Here, we propose an adaptive
strategy for the SOI augmentation thresholds $\epsilon_{adaptive,l}$ of each
level $l$ in (2), see Sec. 3.2. This avoids manual tuning of the SOI
activation threshold which is oftentimes necessary for HLSP sub-problem
solvers of different accuracy and in dependency of the problem configurations.
The method is based on the HSF for S-HLSP globalization, which is recalled in
Sec. 3.1.

###  3.1 The hierarchical step-filter

The HSF 6 based on the SQP step-filter 15 measures the progress in the
approximated HLSP sub-problems with respect to the original NL-HLSP. Each
filter $\mathcal{F}_{l}$ of the levels $l=1,\dots,p$ of the NL-HLSP consists
of pairs $(h_{\cup l-1}$, $\|f_{l}^{+}\|_{2}^{2})$ with

$h_{\cup l-1}(x_{k}+\Delta x_{k})=\|f_{{\mathbb{I}}_{\cup
l-1}}^{+}-v_{{\mathbb{I}}_{\cup l-1}}^{*}\|_{1}+\|f_{{\mathbb{E}}_{\cup
l-1}}-v_{{\mathbb{E}}_{\cup l-1}}^{*}\|_{1}$ (4)

Here

$f_{l}^{+}\coloneqq\begin{bmatrix}f_{{\mathbb{E}}_{l}}\\\
\max(0,f_{{\mathbb{I}}_{l}})\end{bmatrix}$ (5)

$h_{\cup l-1}$ reflects feasibility of the constraints while
$\|f_{l}^{+}(x_{k}+\Delta x_{k})\|_{2}^{2}$ indicates objective optimality. As
can be seen, we use the non-linear slacks $v_{l}=f_{l}^{+}(x_{k}+\Delta
x_{k})$ instead of the linear ones $\hat{v}_{l}$ from the HLSP.

A new point $h_{\cup l-1}(x_{k}+\Delta x_{k})$ and $\|f_{l}^{+}(x_{k}+\Delta
x_{k})\|_{2}^{2}$ resulting from a new primal step $\Delta x_{k}$ of the HLSP
sub-problem is acceptable to all filter points $j\in\mathcal{F}_{l}$ if
sufficient progress in feasibility or optimality has been achieved:

$\displaystyle h_{\cup l-1}\leq\beta h_{\cup
l-1}^{j}\qquad\text{or}\qquad\|f_{l}^{+}\|_{2}^{2}+\gamma h_{\cup
l-1}\leq\|f_{l}^{+j}\|_{2}^{2}$ (6)

$\beta$ is a value close to 1 and $\gamma$ is a value close to zero and adhere
to the condition $0<\gamma<\beta<1$. Since the model reliably represents the
non-linear problem, the trust region radius is increased. Otherwise, the step
is rejected and the trust region radius is reduced. The HSF of level $l$
converges once $\|\Delta x_{k}\|_{2}<\chi$ falls below the threshold $\chi$.
This process is repeated for each priority level $l=1,\dots,p$.

###  3.2 Adaptive SOI thresholding

Algorithm 1 adaptEps

1:Input: $\epsilon_{adaptive}$, $(h_{front},\|f^{+}\|^{2}_{2,front})$,
$(h,\|f^{+}\|^{2}_{2})$, $c$, accepted, $\kappa$, $\underline{\epsilon}$,
$\overline{\epsilon}$

2:Output: $\epsilon_{adaptive}$, $(h_{front},\|f^{+}\|^{2}_{2,front})$, $c$

3:if accepted then

4: if $h\leq
h_{front}\quad\&\quad\|f^{+}\|^{2}_{2}<\delta\|f^{+}\|^{2}_{2,front}$ then

5:
$\epsilon_{adaptive}\leftarrow\min(\epsilon_{adaptive}\cdot\kappa,\overline{\epsilon})$

6: $h_{front}=h$

7: $\|f^{+}\|^{2}_{2,front}=\|f^{+}\|^{2}_{2}$

8: $c=0$

9: end if

10:else if $c>\zeta$ then

11:
$\epsilon_{adaptive}\leftarrow\max(\epsilon_{adaptive}/\kappa,\underline{\epsilon})$

12:end if

13:$c\leftarrow c+1$

14:return $\epsilon_{adaptive}$, $(h_{front},\|f^{+}\|^{2}_{2,front})$, $c$

$\delta\|f^{+}_{l}\|_{2,front}^{2}$$h_{\cup_{l-1},front}$$\|f^{+}_{l}\|_{2,front}^{2}$$\|f_{l}^{+}\|_{2}^{2}$$h_{\cup
l-1}$

Figure 2: Iterates $(h_{\cup l-1}(x_{k}),\|f_{l}^{+}(x_{k})\|_{2}$ of level
$l$. A new filter front needs to lie within the shaded area.

The threshold adaptation strategy for $\epsilon_{adaptive,l}$ is outlined in
Alg. 1. On each level $l=1,\dots,p$ and at every outer iteration, the filter
front $(h_{\cup{l-1},front},\|f^{+}_{l}\|^{2}_{2,front})$ is updated by
adaptEps($\epsilon_{adaptive,l}$, $(h_{\cup
l-1,front},\|f^{+}_{l}\|^{2}_{2,front})$, $(h_{\cup
l-1},\|f^{+}_{l}\|^{2}_{2})$, $c_{l}$, $\text{accepted}_{l}$, $\kappa$,
$\underline{\epsilon}$, $\overline{\epsilon}$). It represents the most optimal
point by the margin $\delta$ that has been encountered so far on a level $l$.

######  Definition 3.1 (Filter front).

A point $(h_{\cup{l-1},front},\|f^{+}_{l}\|^{2}_{2,front})$ is the front of a
filter $\mathcal{F}_{l}$ to degree $\delta\leq 1$, if it dominates all points
of previous iterates $x_{j}$ with $j=1,\dots k$ according to

$\displaystyle h_{\cup{l-1},front}\leq
h^{j}_{\cup{l-1}}\quad\text{and}\quad\|f^{+}_{l}\|^{2}_{2,front}<\delta\|f^{+j}_{l}\|_{2}^{2}\quad$
(7)

An exemplary visualization is given in Fig. 2. The choice $j=1,\dots k$ (and
not considering the filter elements $j\in\mathcal{F}_{l}$) is motivated by the
fact that a filter $\mathcal{F}_{l}$ does not include all iterates $x_{j}$
with $j=0,\dots,k$ due to a concept referred to as f-type iteration where the
focus is put on optimality of the HSF level $l$, see 15. Since this comes
possibly at the cost of increase in constraint violation $h_{\cup l-1}$, the
threshold adaptation takes place on all levels including the current HSF level
$l$. We reinitialize the filter front of each level $i=1,\dots,p$ at the start
of the step filter of a level $l$ by
$(h_{\cup{l-1}}(x_{l-1}^{*}),\|f^{+}_{l}(x_{l-1}^{*})\|^{2}_{2})$.
$x_{l-1}^{*}$ is the primal obtained at the KKT point of the previous level
$l-1$. This handles cases where $x_{0}$ is a feasible ($f^{+}_{l}=0$), but
$x_{l-1}^{*}$ is an infeasible point ($f^{+}_{l}\neq 0$) to constraints, since
otherwise no other filter front can be identified (since the condition
$\|f^{+}_{l}\|^{2}_{2}<\|f^{+}_{l}\|_{2,front}^{2}=0$ would need to be
fulfilled).

The SOI augmentation threshold is relaxed / increased by a factor $\kappa>1$
if a sub-step leads to a new filter front. For one, progress towards
feasibility $\|f^{+}_{l}\|_{2}^{2}=0$ is required. This is ensured by the
degree $\delta\leq 1$. At the same time, the condition $h_{\cup{l-1}}\leq
h_{\cup{l-1},front}$ ensures that the current SOI sufficiently encapsulates
SOI from constraints of previous levels $f^{+}_{\cup l-1}$ (since otherwise
the ill-posed HLSP sub-step may increase constraint violation
$h_{\cup{l-1}}$).

On the other hand, the adaptation strategy tightens / decreases the threshold
if a step is rejected according to (6). Furthermore, $\zeta$ steps must have
been accepted with the current SOI threshold. This promotes trust-region
reductions without SOI augmentation in order to escape local regularized
minima. This is motivated by the analogies between trust-region methods and
the Levenberg-Marquardt method 33 (smaller trust-region radius relates to
higher regularization, whereby we prefer smaller trust-region radii without
regularization / SOI).

The described procedure leads to a more moderate and slower adaptation
strategy than for example directly coupling SOI activations to step
acceptances and rejections. At the same time, the proposed heuristic for
adapting the SOI threshold gives no guarantee that
$0<\|\hat{v}_{l}\|_{2}^{2}=\|{v}_{l}\|_{2}^{2}<\epsilon_{adaptive,l}$ holds at
an optimally infeasible KKT point $x^{*}$. This can be explained by the fact
that by virtue of the trust-region constraint, such a first-order point
$x^{*}$ can always be obtained (the SQP filter convergence proof in 15 only
requires the norm of the Hessian $\|\hat{H}_{l}\|_{2}$ to be bounded above;
this is given for example for physically consistent systems like robots with
bounded Jacobians; no rank requirements are made). Nonetheless, we show on
test functions (Sec. 6.2) that SOI is reliably activated for infeasible
constraints even if the initial threshold is chosen far away.

##  4 Alternating direction method of multipliers for HLSP

HLSP solvers both based on the active-set method 3 and the interior-point
method 34 have been proposed. While the former is very efficient with little
changes of the active-set due to warm-starting capabilities, the latter one
exhibits numerical stability and constant computation times even in the case
of ill-posed problem formulations. However, both methods rely on expensive
matrix factorizations in every inner iteration. In recent years, the ADMM for
solving constrained optimization problems has seen a sharp rise in popularity,
for example in distribute optimization 35. Here, we outline an ADMM for HLSP
which mostly relies on matrix-vector operations. Such first order methods
typically approach a solution of moderate accuracy in few iterations 13. The
proposed solver $\mathcal{N}$ADM2 is based on nullspace projections of active
constraints as described in Sec. 4. We detail our choice of the step-size
parameter (Sec. 4.2) and our warm-starting strategy (Sec. 4.3). Our
derivations are finally concluded with some considerations regarding the
computation of the Lagrange multipliers of the active constraints
${\mathcal{A}}_{\cup l-1}$ (Sec. 4.4).

###  4.1 Reduced Hessian based ADMM for HLSP

Following the approach in 36, which first proposed an ADMM based on the
reduced Hessian, we introduce the change of variables

$\displaystyle\Delta x_{l}=\Delta x_{l-1}^{*}+N_{l-1}\Delta z_{l}$ (8)

with the overall primal solution

$\displaystyle\Delta x^{*}=\sum_{l=1}^{p}N_{l-1}\Delta z_{l}^{*}$ (9)

$N_{l-1}$ is a basis of the nullspace of the active constraints
${\mathcal{A}}_{\cup l-1}$ such that

$A_{{\mathcal{A}}_{\cup l-1}}N_{l-1}=0$ (10)

and $N_{0}=I_{n\times n}$. The particular solution $\Delta x_{l-1}^{*}$ (with
$\Delta x_{0}=0$) fulfills the condition $A_{{\mathcal{A}}_{\cup l-1}}\Delta
x_{l-1}^{*}-b_{{\mathcal{A}}_{\cup l-1}}-v_{{\mathcal{A}}_{\cup l-1}}^{*}=0$
(and which is obtained during the resolution of the higher priority levels).
With appropriate choice of the nullspace basis $N$, this leads to either a
decrease in variables (dense programming) or non-zeros (sparse programming).
In this work, we rely on the turnback algorithm for the computation of sparse
nullspace basis of banded matrices, see Sec. 5.

The change of variables leads to the following projected optimization problem,
where $\tilde{A}$ is the projected variable $\tilde{A}=AN$

$\displaystyle\operatorname*{min.}_{\begin{subarray}{c}\Delta
z_{l},\Delta\hat{z}_{l},v_{\mathbb{E}_{l}},\\\
v_{\mathbb{I}_{l}},w_{\mathbb{I}_{l}},w_{{\mathcal{I}}_{\cup
l-1}}\end{subarray}}$
$\displaystyle\quad\frac{1}{2}\|v_{\mathbb{E}_{l}}\|^{2}_{2}+\frac{1}{2}v_{\mathbb{I}_{l}}^{T}\|^{2}_{2}$
s.t. $\displaystyle\quad\tilde{A}_{\mathbb{E}_{l}}\Delta
z_{l}-_{\mathbb{E}_{l}}=v_{\mathbb{E}_{l}}\qquad l=1,\dots,p$
$\displaystyle\quad\tilde{A}_{\mathbb{I}_{l}}\Delta
z_{l}-_{\mathbb{I}_{l}}\leq v_{\mathbb{I}_{l}}$
$\displaystyle\quad\tilde{A}_{{\mathcal{I}}_{\cup l-1}}\Delta
z_{l}-_{{\mathcal{I}}_{\cup l-1}}\leq 0$ (11)

The vector $\breve{b}_{\Xi}$ represents the expression

$\displaystyle\breve{b}_{\Xi_{l}}\coloneqq b_{\Xi_{l}}-A_{\Xi_{l}}\Delta
x_{l-1}^{*}$ (12)

with the corresponding indices
${\Xi}_{l}=\\{{\mathbb{E}}_{l},{\mathbb{I}}_{l},{\mathcal{I}}_{\cup{l-1}}\\}$.

We introduce the slack variables $w_{{\mathcal{I}}_{\cup l-1}}$ and
$w_{\mathbb{I}_{l}}$, similarly to 34. Furthermore, the auxiliary variable
$\Delta\hat{z}_{l}$ is added to the problem as in 37. The HLSP then writes as

$\displaystyle\operatorname*{min.}_{\begin{subarray}{c}\Delta
z_{l},\Delta\hat{z}_{l},v_{\mathbb{E}_{l}},\\\
v_{\mathbb{I}_{l}},w_{\mathbb{I}_{l}},w_{{\mathcal{I}}_{\cup
l-1}}\end{subarray}}$
$\displaystyle\quad\frac{1}{2}\|\begin{bmatrix}v_{\mathbb{E}_{l}}^{T}&v_{\mathbb{I}_{l}}^{T}\end{bmatrix}^{T}\|^{2}_{2}+\mathrm{I}_{+}(w_{\mathbb{I}_{l}})+\mathrm{I}_{+}(w_{{\mathcal{I}}_{\cup
l-1}})$ s.t. $\displaystyle\quad\tilde{A}_{\mathbb{E}_{l}}\Delta
z_{l}-_{\mathbb{E}_{l}}=v_{\mathbb{E}_{l}}\qquad l=1,\dots,p$
$\displaystyle\quad\tilde{A}_{\mathbb{I}_{l}}\Delta
z_{l}-_{\mathbb{I}_{l}}=v_{\mathbb{I}_{l}}+w_{\mathbb{I}_{l}}$
$\displaystyle\quad\tilde{A}_{{\mathcal{I}}_{\cup l-1}}\Delta
z_{l}-_{{\mathcal{I}}_{\cup l-1}}=w_{{\mathcal{I}}_{\cup l-1}}$
$\displaystyle\quad\Delta\hat{z}_{l}=\Delta z_{l}$ (13)

The slacks are penalized for negative values by the non-smooth indicator
function $\mathrm{I}_{+}$

$\displaystyle\mathrm{I}_{+}(w_{\Psi_{l}})=\begin{bmatrix}0\quad
w_{\Psi_{l}}\geq 0\\\ +\infty\quad\text{otherwise}\end{bmatrix}$ (14)

where

$\Psi_{l}=\\{{\mathcal{I}}_{\cup l-1},\mathbb{I}_{l}\\}$ (15)

The augmented Lagrangian of level $l$ of (11) writes as

$\displaystyle\tilde{\mathcal{L}}_{l}(\Delta
z_{l},\Delta\hat{z}_{l},v_{\mathbb{E}_{l}},v_{\mathbb{I}_{l}},w_{\mathbb{I}_{l}},w_{{\mathcal{I}}_{\cup
l-1}})=\frac{1}{2}\|v_{\mathbb{E}_{l}}\|^{2}_{2}+\frac{1}{2}\|v_{\mathbb{I}_{l}}\|^{2}_{2}+\mathrm{I}_{+}(w_{\mathbb{I}_{l}})+\mathrm{I}_{+}(w_{{\mathcal{I}}_{\cup
l-1}})+\frac{\rho_{\mathbb{E}_{l}}}{2}\|\tilde{A}_{\mathbb{E}_{l}}\Delta
z_{l}-_{\mathbb{E}_{l}}-v_{\mathbb{E}_{l}}+\upsilon_{\mathbb{E}_{l}}\|_{2}^{2}$
(16) $\displaystyle+$
$\displaystyle\frac{\rho_{{\mathbb{I}}_{l}}}{2}\|\tilde{A}_{\mathbb{I}_{l}}\Delta
z_{l}-_{\mathbb{I}_{l}}-v_{\mathbb{I}_{l}}-w_{\mathbb{I}_{l}}+\upsilon_{\mathbb{I}_{l}}\|_{2}^{2}+\frac{\rho_{l}}{2}\|\tilde{A}_{{\mathcal{I}}_{\cup
l-1}}\Delta z_{l}-_{{\mathcal{I}}_{\cup l-1}}-w_{{\mathcal{I}}_{\cup
l-1}}+\upsilon_{{\mathcal{I}}_{\cup
l-1}}\|_{2}^{2}+\frac{\sigma}{2}\|\Delta\hat{z}_{l}-\Delta
z_{l}+\sigma^{-1}\lambda_{\Delta z_{l}}\|^{2}_{2}$

where

$\displaystyle\upsilon\coloneqq\frac{1}{\rho}\lambda$ (17)

The step-size parameters $\sigma>0$ and $\rho$, the distinctions and choices
$\rho_{{\mathbb{E}}_{l}}\rightarrow\infty$ and
$\rho_{\mathbb{I}_{l}}=\rho_{l}$ are further explained in Sec. 4.2. $\lambda$
are the Lagrange multipliers associated with the corresponding problem
constraints $\Xi$. Resulting from the Karush-Kuhn-Tucker (KKT) first order
optimality conditions $\tilde{K}_{v_{\mathbb{E}_{l}}}=0$ and
$\tilde{K}_{v_{\mathbb{I}_{l}}}=0$ (with
$\tilde{K}\coloneqq\nabla\tilde{\mathcal{L}}$), we obtain the primal
substitutions

$\displaystyle v_{\mathbb{E}_{l}}$
$\displaystyle=\tilde{A}_{\mathbb{E}_{l}}\Delta
z_{l}-_{\mathbb{E}_{l}}+\upsilon_{\mathbb{E}_{l}}$ (18) $\displaystyle
v_{\mathbb{I}_{l}}$
$\displaystyle=\frac{\rho_{l}}{1+\rho_{l}}(\tilde{A}_{\mathbb{I}_{l}}\Delta
z_{l}-_{\mathbb{I}_{l}}-w_{\mathbb{I}_{l}}+\upsilon_{\mathbb{I}_{l}})$ (19)

We then successively compute the alternating steps

$\displaystyle\Delta\hat{z}^{k+1}_{l}$
$\displaystyle\leftarrow\operatorname*{arg\,min}_{\hat{z}_{l}}\tilde{\mathcal{L}}_{l}(\Delta
z_{l},\Delta\hat{z}_{l},v_{\mathbb{E}_{l}},v_{\mathbb{I}_{l}},w_{\mathbb{I}_{l}},w_{{\mathcal{I}}_{\cup
l-1}})$ (20) $\displaystyle\Delta z^{k+1}_{l}$
$\displaystyle\leftarrow\alpha\Delta\hat{z}^{k+1}+(1-\alpha)\Delta z^{k}$ (21)
$\displaystyle v^{k+1}_{\mathbb{E}_{l}}$
$\displaystyle\leftarrow\eqref{eq:subvel}$ (22) $\displaystyle
v^{k+1}_{\mathbb{I}_{l}}$ $\displaystyle\leftarrow\eqref{eq:subvil}$ (23)
$\displaystyle\hat{w}_{{\mathbb{I}}_{l}}^{k+1}$
$\displaystyle\leftarrow\tilde{A}_{{\mathbb{I}}_{l}}\Delta\hat{z}^{k+1}-v^{k+1}_{{\mathbb{I}}_{l}}$
(24) $\displaystyle\hat{w}_{{\mathcal{I}}_{\cup l-1}}^{k+1}$
$\displaystyle\leftarrow\tilde{A}_{{\mathcal{I}}_{\cup
l-1}}\Delta\hat{z}^{k+1}$ (25) $\displaystyle w^{k+1}_{\Psi}$
$\displaystyle\leftarrow\max(_{\Psi},\alpha\hat{w}_{\Psi}^{k+1}+(1-\alpha)w_{\Psi}^{k}+\upsilon_{\Psi}^{k})$
(26) $\displaystyle\upsilon^{k+1}_{\Psi}$ $\displaystyle\leftarrow
u^{k}_{\Psi}+\alpha\hat{w}_{\Psi}^{k+1}+(1-\alpha)w_{\Psi}^{k}-w^{k+1}_{\Psi}$
(27) $\displaystyle\upsilon^{k+1}_{{\mathbb{E}}_{l}}$ $\displaystyle\leftarrow
u^{k}_{{\mathbb{E}}_{l}}+\alpha(\tilde{A}_{\mathbb{E}_{l}}\Delta\hat{z}^{k+1}-v^{k+1}_{{\mathbb{E}}_{l}})+(1-\alpha)_{{\mathbb{E}}_{l}}-_{{\mathbb{E}}_{l}}$
(28)

The parameter $\alpha\in(0,2)$ is the over-relaxation parameter (typically
$\alpha=1.6$) 38. For the computation of the primal $\hat{z}^{k+1}$, we
consider the optimality condition $\tilde{K}_{\hat{z}_{l}}=0$ which leads to
the expression

$C_{l}\Delta\hat{z}^{k+1}_{l}=r_{l}$ (29)

The positive definite matrix $C_{l}$ is defined as

$\displaystyle C_{l}$
$\displaystyle=\tilde{A}_{{\mathbb{E}}_{l}}^{T}\tilde{A}_{{\mathbb{E}}_{l}}+\frac{\rho_{l}}{1+\rho_{l}}\tilde{A}_{{\mathbb{I}}_{l}}^{T}\tilde{A}_{{\mathbb{I}}_{l}}+\rho_{l}\tilde{A}_{{\mathcal{I}}_{\cup
l-1}}^{T}\tilde{A}_{{\mathcal{I}}_{\cup l-1}}+\sigma I$ (30)

$I$ is an identity matrix. The right hand side writes as

$\displaystyle r_{l}=$
$\displaystyle\tilde{A}_{{\mathbb{E}}_{l}}^{T}(_{{\mathbb{E}}_{l}}-\upsilon_{{\mathbb{E}}_{l}})+\frac{\rho_{l}}{1+\rho_{l}}\tilde{A}_{{\mathbb{I}}_{l}}^{T}(_{{\mathbb{I}}_{l}}+w_{{\mathbb{I}}_{l}}-\upsilon_{\mathbb{I}_{l}})+{\rho_{l}}\tilde{A}_{{\mathcal{I}}_{\cup
l-1}}^{T}(_{{\mathcal{I}}_{\cup l-1}}+w_{{\mathcal{I}}_{\cup
l-1}}-\upsilon_{{\mathcal{I}}_{\cup l-1}})+\sigma z^{k}_{l}$ (31)

Once the alternating steps of level $l$ have converged with
$\|\tilde{K}_{l}\|_{2}<\eta$, the active constraint sets
${\mathcal{A}}_{l^{*}}$ and ${\mathcal{A}}_{l}$ corresponding to
${\mathcal{I}}_{\cup l-1}$ and ${\mathbb{I}}_{l}$ need to be composed. $\eta$
is a positive numerical threshold. The level $l^{*}$ is referred to as
‘virtual’ priority level and maintains the prioritization between active sets
of ${\mathcal{I}}_{\cup l-1}$ and ${\mathbb{I}}_{l}$ 34. We use the following
decision criteria to determine active constraints

$\displaystyle w_{{\mathcal{I}}_{\cup l-1}}<\nu$
$\displaystyle\qquad\text{and}\qquad\lambda_{{\mathcal{I}}_{\cup l-1}}>\nu$
(32) $\displaystyle w_{\mathbb{I}_{l}}<\nu$
$\displaystyle\qquad\text{and}\qquad v_{\mathbb{I}_{l}}<-\nu$ (33)

The resolution of the HLSP is then continued with the ADMM of the next level
$l+1$ projected into the nullspace $N_{l}$ of the new active set
${\mathcal{A}}_{\cup l}={\mathcal{A}}_{\cup l-1}\cup{\mathcal{A}}_{l}$. The
remaining inactive constraints are contained in the updated inactive set
${\mathcal{I}}_{\cup l}$.

###  4.2 Choice of the step-size parameters $\rho$

Since equality constraints ${\mathbb{E}}_{l}$ are necessarily active at
convergence, we choose $\rho_{{\mathbb{E}}_{l}}\rightarrow\infty$ 39. It can
be seen that this leads to a more efficient algorithm since the dual update
$\upsilon_{{\mathbb{E}}_{l}}$ (28) is zero and therefore does not need to be
computed.

Similarly, the choice $\rho_{{\mathbb{I}}_{l}}\rightarrow\infty$ for the
inequality constraints ${\mathbb{I}}_{l}$ would render its corresponding
equation in (27) obsolete. In this case, the inequality constraints are
treated as equalities. Consequently, at ADMM convergence, feasible inequality
constraints are saturated (with
$\tilde{A}_{{\mathbb{I}}_{l}}z_{l}-_{{\mathbb{I}}_{l}}=0$) and infeasible
constraints are active ($v_{{\mathbb{I}}_{l}}<0$). However, we noticed that
this leads to increased and unnecessary constraint activations due to the
limited convergence accuracy of the ADMM (see also Sec. 4.3). Instead, we set
the step-size parameter $\rho_{{\mathbb{I}}_{l}}=\rho_{l}$ according to 37.

###  4.3 Warm-starting HLSP’s

Oftentimes, a slowly evolving sequence of programs (parametric program) needs
to be resolved, for example in the context of S-HLSP (see Sec. 2.2). In this
case, and in contrast to interior-point methods, the ADMM can be easily warm-
started, i.e., a good initial guess for the primal and dual variables reduces
the number of alternating iterations until convergence. We store the optimal
primal and dual values $z_{l}^{*}$, $w_{\Psi_{l}}^{*}$ and
$\upsilon_{\Psi_{l}}^{*}$ and the step-size parameter $\rho^{*}_{l}$ after
convergence of each level $l=1,\dots,p$. In the next problem instance, the
primal and dual variables are then warm-started with these values. If exactly
the same HLSP is solved, our algorithm therefore converges as expected with
zero iterations with $\Delta x_{k+1}^{*}=\Delta
x^{*}_{k}=\sum_{l=1}^{p}N_{l-1}\Delta z_{l,k}^{*}$ (9).

Nonetheless, we observed that by warm-starting the primal and dual variables,
constraints previously activated tend to be activated again in the next
iteration. Potentially, this is caused by the inherently moderate accuracy of
ADMM. This can artificially delay convergence of outer methods like S-HLSP if
these constraints are not actually active in the corresponding non-linear
program. We therefore reset the primal and dual sub-steps to zero in every new
HLSP instance. One argumentation for this procedure is that at S-HLSP
convergence, the primal sub-step $\|\Delta x\|_{2}\leq\chi$ vanishes and
therefore poses a good initial guess when a non-linear parametric program is
solved.

###  4.4 Lagrange multipliers of active constraints

Considering the dual ascent step $\upsilon_{{\mathcal{A}}_{\cup
l-1}}=\upsilon_{{\mathcal{A}}_{\cup
l-1}}-\nabla_{\upsilon_{{\mathcal{A}}_{\cup l-1}}}\mathcal{L}$ for the update
of the Lagrange multipliers $\upsilon_{{\mathcal{A}}_{\cup l-1}}$, we can see
that the gradient $\nabla_{\upsilon_{{\mathcal{A}}_{\cup l-1}}}\mathcal{L}=0$
since $A_{{\mathcal{A}}_{\cup l-1}}\Delta x_{0}-b_{{\mathcal{A}}_{\cup
l-1}}-v_{{\mathcal{A}}_{\cup l-1}}^{*}=0$ and $A_{{\mathcal{A}}_{\cup
l-1}}(\Delta x_{0}+N_{l-1}\Delta z)-b_{{\mathcal{A}}_{\cup
l-1}}-v_{{\mathcal{A}}_{\cup l-1}}^{*}=A_{{\mathcal{A}}_{\cup
l-1}}N_{l-1}\Delta z=0$ as well. Therefore, the Lagrange multipliers
associated with the active constraints ${{\mathcal{A}}_{\cup l-1}}$ (and whose
nullspace the problem of level $l$ is projected into) are not updated.

As noted in 34, the Lagrange multipliers of the active constraints are not
necessary as none of the other primal or dual variables depend on it. However,
the Lagrange multipliers may be needed within a non-linear solver based on
Newton’s method. Here, the Lagrange multipliers are used for the hierarchical
Hessian. We use a fast conjugate gradient method to compute the Lagrange
multipliers if required by the non-linear solver. In case that we use the
turnback nullspace bases (see Sec. 5), we use the $L$ factor of the LU
decomposition of $A_{{\mathcal{A}}_{\cup l-1}}$ for preconditioning the CG
algorithm for accelerated convergence. Note that with the choice of other
nullspace basis (for example based on the QR decomposition), matrix
factorizations can be re-used for efficient computation of the Lagrange
multipliers 5.

##  5 Turnback algorithm for Euler integrated dynamics

One critical element of the above nullspace method based HLSP solver is to
efficiently compute a basis of the nullspace of the active constraints. The
appropriate choice of the nullspace basis $N$ leads to either a decrease in
variables (dense programming) or non-zeros (sparse programming). In this work,
we rely on the turnback algorithm for the computation of sparse nullspace
basis for banded matrices, which arise in discrete optimal control problems
40. The main computational step of the turnback algorithm is to determine
linearly independent subsets in the matrix $A$, to which a certain number of
columns of $A$ is linearly dependent. These columns are then used to compute a
basis of the nullspace. Additionally, in our desired context of PTO, it is
important to preserve resulting banded structures of the constraints as much
as possible. The turnback algorithm is able to do so by considering nullspace
vectors which are computed with respect to subsets of the block diagonal
matrix instead of the whole one. In this work, we introduce some computational
shortcuts to the turnback algorithm tailored to dynamics discretized by Euler
integration. Importantly, we avoid a costly initial rank-revealing matrix
factorization.

First, we formulate our system dynamics discretized by Euler integration (Sec.
5.1). We then outline the algorithmic details of the original turnback
algorithm (Sec. 5.2). It is based on identifying linearly independent column
subsets in the matrix $A$. In Sec. 5.3, we show how to identify these subsets
in the case of Euler integrated dynamics and derive an upper bound on the
number of columns in the subsets. This enables us in Sec. 5.4 to design an
efficient turnback algorithm without the need of an expensive initial rank-
revealing matrix factorization. Finally, we address the full-rank property of
the resulting basis of nullspace (Sec. 5.5), demonstrate how the high degree
of sparsity in the case of full actuation can be transferred to the case of
under-actuation (Sec. 5.6) and comment on the parallelization of our algorithm
(Sec. 5.7).

###  5.1 Euler integrated dynamics

$\displaystyle\nabla_{x}f_{dyn}=$

$\left[\begin{array}[]{@{}c|cc|cc|cc|cc|cc|cc|cc|cc|cc|cc|c@{}}\ddots&&&&&&&&&&&&\\\
\hline\cr\ldots&&&\bm{E}_{3,t}&\pagecolor{yellow!25}\bm{E}_{4,t}&&&&&&&&&\\\
\ldots&&F_{t}^{ua}&\pagecolor{yellow!25}D_{3,t}^{ua}&D_{4,t}^{ua}&&&&&&&&\\\
\ldots&\pagecolor{blue!25}\bm{B}_{t}&F_{t}&\pagecolor{yellow!25}D_{3,t}&D_{4,t}&&&&&&&&\\\
\hline\cr&&&\bm{E}_{1,t}&\pagecolor{orange!25}\bm{E}_{2,t}&&&\bm{E}_{3,t+1}&\pagecolor{yellow!25}\bm{E}_{4,t+1}&&&&&\\\
&&&\pagecolor{orange!25}D_{1,t}^{ua}&D_{2,t}^{ua}&&F_{t+1}^{ua}&\pagecolor{yellow!25}D_{3,t+1}^{ua}&D_{4,t+1}^{ua}&&&&&\\\
&&&\pagecolor{orange!25}D_{1,t}&D_{2,t}&\pagecolor{blue!25}\bm{B}_{t+1}&F_{t+1}&\pagecolor{yellow!25}D_{3,t+1}&D_{4,t+1}&&&&&\\\
\hline\cr&&&&&&&\bm{E}_{1,t+1}&\pagecolor{orange!25}\bm{E}_{2,t+1}&&&\bm{E}_{3,t+2}&\pagecolor{yellow!25}\bm{E}_{4,t+2}&\\\
&&&&&&&\pagecolor{orange!25}D_{1,t+1}^{ua}&D_{2,t+1}^{ua}&&F_{t+2}^{ua}&\pagecolor{yellow!25}D_{3,t+2}^{ua}&D_{4,t+2}^{ua}\\\
&&&&&&&\pagecolor{orange!25}D_{1,t+1}&D_{2,t+1}&\pagecolor{blue!25}\bm{B}_{t+2}&F_{t+2}&\pagecolor{yellow!25}D_{3,t+2}&D_{4,t+2}&\\\
\hline\cr&&&&&&&&&&&\bm{E}_{1,t+2}&\pagecolor{orange!25}\bm{E}_{2,t+2}\\\
&&&&&&&&&&&\vdots&\vdots&\ddots\end{array}\right]$

(DED) $\displaystyle P_{t}^{T}\nabla_{x}f_{dyn}Q_{t}^{T}=$

$\left[\begin{array}[]{@{}cc|cc|cc||cccc||cc|c@{}}\pagecolor{blue!25}\bm{B}_{t}&&&&&&{D}_{4,t}&F_{t}&&&&\\\
&\bm{E}_{3,t}&\hphantom{I}&&&&&&&&&\\\
\hline\cr&\pagecolor{orange!25}D_{1,t}&\pagecolor{blue!25}\bm{B}_{t+1}&&&{D}_{4,t+1}&D_{2,t}&&F_{t+1}&&&&\\\
&\bm{E}_{1,t}&&\bm{E}_{3,t+1}&&&\pagecolor{orange!25}\bm{E}_{2,t}&&&&&\\\
\hline\cr&&&\pagecolor{orange!25}D_{1,t+1}&\pagecolor{blue!25}\bm{B}_{t+2}&D_{2,t+1}&&&&F_{t+2}&&D_{4,t+2}&\\\
&&&\bm{E}_{1,t+1}&&\pagecolor{orange!25}\bm{E}_{2,t+1}&&&&&\bm{E}_{3,t+2}&&\\\
\hline\cr&&&&&&{D}_{4,t}^{ua}&F_{t}^{ua}&&&&\\\
&\pagecolor{orange!25}D_{1,t}^{ua}&&&&D_{4,t+1}^{ua}&D_{2,t}^{ua}&&F_{t+1}^{ua}&&&\\\
&&&\pagecolor{orange!25}D_{1,t+1}^{ua}&&D_{2,t+1}^{ua}&&&&F_{t+2}^{ua}&&D_{4,t+2}^{ua}\\\
\hline\cr&&&&&&&&&&\bm{E}_{1,t+2}&\pagecolor{orange!25}\bm{E}_{2,t+2}&\\\
&&&&&&&&&&\vdots&\vdots&\ddots\end{array}\right]$

(PDXED) $\displaystyle P_{t}^{T}\nabla_{x}f_{dyn}Q_{t}^{T}=$

$\left[\begin{array}[]{@{}cc|cc|cc|cc||cccccc||cc|c@{}}\pagecolor{blue!25}\bm{B}_{t}&D_{4,t}&&&&&&&\pagecolor{yellow!25}{D}_{3,t}&&F_{t}&&&&\\\
&\pagecolor{yellow!25}\bm{E}_{4,t}&\hphantom{I}&&&&&&\bm{E}_{3,t}&&&&&\\\
\hline\cr&&\pagecolor{blue!25}\bm{B}_{t+1}&D_{4,t+1}&&&&&&\pagecolor{yellow!25}{D}_{3,t+1}&&F_{t+1}&&&&\\\
&&&\pagecolor{yellow!25}\bm{E}_{4,t+1}&\hphantom{I}&&&&\bm{E}_{1,t}&\bm{E}_{3,t+1}&&&&&\\\
\hline\cr&&&D_{2,t+1}&\pagecolor{blue!25}\bm{B}_{t+2}&D_{4,t+2}&&\pagecolor{yellow!25}D_{3,t+2}&&&&&F_{t+2}&&&&\\\
&&&&&\pagecolor{yellow!25}\bm{E}_{4,t+2}&&\bm{E}_{3,t+2}&&\bm{E}_{1,t+1}&&&&\\\
\hline\cr&&&&&D_{2,t+2}&\pagecolor{blue!25}\bm{B}_{t+3}&&&&&&&F_{t+3}&\pagecolor{yellow!25}D_{3,t+3}&D_{4,t+3}&\\\
&&&&&&&\bm{E}_{1,t+2}&&&&&&&\bm{E}_{3,t+3}&\bm{E}_{4,t+3}&\\\
\hline\cr&{D}_{4,t}^{ua}&&&&&&&\pagecolor{yellow!25}{D}_{3,t}^{ua}&&F_{t}^{ua}&&&&\\\
&D_{2,t}^{ua}&&{D}_{4,t+1}^{ua}&&&&&&\pagecolor{yellow!25}{D}_{3,t}^{ua}&&F_{t+1}^{ua}&&&&\\\
&&&D_{2,t+1}^{ua}&&{D}_{4,t+2}^{ua}&&\pagecolor{yellow!25}D_{3,t+2}^{ua}&&&&&F_{t+2}^{ua}&&&\\\
&&&&&D_{2,t+2}^{ua}&&&&&&&&F_{t+3}^{ua}&\pagecolor{yellow!25}D_{3,t+3}^{ua}&D_{4,t+3}^{ua}\\\
\hline\cr&&&&&&&&&&&&&&\bm{E}_{1,t+3}&&\\\
&&&&&&&&&&&&&&\vdots&\vdots&\ddots\end{array}\right]$

(PDIED) Figure 3: Gradient and permuted gradients of the Euler integrated
dynamics. The top matrix shows the un-permuted case. Matrices which only
appear in the explicit case and in the implicit case are colored in orange and
in yellow, respectively. The control matrices $B$ are colored in blue.
Matrices of full column rank according to theorem 5.1 are printed in bold. The
middle and bottom matrices show the permuted subsets for $\mu=0$ in the
explicit and for $\mu=1$ in the implicit case, respectively. These permuted
column subsets are linearly independent to all other columns of
$\nabla_{x}f_{dyn}$.

The dynamics of a rigid-body system are described by the inverse dynamics
Newton-Euler equations 41

$\displaystyle\mathcal{I}\mathcal{D}(q,\dot{q},\tau,\gamma)\coloneqq
M\ddot{q}=S^{T}\tau-V(q,\dot{q})+J^{T}\gamma$ (72)

The joint torques $\tau\in\mathbb{R}^{n_{\tau}}$ and contact forces
$\gamma\in\mathbb{R}^{n_{\gamma}}$ are considered the input variables of the
system. $S\in\mathbb{R}^{n_{\tau}\times n_{q}}$ is a full-rank selection
matrix describing under-actuation of the system $n_{\tau}<n_{q}$. The joint
angles $q\in\mathbb{R}^{n_{q}}$, velocities
$\dot{q}\in\mathbb{R}^{n_{\dot{q}}}$ and accelerations
$\ddot{q}\in\mathbb{R}^{n_{\ddot{q}}}$ describe the system state.
$M(q)\in\mathbb{R}^{n_{q}\times n_{q}}$ is the whole-body inertia matrix.
$V(q,\dot{q})\in\mathbb{R}^{n_{q}}$ describes linear and non-linear force
effects like Coriolis, centrifugal, gravitational and frictional forces. The
Jacobian $J(q)\in\mathbb{R}^{n_{\gamma}\times n_{q}}$ is associated with the
contact points. It has been noted in 42 that the inverse dynamics form
(explicit joint torques) is computationally advantageous compared to the
forward dynamics equations (in contrast to explicit joint accelerations).

In the following, for visualization purposes, we introduce the change of
variables

$\tilde{q}=\frac{1}{\Delta t}q\qquad\text{and}\qquad\tilde{\tau}=\Delta t\tau$
(73)

The states $s\in\mathbb{R}^{Tn_{s}}$ (with $n_{s}=n_{q}+n_{\dot{q}}$) and
controls $u\in\mathbb{R}^{Tn_{u}}$ (with $n_{u}=n_{\tau}+n_{\gamma}$) are
defined as

$\displaystyle s=$
$\displaystyle\begin{bmatrix}\tilde{q}_{1}^{T}&\dot{q}_{1}^{T}&\cdots&\tilde{q}_{T}^{T}&\dot{q}_{T}^{T}\end{bmatrix}^{T}\quad\text{and}\quad
u=\begin{bmatrix}\tilde{\tau}_{0}^{T}&\gamma_{0}^{T}&\cdots&\tilde{\tau}_{T-1}^{T}&\gamma_{T-1}^{T}&\end{bmatrix}^{T}$
(74)

We assume known constant $\tilde{q}_{0}$ and $\dot{q}_{0}$.

We discretize the dynamics by the direct multiple-shooting method 43, namely
by Euler integration. The resulting Euler integrated dynamics (ED) write as

$\displaystyle f_{dyn}(t)$
$\displaystyle=\begin{bmatrix}f_{dyn,1}^{T}(t)&f_{dyn,2}^{T}(t)\end{bmatrix}^{T}\coloneqq
s_{t+1}-s_{t}-\Delta
t\dot{s}_{t(+1)}=\begin{bmatrix}\tilde{q}_{t+1}-\tilde{q}_{t}-\dot{q}_{t(+1)}\\\
L_{t}(\dot{q}_{t+1}-\dot{q}_{t})-\Delta
tG_{t}{\mathcal{I}\mathcal{D}}(q_{t(+1)},\dot{q}_{t(+1)},{\tau}_{t},\gamma_{t})\end{bmatrix}$
(ED)

We set $L_{t}\coloneqq M_{t}$, $L_{t}\coloneqq I$ and $G_{t}\coloneqq I$,
$G_{t}\coloneqq M_{t}^{-1}$ in the case of inverse and forward dynamics,
respectively. The index $(+1)$ indicates implicit Euler integrated dynamics.
In case of under-actuation $n_{\tau}<n_{\dot{q}}$, the corresponding degrees
of freedom (freely swinging pendulum or the ‘free-flyer’ / base of a humanoid
robot) are described in linear coordinates (and not for example with
quaternions) to facilitate the linear integration scheme above. For the
remainder of this work, we therefore assume $n_{q}=n_{\dot{q}}$. Gimbal lock
can be avoided for example as described in 5.

The derivatives of ${\mathcal{I}\mathcal{D}}$ with respect to $q$ and
$\dot{q}$ can be computed according to 44. Similarly, the first and second
order derivatives of a function $f(q)$ with respect to $\tilde{q}$ writes as

$\displaystyle\partial f(q)/\partial\tilde{q}$ $\displaystyle=\Delta t\partial
f(q)/\partial q$ (75) $\displaystyle\partial^{2}f(q)/\partial\tilde{q}^{2}$
$\displaystyle=\Delta t^{2}\partial^{2}f(q)/\partial q^{2}$ (76)

This results in the partial derivatives

$\displaystyle E_{1,t}\coloneqq$ $\displaystyle\frac{\partial
f_{dyn,1}(t)}{\partial{q}_{t}}=-I,\quad E_{2,t}\coloneqq\frac{\partial
f_{dyn,1}(t)}{\partial\dot{q}_{t}}=-I,\quad E_{3,t}\coloneqq\frac{\partial
f_{dyn,1}(t)}{\partial{q}_{t+1}}=I,\quad E_{4,t}\coloneqq\frac{\partial
f_{dyn,1}(t)}{\partial\dot{q}_{t+1}}=-I$ (77) $\displaystyle B_{t}\coloneqq$
$\displaystyle\frac{\partial
f_{dyn,2}}{\partial\tilde{\tau}_{t}}=\frac{\partial
f_{dyn,2}}{\partial{\tau}_{t}}\frac{\partial\tau_{t}}{\partial\tilde{\tau_{t}}}=-G_{t}S^{T},\quad
F_{t}\coloneqq\frac{\partial f_{dyn,2}(t)}{\partial\gamma_{t}}=G_{t}J_{t}^{T}$
(78) $\displaystyle D_{1,t}\coloneqq$ $\displaystyle\frac{\partial
f_{dyn,2}(t)}{\partial\tilde{q}_{t}}=\frac{\partial
f_{dyn,2}(t)}{\partial{q}_{t}}\frac{\partial
q_{t}}{\partial\tilde{q}_{t}}=\frac{\partial f_{dyn,2}(t)}{\partial
q_{t}}\Delta t,\quad D_{2,t}\coloneqq\frac{\partial
f_{dyn,2}(t)}{\partial\dot{q}_{t}}=L_{t}\hskip 1.0pt(-\Delta t\cdots)$ (79)
$\displaystyle D_{3,t}\coloneqq$ $\displaystyle\frac{\partial
f_{dyn,2}(t)}{\partial{\tilde{q}}_{t+1}},\quad D_{4,t}\coloneqq\frac{\partial
f_{dyn,2}(t)}{\partial\dot{q}_{t+1}}=L_{t}\hskip 1.0pt(-\Delta t\cdots)$ (80)

It can be observed that due to the substitutions (74), $\Delta t$ does not
appear as denominator. This is numerically advantageous for small time steps
$\Delta t\ll 1$ s due to better matrix conditioning. Ruiz equilibration
$\hat{A}=S_{l}AS_{r}$ 45 can equally be employed but comes at a higher
computational cost. The nullspace basis of the original matrix $A$ becomes
$Z=S_{r}\hat{Z}$ with $\hat{A}\hat{Z}=0$.

###  5.2 Turnback algorithm

The turnback algorithm based on the LU decomposition to compute a nullspace
basis for a banded matrix $A\in\mathbb{R}^{m\times n}$ consists of the
following steps 8:

  1. 1.

Compute rank revealing $P^{T}LUQ^{T}$ decomposition of $A$ (rank $r_{A})$.
Then, $r_{Z}=n-r_{A}$.

  2. 2.

Determine the index vector $b\in\mathbb{R}^{r_{Z}}$, which indicates the first
non-zero entry of each column of

$Z_{LU}=Q\begin{bmatrix}-U_{1}^{-1}U_{2}\\\ I\end{bmatrix}$ (81)

$Z$ is upper block triangular due to the block-diagonal structure of $A$.

  3. 3.

Determine the turnback pivot columns $\pi\in\mathbb{R}^{r_{Z}}$. They are the
row indices of the permuted identity matrix in (81).

  4. 4.

For each index $i=1,\dots,r_{Z}$ in $b$, add columns to the sub-matrix
$G_{i}\in\mathbb{R}^{n\times r_{Z}}$ to the right of column $b_{i}$ of $A$
until linear dependency is detected. The turnback pivot column $\pi(i)$ is not
added to the sub-matrix.

  5. 5.

Compute the null-vector

$\displaystyle z_{i}=Q\begin{bmatrix}U_{1}^{-1}u_{2}\\\ 0_{\pi(i)-r_{A}-1}\\\
1\\\ 0_{n-\pi(i)}\end{bmatrix}$ (82)

$Q$, $U_{1}$ and $u_{2}$ result from the LU decomposition of the sub-matrix
$G_{i}$. $u_{2}$ corresponds to the column $\pi(i)$ of $A$.

The resulting turnback nullspace basis is full-rank since each pivot-column is
chosen only once during the submatrix augmentation and therefore has a similar
structure to (81) with a permuted identity matrix ensuring full column rank.

###  5.3 Subset determination for turnback algorithm

In the following, we derive a conservative bound for the number of columns
which are needed for linearly independent sub-sets of the Euler integrated
dynamics. We structure the permuted matrices (PDXED) and (PDIED) as

$G_{t}\coloneqq\left[\begin{array}[]{@{}c||c@{}}G_{1,t}&G_{2,t}\\\ \hline\cr
G_{1,t}^{ua}&G_{2,t}^{ua}\\\ \end{array}\right]$ (83)

The operator $\left\lceil a\right\rceil$ rounds the scalar $a$ to its nearest
upper integer.

######  Theorem 5.1.

If $B_{t}$ and $E_{t}$ (or namely, $M_{t}$ and $S_{t}^{T}$) with $t=0,\dots,T$
are of full column rank $r_{B_{t}}=n_{\tau}$ and $r_{E_{t}}=n_{q}$, the basis
of nullspace of $A\coloneqq\nabla_{x}f_{dyn}$ (DED) is of rank
$r_{Z}=T(n_{\tau}+n_{\gamma})$. The linear independent sub-sets of $A$ are
banded within width of $\beta\leq(2+\mu)Tn_{s}+(3+\mu)(n_{\tau}+n_{\gamma})$.
The subset augmentation factor $\mu$ is given by

$\displaystyle\mu$
$\displaystyle=\left(\left\lceil\frac{2n_{ua}}{n_{q}-n_{ua}}\right\rceil\right)\quad\text{for}\quad
0\leq n_{ua}<n_{q}$ (84)

######  Proof 5.2.

Full actuation First, we consider the case of computing a nullspace basis of
DED in the case of full actuation (empty matrices $G_{1,t}^{ua}$,
$G_{2,t}^{ua}$ with $n_{ua}=0$). The rank of DED is $r_{A}=Tn_{s}$ (number of
rows, with full row rank). The dimension of the nullspace basis follows with
$r_{Z}=T(n_{s}+n_{\tau}+n_{\gamma})-Tn_{s}=T(n_{\tau}+n_{\gamma})$ (number of
columns minus rank of matrix $A$). The bandwidth can be identified by finding
row and column permutations $P_{t}$ and $Q_{t}$, such that the column subset
corresponding to time step $t$ is permuted to the upper left, see PDXED for
explicit and PDIED for implicit Euler integrated dynamics, respectively. The
$3(n_{\tau}+n_{q})$ leftmost columns $G_{1,t}$ are clearly full rank due to
the block-diagonal consisting of full-rank elements $B$ and $E$. The rightmost
$n_{q}+3n_{\gamma}$ columns $G_{2,t}$ are linearly dependent of them. The
linearly independent subsets of DED are maximally of length
$\beta=2n_{s}+3(n_{\tau}+n_{\gamma})$. The above is successively applied to
all time steps $t=0,\dots,T$.

Under-actuation We now consider the case of under-actuation of degree
$n_{ua}>0$, such that $n_{\tau}+n_{ua}=n_{\dot{q}}$. In the following, we do
not consider turnback pivot columns corresponding to contact forces $\gamma$.
These columns are already used in the nullspace basis corresponding to the
contact forces itself, while repeated use would violate the full-rank
property, see Sec. 5.5.

Considering the permutations PDXED or PDIED, we see that $G_{t}$ has at most
rank (number of rows of the subset)

$r_{G_{t}}=(3+\mu)(n_{\tau}+n_{q}+n_{ua})$ (85)

The subset augmentation factor $\mu$ adds additional time steps to the sub-
set. The number of columns is (columns of subset minus the pivot columns that
need to be in the linear subset)

$c_{G_{t}}=(3+\mu)n_{\tau}+(4+2\mu)n_{q}$ (86)

The maximum dimension of the nullspace of $G_{t}$ is then

$\displaystyle n_{G_{t}}=c_{G_{t}}-r_{G_{t}}=(1+\mu)n_{q}-(3+\mu)n_{ua}$ (87)

The number of linearly dependent columns within the given subset $n_{G_{t}}$
needs to be larger than the number of pivot columns (as these are used to form
the basis of the nullspace)

$n_{G_{t}}\geq n_{\tau}$ (88)

Inserting (87), the expression for $\mu$ (84) follows.

With this choice of $\mu$, we find a subset which is linear dependent to our
$n_{\tau}$ pivot columns. The $n_{\gamma}$ columns corresponding to the
contact forces are already linearly dependent of $G_{t}$ as discussed above.
The bandwidth of the linearly independent matrix sub-sets therefore becomes

$\beta=(2+\mu)n_{s}+(3+\mu)(n_{\tau}+n_{\gamma})$ (89)

The augmentation factor $\mu$ is a conservative measure since $r_{G_{t}}$ is
an approximation of the exact rank $\hat{r}_{G_{t}}$ of $G_{t}$, with
$r_{G_{t}}\geq\hat{r}_{G_{t}}$ (therefore, the bandwidth is most likely
smaller with $n_{G_{t}}\leq\hat{n}_{G_{t}}$). In case of full under-actuation
$n_{ua}=n_{\dot{q}}$, the nullspace basis becomes dense with
$\mu\rightarrow\infty$ as expected. This means that the system response of
each time $t_{i}$ is fully dependent on the system state at any other given
time $t_{j}$ with $j\neq i$.

As we show in Sec. 5.6, the bands of the turnback nullspace basis for dynamics
integrated by the Euler method exhibit internal sparsity patterns. Still, the
bandwidth $\beta$ (89) is in contrast to an effective bandwidth of
$n_{\tau}+n_{\gamma}$ for DDP (neglecting the cost of the forward roll-out for
the state calculation requiring operations in $n_{s}^{2}$). In future work, we
desire to incorporate DDP principles into HLSP for further computational
efficiency. Nonetheless, the computation of the nullspace basis can be highly
parallelized, as we describe in Sec. 5.7. This is not possible for DDP due to
its recursive nature. Also, the backward recursion would need to be computed
for every priority level. In contrast, the projection into the nullspace of
the dynamics only needs to be done once. This might be more efficient for a
high number of priority levels. Furthermore, multi-stage constraints involving
states and controls from several time steps (aside from dynamics constraints)
are handled due to the broad optimization point of view. Sparsity of such
constraints is preserved by relying on less specialized formulations of the
turnback algorithm as described in 5.

###  5.4 Turnback algorithm for Euler integrated dynamics

Algorithm 2 turnbackParam

1:Input: $T$, $n$, $n_{\tau}$, $n_{\gamma}$, $n_{s}$, $n_{ua}$, $\beta$

2:Output: $r_{A}$, $r_{Z}$, $b\in\mathbb{R}^{T}$, $b^{+}\in\mathbb{R}^{T}$,
$\pi\in\mathbb{R}^{r_{Z}}$

3:$r_{A}=Tn_{s}$

4:$r_{Z}=T(n_{\tau}+n_{\gamma})$

5:$n_{\pi}=0$

6:$n_{b}=0$

7:for $t=0:T-1$ do

8: $b(t)=n_{b}$

9: $b^{+}(t)=\min(n_{b}+\beta,n)$

10: for $j=0:n_{\gamma}$ do

11: $\pi(n_{\pi})=n_{b}+n_{\tau}+j$

12: $n_{\pi}\leftarrow n_{\pi}+1$

13: end for

14: for $j=0:n_{\dot{q}}-n_{ua}$ do

15: if Explicit Euler integrated dynamics then

16: $\pi(n_{\pi})=n_{b}+n_{\tau}+n_{\gamma}+n_{q}+n_{ua}+j$

17: else if Implicit Euler integrated dynamics then

18: $\pi(n_{\pi})=n_{b}+n_{\tau}+n_{\gamma}+n_{ua}+j$

19: end if

20: $n_{\pi}\leftarrow n_{\pi}+1$

21: end for

22: $n_{b}\leftarrow n_{b}+n_{\tau}+n_{\gamma}+n_{s}$

23:end for

24:return $b$, $b^{+}$, $\pi$, $r_{A}$, $r_{Z}$

Based on theorem 5.1, we can implement a computationally efficient version of
the turnback algorithm. Foremost, the linearly dependent column subsets of the
matrix can be chosen according to the known bandwidth of $Z$. This means that
an initial rank revealing LU decomposition of the matrix is not necessary.
Concretely, the indices $b$ indicate the first and $b^{+}=b+\beta$ the last
column of the sub-matrix of $A$. Furthermore, the turnback pivot columns are
set as the columns corresponding to $D_{1,t}$ and the last $n_{\tau}$ columns
of $M_{t}$. The reasoning is that the under-actuated part typically describes
the free-flyer dynamics of the system which are well conditioned as they
represent the full linear and rotational inertia of the system. Note that in
theorem 5.1, we assume full-rank of $M$. This is typically given for
physically consistent systems 46. Algorithm 2 details the computation of above
values. The modified turnback algorithm then consists of following steps:

  1. 1.

$b$, $b^{+}$, $\pi$ $\leftarrow$ Alg. 2.

  2. 2.

For each index $t=1,\dots,T$, compute the LU decomposition of the column
submatrix $G_{t}\coloneqq A(\mathcal{C}_{t})$ of $A$. The column set
$\mathcal{C}_{t}$ is given by the column range from $b(t)$ to $b^{+}(t)$
without the turnback pivot columns contained within. This leads to the set
$\mathcal{C}_{t}=\left[b(t),b^{+}(t)\right]\setminus\pi(i)$ with
$i=t(n_{s}+n_{\tau}+n_{\gamma})+n_{s},\dots,(t+1)(n_{s}+n_{\tau}+n_{\gamma})$.

  3. 3.

Compute the null-vector according to (82).

In case of SOI augmentation
$V\coloneqq\begin{bmatrix}\nabla_{x}f_{dyn}^{T}&R^{T}\end{bmatrix}^{T}$, where
$R$ is a factor of the hierarchical Hessian
$\hat{H}\coloneqq\nabla^{2}_{x}f_{dyn}^{T}=R^{T}R$ 5, we apply following two-
step computation of a basis of the nullspace: first, $\mathcal{N}_{tb,ed}$
computes a basis of the nullspace according to the turnback algorithm for
Euler integrated dynamics described above
($N_{\nabla_{x}f_{dyn}}\leftarrow\mathcal{N}_{tb,ed}(\nabla_{x}f_{dyn})$), and
secondly, $\mathcal{N}_{tb}$ according to the turnback algorithm as described
in 6 ($N_{2}\leftarrow\mathcal{N}_{tb}(RN_{\nabla_{x}f_{dyn}})$). We then have
$VN_{\nabla_{x}f_{dyn}}N_{2}=0$. Note, that $\mathcal{N}_{tb}$ does not
provide any sparsity guarantees but has been observed to reliably deliver
sparse bases on a wide variety of sparsity patterns 36. At the same time, due
to the high variable occupancy of the equation of motion, lower levels
typically are not resolved anymore since most variables are eliminated by the
projections.

###  5.5 Full-rank property of turnback nullspace basis

Due to numerical inaccuracies, it can turn out that the pivot columns of a
time step $t$ are linearly independent of the corresponding column sub-matrix
$A(\mathcal{C}_{t})$ to a small error $\|\hat{u}_{2}\|_{2}\leq\delta$ with
$\delta\ll 1$ such that

$A(\mathcal{C}_{t})=P^{T}L\begin{bmatrix}U_{1}&u_{2}\\\
0&\hat{u}_{2}\end{bmatrix}Q^{T}$ (90)

Furthermore, nullspace vectors may have an error higher than a tolerance
$\|Az\|_{2}>\delta$.

In these cases, we further augment the sub-matrix with blocks corresponding to
timesteps $t^{+}>t$ and $t^{-}<t$ to the ‘left’ and ‘right’ of stage $t$. The
full-rank property of the resulting nullspace basis is preserved by not adding
columns of $A$ that correspond to turnback pivot columns of lower time-steps
$t^{-}<t$. The basis of the nullspace then exhibits the following structure
(we depict the extreme case of full augmentation)

$\displaystyle
Z_{tb}=\begin{bmatrix}X_{1,1}&X_{1,1}&\ldots&X_{1,T-1}&X_{1,T}\\\
I&\pagecolor{green!25}&\ldots&\pagecolor{green!25}&\pagecolor{green!25}\\\
X_{2,1}&X_{2,2}&\ldots&X_{2,T-1}&X_{2,T}\\\
X_{3,1}&I&\ldots&\pagecolor{green!25}&\pagecolor{green!25}\\\
\vdots&\vdots&\ddots&\vdots&\vdots\\\
X_{n-2,1}&X_{n-2,2}&\ldots&X_{n-2,T-1}&X_{n-2,T}\\\
X_{n-1,1}&X_{n-1,2}&\ldots&I&\pagecolor{green!25}\\\
X_{n,1}&X_{n,2}&\ldots&X_{n,T-1}&I\\\
X_{n,1}&X_{n,2}&\ldots&X_{n,T-1}&X_{n,T}\end{bmatrix}$ (91)

The identity matrices correspond to the turnback pivot columns of $A$. These
ensure full-column rank of the turnback nullspace. Furthermore, it can be
easily confirmed that the above is full-rank as

  * •

columns to the left are not a linear combination of each of its columns to the
right, as this would destroy the sparsity (green)

  * •

columns to the right are not a linear combination of each of its columns to
the left, as they can not eliminate the entries on the same rows as the sparse
rows (green).

###  5.6 Under-actuated systems

We consider the basis of nullspace of (PDXED) for fully-actuated systems
($n_{ua}=0$) in the case of explicit Euler integrated dynamics

$\begin{bmatrix}G_{1,t}&G_{2,t}\end{bmatrix}Z_{t}=0\quad\text{with}\quad
Z_{t}=\begin{bmatrix}-G_{1,t}^{-1}G_{2,t}\\\ I\end{bmatrix}$ (92)

Using block-wise inversion 47 of the matrix $G_{1,t}$ with full-rank and
invertible $B$ and $E$ (see theorem 5.1), we get

$\displaystyle G_{1,t}^{-1}G_{2,t}=$ (93)
$\displaystyle\begin{bmatrix}B_{t}^{-1}\\\ &E_{3,t}^{-1}&\\\
&\Upsilon&B_{t+1}^{-1}&\Upsilon&\Upsilon&\Upsilon\\\
&\Upsilon&&E_{3,t+1}^{-1}&\\\ &\Upsilon&&\Upsilon&B_{t+2}^{-1}&\Upsilon\\\
&\Upsilon&&\Upsilon&&E_{2,t+1}^{-1}&\end{bmatrix}\begin{bmatrix}D_{4,t}&F_{t}\\\
\\\ D_{2,t}&\\\ E_{2,t}\\\ &\\\
&\end{bmatrix}=\begin{bmatrix}B_{t}^{-1}D_{3,t}&B_{t}^{-1}F_{t}\\\ \\\
B_{t+1}^{-1}D_{2,t}+\Upsilon E_{2,t}&\\\
{\color[rgb]{0.75,0.75,0.75}\definecolor[named]{pgfstrokecolor}{rgb}{0.75,0.75,0.75}\pgfsys@color@gray@stroke{0.75}\pgfsys@color@gray@fill{0.75}E_{3,t+1}^{-1}E_{2,t}}\\\
\Upsilon E_{2,t}\\\ \Upsilon E_{2,t}\end{bmatrix}$

$\Upsilon$ are place-holders for dense matrix blocks. Elements in gray are
zero blocks. This means that the effective bandwidth of the null-vectors (82)
is $\beta-n_{s}$ in the case of full actuation. In contrast, such sparsity is
not reproducible if $B\in\mathbb{R}^{n_{u}\times n_{u}-n_{nua}}$ is not
invertible due to under-actuation $n_{ua}>0$. Instead of sparse block-wise
inversion, row permutations of $G_{1,t}$ need to be applied in order to
permute invertible pivot elements onto the diagonal.

In order to avoid this, we adapt the robot dynamics by introducing ‘virtual’
controls $u^{*}$ such that the modified control matrix
$\hat{B}=\begin{bmatrix}B^{*}&B\end{bmatrix}\in\mathbb{R}^{n_{\dot{q}}\times
n_{\dot{q}}}$ is full-rank and invertible as in the fully actuated case. At
the same time, the virtual controls are set to zero by two sets of inequality
constraints $|u^{*}|\leq 0$ in order to not influence the robot behavior. The
choice of inequality instead of equality constraints prevents that these
constraints enter the active set (and create non-zero fill-in as without
virtual controls by nullspace projections). This method effectively increases
the number of variables but this is offset by the reduced number of non-zeros
in the turnback nullspace. Such a scheme has been devised in the context of a
sparse nullspace basis for optimal control of linear time-invariant systems
48.

###  5.7 Multi-threaded computation

The turnback algorithm can be highly parallelized. In fact, in the case of
Euler integrated dynamics and with the availability of
$T(n_{\tau}+n_{\gamma})$ threads, each subset of $Z$ could be computed in
parallel, casting the effective computational complexity of the turnback
algorithm to approximately $O(\beta^{3}+\beta^{2})$ with $\beta$ as defined in
(89) (in detail: $T$ subsets are factorized in parallel by $T$ threads, and
the $T(n_{\tau}+n_{\gamma})$ individual nullvectors of bandwidth $\beta$ are
then computed in parallel by $T(n_{\tau}+n_{\gamma})$ threads). This is in
contrast to DDP, where $T$ decompositions of complexity
$O((n_{\tau}+n_{\gamma})^{3})$ need to be computed in sequence. Therefore, a
projector based S-HLSP based on a sparsity retaining turnback algorithm may be
preferred in the presence of high number of cores (and high number of priority
levels, as noted in Sec. 5.3).

##  6 Evaluation

We use the presented solver $\mathcal{N}$ADM2 in combination with the turnback
algorithm for Euler integrated dynamics within the solver S-HLSP 6 for NL-
HLSP. The HLSP solver solves the HLSP sub-problems which arise from the
linearization of the NL-HLSP at its current working point $x$. First, we
evaluate the efficiency of the modified turnback algorithm for Euler
integrated dynamics (see Sec. 6.1). Secondly, S-HLSP in combination with our
proposed HLSP solver is run on a hierarchy composed of test-functions, Sec.
6.2. For one, we investigate whether a high accuracy solution can be
efficiently obtained by first approximating an optimal primal point with our
proposed solver of lower accuracy. We then switch to a high-accuracy sub-
solver and continue the local search. In Sec. 6.2, we can see how a high-
accuracy solution of a NL-HLSP composed of test-functions is obtained with
less computational effort compared to a S-HLSP without an initial primal guess
of lower accuracy. Furthermore, we evaluate the SOI augmentation threshold
strategy developed in Sec. 3. We then evaluate our methods to the following
robot scenarios:

  * •

Inverse kinematics of a humanoid robot HRP-2Kai (Sec. 6.3)

  * •

Time-optimal control of the manipulator UR3e under multi-stage constraint
(Sec. 6.4)

  * •

Swing-up of inverted pendulum (Sec. 6.5)

  * •

Jump of robot dog Solo12 including multi-stage constraint (Sec. 6.6)

The last three examples (inverted pendulum, manipulator and robot dog) are PTO
where we use the turnback algorithm for dynamics integrated by Euler
integration (Sec 5). The latter two simulations are thereby concerned with
under-actuated systems where we follow the developments outlined in Sec. 5.6.

The simulations are run on an 11th Gen Intel Core i7-11800H 2.30GHz $\times$
16 with 23 GB RAM. The implementations of $\mathcal{N}$ADM2 and the turnback
algorithm are based on the Eigen library 49 and implemented in C++. The matrix
$C$ (30) is factorized ($O(n^{3})$) only if the step-size parameter $\rho$ is
updated. We use a LDLT decomposition with low non-zero fill-in (for example
compared to the QR decomposition). Otherwise, $\mathcal{N}$ADM2 relies on
matrix-vector operations ($O(n^{2})$) as a first-order method. The HLSP solver
$\mathcal{N}$IPM2 based on the IPM (matrix factorizations in every iteration),
which we proposed in our previous work 34, is modified by incorporating the
proposed turnback nullspace basis for dynamics integrated by the Euler method.
We can expect computational advantage for $\mathcal{N}$ADM2 if

$\iota_{\text{$\mathcal{N}$ADM${}_{2}$}}+\iota_{\text{$\mathcal{N}$ADM${}_{2}$},\rho}\cdot
n<\iota_{\text{$\mathcal{N}$IPM}}\cdot(1+n)$ (94)

$\iota$ is the number of inner iterations of the respective solvers.
$\iota_{\mathcal{N}\text{ADM${}_{2}$},\rho}$ is the number of factorization
updates of $\mathcal{N}$ADM2. $n$ is the number of problem variables. In case
of an increase of the KKT norm (which is not related to a change of $\rho$),
we increase the regularization factor $\sigma$ ($\sigma_{0}=1\cdot 10^{-6}$)
and reset $\rho$ ($\rho_{0}=0.1$). The number of inner iterations of
$\mathcal{N}$ADM2 is limited to 1500, or 2000 for the test in Sec. 6.5 . We
use the analytical hierarchical Hessian 5 as needed for the Newton’s method in
Fig. 1. In the robotics examples, the NL-HLSP’s and HLSP’s are computed by the
pinocchio library 50 and the automatic differentiation package CppAD 51. The
Lagrange multipliers are computed according to Sec. 4.4 by the Conjugate
Gradient solver LSQR 52. The turnback algorithm is based on the rank-revealing
LU decomposition provided by the library LUSOL 53. Note that we only depict
the computation times of the HLSP sub-solvers. Due to memory limitations, all
simulations except for the turnback algorithm evaluation are run on a single
thread. $\mathcal{N}$ADM2 and $\mathcal{N}$IPM2 are compared to the
hierarchical versions of the off-the-shelf solvers H-MOSEK 54, H-GUROBI 55 and
H-OSQP 37. All solvers rely on the same framework for active and inactive set
composition.

###  6.1 Turnback algorithm for Euler integrated dynamics

Figure 4: Computation time $t_{tb}$, number of non-zeros (nnz) and density
($\varphi$) of $Z^{T}Z$ of the turnback nullspace $Z$ for Euler integrated
dynamics with $n_{q}=n_{\dot{q}}=22$ and $n_{\gamma}=24$ in dependence of
control horizon $T$ and under-actuation $n_{ua}$.

First, we evaluate the computational efficiency of the turnback algorithm
adapted to discrete Euler integrated dynamics. We compose randomized matrices
$M$, $D$ and $B$ in (DED). We choose $n_{q}=n_{\dot{q}}=22$, $n_{\gamma}=24$
and $n_{\tau}=n_{\dot{q}}-n_{ua}$ with variable $n_{ua}$ (this corresponds to
the dimensions of the robot dog Solo12 with four feet exerting forces and
torques, such that $n_{\gamma}=4\cdot 6$, and $n_{ua}=6$). We use 8 threads
for the parallel computations of the turnback algorithm as described in Sec.
5.7.

The results are given in Fig. 4. Depicted are the computation time and the
number of non-zeros and density ($\varphi=nnz(A)/(n\cdot m)$ with
$A\in\mathbb{R}^{m\times n}$) of the normal form $Z^{T}Z$ of the turnback
nullspace depending of the time horizon $T=0,\dots,25$ and under-actuation
$n_{ua}=0,\dots,n_{q}$. We first consider the fully actuated case $n_{ua}=0$.
For $T=25$, the turnback computation time is $t_{tb}=4.9\cdot 10^{-3}$ s. The
resulting normal form $Z^{T}Z$ contains 103412 non-zeros with a density of
0.078. It can be observed that with under-actuation $n_{ua}>0$, there is a
sharp incline in non-zeros and density of $Z^{T}Z$. For example for
$n_{ua}=1$, the density increases to 0.328 with four times as many non-zeros
(415125). This can be explained by the higher coupling within the diagonal
blocks as demonstrated in Sec. 5.6. Nonetheless, the computation time does not
increase as dramatically to $t_{tb}=6.9\cdot 10^{-3}$ s. For full under-
actuation $n_{ua}=n_{q}$, the resulting nullspace basis is dense ($\varphi=1$)
as expected. At the same time, the increase in non-zeros is quadratic. In
contrast, for low $n_{ua}$, the linear increase of non-zero entries in $T$ is
clearly distinguishable.

###  6.2 NL-HLSP test-functions

|  |  |  $\mathcal{N}$ADM2 |  $\mathcal{N}$ADM2 $\rightarrow$ H-MOSEK | H-MOSEK | H-OSQP
---|---|---|---|---|---|---
|  |  | (0.01 s) | (0.01 s$\rightarrow$0.11 s: 0.12 s) | (0.19 s) | (0.23 s)
$l$ |  | $f_{l}(x)\leqq v_{l}$ | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter.
1 | Disk ineq. | $x_{1}^{2}+x_{2}^{2}-1.9\leq v_{1}$ | $1.0\cdot 10^{-5}$ | 6 | $9.8\cdot 10^{-6}$ | 1 | $9.8\cdot 10^{-6}$ | 12 | $1.0\cdot 10^{-5}$ | 7
2 | Ros. eq. | $(1-x_{1})^{2}+100(x_{2}-x_{1}^{2})^{2}=v_{2}$ | $2.9\cdot 10^{-4}$ | 14 | $2.9\cdot 10^{-4}$ | 14 | $2.9\cdot 10^{-4}$ | 17 | $2.9\cdot 10^{-4}$ | 13
3 | Disk eq. | $x_{1}^{2}+x_{2}^{2}-0.9=v_{3}$ | 1 | 2 | 1 | 1 | 1 | 1 | 1 | 2
4 | Disk eq. | $x_{2}^{2}+x_{3}^{2}-1=v_{4}$ | $1.6\cdot 10^{-6}$ | 2 | $1.2\cdot 10^{-16}$ | 1 | $1.9\cdot 10^{-16}$ | 1 | $1.1\cdot 10^{-11}$ | 2
5 | Disk ineq. | $x_{4}^{2}+x_{5}^{2}+1\leq v_{5}$ | 1 | 1 | 1 | 1 | 1 | 1 | 1 | 1
6 | Disk eq. | $x_{6}^{2}+x_{7}^{2}+x_{8}^{2}-4=v_{6}$ | $7.1\cdot 10^{-7}$ | 4 | $1.7\cdot 10^{-8}$ | 1 | $1.6\cdot 10^{-10}$ | 8 | $1.9\cdot 10^{-8}$ | 1
7 | Ros. eq. | $(1-x_{6})^{2}+100(x_{7}-x_{6}^{2})^{2}=v_{7}$ | $4.2\cdot 10^{-4}$ | 1 | $7.6\cdot 10^{-8}$ | 24 | $7.4\cdot 10^{-8}$ | 35 | $1.8\cdot 10^{-4}$ | 111
8 | McC. eq. | $\sin(x_{{}_{9}}+x_{10})+(x_{9}-x_{10})^{2}$ | 18.1 | 1 | 18.1 | 1 | 18.1 | 1 | 24.4 | 1
|  | $-1.5x_{9}+2.5x_{10}+1+M=v_{8}$ |  |  |  |  |  |  |  |
9 | Reg. eq. | $x_{1:10}=v_{9}$ | 2.9 | 0 | 2.9 | 0 | 2.9 | 0 | 7.8 | 0
$\Sigma$ |  |  |  | 31 |  | (31$\rightarrow$44) 75 |  | 76 |  | 138

Table 1: Non-linear test functions: optimal slacks $v^{*}$ and number of outer
iterations (Iter.) per priority level for a NL-HLSP with $p=9$ and $n=10$. The
hierarchy is composed of disk, Rosenbrock (Ros.), McCormick (McC.) and
regularization (Reg.) equality (eq.) and inequality (ineq.) constraints.
Figure 5: Non-linear test functions, data for the different HLSP sub-solvers
over S-HLSP outer iteration: computation times per HLSP solve, number of inner
iterations, KKT residuals and overall number of non-zeros handled throughout
the whole hierarchy. Figure 6: Different formulations of McCormick function:
original $f_{\text{McC}}$, $\ell_{2}$-norm $\sqrt{f_{\text{McC}}^{2}}$, with
offset $f_{\text{McC}}+M$, $\ell_{2}$-norm with offset
$(f_{\text{McC}}+M)^{2}$ ($M=20$). Negative function values are colored in
orange tones. Local minima are marked in black. Figure 7: Non-linear test
functions: primal $x$ over S-HLSP outer iteration (colored triangles) from
start point (black dot) to converged point (large red cross). Local minima are
marked with black crosses. Figure 8: Non-linear test functions,
$\mathcal{N}$ADM2: linear slacks $\|\hat{v}_{l}\|_{2}$, non-linear error
$\|f_{l}^{+}\|_{2}$ (light color) and $\epsilon_{adaptive,l}$ (dashed) for the
levels $l=1,\dots,p$ over outer iteration. The initial value is chosen as
$\epsilon_{adaptive,l}=100$. Gray background color indicates the outer
iterations where the respective level has been resolved by the HSF. Infeasible
levels are indicated by light red background color.

We apply $\mathcal{N}$ADM2 as HLSP sub-solver for S-HLSP to solve a NL-HLSP
composed of test functions as listed in Tab. 1. This problem constellation
tests feasible and infeasible equality and inequality constraints. This also
includes infeasibility arising from conflict with constraints from higher
priority levels. Note that the McCormick function includes a large positive
offset $M$. This enables S-HLSP to identify negative function minima (for
example the minimum $f_{\text{McC}}(-0.547,-1.547)+M=-1.9133+M$) despite its
least-squares formulation. This can be easily confirmed with the following
theorem:

######  Definition 6.1.

A factor $M\geq 0$ is sufficiently large on the domain
$\\{x\in\mathbb{R}^{n}:x\in\mathcal{S}\\}$ if a function
$\hat{f}(x,M)=f(x)+M:\mathcal{S}\rightarrow\mathbb{R}_{>0}$.

######  Theorem 6.2.

If $M\geq 0$ is sufficiently large on the domain $x\in\mathcal{S}$, then
first-order optimality of $\hat{f}(x,M)^{2}$ applies at the same points
$x^{*}\in\mathcal{S}$ as the twice continuously differentiable function
$f(x)$.

######  Proof 6.3.

We consider the first-order derivative of $\hat{f}(x,M)^{2}$ which writes as

$\frac{\partial\hat{f}^{2}}{\partial
x}=2\hat{f}\frac{\partial\hat{f}}{\partial x}=2\hat{f}\frac{\partial
f}{\partial x}$ (95)

Clearly, ${\partial\hat{f}^{2}}/{\partial x}$ has the same zeros as ${\partial
f}/{\partial x}$ for $\hat{f}(x,M)>0$ on the domain $x\in\mathcal{S}$.

Figure 6 shows how the squared McCormick function with offset
$(f_{\text{McC}}+M)^{2}$ has the same minima $(x_{9}^{*},x_{10}^{*})$ as the
original function in the range $x_{9},x_{10}\in\left[-5.5,4\right]$. Here, we
choose $M=20$. For even polynomials, the global value $M^{*}$, such that
$\hat{f}(x^{*},M^{*})=0$, can be found by polynomial optimization 56. In
contrast, the squared McCormick function without offset has additional minima
at the zeros of $f_{\text{McC}}^{2}$, while missing any minima that are
associated with negative function values $f_{\text{McC}}<0$.

S-HLSP with $\mathcal{N}$ADM2 identifies the primal solution $x=$ 0.983,
0.966, 0.257, 0,0, 1.02, 1.04, 1.37, -0.547, -1.547. The evolution of the
primal over the S-HLSP outer iterations is depicted in Fig. 7.
$\mathcal{N}$ADM2 is able to solve the NL-HLSP to moderate accuracy. For
example, according to Tab. 1, the Rosenbrock equality on level 7 is solved to
a residual error of $\|v^{*}_{7}\|^{2}_{2}=4.2\cdot 10^{-4}$ while H-MOSEK
solves the same level to $\|v^{*}_{7}\|^{2}_{2}=7.4\cdot 10^{-8}$. At the same
time, the previous levels are solved to comparable accuracy (note that error
comparisons in hierarchies need to consider that a higher error norm on a
higher priority level can lead to lower error norm on a lower priority level).
While being less accurate, $\mathcal{N}$ADM2 (0.011 s) solves the problem the
fastest out of all the solvers, see Fig. 5. This is partly due to the low
number of outer S-HLSP iterations (31, about half as many as for H-MOSEK with
76). Still, from Fig. 5, it can be observed that the HLSP sub-problems are
solved in about $4\cdot 10^{-4}$ s. $\mathcal{N}$IPM2 solves the sub-problems
slightly slower in about $5\cdot 10^{-4}$ s. This is in accordance with the
number of inner iterations of $\sim 300<\hskip 2.0pt\sim 50\cdot n=\hskip
2.0pt\sim 500$ of $\mathcal{N}$ADM2 and $\mathcal{N}$IPM2, respectively. In
contrast, the next fastest solver H-OSQP solves the inner iterations in about
$1\cdot 10^{-3}$ s. This clearly demonstrates the advantage of solving the KKT
system projected into the nullspace of active constraints.

We furthermore consider the combination of both the low and high accuracy
solvers $\mathcal{N}$ADM2 and H-MOSEK. It can be observed that a high accuracy
solution is obtained when compared to the low accuracy solver
$\mathcal{N}$ADM2 alone (level 7 at $7.6\cdot 10^{-8}$ compared to $4.2\cdot
10^{-4}$). At the same time, the computation time is lower (0.12 s) compared
to the high accuracy solver H-MOSEK alone (0.19 s). Consequently, a sub-
problem solver with moderate accuracy like $\mathcal{N}$ADM2 can be used to
warm-start the S-HLSP with a lower accuracy primal guess. The reduced overall
computation time follows due to the reduced number of high accuracy sub-
problem solutions (44 compared to 76 for H-MOSEK alone).

Finally, we evaluate the adaptive SOI thresholding strategy developed in Sec.
3. For this, we set the initial value to $\epsilon_{adaptive,l}=100$ (note
that in all other examples, we initially set $\epsilon_{adaptive,l}=1\cdot
10^{-12}$) and the lower limit to $1\cdot 10^{-12}$. The corresponding
parameters are chosen as $\zeta=1$ and $\delta=0.95$ (see Alg. 1). As can be
seen from Fig. 8, a similar error reduction as in Tab. 1 is achieved. However,
more iterations are necessary (92 instead of 31) as $\epsilon_{adaptive,l}$ is
adjusted to the infeasibility of the constraints. Importantly, at convergence
of the HSF of the infeasible levels $l=2,3,5,8$, we have
$\|f_{l}^{+}\|_{2}^{2}>\epsilon_{adaptive,l}$ and the SOI is activated. At the
same time, the heuristic relaxes the threshold for example for the infeasible
Rosenbrock constraint on level 2 in instances of sufficient progress in terms
of optimality. Nonetheless, the SOI is erroneously activated for the feasible
level 7. This motivates further investigation with respect to constraint
optimality by avoiding regularized minima, for example based on machine
learning methods for feasibility detection.

###  6.3 Inverse kinematics of humanoid robot HRP-2

|  |  $\mathcal{N}$ADM2 |  $\mathcal{N}$IPM2 |  $\mathcal{N}$A. $\rightarrow$ H-M. | H-MOSEK | H-GUROBI | H-OSQP
---|---|---|---|---|---|---|---
|  | (0.06 s) | (0.40 s) | (0.06 s$\rightarrow$0.39 s: 0.46 s) | (0.59 s) | (1.06 s) | (0.089 s)
$l$ | $f_{l}(x)\leqq v_{l}$ | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter.
1 | J. lim. ineq. | $5.1\cdot 10^{-5}$ | 2 | $3.4\cdot 10^{-8}$ | 1 | $3.4\cdot 10^{-5}$ | 0 | 0 | 0 | 0 | 0 | 0 | 0
2 | LF, RF, LH eq. | $3.1\cdot 10^{-6}$ | 14 | $2.6\cdot 10^{-10}$ | 18 | $7.8\cdot 10^{-9}$ | 10 | $9.9\cdot 10^{-9}$ | 9 | $7.4\cdot 10^{-8}$ | 27 | $4.8\cdot 10^{-8}$ | 6
3 | CoM ineq. | $4.1\cdot 10^{-6}$ | 1 | $1.0\cdot 10^{-8}$ | 1 | $1.0\cdot 10^{-5}$ | 65 | $1.0\cdot 10^{-5}$ | 30 | 0 | 28 | $2.6\cdot 10^{-6}$ | 1
4 | Right hand eq. | 1.04 | 9 | 0.98 | 128 | 1.02 | 2 | 1.02 | 56 | 1.13 | 19 | 0.98 | 12
5 | Reg. eq. | 5.5 | 10 | 4.40 | 7 | 4.1 | 0 | 4.98 | 0 | 4.70 | 0 | 4.39 | 0
$\Sigma$ |  |  | 37 |  | 156 |  | (48$\rightarrow$77) 125 |  | 95 |  | 74 |  | 19

Table 2: HRP-2 inverse kinematics: optimal slacks $v^{*}$ and number of outer
iterations (Iter.) per priority level for a NL-HLSP with $p=5$ and $n=38$. J.
lim.: Joint limits, LF: left foot, RF: right foot, LH: left hand.
$\mathcal{N}$A.: $\mathcal{N}$ADM2; H-M: H-MOSEK. Figure 9: HRP-2 inverse
kinematics, data for the different HLSP sub-solvers over S-HLSP outer
iteration: computation times per HLSP solve, number of inner iterations, KKT
residuals and overall number of non-zeros handled throughout the whole
hierarchy. Figure 10: HRP-2 inverse kinematics, $\mathcal{N}$ADM2: converged
robot posture.

This simulation is concerned with solving an inverse kinematics problem for
the humanoid robot HRP-2 with $n=38$ degrees of freedom. The corresponding
hierarchy is given in Tab. 2 ($p=5$). The first level limits the joint angles.
The second level positions the left and right feet and the left hand. The
third level limits the CoM position to a bounding box. The fifth level
positions the right hand towards an out-of-reach target
$\begin{bmatrix}-0.5&-0.5&-1\end{bmatrix}$ m below its feet. The right foot is
positioned at $\begin{bmatrix}0.015&-0.1&0.1\end{bmatrix}$ m. The $z$
component is approximately at ground level $0.1$ m. Lastly, all variables are
regularized to zero.

The results are given in Tab 2. The converged robot posture for
$\mathcal{N}$ADM2 is depicted in Fig. 10. Our proposed solver
$\mathcal{N}$ADM2 solves the HLSP sub-problems the fastest at about $6\cdot
10^{-2}$ s. Fluctuations in computation time and non-zeros (Fig. 9) is due to
activation and deactivations of SOI on the second level. At the same time,
moderate accuracy is achieved. For example, the end-effector positioning of
the left and right foot and the left hand on level 2 is resolved to an error
of $3.1\cdot 10^{-6}$ m while the error is reduced to less than $4.8\cdot
10^{-8}$ m (H-OSQP) for the other solvers. The right hand task on level 3 is
resolved to an error of 1.04 m. While this is worse than for example
$\mathcal{N}$IPM2 (0.98 m), the adaptive SOI threshold strategy enables SOI
deactivation on the higher priority level 2. Without it, only manual tuning
for each individual solver prevented the SOI activation which causes worse
error convergence on lower priority levels due to high variable occupation.
H-OSQP achieves a solution in the lowest number of outer iterations (19
compared to 48 for $\mathcal{N}$ADM2) but is slower than $\mathcal{N}$ADM2 due
to the slow resolution of the HLSP sub-problems at about $4\cdot 10^{-2}$ s.

###  6.4 Time-optimal control of manipulator

|  |  $\mathcal{N}$ADM2 (4.8 s) |  $\mathcal{N}$IPM2 (4.8 s) | H-MOSEK (72.1 s) | H-GUROBI (3.2 s)
---|---|---|---|---|---
$l$ | $f_{l}(x)\leqq v_{l}$ | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter.
1 |  $q$, $\tau$ lim. ineq. | $1.7\cdot 10^{-5}$ | 1 | $5.4\cdot 10^{-8}$ | 1 | 0 | 1 | 0 | 1
2 | $f_{\text{dyn}}(x)=v_{2}$ | $4.9\cdot 10^{-8}$ | 17 | $1.6\cdot 10^{-4}$ | 29 | $2.3\cdot 10^{-9}$ | 48 | $8.5\cdot 10^{-8}$ | 10
3 | $f_{\text{ef,adtoc}}(x)=v_{3}$ | $0.11$ | 87 | $8.1\cdot 10^{-2}$ | 320 | $7.8\cdot 10^{-2}$ | 317 | $1.8$ | 53
4 | $\dot{h}(x)=v_{4}$ | 1509 | 1 | 1319 | 1 | 1363 | 1 | 211 | 1
5 | $\begin{bmatrix}q&\dot{q}\end{bmatrix}^{T}=v_{5}$ | 231 | 0 | 245 | 1 | 274 | 1 | 91 | 7
6 | $\tau=v_{6}$ | 306 | 1 | 274 | 1 | 276 | 1 | 18 | 13
$\Sigma$ |  |  | 110 |  | 353 |  | 370 |  | 86

Table 3: UR3e time-optimal control: optimal slacks $v^{*}$ and number of outer
iterations (Iter.) per priority level for a NL-HLSP with $p=6$ and $n=361$.
Figure 11: UR3e time-optimal control, data for the different HLSP sub-solvers
over S-HLSP outer iteration: computation times per HLSP solve, number of inner
iterations, KKT residuals and overall number of non-zeros handled throughout
the whole hierarchy. Figure 12: UR3e time-optimal control, $\mathcal{N}$ADM2.
Error reduction of the time-optimal reaching task (top) and joint torques
normalized by their limits (bottom).

In this simulation setting, we aim to identify a time-optimal control under
actuation limits for a kinematic reaching task of the fully actuated
manipulator UR3e. Furthermore, we impose a regularization task of the momentum
evolution for a safe robot movement. Such a constraint includes variables from
several stages, which is a form of constraint typically not handled by
recursive methods like DDP. Time-optimal control in least-squares programming
can be achieved by a continuous approximation of the discrete optimal time
$t^{*}$ 5. Note that this requires a feasible ‘resting’ goal point, i.e. the
robot can physically remain at this point until the end of the control horizon
$T$.

The PTO hierarchy with $p=6$ is given in Tab. 3. It is composed of state and
control limits, explicit inverse Euler integrated dynamics, time-optimal
control $f_{\text{dyn,adtoc}}$ and finally momentum time evolution $\dot{h}$,
joint velocity, angle and torque regularization. The control horizon is chosen
as $T=20$ such that the number of variables is $n=361$. The control time step
is $\Delta t=0.01$ s.

The results in Tab. 3 show S-HLSP convergence in 4.8 s and within 110 and 353
iterations for $\mathcal{N}$ADM2 and $\mathcal{N}$IPM2, respectively. This is
in contrast to H-MOSEK which delivers a time-optimal solution in only 72.1 s.
H-GUROBI fails to resolve the time-optimal control $f_{\text{dyn,adtoc}}$ and
converges quickly due to SOI activation of the dynamics constraints (which
occupies most of the variables such that lower levels show little variable
activity).

Fig. 11 shows how the projector based solvers $\mathcal{N}$ADM2 and
$\mathcal{N}$IPM2 both solve the single S-HLSP iterations the fastest with
run-times of around $0.05$ s and $0.02$ s (note that H-GUROBI only resolves
the hierarchy up to the dynamics constraints due to SOI activation). In
comparison, H-MOSEK solves the HLSP sub-problems in around $0.2$ s. This is
due to the significantly lower number of non-zeros throughout the hierarchy
for $\mathcal{N}$ADM2 and $\mathcal{N}$IPM2 (20000, about half as many as for
H-MOSEK).

Figure 12 shows the resulting joint torques normalized by their limits (lower
graph) for $\mathcal{N}$ADM2. A time-optimal bang-bang control profile
(controls at their limits) can clearly be distinguished. This leads to a sharp
drop-off of the task error $f_{\text{ef,adtoc}}$ at around control iteration
12 (upper graph).

###  6.5 Swing-up of inverted pendulum

|  |  $\mathcal{N}$ADM2 (19.7 s) |  $\mathcal{N}$IPM2 (4.1 s) | H-MOSEK (11.8 s) | H-GUROBI (20.3 s)
---|---|---|---|---|---
$l$ | $f_{l}(x)\leqq v_{l}$ | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter.
1 |  $|q|\leq\overline{q}$, $|F_{cart}|\leq\overline{F}$, $|\tau^{*}|\leq 0$ (only $\mathcal{N}$ADM2 and $\mathcal{N}$IPM2) | $6.6\cdot 10^{-2}$ | 1 | 0 | 1 | $1.7\cdot 10^{-6}$ | 33 | $5.0\cdot 10^{-5}$ | 1
2 | $f_{\text{dyn}}(x)=v_{2}$ | $2.9\cdot 10^{-5}$ | 2 | $4.8\cdot 10^{-7}$ | 81 | $2.8\cdot 10^{-8}$ | 129 | $2.0\cdot 10^{-8}$ | 152
3 | $f_{\text{ef}}(q)=v_{3}$ | 3.90 | 371 | 3.19 | 78 | 6.0 | 2 | 3.2 | 1
4 | $\begin{bmatrix}q&\dot{q}\end{bmatrix}^{T}=v_{5}$ | 268.6 | 1 | 263.0 | 22 | 175.2 | 2 | 274.2 | 18
5 | $F_{cart}=v_{6}$ | 683.5 | 1 | 600.1 | 1 | 416.7 | 1 | 577.9 | 74
$\Sigma$ |  |  | 378 |  | 184 |  | 168 |  | 247

Table 4: Inverted pendulum swing-up: optimal slacks $v^{*}$ and number of
outer iterations (Iter.) per priority level for a NL-HLSP with $p=5$ and
$n=375$ ($n^{*}=450$ for $\mathcal{N}$ADM2 and $\mathcal{N}$IPM2). Figure 13:
Inverted pendulum swing-up, data for the different HLSP sub-solvers over
S-HLSP outer iteration: computation times per HLSP solve, number of inner
iterations, KKT residuals and overall number of non-zeros handled throughout
the whole hierarchy. Figure 14: Inverted pendulum swing-up, $\mathcal{N}$IPM2:
position and velocity $q$ and $\dot{q}$ of the cart and the freely swinging
pendulum, force $F_{cart}$ applied to the cart.

In this two-dimensional simulation setting, we aim to compute a swing up
motion of a freely rotating pendulum mounted to a horizontally moving cart 57.
The control input is given by the force $F_{cart}$ (limit
$\overline{F}=100\leavevmode\nobreak\ N$) applied horizontally to the cart of
weight 0.1 kg. The pendulum is of length 0.25 m and of mass 0.1 kg. The
coordinates
$q=\begin{bmatrix}q_{1}&q_{2}\end{bmatrix}^{T}=\begin{bmatrix}0&\pi\end{bmatrix}^{T}$
describe the horizontal cart position $q_{1}$ and the pendulum angle $q_{2}$.
$q_{2}=0$ corresponds to the upright pendulum position. The planning horizon
is $T=75$ ($\Delta t=0.0025$ s) with $n=375$ ($n^{*}=450$ according to Sec.
5.6 with under-actuation $n_{ua}=1$, for $\mathcal{N}$ADM2 and
$\mathcal{N}$IPM2).

The hierarchy of this trajectory optimization problem is given in Tab 4. On
the first two levels, joint angle and torque limits and the dynamics equations
integrated by the explicit Euler method are defined. The third level contains
a positioning task where the desired Cartesian position of the pendulum tip is
set to 0 m and 0.5 m for the horizontal and vertical axis, respectively.

The solver data is given in Tab. 4 and Fig. 13. $\mathcal{N}$IPM2 is able to
compute a swing-up motion in 4.1 s. The corresponding robot values are
depicted in Fig. 14. It can be seen from the bottom graph that $F_{cart}$
exhibits slight jittering at the upright position from control iteration 45
onwards. This is due to SOI approximations (linear Lagrange multipliers,
regularization, numerical errors, …). A slightly worse solution is delivered
by H-GUROBI (position task error of $\|v_{3}\|_{2}^{2}=3.2$ instead of
$\|v_{3}\|_{2}^{2}=3.19$ for $\mathcal{N}$IPM2). The computation time is
significantly longer at 20.3 s and 247 outer iterations. This is due to the
higher number of non-zeros handled throughout the hierarchy and confirms the
efficiency of the turnback algorithm in the under-actuated case. Both solvers
$\mathcal{N}$ADM2 and H-MOSEK struggle to resolve level 3 of the HLSP sub-
problem (see graph of KKT norm in Fig. 13), giving rise to worse convergence
in the corresponding NL-HLSP ($\|v_{3}^{*}\|_{2}^{2}=3.9$ for
$\mathcal{N}$ADM2 and $\|v_{3}^{*}\|_{2}^{2}=6.0$ for H-MOSEK; note that we
increased the maximum number of inner iterations to 2000 for
$\mathcal{N}$ADM2).

###  6.6 Jump of robot dog Solo12

|  |  $\mathcal{N}$ADM2 (7.0 s) |  $\mathcal{N}$IPM2(12.2 s) | H-MOSEK (80.8 s) | H-GUROBI (7.4 s)
---|---|---|---|---|---
$l$ | $f_{l}(x)\leqq v_{l}$ | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter. | $\|v_{l}^{*}\|_{2}$ | Iter.
1 |  $|q|\leq\overline{q}$, $|\tau|\leq\overline{\tau}$, $|\tau^{*}|\leq 0$ (only $\mathcal{N}$ADM2 and $\mathcal{N}$IPM2), $\gamma_{z}\geq 0$ | $4.8\cdot 10^{-4}$ | 1 | $1.4\cdot 10^{-7}$ | 1 | $0$ | 1 | $4.7\cdot 10^{-5}$ | 33
2 | $f_{\text{dyn}}(x)=v_{2}$ | $1.2\cdot 10^{-8}$ | 5 | $2.8\cdot 10^{-6}$ | 2 | $4.8\cdot 10^{-9}$ | 55 | $3.1\cdot 10^{-7}$ | 1
3 | $\sqrt{\gamma_{x}^{2}+\gamma_{y}^{2}}\leq\mu\gamma_{z}$ | $1.0\cdot 10^{-5}$ | 4 | 0 | 28 | 0 | 30 | $1.4\cdot 10^{-8}$ | 1
4 | ${f_{\text{ef}}}(q)=v_{3,1}$ | $4.9\cdot 10^{-3}$ | 6 | $5.2\cdot 10^{-4}$ | 12 | $2.5\cdot 10^{-4}$ | 30 | $1.8\cdot 10^{-2}$ | 3
| $10^{-3}\cdot q_{act}=v_{3,2}$ | $2.1\cdot 10^{-3}$ |  | $4.4\cdot 10^{-4}$ |  | $1.7\cdot 10^{-3}$ |  | $2.4\cdot 10^{-3}$ |
5 | $\dot{h}(x)=v_{4}$ | 83.0 | 35 | 130.5 | 14 | 176.7 | 51 | 89.5 | 3
6 | $\begin{bmatrix}q^{T}&\dot{q}^{T}\end{bmatrix}^{T}=v_{5}$ | 49.0 | 1 | 36.8 | 4 | 87.3 | 5 | 38.6 | 1
7 | $\begin{bmatrix}\tau^{T}&\gamma^{T}\end{bmatrix}=v_{6}$ | 36.1 | 1 | 108.1 | 12 | 315.5 | 7 | 19.0 | 1
$\Sigma$ |  |  | 54 |  | 74 |  | 180 |  | 44

Table 5: Solo12 jump: optimal slacks $v^{*}$ and number of outer iterations
(Iter.) per priority level for a NL-HLSP with $p=6$ and $n=900$ ($n^{*}=990$
for $\mathcal{N}$ADM2 and $\mathcal{N}$IPM2). Figure 15: Solo12 jump, data for
the different HLSP sub-solvers over S-HLSP outer iteration: computation times
per HLSP solve, number of inner iterations, KKT residuals and overall number
of non-zeros handled throughout the whole hierarchy. Figure 16: Solo12 jump,
$\mathcal{N}$IPM2, torso trajectory, dashed for $\Delta t=0.025$ s. Figure 17:
Solo12 jump, $\mathcal{N}$IPM2, contact forces.

In this example, we compute a whole-body jumping motion of the robot dog
Solo12 over a horizon of $T=15$ with $\Delta t=0.01$ s. The base of the robot
is not actuated ($n_{ua}=6$). We use the turnback algorithm for under-actuated
systems as described in Sec. 5.6. This effectively increases the number of
variables from $n=900$ to $n^{*}=990$.

The control hierarchy is given in Tab. 5. Joint, torque and contact friction
cone ($\mu=1$) constraints enforce robot safety and physicality. The jumping
motion is enforced by removing contacts from the dynamics equation
$f_{\text{dyn}}$ in the time interval from $t=4$ to $t=13$. The contacts
$f_{\text{ef}}$ after landing are shifted by
$\begin{bmatrix}0.05&0.05&0\end{bmatrix}$ m compared to the initial stance. On
the same level, we add a posture regularization task with low weight on the
actuated robot joints. Furthermore, the evolution of the angular momentum
$\dot{h}$ is regularized to zero. This promotes a stable flight phase.

The results are given in Fig. 15. Our solver $\mathcal{N}$ADM2 (7.0 s, 54
outer iterations) solves the NL-HLSP faster than $\mathcal{N}$IPM2 (12.2 s, 74
outer iterations), H-MOSEK (80.8 s, 180 outer iterations) and H-GUROBI (7.4 s,
44 outer iterations). However, the HLSP solve times fluctuate with SOI
activations which introduce a large number of non-zeros due to the SOI of the
dynamics equations (see nnz peaks in bottom graph of Fig. 15). This could for
example be avoided by generating strictly dynamically feasible outer iterates,
which has been proposed in a SQP trust-region method 58. In contrast, SOI
activations of the dynamics constraints do not occur for $\mathcal{N}$IPM2,
which can be contributed to the high-accuracy nature of the solver. Combined
with the lower number of iterations ($\sim 200$ times lower than the ones of
$\mathcal{N}$ADM2), the HLSP sub-problem solve times are limited to under 0.2
s. The HLSP times for H-GUROBI are competitive.. The solver is efficiently
warm-started by the primal of the full-rank level 5, such that levels below
converge with zero iterations. In comparison, both the solvers
$\mathcal{N}$ADM2 and $\mathcal{N}$IPM2 exhibit significantly faster HLSP
solve times than H-MOSEK, which resolves every level of the hierarchy. This
emphasizes the advantage of projection based methods for active constraints
elimination in order to resolve sparse problems with lower number of non-
zeros. This also indicates the high sparsity introduced by the turnback
nullspace for under-actuated systems with virtual controls as described above.

The high accuracy solver $\mathcal{N}$IPM2 achieves the greatest error
reduction consistently throughout the priority levels, see Tab. 5. The
corresponding robot torso trajectory and contact forces are depicted in Fig.
16 and Fig. 17, respectively. It can be observed that for a shorter time
horizon of 0.14 s, the posture task is infeasible and the torso is only moved
around 0.025 m into the desired direction. The robot feet however are
translated to their desired position, as can be seen from the corresponding
error reduction of $f_{\text{ef}}$ in Tab. 5. With a longer time horizon of
0.35 s with $\Delta t=0.025$ s, the robot manages a larger torso transfer and
shifts its body by the desired amount of 0.05 m.

##  7 Conclusion

In this article, we proposed several tools for efficiently solving prioritized
trajectory optimization problems in robot motion planning. We designed a
threshold adaptation strategy in order to appropriately activate or deactivate
SOI. This promotes optimality of the NL-HLSP and numerical stability when
solving its HLSP approximation. We proposed the ADMM solver $\mathcal{N}$ADM2
for efficiently solving HLSP’s. It is based on a reduced Hessian formulation
with nullspace basis projections of active constraints. We directed our
attention to problems of block-diagonal structure arising from optimal control
formulations. Accordingly, we designed a sparsity leveraging turnback
nullspace basis of upper bounded bandwidth for dynamics discretized by Euler
integration.

The proposed HLSP solver’s efficiency was demonstrated within the S-HLSP
framework to solve NL-HLSP’s. The reduced Hessian formulation significantly
reduces the number of non-zeros handled throughout the hierarchy with a
sufficient number of priority levels. With a limited number of inner
iterations, this enables a fast search for an optimal point of lower accuracy.
We showed how such a point can be used to warm-start S-HLSP to find a high
accuracy solution. Applicability to PTO for robot scenarios with multi-stage
constraints was demonstrated for a fully-actuated and under-actuated robots.
In the latter case, we showed how the high sparsity of the turnback nullspace
for fully-actuated robots can be maintained in the case of under-actuation.
The SOI threshold adaptation strategy was shown to adjust to constraint
infeasbility even from a far off starting point.

Our method sparsely and once and for all eliminates the dynamics constraints
from the HLSP sub-problems. In future work, we aim to extend the turnback
algorithm to higher-order integration methods. Furthermore, as a non-recursive
method, our method is associated with a high memory footprint as all control
time-steps need to be handled in one big solution system. We therefore aim to
implement recursive formulations of our method to achieve higher solver
maturity, for example based on DDP principles. While this is associated with
low bandwidth and high degree of sparsity, special attention needs to be paid
with respect to multi-stage constraints and high number of priority levels, as
the recursions need to be computed for every level.

## Funding

This work is partly supported by the Schaeffler Hub for Advanced Research at
Nanyang Technological University, under the ASTAR IAF-ICP Programme
ICP1900093. This work is partly supported by the Research Project I.AM.
through the European Union H2020 program (GA 871899).

## Conflict of interest disclosure

The authors have no relevant financial or non-financial interests to disclose.

## Data availability

Data generated by our algorithms S-HLSP and the HLSP sub-problem solvers are
available from the corresponding author on request.

## References

  * 1 Sherali HD, Soyster AL. Preemptive and nonpreemptive multi-objective programming: Relationship and counterexamples. Journal of Optimization Theory and Applications. 1983;39:173-186.
  * 2 Lai L, Fiaschi L, Cococcioni M, Deb K. Pure and Mixed Lexicographic-Paretian Many-Objective Optimization: State of the Art. Natural Computing. 2022\.
  * 3 Escande A, Mansard N, Wieber PB. Hierarchical quadratic programming: Fast online humanoid-robot motion generation. The International Journal of Robotics Research. 2014;33(7):1006–1028.
  * 4 Djeha M, Gergondet P, Kheddar A. Robust Task-Space Quadratic Programming for Kinematic-Controlled Robots. IEEE Transactions on Robotics. 2023;39(5):3857-3874.
  * 5 Pfeiffer K, Escande A, Gergondet P, Kheddar A. The Hierarchical Newton’s Method for Numerically Stable Prioritized Dynamic Control. IEEE Transactions on Control Systems Technology. 2023:1-14.
  * 6 Pfeiffer K, Kheddar A. Sequential Hierarchical Least-Squares Programming for Prioritized Non-Linear Optimal Control. 2024.
  * 7 Meduri A, Shah P, Viereck J, Khadiv M, Havoutis I, Righetti L. BiConMP: A Nonlinear Model Predictive Control Framework for Whole Body Motion Planning. IEEE Transactions on Robotics. 2023:1-18.
  * 8 Kaneko I, Lawo M, Thierauf G. On computational procedures for the force method. International Journal for Numerical Methods in Engineering. 1982;18:1469-1495.
  * 9 Mayne D. A Second-order Gradient Method for Determining Optimal Trajectories of Non-linear Discrete-time Systems. International Journal of Control. 1966;3(1):85-95.
  * 10 Nocedal J, Wright SJ. Numerical Optimization.  New York, NY, USA: Springer.  second ed., 2006.
  * 11 Forsgren A, Gill PE, Wright MH. Interior Methods for Nonlinear Optimization. SIAM Review. 2002;44(4):525–597.
  * 12 Hestenes MR. Multiplier and gradient methods. Journal of Optimization Theory and Applications. 1969;4:303-320.
  * 13 Boyd S, Parikh N, Chu E, Peleato B, Eckstein J. Distributed Optimization and Statistical Learning via the Alternating Direction Method of Multipliers. Foundations and Trends in Machine Learning. 2011;3:1-122.
  * 14 Boggs P, W. Tolle J. Sequential Quadratic Programming. Acta Numerica. 1995;4:1-51.
  * 15 Fletcher R, Leyffer S, Toint PL. On the Global Convergence of a Filter–SQP Algorithm. SIAM Journal on Optimization. 2002;13(1):44-59.
  * 16 Armijo L. Minimization of functions having Lipschitz continuous first partial derivatives.. Pacific Journal of Mathematics. 1966;16(1):1 – 3.
  * 17 Wächter A, Biegler LT. On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming. Mathematical Programming. 2006;106:25-57.
  * 18 Wang Y, Boyd S. Fast Model Predictive Control Using Online Optimization. IEEE Transactions on Control Systems Technology. 2010;18(2):267-278.
  * 19 Pavlov A, Shames I, Manzie C. Interior Point Differential Dynamic Programming. IEEE Transactions on Control Systems Technology. 2021;PP:1-8.
  * 20 Jallet W, Bambade A, Arlaud E, El-Kazdadi S, Mansard N, Carpentier J. PROXDDP: Proximal Constrained Trajectory Optimization. working paper or preprintnot; 2023\.
  * 21 Geisert M, Del Prete A, Mansard N, Romano F, Nori F. Regularized Hierarchical Differential Dynamic Programming. IEEE Transactions on Robotics. 2017;33(4):819-833.
  * 22 Wang Y, Liu Y, Leibold M, Buss M, Lee J. Hierarchical Incremental MPC for Redundant Robots: a Robust and Singularity-Free Approach. IEEE Transactions on Robotics. 2024:1-20.
  * 23 Tazaki Y, Suzuki T. Constraint-Based Prioritized Trajectory Planning for Multibody Systems. IEEE Transactions on Robotics. 2014;30(5):1227-1234.
  * 24 Topcu A. A contribution to the systematic analysis of finite element structures using the force method. Ph.D. thesis, University of Essen, Germany. 1979\.
  * 25 Gilbert JR, Heath MT. Computing a Sparse Basis for the Null Space. SIAM Journal on Algebraic Discrete Methods. 1987;8(3):446-459.
  * 26 Yang J, Meijer T, Dolk V, Jager Bd, Heemels W. A System-Theoretic Approach to Construct a Banded Null Basis to Efficiently Solve MPC-Based QP Problems. In: 2019:1410-1415.
  * 27 Pfeiffer K, Escande A, Kheddar A. Singularity Resolution in Equality and Inequality Constrained Hierarchical Task-Space Control by Adaptive Nonlinear Least Squares. IEEE Robotics and Automation Letters. 2018;3(4):3630-3637.
  * 28 Cococcioni M, Pappalardo M, Sergeyev YD. Lexicographic multi-objective linear programming using grossone methodology: Theory and algorithm. Applied Mathematics and Computation. 2018;318:298-311.  Recent Trends in Numerical Computations: Theory and Algorithms.
  * 29 Chiaverini S. Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators. IEEE Transactions on Robotics and Automation. 1997;13(3):398–410.
  * 30 Higham N. Computing the Polar Decomposition with Applications. SIAM Journal on Scientific and Statistical Computing. 1986;7(4):1160-1174.
  * 31 Golub GH, Van Loan CF. Matrix Computations (3rd Ed.).  Baltimore, MD, USA: Johns Hopkins University Press, 1996.
  * 32 Broyden CG. The Convergence of a Class of Double-rank Minization Algorithms. Journal of the Mathematics and its Applications. 1970;6:76–90.
  * 33 Moré JJ. The Levenberg-Marquardt algorithm: Implementation and theory. In: Watson GA. , ed. Numerical AnalysisSpringer Berlin Heidelberg 1978; Berlin, Heidelberg:105-116.
  * 34 Pfeiffer K, Escande A, Righetti L. $\mathcal{N}$IPM-HLSP: an efficient interior-point method for hierarchical least-squares programs. Optimization and Engineering. 2023:1573-2924.
  * 35 Liu Z, Guo F, Wang W, Wu X. A distributed parallel optimization algorithm via alternating direction method of multipliers. IET Control Theory & Applications. 2023;17(7):896-905.  doi: https://doi.org/10.1049/cth2.12421
  * 36 Dang TV, Ling KV, Maciejowski J. Banded Null Basis and ADMM for Embedded MPC. IFAC-PapersOnLine. 2017;50(1):13170-13175.  20th IFAC World Congress.
  * 37 Stellato B, Banjac G, Goulart P, Bemporad A, Boyd S. OSQP: an operator splitting solver for quadratic programs. Mathematical Programming Computation. 2020;12(4):637–672.
  * 38 Eckstein J, Bertsekas D. On the Douglas-Rachford splitting method and the proximal point algorithm for maximal monotone operators. Mathematical Programming. 1992;55:293-318.
  * 39 Ghadimi E, Teixeira A, Shames I, Johansson M. Optimal Parameter Selection for the Alternating Direction Method of Multipliers (ADMM): Quadratic Problems. IEEE Transactions on Automatic Control. 2014\.
  * 40 Rao AV, Benson DA, Darby C, et al. Algorithm 902: GPOPS, A MATLAB Software for Solving Multiple-Phase Optimal Control Problems Using the Gauss Pseudospectral Method. ACM Trans. Math. Softw.. 2010;37(2).
  * 41 Li Z, Ge SS. Adaptive robust controls of biped robots. IET Control Theory & Applications. 2013;7(2):161-175.  doi: https://doi.org/10.1049/iet-cta.2012.0066
  * 42 Carpentier J, Mansard N. Analytical Derivatives of Rigid Body Dynamics Algorithms. Robotics: Science and Systems XIV. 2018\.
  * 43 Giftthaler M, Neunert M, Stäuble M, Buchli J, Diehl M. A Family of Iterative Gauss-Newton Shooting Methods for Nonlinear Optimal Control. 2017.
  * 44 Singh S, Russell RP, Wensing PM. Efficient Analytical Derivatives of Rigid-Body Dynamics Using Spatial Vector Algebra. IEEE Robotics and Automation Letters. 2021;7:1776-1783.
  * 45 Knight PA, Ruiz D, Uçar B. A Symmetry Preserving Algorithm for Matrix Scaling. SIAM Journal on Matrix Analysis and Applications. 2014;35(3):931-955.
  * 46 Udwadia F, Schutte A. Equations of motion for general constrained systems in Lagrangian mechanics. Acta Mech. 2010;213.
  * 47 Lu TT, Shiou SH. Inverses of 2 × 2 block matrices. Computers & Mathematics with Applications. 2002;43(1):119-129.
  * 48 Pfeiffer K, Righetti L. $\mathcal{N}$IPM-MPC: An Efficient Null-Space Method Based Interior-Point Method for Model Predictive Control. 2021.
  * 49 Guennebaud G, Jacob B, others . Eigen v3. http://eigen.tuxfamily.org; 2010.
  * 50 Carpentier J, Saurel G, Buondonno G, et al. The Pinocchio C++ library – A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives. In: 2019.
  * 51 Bell M. CppAD: A package for differentiation of C++ algorithms (2024/02/01. http://www.coin-or.org/CppAD; 2024.
  * 52 Paige CC, Saunders Ma. LSQR: An Algorithm for Sparse Linear Equations and Sparse Least Squares. ACM Transactions on Mathematical Software. 1982;8(1):43–71.
  * 53 Gill PE, Murray W, Saunders MA, Wright MH. Maintaining LU factors of a general sparse matrix. Linear Algebra and its Applications. 1987;88-89:239-270.
  * 54 ApS M. MOSEK Fusion API for C++ 10.1.12. 2019.
  * 55 Gurobi Optimization, LLC . Gurobi Optimizer Reference Manual. 2023.
  * 56 Chen T, Lasserre JB, Magron V, Pauwels E. A sublevel moment-SOS hierarchy for polynomial optimization. Computational Optimization and Applications. 2021;81:31 - 66.
  * 57 Cavdaroglu M, Olgac N. Trajectory tracking of cart-pendulum dynamics using multiple time-delayed feedback. Control Theory & Applications, IET. 2008;2:458 - 466.  doi: 10.1049/iet-cta:20070242
  * 58 Tenny MJ, Wright SJ, Rawlings JB. Nonlinear Model Predictive Control via Feasibility-Perturbed Sequential Quadratic Programming. Computational Optimization and Applications. 2004;28(1):87-121.