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

The Control Toolbox - An Open-Source C++ Library for Robotics, Optimal and Model Predictive Control

We introduce the Control Toolbox (CT), an open-source C++ library for efficient modeling, control, estimation, trajectory optimization and Model Predictive Control. The CT is applicable to a broad class of dynamic systems but features interfaces to modeling tools specifically designed for robotic applications. This paper outlines the general concept of the toolbox, its main building blocks, and highlights selected application examples. The library contains several tools to design and evaluate controllers, model dynamical systems and solve optimal control problems. The CT was designed for intuitive modeling of systems governed by ordinary differential or difference equations. It supports rapid prototyping of cost functions and constraints and provides standard interfaces for different optimal control solvers. To date, we support Single Shooting, the iterative Linear-Quadratic Regulator, Gauss-Newton Multiple Shooting and classical Direct Multiple Shooting. We provide interfaces to general purpose NLP solvers and Riccati-based linear-quadratic optimal control solvers. The CT was designed to solve large-scale optimal control and estimation problems efficiently and allows for online control of dynamic systems. Some of the key features to enable fast run-time performance are full compatibility with Automatic Differentiation, derivative code generation, and multi-threading. Still, the CT is designed as a modular framework whose building blocks can also be used for other control and estimation applications such as inverse dynamics control, extended Kalman filters or kinematic planning

Efficient Geometric Linearization of Moving-Base Rigid Robot Dynamics

The linearization of the equations of motion of a robotics system about a given state-input trajectory, including a controlled equilibrium state, is a valuable tool for model-based planning, closed-loop control, gain tuning, and state estimation. Contrary to the case of fixed based manipulators with prismatic or rotary joints, the state space of moving-base robotic systems such as humanoids, quadruped robots, or aerial manipulators cannot be globally parametrized by a finite number of independent coordinates. This impossibility is a direct consequence of the fact that the state of these systems includes the system's global orientation, formally described as an element of the special orthogonal group SO(3). As a consequence, obtaining the linearization of the equations of motion for these systems is typically resolved, from a practical perspective, by locally parameterizing the system's attitude by means of, e.g., Euler or Cardan angles. This has the drawback, however, of introducing artificial parameterization singularities and extra derivative computations. In this contribution, we show that it is actually possible to define a notion of linearization that does not require the use of a local parameterization for the system's orientation, obtaining a mathematically elegant, recursive, and singularity-free linearization for moving-based robot systems. Recursiveness, in particular, is obtained by proposing a nontrivial modification of existing recursive algorithms to allow for computations of the geometric derivatives of the inverse dynamics and the inverse of the mass matrix of the robotic system. The correctness of the proposed algorithm is validated by means of a numerical comparison with the result obtained via geometric finite difference

Estimation and identification study for flexible vehicles

Techniques are studied for the estimation of rigid body and bending states and the identification of model parameters associated with the single-axis attitude dynamics of a flexible vehicle. This problem is highly nonlinear but completely observable provided sufficient attitude and attitude rate data is available and provided all system bending modes are excited in the observation interval. A sequential estimator tracks the system states in the presence of model parameter errors. A batch estimator identifies all model parameters with high accuracy

Adjoint Algorithm for CAD-Based Shape Optimization Using a Cartesian Method

Adjoint solutions of the governing flow equations are becoming increasingly important for the development of efficient analysis and optimization algorithms. A well-known use of the adjoint method is gradient-based shape optimization. Given an objective function that defines some measure of performance, such as the lift and drag functionals, its gradient is computed at a cost that is essentially independent of the number of design variables (geometric parameters that control the shape). More recently, emerging adjoint applications focus on the analysis problem, where the adjoint solution is used to drive mesh adaptation, as well as to provide estimates of functional error bounds and corrections. The attractive feature of this approach is that the mesh-adaptation procedure targets a specific functional, thereby localizing the mesh refinement and reducing computational cost. Our focus is on the development of adjoint-based optimization techniques for a Cartesian method with embedded boundaries.12 In contrast t o implementations on structured and unstructured grids, Cartesian methods decouple the surface discretization from the volume mesh. This feature makes Cartesian methods well suited for the automated analysis of complex geometry problems, and consequently a promising approach to aerodynamic optimization. Melvin et developed an adjoint formulation for the TRANAIR code, which is based on the full-potential equation with viscous corrections. More recently, Dadone and Grossman presented an adjoint formulation for the Euler equations. In both approaches, a boundary condition is introduced to approximate the effects of the evolving surface shape that results in accurate gradient computation. Central to automated shape optimization algorithms is the issue of geometry modeling and control. The need to optimize complex, "real-life" geometry provides a strong incentive for the use of parametric-CAD systems within the optimization procedure. In previous work, we presented an effective optimization framework that incorporates a direct-CAD interface. In this work, we enhance the capabilities of this framework with efficient gradient computations using the discrete adjoint method. We present details of the adjoint numerical implementation, which reuses the domain decomposition, multigrid, and time-marching schemes of the flow solver. Furthermore, we explain and demonstrate the use of CAD in conjunction with the Cartesian adjoint approach. The final paper will contain a number of complex geometry, industrially relevant examples with many design variables to demonstrate the effectiveness of the adjoint method on Cartesian meshes

Discrete mechanics and optimal control: An analysis

The optimal control of a mechanical system is of crucial importance in many application areas. Typical examples are the determination of a time-minimal path in vehicle dynamics, a minimal energy trajectory in space mission design, or optimal motion sequences in robotics and biomechanics. In most cases, some sort of discretization of the original, infinite-dimensional optimization problem has to be performed in order to make the problem amenable to computations. The approach proposed in this paper is to directly discretize the variational description of the system's motion. The resulting optimization algorithm lets the discrete solution directly inherit characteristic structural properties from the continuous one like symmetries and integrals of the motion. We show that the DMOC (Discrete Mechanics and Optimal Control) approach is equivalent to a finite difference discretization of Hamilton's equations by a symplectic partitioned Runge-Kutta scheme and employ this fact in order to give a proof of convergence. The numerical performance of DMOC and its relationship to other existing optimal control methods are investigated

Overview of Sensitivity Analysis and Shape Optimization for Complex Aerodynamic Configurations

This paper presents a brief overview of some of the more recent advances in steady aerodynamic shape-design sensitivity analysis and optimization, based on advanced computational fluid dynamics. The focus here is on those methods particularly well- suited to the study of geometrically complex configurations and their potentially complex associated flow physics. When nonlinear state equations are considered in the optimization process, difficulties are found in the application of sensitivity analysis. Some techniques for circumventing such difficulties are currently being explored and are included here. Attention is directed to methods that utilize automatic differentiation to obtain aerodynamic sensitivity derivatives for both complex configurations and complex flow physics. Various examples of shape-design sensitivity analysis for unstructured-grid computational fluid dynamics algorithms are demonstrated for different formulations of the sensitivity equations. Finally, the use of advanced, unstructured-grid computational fluid dynamics in multidisciplinary analyses and multidisciplinary sensitivity analyses within future optimization processes is recommended and encouraged