[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());