Automatic Differentiation of Rigid Body Dynamics for Optimal Control and Estimation

Many algorithms for control, optimization and estimation in robotics depend on derivatives of the underlying system dynamics, e.g. to compute linearizations, sensitivities 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 Automatic Differentiation. Additionally, we propose how to automatically obtain the derivatives and generate highly efficient source code. We highlight the flexibility and performance of the approach in two application examples. First, we show a Trajectory Optimization example for the quadrupedal robot HyQ, which employs auto-differentiation on the dynamics including a contact model. Second, we present a hardware experiment in which a 6 DoF robotic arm avoids a randomly moving obstacle in a go-to task by fast, dynamic replanning

Algorithmic differentiation improves the computational efficiency of OpenSim-based trajectory optimization of human movement

Algorithmic differentiation (AD) is an alternative to finite differences (FD) for evaluating function derivatives. The primary aim of this study was to demonstrate the computational benefits of using AD instead of FD in OpenSim-based trajectory optimization of human movement. The secondary aim was to evaluate computational choices including different AD tools, different linear solvers, and the use of first- or second-order derivatives. First, we enabled the use of AD in OpenSim through a custom source code transformation tool and through the operator overloading tool ADOL-C. Second, we developed an interface between OpenSim and CasADi to solve trajectory optimization problems. Third, we evaluated computational choices through simulations of perturbed balance, two-dimensional predictive simulations of walking, and three-dimensional tracking simulations of walking. We performed all simulations using direct collocation and implicit differential equations. Using AD through our custom tool was between 1.8 ± 0.1 and 17.8 ± 4.9 times faster than using FD, and between 3.6 ± 0.3 and 12.3 ± 1.3 times faster than using AD through ADOL-C. The linear solver efficiency was problem-dependent and no solver was consistently more efficient. Using second-order derivatives was more efficient for balance simulations but less efficient for walking simulations. The walking simulations were physiologically realistic. These results highlight how the use of AD drastically decreases computational time of trajectory optimization problems as compared to more common FD. Overall, combining AD with direct collocation and implicit differential equations decreases the computational burden of trajectory optimization of human movement, which will facilitate their use for biomechanical applications requiring the use of detailed models of the musculoskeletal system.Postprint (published version

Nonlinear Model Predictive Control for Motion Generation of Humanoids

Das Ziel dieser Arbeit ist die Untersuchung und Entwicklung numerischer Methoden zur Bewegungserzeugung von humanoiden Robotern basierend auf nichtlinearer modell-prädiktiver Regelung. Ausgehend von der Modellierung der Humanoiden als komplexe Mehrkörpermodelle, die sowohl durch unilaterale Kontaktbedingungen beschränkt als auch durch die Formulierung unteraktuiert sind, wird die Bewegungserzeugung als Optimalsteuerungsproblem formuliert. In dieser Arbeit werden numerische Erweiterungen basierend auf den Prinzipien der Automatischen Differentiation für rekursive Algorithmen, die eine effiziente Auswertung der dynamischen Größen der oben genannten Mehrkörperformulierung erlauben, hergeleitet, sodass sowohl die nominellen Größen als auch deren ersten Ableitungen effizient ausgewertet werden können. Basierend auf diesen Ideen werden Erweiterungen für die Auswertung der Kontaktdynamik und der Berechnung des Kontaktimpulses vorgeschlagen. Die Echtzeitfähigkeit der Berechnung von Regelantworten hängt stark von der Komplexität der für die Bewegungerzeugung gewählten Mehrkörperformulierung und der zur Verfügung stehenden Rechenleistung ab. Um einen optimalen Trade-Off zu ermöglichen, untersucht diese Arbeit einerseits die mögliche Reduktion der Mehrkörperdynamik und andererseits werden maßgeschneiderte numerische Methoden entwickelt, um die Echtzeitfähigkeit der Regelung zu realisieren. Im Rahmen dieser Arbeit werden hierfür zwei reduzierte Modelle hergeleitet: eine nichtlineare Erweiterung des linearen inversen Pendelmodells sowie eine reduzierte Modellvariante basierend auf der centroidalen Mehrkörperdynamik. Ferner wird ein Regelaufbau zur GanzkörperBewegungserzeugung vorgestellt, deren Hauptbestandteil jeweils aus einem speziell diskretisierten Problem der nichtlinearen modell-prädiktiven Regelung sowie einer maßgeschneiderter Optimierungsmethode besteht. Die Echtzeitfähigkeit des Ansatzes wird durch Experimente mit den Robotern HRP-2 und HeiCub verifiziert. Diese Arbeit schlägt eine Methode der nichtlinear modell-prädiktiven Regelung vor, die trotz der Komplexität der vollen Mehrkörperformulierung eine Berechnung der Regelungsantwort in Echtzeit ermöglicht. Dies wird durch die geschickte Kombination von linearer und nichtlinearer modell-prädiktiver Regelung auf der aktuellen beziehungsweise der letzten Linearisierung des Problems in einer parallelen Regelstrategie realisiert. Experimente mit dem humanoiden Roboter Leo zeigen, dass, im Vergleich zur nominellen Strategie, erst durch den Einsatz dieser Methode eine Bewegungserzeugung auf dem Roboter möglich ist. Neben Methoden der modell-basierten Optimalsteuerung werden auch modell-freie Methoden des verstärkenden Lernens (Reinforcement Learning) für die Bewegungserzeugung untersucht, mit dem Fokus auf den schwierig zu modellierenden Modellunsicherheiten der Roboter. Im Rahmen dieser Arbeit werden eine allgemeine vergleichende Studie sowie Leistungskennzahlen entwickelt, die es erlauben, modell-basierte und -freie Methoden quantitativ bezüglich ihres Lösungsverhaltens zu vergleichen. Die Anwendung der Studie auf ein akademisches Beispiel zeigt Unterschiede und Kompromisse sowie Break-Even-Punkte zwischen den Problemformulierungen. Diese Arbeit schlägt basierend auf dieser Grundlage zwei mögliche Kombinationen vor, deren Eigenschaften bewiesen und in Simulation untersucht werden. Außerdem wird die besser abschneidende Variante auf dem humanoiden Roboter Leo implementiert und mit einem nominellen modell-basierten Regler verglichen

MBSlib - An Efficient Multibody Systems Library for Kinematics and Dynamics Simulation, Optimization and Sensitivity Analysis

The dynamic behavior of many technical and biomechanical systems can be modeled, simulated and optimized by using a multibody systems approach. Systems with many degrees of freedom typically result in complex and high-dimensional multibody systems models that necessitate capable modeling approaches and efficient computational algorithms as well as impede the numerical solution of related optimization problems. In this paper, the efficient and modular multibody systems library MBSLIB for kinematics and dynamics simulation, optimization and sensitivity analysis is presented. MBSLIB provides an intuitive modeling interface, modular software architecture, efficient computational algorithms as well as the computation of derivatives with respect to system states, control variables or model parameters. In combination with gradient-based optimization methods, the derivatives can be used to facilitate the numerical solution of optimization problems significantly. The library is open and can be obtained from the MBSLIB website. These features make MBSLIB a powerful, flexible and lightweight modeling, simulation and optimization library suitable for many technical and biomechanical applications

Desarrollo de estimadores de estado empleando la librería de simulación física Simbody

RESUMEN: La evolución de la computadora ha hecho posible la simulación de sistemas multicuerpo en tiempo real y el uso de estimadores de estado para inferir parámetros ocultos del estado del sistema. Entre estos estimadores de estado, se encuentran los filtros de Kalman, una opción eficiente y ampliamente usada, pero que no fueron formuladas para sistemas de segundo orden, no lineales y con restricciones, como suelen ser los sistemas multicuerpo. En dichos casos, se suelen utilizar distribuciones no paramétricas, como es el filtro de partículas. En este trabajo se pretende desarrollar una librería en C++ para la implementación de estimadores de estado, utilizando la técnica de filtro de partículas y la librería de simulación física Simbody. ABSTRACT: The evolution of the computer has made possible to achieve real-time simulation of multibody systems and using state observers to infer hidden state variables of them. Kalman filters are an efficient and vastly used option among these states observers, originally not formulated for second-order, non-linear and restricted systems though, such as multibody systems. In those cases, non-parametric distributions are often used, particle filters as an example. The aim of this work is developing a C++ library to implement a state observer, using the particle filter technique and Simbody, a multibody physics library