[PID] Add support for saving i-term when PID is reset

include/control_toolbox/pid.hpp

@@ -57,6 +57,14 @@ namespace control_toolbox
   be subclassed to provide more specific controls
   based on a particular control loop.
 
+  This class also allows for retention of integral
+  term on reset.  This is useful for control loops
+  that are enabled/disabled with a constant steady-state
+  external disturbance.  Once the integrator cancels
+  out the external disturbance, disabling/resetting/
+  re-enabling closed-loop control does not require
+  the integrator to wind up again.
+
   In particular, this class implements the standard
   pid equation:
 
@@ -83,6 +91,8 @@ namespace control_toolbox
 
   \param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$
 
+  \param save_iterm boolean indicating if integral term is retained on reset()
+
   \section Usage
 
   To use the Pid class, you should first call some version of init()
@@ -113,18 +123,27 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    */
   struct Gains
   {
-    // Optional constructor for passing in values without antiwindup
+    // Optional constructor for passing in values without antiwindup and save i-term
     Gains(double p, double i, double d, double i_max, double i_min)
-    : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(false)
+    : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(false),
+      save_iterm_(false)
     {
     }
-    // Optional constructor for passing in values
+    // Optional constructor for passing in values without save i-term
     Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
-    : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup)
+    : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup),
+      save_iterm_(false)
+    {
+    }
+    // Optional constructor for passing in values
+    Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup,
+      bool save_iterm) : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min),
+      antiwindup_(antiwindup), save_iterm_(save_iterm)
     {
     }
     // Default constructor
-    Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0), antiwindup_(false)
+    Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0),
+      antiwindup_(false), save_iterm_(false)
     {
     }
     double p_gain_;   /**< Proportional gain. */
@@ -133,6 +152,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid
     double i_max_;    /**< Maximum allowable integral term. */
     double i_min_;    /**< Minimum allowable integral term. */
     bool antiwindup_; /**< Antiwindup. */
+    bool save_iterm_; /**< Save integral term. */
   };
 
   /*!
@@ -151,7 +171,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    */
   Pid(
     double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0,
-    bool antiwindup = false);
+    bool antiwindup = false, bool save_iterm = false);
 
   /**
    * \brief Copy constructor required for preventing mutexes from being copied
@@ -176,13 +196,19 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    *
    * \note New gains are not applied if i_min_ > i_max_
    */
-  void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
+  void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false,
+    bool save_iterm = false);
 
   /*!
    * \brief Reset the state of this PID controller
    */
   void reset();
 
+  /*!
+   * \brief Clear the saved integrator output of this controller
+   */
+  void clear_saved_iterm();
+
   /*!
    * \brief Get PID gains for the controller.
    * \param p The proportional gain.
@@ -194,6 +220,9 @@ class CONTROL_TOOLBOX_PUBLIC Pid
   void getGains(double & p, double & i, double & d, double & i_max, double & i_min);
   void getGains(
     double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup);
+  void getGains(
+    double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup,
+    bool & save_iterm);
 
   /*!
    * \brief Get PID gains for the controller.
@@ -211,7 +240,8 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    *
    * \note New gains are not applied if i_min > i_max
    */
-  void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
+  void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false,
+    bool save_iterm = false);
 
   /*!
    * \brief Set PID gains for the controller.

include/control_toolbox/pid_ros.hpp

@@ -98,10 +98,12 @@ class CONTROL_TOOLBOX_PUBLIC PidROS
    * \param i_max The max integral windup.
    * \param i_min The min integral windup.
    * \param antiwindup antiwindup.
+   * \param save_iterm save integrator output between resets.
    *
    * \note New gains are not applied if i_min_ > i_max_
    */
-  void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup);
+  void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup,
+    bool save_iterm = false);
 
   /*!
    * \brief Initialize the PID controller based on already set parameters
@@ -153,10 +155,12 @@ class CONTROL_TOOLBOX_PUBLIC PidROS
    * \param i_max The max integral windup.
    * \param i_min The min integral windup.
    * \param antiwindup antiwindup.
+   * \param save_iterm save integrator output between resets.
    *
    * \note New gains are not applied if i_min > i_max
    */
-  void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
+  void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false,
+    bool save_iterm = false);
 
   /*!
    * \brief Set PID gains for the controller.

src/pid.cpp

@@ -46,13 +46,16 @@
 
 namespace control_toolbox
 {
-Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
+Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_iterm)
 : gains_buffer_()
 {
   if (i_min > i_max) {
     throw std::invalid_argument("received i_min > i_max");
   }
-  setGains(p, i, d, i_max, i_min, antiwindup);
+  setGains(p, i, d, i_max, i_min, antiwindup, save_iterm);
+
+  // Initialize saved i-term values
+  clear_saved_iterm();
 
   reset();
 }
@@ -62,15 +65,19 @@ Pid::Pid(const Pid & source)
   // Copy the realtime buffer to the new PID class
   gains_buffer_ = source.gains_buffer_;
 
+  // Initialize saved i-term values
+  clear_saved_iterm();
+
   // Reset the state of this PID controller
   reset();
 }
 
 Pid::~Pid() {}
 
-void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
+void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup,
+  bool save_iterm)
 {
-  setGains(p, i, d, i_max, i_min, antiwindup);
+  setGains(p, i, d, i_max, i_min, antiwindup, save_iterm);
 
   reset();
 }
@@ -79,19 +86,35 @@ void Pid::reset()
 {
   p_error_last_ = 0.0;
   p_error_ = 0.0;
-  i_error_ = 0.0;
   d_error_ = 0.0;
   cmd_ = 0.0;
+
+  // If last integral error is already zero, just return
+  if (std::fabs(i_error_) < std::numeric_limits<double>::epsilon())
+  {
+    return;
+  }
+
+  // Get the gain parameters from the realtime buffer
+  Gains gains = *gains_buffer_.readFromRT();
+  // Check to see if we should reset integral error here
+  if (!gains.save_iterm_) i_error_ = 0.0;
+}
+
+void Pid::clear_saved_iterm()
+{
+  i_error_ = 0.0;
 }
 
 void Pid::getGains(double & p, double & i, double & d, double & i_max, double & i_min)
 {
-  bool antiwindup;
-  getGains(p, i, d, i_max, i_min, antiwindup);
+  bool antiwindup, save_iterm;
+  getGains(p, i, d, i_max, i_min, antiwindup, save_iterm);
 }
 
 void Pid::getGains(
-  double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup)
+  double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup,
+  bool & save_iterm)
 {
   Gains gains = *gains_buffer_.readFromRT();
 
@@ -101,13 +124,15 @@ void Pid::getGains(
   i_max = gains.i_max_;
   i_min = gains.i_min_;
   antiwindup = gains.antiwindup_;
+  save_iterm = gains.save_iterm_;
 }
 
 Pid::Gains Pid::getGains() { return *gains_buffer_.readFromRT(); }
 
-void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
+void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup,
+  bool save_iterm)
 {
-  Gains gains(p, i, d, i_max, i_min, antiwindup);
+  Gains gains(p, i, d, i_max, i_min, antiwindup, save_iterm);
 
   setGains(gains);
 }

src/pid_ros.cpp

@@ -169,6 +169,7 @@ bool PidROS::initPid()
   double p, i, d, i_min, i_max;
   p = i = d = i_min = i_max = std::numeric_limits<double>::quiet_NaN();
   bool antiwindup = false;
+  bool save_iterm = false;
   bool all_params_available = true;
   all_params_available &= getDoubleParam(param_prefix_ + "p", p);
   all_params_available &= getDoubleParam(param_prefix_ + "i", i);
@@ -177,12 +178,13 @@ bool PidROS::initPid()
   all_params_available &= getDoubleParam(param_prefix_ + "i_clamp_min", i_min);
 
   getBooleanParam(param_prefix_ + "antiwindup", antiwindup);
+  getBooleanParam(param_prefix_ + "save_iterm", save_iterm);
 
   if (all_params_available) {
     setParameterEventCallback();
   }
 
-  pid_.initPid(p, i, d, i_max, i_min, antiwindup);
+  pid_.initPid(p, i, d, i_max, i_min, antiwindup, save_iterm);
 
   return all_params_available;
 }
@@ -194,19 +196,21 @@ void PidROS::declareParam(const std::string & param_name, rclcpp::ParameterValue
   }
 }
 
-void PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
+void PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup,
+  bool save_iterm)
 {
   if (i_min > i_max) {
     RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains");
   } else {
-    pid_.initPid(p, i, d, i_max, i_min, antiwindup);
+    pid_.initPid(p, i, d, i_max, i_min, antiwindup, save_iterm);
 
     declareParam(param_prefix_ + "p", rclcpp::ParameterValue(p));
     declareParam(param_prefix_ + "i", rclcpp::ParameterValue(i));
     declareParam(param_prefix_ + "d", rclcpp::ParameterValue(d));
     declareParam(param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(i_max));
     declareParam(param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(i_min));
     declareParam(param_prefix_ + "antiwindup", rclcpp::ParameterValue(antiwindup));
+    declareParam(param_prefix_ + "save_iterm", rclcpp::ParameterValue(save_iterm));
 
     setParameterEventCallback();
   }
@@ -237,7 +241,8 @@ double PidROS::computeCommand(double error, double error_dot, rclcpp::Duration d
 
 Pid::Gains PidROS::getGains() { return pid_.getGains(); }
 
-void PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
+void PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup,
+  bool save_iterm)
 {
   if (i_min > i_max) {
     RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains");
@@ -247,9 +252,10 @@ void PidROS::setGains(double p, double i, double d, double i_max, double i_min,
       rclcpp::Parameter(param_prefix_ + "d", d),
       rclcpp::Parameter(param_prefix_ + "i_clamp_max", i_max),
       rclcpp::Parameter(param_prefix_ + "i_clamp_min", i_min),
-      rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup)});
+      rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup),
+     rclcpp::Parameter(param_prefix_ + "save_iterm", save_iterm)});
 
-    pid_.setGains(p, i, d, i_max, i_min, antiwindup);
+      pid_.setGains(p, i, d, i_max, i_min, antiwindup, save_iterm);
   }
 }
 
@@ -309,6 +315,8 @@ void PidROS::printValues()
                                                     << "  I_Min:        " << gains.i_min_ << "\n"
                                                     << "  Antiwindup:   " << gains.antiwindup_
                                                     << "\n"
+                                                    << "  Save I-Term:  " << gains.save_iterm_
+                                                    << "\n"
                                                     << "  P_Error:      " << p_error_ << "\n"
                                                     << "  I_Error:      " << i_error_ << "\n"
                                                     << "  D_Error:      " << d_error_ << "\n"
@@ -355,6 +363,9 @@ void PidROS::setParameterEventCallback()
         } else if (param_name == param_prefix_ + "antiwindup") {
           gains.antiwindup_ = parameter.get_value<bool>();
           changed = true;
+        } else if (param_name == param_prefix_ + "save_iterm") {
+          gains.save_iterm_ = parameter.get_value<bool>();
+          changed = true;
         }
       } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
         RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Please use the right type: " << e.what());