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

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.Comment: 17 pages, 7 figure

Balancing a humanoid robot with a prioritized contact force distribution

International audienceHumanoid robots propel themselves and perform tasks by interacting with their environment through contact forces. Typically, nonuniqueness of these forces is dealt with by distributing them evenly between the contacts. In the present paper, we introduce strict prioritization in contact force distribution, to reflect situations when an application of certain contact forces should be avoided as much as possible, for example, due to a fragility of the support. We illustrate this by designing a whole body motion controller for a setting with multiple noncoplanar contacts, where application of an optional contact force is allowed only if it is necessary to maintain balance and execute a task. Balance preservation is addressed by imposing a capturability constraint based on anticipation with a linear model adapted to multiple noncoplanar contacts. The controller is evaluated in simulations