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

include/control_toolbox/pid.hpp

@@ -52,6 +52,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:
 
@@ -132,18 +140,20 @@ class Pid
    * \param d The derivative gain.
    * \param i_max The max integral windup.
    * \param i_min The min integral windup.
-   * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced
+   * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced.
    *
    * \throws An std::invalid_argument exception is thrown if i_min > i_max
    */
     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)
     {
     }
+
     // 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)
     {
     }
+
     double p_gain_;   /**< Proportional gain. */
     double i_gain_;   /**< Integral gain. */
     double d_gain_;   /**< Derivative gain. */
@@ -201,6 +211,18 @@ class Pid
    */
   void reset();
 
+  /*!
+   * \brief Reset the state of this PID controller
+   *
+   * \param save_iterm boolean indicating if integral term is retained on reset()
+   */
+  void reset(bool save_iterm);
+
+  /*!
+   * \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.

include/control_toolbox/pid_ros.hpp

@@ -102,6 +102,21 @@ class PidROS
    */
   void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup);
 
+  /*!
+   * \brief Initialize the PID controller and set the parameters
+   * \param p The proportional gain.
+   * \param i The integral gain.
+   * \param d The derivative gain.
+   * \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,
+    bool save_iterm);
+
   /*!
    * \brief Initialize the PID controller based on already set parameters
    * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise
@@ -113,6 +128,13 @@ class PidROS
    */
   void reset();
 
+  /*!
+   * \brief Reset the state of this PID controller
+   *
+   * \param save_iterm boolean indicating if integral term is retained on reset()
+   */
+  void reset(bool save_iterm = false);
+
   /*!
    * \brief Set the PID error and compute the PID command with nonuniform time
    * step size. The derivative error is computed from the change in the error

src/pid.cpp

@@ -54,6 +54,9 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwind
   }
   setGains(p, i, d, i_max, i_min, antiwindup);
 
+  // Initialize saved i-term values
+  clear_saved_iterm();
+
   reset();
 }
 
@@ -62,6 +65,9 @@ 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();
 }
@@ -76,12 +82,30 @@ void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool
 }
 
 void Pid::reset()
+{
+  reset(false);
+}
+
+void Pid::reset(bool save_iterm)
 {
   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::abs(i_error_) < std::numeric_limits<double>::epsilon())
+  {
+    return;
+  }
+
+  // Check to see if we should reset integral error here
+  if (!save_iterm) clear_saved_iterm();
+}
+
+void Pid::clear_saved_iterm()
+{
+  i_error_ = 0.0;
 }
 
 void Pid::getGains(double & p, double & i, double & d, double & i_max, double & i_min)

src/pid_ros.cpp

@@ -177,6 +177,7 @@ bool PidROS::initPid()
   all_params_available &= getDoubleParam(param_prefix_ + "i_clamp_min", i_min);
 
   getBooleanParam(param_prefix_ + "antiwindup", antiwindup);
+  declareParam(param_prefix_ + "save_iterm", rclcpp::ParameterValue(false));
 
   if (all_params_available) {
     setParameterEventCallback();
@@ -195,6 +196,12 @@ 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)
+{
+  initPid(p, i, d, i_max, i_min, antiwindup, false);
+}
+
+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");
@@ -207,12 +214,22 @@ void PidROS::initPid(double p, double i, double d, double i_max, double i_min, b
     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();
   }
 }
 
-void PidROS::reset() { pid_.reset(); }
+void PidROS::reset() {
+  bool save_iterm = false;
+  getBooleanParam(param_prefix_ + "save_iterm", save_iterm);
+  reset(save_iterm);
+}
+
+void PidROS::reset(bool save_iterm)
+{
+  pid_.reset(save_iterm);
+}
 
 std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>> PidROS::getPidStatePublisher()
 {
@@ -249,7 +266,7 @@ void PidROS::setGains(double p, double i, double d, double i_max, double i_min,
       rclcpp::Parameter(param_prefix_ + "i_clamp_min", i_min),
       rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup)});
 
-    pid_.setGains(p, i, d, i_max, i_min, antiwindup);
+      pid_.setGains(p, i, d, i_max, i_min, antiwindup);
   }
 }
 

test/pid_parameters_tests.cpp

@@ -57,8 +57,9 @@ void check_set_parameters(
   const double I_MAX = 10.0;
   const double I_MIN = -10.0;
   const bool ANTIWINDUP = true;
+  const bool SAVE_ITERM = true;
 
-  ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP));
+  ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM));
 
   rclcpp::Parameter param;
 
@@ -81,6 +82,9 @@ void check_set_parameters(
   ASSERT_TRUE(node->get_parameter(prefix + "antiwindup", param));
   ASSERT_EQ(param.get_value<bool>(), ANTIWINDUP);
 
+  ASSERT_TRUE(node->get_parameter(prefix + "save_iterm", param));
+  ASSERT_EQ(param.get_value<bool>(), SAVE_ITERM);
+
   // check gains were set
   control_toolbox::Pid::Gains gains = pid.getGains();
   ASSERT_EQ(gains.p_gain_, P);
@@ -215,8 +219,9 @@ TEST(PidParametersTest, SetParametersTest)
   const double I_MAX = 10.0;
   const double I_MIN = -10.0;
   const bool ANTIWINDUP = true;
+  const bool SAVE_ITERM = false;
 
-  pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP);
+  pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM);
 
   rcl_interfaces::msg::SetParametersResult set_result;
 
@@ -237,6 +242,8 @@ TEST(PidParametersTest, SetParametersTest)
   ASSERT_TRUE(set_result.successful);
   ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP)));
   ASSERT_TRUE(set_result.successful);
+  ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("save_iterm", SAVE_ITERM)));
+  ASSERT_TRUE(set_result.successful);
 
   // process callbacks
   rclcpp::spin_some(node->get_node_base_interface());
@@ -314,7 +321,7 @@ TEST(PidParametersTest, GetParametersTest)
   const double I_MIN = -10.0;
   const bool ANTIWINDUP = true;
 
-  pid.initPid(0.0, 0.0, 0.0, 0.0, 0.0, false);
+  pid.initPid(0.0, 0.0, 0.0, 0.0, 0.0, false, false);
   pid.setGains(P, I, D, I_MAX, I_MIN, ANTIWINDUP);
 
   rclcpp::Parameter param;
@@ -336,6 +343,9 @@ TEST(PidParametersTest, GetParametersTest)
 
   ASSERT_TRUE(node->get_parameter("antiwindup", param));
   ASSERT_EQ(param.get_value<bool>(), ANTIWINDUP);
+
+  ASSERT_TRUE(node->get_parameter("save_iterm", param));
+  ASSERT_EQ(param.get_value<bool>(), false);
 }
 
 TEST(PidParametersTest, GetParametersFromParams)
@@ -380,8 +390,8 @@ TEST(PidParametersTest, MultiplePidInstances)
   const double I_MAX = 10.0;
   const double I_MIN = -10.0;
 
-  ASSERT_NO_THROW(pid_1.initPid(P, I, D, I_MAX, I_MIN, false));
-  ASSERT_NO_THROW(pid_2.initPid(P, I, D, I_MAX, I_MIN, true));
+  ASSERT_NO_THROW(pid_1.initPid(P, I, D, I_MAX, I_MIN, false, false));
+  ASSERT_NO_THROW(pid_2.initPid(P, I, D, I_MAX, I_MIN, true, false));
 
   rclcpp::Parameter param_1, param_2;
   ASSERT_TRUE(node->get_parameter("PID_1.p", param_1));

test/pid_publisher_tests.cpp

@@ -39,7 +39,7 @@ TEST(PidPublisherTest, PublishTest)
 
   control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node);
 
-  pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false);
+  pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false, false);
 
   bool callback_called = false;
   control_msgs::msg::PidState::SharedPtr last_state_msg;
@@ -76,7 +76,7 @@ TEST(PidPublisherTest, PublishTestLifecycle)
     std::dynamic_pointer_cast<rclcpp_lifecycle::LifecyclePublisher<control_msgs::msg::PidState>>(
       pid_ros.getPidStatePublisher());
 
-  pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false);
+  pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false, false);
 
   bool callback_called = false;
   control_msgs::msg::PidState::SharedPtr last_state_msg;

test/pid_tests.cpp

@@ -345,6 +345,36 @@ TEST(CommandTest, integralOnlyTest)
   cmd = pid.computeCommand(1.0, static_cast<uint64_t>(1.0 * 1e9));
   // Expect that the command is cleared since error = -1 * previous command, i-gain = 1, dt = 1
   EXPECT_EQ(0.0, cmd);
+
+  // If initial error = 0, i-gain = 1, dt = 1
+  cmd = pid.computeCommand(-0.5, static_cast<uint64_t>(1.0 * 1e9));
+  // Then expect command = error
+  EXPECT_EQ(-0.5, cmd);
+  // after reset without argument (save_iterm=false)
+  // we expect the command to be 0 if update is called error = 0
+  pid.reset();
+  cmd = pid.computeCommand(0.0, static_cast<uint64_t>(1.0 * 1e9));
+  EXPECT_EQ(0.0, cmd);
+
+  // If initial error = 0, i-gain = 1, dt = 1
+  cmd = pid.computeCommand(-0.5, static_cast<uint64_t>(1.0 * 1e9));
+  // Then expect command = error
+  EXPECT_EQ(-0.5, cmd);
+  // after reset with argument (save_iterm=false)
+  // we expect the command to be 0 if update is called error = 0
+  pid.reset(false);
+  cmd = pid.computeCommand(0.0, static_cast<uint64_t>(1.0 * 1e9));
+  EXPECT_EQ(0.0, cmd);
+
+  // If initial error = 0, i-gain = 1, dt = 1
+  cmd = pid.computeCommand(-0.5, static_cast<uint64_t>(1.0 * 1e9));
+  // Then expect command = error
+  EXPECT_EQ(-0.5, cmd);
+  // after reset with save_iterm=true
+  // we expect still the same command if update is called error = 0
+  pid.reset(true);
+  cmd = pid.computeCommand(0.0, static_cast<uint64_t>(1.0 * 1e9));
+  EXPECT_EQ(-0.5, cmd);
 }
 
 TEST(CommandTest, derivativeOnlyTest)