Fast resolution of hierarchized inverse kinematics with inequality constraints

Classically, the inverse kinematics is performed by computing the singular value decomposition of the matrix to invert. This enables a very simple writing of the algorithm. However, the computation cost is high, especially when applied to complex robots and complex sets of constraints (typically around 5ms for 50 degrees of freedom - DOF). In this paper, we propose a dedicated adaptation of quadratic programming that enables fast computations of the hierarchical inverse kinematics (around 0.1ms for 50 DOF). We then extend this algorithm to deal with unilateral constraints, obtaining sufficiently high performances for reactive control.