Singularity Resolution in Equality and Inequality Constrained Hierarchical Task-Space Control by Adaptive Nonlinear Least Squares

We propose a robust method to handle kinematic and algorithmic singularities of any kinematically redundant robot under task-space hierarchical control with ordered equalities and inequalities. Our main idea is to exploit a second order model of the nonlinear kinematic function, in the sense of the Newton's method in optimization. The second order information is provided by a hierarchical BFGS algorithm omitting the heavy computation required for the true Hessian. In the absence of singularities, which is robustly detected, we use the Gauss-Newton algorithm that has quadratic convergence. In all cases, we keep a least-squares formulation enabling good computation performances. Our approach is demonstrated in simulation with a simple robot and a humanoid robot, and compared to state-of-the-art algorithms.