From 52e89ed1485eb0a16b16136bef958028ad5a1fc3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 06:50:19 +0000 Subject: [PATCH 1/5] Improve validation of i_min/i_max of PID --- control_toolbox/include/control_toolbox/pid.hpp | 12 ++++++++---- control_toolbox/include/control_toolbox/pid_ros.hpp | 4 ++-- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 8d05af1e..33d736f6 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -121,11 +121,15 @@ struct AntiWindupStrategy "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant " "(tracking_time_constant)"); } - if (i_min >= i_max) + if (i_min > 0) { throw std::invalid_argument( - fmt::format( - "PID requires i_min < i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max)); + fmt::format("PID requires i_min to be smaller or equal to 0 (i_min: {})", i_min)); + } + if (i_max < 0) + { + throw std::invalid_argument( + fmt::format("PID requires i_max to be greater or equal to 0 (i_max: {})", i_max)); } if ( type != NONE && type != UNDEFINED && type != BACK_CALCULATION && @@ -358,7 +362,7 @@ class Pid 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. * - * \throws An std::invalid_argument exception is thrown if u_min > u_max. + * \throws An std::invalid_argument exception is thrown if u_min > 0 or u_max < 0. */ Pid( double p = 0.0, double i = 0.0, double d = 0.0, diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index a973aa39..63794c20 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -137,7 +137,7 @@ class PidROS * \param save_i_term save integrator output between resets. * \return True if all parameters are successfully set, False otherwise. * - * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max. + * \note New gains are not applied if antiwindup_strat.i_min > 0, antiwindup_strat.i_max < 0 or u_min > u_max. */ bool initialize_from_args( double p, double i, double d, double u_max, double u_min, @@ -210,7 +210,7 @@ class PidROS tracking_time_constant parameter to tune the anti-windup behavior. * \return True if all parameters are successfully set, False otherwise. * - * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max. + * \note New gains are not applied if antiwindup_strat.i_min > 0, antiwindup_strat.i_max < 0 or u_min > u_max. * \note This method is not RT safe */ bool set_gains( From 64011ec238f2589184feca1dd072e1109d005ba1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 06:56:55 +0000 Subject: [PATCH 2/5] Add test for it --- .../test/pid_ros_parameters_tests.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 62e483bc..38c81d72 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -146,7 +146,10 @@ TEST(PidParametersTest, InitPidTestBadParameter) ANTIWINDUP_STRAT.i_min = I_MIN_BAD; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); + bool ret; + ASSERT_NO_THROW( + ret = pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); + ASSERT_FALSE(ret); rclcpp::Parameter param; @@ -173,6 +176,19 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, 0.0); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); + + // Try other invalid combinations + ANTIWINDUP_STRAT.i_max = 10.; + ANTIWINDUP_STRAT.i_min = 5.; + ASSERT_NO_THROW( + ret = pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); + ASSERT_FALSE(ret); + + ANTIWINDUP_STRAT.i_max = -5.; + ANTIWINDUP_STRAT.i_min = 10.; + ASSERT_NO_THROW( + ret = pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); + ASSERT_FALSE(ret); } TEST(PidParametersTest, InitPid_param_prefix_only) From b419fc35df6bd0417df317ef6b2a986a893fcec0 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 14 Oct 2025 06:54:04 +0000 Subject: [PATCH 3/5] Remove double check --- .../include/control_toolbox/pid.hpp | 21 ++++++++-------- control_toolbox/src/pid.cpp | 2 +- control_toolbox/src/pid_ros.cpp | 10 ++++---- .../test/pid_ros_parameters_tests.cpp | 24 +++++++++---------- control_toolbox/test/pid_tests.cpp | 12 +++++----- 5 files changed, 34 insertions(+), 35 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 33d736f6..46a8dc58 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -139,6 +139,13 @@ struct AntiWindupStrategy } } + void print() const + { + std::cout << "antiwindup_strat: " << to_string() << "\ti_max: " << i_max << ", i_min: " << i_min + << "\ttracking_time_constant: " << tracking_time_constant + << "\terror_deadband: " << error_deadband << std::endl; + } + operator std::string() const { return to_string(); } constexpr bool operator==(Value other) const { return type == other; } @@ -282,8 +289,6 @@ class Pid : p_gain_(p), i_gain_(i), d_gain_(d), - i_max_(antiwindup_strat.i_max), - i_min_(antiwindup_strat.i_min), u_max_(u_max), u_min_(u_min), antiwindup_strat_(antiwindup_strat) @@ -303,12 +308,7 @@ class Pid bool validate(std::string & error_msg) const { - if (i_min_ > i_max_) - { - error_msg = fmt::format("Gains: i_min ({}) must be less than i_max ({})", i_min_, i_max_); - return false; - } - else if (u_min_ >= u_max_) + if (u_min_ >= u_max_) { error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_); return false; @@ -333,9 +333,8 @@ class Pid void print() const { std::cout << "Gains: p: " << p_gain_ << ", i: " << i_gain_ << ", d: " << d_gain_ - << ", i_max: " << i_max_ << ", i_min: " << i_min_ << ", u_max: " << u_max_ - << ", u_min: " << u_min_ << ", antiwindup_strat: " << antiwindup_strat_.to_string() - << std::endl; + << ", u_max: " << u_max_ << ", u_min: " << u_min_ << std::endl; + antiwindup_strat_.print(); } double p_gain_ = 0.0; /**< Proportional gain. */ diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 8a2495d1..49a8d435 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -321,7 +321,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) } } - i_term_ = std::clamp(i_term_, gains_.i_min_, gains_.i_max_); + i_term_ = std::clamp(i_term_, gains_.antiwindup_strat_.i_min, gains_.antiwindup_strat_.i_max); return cmd_; } diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 710c217d..dc8c6b83 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -410,12 +410,12 @@ void PidROS::print_values() << " P Gain: " << gains.p_gain_ << "\n" << " I Gain: " << gains.i_gain_ << "\n" << " D Gain: " << gains.d_gain_ << "\n" - << " I Max: " << gains.i_max_ << "\n" - << " I Min: " << gains.i_min_ << "\n" << " U_Max: " << gains.u_max_ << "\n" << " U_Min: " << gains.u_min_ << "\n" - << " Tracking_Time_Constant: " << gains.antiwindup_strat_.tracking_time_constant << "\n" << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\n" + << " Tracking_Time_Constant: " << gains.antiwindup_strat_.tracking_time_constant << "\n" + << " I Max: " << gains.antiwindup_strat_.i_max << "\n" + << " I Min: " << gains.antiwindup_strat_.i_min << "\n" << "\n" << " P Error: " << p_error << "\n" << " I Term: " << i_term << "\n" @@ -502,12 +502,12 @@ void PidROS::set_parameter_event_callback() } else if (param_name == param_prefix_ + "i_clamp_max") { - gains.i_max_ = parameter.get_value(); + gains.antiwindup_strat_.i_max = parameter.get_value(); changed = true; } else if (param_name == param_prefix_ + "i_clamp_min") { - gains.i_min_ = parameter.get_value(); + gains.antiwindup_strat_.i_min = parameter.get_value(); changed = true; } else if (param_name == param_prefix_ + "u_clamp_max") diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 38c81d72..8cf4b34c 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -105,8 +105,8 @@ void check_set_parameters( ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); - ASSERT_EQ(gains.i_max_, I_MAX); - ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX); + ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN); ASSERT_EQ(gains.u_max_, U_MAX); ASSERT_EQ(gains.u_min_, U_MIN); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); @@ -170,8 +170,8 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.p_gain_, 0.0); ASSERT_EQ(gains.i_gain_, 0.0); ASSERT_EQ(gains.d_gain_, 0.0); - ASSERT_EQ(gains.i_max_, std::numeric_limits::infinity()); - ASSERT_EQ(gains.i_min_, -std::numeric_limits::infinity()); + ASSERT_EQ(gains.antiwindup_strat_.i_max, std::numeric_limits::infinity()); + ASSERT_EQ(gains.antiwindup_strat_.i_min, -std::numeric_limits::infinity()); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, 0.0); @@ -303,8 +303,8 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); - ASSERT_EQ(gains.i_max_, I_MAX); - ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX); + ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN); ASSERT_EQ(gains.u_max_, U_MAX); ASSERT_EQ(gains.u_min_, U_MIN); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); @@ -378,8 +378,8 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); - ASSERT_EQ(gains.i_max_, I_MAX); - ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX); + ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); @@ -400,8 +400,8 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); - ASSERT_EQ(gains.i_max_, I_MAX); - ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX); + ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); @@ -419,8 +419,8 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(updated_gains.p_gain_, P); ASSERT_EQ(updated_gains.i_gain_, I); ASSERT_EQ(updated_gains.d_gain_, D); - ASSERT_EQ(updated_gains.i_max_, I_MAX); - ASSERT_EQ(updated_gains.i_min_, I_MIN); + ASSERT_EQ(updated_gains.antiwindup_strat_.i_max, I_MAX); + ASSERT_EQ(updated_gains.antiwindup_strat_.i_min, I_MIN); ASSERT_EQ(updated_gains.u_max_, U_MAX); ASSERT_EQ(updated_gains.u_min_, U_MIN); ASSERT_EQ(updated_gains.antiwindup_strat_.tracking_time_constant, TRK_TC); diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 5629cd5f..de2fb680 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -420,8 +420,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(p_gain, g1.p_gain_); EXPECT_EQ(i_gain, g1.i_gain_); EXPECT_EQ(d_gain, g1.d_gain_); - EXPECT_EQ(i_max, g1.i_max_); - EXPECT_EQ(i_min, g1.i_min_); + EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max); + EXPECT_EQ(i_min, g1.antiwindup_strat_.i_min); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant); @@ -443,8 +443,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(p_gain_return, g1.p_gain_); EXPECT_EQ(i_gain_return, g1.i_gain_); EXPECT_EQ(d_gain_return, g1.d_gain_); - EXPECT_EQ(antiwindup_strat_return.i_max, g1.i_max_); - EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_); + EXPECT_EQ(antiwindup_strat_return.i_max, g1.antiwindup_strat_.i_max); + EXPECT_EQ(antiwindup_strat_return.i_min, g1.antiwindup_strat_.i_min); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant); @@ -470,8 +470,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(p_gain_return, g1.p_gain_); EXPECT_EQ(i_gain_return, g1.i_gain_); EXPECT_EQ(d_gain_return, g1.d_gain_); - EXPECT_EQ(antiwindup_strat_return.i_max, g1.i_max_); - EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_); + EXPECT_EQ(antiwindup_strat_return.i_max, g1.antiwindup_strat_.i_max); + EXPECT_EQ(antiwindup_strat_return.i_min, g1.antiwindup_strat_.i_min); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant); From 087c395abca981d365c1569082cc8e5a54e691cb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 14 Oct 2025 08:10:20 +0000 Subject: [PATCH 4/5] Cleanup parameter validation --- control_toolbox/include/control_toolbox/pid.hpp | 15 +++------------ control_toolbox/src/pid.cpp | 4 ++-- 2 files changed, 5 insertions(+), 14 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 46a8dc58..cf270446 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -291,24 +291,15 @@ class Pid d_gain_(d), u_max_(u_max), u_min_(u_min), + i_max_(antiwindup_strat.i_max), + i_min_(antiwindup_strat.i_min), antiwindup_strat_(antiwindup_strat) { - if (std::isnan(u_min) || std::isnan(u_max)) - { - throw std::invalid_argument("Gains: u_min and u_max must not be NaN"); - } - if (u_min > u_max) - { - std::cout << "Received invalid u_min and u_max values: " << "u_min: " << u_min - << ", u_max: " << u_max << ". Setting saturation to false." << std::endl; - u_max_ = std::numeric_limits::infinity(); - u_min_ = -std::numeric_limits::infinity(); - } } bool validate(std::string & error_msg) const { - if (u_min_ >= u_max_) + if (u_min_ >= u_max_) // is false if any value is nan { error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_); return false; diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 49a8d435..306b9991 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -50,9 +50,9 @@ Pid::Pid( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) { - if (u_min > u_max) + if (u_min >= u_max) { - throw std::invalid_argument("received u_min > u_max"); + throw std::invalid_argument("received u_min >= u_max"); } set_gains(p, i, d, u_max, u_min, antiwindup_strat); From e74ec11ac9324b54196882a34eff612d992d8c3b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 14 Oct 2025 19:53:57 +0000 Subject: [PATCH 5/5] Fix docstring --- control_toolbox/include/control_toolbox/pid.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index cf270446..e78792b5 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -352,7 +352,7 @@ class Pid 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. * - * \throws An std::invalid_argument exception is thrown if u_min > 0 or u_max < 0. + * \throws An std::invalid_argument exception is thrown if u_min >= u_max. */ Pid( double p = 0.0, double i = 0.0, double d = 0.0,