Efficient Derivative Evaluation for Rigid-Body Dynamics Based on Recursive Algorithms Subject to Kinematic and Loop Constraints

Abstract

Simulation and control of robotic and bio-mechanical systems depend on a mathematical model description, typically a rigid-body system connected by joints, for which efficient algorithms to compute the forward or inverse dynamics exist. Gradient-based optimization and control methods require derivatives of the dynamics, often approximated by numerical differentiation (FD). However, they benefit from accurate gradients, which promote faster convergence, less iterations, and improved handling of nonlinearities or ill-conditioning of the problem formulations, which are particularly observed when kinematic constraints are involved. In this letter, we apply algorithmic differentiation (AD) to propagate sensitivities through dynamics algorithms. To this end, we augment the computational graph of these algorithms with derivative information. We provide analytic derivatives for elementary operations, in particular matrix factorizations of the descriptor form of the equation of motions, which yields a very efficient derivative evaluation for constrained dynamics. The proposed approach is implemented within the free software package rigid body dynamics library (RBDL), which heavily employs so-called spatial transformations in its implementation of the dynamics algorithms. Thus, manipulations of spatial transformations are treated as elementary operations. The efficiency is improved further by sparsity exploitation. We validate and benchmark the implementation against its FD counterpart for a lifting motion of a human model.