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

(cherry picked from commit 37a12e8)

# Conflicts:
#	control_toolbox/src/pid.cpp

Author: christophfroehlich

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,14 @@ void Pid::reset(bool save_i_term)
   }
 }
 
+<<<<<<< HEAD:control_toolbox/src/pid.cpp
 void Pid::clear_saved_iterm() { i_error_ = 0.0; }
+=======
+void Pid::clear_saved_iterm()
+{
+  i_term_ = 0.0;
+}
+>>>>>>> 37a12e8 ([Pid] Save `i_term` instead of error integral (#294)):src/pid.cpp
 
 void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & i_min)
 {
@@ -202,7 +209,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,6 +226,7 @@ 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_;
 
+<<<<<<< HEAD:control_toolbox/src/pid.cpp
   // Calculate the integral of the position error
   i_error_ += dt_s * p_error_;
 
@@ -237,13 +245,23 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
   {
     // Limit i_term so that the limit is meaningful in the output
     i_term = std::clamp(i_term, gains.i_min_, gains.i_max_);
+=======
+  // Calculate integral contribution to command
+  if (gains.antiwindup_) {
+    // 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_);
+  } else {
+    i_term_ += gains.i_gain_ * dt_s * p_error_;
+>>>>>>> 37a12e8 ([Pid] Save `i_term` instead of error integral (#294)):src/pid.cpp
   }
 
   // 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 +273,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)