[Pid] Save i_term instead of error integral (backport #294) (#389)

Co-authored-by: Christoph Froehlich <[email protected]>

Author: mergify[bot]

control_toolbox/include/control_toolbox/pid.hpp

@@ -518,7 +518,7 @@ class Pid
   /*!
    * \brief Return PID error terms for the controller.
    * \param pe  The proportional error.
-   * \param ie  The integral error.
+   * \param ie  The weighted integral error.
    * \param de  The derivative error.
    */
   void get_current_pid_errors(double & pe, double & ie, double & de);
@@ -559,8 +559,8 @@ class Pid
 
   double p_error_last_; /** Save state for derivative state calculation. */
   double p_error_;      /** Error. */
-  double i_error_;      /** Integral of error. */
   double d_error_;      /** Derivative of error. */
+  double i_term_;       /** Integrator state. */
   double cmd_;          /** Command to send. */
   // TODO(christophfroehlich) remove this -> breaks ABI
   [[deprecated("Use d_error_")]] double error_dot_; /** Derivative error */

control_toolbox/include/control_toolbox/pid_ros.hpp

@@ -327,7 +327,7 @@ class PidROS
   /*!
    * \brief Return PID error terms for the controller.
    * \param pe[out] The proportional error.
-   * \param ie[out] The integral error.
+   * \param ie[out] The weighted integral error.
    * \param de[out] The derivative error.
    */
   void get_current_pid_errors(double & pe, double & ie, double & de);

control_toolbox/src/pid.cpp

@@ -111,7 +111,7 @@ void Pid::reset(bool save_i_term)
   }
 }
 
-void Pid::clear_saved_iterm() { i_error_ = 0.0; }
+void Pid::clear_saved_iterm() { i_term_ = 0.0; }
 
 void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & i_min)
 {
@@ -202,7 +202,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
   // Get the gain parameters from the realtime buffer
   Gains gains = *gains_buffer_.readFromRT();
 
-  double p_term, d_term, i_term;
+  double p_term, d_term;
   p_error_ = error;  // this is error = target - state
   d_error_ = error_dot;
 
@@ -219,31 +219,23 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
   // Calculate proportional contribution to command
   p_term = gains.p_gain_ * p_error_;
 
-  // Calculate the integral of the position error
-  i_error_ += dt_s * p_error_;
-
-  if (gains.antiwindup_ && gains.i_gain_ != 0)
+  // Calculate integral contribution to command
+  if (gains.antiwindup_)
   {
-    // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
-    std::pair<double, double> bounds =
-      std::minmax<double>(gains.i_min_ / gains.i_gain_, gains.i_max_ / gains.i_gain_);
-    i_error_ = std::clamp(i_error_, bounds.first, bounds.second);
+    // Prevent i_term_ from climbing higher than permitted by i_max_/i_min_
+    i_term_ = std::clamp(i_term_ + gains.i_gain_ * dt_s * p_error_, gains.i_min_, gains.i_max_);
   }
-
-  // Calculate integral contribution to command
-  i_term = gains.i_gain_ * i_error_;
-
-  if (!gains.antiwindup_)
+  else
   {
-    // Limit i_term so that the limit is meaningful in the output
-    i_term = std::clamp(i_term, gains.i_min_, gains.i_max_);
+    i_term_ += gains.i_gain_ * dt_s * p_error_;
   }
 
   // Calculate derivative contribution to command
   d_term = gains.d_gain_ * d_error_;
 
   // Compute the command
-  cmd_ = p_term + i_term + d_term;
+  // Limit i_term so that the limit is meaningful in the output
+  cmd_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term;
 
   return cmd_;
 }
@@ -255,7 +247,7 @@ double Pid::get_current_cmd() { return cmd_; }
 void Pid::get_current_pid_errors(double & pe, double & ie, double & de)
 {
   pe = p_error_;
-  ie = i_error_;
+  ie = i_term_;
   de = d_error_;
 }
 

control_toolbox/src/pid_ros.cpp

@@ -297,8 +297,8 @@ void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt)
 {
   Pid::Gains gains = pid_.get_gains();
 
-  double p_error, i_error, d_error;
-  get_current_pid_errors(p_error, i_error, d_error);
+  double p_error, i_term, d_error;
+  get_current_pid_errors(p_error, i_term, d_error);
 
   // Publish controller state if configured
   if (rt_state_pub_)
@@ -310,7 +310,7 @@ void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt)
       rt_state_pub_->msg_.error = error;
       rt_state_pub_->msg_.error_dot = d_error;
       rt_state_pub_->msg_.p_error = p_error;
-      rt_state_pub_->msg_.i_error = i_error;
+      rt_state_pub_->msg_.i_error = i_term;
       rt_state_pub_->msg_.d_error = d_error;
       rt_state_pub_->msg_.p_term = gains.p_gain_;
       rt_state_pub_->msg_.i_term = gains.i_gain_;
@@ -340,8 +340,8 @@ void PidROS::print_values()
 {
   Pid::Gains gains = pid_.get_gains();
 
-  double p_error, i_error, d_error;
-  get_current_pid_errors(p_error, i_error, d_error);
+  double p_error, i_term, d_error;
+  get_current_pid_errors(p_error, i_term, d_error);
 
   RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Current Values of PID template:\n"
                                                     << "  P Gain:       " << gains.p_gain_ << "\n"
@@ -352,7 +352,7 @@ void PidROS::print_values()
                                                     << "  Antiwindup:   " << gains.antiwindup_
                                                     << "\n"
                                                     << "  P_Error:      " << p_error << "\n"
-                                                    << "  I_Error:      " << i_error << "\n"
+                                                    << "  i_term:      " << i_term << "\n"
                                                     << "  D_Error:      " << d_error << "\n"
                                                     << "  Command:      " << get_current_cmd(););
 }

control_toolbox/test/pid_tests.cpp

@@ -117,6 +117,7 @@ TEST(ParameterTest, integrationClampZeroGainTest)
   EXPECT_EQ(0.0, cmd);
 }
 
+constexpr double EPS = 1e-9;
 TEST(ParameterTest, integrationAntiwindupTest)
 {
   RecordProperty(
@@ -131,16 +132,16 @@ TEST(ParameterTest, integrationAntiwindupTest)
   double cmd = 0.0;
 
   cmd = pid.compute_command(-1.0, 1.0);
-  EXPECT_EQ(-1.0, cmd);
+  EXPECT_NEAR(-1.0, cmd, EPS);
 
   cmd = pid.compute_command(-1.0, 1.0);
-  EXPECT_EQ(-1.0, cmd);
+  EXPECT_NEAR(-1.0, cmd, EPS);
 
   cmd = pid.compute_command(0.5, 1.0);
-  EXPECT_EQ(0.0, cmd);
+  EXPECT_NEAR(0.0, cmd, EPS);
 
   cmd = pid.compute_command(-1.0, 1.0);
-  EXPECT_EQ(-1.0, cmd);
+  EXPECT_NEAR(-1.0, cmd, EPS);
 }
 
 TEST(ParameterTest, negativeIntegrationAntiwindupTest)
@@ -157,16 +158,16 @@ TEST(ParameterTest, negativeIntegrationAntiwindupTest)
   double cmd = 0.0;
 
   cmd = pid.compute_command(0.1, 1.0);
-  EXPECT_EQ(-0.2, cmd);
+  EXPECT_NEAR(-0.2, cmd, EPS);
 
   cmd = pid.compute_command(0.1, 1.0);
-  EXPECT_EQ(-0.2, cmd);
+  EXPECT_NEAR(-0.2, cmd, EPS);
 
   cmd = pid.compute_command(-0.05, 1.0);
-  EXPECT_EQ(-0.075, cmd);
+  EXPECT_NEAR(-0.075, cmd, EPS);
 
   cmd = pid.compute_command(0.1, 1.0);
-  EXPECT_EQ(-0.2, cmd);
+  EXPECT_NEAR(-0.2, cmd, EPS);
 }
 
 TEST(ParameterTest, gainSettingCopyPIDTest)