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.

[1]  Ferdinando Cannella,et al.  Design of HyQ – a hydraulically and electrically actuated quadruped robot , 2011 .

[2]  Javier García de Jalón,et al.  Performance of automatic differentiation tools in the dynamic simulation of multibody systems , 2014, Adv. Eng. Softw..

[3]  Robin J. Hogan,et al.  Fast Reverse-Mode Automatic Differentiation using Expression Templates in C++ , 2014, ACM Trans. Math. Softw..

[4]  Jun Nakanishi,et al.  Inverse Dynamics Control with Floating Base and Constraints , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[5]  Christian Bischof,et al.  On the Automatic Differentiation of Computer Programs and an Application to Multibody Systems , 1996 .

[6]  Andrea Walther,et al.  Getting Started with ADOL-C , 2009, Combinatorial Scientific Computing.

[7]  Darwin G. Caldwell,et al.  RobCoGen: a code generator for efficient kinematics and dynamics of articulated robots, based on Domain Specific Languages , 2016 .

[8]  Andreas Griewank,et al.  Evaluating derivatives - principles and techniques of algorithmic differentiation, Second Edition , 2000, Frontiers in applied mathematics.

[9]  Bob Carpenter,et al.  The Stan Math Library: Reverse-Mode Automatic Differentiation in C++ , 2015, ArXiv.

[10]  H. Bock,et al.  A Multiple Shooting Algorithm for Direct Solution of Optimal Control Problems , 1984 .

[11]  Stefan Schaal,et al.  Inverse dynamics control of floating-base robots with external constraints: A unified view , 2011, 2011 IEEE International Conference on Robotics and Automation.

[12]  James D. Turner,et al.  Some Applications of Automatic Differentiation to Rigid, Flexible, and Constrained Multibody Dynamics , 2005 .

[13]  Lorenz T. Biegler,et al.  On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming , 2006, Math. Program..

[14]  Michael A. Saunders,et al.  SNOPT: An SQP Algorithm for Large-Scale Constrained Optimization , 2002, SIAM J. Optim..

[15]  Willy Klier,et al.  Comparison of Automatic and Symbolic Differentiation in Mathematical Modeling and Computer Simulation of Rigid-Body Systems , 2002 .

[16]  J. Bobrow,et al.  An efficient sequential linear quadratic algorithm for solving nonlinear optimal control problems , 2005 .

[17]  Oskar von Stryk,et al.  MBSlib–An Efficient Multibody Systems Library for Kinematics and Dynamics Simulation, Optimization and Sensitivity Analysis , 2016, IEEE Robotics and Automation Letters.

[18]  Pierre-Brice Wieber,et al.  Fast Direct Multiple Shooting Algorithms for Optimal Robot Control , 2005 .

[19]  C. Bendtsen.  FADBAD, a flexible C++ package for automatic differentiation - using the forward and backward method , 1996 .

[20]  Alin Albu-Schäffer,et al.  On the closed form computation of the dynamic matrices and their differentiations , 2013, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[21]  Peter Eberhard,et al.  Automatic differentiation of numerical integration algorithms , 1999, Math. Comput..