The Hierarchical Newton's Method for Numerically Stable Prioritized Dynamic Control

This work links optimization approaches from hierarchical least-squares programming to instantaneous prioritized whole-body robot control. Concretely, we formulate the hierarchical Newton's method which solves prioritized nonlinear least-squares problems in a numerically stable fashion even in the presence of kinematic and algorithmic singularities of the approximated kinematic constraints. These results are then transferred to control problems which exhibit the additional variability of time. This is necessary to formulate acceleration-based controllers and to incorporate the second-order dynamics. However, we show that the Newton's method without complicated adaptations is not appropriate in the acceleration domain. We therefore formulate a velocity-based controller which exhibits second-order proportional derivative (PD) convergence characteristics. Our developments are verified in toy robot control scenarios as well as in complex robot experiments which stress the importance of prioritized control and its singularity resolution.