From aac226e0cf4636842e87a0242bc20a898feaae5f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 12 Mar 2025 11:51:11 +0100 Subject: [PATCH 1/2] [Pid] Save `i_term` instead of error integral (#294) (cherry picked from commit 37a12e854a9c389b3857e39f5556f7310babad9f) # Conflicts: # control_toolbox/src/pid.cpp --- .../include/control_toolbox/pid.hpp | 4 ++-- .../include/control_toolbox/pid_ros.hpp | 2 +- control_toolbox/src/pid.cpp | 24 ++++++++++++++++--- control_toolbox/src/pid_ros.cpp | 12 +++++----- control_toolbox/test/pid_tests.cpp | 17 ++++++------- 5 files changed, 39 insertions(+), 20 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 75f6e935..ed537b5a 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/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 */ diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index 46b05995..427555ca 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/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); diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 24982abe..acc502ec 100644 --- a/control_toolbox/src/pid.cpp +++ b/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_; } diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 402246c4..a2f055a5 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/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();); } diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 08ddf291..7a5386ec 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/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) From c9532588a6bdc469da007163b4cce25f31c69274 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 7 Jun 2025 20:07:16 +0000 Subject: [PATCH 2/2] Fix merge conflicts --- control_toolbox/src/pid.cpp | 40 +++++++------------------------------ 1 file changed, 7 insertions(+), 33 deletions(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index acc502ec..11fc13b3 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -111,14 +111,7 @@ 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::clear_saved_iterm() { i_term_ = 0.0; } void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & i_min) { @@ -226,34 +219,15 @@ 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_; - - if (gains.antiwindup_ && gains.i_gain_ != 0) - { - // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_ - std::pair bounds = - std::minmax(gains.i_min_ / gains.i_gain_, gains.i_max_ / gains.i_gain_); - i_error_ = std::clamp(i_error_, bounds.first, bounds.second); - } - // Calculate integral contribution to command - i_term = gains.i_gain_ * i_error_; - - if (!gains.antiwindup_) + if (gains.antiwindup_) { - // 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_ = 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 @@ -261,7 +235,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) // Compute the command // 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; + cmd_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; return cmd_; }