Add derivative filter and discretization

control_toolbox/include/control_toolbox/pid.hpp

@@ -33,8 +33,10 @@
 #ifndef CONTROL_TOOLBOX__PID_HPP_
 #define CONTROL_TOOLBOX__PID_HPP_
 
+#include <rcl/time.h>
 #include <chrono>
 #include <cmath>
+#include <cstdint>
 #include <iostream>
 #include <limits>
 #include <string>
@@ -118,15 +120,15 @@ struct AntiWindupStrategy
       (tracking_time_constant < 0.0 || !std::isfinite(tracking_time_constant)))
     {
       throw std::invalid_argument(
-        "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant "
+        "AntiWindupStrategy 'back_calculation' requires a non-negative tracking time constant "
         "(tracking_time_constant)");
     }
-    if (i_min > 0)
+    if (i_min >= 0)
     {
       throw std::invalid_argument(
         fmt::format("PID requires i_min to be smaller or equal to 0 (i_min: {})", i_min));
     }
-    if (i_max < 0)
+    if (i_max <= 0)
     {
       throw std::invalid_argument(
         fmt::format("PID requires i_max to be greater or equal to 0 (i_max: {})", i_max));
@@ -179,6 +181,87 @@ struct AntiWindupStrategy
     std::numeric_limits<double>::epsilon(); /**< Error deadband to avoid integration. */
 };
 
+struct DiscretizationMethod
+{
+public:
+  enum Value : int8_t
+  {
+    UNDEFINED = -1,
+    FORWARD_EULER,
+    BACKWARD_EULER,
+    TRAPEZOIDAL
+  };
+
+  constexpr DiscretizationMethod() : type(FORWARD_EULER) {}
+  explicit constexpr DiscretizationMethod(Value v) : type(v) {}
+  explicit DiscretizationMethod(const std::string & s) { set_type(s); }
+
+  void set_type(const std::string & s)
+  {
+    if (s == "forward_euler")
+    {
+      type = FORWARD_EULER;
+    }
+    else if (s == "backward_euler")
+    {
+      type = BACKWARD_EULER;
+    }
+    else if (s == "trapezoidal")
+    {
+      type = TRAPEZOIDAL;
+    }
+    else
+    {
+      type = UNDEFINED;
+      throw std::invalid_argument(
+        "DiscretizationMethod: Unknown discretization method : '" + s +
+        "'. Valid methods are: 'forward_euler', 'backward_euler', 'trapezoidal'.");
+    }
+  }
+
+  void validate() const
+  {
+    if (type == UNDEFINED)
+    {
+      throw std::invalid_argument("DiscretizationMethod is UNDEFINED. Please set a valid type");
+    }
+    if (type != FORWARD_EULER && type != BACKWARD_EULER && type != TRAPEZOIDAL && type != UNDEFINED)
+    {
+      throw std::invalid_argument("DiscretizationMethod has an invalid type");
+    }
+  }
+
+  operator std::string() const { return to_string(); }
+
+  constexpr bool operator==(Value other) const { return type == other; }
+  constexpr bool operator!=(Value other) const { return type != other; }
+  constexpr bool operator==(const DiscretizationMethod & other) const { return type == other.type; }
+  constexpr bool operator!=(const DiscretizationMethod & other) const { return type != other.type; }
+  constexpr DiscretizationMethod & operator=(Value v)
+  {
+    type = v;
+    return *this;
+  }
+
+  std::string to_string() const
+  {
+    switch (type)
+    {
+      case FORWARD_EULER:
+        return "forward_euler";
+      case BACKWARD_EULER:
+        return "backward_euler";
+      case TRAPEZOIDAL:
+        return "trapezoidal";
+      case UNDEFINED:
+      default:
+        return "UNDEFINED";
+    }
+  }
+
+  Value type = UNDEFINED;
+};
+
 template <typename T>
 inline bool is_zero(T value, T tolerance = std::numeric_limits<T>::epsilon())
 {
@@ -215,6 +298,7 @@ inline bool is_zero(T value, T tolerance = std::numeric_limits<T>::epsilon())
   \param p                          Proportional gain. Reacts to current error.
   \param i                          Integral gain. Accumulates past error to eliminate steady-state error.
   \param d                          Derivative gain. Predicts future error to reduce overshoot and settling time.
+  \param tf                         Derivative filter time constant. Used to filter high-frequency noise in the derivative term.
   \param u\_min                     Minimum bound for the controller output.
   \param u\_max                     Maximum bound for the controller output.
   \param tracking\_time\_constant   Tracking time constant for BACK_CALCULATION anti-windup.
@@ -227,6 +311,14 @@ inline bool is_zero(T value, T tolerance = std::numeric_limits<T>::epsilon())
                                      and unsaturated outputs using \c tracking\_time\_constant.
                                    - CONDITIONAL_INTEGRATION: only integrates when output is not saturated
                                      or error drives it away from saturation.
+  \param integration\_method        Method to compute the integral contribution. Options:
+                                   - "forward_euler"
+                                   - "backward_euler"
+                                   - "trapezoidal"
+  \param derivative\_method         Method to compute the derivative contribution. Options:
+                                   - "forward_euler"
+                                   - "backward_euler"
+                                   - "trapezoidal"
 
   \section antiwindup Anti-Windup Strategies
   Without anti-windup, clamping causes integral windup, leading to overshoot and sluggish
@@ -286,30 +378,71 @@ class Pid
     Gains(
       double p, double i, double d, double u_max, double u_min,
       const AntiWindupStrategy & antiwindup_strat)
+    : Gains(
+        p, i, d,
+        /*tf*/ 0.0, u_max, u_min, antiwindup_strat,
+        /*i_method*/ DiscretizationMethod(),
+        /*d_method*/ DiscretizationMethod())
+    {
+    }
+
+    /*!
+   * \brief Constructor for passing in values.
+   *
+   * \param p The proportional gain.
+   * \param i The integral gain.
+   * \param d The derivative gain.
+   * \param tf The derivative filter time constant.
+   * \param u_max Upper output clamp.
+   * \param u_min Lower output clamp.
+   * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation',
+        'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the
+        tracking_time_constant parameter to tune the anti-windup behavior.
+   * \param i_method Method to compute the integral contribution.
+   * \param d_method Method to compute the derivative contribution.
+   *
+   */
+    Gains(
+      double p, double i, double d, double tf, double u_max, double u_min,
+      const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method,
+      DiscretizationMethod d_method)
     : p_gain_(p),
       i_gain_(i),
       d_gain_(d),
+      tf_(tf),
+      i_max_(antiwindup_strat.i_max),
+      i_min_(antiwindup_strat.i_min),
       u_max_(u_max),
       u_min_(u_min),
-      antiwindup_strat_(antiwindup_strat)
+      antiwindup_strat_(antiwindup_strat),
+      i_method_(i_method),
+      d_method_(d_method)
     {
     }
 
     bool validate(std::string & error_msg) const
     {
-      if (u_min_ >= u_max_)  // is false if any value is nan
+      if (u_min_ > u_max_)  // is false if any value is nan
       {
-        error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_);
+        error_msg =
+          fmt::format("Gains: u_min ({}) must be less than or equal to u_max ({})", u_min_, u_max_);
         return false;
       }
       else if (std::isnan(u_min_) || std::isnan(u_max_))
       {
         error_msg = "Gains: u_min and u_max must not be NaN";
         return false;
       }
+      else if (tf_ < 0.0)
+      {
+        error_msg = "Gains: tf (derivative filter time constant) must be non-negative";
+        return false;
+      }
       try
       {
         antiwindup_strat_.validate();
+        i_method_.validate();
+        d_method_.validate();
       }
       catch (const std::exception & e)
       {
@@ -322,16 +455,26 @@ class Pid
     void print() const
     {
       std::cout << "Gains: p: " << p_gain_ << ", i: " << i_gain_ << ", d: " << d_gain_
-                << ", u_max: " << u_max_ << ", u_min: " << u_min_ << std::endl;
+                << ", tf: " << tf_ << ", i_max: " << i_max_ << ", i_min: " << i_min_
+                << ", u_max: " << u_max_ << ", u_min: " << u_min_
+                << ", i_method: " << i_method_.to_string()
+                << ", d_method: " << d_method_.to_string() << std::endl;
       antiwindup_strat_.print();
     }
 
-    double p_gain_ = 0.0;                                     /**< Proportional gain. */
-    double i_gain_ = 0.0;                                     /**< Integral gain. */
-    double d_gain_ = 0.0;                                     /**< Derivative gain. */
+    double p_gain_ = 0.0; /**< Proportional gain. */
+    double i_gain_ = 0.0; /**< Integral gain. */
+    double d_gain_ = 0.0; /**< Derivative gain. */
+    double tf_ = 0.0;     /**< Derivative filter time constant. */
+    double i_max_ =
+      std::numeric_limits<double>::infinity(); /**< Maximum allowable integral term. */
+    double i_min_ =
+      -std::numeric_limits<double>::infinity(); /**< Minimum allowable integral term. */
     double u_max_ = std::numeric_limits<double>::infinity();  /**< Maximum allowable output. */
     double u_min_ = -std::numeric_limits<double>::infinity(); /**< Minimum allowable output. */
     AntiWindupStrategy antiwindup_strat_;                     /**< Anti-windup strategy. */
+    DiscretizationMethod i_method_;                           /**< Integration method. */
+    DiscretizationMethod d_method_;                           /**< Derivative method.  */
   };
 
   /*!
@@ -354,6 +497,28 @@ class Pid
     double u_min = -std::numeric_limits<double>::infinity(),
     const AntiWindupStrategy & antiwindup_strat = AntiWindupStrategy());
 
+  /*!
+   * \brief Constructor, initialize Pid-gains and term limits.
+   *
+   * \param p The proportional gain.
+   * \param i The integral gain.
+   * \param d The derivative gain.
+   * \param tf The derivative filter time constant.
+   * \param u_max Upper output clamp.
+   * \param u_min Lower output clamp.
+   * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation',
+        'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the
+        tracking_time_constant parameter to tune the anti-windup behavior.
+   * \param i_method Method to compute the integral contribution.
+   * \param d_method Method to compute the derivative contribution.
+   *
+   * \throws An std::invalid_argument exception is thrown if u_min > u_max.
+   */
+  Pid(
+    double p, double i, double d, double tf, double u_max, double u_min,
+    const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method,
+    DiscretizationMethod d_method);
+
   /*!
    * \brief Copy constructor required for preventing mutexes from being copied
    * \param source - Pid to copy
@@ -384,6 +549,29 @@ class Pid
     double p, double i, double d, double u_max, double u_min,
     const AntiWindupStrategy & antiwindup_strat);
 
+  /*!
+   * \brief Initialize Pid-gains and term limits.
+   *
+   * \param p The proportional gain.
+   * \param i The integral gain.
+   * \param d The derivative gain.
+   * \param tf The derivative filter time constant.
+   * \param u_max Upper output clamp.
+   * \param u_min Lower output clamp.
+   * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation',
+        'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the
+        tracking_time_constant parameter to tune the anti-windup behavior.
+   * \param i_method Method to compute the integral contribution.
+   * \param d_method Method to compute the derivative contribution.
+   * \return True if all parameters are successfully set, False otherwise.
+   *
+   * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max.
+   */
+  bool initialize(
+    double p, double i, double d, double tf, double u_max, double u_min,
+    const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method,
+    DiscretizationMethod d_method);
+
   /*!
    * \brief Reset the state of this PID controller
    * @note The integral term is not retained and it is reset to zero
@@ -419,6 +607,27 @@ class Pid
     double & p, double & i, double & d, double & u_max, double & u_min,
     AntiWindupStrategy & antiwindup_strat);
 
+  /*!
+   * \brief Get PID gains for the controller (preferred).
+   * \param p The proportional gain.
+   * \param i The integral gain.
+   * \param d The derivative gain.
+   * \param tf The derivative filter time constant.
+   * \param u_max Upper output clamp.
+   * \param u_min Lower output clamp.
+   * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation',
+        'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the
+        tracking_time_constant parameter to tune the anti-windup behavior.
+   * \param i_method Method to compute the integral contribution.
+   * \param d_method Method to compute the derivative contribution.
+   *
+   * \note This method is not RT safe
+   */
+  void get_gains(
+    double & p, double & i, double & d, double & tf, double & u_max, double & u_min,
+    AntiWindupStrategy & antiwindup_strat, DiscretizationMethod & i_method,
+    DiscretizationMethod & d_method);
+
   /*!
    * \brief Get PID gains for the controller.
    * \return gains A struct of the PID gain values
@@ -455,6 +664,30 @@ class Pid
     double p, double i, double d, double u_max, double u_min,
     const AntiWindupStrategy & antiwindup_strat);
 
+  /*!
+   * \brief Set PID gains for the controller.
+   *
+   * \param p The proportional gain.
+   * \param i The integral gain.
+   * \param d The derivative gain.
+   * \param tf The derivative filter time constant.
+   * \param u_max Upper output clamp.
+   * \param u_min Lower output clamp.
+   * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation',
+        'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the
+        tracking_time_constant parameter to tune the anti-windup behavior.
+   * \param i_method Method to compute the integral contribution.
+   * \param d_method Method to compute the derivative contribution.
+   * \return True if all parameters are successfully set, False otherwise.
+   *
+   * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max.
+   * \note This method is not RT safe
+   */
+  bool set_gains(
+    double p, double i, double d, double tf, double u_max, double u_min,
+    const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method,
+    DiscretizationMethod d_method);
+
   /*!
    * \brief Set PID gains for the controller.
    * \param gains A struct of the PID gain values.
@@ -611,19 +844,29 @@ class Pid
     0.0,
     0.0,
     0.0,
+    0.0,
     std::numeric_limits<double>::infinity(),
     -std::numeric_limits<double>::infinity(),
-    AntiWindupStrategy()};
+    AntiWindupStrategy(),
+    DiscretizationMethod(),
+    DiscretizationMethod()};
   // Store the PID gains in a realtime box to allow dynamic reconfigure to update it without
   // blocking the realtime update loop
   realtime_tools::RealtimeThreadSafeBox<Gains> gains_box_{gains_};
 
   double p_error_last_ = 0; /** Save state for derivative state calculation. */
   double p_error_ = 0;      /** Error. */
+
+  double d_error_last_ = 0; /** Save state for derivative state calculation. */
+  double d_term_last_ = 0;  /** Save state for derivative filter. */
   double d_error_ = 0;      /** Derivative of error. */
+
   double i_term_ = 0;       /** Integrator state. */
-  double cmd_ = 0;          /** Command to send. */
-  double cmd_unsat_ = 0;    /** command without saturation. */
+  double i_term_last_ = 0;  /** Last integrator state. */
+  double aw_term_last_ = 0; /** Last anti-windup term. */
+
+  double cmd_ = 0;       /** Command to send. */
+  double cmd_unsat_ = 0; /** command without saturation. */
 };
 
 }  // namespace control_toolbox