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

TL;DR

This paper introduces a novel primal-dual interior-point method tailored for hierarchical least-squares programs in robotics, offering consistent computational efficiency and robustness compared to traditional active-set methods.

Contribution

The paper presents an efficient interior-point method based on the nullspace approach for dense HLSPs, reducing computational time and maintaining constant iteration counts during large active set changes.

Findings

Solver maintains constant iterations in large active set changes

Avoids quadratic Hessian formation, preserving sparsity

Reliably solves ill-posed hierarchical control problems

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 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.