Analytical Derivatives of Rigid Body Dynamics Algorithms

J. Carpentier (1, 2)

1 UT3 - Université Toulouse III - Paul Sabatier
2 LAAS-GEPETTO - Équipe Mouvement des Systèmes Anthropomorphes

Rigid body dynamics is a well-established methodology in robotics. It can be exploited to exhibit the analytic form of kinematic and dynamic functions of the robot model. Two major algorithms, namely the recursive Newton-Euler algorithm (RNEA) and the articulated body algorithm (ABA), have been proposed so far to compute inverse dynamics and forward dynamics in a few microseconds. However, computing their derivatives remains a costly process, either using finite differences (costly and approximate) or automatic differentiation (difficult to implement and suboptimal). As computing the derivatives becomes an important issue (in optimal control, estimation, co-design or reinforcement learning), we propose in this paper new algorithms to efficiently compute them using closed-form formulations. We first explicitly differentiate RNEA, using the chain rule and adequate algebraic differentiation of spatial algebra. Then, using properties about the derivative of function composition, we show that the same algorithm can also be used to compute the derivatives of the forward dynamics with marginal additional cost. To this end, we finally introduce a new algorithm to compute the inverse of the joint-space inertia matrix, without explicitly computing the matrix itself. The algorithms have been implemented in our open-source C++ framework called Pinocchio. The reported benchmarks, based on several robot models, display computational costs varying between 3 microseconds (for a 7-dof arm) up to 17 microseconds (for a 36-dof humanoid), i.e. outperforms state-of-the-art results.

Mots clés: Analytical Derivatives; Robotics; Humanoid

Domaines: Automatique / Robotique

J. Carpentier. Analytical Derivatives of Rigid Body Dynamics Algorithms. 2018. ⟨hal-01790971v1⟩