From 9ffcd1c2a7b40259c1740d012bae7ffe5da34c99 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 9 Oct 2025 12:22:09 +0000 Subject: [PATCH 1/3] Remove deprecation of i_min/i_max PID parameters partial backport of #436 --- .../include/control_toolbox/pid.hpp | 32 ++++++++----------- control_toolbox/src/pid.cpp | 2 ++ control_toolbox/src/pid_ros.cpp | 4 +-- .../test/pid_ros_parameters_tests.cpp | 10 +++--- control_toolbox/test/pid_tests.cpp | 8 +++-- 5 files changed, 28 insertions(+), 28 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 9cb565ca..bace4592 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -82,8 +82,8 @@ struct AntiWindupStrategy AntiWindupStrategy() : type(UNDEFINED), - i_min(std::numeric_limits::quiet_NaN()), - i_max(std::numeric_limits::quiet_NaN()), + i_min(-std::numeric_limits::infinity()), + i_max(std::numeric_limits::infinity()), legacy_antiwindup(false), tracking_time_constant(0.0), error_deadband(std::numeric_limits::epsilon()) @@ -135,20 +135,11 @@ struct AntiWindupStrategy "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant " "(tracking_time_constant)"); } - if (type == LEGACY && ((i_min > i_max) || !std::isfinite(i_min) || !std::isfinite(i_max))) + if (i_min >= i_max) { throw std::invalid_argument( fmt::format( - "AntiWindupStrategy 'legacy' requires i_min < i_max and to be finite (i_min: {}, i_max: " - "{})", - i_min, i_max)); - } - if (type != LEGACY && (std::isfinite(i_min) || std::isfinite(i_max))) - { - std::cout << "Warning: The i_min and i_max are only valid for the deprecated LEGACY " - "antiwindup strategy. Please use the AntiWindupStrategy::set_type() method to " - "set the type of antiwindup strategy you want to use." - << std::endl; + "PID requires i_min < i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max)); } if ( type != NONE && type != UNDEFINED && type != LEGACY && type != BACK_CALCULATION && @@ -182,8 +173,8 @@ struct AntiWindupStrategy } Value type = UNDEFINED; - double i_min = std::numeric_limits::quiet_NaN(); /**< Minimum allowable integral term. */ - double i_max = std::numeric_limits::quiet_NaN(); /**< Maximum allowable integral term. */ + double i_min = -std::numeric_limits::infinity(); /**< Minimum allowable integral term. */ + double i_max = std::numeric_limits::infinity(); /**< Maximum allowable integral term. */ bool legacy_antiwindup = false; /**< Use legacy anti-windup strategy. */ @@ -430,8 +421,10 @@ class Pid double p_gain_ = 0.0; /**< Proportional gain. */ double i_gain_ = 0.0; /**< Integral gain. */ double d_gain_ = 0.0; /**< Derivative gain. */ - double i_max_ = 0.0; /**< Maximum allowable integral term. */ - double i_min_ = 0.0; /**< Minimum allowable integral term. */ + double i_max_ = + std::numeric_limits::infinity(); /**< Maximum allowable integral term. */ + double i_min_ = + -std::numeric_limits::infinity(); /**< Minimum allowable integral term. */ double u_max_ = std::numeric_limits::infinity(); /**< Maximum allowable output. */ double u_min_ = -std::numeric_limits::infinity(); /**< Minimum allowable output. */ bool antiwindup_ = false; /**< Anti-windup. */ @@ -456,8 +449,9 @@ class Pid */ [[deprecated("Use constructor with AntiWindupStrategy only.")]] Pid( - double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0, - bool antiwindup = false); + double p = 0.0, double i = 0.0, double d = 0.0, + double i_max = std::numeric_limits::infinity(), + double i_min = -std::numeric_limits::infinity(), bool antiwindup = false); /*! * \brief Constructor, initialize Pid-gains and term limits. diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index edc49287..c8e23f5d 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -431,6 +431,8 @@ 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_); + return cmd_; } diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index e03d4ace..84043469 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -257,8 +257,8 @@ bool PidROS::initialize_from_ros_parameters() double p, i, d, i_max, i_min, u_max, u_min, tracking_time_constant, error_deadband; p = i = d = i_max = i_min = tracking_time_constant = std::numeric_limits::quiet_NaN(); error_deadband = std::numeric_limits::epsilon(); - u_max = UMAX_INFINITY; - u_min = -UMAX_INFINITY; + u_max = i_max = UMAX_INFINITY; + u_min = i_min = -UMAX_INFINITY; bool antiwindup = false; std::string antiwindup_strat_str = "legacy"; bool all_params_available = true; diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 60c86a0c..e8e3ce91 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -178,8 +178,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_, 0.0); - ASSERT_EQ(gains.i_min_, 0.0); + ASSERT_EQ(gains.i_max_, std::numeric_limits::infinity()); + ASSERT_EQ(gains.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); @@ -608,7 +608,7 @@ TEST(PidParametersTest, GetParametersFromParams) const bool ACTIVATE_STATE_PUBLISHER = false; TestablePidROS pid(node, "", "", ACTIVATE_STATE_PUBLISHER); - ASSERT_FALSE(pid.initialize_from_ros_parameters()); + ASSERT_TRUE(pid.initialize_from_ros_parameters()); rclcpp::Parameter param_p; ASSERT_TRUE(node->get_parameter("p", param_p)); @@ -624,11 +624,11 @@ TEST(PidParametersTest, GetParametersFromParams) rclcpp::Parameter param_i_clamp_max; ASSERT_TRUE(node->get_parameter("i_clamp_max", param_i_clamp_max)); - EXPECT_TRUE(std::isnan(param_i_clamp_max.get_value())); + EXPECT_FALSE(std::isfinite(param_i_clamp_max.get_value())); rclcpp::Parameter param_i_clamp_min; ASSERT_TRUE(node->get_parameter("i_clamp_min", param_i_clamp_min)); - EXPECT_TRUE(std::isnan(param_i_clamp_min.get_value())); + EXPECT_FALSE(std::isfinite(param_i_clamp_min.get_value())); rclcpp::Parameter param_u_clamp_max; ASSERT_TRUE(node->get_parameter("u_clamp_max", param_u_clamp_max)); diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 1d09016d..3ccfd06b 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -587,7 +587,9 @@ TEST(CommandTest, proportionalOnlyTest) "only."); // Set only proportional gain - Pid pid(1.0, 0.0, 0.0, 0.0, 0.0); + Pid pid( + 1.0, 0.0, 0.0, std::numeric_limits::infinity(), + -std::numeric_limits::infinity()); double cmd = 0.0; // If initial error = 0, p-gain = 1, dt = 1 @@ -685,7 +687,9 @@ TEST(CommandTest, derivativeOnlyTest) "with own differentiation (ATTENTION: this test depends on the differentiation scheme)."); // Set only derivative gain - Pid pid(0.0, 0.0, 1.0, 0.0, 0.0); + Pid pid( + 0.0, 0.0, 1.0, std::numeric_limits::infinity(), + -std::numeric_limits::infinity()); double cmd = 0.0; // If initial error = 0, d-gain = 1, dt = 1 From afa13f3b87ad9feae7567e3c3ad6e87aa26e769d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 9 Oct 2025 17:47:32 +0000 Subject: [PATCH 2/3] Rename constexpr variable --- control_toolbox/src/pid_ros.cpp | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 84043469..6ef66d93 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -40,7 +40,7 @@ namespace control_toolbox { -constexpr double UMAX_INFINITY = std::numeric_limits::infinity(); +constexpr double MAX_INFINITY = std::numeric_limits::infinity(); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" PidROS::PidROS( @@ -257,8 +257,8 @@ bool PidROS::initialize_from_ros_parameters() double p, i, d, i_max, i_min, u_max, u_min, tracking_time_constant, error_deadband; p = i = d = i_max = i_min = tracking_time_constant = std::numeric_limits::quiet_NaN(); error_deadband = std::numeric_limits::epsilon(); - u_max = i_max = UMAX_INFINITY; - u_min = i_min = -UMAX_INFINITY; + u_max = i_max = MAX_INFINITY; + u_min = i_min = -MAX_INFINITY; bool antiwindup = false; std::string antiwindup_strat_str = "legacy"; bool all_params_available = true; @@ -278,8 +278,8 @@ bool PidROS::initialize_from_ros_parameters() get_boolean_param(param_prefix_ + "saturation", saturation); if (!saturation) { - u_max = UMAX_INFINITY; - u_min = -UMAX_INFINITY; + u_max = MAX_INFINITY; + u_min = -MAX_INFINITY; } get_boolean_param(param_prefix_ + "antiwindup", antiwindup); get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat_str); @@ -342,7 +342,7 @@ bool PidROS::initialize_from_args( antiwindup_strat.i_min = i_min; antiwindup_strat.legacy_antiwindup = antiwindup; - return initialize_from_args(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, false); + return initialize_from_args(p, i, d, MAX_INFINITY, -MAX_INFINITY, antiwindup_strat, false); } #pragma GCC diagnostic pop @@ -355,8 +355,7 @@ bool PidROS::initialize_from_args( antiwindup_strat.i_min = i_min; antiwindup_strat.legacy_antiwindup = antiwindup; - return initialize_from_args( - p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, save_i_term); + return initialize_from_args(p, i, d, MAX_INFINITY, -MAX_INFINITY, antiwindup_strat, save_i_term); } bool PidROS::initialize_from_args( @@ -454,7 +453,7 @@ bool PidROS::set_gains(double p, double i, double d, double i_max, double i_min, antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.legacy_antiwindup = antiwindup; - return set_gains(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat); + return set_gains(p, i, d, MAX_INFINITY, -MAX_INFINITY, antiwindup_strat); } bool PidROS::set_gains( @@ -610,8 +609,8 @@ void PidROS::set_parameter_event_callback() RCLCPP_WARN( node_logging_->get_logger(), "Saturation is set to false, Changing the u_min and u_max to -inf and inf"); - gains.u_min_ = -UMAX_INFINITY; - gains.u_max_ = UMAX_INFINITY; + gains.u_min_ = -MAX_INFINITY; + gains.u_max_ = MAX_INFINITY; } else { @@ -659,7 +658,7 @@ void PidROS::set_parameter_event_callback() } else if (param_name == param_prefix_ + "u_clamp_max") { - gains.u_max_ = saturation ? parameter.get_value() : UMAX_INFINITY; + gains.u_max_ = saturation ? parameter.get_value() : MAX_INFINITY; RCLCPP_WARN_EXPRESSION( node_logging_->get_logger(), !saturation, "Saturation is set to false, Changing the u_clamp_max inf"); @@ -667,7 +666,7 @@ void PidROS::set_parameter_event_callback() } else if (param_name == param_prefix_ + "u_clamp_min") { - gains.u_min_ = saturation ? parameter.get_value() : -UMAX_INFINITY; + gains.u_min_ = saturation ? parameter.get_value() : -MAX_INFINITY; RCLCPP_WARN_EXPRESSION( node_logging_->get_logger(), !saturation, "Saturation is set to false, Changing the u_clamp_min -inf"); From dfac1d4679a88fcb5f621cefd9d42a168e42844e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 10 Oct 2025 07:49:55 +0000 Subject: [PATCH 3/3] Fix legacy clamping limit -0/0 --- control_toolbox/include/control_toolbox/pid.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index bace4592..7d151e69 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -135,11 +135,11 @@ struct AntiWindupStrategy "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant " "(tracking_time_constant)"); } - if (i_min >= i_max) + if (i_min > i_max) { throw std::invalid_argument( fmt::format( - "PID requires i_min < i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max)); + "PID requires i_min <= i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max)); } if ( type != NONE && type != UNDEFINED && type != LEGACY && type != BACK_CALCULATION &&