Analytical Derivatives of Rigid Body Dynamics Algorithms

Rigid body dynamics is a well-established framework in robotics. It can be used to expose the analytic form of kinematic and dynamic functions of the robot model. So far, two major algorithms, namely the recursive Newton-Euler algorithm (RNEA) and the articulated body algorithm (ABA), have been proposed to compute the inverse dynamics and the forward dynamics in a few microseconds. Evaluating their derivatives is an important challenge for various robotic applications (optimal control, estimation, co-design or reinforcement learning). However it remains time consuming, whether using finite differences or automatic differentiation. In this paper, we propose new algorithms to efficiently compute them thanks to closed-form formulations. Using the chain rule and adequate algebraic differentiation of spatial algebra, we firstly differentiate explicitly RNEA. Then, using properties about the derivative of function composition, we show that the same algorithm can also be used to compute the derivatives of ABA with a marginal additional cost. For this purpose, we introduce a new algorithm to compute the inverse of the joint-space inertia matrix, without explicitly computing the matrix itself. All the algorithms are implemented in our open-source C++ framework called Pinocchio. Benchmarks show computational costs varying between 3 microseconds (for a 7-dof arm) up to 17 microseconds (for a 36-dof humanoid), outperforming the alternative approaches of the state of the art.