smooth_feedback: Control and estimation on Lie groups

[![CI Build and Test][ci-shield]][ci-link] [![Code coverage][cov-shield]][cov-link] [![License][license-shield]][license-link]

Tool collection for control and estimation on Lie groups leveraging the [smooth][smooth-link] library.

• Requirements: C++20, Eigen 3.4, boost::numeric::odeint, [smooth][smooth-link]
• [Documentation][doc-link]

Control on Lie groups

These controllers are implemented for systems with dynamics on the form where T is a smooth::feedback::Time, X is a smooth::LieGroup, and U is a smooth::Manifold.

Nonlinearities are handled via linearization around a reference point or trajectory. For group-linear dynamics this automatically results in a linear system in the tangent space, in which case these algorithms are expected to work very well. Linearization is done via [automatic differentiation][ad-link]. For this to work with the most performant methods (e.g. [autodiff][autodiff-link]) the functions must be templated on the scalar type. The dynamical system

can be defined via a lambda function that supports automatic differentiation as follows:

#include <Eigen/Core>

#include <smooth/bundle.hpp>
#include <smooth/se2.hpp>

#include <chrono>

using T = std::chrono::duration<double>;
template<typename S>
using X = smooth::Bundle<smooth::SE2<S>, Eigen::Matrix<S, 3, 1>>;
template<typename S>
using U = Eigen::Matrix<S, 2, 1>;

const Eigen::Matrix3d A{
  {-0.2, 0, 0},
  {0, 0, 0},
  {0, 0, -0.4},
};
const Eigen::Matrix<double, 3, 2> B{
  {1, 0},
  {0, 0},
  {0, 1},
};

auto Sigma = []<typename S>(T, const X<S> & x, const U<S> & u) -> smooth::Tangent<X<S>> {
  smooth::Tangent<X<S>> dx_dt;
  dx_dt.head(3) = x.template part<1>();
  dx_dt.tail(3) = A * x.template part<1>() + B * u;
  return dx_dt;
};

PID Control

• Model-free
• Assumes that inputs control body acceleration. See examples/pid_se2.cpp for an example of allocating PID inputs to actuators.

Example PID controller on SE(2)

#include <smooth/feedpack/pid.hpp>

smooth::feedback::PID<T, smooth::SE2d> pid;

// set desired motion
pid.set_xdes([](T T) -> std::tuple<smooth::SE2d, Eigen::Vector3d, Eigen::Vector3d> {
  return {
    smooth::SE2d::Identity(),  // position
    Eigen::Vector3d::Zero(),   // velocity (right derivative of position w.r.t. t)
    Eigen::Vector3d::Zero(),   // acceleration (second right derivative of position w.r.t. t)
  };
});

T t               = T(1);                       // current time
smooth::SE2d x    = smooth::SE2d::Random();     // current state
Eigen::Vector3d v = Eigen::Vector3d::Random();  // current body velocity

Eigen::Vector3d u = pid(t, x, v);

Model-Predictive Control

• Automatic linearization and time discretization of nonlinear continuous dynamics
• Define state and input reference trajectories via arbitrary functions T -> X and T -> U for a time type T. The bus in the video above uses MPC to track the boundary of the circle.

Example: Model-predictive control for the system Sigma (see also examples/mpc_asif_vehicle.cpp)

```cpp #include <smooth/feedback/mpc.hpp>

smooth::feedback::MPC<T, X, U, decltype(Sigma)> mpc(Sigma, {.T = 5, .K = 50});

File truncated at 100 lines see the full file