Efficient and accurate partial derivatives of rigid-body dynamics for trajectory optimization

In the past few years, trajectory optimization has increasingly been used for motion planning in dynamic-legged robots, like humanoids, quadrupeds, and manipulators. Optimization-based closed-loop controllers like Model-Predictive Control solve trajectory optimization problems sequentially in a receding time horizon fashion. However, the partial derivatives of the rigid-body dynamics used for optimization take almost 90% of the run-time for the optimization problem. Hence, efficiency and accuracy for computing the derivatives determine how fast the optimization algorithms can converge. Conventionally, numerical methods like Finite-Difference and automated Chain-Rule methods called Automatic Differentiation (AD) have been used to compute the derivatives. However, numerical difference results in approximate derivatives, and hence compromises the accuracy, while AD methods often lead to increased run times due to the large amount of matrix-vector products in the algorithms. This work provides analytical recursive expressions and algorithms to compute the first and second-order partial derivatives of rigid-body dynamics, for robotic models with open-loop connectivity trees, multi-DoF joints, and external forces in the form of contacts. Featherstone's Spatial Vector Algebra (SVA) has been extensively used to derive the underlying analytical expressions. Firstly, the first-order partial derivatives of Inverse Dynamics are presented. These derivatives are then extended to derive the second-order derivatives of Inverse Dynamics by extending SVA for tensor objects. Then, efficient tricks to compute the first/second-order derivatives of Forward Dynamics using all the previously derived derivatives are presented. Then, related model-based recursive algorithms are developed and compared in run-time with Finite-Difference, Complex-Step, Automatic Differentiation, and manual chain-rule methods. For first-order Inverse-Dynamics derivatives, run-time analysis with the state-of-the-art library Pinocchio (in C++) shows improvements up to 2x for the ATLAS humanoid. Run-time comparison of the Inverse-Dynamics and Forward-Dynamics second-order derivatives with the AD approach shows speed-ups up to 11x, and 4x respectively. Open-source implementation in Pinocchio and Featherstone's library are also provided for all the developed algorithms. The resulting derivatives show machine precision accuracy for the first-order, and up to 10⁻¹⁰ for the second-order derivatives when compared with the Complex-Step method. The derivatives of the impact dynamics used to model the impulsive interaction of the robot with a rigid surface are also presented. Finally, a Multi-Shooting Differential Dynamic Programming (DDP) optimizer is used for solving a bounding gait optimization problem for a 2D 7-DoF planar quadruped model of the MIT Mini-Cheetah. Here, for the first time, both the first and the second-order derivatives are computed using analytical methods for the different modes -stance, impact, and flight for the quadruped. The Quasi-Newton (QN) method used to approximate the State Transition Matrix shows speed-ups up to 10x over the full DDP approach.