Ai Tutoring System

This project focuses on ai tutoring system using modern AI and machine learning techniques. The content below is adapted from research literature and practical implementation notes.

The Control Toolbox - An Open-Source C++ Library for Robotics,

Optimal and Model Predictive Control

Markus Giftthaler† , Michael Neunert† , Markus Stäuble and Jonas Buchli∗

Abstract— We introduce the Control Toolbox (CT), an open- such as Nonlinear Model Predictive Control (NMPC) or

source C++ library for efficient modeling, control, estimation, numerical optimal control quickly and with minimal effort. trajectory optimization and Model Predictive Control. The CT In contrast to highly integrated frameworks, the CT follows

is applicable to a broad class of dynamic systems but features interfaces to modeling tools specifically designed for robotic a modular library approach. Thus, it can be easily interfaced applications. This paper outlines the general concept of the with external modeling and solver frameworks. Additionally, toolbox, its main building blocks, and highlights selected ap- building blocks such as Automatic-Differentiation are usable

plication examples. The library contains several tools to design in several applications including optimal control, classical and evaluate controllers, model dynamical systems and solve feedback control (e.g. LQRs), Kalman filtering or sensitivity optimal control problems. The CT was designed for intuitive modeling of systems governed by ordinary differential or differ- analysis. In summary, several elements of a planning and

ence equations. It supports rapid prototyping of cost functions control pipeline for an application such as joint space PID and constraints and provides standard interfaces for different control, inverse dynamics control and motion planning can optimal control solvers. To date, we support Single Shooting, the be developed using a single library. iterative Linear-Quadratic Regulator, Gauss-Newton Multiple The CT has been designed to provide the tools needed

Shooting and classical Direct Multiple Shooting. We provide

interfaces to general purpose NLP solvers and Riccati-based for fast development and evaluation of control methods linear-quadratic optimal control solvers. The CT was designed while being optimized for efficiency and allowing for online to solve large-scale optimal control and estimation problems operation. While the emphasis lies on control, the provided efficiently and allows for online control of dynamic systems. tools can also be used for simulation, estimation or other

Some of the key features to enable fast run-time performance optimization applications. are full compatibility with Automatic Differentiation, derivative code generation, and multi-threading. Still, the CT is designed There are four key components of the control toolbox: as a modular framework whose building blocks can also be Modelling of continuous and discrete-time dynamical sys- used for other control and estimation applications such as tems, Automatic Differentiation, optimal control algorithms

inverse dynamics control, extended Kalman filters or kinematic and Rigid Body Dynamics algorithms. For many of these planning. The CT is available as open-source software under components, we have carefully selected existing implemen- the Apache v2 license and can be retrieved from https://bitbucket.org/adrlab/ct. tations and provide a seamless integration between them without creating a rigid framework. Instead, CT offers easy to

I. I NTRODUCTION use tools for fast prototyping, such as numerical integrators

A. What is the Control Toolbox? or LQR design. More complex approaches are provided in the form of reference implementations, such as generating a

A common task for robotics researchers and practitioners

whole-body Nonlinear Model Predictive Control setup for a is to model systems, implement equations of motion and robot based solely on a semantic description. design model-based controllers, estimators, planning algo- rithms, etc. Sooner or later, one is confronted with questions B. Related Work of efficient implementation, computing derivatives, formulat-

When looking at software tools in robotics, the library that

ing cost functions and constraints or running controllers in matches the scope of CT the closest is Drake . However, a model-predictive control fashion.

Drake started out as a Matlab implementation, which made

The Control Toolbox is specially designed for these tasks. it unsuitable for hard-realtime and online control. While its

It is written entirely in C++ and has a strong focus on

codebase is gradually moving towards C++, not all features highly efficient code that can be run online (in the loop) have been ported yet and Auto-Diff support is limited. on robots or other actuated hardware. A significant contri-

Another popular software library is MuJoCo , which

bution of the CT is its implementation of optimal control excels at simulation but follows a closed-source policy. algorithms, spanning a range from simple LQR reference

There are many optimal control toolboxes outside of

implementations to constrained Model Predictive Control. the robotics community which focus on transcribing and

The CT supports Automatic Differentiation (Auto-Diff) and

solving nonlinear optimal control problems and influenced allows to generate derivative code for arbitrary scalar and the development of CT. Notable examples are ACADO , vector-valued functions. The toolbox was designed with its successor ACADOS , PSOPT , the closed-source usability in mind, allowing users to apply advanced concepts † toolbox MUSCOD as well as the commercial tools These authors contributed equally to this work. ∗

GPOPS and ForcesPro . Broadly speaking, there are

linear-quadratic optimal control solvers such as HPIPM , D. Structure of This Paper general quadratic programming solvers such as qpOases , This paper is structured as follows. In Section II, we and general-purpose nonlinear-programming packages, such present an overview of the CT’s design and implementation as IPOPT , SNOPT or NLopt . Developing such and give an outline of its structure. The different main solvers is a research field by itself and not the primary scope modules of the CT are highlighted in Sections III to VI.

of the CT. Therefore, we provide an interface to SNOPT, Selected application examples are given in Section VII. For IPOPT, and HPIPM as well as custom implementations of real-time applications, optimizing runtime performance is iLQR and Gauss-Newton Multiple Shooting . This an important issue, on which we comment in Section VIII. gives the user the opportunity to evaluate different solver The paper is concluded by important links and licence in-

types and implementations. formation (Section IX) and acknowledgements to additional For Automatic Differentiation, we rely on CppAD paired contributors in Section X. with the code-generation framework CppADCodeGen .

For a more detailed review on Auto-Diff frameworks as II. OVERVIEW

well as why symbolic differentiation as e.g. available in

Matlab , Mathematica or MapleSim compares A. Fundamental Dependencies

unfavorably to Auto-Diff regarding speed, we refer to . The CT is written in C++ and has been tested under For modelling rigid body dynamics, there exists a variety Ubuntu 14.04 and 16.04 with library versions as provided of mature libraries, with notable examples being RBDL , in the package sources. Building the CT requires a C++ DART , Pinocchio , iDynTree and RobCo- compiler with C++11 support. Since the CT is designed as Gen . Due to its modularity, CT can be interfaced with a toolbox rather than an integrated application, we tried to

either of these libraries. We decided to provide a reference provide maximum flexibility to the users. Therefore, it is not interface to RobCoGen due to its wide Auto-Diff support tied to a specific middleware such as ROS and dependencies and suitability for online control. Support and interfaces to are kept at a minimum. The two essential dependencies other rigid body dynamics libraries are going to be added to for CT are Eigen and kindr (which is based on

CT once their Auto-Diff support improves. Eigen). Eigen is a popular library for linear algebra in C++ and provides efficient implementations of standard matrix C. Scope operations as well as more advanced linear algebra methods.

Kindr is a header only kinematics library which builds

For control, especially numerical optimal control in a on top of it and provides data types for different rotation robotics context, there are many individual libraries available representations such as quaternions, Euler angles or rotation that provide key ingredients such as modeling frameworks, matrices.

Auto-Diff, and optimal control solvers. However, due to

different data representations, modeling assumptions, basic B. Structure and Modules of the CT conventions and the lack of reference implementations in The Control Toolbox consists of three main modules. a robotics context, integrating these components is a te-

The core module (ct core), the optimal control module

dious, time-consuming and error-prone process. Also, for (ct optcon) and the rigid body dynamics module (ct rbd). researchers entering the field, the correct choice of modeling

There is a clear hierarchy between the modules, which means

framework or solver is difficult to make since the solvers’ the modules depend on each other in this order. For example, scope and functionality differ strongly. Furthermore, their one can use the core module without ct optcon or ct rbd. performance highly depends on the specific modeling im- plementation. • ct core provides general type definitions and mathe-

For this reason, the CT aims at providing users with the matical tools. For example, it contains most data type tools to quickly implement well-established control methods definitions, definitions for systems and controllers, as and combine their problem with different solvers, transcrip- well as basic functionality such as numerical integrators tion methods, and modeling approaches. Since the CT is for differential equations. open-source, it is easy to adapt individual components to • ct optcon builds on top of the ‘core’ module and adds

specific use cases or integrate custom modeling or solver infrastructure for defining and solving optimal control frameworks. On top of this integration, the CT follows a problems. It contains the functionality for defining cost holistic approach to robot control: the individual components functions, constraints, solver backends and a generic are not only useful for numerical optimal control but can also NMPC wrapper. be employed for classical feedback control, inverse dynamics • ct rbd provides tools for modelling rigid body dynamics

control, estimation and planning. The CT provides features systems and interfaces with ct core and ct optcon. such as Kalman filtering or contact constraint projection for For testing as well as for giving examples, we provide Rigid Body dynamics . Thus, the CT can be used for a fourth module: the ‘models’ module (ct models) contains several aspects of a robotics control and planning toolchain: various robot models including a quadruped, a robotic arm,

from low-level realtime closed-loop control to kinematic a normal quadrotor and a quadrotor with a slung load. These planning or dynamic whole-body trajectory optimization. four different modules are detailed in Sections III-VI.

III. C ORE M ODULE

C OMPARING DIFFERENT OPTIONS TO OBTAIN DERIVATIVES

A. Basic System Definitions Derivative Numerical Computation Setup Error

method Accuracy Speed Time Safety

The core module defines basic data types and interfaces

Num-Diff − − +++ +++

to describe non-linear system dynamics of the forms Analytic Deriv. +++ ++ − −

Symbolic Engine +++ + + ++

ẋ = f (x(t), t)) (1) Auto-diff +++ + ++ ++ ẋ = f (x(t), u(t), t) . (2) Auto-diff Codegen +++ +++ ++ ++

The right-hand side (RHS) of Equation (1) only depends

on the time and state x(t) (core::StateVector) and is called a C. Computing Derivatives core::System. The RHS of Equation (2) additionally depends

The CT can be used to compute derivatives of arbitrary

on the control input u(t) (core::ControlVector) and is named vector-valued smooth nonlinear functions f (x). For com- core::ControlledSystem. The dynamics equations can be im- puting first order derivatives (Jacobians) J = df dx , the most- plemented by the user in any desired way but are currently widespread methods are restricted to ordinary ODEs and difference equations. For the remainder of this section, we limit the scope to a continuous- 1) Numerical differentiation, e.g. by the method of finite-

time perspective. Note that for modeling robotic systems in differences, continuous-time, the rigid body dynamics module provides 2) Analytical derivation, e.g. performed manually, a variety of tools, which are detailed in Section V. 3) Symbolic math engines,

As the name suggests, the core::ControlledSystem pro- 4) Automatic Differentiation, also known as Algorithmic

vides the interface for closing a feedback control loop. Every Differentiation, with an optional source code genera- controlled system can take a pointer to a control law deriving tion step. from core::Controller. Full flexibility for implementing a The different approaches are compared in Table I. Auto- policy of general form u(x(t), t) is given to the user. matic Differentiation allows to conveniently obtain derivative This includes special cases where the control is merely information: it relieves the user from computing analytical

constant, depending on neither x(t) nor t, time-varying, only derivatives manually or symbolically1, which may be in- depending on t, or a general feedback controller, depending tractable for complex systems. However, it is as accurate on both x(t) and t. and fast as analytic derivatives and outperforms numerical We provide a set of pre-defined control laws, which differentiation in terms of accuracy and speed while pro-

includes a core::ConstantController with fixed u, a classical viding a similar level of convenience. Combining Automatic

PID controller (core::PIDController), or a full time-varying Differentiation with source code generation (Auto-Diff Code-

core::StateFeedbackController with feedforward term of gen) results in the best runtime of the differentiation methods form uf f (t) + K(t)(x(t) − xref (t)). supported by CT. For a detailed review and numerical examples, the interested reader is referred to .

B. Integration and Simulation

The CT provides different numerical integrators D. Linearizing Dynamic Systems

(core::Integrator). We offer own implementations and The CT defines the structure of a linear system integrators based on ‘boost odeint’ . (core::LinearSystem) as

The CT currently features fixed-step integrators like

ẋ(t) = A(t)x(t) + B(t)u(t) (3)

Euler and fourth-order Runge-Kutta as well as differ-

ent (error controlled) variable step integrators. Addition- where A and B are the Jacobians of a non-linear, time- ally, for symplectic systems (core::SymplecticSystem) a varying system evaluated at desired setpoints for x and semi-implicit Euler integrator (core::SymplecticIntegrator) u. In order to compute this linearization for a non- is available, which can help with stiff systems. All linear system, CT provides two different helper classes.

integrators take a pointer to a system and return The core::SystemLinearizer takes a core::ControlledSystem trajectories (core::DiscreteTrajectory), i.e. timed series and applies numerical differentiation to compute the Ja- of states and control inputs (core::StateTrajectory and cobians. Alternatively, the core::AutoDiffLinearizer can be core::ControlTrajectory) respectively. These trajectories can used to apply Auto-Differentiation for the Jacobians which

be either equidistant in time or unevenly sampled. In both is more accurate than numerical differentiation. Finally, cases, an interpolation strategy can be applied to obtain states Auto-Differentiation is combined with code generation in and inputs at a specific time which is not directly stored. core::ADCodegenLinearizer which is as accurate as analyt- For rapid prototyping and testing of control loops, we ical derivatives and typically fast to evaluate. The code-gen

provide a core::ControlSimulator which allows running con- linearizer employs a technique called just-in time compilation trollers and system integration in parallel and in real-time. (JIT), which generates the derivative code at runtime. Since Please note, however, that the CT cannot replace a high- this can take a few seconds, the derivative code can be stored fidelity physics simulator. For such purposes, we refer for 1 Auto-Diff uses graph structures to compute derivatives. Hence, it is

example to . inherently different from symbolic engines such as Maple or Maxima.

to file and compiled in separate libraries. Examples for this and solvers in an almost arbitrary way and therefore allows approach are given in ct models, see Section VI. for rapid prototyping of optimal control setups, including Nonlinear Model Predictive Control.

E. Computing Approximated and Exact Sensitivities

Many control algorithms, for example the direct ap- A. Cost Functions

proaches to optimal control shown in Section IV, require The cost function package provides means of quickly a discrete-time approximation of the nonlinear system dy- prototyping objective functions based on a highly modular namics of form xn+1 = An xn + B n un , where we call approach. A CT cost function is assumed to consist of a An and B n ‘sensitivities’. In many cases it may suffice sum of elementary cost function building blocks, which are

to approximate these matrices based on the continuous- called ‘terms’. Each term evaluates to a scalar as a function time counterparts A(t), B(t) and a simple Forward-Euler, of the current time, control input, and state and derives from Backward-Euler or Tustin discretization scheme. The CT optcon::TermBase. provides the core::SensitivityApproximation class, which can The overall cost function is designed such that it holds be used to compute such low-order approximations in a intermediate terms and final terms, which can be assigned

straight-forward way. individually. The intermediate and final costs are then given However, especially when aiming at a coarse time- as the sums over the evaluations of all intermediate and final discretization while dealing with a highly nonlinear dynamic terms. Equivalently, the intermediate and final derivatives re- systems, it can be beneficial to use higher-order integration sult as the sums of the individual intermediate and final term

schemes to compute An , B n . The core::SensitivityIntegrator gradients. The cost function package supports both analytic solves the integrals derivatives for terms as well as Automatic Differentiation and Z ∆t just-in-time compilation (JIT) up to second order derivatives. ∂f (x(t + τ ), u(t + τ ), t + τ ) We offer a selection of frequently used standard cost func-

An = dτ

0 ∂x(t) tion terms, which penalize the deviations from given control

Z ∆t

∂f (x(t + τ ), u(t + τ ), t + τ ) and state reference points, including a purely quadratic term

Bn = dτ

0 ∂u(t) (optcon::TermQuadratic), a cross-term (optcon::TermMixed) and a purely linear term (optcon::TermLinear). Furthermore for a given starting time t and time-step ∆t by means of there are terms for tracking reference trajectories in state integrating a Sensitivity ODE. Special cases for obtaining and control (optcon::TermQuadTracking) and terms which exact sensitivities for symplectic integration schemes are formulate soft constraints on state and control variables

included, too. Exact sensitivities can help to robustify and (optcon::TermStateBarrier). improve the convergence behavior of many optimal control

All existing terms can be automatically constructed from

algorithms in the CT, which are summarized below. text-files, in which the cost function weights and parameters IV. O PTIMAL C ONTROL M ODULE can be structured in a simple manner. For custom terms, reading from a file is simple to implement thanks to a pre-

A broad variety of model-based optimal control tasks can

specified set of loading methods. Additionally, all terms can be formulated as continuous-time optimal control problems. be made-time-varying using time-activation functions, which

From a robotics perspective, this includes tasks such as agile

can be used to introduce way-point costs, for example. flight, reaching an object in a cluttered scene, moving a mobile manipulator or quadrupedal locomotion. In direct op- B. Constraints timal control, the continuous-time optimal control problem is

The constraint package generalizes the modular idea pre-

first transcribed into a numerically tractable discrete problem. sented for cost functions in Section IV-A to vector-valued Two possible ways to complete this step are: functions. The corresponding elementary building blocks 1) Transcribing the problem into a nonlinear program derive from optcon::ConstraintBase and again support both (NLP) using multiple-shooting, single shooting or analytic derivatives, Automatic-Differentiation and Auto-Diff direct collocation and subsequently solving it using with JIT. For constraints, the terms are not summarized but

standard NLP solvers such as IPOPT or SNOPT. stacked in a so-called ‘constraint container’. Every container 2) Using iterative Riccati-based shooting methods derived additionally features an upper and a lower bound. For con- from the Principle of Optimality such as DDP , straints, we currently only support first-order derivatives (opt- their Gauss-Newton counterparts, iLQR or Gaus- con::LinearConstraintContainer). To date, the predefined Newton Multiple Shooting (GNMS) . These meth- terms include simple linear path inequality constraints and

ods are popular due to their overall efficiency and box constraints on states and controls. linear time complexity.

The package ct optcon covers both classical off-the-shelf C. Optimal Control Problem Containers

NLP solvers and custom Riccati-based solutions, paired A optcon::OptConProblem is a unified container for non- with different flavors of Single and Multiple Shooting. An linear controlled system dynamics, Equation (2), nonlinear important design feature is the CT’s modularity, which allows cost functions, nonlinear constraints, a time horizon variable combining different cost functions, dynamics, constraints, and an initial state. It serves as the main interface between

a user and the different implementations of optimal control G. Classical Direct Multiple Shooting algorithms and NMPC. Complementary to GNMS, the CT also implements the

Similarly, the container optcon::LQOCProblem is dedi- original Direct Multiple Shooting (DMS) method by Bock

cated to constrained linear-quadratic optimal control prob- and Plitt , which we solve using a classical NLP solver lems. However, this container is designed to directly store (see Section IV-E. We provide this method separately since it the linearized dynamics, the Jacobians and Hessians of complements the other algorithms in several aspects. While the cost function and the constraint Jacobians in matrix GNMS currently only supports a constant control param-

representation. eterization, DMS also supports linear interpolation. DMS

D. LQR and Linear Quadratic Solvers in combination with IPOPT can furthermore leverage exact

Hessians or other Hessian approximations. DMS further-

The CT provides C++ code for different variants of

more supports adaptive step-size integration. Lastly, DMS the classical Linear Quadratic Regulator. We provide direct can make use of more advanced globalization techniques and iterative solvers for the continuous-time Algebraic Ric- as employed by the NLP solvers, such as complex filter cati Equation (optcon::CARE), and iterative solvers for the schemes . However, for problems with long time hori- discrete-time Algebraic Riccati Equation (optcon::DARE). zons, the DMS implementation cannot compete with GNMS

Those can be used to design infinite-horizon LQR controllers

or iLQR at runtime, due to computational limitations of the and state- and disturbance estimators in both continuous- currently available off-the-shelf NLP solvers. and discrete time. Furthermore, there is a time-varying, finite-horizon discrete-time LQR version available (opt- H. Nonlinear Model Predictive Control con::FHDTLQR). Thanks to a dedicated design of interfaces between solvers For unconstrained linear-quadratic optimal control prob- and the optimal control problem definition, the CT opti-

lems, the CT offers a custom Riccati solver, opt- mal control problem solvers can be automatically run in con::GNRiccatiSolver, which achieves high efficiency using Nonlinear Model Predictive Control fashion using the class advanced options such as fixed Hessian regularization. optcon::MPC. The latter offers options like automatic warm-

For constrained LQ optimal control problems the CT starting, pre-integration for delay-compensation, different

includes an interface to the interior point solver HPIPM , modes to handle time horizons (e.g. receding horizon, fixed which is a comp

Frequently Asked Questions

What is this project about?

This project covers practical implementation and research aspects of the topic using AI/ML techniques.

This section provides additional detailed analysis and supporting information derived from the research paper content to ensure comprehensive coverage of the topic with expanded discussion on key concepts, methods, and findings.

Speaker Verification Xvector