Nicolas Mansard, Justin Carpentier, Équipe Mouvement des Systèmes Anthropomorphes (LAAS-GEPETTO), Laboratoire d'analyse et d'architecture des systèmes (LAAS), Université Toulouse - Jean Jaurès (UT2J)-Université Toulouse 1 Capitole (UT1), Université Fédérale Toulouse Midi-Pyrénées-Université Fédérale Toulouse Midi-Pyrénées-Centre National de la Recherche Scientifique (CNRS)-Université Toulouse III - Paul Sabatier (UT3), Université Fédérale Toulouse Midi-Pyrénées-Institut National des Sciences Appliquées - Toulouse (INSA Toulouse), Institut National des Sciences Appliquées (INSA)-Institut National des Sciences Appliquées (INSA)-Institut National Polytechnique (Toulouse) (Toulouse INP), Université Fédérale Toulouse Midi-Pyrénées-Université Toulouse - Jean Jaurès (UT2J)-Université Toulouse 1 Capitole (UT1), Université Fédérale Toulouse Midi-Pyrénées, Université Toulouse III - Paul Sabatier (UT3), ANR-16-CE33-0003,LOCO3D,Locomotion en environnement complexe(2016), European Project: 284951,EC:FP7:ICT,FP7-ICT-2011-FET-F,CA-ROBOCOM(2011), European Project: 780684,H2020,MEMMO(2018), Équipe Mouvement des Systèmes Anthropomorphes ( LAAS-GEPETTO ), Laboratoire d'analyse et d'architecture des systèmes [Toulouse] ( LAAS ), Institut National Polytechnique [Toulouse] ( INP ) -Institut National des Sciences Appliquées - Toulouse ( INSA Toulouse ), Institut National des Sciences Appliquées ( INSA ) -Institut National des Sciences Appliquées ( INSA ) -Université Paul Sabatier - Toulouse 3 ( UPS ) -Centre National de la Recherche Scientifique ( CNRS ) -Institut National Polytechnique [Toulouse] ( INP ) -Institut National des Sciences Appliquées - Toulouse ( INSA Toulouse ), Institut National des Sciences Appliquées ( INSA ) -Institut National des Sciences Appliquées ( INSA ) -Université Paul Sabatier - Toulouse 3 ( UPS ) -Centre National de la Recherche Scientifique ( CNRS ), Université Paul Sabatier - Toulouse 3 ( UPS ), European Project : 284951,EC:FP7:ICT,FP7-ICT-2011-FET-F,CA-ROBOCOM ( 2011 ), Université Toulouse Capitole (UT Capitole), Université de Toulouse (UT)-Université de Toulouse (UT)-Institut National des Sciences Appliquées - Toulouse (INSA Toulouse), Institut National des Sciences Appliquées (INSA)-Université de Toulouse (UT)-Institut National des Sciences Appliquées (INSA)-Université Toulouse - Jean Jaurès (UT2J), Université de Toulouse (UT)-Université Toulouse III - Paul Sabatier (UT3), Université de Toulouse (UT)-Centre National de la Recherche Scientifique (CNRS)-Institut National Polytechnique (Toulouse) (Toulouse INP), Université de Toulouse (UT)-Université Toulouse Capitole (UT Capitole), and Université de Toulouse (UT)
Robotics: Science and Systems (To Appear); International audience; 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.