Fast derivatives of rigid body dynamics for control, optimization and estimation

Many algorithms for control, optimization and estimation in robotics depend on derivatives of the underlying system dynamics, e.g. to compute linearizations or gradient directions. However, we show that when dealing with Rigid Body Dynamics, these derivatives are difficult to derive analytically and to implement efficiently. To overcome this issue, we extend the modelling tool “RobCoGen” to be compatible with Auto Differentiation. Additionally, we propose how to automatically obtain the derivatives and generate highly efficient source code. Finally, we demonstrate an example application using Trajectory Optimization to underline the potential gain of using these derivatives in a control setting.