Analytical Second-Order Partial Derivatives of Rigid-Body Inverse Dynamics
Shubham Singh, Ryan P. Russell, Patrick M. Wensing
Abstract
Optimization-based robot control strategies often rely on first-order dynamics approximation methods, as in iLQR. Using second-order approximations of the dynamics is expensive due to the costly second-order partial derivatives of the dynamics with respect to the state and control. Current approaches for calculating these derivatives typically use automatic differentiation (AD) and chain-rule accumulation or finite-difference. In this paper, for the first time, we present analytical expressions for the second-order partial derivatives of inverse dynamics for open-chain rigid-body systems with floating base and multi-DoF joints. A new extension of spatial vector algebra is proposed that enables the analysis. A recursive algorithm with complexity of \mathcal{O}(Nd^{2})\mathcal{O}(Nd^{2}) is also provided where N is the number of bodies and d is the depth of the kinematic tree. A comparison with AD in CasADi shows speedups of 1.5-3 x for serial kinematic trees with N > 5, and a C++ implementation shows runtimes of \approx \mathbf{5 1} \mu \mathrm{s}\approx \mathbf{5 1} \mu \mathrm{s} for a quadruped.
BibTeX
@inproceedings{iros2022_analyticalsecond,
title = {Analytical Second-Order Partial Derivatives of Rigid-Body Inverse Dynamics},
author = {Shubham Singh and Ryan P. Russell and Patrick M. Wensing},
booktitle = {IROS 2022},
year = {2022}
}