Closed-Form Second-Order Partial Derivatives of Rigid-Body Inverse Dynamics
share
Optimization-based control methods for robots often rely on first-order dynamics approximation methods like in iLQR. Using second-order approximations of the dynamics is expensive due to the costly second-order partial derivatives of 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 closed-form analytical second-order partial derivatives of inverse dynamics for 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 𝒪(Nd^2) algorithm 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× for serial kinematic trees with N> 5, and a C++ implementation shows runtimes of ≈400μ s for a quadruped.
READ FULL TEXT