On Second-Order Derivatives of Rigid-Body Dynamics: Theory & Implementation

TL;DR

This paper develops analytical methods for efficiently computing second-order derivatives of rigid-body dynamics, significantly improving the speed and accuracy of derivatives used in advanced robot control algorithms like DDP and iLQR.

Contribution

It introduces analytical formulas for second-order derivatives of rigid-body dynamics and compares their performance with automatic differentiation, enabling faster and more accurate control computations.

Findings

Analytical derivatives are faster than AD for complex robots.

Second-order derivatives enable more precise control algorithms.

Speedup of approximately 3x over AD for a 36 DoF humanoid.

Abstract

Model-based control for robots has increasingly been dependent on optimization-based methods like Differential Dynamic Programming and iterative LQR (iLQR). These methods can form the basis of Model-Predictive Control (MPC), which is commonly used for controlling legged robots. Computing the partial derivatives of the dynamics is often the most expensive part of these algorithms, regardless of whether analytical methods, Finite Difference, Automatic Differentiation (AD), or Chain-Rule accumulation is used. Since the second-order derivatives of dynamics result in tensor computations, they are often ignored, leading to the use of iLQR, instead of the full second-order DDP method. In this paper, we present analytical methods to compute the second-order derivatives of inverse and forward dynamics for open-chain rigid-body systems with multi-DoF joints and fixed/floating bases. An extensive comparison of accuracy and run-time performance with AD and other methods is provided, including the consideration of code-generation techniques in C/C++ to speed up the computations. For the 36 DoF ATLAS humanoid, the second-order Inverse, and the Forward dynamics derivatives take approx 200 mu s, and approx 2.1 ms respectively, resulting in a 3x speedup over the AD approach.