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

Whole-Body MPC for a Dynamically Stable Mobile Manipulator

Autonomous mobile manipulation offers a dual advantage of mobility provided by a mobile platform and dexterity afforded by the manipulator. In this paper, we present a whole-body optimal control framework to jointly solve the problems of manipulation, balancing and interaction as one optimization problem for an inherently unstable robot. The optimization is performed using a Model Predictive Control (MPC) approach; the optimal control problem is transcribed at the end-effector space, treating the position and orientation tasks in the MPC planner, and skillfully planning for end-effector contact forces. The proposed formulation evaluates how the control decisions aimed at end-effector tracking and environment interaction will affect the balance of the system in the future. We showcase the advantages of the proposed MPC approach on the example of a ball-balancing robot with a robotic manipulator and validate our controller in hardware experiments for tasks such as end-effector pose tracking and door opening

Contact-Implicit Trajectory Optimization Based on a Variable Smooth Contact Model and Successive Convexification

In this paper, we propose a contact-implicit trajectory optimization (CITO) method based on a variable smooth contact model (VSCM) and successive convexification (SCvx). The VSCM facilitates the convergence of gradient-based optimization without compromising physical fidelity. On the other hand, the proposed SCvx-based approach combines the advantages of direct and shooting methods for CITO. For evaluations, we consider non-prehensile manipulation tasks. The proposed method is compared to a version based on iterative linear quadratic regulator (iLQR) on a planar example. The results demonstrate that both methods can find physically-consistent motions that complete the tasks without a meaningful initial guess owing to the VSCM. The proposed SCvx-based method outperforms the iLQR-based method in terms of convergence, computation time, and the quality of motions found. Finally, the proposed SCvx-based method is tested on a standard robot platform and shown to perform efficiently for a real-world application.Comment: Accepted for publication in ICRA 201

Angular velocity nonlinear observer from single vector measurements

The paper proposes a technique to estimate the angular velocity of a rigid body from single vector measurements. Compared to the approaches presented in the literature, it does not use attitude information nor rate gyros as inputs. Instead, vector measurements are directly filtered through a nonlinear observer estimating the angular velocity. Convergence is established using a detailed analysis of a linear-time varying dynamics appearing in the estimation error equation. This equation stems from the classic Euler equations and measurement equations. As is proven, the case of free-rotation allows one to relax the persistence of excitation assumption. Simulation results are provided to illustrate the method.Comment: 10 pages, 8 figures. arXiv admin note: substantial text overlap with arXiv:1503.0287

Derivative-free online learning of inverse dynamics models

This paper discusses online algorithms for inverse dynamics modelling in robotics. Several model classes including rigid body dynamics (RBD) models, data-driven models and semiparametric models (which are a combination of the previous two classes) are placed in a common framework. While model classes used in the literature typically exploit joint velocities and accelerations, which need to be approximated resorting to numerical differentiation schemes, in this paper a new `derivative-free' framework is proposed that does not require this preprocessing step. An extensive experimental study with real data from the right arm of the iCub robot is presented, comparing different model classes and estimation procedures, showing that the proposed `derivative-free' methods outperform existing methodologies.Comment: 14 pages, 11 figure

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