Robust solution of prioritized inverse kinematics based on Hestenes-Powell multiplier method

A robust numerical solution of the prioritized inverse kinematics is proposed. It is based on the augmented Lagrangian function and Hestenes-Powell's multiplier method. It originates the weighted inverse kinematics and only requires a small modification including an accumulation of the error of the high-priority constraint at each step of iteration and an estimation of Lagrange's multiplier. Hence, it is preferable to the conventional method, which is accompanied with an explicit complex computation of the kernel space, from the viewpoint of both the implementation cost and the computation cost per step. A drawback is that the proposed method becomes slow in some situations since Lagrange's multiplier linearly converges, while the joint displacements superlinearly converge. In some unlucky situations, it requires more computation cost in total than the conventional method. However, the proposed method is robust even in cases where the high-priority constraint is unsatisfiable. In fact, the proposed method solely succeeded in all the tested cases including unsolvable ones.