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

TL;DR

This paper introduces new closed-form expressions and a recursive algorithm for efficiently computing the first-order partial derivatives of inverse dynamics in rigid-body systems, significantly improving computational speed over previous methods.

Contribution

The paper presents novel analytical derivatives and a recursive algorithm that outperform existing chain-rule based approaches in efficiency and speed.

Findings

Achieved up to 1.4x speedup in derivative calculations for a 50-DOF humanoid

Developed a recursive algorithm based on new closed-form expressions

Benchmarking shows improved efficiency over existing implementations

Abstract

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 to this problem 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. In this paper, we contribute new closed-form expressions for the first-order partial derivatives of inverse dynamics, leading to a recursive algorithm. 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 new 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.