Linear-time Differential Inverse Kinematics: an Augmented Lagrangian Perspective

Abstract

For decades, inverse kinematics (IK) was an intense and active research area in robotics. Beyond analytical solutions limited to a restricted range of robotic systems and applications, differential inverse kinematics has emerged as a generic class of methods, able to cope with a wider variety of robots and scenarios, with quadratic programming-based approaches as the main paradigm. In this paper, we propose to revisit differential inverse kinematics from the perspective of augmented Lagrangian methods (AL) and the well-known related alternating direction method of multipliers (ADMM). Notably, by leveraging AL techniques and in the spirit of Featherstone algorithms, we introduce a rigid-body dynamics algorithm that solves equality-constrained IK problems with linear complexity in the number of robot joints and number of constraints. Combined with the ADMM strategy developed in the OSQP solver, we provide a new solution for the same class of problems as QP-based differential IK, yet with linear complexity in problem dimensions. We propose an open-source C++ implementation of this approach, which we validate on a large set of problems including manipulation and humanoid locomotion tasks. Our benchmark measures computation times 2--3 $\times$ shorter than the QP-based state of the art.