Constrained closed loop inverse kinematics

This paper introduces a kinematically constrained closed loop inverse kinematics algorithm for motion control of robots or other articulated rigid body systems. The proposed strategy utilizes gradients of collision and joint limit potential functions to arrive at an appropriate weighting matrix to penalize and dampen motion approaching constraint surfaces. The method is particularly suitable for self collision avoidance of highly articulated systems which may have multiple collision points among several segment pairs. In that respect, the proposed method has a distinct advantage over existing gradient projection based methods which rely on numerically unstable null-space projections when there are multiple intermittent constraints. We also show how this approach can be augmented with a previously reported method based on redirection of constraints along virtual surface manifolds. The hybrid strategy is effective, robust, and does not require parameter tuning. The efficacy of the proposed algorithm is demonstrated for a self collision avoidance problem where the reference motion is obtained from human observations. We show simulation and experimental results on the humanoid robot ASIMO.