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

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

Model Predictive Control for Micro Aerial Vehicles: A Survey

This paper presents a review of the design and application of model predictive control strategies for Micro Aerial Vehicles and specifically multirotor configurations such as quadrotors. The diverse set of works in the domain is organized based on the control law being optimized over linear or nonlinear dynamics, the integration of state and input constraints, possible fault-tolerant design, if reinforcement learning methods have been utilized and if the controller refers to free-flight or other tasks such as physical interaction or load transportation. A selected set of comparison results are also presented and serve to provide insight for the selection between linear and nonlinear schemes, the tuning of the prediction horizon, the importance of disturbance observer-based offset-free tracking and the intrinsic robustness of such methods to parameter uncertainty. Furthermore, an overview of recent research trends on the combined application of modern deep reinforcement learning techniques and model predictive control for multirotor vehicles is presented. Finally, this review concludes with explicit discussion regarding selected open-source software packages that deliver off-the-shelf model predictive control functionality applicable to a wide variety of Micro Aerial Vehicle configurations

A Family of Iterative Gauss-Newton Shooting Methods for Nonlinear Optimal Control

This paper introduces a family of iterative algorithms for unconstrained nonlinear optimal control. We generalize the well-known iLQR algorithm to different multiple-shooting variants, combining advantages like straight-forward initialization and a closed-loop forward integration. All algorithms have similar computational complexity, i.e. linear complexity in the time horizon, and can be derived in the same computational framework. We compare the full-step variants of our algorithms and present several simulation examples, including a high-dimensional underactuated robot subject to contact switches. Simulation results show that our multiple-shooting algorithms can achieve faster convergence, better local contraction rates and much shorter runtimes than classical iLQR, which makes them a superior choice for nonlinear model predictive control applications.Comment: 8 page

Ungar \unicode{x2013} A C++ Framework for Real-Time Optimal Control Using Template Metaprogramming

We present Ungar, an open-source library to aid the implementation of high-dimensional optimal control problems (OCPs). We adopt modern template metaprogramming techniques to enable the compile-time modeling of complex systems while retaining maximum runtime efficiency. Our framework provides syntactic sugar to allow for expressive formulations of a rich set of structured dynamical systems. While the core modules depend only on the header-only Eigen and Boost.Hana libraries, we bundle our codebase with optional packages and custom wrappers for automatic differentiation, code generation, and nonlinear programming. Finally, we demonstrate the versatility of Ungar in various model predictive control applications, namely, four-legged locomotion and collaborative loco-manipulation with multiple one-armed quadruped robots. Ungar is available under the Apache License 2.0 at https://github.com/fdevinc/ungar.Comment: 2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 7 pages, 2 figures. Library available at https://github.com/fdevinc/ungar. Presentation available at https://www.youtube.com/watch?v=iKQ6felf45

SOTER: A Runtime Assurance Framework for Programming Safe Robotics Systems

The recent drive towards achieving greater autonomy and intelligence in robotics has led to high levels of complexity. Autonomous robots increasingly depend on third party off-the-shelf components and complex machine-learning techniques. This trend makes it challenging to provide strong design-time certification of correct operation. To address these challenges, we present SOTER, a robotics programming framework with two key components: (1) a programming language for implementing and testing high-level reactive robotics software and (2) an integrated runtime assurance (RTA) system that helps enable the use of uncertified components, while still providing safety guarantees. SOTER provides language primitives to declaratively construct a RTA module consisting of an advanced, high-performance controller (uncertified), a safe, lower-performance controller (certified), and the desired safety specification. The framework provides a formal guarantee that a well-formed RTA module always satisfies the safety specification, without completely sacrificing performance by using higher performance uncertified components whenever safe. SOTER allows the complex robotics software stack to be constructed as a composition of RTA modules, where each uncertified component is protected using a RTA module. To demonstrate the efficacy of our framework, we consider a real-world case-study of building a safe drone surveillance system. Our experiments both in simulation and on actual drones show that the SOTER-enabled RTA ensures the safety of the system, including when untrusted third-party components have bugs or deviate from the desired behavior

Multi-rotor with suspended load: System Dynamics and Control Toolbox

There is an increasing demand for Unmanned Aerial Systems (UAS) to carry suspended loads as this can provide significant benefits to several applications in agriculture, law enforcement and construction. The load impact on the underlying system dynamics should not be neglected as significant feedback forces may be induced on the vehicle during certain flight manoeuvres. The constant variation in operating point induced by the slung load also causes conventional controllers to demand increased control effort. Much research has focused on standard multi-rotor position and attitude control with and without a slung load. However, predictive control schemes, such as Nonlinear Model Predictive Control (NMPC), have not yet been fully explored. To this end, we present a novel controller for safe and precise operation of multi-rotors with heavy slung load in three dimensions. The paper describes a System Dynamics and Control Simulation Toolbox for use with MATLAB/SIMULINK which includes a detailed simulation of the multi-rotor and slung load as well as a predictive controller to manage the nonlinear dynamics whilst accounting for system constraints. It is demonstrated that the controller simultaneously tracks specified waypoints and actively damps large slung load oscillations. A linear-quadratic regulator (LQR) is derived and control performance is compared. Results show the improved performance of the predictive controller for a larger flight envelope, including aggressive manoeuvres and large slung load displacements. The computational cost remains relatively small, amenable to practical implementations