pid.hpp

// Copyright (C) 2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <chrono>
 
#include <smooth/concepts/lie_group.hpp>
#include <smooth/spline/spline.hpp>
 
#include "time.hpp"
 
namespace smooth::feedback {
 
struct PIDParams
{
  double windup_limit = std::numeric_limits<double>::infinity();
};
 
template<Time T, LieGroup G>
  requires(Dof<G> > 0)
class PID
{
public:
  using TrajectoryReturnT = std::tuple<G, Tangent<G>, Tangent<G>>;
 
  inline PID(const PIDParams & prm = PIDParams{}) noexcept : prm_(prm) {}
  PID(const PID &) = default;
  PID(PID &&) = default;
  PID & operator=(const PID &) = default;
  PID & operator=(PID &&) = default;
  ~PID() = default;
 
  inline Tangent<G> operator()(const T & t, const G & x, const Tangent<G> & v)
  {
    const auto [g_des, v_des, a_des] = x_des_(t);
    const Tangent<G> g_err           = g_des - x;
 
    if (t_last && t > t_last.value()) {
      // update integral state
      i_err_ += time_trait<T>::minus(t, t_last.value()) * g_err;
      i_err_ = i_err_.cwiseMax(-prm_.windup_limit).cwiseMin(prm_.windup_limit);
    }
    t_last = t;
 
    return a_des + kp_.cwiseProduct(g_err) + kd_.cwiseProduct(v_des - v) + ki_.cwiseProduct(i_err_);
  }
 
  inline void set_kp(double kp) { kp_.setConstant(kp); }
 
  template<typename Derived>
  inline void set_kp(const Eigen::MatrixBase<Derived> & kp)
  {
    kp_ = kp;
  }
 
  inline void set_kd(double kd) { kd_.setConstant(kd); }
 
  template<typename Derived>
  inline void set_kd(const Eigen::MatrixBase<Derived> & kd)
  {
    kd_ = kd;
  }
 
  inline void set_ki(double ki) { ki_.setConstant(ki); }
 
  template<typename Derived>
  inline void set_ki(const Eigen::MatrixBase<Derived> & ki)
  {
    ki_ = ki;
  }
 
  inline void reset_integral() { i_err_.setZero(); }
 
  template<int K>
  inline void set_xdes(T t0, const smooth::Spline<K, G> & c)
  {
    set_xdes(t0, smooth::Spline<K, G>(c));
  }
 
  template<int K>
  inline void set_xdes(T t0, smooth::Spline<K, G> && c)
  {
    set_xdes([t0 = std::move(t0), c = std::move(c)](T t) -> TrajectoryReturnT {
      Tangent<G> vel, acc;
      G x = c(time_trait<T>::minus(t, t0), vel, acc);
      return TrajectoryReturnT(std::move(x), std::move(vel), std::move(acc));
    });
  }
 
  inline void set_xdes(const std::function<TrajectoryReturnT(T)> & f)
  {
    auto f_copy = f;
    set_xdes(std::move(f_copy));
  }
 
  inline void set_xdes(std::function<TrajectoryReturnT(T)> && f) { x_des_ = std::move(f); }
 
private:
  PIDParams prm_;
 
  // gains
  Tangent<G> kd_ = Tangent<G>::Ones();
  Tangent<G> kp_ = Tangent<G>::Ones();
  Tangent<G> ki_ = Tangent<G>::Zero();
 
  // integral state
  std::optional<T> t_last;
  Tangent<G> i_err_ = Tangent<G>::Zero();
 
  // desired trajectory
  std::function<TrajectoryReturnT(T)> x_des_ = [](T) -> TrajectoryReturnT {
    return TrajectoryReturnT(Identity<G>(), Tangent<G>::Zero(), Tangent<G>::Zero());
  };
};
 
}  // namespace smooth::feedback