#  $\mathcal{N}$IPM-HLSP: An Efficient Interior-Point Method for Hierarchical
Least-Squares Programs

Kai Pfeiffer1, Adrien Escande2,3, Ludovic Righetti1 1Machines in Motion
Laboratory, Tandon School of Engineering, New York University, New York,
USA2CNRS-AIST JRL (Joint Robotics Laboratory), IRL, Tsukuba, Japan3National
Institute of Advanced Industrial Science and Technology (AIST), Tsukuba,
JapanPart of this work was supported by New York University, NSF grants
1936332, 1824434, 1833666, 1564142, 1925079, 1825993; NYU WIRELESS and its
industrial affiliates; NIST grant 70NANB17H166; SRC; and a research grant from
OPPO.

###### Abstract

Hierarchical least-squares programs with linear constraints (HLSP) are a type
of optimization problem very common in robotics. Each priority level contains
an objective in least-squares form which is subject to the linear constraints
of the higher priority hierarchy levels. Active-set methods (ASM) are a
popular choice for solving them. However, they can perform poorly in terms of
computational time if there are large changes of the active set. We therefore
propose a computationally efficient primal-dual interior-point method (IPM)
for HLSP’s which is able to maintain constant numbers of solver iterations in
these situations. We base our IPM on the null-space method which requires only
a single decomposition per Newton iteration instead of two as it is the case
for other IPM solvers. After a priority level has converged we compose a set
of active constraints judging upon the dual and project lower priority levels
into their null-space. We show that the IPM-HLSP can be expressed in least-
squares form which avoids the formation of the quadratic Karush-Kuhn-Tucker
(KKT) Hessian. Due to our choice of the null-space basis the IPM-HLSP is as
fast as the state-of-the-art ASM-HLSP solver for equality only problems.

##  I Introduction

###  I-A Hierarchical control in robotics

Hierarchical control is a well established and well-used methodology in
robotics. It allows the full exploitation of the robot’s kinematic redundancy
resulting in increased dexterity and versatility [1]. Furthermore, it helps
reflecting the physical reality of the robot as accurately as possible. An
example would be to separate the equation of motion from self-collision or
geometrical contact constraints by defining them on different priority levels
[2, 3]. Prioritized controllers also exhibit a high level of safety since they
enable to strictly separate safety constraints, physical stability constraints
and objective tasks.

Starting with the seminal work of [1], the authors proposed an efficient way
of computing a hierarchical control law where each task is only fulfilled to
such a degree that the performance of higher priority tasks is not influenced.
The authors in [4] proposed a similar approach but avoid the issue of
algorithmic singularities which arise when a task is in conflict with a higher
priority one. These methods have been successfully implemented for both
velocity [5] and torque [6] controlled robots. However, they do not enable the
incorporation of inequality constraints like joint limits [7], collision
avoidance [8] or visibility [9]. The integration of such tasks by means of a
potential function on the highest priority level has been proposed [10] but
this boils down to a reformulation to an equality constraint. For a continuous
formulation without switches of priority order [11] this means that the
constraint is always active and may negatively influence the robot control
performance. With the rise of optimization based control approaches in
robotics however, in particular least-squares (LSP) programming [12, 13, 14,
15], several works have been presented to provide a framework to incorporate
inequality constraints. The authors in [11] solve each level of the priority
as its own optimization problem. Introducing slack variables enables the
incorporation of inequality constraints on any priority level. A big
disadvantage of this approach is that each optimization problem grows in the
number of constraints since the ones from the previous levels need to be
considered. In [16] this is avoided by only considering feasible inequality
constraints on the first level. By solving each level in the null-space of the
previous ones the problem dimensions are progressively reduced. The approach
considered in [17] is also based on such a formulation of the null-space
method but enables the incorporation of inequality constraints on any priority
level. Unlike the previous cascade-like approaches [11, 16] their active-set
search solves the whole hierarchy at once which adds further efficiency by
unifying the search of all the levels into one. The latest dedicated solver
for hierarchical least-squares programs proposes the use of non-orthogonal
bases of the null-space which exposes a computationally efficient block
structure [18].

###  I-B The hierarchical active-set method

A hierarchical LSP with linear constraints (HLSP) is an optimization problem
which is not limited to a single least squares objective with linear
constraints but is extended to an arbitrary number of priority levels. Each
level consists of a least squares objective which is subject to the linear
constraints of the higher priority levels. In the sense of [11] such an
optimization problem boils down to the following form

$\displaystyle\operatorname*{min.}_{x,v_{l,e},v_{l,i}}$
$\displaystyle\qquad\frac{1}{2}\|v_{l,e}\|^{2}+\frac{1}{2}\|v_{l,i}\|^{2}\qquad
l=1,\dots,p$ (1) s.t. $\displaystyle\qquad A_{l,e}x-b_{l,e}=v_{l,e}$
$\displaystyle\qquad A_{l,i}x-b_{l,i}\geq v_{l,i}$
$\displaystyle\qquad\underline{A}_{l-1,e}x-\underline{b}_{l-1,e}=\underline{v}_{l-1,e}^{*}$
$\displaystyle\qquad\underline{A}_{l-1,i}x-\underline{b}_{l-1,i}\geq 0$

The variable vector $x\in\mathbb{R}^{n}$ consists of $n$ variables. The slack
variables $v_{l,e}$ and $v_{l,i}$ relax the equality and inequality
constraints $A_{l,e}$ and $A_{l,i}$ of the current level $l$. This relaxation
allows handling of inequality and infeasible constraints on any priority level
[19]. The hierarchy is resolved in a cascade-like manner from the first to the
last level $p$ while not violating the inequality constraints
$\underline{A}_{l-1,i}$ and not changing the optimal relaxation
$\underline{v}_{l-1,e}^{*}$ identified for the equality constraints
$\underline{A}_{l-1,e}$ of the previous levels. Underlined matrices and
vectors are concatenations of the form
$\underline{M}_{l-1}=\begin{bmatrix}M_{1}^{T}&\cdots&M_{l-1}^{T}\end{bmatrix}^{T}$.

The dedicated solvers for HLSP’s [17, 18] are both based on the ASM. At each
solver iteration an equality only problem consisting of all equality
constraints and the active inequality constraints
$\underline{\mathcal{A}}_{l-1}$ from the lower priority levels $1$ to $l-1$ is
solved. The most violated constraint of all the priority levels is then added
to the active set. If all constraints are feasible, the dual is evaluated and
the constraint preventing progression towards the optimal the most is removed.
The ASM converges if no constraint needs to be added to or removed from the
active set. In ASM’s based on the null-space method the objective matrix
$A_{l}$ is projected into the null-space of the active constraints
$\underline{A}_{\underline{\mathcal{A}}_{l-1}}$ [20, 21]. With the right
choice of null-space basis this leads to a possibly significant reduction of
variables especially on lower priority levels in case of a large number of
linearly independent active constraints. This makes the solvers very
efficient. While [17] uses orthogonal bases the authors in [18] implement non-
orthogonal bases with a further computational advantage due to their block
structure. To the best of our knowledge, it is the state-of-the-art HLSP
solver and is most efficient on problems which can be warm-started with a
limited number of active-set iterations. For this solver some computational
improvements have been proposed [22] in order to efficiently handle
significant changes of the active-set. However, a strategy for decomposition
updates, which would bring significant computational improvements in case of
large number of active set iterations, has yet to be proposed.

The ASM is only computationally tractable if there are limited changes of the
active set. This is usually the case for slowly developing robot problems
(from a control rate perspective) and can be especially leveraged by warm
starting the active set $\mathcal{A}^{(i)}$ of the current control iteration
$i$ with the active set $\mathcal{A}^{(i-1)}$ of the previous control
iteration $i-1$ [23]. However, in robotic real-time control there are cases
where large shifts of the active set occur like in instances close to contact
shifting or oscillations in case of numerical instabilities due to ill-posed
constraint matrices. In these cases the convergence rate of the ASM may be
exponential in the number of constraints as all possible active-set
combinations need to be explored [24]. In the robotics control case this may
occur in the presence of kinematic or algorithmic singularities [4] (an
efficient way of resolving such singularities in the kinematic and dynamic
hierarchical control case has been proposed in [25, 26]). Numerical issues
like cycling can further increase the number of active set iterations such
that it becomes intractable for real-time robot control. While methods
mitigating such effects like decomposition updates [27] and cycling handling
[28] exist, they might not be enough to make up for the possibly large number
of active set iterations until convergence [26].

This is in contrast to the IPM where all constraints including inactive
inequality constraints are included. Consequently, a single iteration of the
IPM is relatively expensive but convergence is achieved robustly in a
deterministic number of iterations independent of conditioning [29, 30]. The
authors in [31] for example use this circumstance and switch from the fast ASM
to the reliable IPM in case of ASM failures. The IPM is more suitable for
large scale problems with a large number of variables and constraints while
for small scale problems the ASM is usually preferred [32]. This also applies
to a further method of resolving QP’s, the Alternating Direction Method of
Multipliers (ADMM) [33, 34]. This operator-splitting method has very good
practical convergence behavior, warm-start capabilities and low iteration
cost. While the solver is not designed to converge at high accuracy, it has
been recently proposed to derive active set guesses from intermediate solver
states and solve the corresponding equality only problem [35]. If the active-
set guess is correct the solution has zero primal and dual residual. The same
framework also overcomes tuning difficulties associated with operator-
splitting methods and requires only a single factorization for solving the QP,
making it very fast especially for large-scale problems.

###  I-C Our contribution

While the IPM has been developed for LSP [36] and could be used to solve (1)
directly, there exist no dedicated IPM (or ADMM thereof) based solvers for
HLSP. In this work we will provide the theoretical foundations to achieve just
that. Our IPM-HLSP formulation is as fast as the state-of-the-art ASM-HLSP
solver [18] in case of equality only problems. At the same time the IPM based
HLSP solver provides predictability in terms of computation times due to its
constant number of Newton iterations even in case of ill-posed constraints
matrices. Here the ASM-HLSP oftentimes requires large number of active set
iterations and is therefore superseded by the IPM-HLSP. This can also be
relevant when critical safety constraints need to be dealt with but the ASM-
HLSP fails to find an optimal point in a reasonable number of active set
iterations.

Our contributions are threefold:

  * •

With our first contribution we formulate the IPM for HLSP. We thereby propose
a HLSP solver purely based on the IPM but also present a solver design that is
a combination of both the IPM and the ASM which utilizes the advantages of
both schemes - that is reliability and fastness.

  * •

We suggest to apply the null-space method to the IPM for HLSP instead of the
commonly used Schur complement [37, 38]. While this has been previously done
for QP’s in MPC [39] we give a more detailed explanation for example on our
choice of null-space basis. With this formulation we reduce the necessary
number of decompositions of the KKT system per Newton iteration from two to
one. Furthermore, the number of times the Lagrange multipliers associated with
the equality constraints are evaluated can be reduced significantly with a
special convergence test.

  * •

We show that the projected normal equations of the IPM-HLSP can be expressed
in least squares form. This way the formation of expensive matrix products is
avoided and a basic solution can be very efficiently computed by a cheap QR
decomposition.

This paper is structured as follows: In sec. II we first introduce the notion
of active sets into the optimization problem (1). We then continue to outline
the formulation of the IPM-HLSP and the corresponding hierarchical augmented
system. Section III oversees the efficient computation of the IPM solver
iterations. We apply the null-space method to the augmented system which
results in the projected normal equations, sec. III-A. This requires a concept
we refer to as virtual priority levels and which is detailed in sec. III-B.
The choice of null-space basis is given in sec. III-C. With a special
convergence test the calculation of the dual associated with the equality
constraints in every Newton iteration can be avoided (sec. III-D). In sec.
III-E the projected normal equations are expressed as a least squares problem.
Section III-F oversees the development of Mehrotra’s predictor-corrector-
algorithm for HLSP’s. Bound constraints can be specially handled by a sparse
QR decomposition (sec. III-E2) for a more computationally efficient algorithm
which is finally outlined in sec. IV. Finally, we give a computational
comparison between the classical and projected normal equations and the least
squares form (sec. V). The proposed algorithms are evaluated in sec. VI. We
conclude the paper with some remarks and considerations for future work (see
sec. VII).

The solver is open source and its C++ code based on the Eigen library [40] is
available at https://www.github.com/pfeiffer-kai/NIPM-HLSP.

##  II Formulating the IPM-HLSP

First, we look at the top three lines of the above optimization problem (1).
This problem corresponds to finding a feasible point ($v_{l,e}^{*}=0$,
$v_{l,i}^{*}\geq 0$) or optimal infeasible point ($v_{l,e}^{*}\neq 0$,
$v_{l,i}^{*}<0$) of the constraints. In the context of linear programming the
self-dual embedding model [41] has been proposed. Just like [37] for quadratic
programming, infeasible initial points with respect to the equality
constraints are handled but inequality constraints are assumed to be feasible
with $A_{l,i}x-b_{l,i}\geq 0$. An algorithm overcoming this issue is proposed
in [23] where the initial feasible point is determined by minimizing the sum
of infeasibilities. However, the algorithm fails when no feasible but only an
optimal infeasible point $A_{l,i}x-b_{l,i}<0$ exists. The solvers proposed in
[19, 17, 18] are based on the ASM and are able of handling all above cases.

We rewrite (1), which is only valid if all inequality constraints on all
levels are feasible, to

$\displaystyle\operatorname*{min.}_{x,v_{l,e},v_{l,i}}$
$\displaystyle\qquad\frac{1}{2}\|v_{l,e}\|^{2}+\frac{1}{2}\|v_{l,i}\|^{2}\qquad
l=1,\dots,p$ (2) s.t. $\displaystyle\qquad A_{l,e}x-b_{l,e}=v_{l,e}$
$\displaystyle\qquad A_{l,i}x-b_{l,i}\geq v_{l,i}$
$\displaystyle\qquad\underline{A}_{\underline{\mathcal{A}}_{l-1}}x-\underline{b}_{\underline{\mathcal{A}}_{l-1}}=\underline{v}_{\underline{\mathcal{A}}_{l-1}}^{*}$
$\displaystyle\qquad\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}x-\underline{b}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\geq
0$

The active set $\underline{\mathcal{A}}_{l-1}$ represents all equality
constraints and the inequality constraints that were infeasible and violated
by $v_{\underline{\mathcal{A}}_{l-1}}^{*}$ at the optimal points of the levels
$1$ to $l-1$. Consequently,
$\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ are the remaining
inequality constraints that were feasible and satisfied at the optimal points
of the levels $1$ to $l-1$ and which are labeled as ‘inactive’
($\underline{\mathcal{A}}_{l-1}^{\notin}$). More details on sourcing these
active sets are given in sec. III-B.

The aim of the optimization problem is now

  * •

to keep the previous levels $1,\dots,l-1$ optimal at the current violation
$v_{\underline{\mathcal{A}}_{l-1}}^{*}$ $\rightarrow$ Null-space method

  * •

to keep the inactive inequality constraints feasible
$A_{\underline{\mathcal{A}}_{l-1}^{\notin}}x-b_{\underline{\mathcal{A}}_{l-1}^{\notin}}\geq
0$ $\rightarrow$ IPM

  * •

while finding the feasible or optimal infeasible point $v_{l,e}^{*}$ and
$v_{l,i}^{*}$ of the $l$-th level
$\rightarrow$ IPM or ASM.

The first point is outlined in the next section III-A. The second and third
point we intend to achieve by the IPM. The third point can equally be achieved
by the ASM as is explained in Appendix E. While very good behavior is achieved
in practice mathematical proof of convergence may be required for the overlay
of the IPM with the ASM. Therefore, throughout this paper we focus on the pure
IPM-HLSP formulation but include the IPM-ASM-HLSP into our evaluation which
proves to be superior in terms of computation times with respect to the pure
IPM-HLSP formulation.

We introduce two positive slack variables $w_{l,i}$ for the inequality
constraints on the current level $l$ and
$\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ for the inactive
inequality constraints of the previous levels. They are then bounded away from
zero by the log-barrier function:

$\displaystyle\min_{x}.$
$\displaystyle\qquad\frac{1}{2}\|v_{l,e}\|^{2}+\frac{1}{2}\|v_{l,i}\|^{2}-\sigma_{l,i}\mu_{l,i}\sum\log(w_{l,i})$
(3)
$\displaystyle\qquad-\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}\sum\log(\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}})$
s.t. $\displaystyle\qquad A_{l,e}x-b_{l,e}=v_{l,e}$ $\displaystyle\qquad
A_{l,i}x-b_{l,i}-v_{l,i}=w_{l,i}$ $\displaystyle\qquad w_{l,i}\geq 0$
$\displaystyle\qquad\underline{A}_{\underline{\mathcal{A}}_{l-1}}x-\underline{b}_{\underline{\mathcal{A}}_{l-1}}=\underline{v}_{\underline{\mathcal{A}}_{l-1}}^{*}$
$\displaystyle\qquad\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}x-\underline{b}_{\underline{\mathcal{A}}_{l-1}^{\notin}}=\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$
$\displaystyle\qquad\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\geq
0$

The Lagrangian of the optimization problem (3) writes as

$\displaystyle\mathcal{L}$
$\displaystyle=\frac{1}{2}\|v_{l,e}\|^{2}+\frac{1}{2}\|v_{l,i}\|^{2}$ (4)
$\displaystyle-\sigma_{l,i}\mu_{l,i}\sum\log(w_{l,i})-\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}\sum\log(\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}})$
$\displaystyle-\lambda_{l,e}^{T}(A_{l,e}x-b_{l,e}-v_{l,e})-\lambda_{l,i}^{T}(A_{l,i}x-b_{l,i}-v_{l,i}-w_{l,i})$
$\displaystyle-\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}^{T}(\underline{A}_{\underline{\mathcal{A}}_{l-1}}x-\underline{b}_{\underline{\mathcal{A}}_{l-1}}-\underline{v}_{\underline{\mathcal{A}}_{l-1}}^{*})$
$\displaystyle-\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}(\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}x-\underline{b}_{\underline{\mathcal{A}}_{l-1}^{\notin}}-\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}})$

The Lagrange multipliers associated with the inactive inequality constraints
of the higher priority levels $1,\dots,l-1$ are
$\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$
($\underline{\Lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}=\operatorname{diag}(\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}})$).

The first order optimality or KKT conditions $\nabla_{q}\mathcal{L}=0$ with
the variable vector $q$

$q\coloneqq\begin{bmatrix}x^{T}&v_{l,e}^{T}&v_{l,i}^{T}&w_{l,i}^{T}&\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}^{T}&\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}^{\notin}&\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}\end{bmatrix}^{T}$
(5)

are (in a slightly rewritten form)

$\displaystyle K_{l}$
$\displaystyle\coloneqq\begin{bmatrix}k_{l,1}^{T}&k_{l,2}^{T}&k_{l,3}^{T}&k_{l,4}^{T}&k_{l,5}^{T}&k_{l,6}^{T}&k_{l,7}^{T}\end{bmatrix}^{T}=$
(6)
$\displaystyle\begin{bmatrix}A_{l,e}^{T}v_{l,e}+A_{l,i}^{T}v_{l,i}-\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{T}\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}-\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\\\
b_{l,e}-A_{l,e}x+v_{l,e}\\\ b_{l,i}-A_{l,i}x+v_{l,i}+w_{l,i}\\\ w_{l,i}\odot
v_{l,i}+\sigma_{l,i}\mu_{l,i}e\\\
\underline{b}_{\underline{\mathcal{A}}_{l-1}}-\underline{A}_{\underline{\mathcal{A}}_{l-1}}x+\underline{v}_{\underline{\mathcal{A}}_{l-1}}^{*}\\\
\underline{b}_{\underline{\mathcal{A}}_{l-1}^{\notin}}-\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}x+\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\\\
\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\odot\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}-\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}e\end{bmatrix}=0$

with the substitutions $v_{l,e}=-\lambda_{l,e}$ and $v_{l,i}=-\lambda_{l,i}$.

The duality measures $\mu$ and the centering parameters $\sigma$ [42] are
given by

$\displaystyle\mu_{l,i}$
$\displaystyle=\lambda_{l,i}^{T}w_{i}/(n+{m}_{l,i})=-v_{l,i}^{T}w_{i}/(n+{m}_{l,i})$
(7) $\displaystyle\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}$
$\displaystyle=\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}/(n+\underline{m}_{\underline{\mathcal{A}}_{l-1}^{\notin}})$
(8)
$\displaystyle\sigma_{l,i},\sigma_{\underline{m}_{\underline{\mathcal{A}}_{l-1}^{\notin}}}\in[0,1]$
(9)

or the values can be determined by Mehtrotra’s predictor-corrector algorithm
[43], see sec. III-F.

Additionally, we have the dual feasibility conditions which are ensured by
line search. The IPM for finding the initial feasible or optimal infeasible
point requires

$v_{l,i}\leq 0\qquad\text{and}\qquad w_{l,i}\geq 0$ (10)

For the IPM of the inactive constraints we have

$\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\geq
0\qquad\text{and}\qquad\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\geq
0$ (11)

The KKT conditions are non-linear so we apply the Newton step

$K_{l}(q+\Delta q)\approx K_{l}(q)+\nabla_{q}K_{l}(q)\Delta q$ (12)

with the KKT Hessian

$\displaystyle\nabla_{q}K_{l}(q)\coloneqq$ (13)
$\displaystyle\begin{bmatrix}0&A_{l,e}^{T}&A_{l,i}^{T}&0&-\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{T}&-\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}&0\\\
-A_{l,e}&I&0&0&0&0&0\\\ -A_{l,i}&0&I&I&0&0&0\\\ 0&0&W_{l,i}&V_{l,i}&0&0&0\\\
-\underline{A}_{\underline{\mathcal{A}}_{l-1}}&0&0&0&0&0&0\\\
-\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}&0&0&0&0&0&I\\\
0&0&0&0&0&\underline{W}_{\underline{\mathcal{A}}_{l-1}^{\notin}}&\underline{\Lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\end{bmatrix}$

We sequentially apply substitutions for $\Delta v_{l,e}$, $\Delta v_{l,i}$,
$\Delta\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$, $\Delta
w_{l,i}$ and
$\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ which
leads to the hierarchical augmented system

$\displaystyle\begin{bmatrix}C_{l}&-\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{T}\\\
-\underline{A}_{\underline{\mathcal{A}}_{l-1}}&0\end{bmatrix}\begin{bmatrix}\Delta
x\\\
\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}\end{bmatrix}=\begin{bmatrix}r_{l,1}\\\
r_{l,2}\end{bmatrix}$ (14)

with

$\displaystyle C_{l}$
$\displaystyle=A_{l,e}^{T}A_{l,e}+A_{l,i}^{T}\left(I+({V_{l,i}-W_{l,i}})^{-1}{W_{l,i}}\right)A_{l,i}$
(15)
$\displaystyle+\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}\underline{W}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\underline{\Lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$

and the right hand side

$\displaystyle r_{1}$
$\displaystyle=\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{T}\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}+\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}F+A_{l,e}^{T}(b_{l,e}-A_{l,e}x)+A_{l,i}^{T}G$
(16) $\displaystyle r_{2}$
$\displaystyle=\underline{A}_{\underline{\mathcal{A}}_{l-1}}x-\underline{b}_{\underline{\mathcal{A}}_{l-1}}-\underline{w}_{\underline{\mathcal{A}}_{l-1}}^{*}$
(17)

$G$ and $F$ are given by

$\displaystyle F$
$\displaystyle=\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}+W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}(\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\odot(\underline{b}_{\underline{\mathcal{A}}_{l-1}^{\notin}}-\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}x)$
(18)
$\displaystyle+\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}e)$
$\displaystyle G$ $\displaystyle=b_{l,i}-A_{l,i}x+w_{l,i}$ (19)
$\displaystyle-({V_{l,i}-W_{l,i}})^{-1}({\sigma_{l,i}\mu_{l,i}e+w_{l,i}\odot(A_{l,i}x-b_{l,i}-w_{l,i})})$

respectively.

The Newton’s method (14) of level $l$ is now solved repeatedly until the norm
of the KKT conditions $\left\|K_{l}\right\|_{2}<\epsilon$ (6) is below a
certain threshold $\epsilon=10^{-12}$.

##  III Computing the IPM-HLSP

###  III-A The null-space method based IPM-HLSP

Solving the augmented system (14) directly is inefficient as the zero block in
the lower right corner of the left hand side matrix would be ignored. One way
to circumvent this is to form the Schur complement [38]

$\displaystyle\Delta x$
$\displaystyle=C_{l}^{-1}(r_{l,1}+\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{T}\Delta\lambda_{\underline{\mathcal{A}}_{l-1}})$
(20)

The resulting ‘classical’ normal equations are given below:

$\underline{A}_{\underline{\mathcal{A}}_{l-1}}C_{l}^{-1}\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{T}\Delta\lambda_{\underline{\mathcal{A}}_{l-1}}=-r_{l,2}-\underline{A}_{\underline{\mathcal{A}}_{l-1}}C_{l}^{-1}r_{l,2}$
(21)

This substitution yields a dense matrix
$A_{\underline{\mathcal{A}}_{l-1}}C_{l}^{-1}A_{\underline{\mathcal{A}}_{l-1}}^{T}$
and potential sparsity is destroyed [42]. This is especially relevant in
robotics where the columns rather than the rows of
$\underline{A}_{\underline{\mathcal{A}}_{l-1}}$ are zero when considering
multi-limbed robots with tree-like structures. Additionally, in order to solve
this system two decompositions per Newton iteration are required, one for
$C_{l}^{-1}$ and one for
$\underline{A}_{\underline{\mathcal{A}}_{l-1}}C_{l}^{-1}\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{T}$.
Efficient strategies especially in the context of Model-Predictive-Control
(MPC) leveraging banded matrix structures have been proposed [38]. However, we
would like to avoid the computation of two decompositions per Newton iteration
after all. First, we assume that Furthermore, in robotic control hierarchies,
$C_{l}$ is generally not full rank (unless it is a regularization term on some
low priority level) which renders the classical normal equations not
applicable for hierarchies (since $C_{l}^{-1}$ can not be computed). In case
that $C_{l}$ is full rank, there might be linear dependent constraints in
$\underline{A}_{\underline{\mathcal{A}}_{l-1}}$ at which point every Newton
iteration requires a rank revealing decomposition for
$\underline{A}_{\underline{\mathcal{A}}_{l-1}}C_{l}^{-1}\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{T}\in\mathbb{R}^{\underline{m}_{\underline{\mathcal{A}}_{l-1}},\underline{m}_{\underline{\mathcal{A}}_{l-1}}}$
($O(4/3\underline{m}_{\underline{\mathcal{A}}_{l-1}}^{3})$ in case of QR
decomposition).

However, we can avoid the computation of two decompositions per Newton
iteration by first assuming that

$r_{l,2}=0$ (22)

This condition holds as $x$ is updated after every Newton iteration during the
resolution of the higher priority levels $l=1,\dots,l-1$ and is therefore
feasible with respect to the active constraints
$\underline{\mathcal{A}}_{l-1}$. The computation of $r_{l,1}$ requires this
step since it depends on the latest $x$. On the other hand, ASM-HLSP requires
the evaluation of the new $x$ only at the end of each active set iteration
resolving the equality only HLSP.

We then apply the null-space method [44] by introducing the variable change

$\Delta x=N_{l-1}\Delta z$ (23)

The null-space basis $N_{l-1}$ of
$\underline{A}_{\underline{\mathcal{A}}_{l-1}}$ fulfills the condition
$\underline{A}_{\underline{\mathcal{A}}_{l-1}}N_{l-1}=0$. Additionally,
$\underline{A}_{\underline{\mathcal{A}}_{l-1}}\Delta x=0$ such that the
condition $r_{l,2}=0$ continues to be fulfilled.

An additional multiplication of the first row of (14) by $N_{l-1}^{T}$ from
the left results in the projected hierarchical normal equations

$\displaystyle N_{l-1}^{T}C_{l}N_{l-1}\Delta z=N_{l-1}^{T}r_{l,1}$ (24)

The Lagrange multipliers $\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}$
corresponding to the active constraints $\underline{\mathcal{A}}_{l-1}$ are
obtained by solving

$\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{T}\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}=C_{l}\Delta
x-r_{l,1}$ (25)

An efficient recursive method of calculation is detailed in Appendix A.

###  III-B Notion of virtual priority levels

Once the Newton’s method of a priority level $l$ has converged constraints
from $\underline{A}_{\underline{\mathcal{A}}_{l}^{\notin}}$ are either
saturated with
$\underline{A}_{\underline{\mathcal{A}}_{l}^{\notin}}x-\underline{b}_{\underline{\mathcal{A}}_{l-1}^{\notin}}=0$
or satisfied with
$\underline{A}_{\underline{\mathcal{A}}_{l}^{\notin}}x-\underline{b}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\geq
0$. Additionally, inequality constraints from the current level $l$ are either
satisfied with $A_{l,i}x-b_{l,i}\geq 0$ or violated with $A_{l,i}x-b_{l,i}<0$.
In order to be able to proceed with the cascade-like resolution of the next
priority level we need to determine two distinct active sets
$\mathcal{A}_{l^{*}}$ and $\mathcal{A}_{l}$
($\underline{\mathcal{A}}_{l}=\begin{bmatrix}\mathcal{A}_{1}^{*}&\mathcal{A}_{1}&\cdots&\mathcal{A}_{l}^{*}&\mathcal{A}_{l}\end{bmatrix}$).
The index $l^{*}$ represents a ‘virtual’ priority level whose active set
$\mathcal{A}_{l^{*}}$ contains saturated constraints from
$\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{\notin}$. This is necessary as
the projection of $\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{\notin}$
into the null-space basis of the active constraints of level $l$ would
contradict the priority order. Therefore, a virtual priority level $l^{*}$ has
lower priority than level $l-1$ but higher priority than level $l$ which
comprises of the equality constraints $A_{l,e}$ and the activated constraints
from $A_{l,i}$.

This also has the effect of reducing the size of
$\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ while a significant
null-space basis can be augmented which reduces the number of variables for
the Newton’s method of the next priority level for a cheaper algorithm.

We activate a constraint from
$\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ if the corresponding
pair of slack and Lagrange multiplier fulfills the conditions

$\displaystyle\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}<\xi\qquad\text{
and }\qquad\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}>\xi$
(26)

While $\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}(i)<\xi$ ensures
that only saturated constraints are activated, the condition
$\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}(i)>\xi$ only
selects constraints that are in significant conflict with constraints from
level $l$ and are not just accidentally saturated, for example by a randomly
chosen initial point. Otherwise the ability of resolving the lower priority
level $l$ is artificially restricted by an unnecessarily inflated active set
$\underline{\mathcal{A}}_{l-1}$.

The thresholding $\xi=10^{-8}$ is necessary as the Newton’s method is usually
not run to complete convergence $\|K_{l}\|_{2}=0$ but stopped earlier with
some threshold $\|K_{l}\|_{2}<\epsilon$. Note that our small choice for $\xi$
requires the Newton’s method to converge to a similarly accurate degree
$\epsilon=10^{-12}$ in order to have an accurate value of the dual
$\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ and
$\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$. In case that
the Newton’s method converges sub-optimally, we recalculate
$\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}=\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}x-\underline{b}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$
explicitly which has a positive effect on the convergence of the lower
priority levels (similarly for $v_{l,i}$).

The following steps are then conducted for $l^{*}$:

  * •

Add the newly activated constraints from
${\underline{\mathcal{A}}_{l-1}^{\notin}}$ to ${\mathcal{A}_{l^{*}}}$ as a
‘virtual’ priority level $l^{*}$.

  * •

Calculate its null-space basis $Z_{l^{*}}$ of
$A_{\mathcal{A}_{l^{*}}}N_{l-1}$.

  * •

Continue $N_{l-1}$ to $N_{l^{*}}=N_{l-1}Z_{l^{*}}$

Now we handle the violated constraints from level $l$ for the active set
$\mathcal{A}_{l}$ which are activated if the following conditions are
fulfilled:

$w_{l,i}<\xi\qquad\text{ and }\qquad v_{l,i}<-\xi$ (27)

The condition $v_{l,i}<-\xi$ ensures that only significantly violated
constraints are activated. The condition $w_{l,i}<\xi$ sorts out constraints
that are satisfied. Then following steps complete the active set assembly:

  * •

Assemble the active set $\mathcal{A}_{l}$ of level $l$ with all equality
constraints and the active inequality constraints.

  * •

Inactive constraints are added to ${\underline{\mathcal{A}}_{l}^{\notin}}$.
Note that in the case of bound constraints we check whether the variable is
already constrained by the same constraint. If this is the case no new
constraint is added and the tighter bound of the two is used as the new right
hand side $b_{\underline{\mathcal{A}}_{l}^{\notin}}$. This prevents
unnecessarily increasing the size of
$\underline{A}_{\underline{\mathcal{A}}_{l}^{\notin}}$.

  * •

Calculate the null-space basis $Z_{l}$ of $A_{\mathcal{A}_{l}}N_{l^{*}}$.

  * •

Continue $N_{l^{*}}$ to $N_{l}=N_{l^{*}}Z_{l}$.

Note that even though we assemble the null-space $N_{l-1}$ step-wise like this
we always have $\underline{A}_{\underline{\mathcal{A}}_{l-1}}N_{l-1}=0$.

###  III-C Choice of null-space basis and variable elimination

For an overall cheap algorithm we require that

  * •

The projection $\tilde{M}=MN_{l}=MN_{l-1}Z_{l}$ reduces the number of
variables of $M$ depending on the number of linear independent constraints in
$A_{\mathcal{A}_{l}}$.

  * •

The matrix product $\tilde{M}=MN_{l-1}Z_{l}$ needs to be computed cheaply.

  * •

The decomposition for computing $Z_{l}$ should be reusable for the computation
of the dual $\underline{\lambda}_{\underline{\mathcal{A}}_{l}}$.

The non-orthogonal basis

$Z_{l}=P\begin{bmatrix}-R^{-1}T\\\ I\end{bmatrix}$ (28)

derived from the rank-revealing QR decomposition

$A_{\mathcal{A}_{l}}=Q\begin{bmatrix}R&T\\\ 0&0\end{bmatrix}P^{T}$ (29)

fulfills all these requirements [18]. $Q$ is an orthogonal matrix, $R$ is
upper triangular, $T$ is a rectangular matrix and $P$ is a permutation matrix.
The bottom zero row is due to possible linear dependencies in
$A_{\mathcal{A}}$.

If $r_{l}=\text{rank}(A_{\mathcal{A}_{l}}N_{l-1})$ then its null-space
$Z_{l}\in\mathbb{R}^{n,n_{r}=n_{r}-r_{l}}$ and the same for
$N_{l}=N_{l-1}Z_{l}=Z_{1}\cdots Z_{l}\in\mathbb{R}^{n,n_{r}=n_{r}-r_{l}}$.
$n_{r}$ is the number of remaining variables of the current level $l$. We
therefore solve increasingly smaller Newton’s methods (24) as we descend the
hierarchy by eliminating variables through the null-space projections. This
scheme is therefore commonly referred to as variable elimination [45].

###  III-D Avoiding the calculation of
$\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}$

The cost of calculating the dual step
$\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}$ associated with
the equality constraints $A_{\underline{\mathcal{A}}_{l-1}}$ is of magnitude
$O(r_{\underline{\mathcal{A}}_{l-1}}^{2})$. The rank of
$A_{\underline{\mathcal{A}}_{l-1}}$ is given as
$r_{\underline{\mathcal{A}}_{l-1}}$. However, we do not necessarily need to
update the Lagrange multipliers $\lambda_{\underline{\mathcal{A}}_{l-1}}$
since none of the other primal or dual variables depend on them. Additionally,
we can explicitly calculate $\lambda_{\underline{\mathcal{A}}_{l-1}}$ by
solving $k_{l,1}=0$ (6). This is in contrast to the augmented system (14) or
the classical normal equations (21) which require the calculation of the dual
in order to obtain the primal.

The dual $\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}$ is only
necessary to calculate $k_{l,1}$ for the evaluation of the norm of $K_{l}$ (6)
to determine whether the IPM has converged with $\|K_{l}\|_{2}<\epsilon$. We
can reduce the number of dual evaluations (25) with our adapted convergence
test:

  * •

Check whether
$\left\|\begin{bmatrix}k_{l,2}^{T}&\cdots&k_{l,7}^{T}\end{bmatrix}^{T}\right\|_{2}<\epsilon$.

  * •

If this is the case solve $k_{l,1}=0$ (6) for
$\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}$. Then calculate the
residual $k_{l,1}$ and do the convergence test
$\left\|K_{l}\right\|_{2}<\epsilon$.

  * •

Otherwise, $\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}$ and $k_{l,1}$
are not evaluated.

###  III-E The least-squares form of the IPM-HLSP

The HLSP (24) can be rewritten to least squares form as is demonstrated in
Appendix B. The overall least-squares form of the IPM-HLSP then becomes

$\operatorname*{min.}_{\Delta
z}\qquad\left\|\begin{bmatrix}\sqrt{\underline{W}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\underline{\Lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}}\underline{\tilde{A}}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\\\
\sqrt{I+({V_{1,i}-W_{1,i}})^{-1}{W_{1,i}}}\tilde{A}_{l,i}\\\
\tilde{A}_{l,e}\end{bmatrix}\Delta
z-\begin{bmatrix}\sqrt{\underline{W}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\underline{\Lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}}F\\\
\sqrt{I+({V_{1,i}-W_{1,i}})^{-1}{W_{1,i}}}^{-1}G\\\
b_{l,e}-A_{l,e}x\end{bmatrix}\right\|^{2}$

$F$ and $G$ are defined in (18) and (19), respectively. We use the notation
$\tilde{M}\coloneqq MN_{l-1}$.

Following our considerations from sec. V we use the condition

$\underline{m}_{\underline{\mathcal{A}}_{l-1}^{\notin}}+m_{l,i}+2m_{l,e}<2n_{r}$
(30)

to decide whether the least-squares form (III-E) is more appropriate than the
projected normal equations (24) (assuming the use of a QR decomposition as the
full rank condition of $C_{l}$ is usually not fulfilled in hierarchies except
for the last level $p$).

In appendix C we present an efficient rank revealing QR decomposition (RRQR)
of (III-E) leveraging the triangular structure of a pre-computed RRQR of
$A_{l,e}N_{l-1}$ which is constant over the course of the Newton’s method.

####  III-E1 Accordance of the IPM-HLSP with the ASM-HLSP

The least-squares form of the IPM-HLSP (III-E) clearly exposes the parallels
with the ASM-HLSP. The IPM-HLSP considers the whole HLSP in each iteration of
the IPM whereas the ASM-HLSP only considers active constraints while inactive
constraints are ignored.

This means that for equality only problems
($\underline{m}_{\underline{\mathcal{A}}_{l-1}^{\notin}}+m_{l,i}=0$) the IPM-
HLSP and the ASM-HLSP are identical:

$\operatorname*{min.}_{\Delta z}\qquad\left\|A_{l,e}N_{l-1}\Delta
z+(A_{l,e}x-b_{l,e})\right\|^{2}$ (31)

This corresponds to the least squares form of the ASM-HLSP formulated in [18]
which can be considered the fastest way of resolving an equality only HLSP
with performance rates similar to an LU decomposition of an equivalently
weighted LSP.

####  III-E2 Handling bound constraints in the set of higher priority inactive
constraints and the inequalities of the current level

One disadvantage of the null-space method is that sparsity of the constraints
is usually not maintained due to our choice of dense null-space basis.
Nonetheless, some sparsity especially in the least squares form of the
projected normal equation remains. We treat inactive bound constraints in
$\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ and the inequalities
of the current level $A_{l,i}$ in a sparse manner. This can result in
significant computational improvement especially in the robotics context where
such constraints are common (joint limits, unilateral contact forces). We
implement a sparse QR decomposition by zeroing sparse elements by Givens
rotations. Tracking the sparsity comes basically for free as we can recognize
sparse columns when computing the column norms during the rank revealing QR
decomposition for the null-space calculation $Z_{l^{*}}$ and $Z_{l}$ of
$A_{\mathcal{A}_{l}^{*}}N_{l-1}$ and $A_{\mathcal{A}_{l}}N_{l^{*}}$. Zero
columns of these matrices preserve sparse columns of
$\underline{A}_{\underline{\mathcal{A}}_{l^{*}}^{\notin}}N_{l-1}Z_{l^{*}}$,
$\underline{A}_{\underline{\mathcal{A}}_{l}^{\notin}}N_{l^{*}}Z_{l}$,
$A_{l,i}N_{l-1}Z_{l^{*}}$ and $A_{l,i}N_{l^{*}}Z_{l}$.

During stage 2 and stage 3 of our special RRQR (appendix C) we measure the
density degree (number of dense entries / number of overall rows) of each
sparse column (the further we advance the denser the remaining columns
become). We apply Givens rotations as long as the density-degree is below a
certain threshold (0.4, tuned manually). Otherwise we apply Householder
transformations on the whole column neglecting the remaining sparsity since
this proves to be faster due to the fast implementation of Eigen [40]
leveraging block operations.

###  III-F Mehrotra’s predictor corrector algorithm

As much as we have reduced the computational complexity of the Newton’s method
it is still key to keep the overall number of Newton iterations at a minimum.
A very effective measure is Mehrotra’s predictor-corrector algorithm [43]
which enables fast convergence by choosing
$\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ and $\sigma_{l,i}$
appropriately. The algorithm for LSP is described in [44] (Alg. 16.4) and only
requires slight adaptations for HLSP which are detailed in Appendix D.

Even though the primal ($O(n_{r}^{2})$) needs to be computed twice per Newton
iteration (and the dual only once
($O(\underline{m}_{\underline{\mathcal{A}}_{l-1}}^{2})$) for the corrector
step if required by the convergence test, see sec. III-D) this effort is
negligible in comparison to the cost of the QR decomposition
($O(4/3n_{r}^{3})$) which needs to be conducted fewer times due to the
improved convergence of Mehrotra’s algorithm.

##  IV Algorithm for IPM-HLSP

Algorithm 1 Primal-dual NIPM-HLSP

1:procedure intPtHLSP($p,\underline{A}_{p},\underline{b}_{p}$)

2: $\text{rank}=0$

3: for l=1:p do

4: while $\|K_{l}\|_{2}>\epsilon$ & $\text{iter}<\text{maxIter}$ do

5: CalcDualAndPrimalStep(predictor)

6: CalcDualAndPrimalStep(corrector)

7: Make step with $\\_=\\_+\alpha\Delta\\_$ for new $x$, $w_{l,i}$, $v_{l,i}$,
$\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$,
$\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin},l}$

8: if $l>1$ then

9: If required by the convergence test in sec. III-D, compute the dual
$\underline{\lambda}_{\underline{\mathcal{A}}_{l-1},l}$ with (65)

10: end if

11: end while

12: project(inactive)

13: if $\text{rank}\geq n$ then return $x$,
$\underline{\Lambda}_{\underline{\mathcal{A}}_{p},p}$ end if

14: project(current)

15: if $\text{rank}\geq n$ then return $x$,
$\underline{\Lambda}_{\underline{\mathcal{A}}_{p},p}$ end if

16: end for

17: return $x$, $\underline{\Lambda}_{\underline{\mathcal{A}}_{p},p}$

18:end procedure

Algorithm 2 Primal-dual NIPM-HLSP

1:procedure calcPrimalAndDualStep(type)

2:if type == predictor then

3:Solve (24) or (III-E) (if
$\underline{m}_{\underline{\mathcal{A}}_{l-1}^{\notin}}+m_{l,i}+2m_{l,e}<n_{r}$)
for the affine primal step $\Delta z$ with
$\sigma_{l,i}=\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}=0$ and (54)

4:else

5:Solve (24) or (III-E) (if
$\underline{m}_{\underline{\mathcal{A}}_{l-1}^{\notin}}+m_{l,i}+2m_{l,e}<n_{r}$)
for the centered primal step $\Delta z$ with $\sigma$ as in (62) and with
(66).

6:end if

7:$\Delta x=N_{l-1}\Delta z$

8:Calculate $\Delta v_{l,i}$, $\Delta w_{l,i}$,
$\Delta\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$,
$\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ with
$\Delta x$

9:Line search for $\alpha$ on $v_{l,i}\leq 0$, $w_{l,i}\geq 0$,
$\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\geq 0$ and
$\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\geq 0$.

10:end procedure

Algorithm 3 Primal-dual NIPM-HLSP

1:procedure project(type)

2:if type==inactive then

3:Gather active set ${\mathcal{A}_{l^{*}}}$ from constraints in
${\underline{\mathcal{A}}_{l-1}^{\notin}}$ if
$\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\leq\xi$ and
$\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\geq\xi$

4:Calculate $Z_{l^{*}}$ of $A_{\mathcal{A}_{l^{*}}}N_{l-1}$ and project such
that $N_{l^{*}}=N_{l-1}Z_{l^{*}}$

5:$\text{rank}=\text{rank}+\text{rank}(A_{\mathcal{A}_{l^{*}}}N_{l-1})$

6:else

7:Gather active set $\mathcal{A}_{l}$ comprising of all equalities and all
active inequalities that are active with $v_{l,i}<-\xi$ and $w_{l,i}<\xi$.

8:Add the remaining inactive inequality constraints ($v_{l,i}\geq-\epsilon$
and $w_{l,i}\geq\xi$) to
$\underline{A}_{\underline{\mathcal{A}}_{l}^{\notin}}$.

9:Calculate $Z_{l}$ of $A_{\mathcal{A}_{l}}N_{l^{*}}$ and project such that
$N_{l}=N_{l^{*}}Z_{l}$

10:$\text{rank}=\text{rank}+\text{rank}(A_{\mathcal{A}_{l}}N_{l^{*}})$

11:end if

12:end procedure

Algorithms 1, 2 and 3 give a comprehensive overview of an efficient
implementation of the IPM-HLSP. Algorithm 1 describes the overall routine
while alg. 2 details the predictor and corrector step calculation and alg. 3
details the active set compositions and projections. A simplified overview is
given below:

  * •

Go through the hierarchy from level $1$ to $p$

  * •

For each level $l$, conduct the hierarchical Newton’s method

  * •

Upon convergence of the Newton’s method, gather all inactive inequality
constraints from higher priority levels
${\underline{\mathcal{A}}_{l-1}^{\notin}}$ that are saturated and add them to
the active set of the virtual priority level $l^{*}$.

  * •

Compute its nullspace and project lower priority levels and the remaining
inactive inequality constraints into it.

  * •

Add all equalities and the violated inequality constraints from level $l$ to
the active set of level $l$.

  * •

Compute its nullspace and project lower priority levels and the remaining
inactive inequality constraints into it.

For more details please directly refer to the code at
https://www.github.com/pfeiffer-kai/NIPM-HLSP.

##  V Comparison of the operation counts between the different Newton’s
methods

Classical normal equations (21) 1. Form the matrix product
$A_{1,i}^{T}W_{1,i}^{-1}\Lambda_{1,i}A_{1,i}$ in $O(m_{1,i}n^{2})$ operations
with $A_{1,i}\in\mathbb{R}^{m_{1,i},n}$. 2. Cholesky decomposition
$O(n^{3}/3)$ of $C_{2}$. 3. Form matrix $A_{1,e}C_{2}^{-1}A_{1,e}$ in
$O(m_{1,e}n^{2}+m_{1,e}^{2}n)$ operations as proposed in [38]. 4. Cholesky
decomposition of $A_{1,e}C_{2}^{-1}A_{1,e}$ ($O(m_{1,e}^{3}/3)$). 5. Calculate
the dual $\lambda_{1,e}$ in $O(m_{1,e}^{2})$ operations. 6. Calculate the
primal $\Delta x$ in $O(n^{2})$ operations. $\sum:$
$O(m_{1,i}n^{2}+n^{3}/3+m_{1,e}n^{2}+m_{1,e}^{2}n+m_{1,e}^{3}/3+m_{1,e}^{2}+n^{2})$
Projected normal equations (24) 1. Form the matrix product
$(A_{1,i}Z_{1,e})^{T}W_{1,i}^{-1}\Lambda_{1,i}(A_{1,i}Z_{1,e})$ in
$O(m_{1,i}n_{r}^{2})$ operations for
$A_{1,i}Z_{1,e}\in\mathbb{R}^{m_{1,i},n_{r}}$. 2. QR decomposition of
$Z_{1,e}^{T}C_{2}Z_{1,e}$ ($O(4n_{r}^{3}/3)$). 3. Solve in $O(n_{r}^{2})$ for
$\Delta z$ and project for $\Delta x$ in $O(nn_{r})$ operations. $\sum:$
$O(m_{1,i}n_{r}^{2}+n_{r}^{3}/3+n_{r}^{2}+nn_{r})$ Least squares form (III-E)
1. QR decomposition of cost $O(2n_{r}^{2}(m_{1,i}+m_{2,e})-2n_{r}^{3}/3)$. 2.
Solve in $O(n_{r}^{2})$ for $\Delta z$ and project for $\Delta x$ in
$O(nn_{r})$ operations. $\sum:$
$O(2n_{r}^{2}(m_{1,i}+m_{2,e})-2n_{r}^{3}/3+n_{r}^{2}+nn_{r})$ Figure 1:
Number of operations per Newton iteration for the different IPM-LSP
formulations.

Figure 1 details the number of operations per Newton iteration for the
classical (21) and projected normal equations (24) and the least squares form
(III-E) for a LSP (i.e. a two-level hierarchy $p=2$). $A_{1,e}$ consists of
linearly independent constraints and $C_{2}$ is a square full-rank matrix
which is necessary in order for the Cholesky decomposition to be applied.

Each method thereby requires calculations at the beginning of the Newton’s
method.

For the classical normal equations (21) we need to precalculate
$A_{2,e}^{T}A_{2,e}$ in $O(m_{2,e}n^{2})$.

For the projected normal equations (24) we need to precalculate the null-space
basis $Z_{1,e}$ in $O(2m_{1,e}^{2}n-2/3m_{1,e}^{3})$ and do the projections
$A_{2,e}Z_{1,e}$ ($O(2m_{2,e}nn_{r})$) and $A_{1,i}Z_{1,e}$
($O(2m_{1,i}nn_{r})$). $(A_{2,e}Z_{1,e})^{T}(A_{2,e}Z_{1,e})$ is then
calculated in $O(m_{2,e}n_{r}^{2})$.

The least squares form (III-E) requires precalculation of the null-space basis
$Z_{1,e}$ in $O(2m_{1,e}^{2}n-2/3m_{1,e}^{3})$ and the projections
$A_{2,e}Z_{1,e}$ in $O(2m_{2,e}nn_{r})$ and $A_{1,i}Z_{1,e}$ in
$O(2m_{1,i}nn_{r})$.

A direct comparison between the methods is difficult due to the dependency on
the different factors at play (number of overall Newton iterations, $n$,
$m_{1,i}$, $m_{1,e}$, rank($A_{1,e}$)). The projected normal equations (3
steps) and the least squares form (2 steps) however require significantly less
steps per Newton iteration than the classical normal equations (6 steps). If
there is a sufficient number of Newton iterations this circumstance has the
potential of setting off the cost of the null-space calculation at the
beginning of the Newton’s method. Also note that the classical normal
equations exhibit a high degree of sparsity if the constraint matrices are
sparse. This is not the case for the projected normal equations due to our
choice of dense null-space basis.

Deriving from the equality
$O(m_{1,i}n_{r}^{2})+O(n_{r}^{3}/3)=O(2n_{r}^{2}m-2n_{r}^{3}/3)$ we can
roughly state that the least squares form is more efficient than the projected
normal equations if we have $m_{1,i}+2m_{2,e}<n_{r}$.

Figure 2: Computation times of the classical and projected normal equations
and the least-squares form for an equality only constrained LSP. The number of
equality constraints on the first level are $m_{1,e}=0,15,30,45,60$ from top
to bottom. The x-axis shows the number of equality constraints on the second
level $m_{2,e}$.

Figure 2 shows the computation times (Intel(R) Core(TM) i7-9750H CPU @ 2.60GHz
processor with 32 Gb of RAM) for the different IPM formulations when an
equality only LSP is solved. $n=60$, $A_{1,e}$ only contains linearly
independent constraints and $C_{2}$ is full rank. Both matrices are dense. In
case of the projected normal equations (nf-IPM) and the least-squares form
(ls-IPM) the dual $\lambda_{1,e}$ is not calculated because the KKT norm does
not need to be computed for a convergence test (equality only problems
converge within one iteration since the KKT conditions are linear). For
reference we also show the computation times of the solver presented in [18]
which is an active-set method (ASM) based HLSP solver.

The LSP is solved faster in the classical normal equations form (clas. IPM)
when there is a low number of equality constraints on the first level. Note
that the clas. IPM is only applicable if $C_{2}$ is full rank with
$m_{2,e}\geq n$ (hence no computation times are given otherwise). In the case
of $m_{1,e}=0$ (see top graph) one Cholesky decomposition of $C_{2}$ is
sufficient to solve the unconstrained LSP to obtain the primal $\Delta x$.
Both ls-IPM and the ASM solver apply one rank revealing QR decomposition on
$A_{2,e}$ which is about 0.1 ms slower for $m_{2,e}=240$. ls-IPM can reuse
this decomposition for the computation of the null-space basis $Z_{2,e}$ since
$m_{1,i}=0$. nf-IPM however conducts two rank revealing QR decompositions, one
on $C_{2}$ and another on $A_{2,e}$ in order to obtain a basis of its null-
space $Z_{2,e}$ (which could be avoided since there are no further priority
levels but we show it for demonstration purposes).

As the number of equality constraints $m_{1,e}$ increases both nf-IPM and ls-
IPM make up for the computation time of the null-space basis computation
$Z_{1,e}$ and projection $A_{2,e}Z_{1,e}$ as increasingly smaller problems
$n_{r}<n$ need to be solved on the second level. This is in contrast to the
clas. IPM where an increasingly more expensive second Cholesky decomposition
of $A_{1,e}C_{2}^{-1}A_{1,e}$ needs to be computed. Eventually, $n_{r}=0$ for
$m_{1,e}=n=60$ which means that both nf-IPM and ls-IPM finish after solving
the first level. Again, nf-IPM is slower than ls-IPM as it computes a rank
revealing QR decomposition of $A_{1,e}^{T}A_{1,e}$ for the primal and a rank
revealing QR decomposition of $A_{1,e}$ for the calculation of the basis of
its null-space $Z_{1,e}$.

With the presence of inequality constraints the computational effort of
calculating the null-space basis for the projected IPM at the beginning of the
Newton’s method is ‘spread’ over the increasing number of Newton iterations
gaining some advantage with respect to the classical normal equations, see
sec. VI-A.

Both ASM and ls-IPM are doing the same operations for equality only problems
except that the primal $x$ is explicitly calculated after every priority
level. Since this is only a minor step we assume that the slight differences
in computation times are due to their specific implementations.

In order to gauge the operational cost of the IPM and the ASM for problems
with inequality constraints we give a rough approximation of the number of
operations that need to be conducted by each solver. The IPM-HLSP needs to
resolve the Newton’s method for each level. Once it converges the null-space
of its active-set has to be computed and the remaining levels need to be
projected into it. The pseudo-cost then becomes

$\displaystyle c_{\text{IPM-HLSP}}=\sum_{l=1}^{p}$ $\displaystyle
c_{\text{IPM}}(\text{NIter}_{l},n_{r},m_{\underline{\mathcal{A}}_{l-1}^{\notin}}+m_{l,i},m_{l,e})$
$\displaystyle+$ $\displaystyle
c_{\text{proj}}(\underline{m}_{\underline{\mathcal{A}}_{l}},m_{\underline{\mathcal{A}}_{l-1}^{\notin}},\underline{m}_{l+1:p},n_{r})$
(32)

which is a function of $n_{r}$, the number of active constraints
$\underline{m}_{\underline{\mathcal{A}}_{l}}$ of each respective level, the
number of inactive constraints from the previous levels
$m_{\underline{\mathcal{A}}_{l-1}^{\notin}}$, the number of equalities and
inequalities of the current level $m_{l,i}$ and $m_{l,e}$, the number of all
equality and inequality constraints from the remaining levels
$\underline{m}_{l+1:p}$ and the number of Newton iterations Niterl of level
$l$.

The pseudo-cost of the ASM-HLSP is composed as follows:

$\displaystyle c_{\text{ASM-HLSP}}$
$\displaystyle=\sum_{i=1}^{\text{ASIter}}\sum_{l=1}^{p}c_{\text{proj}}(\underline{m}_{\underline{\mathcal{A}}_{l}},\underline{m}_{l+1:p},n_{r})$
(33)

ASIter are the number of active set iterations. Note that the cost of the ASM
$c_{\text{ASM}}$ is included in the projection cost (calculation of a rank
revealing QR decomposition of the current level of with
$\underline{m}_{\underline{\mathcal{A}}_{l}}$ active constraints, projection
of $\underline{m}_{l+1:p}$ constraints of the lower levels into its null-space
basis). As can be seen, the null-space projection of the whole active set
needs to be repeated for each active set iteration whereas the IPM-HLSP needs
to project the whole HLSP (including inactive constraints) only once per HLSP
resolution. This leads to less operations for the IPM-HLSP in case of large
number of active set iterations for the ASM-HLSP.

##  VI Evaluation

In the following we evaluate our presented solver NIPM-HLSP in normal
equations form (pure IPM (nf-IPM) (24) and IPM in conjunction with ASM (nf-
IPM-ASM) (75)) and in least-squares form (pure IPM (ls-IPM) (III-E) and in
conjunction with ASM (ls-IPM-ASM) (E)). We compare their performance with the
IPM in classical normal equations form (21) (clas. IPM) and the ASM based HLSP
solver proposed in [18] (simply referred to as ASM). The least-squares NIPM
solvers are only used in the first simulation (sec. VI-A) since the number of
inequality constraints usually violate the condition
$\underline{m}_{\underline{\mathcal{A}}_{l-1}^{\notin}}+2m_{l,e}<n_{r}$ for
which the least-squares form is faster. We also solve (2) with the IPM based
solver GUROBI [46] and the ADMM based solver OSQP [35] over the C++ wrapper
osqp-eigen. Unlike our solvers nf-IPM and nf-IPM-ASM, both these solvers are
sparse solvers. We use them at standard configurations. For OSQP, warm-
starting each level is disabled since the problem size can change over the
course of the simulation. Since ADMM based solvers have the tendency to
converge with only moderate accuracy, OSQP possesses the functionality to make
an educated guess about the active set (‘polishing’). We enable this
functionality for the second (sec. VI-B) and third simulation (sec. VI-C).

We sample our simulations all from real-time robot control as it allows us to
intentionally create numerically ill-posed problems which lead to high number
of active set problems. We start with a kinematic robot control situation
where the classical and projected normal equations for LSP are compared (see
sec. VI-A). The evaluation is continued with a dynamic robot simulation
depicting the effect of a larger number of variables and inequality
constraints on the solver timings (sec. VI-B). The evaluation is concluded
with a push simulation in order to observe the distinctive difference in
behavior between the two solvers in case of a singular instance of a large
change of the active set (sec. VI-C). For the simulations we use the HRP-2Kai
humanoid robot with 38 degrees of freedom (DoF) [47].

We depict the overall solver times, the required number of iterations (for
OSQP both iterations and number of factorization updates as a dashed line),
the time per Newton iteration (solver time divided by number of iterations;
not for ASM and OSQP) and the maximum KKT norm of all the levels. Most plots
are in logarithmic scale for better readability.

The simulations are run on an Intel(R) Core(TM) i7-9750H CPU @ 2.60GHz
processor with 32 Gb of RAM.

###  VI-A Kinematic robot example - Comparing the classical and projected IPM

Hierarchy A 1. • 76 inequalities as bound constraints for the lower and upper
joint angle limits • 18 equality constraints for the geometric contact
constraints of the two feet and the left hand 2. • 2 inequality constraints on
the center of mass (CoM) in the transverse plane • 3 equality constraints on
the position of the right hand • 38 regularization terms for the joint
velocities Figure 3: Hierarchy A Figure 4: HRP2 stretching into kinematic and
algorithmic singularity in order to move the right hand as close as possible
to the blue ball. The robot oscillates in a high frequency and low amplitude
manner due to numerical instabilities. Figure 5: Hierarchy A, fig. 3. nf-IPM-
ASM and ls-IPM-ASM are about 50% faster than the clas. IPM.

In the first simulation we want to demonstrate that our IPM-HLSP (24)
projected into the null-space of active constraints can be computationally
superior with respect to the clas. IPM (21).

For this we design a stretch demonstration with the HRP-2Kai robot according
to the control hierarchy A given in Fig. 3. The robot control constraints are
composed of joint angle limits and geometric contact constraints for both feet
to be on the ground and the left hand to be grabbing onto a pole in front of
the robot. Note that the resulting $A_{1,e}$ contains linear dependent
constraints so for the clas. IPM we rely on a rank revealing QR decomposition
for $A_{1,e}C_{2}^{-1}A_{1,e}^{T}$ at cost of $O(4/3m_{1,e}^{3})$ instead of
$O(1/3m_{1,e}^{3})$ for a Cholesky decomposition. The control objective is for
the CoM to stay within the support polygon spanned by the feet and the right
hand to reach for an out-of-reach target on the upper right of the robot. A
regularization task on the joint velocities ensures the full rank property of
the objective and also prevents numerically unstable robot behavior due to
kinematic and algorithmic singularities. This leads to the robot taking a
posture similar to the one depicted in Fig. 4 without the joint oscillations
and a slightly less stretched right arm.

The results are given in Fig. 5. The top graph shows the computation times of
the six different solvers while the second graph shows the ratio of their
performance with respect to the one of the clas. IPM. The second graph from
the bottom shows the required Newton and active set iterations to resolve the
HLSP. The bottom graph shows the sum of the KKT norms of each priority level
at convergence (log scale, not for ASM).

The clas. IPM is slower than the nf-IPM-ASM and ls-IPM-ASM as two
decompositions per Newton iteration need to be resolved instead of one and
there is a sufficiently large number of Newton iterations to make up for the
cost of the null-space base computation. Consequently, and with the additional
effect of variable elimination due to our choice of null-space basis, the nf-
IPM-ASM is about 1.5 times as fast as the classical one. Thereby, the ls-IPM-
ASM is slightly slower than the nf-IPM-ASM as $m_{1,i}+2m_{2,e}>n_{r}$.

On the contrary, both nf-IPM and ls-IPM are clearly outperformed in terms of
computation times by their ASM based siblings nf-IPM-ASM and ls-IPM-ASM due to
the Newton iterations count being almost twice as high. This is reasonable as
both nf-IPM and ls-IPM conduct a Newton’s method to identify the initial
feasible or optimal infeasible point of the first level with an unreduced
number of variables. This is in contrast to nf-IPM-ASM and ls-IPM-ASM which
conduct an ASM on the first level which is fast and effective due to efficient
updating of the QR decomposition of $\tilde{A}_{\mathcal{A}_{l}}$ in case of
possible changes of the active set. Note that in these cases null-space bases
do not have to be updated as it is the case for ASM [18] or the solver
presented in [17].

As fast as nf-IPM-ASM and ls-IPM-ASM are, they can not compete with the ASM
which solves the problem in less than one tenth of the time of the clas. IPM
due to warm starting the active set. However, identifying the initial active
set in the first control iterations takes a considerable amount of active set
iterations both for the ls-IPM-ASM (which implements an ASM for identifying
the initial feasible or optimal infeasible point) and the ASM-HLSP.
Afterwards, the LSP is mostly resolved in one active set iteration or a more
or less constant number of Newton iterations in case of the IPM.

According to (2) OSQP solves two problems: firstly, finding the feasible or
optimal infeasible point of the first level and secondly, resolving the second
level of the hierarchy. Overall computation times are slightly faster than
IPM-ASM as long as no factorization updates (dashed red line in middle graph)
are required. Otherwise, both solvers require approximately the same solving
time. Polishing is disabled as can be seen from the KKT norms which are
constant at about $10^{-5}$ compared to $10^{-12}$ of the IPM based solvers.

GUROBI solves the problem in approximately the same time as ls-IPM. The time
per Newton iteration ranks slightly better since GUROBI requires more
iterations until convergence (due to solver settings).

The high degree of convergence of the IPM solvers shows a successful search
for the feasible or optimal infeasible point on both levels by the ASM while
keeping inactive constraints feasible by the IPM.

###  VI-B Dynamic robot example - A numerically unstable robot movement

Hierarchy B 1. • 152 inequality constraints for lower and upper joint angle
and joint velocity limits • 76 lower and upper torque limits • 32 unilateral
contact constraints for the feet 2. 18 equality constraints for the geometric
contact constraints of the two feet and the left hand 3. 2 inequality
constraints on the center of mass (CoM) in the transverse plane 4. 3 equality
constraints on the position of the right hand 5. 38 regularization terms for
the joint velocities 6. 36 regularization terms for the contact forces Figure
6: Hierarchy B Figure 7: Hierarchy B, fig. 6. The right arm starts stretching
at control iteration 750 after which the robot behavior becomes numerically
unstable. Figure 8: Hierarchy B, fig. 6. nf-IPM’s behavior for each priority
level. $n_{r}$ is the number of remaining variables on each respective
priority level while $m_{inact}$ corresponds to the number of inactive
constraints $\underline{m}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$. Both
plots are in linear scale. Figure 9: Hierarchy B, fig. 6. OSQP’s behavior for
each priority level. ‘Nr. fact. upd.’ (in linear scale) indicates the number
of (expensive) factorization updates per priority level.

The following simulation shows the behavior of the solvers if the problem to
solve is scaled up in number of variables and constraints. Also, a singular
task causes highly dynamic behavior with large changes of the active
unilateral contact force constraints and the joint angle, velocity and torque
constraints between the control iterations. We implement the same robotic
simulation setup as in sec. VI-A but this time for the dynamic control case.
We include the equation of motion with 3 contact points on the two feet and
the left hand. The joint torques are subsequently substituted by the equation
of motion [3] such that we end up with 74 variables consisting of 38 joint
velocities and 36 contact forces for the two feet and the left hand. The
singular task is added to the hierarchy in form of a stretch task trying to
reach for a cartesian position outside of the workspace of the robot. If such
a task is not regularized [4] it leads to numerical instabilities in form of
high frequency oscillations in presence of a trust region joint velocity
constraint [26]. For this we integrate the joint accelerations (another
possibility would be to derive the joint velocity constraint in time).

The complete control hierarchy B is given in Fig. 10.

The computation times for nf-IPM, nf-IPM-ASM, ASM, OSQP and GUROBI are given
in fig. 7. nf-IPM and nf-IPM-ASM resolve the hierarchy in about 8 ms and 6 ms
while the robot is numerically stable. Once the right arm starts stretching at
control iteration 750 there is a slight uptick in computation times of about 2
ms due to an increased number of Newton iterations for the ill-posed arm
stretch level, see fig. 8. The number of iterations per priority level is
limited to 15, meaning that the fourth level converges slightly worse with a
KKT norm of about $10^{-5}$. The fifth level suffers from similarly bad
convergence with a maximum KKT norm of around $20$. Nonetheless, the robot
continues to behave as expected with both feet keeping contact to the ground
and high frequency oscillations especially on the right arm.

GUROBI constantly shows a very high convergence rate (except for a single peak
of $1.5\times 10^{6}$ due to numerical difficulties) but requires more Newton
iterations to do so (around 90 compared to 40 for nf-IPM). This partly
explains why the time per iteration is about 0.05ms less with respect to nf-
IPM since any computational overhead gets averaged to a higher degree. On the
other hand, GUROBI efficiently leverages sparsity of the constraint matrices
which is not possible for nf-IPM due to dense null-space projections.
Furthermore, from the second graph from the bottom of fig. 8 it can be
recognized that the number of variables especially after the first level is
only reduced by 8% (from 74 to 68) and may contribute to the higher
computation time per iteration.

ASM fails to find a reasonable approximation of the true active set within the
given limit of 200 iterations in many control iterations. This leads to the
feet losing contact and the right arm swaying uncontrollably. Also, such a
large number of active set iterations causes ASM to violate the real-time
constraint with computation times of about 37 ms.

OSQP converges with a maximum KKT norm of about $0.14$ on level 3. While the
hierarchy is mostly resolved around 2 ms faster than by IPM-ASM there are a
lot of instances where OSQP requires both a high number of iterations and
factorization updates. This leads to computation times of up to 42 ms (8175
iterations, 9 factorization updates). We observed that it is quite hard to
maintain feasibility of the subsequent LSP’s of (2) due to the inherent
moderate convergence accuracy of the ADMM. As can be seen from the KKT norms
in fig. 9 there is always a level where the polishing process would fail such
that the corresponding level converges at about $10^{-3}$. This negatively
influences the determination of the optimal infeasible points. While this may
be acceptable for LSP’s it is problematic for HLSP’s since wrongly determined
violations $\underline{v}_{\underline{\mathcal{A}}_{l-1}}^{*}$ render the
subsequent priority levels infeasible and eventually lead to solver failure
and numerical difficulties. We explicitly recalculate the violations
$v_{l,e}=A_{l,e}x-b_{l,e}$ and $v_{l,i}=A_{l,i}x-b_{l,i}$ at convergence of
each priority level which can slightly mitigate this circumstance. The design
of a dedicated hierarchical ADMM solver may be able to better handle such
situation.

###  VI-C Dynamic robot example - Handling a push

Hierarchy C 1. • 152 inequality constraints for lower and upper joint angle
and joint velocity limits • 76 lower and upper torque limits • 32 unilateral
contact constraints for the feet 2. 18 equality constraints for the geometric
contact constraints of the two feet and the left hand 3. • 3 equality
constraints on the center of mass (CoM) in the transverse plane • 38 equality
constraints in case of second order regularization 4. 38 regularization terms
for the joint velocities 5. 36 regularization terms for the contact forces
Figure 10: Hierarchy C Figure 11: Hierarchy C, fig. 10. At control iteration
400 a push from behind of magnitude 400 N is applied to the robot body.

While the previous example in sec. VI-B is interesting from a numerical point
of view, there exist various regularization methods to prevent such
numerically unstable behavior [4, 48, 49, 50, 51, 22, 26]. This last example
therefore looks at a situation which may occur in a real robot situation,
namely handling a push. For this we use the control hierarchy C given in Fig.
10. In contrast to the previous examples we now only have one control task on
the CoM as equality constraints in all three spatial directions.

The robot is asked to lower its CoM. During the process at control iteration
400 a push from behind of magnitude 800 N is applied to the upper body. As can
be seen from Fig. 11, this leads to a singular instance of a large number of
132 active set iterations which takes the ASM solver approximately 50 ms to
resolve. We assume that this comes from an unfavorable interplay between the
trust region and the joint torque limits caused by possibly ill conditioned
equations of motion [52].

This also influences the convergence behavior of OSQP negatively. While OSQP
manages successful polishing in most control iterations it does not do so
during the push and exposes a higher KKT norm in these instances. The high
number of iterations and factorization updates require around 19 ms to
resolve.

In contrast, such behavior is observed only to a small degree for the IPM. As
can be seen from the second and third graph from the bottom, the KKT norms and
number of iterations of nf-IPM, nf-IPM-ASM and GUROBI are barely influenced.
While the KKT norm of nf-IPM slightly increases in the aftermath of the push
it actually decreases slightly for GUROBI. The HLSP is resolved in about 8 ms
by nf-IPM-ASM with only a few peaks which can be attributed to computational
artifacts of the machine running the simulation.

The push leads to the CoM of the robot being shifted to the front and
increases the error norm of the CoM task as can be seen from the bottom graph.
The difference in CoM behavior seen for GUROBI is due to a regularization term
on the joint velocities and contact forces which amounts to the reformulated
cost function
$\operatorname*{min.}_{x,v_{l,e},v_{l,i}}\qquad\frac{1}{2}10^{-5}\|x\|^{2}+\frac{1}{2}\|v_{l,e}\|^{2}+\frac{1}{2}\|v_{l,i}\|^{2}\qquad$,
$l=1,\dots,p$ in (2). Otherwise we observed numerical difficulties.

##  VII Conclusion

With this work we have formulated an IPM-HLSP based on the null-space method
which requires only a single decomposition of the KKT system per Newton
iteration instead of two. This can prove computationally superior with respect
to the classical IPM-LSP depending on the number of equality constraints and
Newton iterations. We believe that this theoretical advancement is an
important step in overcoming numerical difficulties seen with the ASM-HLSP.
Our simulations showed that the IPM based solvers exhibit nearly constant
number of Newton iterations throughout the simulations whereas the ASM-HLSP
tends to require high number of active set iterations in dynamic or
numerically unstable control situations due to ill-posed constraint matrices.
Our IPM-HLSP formulation therefore may be preferred to the ASM-HSLP if solver
predictability is regarded more important than very fast computation times but
limited by instances of unsuccessful active set searches.

The IPM based solvers achieve very fast control performance for the kinematic
control case but may be too slow for real world dynamic control applications.
However, we see further potential for algorithmic improvements, for example by
a sparse solver formulation, heuristically reducing the number of inactive
constraints or restricting the number of Newton iterations as seen in [37].
The last point may require a primal feasible formulation (primal-barrier
interior-point method) of our solver.

We believe that our formulation of the null-space method based IPM-HLSP is not
only relevant for instantaneous robotic control but may also find application
in other areas where the IPM is prevalent. Especially, we want to explore the
possibility of applying the null-space method based IPM to MPC. Special
attention requires its block diagonal structure which may be exploited for
computational efficiency in our algorithm.

Another possible research direction poses the development of a dedicated ADMM
solver for HLSP’s. Despite the difficulties we observed for resolving
hierarchies with the ADMM it converges very reliably to a moderately accurate
solution. Additionally, in contrast to the IPM, ADMM solvers can be warm-
started and oftentimes only require a single factorization per solver
iteration.

## References

  * [1] B. Siciliano and J.-J. E. Slotine, “The general framework for managing multiple tasks in high redundant robotic systems,” in _International Conference on Advanced Robotics_ , 1991, pp. 1211 – 1216 vol.2.
  * [2] A. Sherikov, D. Dimitrov, and P. Wieber, “Balancing a humanoid robot with a prioritized contact force distribution,” in _2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids)_ , 2015, pp. 223–228.
  * [3] A. Herzog, N. Rotella, S. Mason, F. Grimminger, S. Schaal, and L. Righetti, “Momentum control with hierarchical inverse dynamics on a torque-controlled humanoid,” _Autonomous Robots_ , vol. 40, no. 3, pp. 473–491, Mar 2016. [Online]. Available: https://doi.org/10.1007/s10514-015-9476-6
  * [4] S. Chiaverini, “Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators,” _IEEE Transactions on Robotics and Automation_ , vol. 13, no. 3, pp. 398–410, 1997.
  * [5] N. Mansard and F. Chaumette, “Task sequencing for high level sensor-based control,” _IEEE Trans. on Robotics_ , vol. 23, no. 1, pp. 60–72, 2007. [Online]. Available: https://hal.inria.fr/inria-00350593
  * [6] L. Sentis and O. Khatib, “Control of free-floating humanoid robots through task prioritization,” in _Proceedings of the 2005 IEEE International Conference on Robotics and Automation_ , April 2005, pp. 1718–1723.
  * [7] A. Liégeois, “Automatic Supervisory Control of the Configuration and Behavior of Multibody Mechanisms,” _IEEE Transactions on Systems, Man, and Cybernetics_ , vol. 7, no. 12, pp. 868–871, 1977.
  * [8] E. Marchand and G. Hager, “Dynamic sensor planning in visual servoing,” in _Proceedings. 1998 IEEE International Conference on Robotics and Automation (Cat. No.98CH36146)_ , vol. 3, 1998, pp. 1988–1993 vol.3.
  * [9] A. Remazeilles, N. Mansard, and F. Chaumette, “A qualitative visual servoing to ensure the visibility constraint,” in _2006 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2006, pp. 4297–4303.
  * [10] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” in _Proceedings. 1985 IEEE International Conference on Robotics and Automation_ , vol. 2, 1985, pp. 500–505.
  * [11] O. Kanoun, F. Lamiraux, P.-B. Wieber, F. Kanehiro, E. Yoshida, and J.-P. Laumond, “Prioritizing linear equality and inequality systems: Application to local motion planning for redundant robots,” _2009 IEEE International Conference on Robotics and Automation_ , no. May, pp. 2939–2944, 2009. [Online]. Available: http://ieeexplore.ieee.org/xpls/abs{_}all.jsp?arnumber=5152293{%}5Cnhttp://ieeexplore.ieee.org/lpdocs/epic03/wrapper.htm?arnumber=5152293
  * [12] Y. Abe, M. Da Silva, and J. Popović, “Multiobjective control with frictional contacts,” _ACM SIGGRAPH Symposium on Computer Animation_ , vol. 1, pp. 249–258, 2007.
  * [13] C. Collette, A. Micaelli, C. Andriot, and P. Lemerle, “Dynamic balance control of humanoids for multiple grasps and non coplanar frictional contacts,” in _IEEE/RAS International Conference on Humanoid Robots_ , Pittsburgh, PA, November 29 - December 1 2007, pp. 81–88.
  * [14] J. Vaillant, A. Kheddar, H. Audren, F. Keith, S. Brossette, A. Escande, K. Bouyarmane, K. Kaneko, M. Morisawa, P. Gergondet, E. Yoshida, S. Kajita, and F. Kanehiro, “Multi-contact vertical ladder climbing with an HRP-2 humanoid,” _Autonomous Robots_ , vol. 40, no. 3, pp. 561–580, 2016.
  * [15] K. Pfeiffer, A. Escande, and A. Kheddar, “Nut fastening with a humanoid robot,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , Sep. 2017, pp. 6142–6148.
  * [16] M. De Lasa and A. Hertzmann, “Prioritized optimization for task-space control,” _2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2009_ , vol. 3, no. 2, pp. 5755–5762, 2009.
  * [17] A. Escande, N. Mansard, and P.-B. Wieber, “Hierarchical quadratic programming: Fast online humanoid-robot motion generation,” _The International Journal of Robotics Research_ , vol. 33, no. 7, pp. 1006–1028, 2014\.
  * [18] D. Dimitrov, A. Sherikov, and P.-B. Wieber, “Efficient resolution of potentially conflicting linear constraints in robotics,” Aug. 2015, submitted to IEEE TRO (05/August/2015). [Online]. Available: https://hal.inria.fr/hal-01183003
  * [19] O. Kanoun, F. Lamiraux, and P.-B. Wieber, “Kinematic control of redundant manipulators: generalizing the task priority framework to inequality tasks,” _IEEE Trans. on Robotics_ , vol. 27, no. 4, pp. 785–792, 2011.
  * [20] T. F. Coleman, _Large Sparse Numerical Optimization_. Berlin, Heidelberg: Springer-Verlag, 1984.
  * [21] M. Benzi, G. H. Golub, and J. Liesen, “Numerical solution of saddle point problems,” _Acta Numerica_ , vol. 14, p. 1–137, 2005.
  * [22] K. Pfeiffer, “Efficient kinematic and algorithmic singularity resolution for multi-contact and multi-level constrained dynamic robot control,” Ph.D. dissertation, 12 2019.
  * [23] P. Gill, S. Hammarling, W. Murray, M. Saunders, M. Wright, and S. U. C. S. O. LAB., _LSSOL (Version 1.0): a Fortran Package for Constrained Linear Least-Squares and Convex Quadratic Programming. User’s Guide_. Defense Technical Information Center, 1986. [Online]. Available: https://books.google.com/books?id=gRGotgAACAAJ
  * [24] C. V. Rao, S. J. Wright, and J. B. Rawlings, “Application of interior-point methods to model predictive control,” _JOURNAL OF OPTIMIZATION THEORY AND APPLICATIONS_ , vol. 99, pp. 723–757, 1998.
  * [25] K. Pfeiffer, A. Escande, and A. Kheddar, “Singularity resolution in equality and inequality constrained hierarchical task-space control by adaptive nonlinear least squares,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 4, pp. 3630–3637, Oct 2018.
  * [26] K. Pfeiffer, A. Escande, P. Gergondet, and A. Kheddar, “Singularity Resolution for Multi-Level Constrained Dynamically Feasible Kinematic Control,” Jul. 2020, working paper or preprint. [Online]. Available: https://hal.archives-ouvertes.fr/hal-02896719
  * [27] S. Hammarling and C. Lucas, “Updating the qr factorization and the least squares problem,” 01 2008.
  * [28] P. Gill, W. Murray, M. Saunders, and M. Wright, “Practical anti-cycling procedure for linearly constrained optimization,” _Mathematical Programming_ , vol. 45, pp. 437–474, 08 1989.
  * [29] Y. Nesterov and A. Nemirovskii, _Interior-Point Polynomial Algorithms in Convex Programming_. Society for Industrial and Applied Mathematics, 1994. [Online]. Available: https://epubs.siam.org/doi/abs/10.1137/1.9781611970791
  * [30] R. A. Bartlett, A. Wachter, and L. T. Biegler, “Active set vs. interior point strategies for model predictive control,” in _Proceedings of the 2000 American Control Conference. ACC (IEEE Cat. No.00CH36334)_ , vol. 6, 2000, pp. 4229–4233 vol.6.
  * [31] S. Kuindersma, F. Permenter, and R. Tedrake, “An efficiently solvable quadratic program for stabilizing dynamic locomotion,” in _IEEE International Conference on Robotics and Automation_ , Hong Kong, China, 2014.
  * [32] S. Wright, “Applying new optimization algorithms to model predictive control,” in _Fifth International Conference on Chemical Process Control – CPC V_. CACHE Publications, 1997, pp. 147–155.
  * [33] D. Gabay and B. Mercier, “A dual algorithm for the solution of nonlinear variational problems via finite element approximation,” _Computers & Mathematics with Applications_, vol. 2, no. 1, pp. 17–40, 1976. [Online]. Available: https://www.sciencedirect.com/science/article/pii/0898122176900031
  * [34] S. Boyd, N. Parikh, E. Chu, B. Peleato, and J. Eckstein, “Distributed optimization and statistical learning via the alternating direction method of multipliers,” _Found. Trends Mach. Learn._ , vol. 3, no. 1, p. 1–122, Jan. 2011. [Online]. Available: https://doi.org/10.1561/2200000016
  * [35] B. Stellato, G. Banjac, P. Goulart, A. Bemporad, and S. Boyd, “OSQP: an operator splitting solver for quadratic programs,” _Mathematical Programming Computation_ , vol. 12, no. 4, pp. 637–672, 2020. [Online]. Available: https://doi.org/10.1007/s12532-020-00179-2
  * [36] R. J. Vanderbei, “Loqo:an interior point code for quadratic programming,” _Optimization Methods and Software_ , vol. 11, no. 1-4, pp. 451–484, 1999\. [Online]. Available: https://doi.org/10.1080/10556789908805759
  * [37] Y. Wang and S. Boyd, “Fast model predictive control using online optimization,” _IEEE Transactions on Control Systems Technology_ , vol. 18, no. 2, pp. 267–278, 2010.
  * [38] A. Domahidi, A. U. Zgraggen, M. N. Zeilinger, M. Morari, and C. N. Jones, “Efficient interior point methods for multistage problems arising in receding horizon control,” in _2012 IEEE 51st IEEE Conference on Decision and Control (CDC)_ , 2012, pp. 668–674.
  * [39] G. Frison and M. Diehl, “Hpipm: a high-performance quadratic programming framework for model predictive control**this research was supported by the german federal ministry for economic affairs and energy (bmwi) via eco4wind (0324125b) and dyconpv (0324166b), and by dfg via research unit for 2401.” _IFAC-PapersOnLine_ , vol. 53, no. 2, pp. 6563–6569, 2020, 21th IFAC World Congress. [Online]. Available: https://www.sciencedirect.com/science/article/pii/S2405896320303293
  * [40] G. Guennebaud, B. Jacob _et al._ , “Eigen v3,” http://eigen.tuxfamily.org, 2010.
  * [41] Y. Ye, M. Todd, and S. Mizuno, “An o(nl)-iteration homogeneous and self-dual linear programming algorithm,” _Mathematics of Operations Research - MOR_ , vol. 19, 02 1994.
  * [42] R. Vanderbei, _Linear Programming: Foundations and Extensions_ , ser. International Series in Operations Research & Management Science. Springer US, 2013. [Online]. Available: https://books.google.com/books?id=HjznBwAAQBAJ
  * [43] S. Mehrotra, “On the implementation of a primal-dual interior point method,” _SIAM Journal on Optimization_ , vol. 2, pp. 575–601, 11 1992.
  * [44] J. Nocedal and S. J. Wright, _Numerical Optimization_ , 2nd ed. New York, NY, USA: Springer, 2006.
  * [45] A. Björck, _Numerical Methods for Least Squares Problems_. Society for Industrial and Applied Mathematics, 1996\. [Online]. Available: https://epubs.siam.org/doi/abs/10.1137/1.9781611971484
  * [46] L. Gurobi Optimization, “Gurobi optimizer reference manual,” 2021. [Online]. Available: http://www.gurobi.com
  * [47] K. Kaneko, M. Morisawa, S. Kajita, S. Nakaoka, T. Sakaguchi, R. Cisneros, and F. Kanehiro, “Humanoid robot hrp-2kai - improvement of hrp-2 towards disaster response tasks -,” 11 2015.
  * [48] P. Baerlocher and R. Boulic, “An inverse kinematics architecture enforcing an arbitrary number of strict priority levels,” _Visual Computer_ , vol. 20, no. 6, pp. 402–417, 2004.
  * [49] S. R. Buss and J.-S. Kim, “Selectively damped least squares for inverse kinematics,” _Journal of Graphics Tools_ , vol. 10, no. 3, pp. 37–49, 2005\. [Online]. Available: https://doi.org/10.1080/2151237X.2005.10129202
  * [50] T. Sugihara, “Solvability-unconcerned inverse kinematics by the levenberg–marquardt method,” _IEEE Transactions on Robotics_ , vol. 27, no. 5, pp. 984–991, October 2011.
  * [51] P. Harish, M. Mahmudi, B. L. Callennec, and R. Boulic, “Parallel inverse kinematics for multithreaded architectures,” _ACM Trans. Graph._ , vol. 35, no. 2, pp. 19:1–19:13, Feb. 2016. [Online]. Available: http://doi.acm.org/10.1145/2887740
  * [52] A. A. Maciejewski, “Dealing with the ill-conditioned equations of motion for articulated figures,” _IEEE Computer Graphics and Applications_ , vol. 10, no. 3, pp. 63–71, 1990.

##  Appendix A Recursive computation of the Lagrange multipliers associated
with active constraints

(25) can be rewritten to

$\displaystyle\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{T}\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}=$
$\displaystyle\begin{bmatrix}\underline{A}_{\underline{\mathcal{A}}_{l-1}}\\\
\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\\\ A_{l,i}\\\
A_{l,e}\end{bmatrix}^{T}\begin{bmatrix}-\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}\\\
D\\\ E\\\ A_{l,e}(x+\Delta x)-b_{l,e}\\\ \end{bmatrix}$ (34)

with

$\displaystyle
D=-\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ (35)
$\displaystyle-
W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}(\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\odot(b_{\underline{\mathcal{A}}_{l-1}^{\notin}}-\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}(x+\Delta
x))+\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}e)$

and

$\displaystyle E$
$\displaystyle=\left(I+(V_{l,i}-W_{l,i})^{-1}{W_{l,i}}\right)A_{l,i}\Delta x$
$\displaystyle+A_{l,i}x-b_{l,i}-w_{l,i}$ (36)
$\displaystyle+({V_{l,i}-W_{l,i}})^{-1}({\sigma_{l,i}\mu_{l,i}e+w_{l,i}\odot(A_{l,i}x-b_{l,i}-w_{l,i})})$

The Lagrange multipliers can be calculated recursively by

$\displaystyle
N_{j-1}^{T}A_{\mathcal{A}_{j}}^{T}\Delta\lambda_{\mathcal{A}_{j}}=$ (37)
$\displaystyle
N_{j-1}^{T}\left(\begin{bmatrix}\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\\\
A_{l,i}\\\ A_{l,e}\end{bmatrix}^{T}\begin{bmatrix}D\\\ E\\\ A_{l,e}(x+\Delta
x)-b_{l,e}\\\
\end{bmatrix}-\sum_{k=j+1}^{l-1}A^{T}_{\mathcal{A}_{k}}\Delta\lambda_{\mathcal{A}_{k}}\right)$

with $j=l-1,\dots,1$. The QR decompositions of $A_{\mathcal{A}_{l}}N_{l-1}$
for the calculation of the null-space basis $Z_{l}$ are thereby reused.

##  Appendix B Deriving the least-squares form of the IPM-HLSP

In the following we show that the projected normal equations (24) can be
expressed in least squares form which avoids forming the square KKT Hessian
and is solved in a more computationally efficient way in certain cases as we
show in sec. V. For this we rewrite the part of the left hand side that
corresponds to the inactive constraints of the previous levels
$A_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ and the equality constraints of
the current level $A_{l,e}$

$\displaystyle
N_{l-1}^{T}(A_{l,e}^{T}A_{l,e}+A_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\Lambda_{\underline{\mathcal{A}}_{l-1}^{\notin}}A_{\underline{\mathcal{A}}_{l-1}^{\notin}})N_{l-1}\Delta
z$ (38) $\displaystyle=$ $\displaystyle
N_{l-1}^{T}\begin{bmatrix}A_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}\sqrt{W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\Lambda_{\underline{\mathcal{A}}_{l-1}^{\notin}}}&A_{l,e}^{T}\end{bmatrix}$
$\displaystyle\begin{bmatrix}\sqrt{W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\Lambda_{\underline{\mathcal{A}}_{l-1}^{\notin}}}A_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}&A_{l,e}^{T}\end{bmatrix}^{T}N_{l-1}\Delta
z$

The right hand side is rewritten accordingly

$\displaystyle
N_{l-1}^{T}(-A_{l,e}^{T}(A_{l,e}x-b_{l,e})+\underline{A}_{{\underline{\mathcal{A}}_{l-1}}}^{T}\lambda_{{\underline{\mathcal{A}}_{l-1}}}+A_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}F)$
(39) $\displaystyle=$ $\displaystyle
N_{l-1}^{T}(-A_{l,e}^{T}(A_{l,e}x-b_{l,e})$ $\displaystyle+$ $\displaystyle
A_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}\sqrt{W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\Lambda_{\underline{\mathcal{A}}_{l-1}^{\notin}}}\sqrt{W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\Lambda_{\underline{\mathcal{A}}_{l-1}^{\notin}}}^{-1}F)$
$\displaystyle=$ $\displaystyle
N_{l-1}^{T}\begin{bmatrix}A_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}\sqrt{W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\Lambda_{\underline{\mathcal{A}}_{l-1}^{\notin}}}&A_{l,e}^{T}\end{bmatrix}$
$\displaystyle\begin{bmatrix}\sqrt{W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\Lambda_{\underline{\mathcal{A}}_{l-1}^{\notin}}}^{-1}F^{T}&-(A_{l,e}x-b_{l,e})^{T}\end{bmatrix}^{T}$

with
$N_{l-1}^{T}\underline{A}_{{\underline{\mathcal{A}}_{l-1}}}^{T}\underline{\lambda}_{{\underline{\mathcal{A}}_{l-1}}}=0$.

The above works since
$W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\Lambda_{\underline{\mathcal{A}}_{l-1}^{\notin}}\geq
0$ per the line search and if

$\displaystyle\sqrt{W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\Lambda_{\underline{\mathcal{A}}_{l-1}^{\notin}}}\sqrt{W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\Lambda_{\underline{\mathcal{A}}_{l-1}^{\notin}}}^{-1}$
(40) $\displaystyle=$
$\displaystyle\sqrt{W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\Lambda_{\underline{\mathcal{A}}_{l-1}^{\notin}}}\sqrt{W_{\underline{\mathcal{A}}_{l-1}^{\notin}}\Lambda_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}}=I$

The square root of the part of (24) corresponding to the inequality
constraints of level $l$ can be extracted as follows:

$\displaystyle
N_{l-1}^{T}A_{l,i}^{T}(-G+\left(I+(V_{l,i}-W_{l,i})^{-1}{W_{l,i}}\right)A_{l,i}N_{l-1}\Delta
x)=$ (41) $\displaystyle
N_{l-1}^{T}A_{l,i}^{T}\sqrt{I+(V_{l,i}-W_{l,i})^{-1}{W_{l,i}}}$
$\displaystyle\Big{(}\sqrt{I+(V_{l,i}-W_{l,i})^{-1}{W_{l,i}}}^{-1}G$
$\displaystyle+\sqrt{I+(V_{l,i}-W_{l,i})^{-1}{W_{l,i}}}A_{l,i}N_{l-1}\Delta
x\Big{)}$

The expression under the square root is guaranteed to be semi-positive per the
line search. We have that ${V_{1,i}-W_{1,i}}\leq 0$ because of the dual
feasibility conditions (10). This leads to

$\displaystyle I+({V_{1,i}-W_{1,i}})^{-1}{W_{1,i}}$ $\displaystyle\geq 0$
$\displaystyle W_{1,i}$ $\displaystyle\leq-V_{1,i}+W_{1,i}$ (42)
$\displaystyle V_{1,i}$ $\displaystyle\leq 0$

which is true and ensured by line search.

The resulting least-squares form of the IPM-HLSP is given in (III-E).

##  Appendix C An efficient RRQR of the least squares Newton’s method

Here we present an efficient rank revealing QR decomposition (RRQR) of

$\begin{bmatrix}B\\\
A\end{bmatrix}\coloneqq\begin{bmatrix}\sqrt{\underline{W}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\underline{\Lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}}\underline{\tilde{A}}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\\\
\sqrt{I+({V_{1,i}-W_{1,i}})^{-1}{W_{l,i}}}\tilde{A}_{l,i}\\\
\tilde{A}_{l,e}\end{bmatrix}$ (43)

of (III-E) which leverages the triangular structure of a precomputed QR
decomposition of $A\coloneqq\tilde{A}_{l,e}$. Thereby we used the notation
$\tilde{M}\coloneqq MN_{l-1}$.

We use the fact that $\tilde{A}_{l,e}$ does not change over the course of the
iterations of the Newton’s method. We therefore can precompute a RRQR of
$\tilde{A}_{l,e}$ only once at the beginning of the Newton’s method (we refer
to it as stage 1 or St1 of the overall QR decomposition):

$A=Q_{1}\begin{bmatrix}R_{1}&T_{1}\\\
0&0\end{bmatrix}\begin{bmatrix}P_{1}^{T}\\\ P_{2}^{T}\end{bmatrix}$ (44)

The matrix to decompose in each Newton iteration becomes

$\begin{bmatrix}B\\\ A\end{bmatrix}=\begin{bmatrix}I&0\\\
0&Q_{1}\end{bmatrix}\begin{bmatrix}BP_{1}&BP_{2}\\\
R_{1}&T_{1}\end{bmatrix}\begin{bmatrix}P_{1}^{T}\\\ P_{2}^{T}\end{bmatrix}$
(45)

Each Newton iteration then first requires stage 2 of the overall 3-step QR
decomposition which consists of a non-permuted QR decomposition on the first
part associated with $R_{1}$:

$\begin{bmatrix}BP_{1}\\\ R_{1}\end{bmatrix}=\begin{bmatrix}U_{11}&U_{12}\\\
U_{21}&U_{22}\end{bmatrix}\begin{bmatrix}R_{2}\\\ 0\end{bmatrix}$ (46)

The computational saving is achieved by considering the upper triangular
structure of $R_{1}$.

We now have

$\begin{bmatrix}B\\\ A\end{bmatrix}=\begin{bmatrix}I&0\\\
0&Q_{1}\end{bmatrix}\begin{bmatrix}U_{11}&U_{12}\\\
U_{21}&U_{22}\end{bmatrix}\begin{bmatrix}R_{2}&S_{2}\\\
0&S_{3}\end{bmatrix}\begin{bmatrix}P_{1}^{T}\\\ P_{2}^{T}\end{bmatrix}$ (47)

with $S_{2}=U_{11}^{T}BP_{2}+U_{21}^{T}T_{1}$ and
$S_{3}=U_{12}^{T}BP_{2}+U_{22}^{T}T_{1}$.

Secondly, we conduct stage 3 of the process which is a permuted QR
decomposition on the remaining part

$S_{3}=\begin{bmatrix}V_{1}&V_{2}\end{bmatrix}\begin{bmatrix}R_{3}&T_{3}\\\
0&0\end{bmatrix}\begin{bmatrix}\Pi_{1}^{T}\\\ \Pi_{2}^{T}\end{bmatrix}$ (48)

The overall decomposition then becomes

$\displaystyle\begin{bmatrix}B\\\ A\end{bmatrix}=$
$\displaystyle\begin{bmatrix}I&0\\\
0&Q_{1}\end{bmatrix}\begin{bmatrix}U_{11}&U_{12}\\\
U_{21}&U_{22}\end{bmatrix}\begin{bmatrix}I&0&0\\\ 0&V_{1}&V_{2}\end{bmatrix}$
(49) $\displaystyle\begin{bmatrix}R_{2}&S_{2}\Pi_{1}&S_{2}\Pi_{2}\\\
0&R_{3}&T_{3}\\\ 0&0&0\end{bmatrix}\begin{bmatrix}I&0\\\ 0&\Pi_{1}^{T}\\\
0&\Pi^{T}_{2}\end{bmatrix}\begin{bmatrix}P_{1}^{T}\\\ P_{2}^{T}\end{bmatrix}$

A cheap forward substitution is now sufficient to obtain a basic solution for
$\Delta z$ of (III-E).

Therefore, in each Newton iteration we need to apply a QR decomposition
roughly only on
$\underline{\tilde{A}}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ since we can
neglect the lower triangular zero block of $R_{1}$. Note that this RRQR does
not choose the pivot elements with decreasing column norms as it is described
in [45]. However, in our simulations we did not observe any adverse numerical
effects by not doing so.

As a side note, we can not easily implement a separation of the form

$\displaystyle\begin{bmatrix}\sqrt{\underline{W}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\underline{\Lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}}\underline{\tilde{A}}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\\\
\sqrt{I+({V_{1,i}-W_{1,i}})^{-1}{W_{l,i}}}\tilde{A}_{l,i}\\\
\tilde{A}_{l,e}\end{bmatrix}$ (50)
$\displaystyle=\begin{bmatrix}\sqrt{\underline{W}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\underline{\Lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}}&0&0\\\
0&\sqrt{I+({V_{1,i}-W_{1,i}})^{-1}{W_{l,i}}}&0\\\
0&0&I\end{bmatrix}\begin{bmatrix}\underline{\tilde{A}}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\\\
\tilde{A}_{l,i}\\\ \tilde{A}_{l,e}\end{bmatrix}$ (51) $\displaystyle\eqqcolon
D\begin{bmatrix}\underline{\tilde{A}}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}&\tilde{A}_{l,i}^{T}&\tilde{A}_{l,e}^{T}\end{bmatrix}^{T}$
(52)

with a precomputed RRQR of
$\begin{bmatrix}\underline{\tilde{A}}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}&\tilde{A}_{l,i}^{T}&\tilde{A}_{l,e}^{T}\end{bmatrix}^{T}$
since the extracted matrix $D$ is not necessarily full rank.

##  Appendix D Mehrotra’s predictor corrector algorithm for IPM-HLSP

First, a decomposition of the projected normal equations (24) or the least
squares form (III-E) is computed. Note that this only needs to be done once
per Newton iteration. It is then used to first calculate the affine scaling
step $\Delta z_{\text{aff}}$, $\Delta x_{\text{aff}}$, $\Delta
w_{l,i}^{\text{aff}}$, $v_{l,i}^{\text{aff}}$,
$\Delta\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}}$
and
$\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}}$
with

$\displaystyle\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}$
$\displaystyle=0$ (53) $\displaystyle F_{\text{aff}}$
$\displaystyle=\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}-W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\underline{\Lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}(A_{\underline{\mathcal{A}}_{l-1}^{\notin}}x-b_{\underline{\mathcal{A}}_{l-1}^{\notin}})$
(54) $\displaystyle\sigma_{l,i}$ $\displaystyle=0$ (55) $\displaystyle
G_{\text{aff}}$ $\displaystyle=b_{l,i}+w_{l,i}-A_{l,i}x$ (56)
$\displaystyle-({V_{1,i}-W_{1,i}})^{-1}{W_{l,i}(A_{l,i}x-b_{l,i}-w_{l,i})}$

Note that the dual $\underline{A}_{\underline{\mathcal{A}}_{l-1}}$ does not
need to be computed. Line search $\alpha_{\text{aff}}$ is conducted in order
to keep the dual feasible with $w_{l,i}+\alpha_{\text{aff}}\Delta
w_{l,i}^{\text{aff}}\geq 0$, $v_{l,i}+\alpha_{\text{aff}}\Delta
v_{l,i}^{\text{aff}}\leq 0$,
$\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}+\alpha_{\text{aff}}\Delta\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}}\geq
0$ and
$\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}+\alpha_{\text{aff}}\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}}\geq
0$.

With this information we calculate the corrector step $\Delta z$, $\Delta x$,
$\Delta v_{l,i}$, $w_{l,i}$,
$\Delta\underline{w}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ and
$\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$ with

$\displaystyle\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}$
$\displaystyle=\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}w_{\underline{\mathcal{A}}_{l-1}^{\notin}}/\underline{m}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$
(57) $\displaystyle\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}}$
$\displaystyle=(\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}+\alpha_{\text{aff}}\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}})^{T}$
(58)
$\displaystyle(w_{l,\underline{\mathcal{A}}_{l-1}^{\notin}}+\alpha_{\text{aff}}\Delta
w_{l,\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}})/\underline{m}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$
$\displaystyle\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}$
$\displaystyle=(\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}}/\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}})^{3}$
(59) $\displaystyle\mu_{l,i}$ $\displaystyle=-v_{l,i}^{T}w_{l,i}/m_{l,i}$ (60)
$\displaystyle\mu_{l,i}^{\text{aff}}$
$\displaystyle=(v_{l,i}+\alpha_{\text{aff}}\Delta v_{l,i}^{\text{aff}})^{T}$
(61) $\displaystyle(w_{l,i}+\alpha_{\text{aff}}\Delta
w_{l,i}^{\text{aff}})/m_{l,i}$ $\displaystyle\sigma_{l,i}$
$\displaystyle=(\mu_{l,i}^{\text{aff}}/\mu_{l,i})^{3}$ (62)

For the corrector step we have

$\displaystyle k_{4}=v_{l,i}\odot w_{l,i}+\Delta
v_{l,i}^{\text{aff}}\odot\Delta w_{l,i}^{\text{aff}}+\sigma_{l,i}\mu_{l,i}e$
(63) $\displaystyle
k_{7}=\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\odot
w_{\underline{\mathcal{A}}_{l-1}^{\notin}}+\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}}\odot\Delta
w_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}}-\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}e$
(64)

such that

$\displaystyle D_{\text{cor}}$
$\displaystyle=-\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$
(65) $\displaystyle-
W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}(\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\odot(b_{\underline{\mathcal{A}}_{l-1}^{\notin}}-\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}(x+\Delta
x))$
$\displaystyle-\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}}\odot\Delta
w_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}}+\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}e)$
$\displaystyle F_{\text{cor}}$
$\displaystyle=\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}+W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}(\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\odot(b_{\underline{\mathcal{A}}_{l-1}^{\notin}}-\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}x)$
(66)
$\displaystyle-\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}}\odot\Delta
w_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{\text{aff}}+\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}e)$
$\displaystyle E_{\text{cor}}$
$\displaystyle=-\left(I+({V_{1,i}-W_{1,i}})^{-1}{W_{l,i}}\right)A_{l,i}$ (67)
$\displaystyle-A_{l,i}x+b_{l,i}+w_{l,i}$
$\displaystyle-({V_{1,i}-W_{1,i}})^{-1}(\sigma_{l,i}\mu_{l,i}e$
$\displaystyle+\Delta v_{l,i}^{\text{aff}}\odot\Delta
w_{l,i}^{\text{aff}}+w_{l,i}\odot(A_{l,i}x-b_{l,i}-w_{l,i}))$ $\displaystyle
G_{\text{cor}}$ $\displaystyle=b_{l,i}+w_{l,i}-A_{l,i}x$ (68) $\displaystyle-$
$\displaystyle({V_{l,i}-W_{l,i}})^{-1}$
$\displaystyle({\sigma_{l,i}\mu_{l,i}e+\Delta v_{l,i}^{\text{aff}}\odot\Delta
w_{l,i}^{\text{aff}}+w_{l,i}\odot(A_{l,i}x-b_{l,i}-w_{l,i})})$

##  Appendix E The IPM-ASM-HLSP formulation

We first apply the ASM to the top three lines of (2)

$\displaystyle\min_{x,v_{\mathcal{A}_{l}}}.$
$\displaystyle\qquad\frac{1}{2}\|v_{\mathcal{A}_{l}}\|^{2}$ (69) s.t.
$\displaystyle\qquad
A_{\mathcal{A}_{l}}x-b_{\mathcal{A}_{l}}=v_{\mathcal{A}_{l}}$

$A_{\mathcal{A}_{l}}$, $b_{\mathcal{A}_{l}}$ and $v_{\mathcal{A}_{l}}$
represent all equality constraints and the active inequality constraints in
the active set $\mathcal{A}_{l}$.

The active set search then takes following form:

  * •

Choose initial active set $\mathcal{A}_{l}$.

  * •

Solve (69) for the new $x$ and calculate $v_{1,l}=A_{1,l}x-b_{1,l}$ (including
the inactive inequality constraints)

  * •

If there is a constraint with $v_{1,l}<-\epsilon$ add this constraint to the
active set $\mathcal{A}_{l}$. If there are several violated constraints, add
the one with the largest violation $v_{1,l}$.

  * •

If all constraints are saturated or satisfied, remove the constraint with the
largest satisfaction $v_{1,l}\geq\epsilon$ from $\mathcal{A}_{l}$.

  * •

Repeat until no constraint needs to be added to or removed from the active
set.

If a slightly changed problem is solved over several iterations like it is the
case for instantaneous robot control we warm start the active set
$\mathcal{A}_{l}^{(i)}$ of the current control iteration $i$ by adding all
constraints which were active at the optimal point $\mathcal{A}_{l}^{(i-1)}$
of the last control iteration $i-1$.

We now integrate the above development (69) into the optimization problem (2):

$\displaystyle\operatorname*{min.}_{x,v_{\mathcal{A}_{l}}}$
$\displaystyle\qquad\frac{1}{2}\|v_{\mathcal{A}_{l}}\|^{2}$ (70) s.t.
$\displaystyle\qquad
A_{\mathcal{A}_{l}}x-b_{\mathcal{A}_{l}}=v_{\mathcal{A}_{l}}$
$\displaystyle\qquad\underline{A}_{\underline{\mathcal{A}}_{l-1}}x-b_{\underline{\mathcal{A}}_{l-1}}=v_{\underline{\mathcal{A}}_{l-1}}^{*}$
$\displaystyle\qquad
A_{\underline{\mathcal{A}}_{l-1}^{\notin}}x-b_{\underline{\mathcal{A}}_{l-1}^{\notin}}\geq
0$

This form is very similar to (3) except that inactive inequality constraints
of $A_{l,i}$ are not considered by means of the IPM. Instead, all equality
constraints $A_{l,e}$ and the active inequality constraints are summarized in
$A_{\mathcal{A}_{l}}$. Accordingly, the further developments are closely
related to the one of the pure IPM-HLSP only that all entries related to $l,i$
are removed and the index change $l,e\rightarrow\mathcal{A}_{l}$ is applied.

For the augmented system we then have

$\displaystyle\begin{bmatrix}C_{l}&-\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{T}\\\
-\underline{A}_{\underline{\mathcal{A}}_{l-1}}&0\end{bmatrix}\begin{bmatrix}\Delta
x\\\
\Delta\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}\end{bmatrix}=\begin{bmatrix}r_{1}\\\
r_{2}\end{bmatrix}$ (71)

with

$\displaystyle C_{l}$
$\displaystyle=A_{\mathcal{A}_{l}}^{T}A_{\mathcal{A}_{l}}+\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}\underline{W}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\underline{\Lambda}_{\underline{\mathcal{A}}_{l-1}}^{\notin}\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}$

The right hand side is given by

$\displaystyle r_{1}$
$\displaystyle=\underline{A}_{\underline{\mathcal{A}}_{l-1}}^{T}\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}}+\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{T}F+A_{\mathcal{A}_{l}}^{T}(b_{\mathcal{A}_{l}}-A_{\mathcal{A}_{l}}x)$
(72) $\displaystyle r_{2}$
$\displaystyle=\underline{A}_{\underline{\mathcal{A}}_{l-1}}x-\underline{b}_{\underline{\mathcal{A}}_{l-1}}-\underline{w}_{\underline{\mathcal{A}}_{l-1}}^{*}$
(73)

with

$\displaystyle F$
$\displaystyle=\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}+W_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}(\underline{\lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\odot(\underline{b}_{\underline{\mathcal{A}}_{l-1}^{\notin}}-\underline{A}_{\underline{\mathcal{A}}_{l-1}^{\notin}}x)$
(74)
$\displaystyle+\sigma_{\underline{\mathcal{A}}_{l-1}^{\notin}}\mu_{\underline{\mathcal{A}}_{l-1}^{\notin}}e)$

The normal equations form is

$\displaystyle N_{l-1}^{T}C_{l}N_{l-1}\Delta z=N_{l-1}^{T}r_{1}$ (75)

The least-squares form of the IPM-ASM-HLSP is

$\operatorname*{min.}_{\Delta
z}\qquad\left\|\begin{bmatrix}\sqrt{\underline{W}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}\underline{\Lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}}\underline{\tilde{A}}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\\\
\tilde{A}_{\mathcal{A}_{l}}\end{bmatrix}\Delta
z-\begin{bmatrix}\sqrt{\underline{W}_{\underline{\mathcal{A}}_{l-1}^{\notin}}\underline{\Lambda}_{\underline{\mathcal{A}}_{l-1}^{\notin}}^{-1}}F\\\
b_{\mathcal{A}_{l}}-A_{\mathcal{A}_{l}}x\end{bmatrix}\right\|^{2}$

Note that the QR decomposition of $\tilde{A}_{\mathcal{A}_{l}}$ can be cheaply
updated in case of active set changes if the special rank revealing QR
decomposition is to be used (sec. C) (no updates of null-space bases
necessary).