  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 levels. Active-set methods 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 dense
HLSP's which is able to maintain constant numbers of solver iterations in these
situations. We base our IPM on the computationally efficient nullspace method
as it requires only a single matrix factorization per solver iteration instead
of two as it is the case for other IPM formulations. We show that the resulting
normal equations can be expressed in least-squares form. This avoids the
formation of the quadratic Lagrangian Hessian and can possibly maintain high
levels of sparsity. Our solver reliably solves ill-posed instantaneous
hierarchical robot control problems without exhibiting the large variations in
computation time seen in active-set methods.
