A Weighted Method for Fast Resolution of Strictly Hierarchical Robot Task Specifications Using Exact Penalty Functions

Extensive work has been done on efficiently resolving hierarchical robot task specifications that minimize the $\ell$-2 norm of linear constraint violations, but not for $\ell$-1 norm, in which there has recently been growing interest for sparse control. Both approaches require solving a cascade of quadratic programs (QP) or linear programs (LP). In this letter, we introduce alternate and more efficient approaches to hierarchical $\ell$-1 norm minimization by formulating it as a single LP that can be solved by any off-the-shelf solver. The first approach is a recursive method that transforms the lexicographic LP (LLP) into a single objective problem using Lagrangian duality. The second approach, which forms the main focus of this letter, is a weighted method based on the exact penalty method, that is equivalent to the original LLP for a well chosen set of weights. We propose methods to compute and adapt these weights. The algorithms were applied on an interesting dual arm robot task. We discuss and benchmark the computational efficiency of these methods. Simplicity of the weighted method makes it a promising approach for tackling challenging prioritized robot control problems involving a control horizon or nonlinear constraints. Within this letter, we take the first step towards that goal by demonstrating the efficacy of the weighted method on a simpler instantaneous robot control problem with linear constraints.