Efficient Analytical Derivatives of Rigid-Body Dynamics Using Spatial Vector Algebra

An essential need for many model-based robot control algorithms is the ability to quickly and accurately compute partial derivatives of the equations of motion. State of the art approaches often use analytical methods based on the chain rule applied to existing dynamics algorithms. Although these methods are an improvement over finite differences in terms of accuracy, they are not always the most efficient. This letter provides a) closed-form spatial-vector expressions for the first-order partial derivatives of inverse dynamics, and b) a highly efficient recursive algorithm based on these expressions, which is applicable for robots with general multi-DoF Lie group joints (e.g., revolute, spherical, and floating base). The algorithm is benchmarked against chain-rule approaches in Fortran and against an existing algorithm from the Pinocchio library in C++. Tests consider computing the partial derivatives of inverse and forward dynamics for robots ranging from kinematic chains to humanoids and quadrupeds. Compared to the previous open-source Pinocchio implementation, our analytical results uncover a key computational restructuring that enables efficiency gains. Speedups of up to 1.4x are reported for calculating the partial derivatives of inverse dynamics for the 50-DoF Talos humanoid.