From 4832a88cef3557754eeb0475a351884ef8287831 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 7 Mar 2025 20:52:47 +0000 Subject: [PATCH 1/2] Save i_term instead of error --- include/control_toolbox/pid.hpp | 2 +- src/pid.cpp | 30 +++++++++++------------------- src/pid_ros.cpp | 12 ++++++------ test/pid_tests.cpp | 17 +++++++++-------- 4 files changed, 27 insertions(+), 34 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 5248a97d..ee05284c 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -431,8 +431,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. */ }; diff --git a/src/pid.cpp b/src/pid.cpp index 44ee4169..b0d7f62f 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -99,7 +99,7 @@ void Pid::reset(bool save_i_term) void Pid::clear_saved_iterm() { - i_error_ = 0.0; + i_term_ = 0.0; } void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & i_min) @@ -182,7 +182,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; @@ -194,29 +194,21 @@ 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) { - // 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_) { - // Limit i_term so that the limit is meaningful in the output - i_term = std::clamp(i_term, gains.i_min_, gains.i_max_); + 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_; } // 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_; } @@ -228,7 +220,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/src/pid_ros.cpp b/src/pid_ros.cpp index fee5310e..04fefe15 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -273,8 +273,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_) { @@ -284,7 +284,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_; @@ -314,8 +314,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" @@ -326,7 +326,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/test/pid_tests.cpp b/test/pid_tests.cpp index d8559a5a..14b8c147 100644 --- a/test/pid_tests.cpp +++ b/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 8176afc6dac177a2cafd2d94cb25f8e0c9793d7f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 7 Mar 2025 20:56:47 +0000 Subject: [PATCH 2/2] Fix docstring --- include/control_toolbox/pid.hpp | 2 +- include/control_toolbox/pid_ros.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index ee05284c..8ba57117 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -400,7 +400,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); diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index 83c2d91e..388f1300 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -217,7 +217,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);