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..11fc13b3 100644 --- a/control_toolbox/src/pid.cpp +++ b/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 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); + // 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_; } 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)