From 4045cd07e7b95ec2d368ef4e9de7dd999abd3aa3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 9 Jun 2025 18:16:55 +0200 Subject: [PATCH 01/44] Add UNDEFINED, INTEGRATOR_CLAMPING types --- .../include/control_toolbox/pid.hpp | 36 +++++++++++++----- .../include/control_toolbox/pid_ros.hpp | 10 ++--- control_toolbox/src/pid.cpp | 13 ++++--- control_toolbox/src/pid_ros.cpp | 14 +++---- .../test/pid_ros_parameters_tests.cpp | 38 ++++++++++--------- .../test/pid_ros_publisher_tests.cpp | 6 ++- control_toolbox/test/pid_tests.cpp | 13 ++++--- 7 files changed, 78 insertions(+), 52 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 6e9f61ba..56b8f079 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -49,12 +49,14 @@ class AntiwindupStrategy public: enum Value : int8_t { - NONE = 0, + UNDEFINED = -1, + NONE, + INTEGRATOR_CLAMPING, BACK_CALCULATION, CONDITIONAL_INTEGRATION }; - constexpr AntiwindupStrategy() : value_(NONE) {} + constexpr AntiwindupStrategy() : value_(INTEGRATOR_CLAMPING) {} constexpr AntiwindupStrategy(Value v) : value_(v) {} // NOLINT(runtime/explicit) explicit AntiwindupStrategy(const std::string & s) @@ -67,10 +69,19 @@ class AntiwindupStrategy { value_ = CONDITIONAL_INTEGRATION; } - else + else if (s == "integrator_clamping") + { + value_ = INTEGRATOR_CLAMPING; + } + else if (s == "none") { value_ = NONE; } + else + { + value_ = UNDEFINED; + std::cerr << "Unknown antiwindup strategy: '" << s << "'. Using UNDEFINED." << std::endl; + } } operator std::string() const { return to_string(); } @@ -89,14 +100,18 @@ class AntiwindupStrategy return "back_calculation"; case CONDITIONAL_INTEGRATION: return "conditional_integration"; + case INTEGRATOR_CLAMPING: + return "integrator_clamping"; case NONE: - default: return "none"; + case UNDEFINED: + default: + return "UNDEFINED"; } } private: - Value value_; + Value value_ = UNDEFINED; }; template @@ -198,7 +213,8 @@ class Pid Gains(double p, double i, double d, double i_max, double i_min) : Gains( p, i, d, i_max, i_min, std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), 0.0, true, AntiwindupStrategy::NONE) + -std::numeric_limits::infinity(), 0.0, true, + AntiwindupStrategy::INTEGRATOR_CLAMPING) { } @@ -220,7 +236,8 @@ class Pid Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) : Gains( p, i, d, i_max, i_min, std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), 0.0, antiwindup, AntiwindupStrategy::NONE) + -std::numeric_limits::infinity(), 0.0, antiwindup, + AntiwindupStrategy::INTEGRATOR_CLAMPING) { } @@ -321,7 +338,8 @@ class Pid double u_min_ = -std::numeric_limits::infinity(); /**< Minimum allowable output. */ double trk_tc_ = 0.0; /**< Tracking time constant. */ bool antiwindup_ = false; /**< Anti-windup. */ - AntiwindupStrategy antiwindup_strat_ = AntiwindupStrategy::NONE; /**< Anti-windup strategy. */ + AntiwindupStrategy antiwindup_strat_ = + AntiwindupStrategy::INTEGRATOR_CLAMPING; /**< Anti-windup strategy. */ }; /*! @@ -606,7 +624,7 @@ class Pid void set_gains( double p, double i, double d, double i_max, double i_min, double u_max, double u_min, double trk_tc = 0.0, bool antiwindup = false, - AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE); + AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::INTEGRATOR_CLAMPING); /*! * \brief Set PID gains for the controller. diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index a892f0a7..cf1258c2 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -141,10 +141,10 @@ class PidROS 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other than 'none' is selected, it will override the controller's default anti-windup behavior. - * \deprecated{only when `antiwindup_strat == AntiwindupStrategy::NONE`:} + * \deprecated{only when `antiwindup_strat == AntiwindupStrategy::INTEGRATOR_CLAMPING`:} * Old anti-windup technique is deprecated and will be removed by * the ROS 2 Kilted Kaiju release. - * \warning{If you pass `AntiwindupStrategy::NONE`, at runtime a warning will be printed:} + * \warning{If you pass `AntiwindupStrategy::INTEGRATOR_CLAMPING`, at runtime a warning will be printed:} * `"Old anti-windup technique is deprecated. This option will be removed by the ROS 2 Kilted Kaiju release."` * \param save_i_term save integrator output between resets. * @@ -263,10 +263,10 @@ class PidROS 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other than 'none' is selected, it will override the controller's default anti-windup behavior. - * \deprecated{only when `antiwindup_strat == AntiwindupStrategy::NONE`:} + * \deprecated{only when `antiwindup_strat == AntiwindupStrategy::INTEGRATOR_CLAMPING`:} * Old anti-windup technique is deprecated and will be removed by * the ROS 2 Kilted Kaiju release. - * \warning{If you pass `AntiwindupStrategy::NONE`, at runtime a warning will be printed:} + * \warning{If you pass `AntiwindupStrategy::INTEGRATOR_CLAMPING`, at runtime a warning will be printed:} * `"Old anti-windup technique is deprecated. This option will be removed by * the ROS 2 Kilted Kaiju release."` * @@ -276,7 +276,7 @@ class PidROS void set_gains( double p, double i, double d, double i_max, double i_min, double u_max, double u_min, double trk_tc = 0.0, bool antiwindup = false, - AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE); + AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::INTEGRATOR_CLAMPING); /*! * \brief Set PID gains for the controller (preferred). diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index dc0246ad..d0b047f1 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -51,7 +51,8 @@ constexpr double UMAX_INFINITY = std::numeric_limits::infinity(); #pragma GCC diagnostic ignored "-Wdeprecated-declarations" Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) : Pid( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, AntiwindupStrategy::NONE) + p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, + AntiwindupStrategy::INTEGRATOR_CLAMPING) { } #pragma GCC diagnostic pop @@ -117,7 +118,7 @@ void Pid::initialize(double p, double i, double d, double i_max, double i_min, b #pragma GCC diagnostic ignored "-Wdeprecated-declarations" initialize( p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, - AntiwindupStrategy::NONE); + AntiwindupStrategy::INTEGRATOR_CLAMPING); #pragma GCC diagnostic pop reset(); @@ -229,7 +230,7 @@ void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bo { set_gains( p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, - AntiwindupStrategy::NONE); + AntiwindupStrategy::INTEGRATOR_CLAMPING); } #pragma GCC diagnostic pop @@ -384,18 +385,18 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) d_term = gains.d_gain_ * d_error_; // Calculate integral contribution to command - if (gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) + if (gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::INTEGRATOR_CLAMPING) { // 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 if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) + else if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::INTEGRATOR_CLAMPING) { i_term_ += gains.i_gain_ * dt_s * p_error_; } // Compute the command - if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) + if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::INTEGRATOR_CLAMPING) { // Limit i_term so that the limit is meaningful in the output cmd_unsat_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index de49661c..1f37fd1b 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -272,8 +272,8 @@ void PidROS::initialize_from_args( #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" initialize_from_args( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, AntiwindupStrategy::NONE, - false); + p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, + AntiwindupStrategy::INTEGRATOR_CLAMPING, false); #pragma GCC diagnostic pop } @@ -283,8 +283,8 @@ void PidROS::initialize_from_args( #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" initialize_from_args( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, AntiwindupStrategy::NONE, - save_i_term); + p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, + AntiwindupStrategy::INTEGRATOR_CLAMPING, save_i_term); #pragma GCC diagnostic pop } @@ -302,7 +302,7 @@ void PidROS::initialize_from_args( } else { - if (antiwindup_strat == AntiwindupStrategy::NONE) + if (antiwindup_strat == AntiwindupStrategy::INTEGRATOR_CLAMPING) { std::cout << "Old anti-windup technique is deprecated. " "This option will be removed by the ROS 2 Kilted Kaiju release." @@ -399,7 +399,7 @@ void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, #pragma GCC diagnostic ignored "-Wdeprecated-declarations" set_gains( p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, - AntiwindupStrategy::NONE); + AntiwindupStrategy::INTEGRATOR_CLAMPING); #pragma GCC diagnostic pop } @@ -417,7 +417,7 @@ void PidROS::set_gains( } else { - if (antiwindup_strat == AntiwindupStrategy::NONE) + if (antiwindup_strat == AntiwindupStrategy::INTEGRATOR_CLAMPING) { std::cout << "Old anti-windup technique is deprecated. " "This option will be removed by the ROS 2 Kilted Kaiju release." diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 2ab043be..4b9f9a0f 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -63,7 +63,7 @@ void check_set_parameters( const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::INTEGRATOR_CLAMPING; const bool SAVE_I_TERM = true; ASSERT_NO_THROW(pid.initialize_from_args( @@ -119,7 +119,7 @@ void check_set_parameters( ASSERT_EQ(gains.u_min_, U_MIN); ASSERT_EQ(gains.trk_tc_, TRK_TC); ASSERT_TRUE(gains.antiwindup_); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); } TEST(PidParametersTest, InitPidTest) @@ -150,8 +150,8 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double TRK_TC = 4.0; ASSERT_NO_THROW(pid.initialize_from_args( - P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD, TRK_TC, false, AntiwindupStrategy::NONE, - false)); + P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD, TRK_TC, false, + AntiwindupStrategy::INTEGRATOR_CLAMPING, false)); rclcpp::Parameter param; @@ -179,7 +179,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.trk_tc_, 0.0); ASSERT_FALSE(gains.antiwindup_); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); } TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot) @@ -264,7 +264,7 @@ TEST(PidParametersTest, SetParametersTest) const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::INTEGRATOR_CLAMPING; const bool SAVE_I_TERM = false; pid.initialize_from_args( @@ -318,7 +318,7 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.u_min_, U_MIN); ASSERT_EQ(gains.trk_tc_, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); } TEST(PidParametersTest, SetBadParametersTest) @@ -341,7 +341,7 @@ TEST(PidParametersTest, SetBadParametersTest) const double TRK_TC = 4.0; const bool SATURATION = false; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::INTEGRATOR_CLAMPING; pid.initialize_from_args( P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, ANTIWINDUP, ANTIWINDUP_STRAT, false); @@ -393,7 +393,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.trk_tc_, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); // Set the good gains @@ -416,7 +416,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.trk_tc_, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); // Now re-enabling it should have the old gains back ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", true))); @@ -436,7 +436,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(updated_gains.u_min_, U_MIN); ASSERT_EQ(updated_gains.trk_tc_, TRK_TC); ASSERT_EQ(updated_gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(updated_gains.antiwindup_strat_, AntiwindupStrategy::NONE); + ASSERT_EQ(updated_gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); } TEST(PidParametersTest, GetParametersTest) @@ -456,10 +456,11 @@ TEST(PidParametersTest, GetParametersTest) const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::INTEGRATOR_CLAMPING; pid.initialize_from_args( - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, AntiwindupStrategy::NONE, false); + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING, + false); std::cout << "Setting gains with set_gains()" << std::endl; pid.set_gains(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, ANTIWINDUP, ANTIWINDUP_STRAT); @@ -515,10 +516,11 @@ TEST(PidParametersTest, GetParametersTest) const double U_MIN = -std::numeric_limits::infinity(); const double TRK_TC = 4.0; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::INTEGRATOR_CLAMPING; pid.initialize_from_args( - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, AntiwindupStrategy::NONE, false); + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING, + false); pid.set_gains(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, ANTIWINDUP, ANTIWINDUP_STRAT); rclcpp::Parameter param; @@ -619,9 +621,11 @@ TEST(PidParametersTest, MultiplePidInstances) const double TRK_TC = 4.0; ASSERT_NO_THROW(pid_1.initialize_from_args( - P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false, AntiwindupStrategy::NONE, false)); + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false, AntiwindupStrategy::INTEGRATOR_CLAMPING, + false)); ASSERT_NO_THROW(pid_2.initialize_from_args( - P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true, AntiwindupStrategy::NONE, false)); + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true, AntiwindupStrategy::INTEGRATOR_CLAMPING, + false)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index 9d776800..49e8cf40 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -44,7 +44,8 @@ TEST(PidPublisherTest, PublishTest) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); pid_ros.initialize_from_args( - 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, AntiwindupStrategy::NONE, false); + 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING, + false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -82,7 +83,8 @@ TEST(PidPublisherTest, PublishTestLifecycle) pid_ros.get_pid_state_publisher()); pid_ros.initialize_from_args( - 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, AntiwindupStrategy::NONE, false); + 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING, + false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 128cf97c..57b26990 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -54,7 +54,8 @@ TEST(ParameterTest, UTermBadIBoundsTestConstructor) // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, // double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); EXPECT_THROW( - Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, AntiwindupStrategy::NONE), + Pid pid( + 1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING), std::invalid_argument); } @@ -66,13 +67,13 @@ TEST(ParameterTest, UTermBadIBoundsTest) // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, // double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); - Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 0.0, false, AntiwindupStrategy::NONE); + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 0.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING); auto gains = pid.get_gains(); EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); // Try to set bad u-bounds, i.e. u_min > u_max - EXPECT_NO_THROW( - pid.set_gains(1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, AntiwindupStrategy::NONE)); + EXPECT_NO_THROW(pid.set_gains( + 1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING)); // Check if gains were not updated because u-bounds are bad, i.e. u_min > u_max EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); @@ -428,7 +429,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double u_min = -1 * u_max; double trk_tc = std::rand() % 100; bool antiwindup = false; - AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE; + AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::INTEGRATOR_CLAMPING; // Initialize the default way Pid pid1( @@ -467,7 +468,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) u_min = -1 * u_max; trk_tc = std::rand() % 100; antiwindup = false; - antiwindup_strat = AntiwindupStrategy::NONE; + antiwindup_strat = AntiwindupStrategy::INTEGRATOR_CLAMPING; pid1.set_gains( p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); From ce86e2c36ecdaa4af470b4831e52ee4a24f0ef4e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 9 Jun 2025 20:28:21 +0200 Subject: [PATCH 02/44] Modify AntiwindupStrategy to use as options at every level --- .../include/control_toolbox/pid.hpp | 250 +++++++----------- control_toolbox/src/pid.cpp | 78 ++---- 2 files changed, 105 insertions(+), 223 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 56b8f079..885bb7af 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -44,57 +44,81 @@ namespace control_toolbox { -class AntiwindupStrategy +struct AntiwindupStrategy { public: enum Value : int8_t { UNDEFINED = -1, NONE, + LEGACY, INTEGRATOR_CLAMPING, BACK_CALCULATION, CONDITIONAL_INTEGRATION }; - constexpr AntiwindupStrategy() : value_(INTEGRATOR_CLAMPING) {} - constexpr AntiwindupStrategy(Value v) : value_(v) {} // NOLINT(runtime/explicit) + constexpr AntiwindupStrategy() : type(LEGACY) {} + constexpr AntiwindupStrategy(Value v) : type(v) {} // NOLINT(runtime/explicit) - explicit AntiwindupStrategy(const std::string & s) + void set_type(const std::string & s) { if (s == "back_calculation") { - value_ = BACK_CALCULATION; + type = BACK_CALCULATION; } else if (s == "conditional_integration") { - value_ = CONDITIONAL_INTEGRATION; + type = CONDITIONAL_INTEGRATION; } else if (s == "integrator_clamping") { - value_ = INTEGRATOR_CLAMPING; + type = INTEGRATOR_CLAMPING; + } + else if (s == "legacy") + { + type = LEGACY; } else if (s == "none") { - value_ = NONE; + type = NONE; } else { - value_ = UNDEFINED; + type = UNDEFINED; std::cerr << "Unknown antiwindup strategy: '" << s << "'. Using UNDEFINED." << std::endl; } } - operator std::string() const { return to_string(); } + void validate() const + { + if (type == UNDEFINED) + { + throw std::runtime_error("AntiwindupStrategy is UNDEFINED. Please set a valid type."); + } + if (type == BACK_CALCULATION && (trk_tc < 0.0 || !std::isfinite(trk_tc))) + { + throw std::runtime_error( + "AntiwindupStrategy 'back_calculation' requires a valid positive tracking time constant " + "(trk_tc)."); + } + if (type == INTEGRATOR_CLAMPING && (i_min >= i_max)) + { + throw std::runtime_error("AntiwindupStrategy 'integrator_clamping' requires i_min < i_max."); + } + if (type == LEGACY && (i_min >= i_max)) + { + throw std::runtime_error("AntiwindupStrategy 'legacy' requires i_min < i_max."); + } + } - constexpr bool operator==(AntiwindupStrategy other) const { return value_ == other.value_; } - constexpr bool operator!=(AntiwindupStrategy other) const { return value_ != other.value_; } + operator std::string() const { return to_string(); } - constexpr bool operator==(Value other) const { return value_ == other; } - constexpr bool operator!=(Value other) const { return value_ != other; } + constexpr bool operator==(Value other) const { return type == other; } + constexpr bool operator!=(Value other) const { return type != other; } std::string to_string() const { - switch (value_) + switch (type) { case BACK_CALCULATION: return "back_calculation"; @@ -102,6 +126,8 @@ class AntiwindupStrategy return "conditional_integration"; case INTEGRATOR_CLAMPING: return "integrator_clamping"; + case LEGACY: + return "legacy"; case NONE: return "none"; case UNDEFINED: @@ -110,8 +136,15 @@ class AntiwindupStrategy } } -private: - Value value_ = UNDEFINED; + Value type = UNDEFINED; + double i_min = 0.0; /**< Minimum allowable integral term. */ + double i_max = 0.0; /**< Maximum allowable integral term. */ + + bool legacy_antiwindup = false; /**< Use legacy anti-windup strategy. */ + + // trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. + // If set to 0.0 when this strategy is selected, a recommended default value will be applied. + double trk_tc = 0.0; /**< Tracking time constant for back_calculation strategy. */ }; template @@ -211,11 +244,20 @@ class Pid */ [[deprecated("Use constructor with AntiwindupStrategy instead.")]] Gains(double p, double i, double d, double i_max, double i_min) - : Gains( - p, i, d, i_max, i_min, std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), 0.0, true, - AntiwindupStrategy::INTEGRATOR_CLAMPING) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(std::numeric_limits::infinity()), + u_min_(-std::numeric_limits::infinity()), + saturation_(false), + antiwindup_(false) { + antiwindup_strat_.type = AntiwindupStrategy::LEGACY; + antiwindup_strat_.i_max = i_max; + antiwindup_strat_.i_min = i_min; + antiwindup_strat_.validate(); } /*! @@ -234,47 +276,44 @@ class Pid */ [[deprecated("Use constructor with AntiwindupStrategy instead.")]] Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) - : Gains( - p, i, d, i_max, i_min, std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), 0.0, antiwindup, - AntiwindupStrategy::INTEGRATOR_CLAMPING) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(std::numeric_limits::infinity()), + u_min_(-std::numeric_limits::infinity()), + saturation_(false), + antiwindup_(antiwindup) { + antiwindup_strat_.type = AntiwindupStrategy::LEGACY; + antiwindup_strat_.i_max = i_max; + antiwindup_strat_.i_min = i_min; + antiwindup_strat_.validate(); } /*! - * \brief Optional constructor for passing in values + * \brief Constructor for passing in values. * * \param p The proportional gain. * \param i The integral gain. * \param d The derivative gain. - * \param i_max Upper integral clamp. - * \param i_min Lower integral clamp. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. + tracking_time_constant parameter to tune the anti-windup behavior. * */ - [[deprecated("Use constructor with AntiwindupStrategy only.")]] Gains( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat) + double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat) : p_gain_(p), i_gain_(i), d_gain_(d), - i_max_(i_max), - i_min_(i_min), + i_max_(antiwindup_strat.i_max), + i_min_(antiwindup_strat.i_min), u_max_(u_max), u_min_(u_min), - trk_tc_(trk_tc), antiwindup_(antiwindup), antiwindup_strat_(antiwindup_strat) { @@ -291,28 +330,6 @@ class Pid } } - /*! - * \brief Constructor for passing in values. - * - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. - * \param u_max Upper output clamp. - * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. - * - */ - Gains( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat) - : Gains(p, i, d, 0.0, 0.0, u_max, u_min, trk_tc, false, antiwindup_strat) - { - } - // Default constructor [[deprecated( "Use constructor with AntiwindupStrategy only. The default constructor might be deleted in " @@ -332,14 +349,16 @@ 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. */ + [[deprecated("Use antiwindup_strat_.i_max instead.")]] + double i_max_ = 0.0; /**< Maximum allowable integral term. */ + [[deprecated("Use antiwindup_strat_.i_min instead.")]] + double i_min_ = 0.0; /**< Minimum allowable integral term. */ double u_max_ = std::numeric_limits::infinity(); /**< Maximum allowable output. */ double u_min_ = -std::numeric_limits::infinity(); /**< Minimum allowable output. */ double trk_tc_ = 0.0; /**< Tracking time constant. */ bool antiwindup_ = false; /**< Anti-windup. */ AntiwindupStrategy antiwindup_strat_ = - AntiwindupStrategy::INTEGRATOR_CLAMPING; /**< Anti-windup strategy. */ + AntiwindupStrategy::UNDEFINED; /**< Anti-windup strategy. */ }; /*! @@ -363,34 +382,6 @@ class 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); - /*! - * \brief Constructor, initialize Pid-gains and term limits. - * - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. - * \param i_max Upper integral clamp. - * \param i_min Lower integral clamp. - * \param u_max Upper output clamp. - * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. - * - * \throws An std::invalid_argument exception is thrown if i_min > i_max or u_min > u_max - */ - [[deprecated("Use constructor with AntiwindupStrategy only.")]] - Pid( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); - /*! * \brief Constructor, initialize Pid-gains and term limits. * @@ -399,8 +390,6 @@ class Pid * \param d The derivative gain. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. @@ -408,8 +397,7 @@ class Pid * \throws An std::invalid_argument exception is thrown if u_min > u_max */ Pid( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat); + double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); /*! * \brief Copy constructor required for preventing mutexes from being copied @@ -441,34 +429,6 @@ class Pid void initialize( double p, double i, double d, double i_max, double i_min, bool antiwindup = false); - /*! - * \brief Initialize Pid-gains and term limits - * - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. - * \param i_max Upper integral clamp. - * \param i_min Lower integral clamp. - * \param u_max Upper output clamp. - * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. - * - * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max - */ - [[deprecated("Use initialize with AntiwindupStrategy only.")]] - void initialize( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); - /*! * \brief Initialize Pid-gains and term limits. * @@ -486,8 +446,7 @@ class Pid * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max */ void initialize( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat); + double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); /*! * \brief Reset the state of this PID controller @@ -598,34 +557,6 @@ class Pid [[deprecated("Use set_gains with AntiwindupStrategy instead.")]] void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); - /*! - * \brief Set PID gains for the controller. - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. - * \param i_max Upper integral clamp. - * \param i_min Lower integral clamp. - * \param u_max Upper output clamp. - * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. - * - * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max - */ - [[deprecated("Use set_gains with AntiwindupStrategy only.")]] - void set_gains( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc = 0.0, bool antiwindup = false, - AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::INTEGRATOR_CLAMPING); - /*! * \brief Set PID gains for the controller. * @@ -634,8 +565,6 @@ class Pid * \param d The derivative gain. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. @@ -643,8 +572,7 @@ class Pid * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max */ void set_gains( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat); + double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); /*! * \brief Set PID gains for the controller. diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index d0b047f1..9fd5a730 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -50,47 +50,31 @@ constexpr double UMAX_INFINITY = std::numeric_limits::infinity(); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) -: Pid( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, - AntiwindupStrategy::INTEGRATOR_CLAMPING) -{ -} -#pragma GCC diagnostic pop - -Pid::Pid( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat) : gains_buffer_() { - if (i_min > i_max) - { - throw std::invalid_argument("received i_min > i_max"); - } - else if (u_min > u_max) - { - throw std::invalid_argument("received u_min > u_max"); - } -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::LEGACY; + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.validate(); + set_gains(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat); // Initialize saved i-term values clear_saved_iterm(); reset(); } +#pragma GCC diagnostic pop Pid::Pid( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat) + double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat) : gains_buffer_() { if (u_min > u_max) { throw std::invalid_argument("received u_min > u_max"); } - set_gains(p, i, d, u_max, u_min, trk_tc, antiwindup_strat); + set_gains(p, i, d, u_max, u_min, antiwindup_strat); // Initialize saved i-term values clear_saved_iterm(); @@ -116,31 +100,16 @@ void Pid::initialize(double p, double i, double d, double i_max, double i_min, b { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - initialize( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, - AntiwindupStrategy::INTEGRATOR_CLAMPING); -#pragma GCC diagnostic pop - - reset(); -} - -void Pid::initialize( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat) -{ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); + set_gains(p, i, d, i_max, i_min, antiwindup); #pragma GCC diagnostic pop reset(); } void Pid::initialize( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat) + double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat) { - set_gains(p, i, d, u_max, u_min, trk_tc, antiwindup_strat); + set_gains(p, i, d, u_max, u_min, antiwindup_strat); reset(); } @@ -228,35 +197,20 @@ Pid::Gains Pid::get_gains() { return *gains_buffer_.readFromRT(); } #pragma GCC diagnostic ignored "-Wdeprecated-declarations" void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - set_gains( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, - AntiwindupStrategy::INTEGRATOR_CLAMPING); -} -#pragma GCC diagnostic pop - #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" -void Pid::set_gains( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat) -{ - Gains gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); - + Gains gains(p, i, d, i_max, i_min, antiwindup); +#pragma GCC diagnostic pop set_gains(gains); } #pragma GCC diagnostic pop void Pid::set_gains( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat) + double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat) { - double i_min = 0.0; - double i_max = 0.0; - bool antiwindup = false; - #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - Gains gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); + Gains gains(p, i, d, u_max, u_min, antiwindup_strat); #pragma GCC diagnostic pop set_gains(gains); From 29f0b14fd898ca2091cba953107ae156db742fa4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 10 Jun 2025 23:17:14 +0200 Subject: [PATCH 03/44] first compilable version --- control_toolbox/CMakeLists.txt | 8 +- .../include/control_toolbox/pid.hpp | 7 +- .../include/control_toolbox/pid_ros.hpp | 81 +------ control_toolbox/src/pid.cpp | 15 +- control_toolbox/src/pid_ros.cpp | 209 +++++++++--------- control_toolbox/test/pid_tests.cpp | 144 +++++++----- 6 files changed, 205 insertions(+), 259 deletions(-) diff --git a/control_toolbox/CMakeLists.txt b/control_toolbox/CMakeLists.txt index 4a79bdec..d35e6fce 100644 --- a/control_toolbox/CMakeLists.txt +++ b/control_toolbox/CMakeLists.txt @@ -176,11 +176,11 @@ if(BUILD_TESTING) ament_add_gmock(rate_limiter_tests test/rate_limiter.cpp) target_link_libraries(rate_limiter_tests control_toolbox) - ament_add_gmock(pid_ros_parameters_tests test/pid_ros_parameters_tests.cpp) - target_link_libraries(pid_ros_parameters_tests control_toolbox) + # ament_add_gmock(pid_ros_parameters_tests test/pid_ros_parameters_tests.cpp) + # target_link_libraries(pid_ros_parameters_tests control_toolbox) - ament_add_gmock(pid_ros_publisher_tests test/pid_ros_publisher_tests.cpp) - target_link_libraries(pid_ros_publisher_tests control_toolbox rclcpp_lifecycle::rclcpp_lifecycle) + # ament_add_gmock(pid_ros_publisher_tests test/pid_ros_publisher_tests.cpp) + # target_link_libraries(pid_ros_publisher_tests control_toolbox rclcpp_lifecycle::rclcpp_lifecycle) ## Control Filters ### Gravity Compensation diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 885bb7af..9c4bbd47 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -251,7 +251,6 @@ class Pid i_min_(i_min), u_max_(std::numeric_limits::infinity()), u_min_(-std::numeric_limits::infinity()), - saturation_(false), antiwindup_(false) { antiwindup_strat_.type = AntiwindupStrategy::LEGACY; @@ -283,7 +282,6 @@ class Pid i_min_(i_min), u_max_(std::numeric_limits::infinity()), u_min_(-std::numeric_limits::infinity()), - saturation_(false), antiwindup_(antiwindup) { antiwindup_strat_.type = AntiwindupStrategy::LEGACY; @@ -314,7 +312,6 @@ class Pid i_min_(antiwindup_strat.i_min), u_max_(u_max), u_min_(u_min), - antiwindup_(antiwindup), antiwindup_strat_(antiwindup_strat) { if (std::isnan(u_min) || std::isnan(u_max)) @@ -524,14 +521,12 @@ class Pid * \param d The derivative gain. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. */ void get_gains( - double & p, double & i, double & d, double & u_max, double & u_min, double & trk_tc, + double & p, double & i, double & d, double & u_max, double & u_min, AntiwindupStrategy & antiwindup_strat); /*! diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index cf1258c2..b0dd3fcf 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -122,39 +122,6 @@ class PidROS void initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term); - /*! - * \brief Initialize the PID controller and set the parameters - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. - * \param i_max The max integral windup. - * \param i_min The min integral windup. - * \param u_max Upper output clamp. - * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. - * \deprecated{only when `antiwindup_strat == AntiwindupStrategy::INTEGRATOR_CLAMPING`:} - * Old anti-windup technique is deprecated and will be removed by - * the ROS 2 Kilted Kaiju release. - * \warning{If you pass `AntiwindupStrategy::INTEGRATOR_CLAMPING`, at runtime a warning will be printed:} - * `"Old anti-windup technique is deprecated. This option will be removed by the ROS 2 Kilted Kaiju release."` - * \param save_i_term save integrator output between resets. - * - * \note New gains are not applied if i_min_ > i_max_ or if u_min_ > u_max_. - */ - [[deprecated("Use initialize_from_args with AntiwindupStrategy only.")]] - void initialize_from_args( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat, bool save_i_term); - /*! * \brief Initialize the PID controller and set the parameters. * @@ -163,18 +130,17 @@ class PidROS * \param d The derivative gain. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. * \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 u_min_ > u_max_. */ - void initialize_from_args( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat, bool save_i_term); + bool initialize_from_args( + double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat, + bool save_i_term); /*! * \brief Initialize the PID controller based on already set parameters @@ -244,40 +210,6 @@ class PidROS [[deprecated("Use set_gains with AntiwindupStrategy instead.")]] void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); - /*! - * \brief Initialize the PID controller and set the parameters - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. - * \param i_max The max integral windup. - * \param i_min The min integral windup. - * \param u_max Upper output clamp. - * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. - * \deprecated{only when `antiwindup_strat == AntiwindupStrategy::INTEGRATOR_CLAMPING`:} - * Old anti-windup technique is deprecated and will be removed by - * the ROS 2 Kilted Kaiju release. - * \warning{If you pass `AntiwindupStrategy::INTEGRATOR_CLAMPING`, at runtime a warning will be printed:} - * `"Old anti-windup technique is deprecated. This option will be removed by - * the ROS 2 Kilted Kaiju release."` - * - * \note New gains are not applied if i_min > i_max or if u_min_ > u_max_. - */ - [[deprecated("Use set_gains with AntiwindupStrategy only.")]] - void set_gains( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc = 0.0, bool antiwindup = false, - AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::INTEGRATOR_CLAMPING); - /*! * \brief Set PID gains for the controller (preferred). * @@ -286,8 +218,6 @@ class PidROS * \param d The derivative gain. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. @@ -295,8 +225,7 @@ class PidROS * \note New gains are not applied if u_min_ > u_max_. */ void set_gains( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat); + double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); /*! * \brief Set PID gains for the controller. diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 9fd5a730..acdedef4 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -177,7 +177,7 @@ void Pid::get_gains( } void Pid::get_gains( - double & p, double & i, double & d, double & u_max, double & u_min, double & trk_tc, + double & p, double & i, double & d, double & u_max, double & u_min, AntiwindupStrategy & antiwindup_strat) { Gains gains = *gains_buffer_.readFromRT(); @@ -187,7 +187,6 @@ void Pid::get_gains( d = gains.d_gain_; u_max = gains.u_max_; u_min = gains.u_min_; - trk_tc = gains.trk_tc_; antiwindup_strat = gains.antiwindup_strat_; } @@ -339,18 +338,24 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) d_term = gains.d_gain_ * d_error_; // Calculate integral contribution to command - if (gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::INTEGRATOR_CLAMPING) + if ( + gains.antiwindup_strat_.legacy_antiwindup && + gains.antiwindup_strat_ == AntiwindupStrategy::LEGACY) { // 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 if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::INTEGRATOR_CLAMPING) + else if ( + !gains.antiwindup_strat_.legacy_antiwindup && + gains.antiwindup_strat_ == AntiwindupStrategy::LEGACY) { i_term_ += gains.i_gain_ * dt_s * p_error_; } // Compute the command - if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::INTEGRATOR_CLAMPING) + if ( + !gains.antiwindup_strat_.legacy_antiwindup && + gains.antiwindup_strat_ == AntiwindupStrategy::LEGACY) { // Limit i_term so that the limit is meaningful in the output cmd_unsat_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 1f37fd1b..4f279f02 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -248,12 +248,24 @@ bool PidROS::initialize_from_ros_parameters() << std::endl; } - AntiwindupStrategy antiwindup_strat(antiwindup_strat_str); + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.set_type(antiwindup_strat_str); + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.trk_tc = trk_tc; + antiwindup_strat.legacy_antiwindup = antiwindup; + + try + { + antiwindup_strat.validate(); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); + return false; + } -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - pid_.initialize(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop + pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat); return all_params_available; } @@ -269,82 +281,80 @@ void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValu void PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup) { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - initialize_from_args( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, - AntiwindupStrategy::INTEGRATOR_CLAMPING, false); -#pragma GCC diagnostic pop -} + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::LEGACY; + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.legacy_antiwindup = antiwindup; + try + { + antiwindup_strat.validate(); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); + return; + } -void PidROS::initialize_from_args( - double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term) -{ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - initialize_from_args( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, - AntiwindupStrategy::INTEGRATOR_CLAMPING, save_i_term); + initialize_from_args(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, false); #pragma GCC diagnostic pop } void PidROS::initialize_from_args( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat, bool save_i_term) + double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term) { - if (i_min > i_max) + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::LEGACY; + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.legacy_antiwindup = antiwindup; + try { - RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); + antiwindup_strat.validate(); } - else if (u_min > u_max) + catch (const std::runtime_error & e) { - RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); + return; } - else - { - if (antiwindup_strat == AntiwindupStrategy::INTEGRATOR_CLAMPING) - { - std::cout << "Old anti-windup technique is deprecated. " - "This option will be removed by the ROS 2 Kilted Kaiju release." - << std::endl; - } - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - pid_.initialize(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop - const Pid::Gains gains = pid_.get_gains(); - declare_param(param_prefix_ + "p", rclcpp::ParameterValue(gains.p_gain_)); - declare_param(param_prefix_ + "i", rclcpp::ParameterValue(gains.i_gain_)); - declare_param(param_prefix_ + "d", rclcpp::ParameterValue(gains.d_gain_)); - declare_param(param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(gains.i_max_)); - declare_param(param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(gains.i_min_)); - declare_param(param_prefix_ + "u_clamp_max", rclcpp::ParameterValue(gains.u_max_)); - declare_param(param_prefix_ + "u_clamp_min", rclcpp::ParameterValue(gains.u_min_)); - declare_param(param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(gains.trk_tc_)); - declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(true)); - declare_param(param_prefix_ + "antiwindup", rclcpp::ParameterValue(gains.antiwindup_)); - declare_param( - param_prefix_ + "antiwindup_strategy", - rclcpp::ParameterValue(gains.antiwindup_strat_.to_string())); - declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); - - set_parameter_event_callback(); - } + initialize_from_args(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, save_i_term); } -void PidROS::initialize_from_args( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat, bool save_i_term) +bool PidROS::initialize_from_args( + double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat, + bool save_i_term) { if (u_min > u_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + return false; + } + else if (std::isnan(u_min) || std::isnan(u_max)) + { + RCLCPP_ERROR(node_logging_->get_logger(), "u_min or u_max is NaN, skip new gains"); + return false; + } + else if (std::isnan(p) || std::isnan(i) || std::isnan(d)) + { + RCLCPP_ERROR(node_logging_->get_logger(), "p or i or d is NaN, skip new gains"); + return false; } else { - pid_.initialize(p, i, d, u_max, u_min, trk_tc, antiwindup_strat); + try + { + antiwindup_strat.validate(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); + return false; + } + + pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat); const Pid::Gains gains = pid_.get_gains(); declare_param(param_prefix_ + "p", rclcpp::ParameterValue(gains.p_gain_)); @@ -352,7 +362,9 @@ void PidROS::initialize_from_args( declare_param(param_prefix_ + "d", rclcpp::ParameterValue(gains.d_gain_)); declare_param(param_prefix_ + "u_clamp_max", rclcpp::ParameterValue(gains.u_max_)); declare_param(param_prefix_ + "u_clamp_min", rclcpp::ParameterValue(gains.u_min_)); - declare_param(param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(gains.trk_tc_)); + // @todo(sai) add other parameters or optimize it + declare_param( + param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(antiwindup_strat.trk_tc)); declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(true)); declare_param( param_prefix_ + "antiwindup_strategy", @@ -361,6 +373,7 @@ void PidROS::initialize_from_args( set_parameter_event_callback(); } + return true; } void PidROS::reset() @@ -395,60 +408,25 @@ Pid::Gains PidROS::get_gains() { return pid_.get_gains(); } void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - set_gains( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, - AntiwindupStrategy::INTEGRATOR_CLAMPING); -#pragma GCC diagnostic pop -} - -void PidROS::set_gains( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat) -{ - if (i_min > i_max) + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::LEGACY; + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.legacy_antiwindup = antiwindup; + try { - RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); - } - else if (u_min > u_max) - { - RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + antiwindup_strat.validate(); } - else + catch (const std::runtime_error & e) { - if (antiwindup_strat == AntiwindupStrategy::INTEGRATOR_CLAMPING) - { - std::cout << "Old anti-windup technique is deprecated. " - "This option will be removed by the ROS 2 Kilted Kaiju release." - << std::endl; - } - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - pid_.set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop - const Pid::Gains gains = pid_.get_gains(); - - node_params_->set_parameters( - {rclcpp::Parameter(param_prefix_ + "p", gains.p_gain_), - rclcpp::Parameter(param_prefix_ + "i", gains.i_gain_), - rclcpp::Parameter(param_prefix_ + "d", gains.d_gain_), - rclcpp::Parameter(param_prefix_ + "i_clamp_max", gains.i_max_), - rclcpp::Parameter(param_prefix_ + "i_clamp_min", gains.i_min_), - rclcpp::Parameter(param_prefix_ + "u_clamp_max", gains.u_max_), - rclcpp::Parameter(param_prefix_ + "u_clamp_min", gains.u_min_), - rclcpp::Parameter(param_prefix_ + "tracking_time_constant", gains.trk_tc_), - rclcpp::Parameter(param_prefix_ + "saturation", true), - rclcpp::Parameter(param_prefix_ + "antiwindup", gains.antiwindup_), - rclcpp::Parameter( - param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string())}); + RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); + return; } + set_gains(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat); } void PidROS::set_gains( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat) + double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat) { if (u_min > u_max) { @@ -456,15 +434,26 @@ void PidROS::set_gains( } else { - pid_.set_gains(p, i, d, u_max, u_min, trk_tc, antiwindup_strat); + try + { + antiwindup_strat.validate(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); + return; + } + + pid_.set_gains(p, i, d, u_max, u_min, antiwindup_strat); const Pid::Gains gains = pid_.get_gains(); + // @todo(sai) add other parameters or optimize it node_params_->set_parameters( {rclcpp::Parameter(param_prefix_ + "p", gains.p_gain_), rclcpp::Parameter(param_prefix_ + "i", gains.i_gain_), rclcpp::Parameter(param_prefix_ + "d", gains.d_gain_), rclcpp::Parameter(param_prefix_ + "u_clamp_max", gains.u_max_), rclcpp::Parameter(param_prefix_ + "u_clamp_min", gains.u_min_), - rclcpp::Parameter(param_prefix_ + "tracking_time_constant", gains.trk_tc_), + rclcpp::Parameter(param_prefix_ + "tracking_time_constant", antiwindup_strat.trk_tc), rclcpp::Parameter(param_prefix_ + "saturation", true), rclcpp::Parameter( param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string())}); @@ -655,7 +644,7 @@ void PidROS::set_parameter_event_callback() } else if (param_name == param_prefix_ + "antiwindup_strategy") { - gains.antiwindup_strat_ = AntiwindupStrategy(parameter.get_value()); + // gains.antiwindup_strat_ = AntiwindupStrategy(parameter.get_value()); changed = true; } } diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 57b26990..5cc3d1a4 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -51,12 +51,12 @@ TEST(ParameterTest, UTermBadIBoundsTestConstructor) "description", "This test checks if an error is thrown for bad u_bounds specification (i.e. u_min > u_max)."); - // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - // double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); - EXPECT_THROW( - Pid pid( - 1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING), - std::invalid_argument); + // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::LEGACY; + antiwindup_strat.i_max = 1.0; + antiwindup_strat.i_min = -1.0; + EXPECT_THROW(Pid pid(1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat), std::invalid_argument); } TEST(ParameterTest, UTermBadIBoundsTest) @@ -65,15 +65,17 @@ TEST(ParameterTest, UTermBadIBoundsTest) "description", "This test checks if gains remain for bad u_bounds specification (i.e. u_min > u_max)."); - // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - // double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); - Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 0.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING); + // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::LEGACY; + antiwindup_strat.i_max = 1.0; + antiwindup_strat.i_min = -1.0; + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat); auto gains = pid.get_gains(); EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); // Try to set bad u-bounds, i.e. u_min > u_max - EXPECT_NO_THROW(pid.set_gains( - 1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING)); + EXPECT_NO_THROW(pid.set_gains(1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat)); // Check if gains were not updated because u-bounds are bad, i.e. u_min > u_max EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); @@ -84,10 +86,12 @@ TEST(ParameterTest, outputClampTest) RecordProperty( "description", "This test succeeds if the output is clamped when the saturation is active."); - // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - // double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); + // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::BACK_CALCULATION; + antiwindup_strat.trk_tc = 0.0; // Set to 0.0 to use the default value // Setting u_max = 1.0 and u_min = -1.0 to test clamping - Pid pid(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, false, AntiwindupStrategy::BACK_CALCULATION); + Pid pid(1.0, 0.0, 0.0, 1.0, -1.0, antiwindup_strat); double cmd = 0.0; @@ -137,12 +141,14 @@ TEST(ParameterTest, noOutputClampTest) RecordProperty( "description", "This test succeeds if the output isn't clamped when the saturation is false."); - // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - // double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); + // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::BACK_CALCULATION; + antiwindup_strat.trk_tc = 0.0; // Set to 0.0 to use the default value // Setting u_max = INF and u_min = -INF to disable clamping Pid pid( - 1.0, 0.0, 0.0, 0.0, 0.0, std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), 0.0, false, AntiwindupStrategy::BACK_CALCULATION); + 1.0, 0.0, 0.0, std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), antiwindup_strat); double cmd = 0.0; @@ -194,9 +200,11 @@ TEST(ParameterTest, integrationBackCalculationZeroGainTest) "This test succeeds if the integral contribution is clamped when the integral gain is zero for " "the back calculation technique."); - // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - // double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); - Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, AntiwindupStrategy::BACK_CALCULATION); + // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::BACK_CALCULATION; + antiwindup_strat.trk_tc = 0.0; // Set to 0.0 to use the default value + Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, antiwindup_strat); double cmd = 0.0; double pe, ie, de; @@ -241,10 +249,10 @@ TEST(ParameterTest, integrationConditionalIntegrationZeroGainTest) "This test succeeds if the integral contribution is clamped when the integral gain is zero for " "the conditional integration technique."); - // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - // double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); - Pid pid( - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, AntiwindupStrategy::CONDITIONAL_INTEGRATION); + // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::CONDITIONAL_INTEGRATION; + Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, antiwindup_strat); double cmd = 0.0; double pe, ie, de; @@ -302,13 +310,13 @@ TEST(ParameterTest, ITermBadIBoundsTest) Pid pid(1.0, 1.0, 1.0, 1.0, -1.0); auto gains = pid.get_gains(); - EXPECT_DOUBLE_EQ(gains.i_min_, -1.0); - EXPECT_DOUBLE_EQ(gains.i_max_, 1.0); + EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_min, -1.0); + EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_max, 1.0); // Try to set bad i-bounds, i.e. i_min > i_max EXPECT_NO_THROW(pid.set_gains(1.0, 1.0, 1.0, -2.0, 2.0)); // Check if gains were not updated because i-bounds are bad, i.e. i_min > i_max - EXPECT_DOUBLE_EQ(gains.i_min_, -1.0); - EXPECT_DOUBLE_EQ(gains.i_max_, 1.0); + EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_min, -1.0); + EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_max, 1.0); } TEST(ParameterTest, integrationClampTest) @@ -429,11 +437,15 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double u_min = -1 * u_max; double trk_tc = std::rand() % 100; bool antiwindup = false; - AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::INTEGRATOR_CLAMPING; + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::LEGACY; + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.trk_tc = trk_tc; + antiwindup_strat.legacy_antiwindup = antiwindup; // Initialize the default way - Pid pid1( - p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); + Pid pid1(p_gain, i_gain, d_gain, u_max, u_min, antiwindup_strat); // Test return values ------------------------------------------------- double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, @@ -442,8 +454,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) AntiwindupStrategy antiwindup_strat_return; pid1.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, - u_min_return, trk_tc_return, antiwindup_return, antiwindup_strat_return); + p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, + antiwindup_strat_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); @@ -452,9 +464,11 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_min, i_min_return); EXPECT_EQ(u_max, u_max_return); EXPECT_EQ(u_min, u_min_return); - EXPECT_EQ(trk_tc, trk_tc_return); - EXPECT_EQ(antiwindup, antiwindup_return); - EXPECT_EQ(antiwindup_strat, antiwindup_strat_return); + EXPECT_EQ(trk_tc, antiwindup_strat_return.trk_tc); + EXPECT_EQ(i_min, antiwindup_strat_return.i_min); + EXPECT_EQ(i_max, antiwindup_strat_return.i_max); + EXPECT_EQ(antiwindup, antiwindup_strat_return.legacy_antiwindup); + EXPECT_EQ(antiwindup_strat.to_string(), antiwindup_strat_return.to_string()); // Test return values using struct ------------------------------------------------- @@ -468,10 +482,13 @@ TEST(ParameterTest, gainSettingCopyPIDTest) u_min = -1 * u_max; trk_tc = std::rand() % 100; antiwindup = false; - antiwindup_strat = AntiwindupStrategy::INTEGRATOR_CLAMPING; + antiwindup_strat.type = AntiwindupStrategy::LEGACY; + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.trk_tc = trk_tc; + antiwindup_strat.legacy_antiwindup = antiwindup; - pid1.set_gains( - p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); + pid1.set_gains(p_gain, i_gain, d_gain, u_max, u_min, antiwindup_strat); Pid::Gains g1 = pid1.get_gains(); EXPECT_EQ(p_gain, g1.p_gain_); @@ -481,9 +498,11 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_min, g1.i_min_); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); - EXPECT_EQ(trk_tc, g1.trk_tc_); - EXPECT_EQ(antiwindup, g1.antiwindup_); - EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); + EXPECT_EQ(trk_tc, g1.antiwindup_strat_.trk_tc); + EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max); + EXPECT_EQ(i_min, g1.antiwindup_strat_.i_min); + EXPECT_EQ(antiwindup, g1.antiwindup_strat_.legacy_antiwindup); + EXPECT_EQ(antiwindup_strat.to_string(), g1.antiwindup_strat_.to_string()); // Send update command to populate errors ------------------------------------------------- pid1.set_current_cmd(10); @@ -493,8 +512,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid2(pid1); pid2.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, - u_min_return, trk_tc_return, antiwindup_return, antiwindup_strat_return); + p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, + antiwindup_strat_return); EXPECT_EQ(p_gain, g1.p_gain_); EXPECT_EQ(i_gain, g1.i_gain_); @@ -503,9 +522,11 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_min, g1.i_min_); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); - EXPECT_EQ(trk_tc, g1.trk_tc_); - EXPECT_EQ(antiwindup, g1.antiwindup_); - EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); + EXPECT_EQ(trk_tc, g1.antiwindup_strat_.trk_tc); + EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max); + EXPECT_EQ(i_min, g1.antiwindup_strat_.i_min); + EXPECT_EQ(antiwindup, g1.antiwindup_strat_.legacy_antiwindup); + EXPECT_EQ(antiwindup_strat.to_string(), g1.antiwindup_strat_.to_string()); // Test that errors are zero double pe2, ie2, de2; @@ -529,9 +550,11 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_min, g1.i_min_); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); - EXPECT_EQ(trk_tc, g1.trk_tc_); - EXPECT_EQ(antiwindup, g1.antiwindup_); - EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); + EXPECT_EQ(trk_tc, g1.antiwindup_strat_.trk_tc); + EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max); + EXPECT_EQ(i_min, g1.antiwindup_strat_.i_min); + EXPECT_EQ(antiwindup, g1.antiwindup_strat_.legacy_antiwindup); + EXPECT_EQ(antiwindup_strat.to_string(), g1.antiwindup_strat_.to_string()); // Test that errors are zero double pe3, ie3, de3; @@ -722,9 +745,12 @@ TEST(CommandTest, backCalculationPIDTest) "This test checks that a command is computed correctly using a complete PID controller with " "back calculation technique."); - // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - // double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); - Pid pid(0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, false, AntiwindupStrategy::BACK_CALCULATION); + // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + // Setting u_max = 5.0 and u_min = -5.0 to test clamping + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::BACK_CALCULATION; + antiwindup_strat.trk_tc = 1.0; // Set to 0.0 to use the default value + Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); double cmd = 0.0; double pe, ie, de; @@ -779,10 +805,12 @@ TEST(CommandTest, conditionalIntegrationPIDTest) "This test checks that a command is computed correctly using a complete PID controller with " "conditional integration technique."); - // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - // double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); - Pid pid( - 0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, false, AntiwindupStrategy::CONDITIONAL_INTEGRATION); + // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + // Setting u_max = 5.0 and u_min = -5.0 to test clamping + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::CONDITIONAL_INTEGRATION; + antiwindup_strat.trk_tc = 1.0; + Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); double cmd = 0.0; double pe, ie, de; From 9a4362c94eb35ea53745d7aaf916a1337a70d909 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 11 Jun 2025 18:52:40 +0200 Subject: [PATCH 04/44] Fix logic to fix the tests --- .../include/control_toolbox/pid.hpp | 7 ++-- control_toolbox/src/pid.cpp | 34 ++++++++++++++----- control_toolbox/test/pid_tests.cpp | 26 ++++++++------ 3 files changed, 47 insertions(+), 20 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 9c4bbd47..17f9c805 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -103,11 +103,13 @@ struct AntiwindupStrategy } if (type == INTEGRATOR_CLAMPING && (i_min >= i_max)) { - throw std::runtime_error("AntiwindupStrategy 'integrator_clamping' requires i_min < i_max."); + throw std::invalid_argument( + "AntiwindupStrategy 'integrator_clamping' requires i_min < i_max."); } if (type == LEGACY && (i_min >= i_max)) { - throw std::runtime_error("AntiwindupStrategy 'legacy' requires i_min < i_max."); + // throw std::invalid_argument("AntiwindupStrategy 'legacy' requires i_min < i_max."); + std::cerr << "AntiwindupStrategy 'legacy' requires i_min < i_max." << std::endl; } } @@ -312,6 +314,7 @@ class Pid i_min_(antiwindup_strat.i_min), u_max_(u_max), u_min_(u_min), + trk_tc_(antiwindup_strat.trk_tc), antiwindup_strat_(antiwindup_strat) { if (std::isnan(u_min) || std::isnan(u_max)) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index acdedef4..a86dd804 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -52,10 +52,15 @@ constexpr double UMAX_INFINITY = std::numeric_limits::infinity(); Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) : gains_buffer_() { + if (i_min > i_max) + { + throw std::invalid_argument("received i_min > i_max"); + } AntiwindupStrategy antiwindup_strat; antiwindup_strat.type = AntiwindupStrategy::LEGACY; antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; + antiwindup_strat.legacy_antiwindup = antiwindup; antiwindup_strat.validate(); set_gains(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat); @@ -167,12 +172,12 @@ void Pid::get_gains( p = gains.p_gain_; i = gains.i_gain_; d = gains.d_gain_; - i_max = gains.i_max_; - i_min = gains.i_min_; + i_max = gains.antiwindup_strat_.i_max; + i_min = gains.antiwindup_strat_.i_min; u_max = gains.u_max_; u_min = gains.u_min_; trk_tc = gains.trk_tc_; - antiwindup = gains.antiwindup_; + antiwindup = gains.antiwindup_strat_.legacy_antiwindup; antiwindup_strat = gains.antiwindup_strat_; } @@ -196,23 +201,36 @@ Pid::Gains Pid::get_gains() { return *gains_buffer_.readFromRT(); } #pragma GCC diagnostic ignored "-Wdeprecated-declarations" void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { + try + { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - Gains gains(p, i, d, i_max, i_min, antiwindup); + Gains gains(p, i, d, i_max, i_min, antiwindup); + set_gains(gains); #pragma GCC diagnostic pop - set_gains(gains); + } + catch (const std::exception & e) + { + std::cerr << e.what() << '\n'; + } } #pragma GCC diagnostic pop void Pid::set_gains( double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat) { + try + { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - Gains gains(p, i, d, u_max, u_min, antiwindup_strat); + Gains gains(p, i, d, u_max, u_min, antiwindup_strat); #pragma GCC diagnostic pop - - set_gains(gains); + set_gains(gains); + } + catch (const std::exception & e) + { + std::cerr << e.what() << '\n'; + } } void Pid::set_gains(const Gains & gains_in) diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 5cc3d1a4..18371c98 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -51,7 +51,8 @@ TEST(ParameterTest, UTermBadIBoundsTestConstructor) "description", "This test checks if an error is thrown for bad u_bounds specification (i.e. u_min > u_max)."); - // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiwindupStrategy antiwindup_strat); AntiwindupStrategy antiwindup_strat; antiwindup_strat.type = AntiwindupStrategy::LEGACY; antiwindup_strat.i_max = 1.0; @@ -65,7 +66,8 @@ TEST(ParameterTest, UTermBadIBoundsTest) "description", "This test checks if gains remain for bad u_bounds specification (i.e. u_min > u_max)."); - // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiwindupStrategy antiwindup_strat); AntiwindupStrategy antiwindup_strat; antiwindup_strat.type = AntiwindupStrategy::LEGACY; antiwindup_strat.i_max = 1.0; @@ -86,7 +88,8 @@ TEST(ParameterTest, outputClampTest) RecordProperty( "description", "This test succeeds if the output is clamped when the saturation is active."); - // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiwindupStrategy antiwindup_strat); AntiwindupStrategy antiwindup_strat; antiwindup_strat.type = AntiwindupStrategy::BACK_CALCULATION; antiwindup_strat.trk_tc = 0.0; // Set to 0.0 to use the default value @@ -141,7 +144,8 @@ TEST(ParameterTest, noOutputClampTest) RecordProperty( "description", "This test succeeds if the output isn't clamped when the saturation is false."); - // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiwindupStrategy antiwindup_strat); AntiwindupStrategy antiwindup_strat; antiwindup_strat.type = AntiwindupStrategy::BACK_CALCULATION; antiwindup_strat.trk_tc = 0.0; // Set to 0.0 to use the default value @@ -200,7 +204,8 @@ TEST(ParameterTest, integrationBackCalculationZeroGainTest) "This test succeeds if the integral contribution is clamped when the integral gain is zero for " "the back calculation technique."); - // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiwindupStrategy antiwindup_strat); AntiwindupStrategy antiwindup_strat; antiwindup_strat.type = AntiwindupStrategy::BACK_CALCULATION; antiwindup_strat.trk_tc = 0.0; // Set to 0.0 to use the default value @@ -249,7 +254,8 @@ TEST(ParameterTest, integrationConditionalIntegrationZeroGainTest) "This test succeeds if the integral contribution is clamped when the integral gain is zero for " "the conditional integration technique."); - // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiwindupStrategy antiwindup_strat); AntiwindupStrategy antiwindup_strat; antiwindup_strat.type = AntiwindupStrategy::CONDITIONAL_INTEGRATION; Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, antiwindup_strat); @@ -460,8 +466,6 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); EXPECT_EQ(d_gain, d_gain_return); - EXPECT_EQ(i_max, i_max_return); - EXPECT_EQ(i_min, i_min_return); EXPECT_EQ(u_max, u_max_return); EXPECT_EQ(u_min, u_min_return); EXPECT_EQ(trk_tc, antiwindup_strat_return.trk_tc); @@ -745,7 +749,8 @@ TEST(CommandTest, backCalculationPIDTest) "This test checks that a command is computed correctly using a complete PID controller with " "back calculation technique."); - // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiwindupStrategy antiwindup_strat); // Setting u_max = 5.0 and u_min = -5.0 to test clamping AntiwindupStrategy antiwindup_strat; antiwindup_strat.type = AntiwindupStrategy::BACK_CALCULATION; @@ -805,7 +810,8 @@ TEST(CommandTest, conditionalIntegrationPIDTest) "This test checks that a command is computed correctly using a complete PID controller with " "conditional integration technique."); - // Pid(double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiwindupStrategy antiwindup_strat); // Setting u_max = 5.0 and u_min = -5.0 to test clamping AntiwindupStrategy antiwindup_strat; antiwindup_strat.type = AntiwindupStrategy::CONDITIONAL_INTEGRATION; From ae3f445700512d6b540968cc162f53b68cd6f890 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 11 Jun 2025 18:56:27 +0200 Subject: [PATCH 05/44] Remove tracking time constant in the Gains struct --- control_toolbox/include/control_toolbox/pid.hpp | 5 +---- control_toolbox/src/pid.cpp | 13 +++++++------ control_toolbox/src/pid_ros.cpp | 4 ++-- 3 files changed, 10 insertions(+), 12 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 17f9c805..fe30868a 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -314,7 +314,6 @@ class Pid i_min_(antiwindup_strat.i_min), u_max_(u_max), u_min_(u_min), - trk_tc_(antiwindup_strat.trk_tc), antiwindup_strat_(antiwindup_strat) { if (std::isnan(u_min) || std::isnan(u_max)) @@ -341,8 +340,7 @@ class Pid { 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_ << ", trk_tc: " << trk_tc_ - << ", antiwindup: " << antiwindup_ + << ", u_min: " << u_min_ << ", antiwindup: " << antiwindup_ << ", antiwindup_strat: " << antiwindup_strat_.to_string() << std::endl; } @@ -355,7 +353,6 @@ class Pid double i_min_ = 0.0; /**< Minimum allowable integral term. */ double u_max_ = std::numeric_limits::infinity(); /**< Maximum allowable output. */ double u_min_ = -std::numeric_limits::infinity(); /**< Minimum allowable output. */ - double trk_tc_ = 0.0; /**< Tracking time constant. */ bool antiwindup_ = false; /**< Anti-windup. */ AntiwindupStrategy antiwindup_strat_ = AntiwindupStrategy::UNDEFINED; /**< Anti-windup strategy. */ diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index a86dd804..5fcccf9a 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -176,7 +176,7 @@ void Pid::get_gains( i_min = gains.antiwindup_strat_.i_min; u_max = gains.u_max_; u_min = gains.u_min_; - trk_tc = gains.trk_tc_; + trk_tc = gains.antiwindup_strat_.trk_tc; antiwindup = gains.antiwindup_strat_.legacy_antiwindup; antiwindup_strat = gains.antiwindup_strat_; } @@ -253,15 +253,15 @@ void Pid::set_gains(const Gains & gains_in) if (gains.antiwindup_strat_ == AntiwindupStrategy::BACK_CALCULATION) { - if (is_zero(gains.trk_tc_) && !is_zero(gains.d_gain_)) + if (is_zero(gains.antiwindup_strat_.trk_tc) && !is_zero(gains.d_gain_)) { // Default value for tracking time constant for back calculation technique - gains.trk_tc_ = std::sqrt(gains.d_gain_ / gains.i_gain_); + gains.antiwindup_strat_.trk_tc = std::sqrt(gains.d_gain_ / gains.i_gain_); } - else if (is_zero(gains.trk_tc_) && is_zero(gains.d_gain_)) + else if (is_zero(gains.antiwindup_strat_.trk_tc) && is_zero(gains.d_gain_)) { // Default value for tracking time constant for back calculation technique - gains.trk_tc_ = gains.p_gain_ / gains.i_gain_; + gains.antiwindup_strat_.trk_tc = gains.p_gain_ / gains.i_gain_; } } gains_buffer_.writeFromNonRT(gains); @@ -402,7 +402,8 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) if (gains.antiwindup_strat_ == AntiwindupStrategy::BACK_CALCULATION && !is_zero(gains.i_gain_)) { - i_term_ += dt_s * (gains.i_gain_ * error + 1 / gains.trk_tc_ * (cmd_ - cmd_unsat_)); + i_term_ += + dt_s * (gains.i_gain_ * error + 1 / gains.antiwindup_strat_.trk_tc * (cmd_ - cmd_unsat_)); } else if (gains.antiwindup_strat_ == AntiwindupStrategy::CONDITIONAL_INTEGRATION) { diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 4f279f02..48b88919 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -531,7 +531,7 @@ void PidROS::print_values() << " I Min: " << gains.i_min_ << "\n" << " U_Max: " << gains.u_max_ << "\n" << " U_Min: " << gains.u_min_ << "\n" - << " Tracking_Time_Constant: " << gains.trk_tc_ << "\n" + << " Tracking_Time_Constant: " << gains.antiwindup_strat_.trk_tc << "\n" << " Antiwindup: " << gains.antiwindup_ << "\n" << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\n" @@ -634,7 +634,7 @@ void PidROS::set_parameter_event_callback() } else if (param_name == param_prefix_ + "tracking_time_constant") { - gains.trk_tc_ = parameter.get_value(); + gains.antiwindup_strat_.trk_tc = parameter.get_value(); changed = true; } else if (param_name == param_prefix_ + "antiwindup") From 45fb30344eb0ef5ea1d7a7cd13bb1f6ffec45e3d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 11 Jun 2025 19:05:30 +0200 Subject: [PATCH 06/44] Fix the compilation of the PidROS tests --- control_toolbox/CMakeLists.txt | 4 +- .../test/pid_ros_parameters_tests.cpp | 99 ++++++++++++------- 2 files changed, 66 insertions(+), 37 deletions(-) diff --git a/control_toolbox/CMakeLists.txt b/control_toolbox/CMakeLists.txt index d35e6fce..a1437d81 100644 --- a/control_toolbox/CMakeLists.txt +++ b/control_toolbox/CMakeLists.txt @@ -176,8 +176,8 @@ if(BUILD_TESTING) ament_add_gmock(rate_limiter_tests test/rate_limiter.cpp) target_link_libraries(rate_limiter_tests control_toolbox) - # ament_add_gmock(pid_ros_parameters_tests test/pid_ros_parameters_tests.cpp) - # target_link_libraries(pid_ros_parameters_tests control_toolbox) + ament_add_gmock(pid_ros_parameters_tests test/pid_ros_parameters_tests.cpp) + target_link_libraries(pid_ros_parameters_tests control_toolbox) # ament_add_gmock(pid_ros_publisher_tests test/pid_ros_publisher_tests.cpp) # target_link_libraries(pid_ros_publisher_tests control_toolbox rclcpp_lifecycle::rclcpp_lifecycle) diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 4b9f9a0f..359af0a8 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -63,11 +63,15 @@ void check_set_parameters( const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::INTEGRATOR_CLAMPING; + AntiwindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; const bool SAVE_I_TERM = true; - ASSERT_NO_THROW(pid.initialize_from_args( - P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM)); + ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, SAVE_I_TERM)); rclcpp::Parameter param; @@ -117,7 +121,7 @@ void check_set_parameters( ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_EQ(gains.u_max_, U_MAX); ASSERT_EQ(gains.u_min_, U_MIN); - ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_TRUE(gains.antiwindup_); ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); } @@ -149,9 +153,14 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double U_MIN_BAD = 10.0; const double TRK_TC = 4.0; - ASSERT_NO_THROW(pid.initialize_from_args( - P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD, TRK_TC, false, - AntiwindupStrategy::INTEGRATOR_CLAMPING, false)); + AntiwindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX_BAD; + ANTIWINDUP_STRAT.i_min = I_MIN_BAD; + ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = false; + + ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); rclcpp::Parameter param; @@ -177,7 +186,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.i_min_, 0.0); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); - ASSERT_EQ(gains.trk_tc_, 0.0); + ASSERT_EQ(gains.antiwindup_strat_.trk_tc, 0.0); ASSERT_FALSE(gains.antiwindup_); ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); } @@ -264,11 +273,15 @@ TEST(PidParametersTest, SetParametersTest) const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::INTEGRATOR_CLAMPING; + AntiwindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; const bool SAVE_I_TERM = false; - pid.initialize_from_args( - P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM); + pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, SAVE_I_TERM); rcl_interfaces::msg::SetParametersResult set_result; @@ -316,7 +329,7 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_EQ(gains.u_max_, U_MAX); ASSERT_EQ(gains.u_min_, U_MIN); - ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); } @@ -341,10 +354,15 @@ TEST(PidParametersTest, SetBadParametersTest) const double TRK_TC = 4.0; const bool SATURATION = false; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::INTEGRATOR_CLAMPING; - pid.initialize_from_args( - P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, ANTIWINDUP, ANTIWINDUP_STRAT, false); + AntiwindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; + + pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false); rcl_interfaces::msg::SetParametersResult set_result; @@ -391,7 +409,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.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.trk_tc_, TRK_TC); + ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); @@ -414,7 +432,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.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.trk_tc_, TRK_TC); + ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); @@ -434,7 +452,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(updated_gains.i_min_, I_MIN); ASSERT_EQ(updated_gains.u_max_, U_MAX); ASSERT_EQ(updated_gains.u_min_, U_MIN); - ASSERT_EQ(updated_gains.trk_tc_, TRK_TC); + ASSERT_EQ(updated_gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_EQ(updated_gains.antiwindup_, ANTIWINDUP); ASSERT_EQ(updated_gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); } @@ -456,13 +474,17 @@ TEST(PidParametersTest, GetParametersTest) const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::INTEGRATOR_CLAMPING; - pid.initialize_from_args( - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING, - false); + AntiwindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; + + pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false); std::cout << "Setting gains with set_gains()" << std::endl; - pid.set_gains(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, ANTIWINDUP, ANTIWINDUP_STRAT); + pid.set_gains(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT); rclcpp::Parameter param; @@ -516,12 +538,16 @@ TEST(PidParametersTest, GetParametersTest) const double U_MIN = -std::numeric_limits::infinity(); const double TRK_TC = 4.0; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::INTEGRATOR_CLAMPING; - pid.initialize_from_args( - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING, - false); - pid.set_gains(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, ANTIWINDUP, ANTIWINDUP_STRAT); + AntiwindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; + + pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false); + pid.set_gains(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT); rclcpp::Parameter param; @@ -619,13 +645,16 @@ TEST(PidParametersTest, MultiplePidInstances) const double U_MAX = 10.0; const double U_MIN = -10.0; const double TRK_TC = 4.0; - - ASSERT_NO_THROW(pid_1.initialize_from_args( - P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false, AntiwindupStrategy::INTEGRATOR_CLAMPING, - false)); - ASSERT_NO_THROW(pid_2.initialize_from_args( - P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true, AntiwindupStrategy::INTEGRATOR_CLAMPING, - false)); + AntiwindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = false; + + ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); + ANTIWINDUP_STRAT.legacy_antiwindup = true; + ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); From 0ca2cd6480c65874438f21974a8c4ff1e1301901 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 00:45:58 +0200 Subject: [PATCH 07/44] fix tests --- .../include/control_toolbox/pid.hpp | 3 ++ control_toolbox/src/pid.cpp | 5 +- control_toolbox/src/pid_ros.cpp | 46 +++++++++++-------- .../test/pid_ros_parameters_tests.cpp | 12 ++--- 4 files changed, 39 insertions(+), 27 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index fe30868a..d1c66917 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -258,6 +258,7 @@ class Pid antiwindup_strat_.type = AntiwindupStrategy::LEGACY; antiwindup_strat_.i_max = i_max; antiwindup_strat_.i_min = i_min; + antiwindup_strat_.legacy_antiwindup = false; antiwindup_strat_.validate(); } @@ -289,6 +290,7 @@ class Pid antiwindup_strat_.type = AntiwindupStrategy::LEGACY; antiwindup_strat_.i_max = i_max; antiwindup_strat_.i_min = i_min; + antiwindup_strat_.legacy_antiwindup = antiwindup; antiwindup_strat_.validate(); } @@ -314,6 +316,7 @@ class Pid i_min_(antiwindup_strat.i_min), u_max_(u_max), u_min_(u_min), + antiwindup_(antiwindup_strat.legacy_antiwindup), antiwindup_strat_(antiwindup_strat) { if (std::isnan(u_min) || std::isnan(u_max)) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 5fcccf9a..b9247aad 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -400,12 +400,13 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) cmd_ = cmd_unsat_; } - if (gains.antiwindup_strat_ == AntiwindupStrategy::BACK_CALCULATION && !is_zero(gains.i_gain_)) + if ( + gains.antiwindup_strat_.type == AntiwindupStrategy::BACK_CALCULATION && !is_zero(gains.i_gain_)) { i_term_ += dt_s * (gains.i_gain_ * error + 1 / gains.antiwindup_strat_.trk_tc * (cmd_ - cmd_unsat_)); } - else if (gains.antiwindup_strat_ == AntiwindupStrategy::CONDITIONAL_INTEGRATION) + else if (gains.antiwindup_strat_.type == AntiwindupStrategy::CONDITIONAL_INTEGRATION) { if (!(!iszero(cmd_unsat_ - cmd_) && error * cmd_unsat_ > 0)) { diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 48b88919..1d25746a 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -360,8 +360,15 @@ bool PidROS::initialize_from_args( declare_param(param_prefix_ + "p", rclcpp::ParameterValue(gains.p_gain_)); declare_param(param_prefix_ + "i", rclcpp::ParameterValue(gains.i_gain_)); declare_param(param_prefix_ + "d", rclcpp::ParameterValue(gains.d_gain_)); + declare_param( + param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(gains.antiwindup_strat_.i_max)); + declare_param( + param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(gains.antiwindup_strat_.i_min)); declare_param(param_prefix_ + "u_clamp_max", rclcpp::ParameterValue(gains.u_max_)); declare_param(param_prefix_ + "u_clamp_min", rclcpp::ParameterValue(gains.u_min_)); + declare_param( + param_prefix_ + "antiwindup", + rclcpp::ParameterValue(gains.antiwindup_strat_.legacy_antiwindup)); // @todo(sai) add other parameters or optimize it declare_param( param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(antiwindup_strat.trk_tc)); @@ -522,24 +529,24 @@ void PidROS::print_values() 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" - << " 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_.trk_tc << "\n" - << " Antiwindup: " << gains.antiwindup_ << "\n" - << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() - << "\n" - << "\n" - << " P Error: " << p_error << "\n" - << " I Term: " << i_term << "\n" - << " D Error: " << d_error << "\n" - << " Command: " << get_current_cmd();); + RCLCPP_INFO_STREAM( + node_logging_->get_logger(), + "Current Values of PID template:\n" + << " 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_.trk_tc << "\n" + << " Antiwindup: " << gains.antiwindup_strat_.legacy_antiwindup << "\n" + << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\n" + << "\n" + << " P Error: " << p_error << "\n" + << " I Term: " << i_term << "\n" + << " D Error: " << d_error << "\n" + << " Command: " << get_current_cmd();); } void PidROS::set_parameter_event_callback() @@ -639,11 +646,12 @@ void PidROS::set_parameter_event_callback() } else if (param_name == param_prefix_ + "antiwindup") { - gains.antiwindup_ = parameter.get_value(); + gains.antiwindup_strat_.legacy_antiwindup = parameter.get_value(); changed = true; } else if (param_name == param_prefix_ + "antiwindup_strategy") { + // @todo decide if this can be changed in the first place // gains.antiwindup_strat_ = AntiwindupStrategy(parameter.get_value()); changed = true; } diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 359af0a8..7a413ef7 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -123,7 +123,7 @@ void check_set_parameters( ASSERT_EQ(gains.u_min_, U_MIN); ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_TRUE(gains.antiwindup_); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::LEGACY); } TEST(PidParametersTest, InitPidTest) @@ -188,7 +188,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.trk_tc, 0.0); ASSERT_FALSE(gains.antiwindup_); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::LEGACY); } TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot) @@ -331,7 +331,7 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.u_min_, U_MIN); ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::LEGACY); } TEST(PidParametersTest, SetBadParametersTest) @@ -411,7 +411,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::LEGACY); // Set the good gains @@ -434,7 +434,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::LEGACY); // Now re-enabling it should have the old gains back ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", true))); @@ -454,7 +454,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(updated_gains.u_min_, U_MIN); ASSERT_EQ(updated_gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_EQ(updated_gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(updated_gains.antiwindup_strat_, AntiwindupStrategy::INTEGRATOR_CLAMPING); + ASSERT_EQ(updated_gains.antiwindup_strat_, AntiwindupStrategy::LEGACY); } TEST(PidParametersTest, GetParametersTest) From 73d8b7bd81180885f0bab012cc2bfee047798e6c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 00:50:28 +0200 Subject: [PATCH 08/44] fix the tests of pid_ros_publisher_tests --- control_toolbox/CMakeLists.txt | 4 ++-- .../test/pid_ros_publisher_tests.cpp | 20 +++++++++++++------ 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/control_toolbox/CMakeLists.txt b/control_toolbox/CMakeLists.txt index a1437d81..4a79bdec 100644 --- a/control_toolbox/CMakeLists.txt +++ b/control_toolbox/CMakeLists.txt @@ -179,8 +179,8 @@ if(BUILD_TESTING) ament_add_gmock(pid_ros_parameters_tests test/pid_ros_parameters_tests.cpp) target_link_libraries(pid_ros_parameters_tests control_toolbox) - # ament_add_gmock(pid_ros_publisher_tests test/pid_ros_publisher_tests.cpp) - # target_link_libraries(pid_ros_publisher_tests control_toolbox rclcpp_lifecycle::rclcpp_lifecycle) + ament_add_gmock(pid_ros_publisher_tests test/pid_ros_publisher_tests.cpp) + target_link_libraries(pid_ros_publisher_tests control_toolbox rclcpp_lifecycle::rclcpp_lifecycle) ## Control Filters ### Gravity Compensation diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index 49e8cf40..2d44d42e 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -43,9 +43,13 @@ TEST(PidPublisherTest, PublishTest) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); - pid_ros.initialize_from_args( - 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING, - false); + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::LEGACY; + antiwindup_strat.i_max = 5.0; + antiwindup_strat.i_min = -5.0; + antiwindup_strat.legacy_antiwindup = false; + antiwindup_strat.trk_tc = 1.0; + pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -82,9 +86,13 @@ TEST(PidPublisherTest, PublishTestLifecycle) std::dynamic_pointer_cast>( pid_ros.get_pid_state_publisher()); - pid_ros.initialize_from_args( - 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, AntiwindupStrategy::INTEGRATOR_CLAMPING, - false); + AntiwindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiwindupStrategy::LEGACY; + antiwindup_strat.i_max = 5.0; + antiwindup_strat.i_min = -5.0; + antiwindup_strat.legacy_antiwindup = false; + antiwindup_strat.trk_tc = 1.0; + pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; From 574b497ce1c3d528590de3c16e1e85568bef1d64 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 10:19:53 +0200 Subject: [PATCH 09/44] use const reference for the antiwindup_strat argument --- control_toolbox/include/control_toolbox/pid.hpp | 12 ++++++++---- control_toolbox/include/control_toolbox/pid_ros.hpp | 7 ++++--- control_toolbox/src/pid.cpp | 9 ++++++--- control_toolbox/src/pid_ros.cpp | 7 ++++--- 4 files changed, 22 insertions(+), 13 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index d1c66917..3fdaedb0 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -308,7 +308,8 @@ class Pid * */ Gains( - double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat) + double p, double i, double d, double u_max, double u_min, + const AntiwindupStrategy & antiwindup_strat) : p_gain_(p), i_gain_(i), d_gain_(d), @@ -397,7 +398,8 @@ class Pid * \throws An std::invalid_argument exception is thrown if u_min > u_max */ Pid( - double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + double p, double i, double d, double u_max, double u_min, + const AntiwindupStrategy & antiwindup_strat); /*! * \brief Copy constructor required for preventing mutexes from being copied @@ -446,7 +448,8 @@ class Pid * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max */ void initialize( - double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + double p, double i, double d, double u_max, double u_min, + const AntiwindupStrategy & antiwindup_strat); /*! * \brief Reset the state of this PID controller @@ -570,7 +573,8 @@ class Pid * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max */ void set_gains( - double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + double p, double i, double d, double u_max, double u_min, + const AntiwindupStrategy & antiwindup_strat); /*! * \brief Set PID gains for the controller. diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index b0dd3fcf..f09c2a7a 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -139,8 +139,8 @@ class PidROS * \note New gains are not applied if u_min_ > u_max_. */ bool initialize_from_args( - double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat, - bool save_i_term); + double p, double i, double d, double u_max, double u_min, + const AntiwindupStrategy & antiwindup_strat, bool save_i_term); /*! * \brief Initialize the PID controller based on already set parameters @@ -225,7 +225,8 @@ class PidROS * \note New gains are not applied if u_min_ > u_max_. */ void set_gains( - double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat); + double p, double i, double d, double u_max, double u_min, + const AntiwindupStrategy & antiwindup_strat); /*! * \brief Set PID gains for the controller. diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index b9247aad..bda0934b 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -72,7 +72,8 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwind #pragma GCC diagnostic pop Pid::Pid( - double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat) + double p, double i, double d, double u_max, double u_min, + const AntiwindupStrategy & antiwindup_strat) : gains_buffer_() { if (u_min > u_max) @@ -112,7 +113,8 @@ void Pid::initialize(double p, double i, double d, double i_max, double i_min, b } void Pid::initialize( - double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat) + double p, double i, double d, double u_max, double u_min, + const AntiwindupStrategy & antiwindup_strat) { set_gains(p, i, d, u_max, u_min, antiwindup_strat); @@ -217,7 +219,8 @@ void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bo #pragma GCC diagnostic pop void Pid::set_gains( - double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat) + double p, double i, double d, double u_max, double u_min, + const AntiwindupStrategy & antiwindup_strat) { try { diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 1d25746a..88479f65 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -324,8 +324,8 @@ void PidROS::initialize_from_args( } bool PidROS::initialize_from_args( - double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat, - bool save_i_term) + double p, double i, double d, double u_max, double u_min, + const AntiwindupStrategy & antiwindup_strat, bool save_i_term) { if (u_min > u_max) { @@ -433,7 +433,8 @@ void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, } void PidROS::set_gains( - double p, double i, double d, double u_max, double u_min, AntiwindupStrategy antiwindup_strat) + double p, double i, double d, double u_max, double u_min, + const AntiwindupStrategy & antiwindup_strat) { if (u_min > u_max) { From 87c7cece9e70bbff581982fea8f12e6160fd37df Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 11:51:05 +0200 Subject: [PATCH 10/44] Remove newly introduced get_gains method --- .../include/control_toolbox/pid.hpp | 25 ------------- control_toolbox/src/pid.cpp | 36 ++++--------------- control_toolbox/test/pid_tests.cpp | 33 ++++++++--------- 3 files changed, 24 insertions(+), 70 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 3fdaedb0..428b9084 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -495,31 +495,6 @@ class Pid void get_gains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup); - /*! - * \brief Get PID gains for the controller. - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. - * \param i_max Upper integral clamp. - * \param i_min Lower integral clamp. - * \param u_max Upper output clamp. - * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. - */ - [[deprecated("Use get_gains overload with AntiwindupStrategy only.")]] - void get_gains( - double & p, double & i, double & d, double & i_max, double & i_min, double & u_max, - double & u_min, double & trk_tc, bool & antiwindup, AntiwindupStrategy & antiwindup_strat); - /*! * \brief Get PID gains for the controller (preferred). * \param p The proportional gain. diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index bda0934b..2245babd 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -143,13 +143,10 @@ void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & { double u_max; double u_min; - double trk_tc; - bool antiwindup; AntiwindupStrategy antiwindup_strat; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - get_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop + get_gains(p, i, d, u_max, u_min, antiwindup_strat); + i_max = antiwindup_strat.i_max; + i_min = antiwindup_strat.i_min; } void Pid::get_gains( @@ -157,30 +154,11 @@ void Pid::get_gains( { double u_max; double u_min; - double trk_tc; AntiwindupStrategy antiwindup_strat; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - get_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop -} - -void Pid::get_gains( - double & p, double & i, double & d, double & i_max, double & i_min, double & u_max, - double & u_min, double & trk_tc, bool & antiwindup, AntiwindupStrategy & antiwindup_strat) -{ - Gains gains = *gains_buffer_.readFromRT(); - - p = gains.p_gain_; - i = gains.i_gain_; - d = gains.d_gain_; - i_max = gains.antiwindup_strat_.i_max; - i_min = gains.antiwindup_strat_.i_min; - u_max = gains.u_max_; - u_min = gains.u_min_; - trk_tc = gains.antiwindup_strat_.trk_tc; - antiwindup = gains.antiwindup_strat_.legacy_antiwindup; - antiwindup_strat = gains.antiwindup_strat_; + get_gains(p, i, d, u_max, u_min, antiwindup_strat); + i_max = antiwindup_strat.i_max; + i_min = antiwindup_strat.i_min; + antiwindup = antiwindup_strat.legacy_antiwindup; } void Pid::get_gains( diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 18371c98..d5d2a9d3 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -502,6 +502,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_min, g1.i_min_); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); + EXPECT_EQ(antiwindup, g1.antiwindup_); EXPECT_EQ(trk_tc, g1.antiwindup_strat_.trk_tc); EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max); EXPECT_EQ(i_min, g1.antiwindup_strat_.i_min); @@ -519,16 +520,16 @@ TEST(ParameterTest, gainSettingCopyPIDTest) p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, antiwindup_strat_return); - 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(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(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); EXPECT_EQ(trk_tc, g1.antiwindup_strat_.trk_tc); - EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max); - EXPECT_EQ(i_min, g1.antiwindup_strat_.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(antiwindup, g1.antiwindup_strat_.legacy_antiwindup); EXPECT_EQ(antiwindup_strat.to_string(), g1.antiwindup_strat_.to_string()); @@ -544,19 +545,19 @@ TEST(ParameterTest, gainSettingCopyPIDTest) pid3 = pid1; pid3.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, - u_min_return, trk_tc_return, antiwindup_return, antiwindup_strat_return); + p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, + antiwindup_strat_return); - 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(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(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); EXPECT_EQ(trk_tc, g1.antiwindup_strat_.trk_tc); - EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max); - EXPECT_EQ(i_min, g1.antiwindup_strat_.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(antiwindup, g1.antiwindup_strat_.legacy_antiwindup); EXPECT_EQ(antiwindup_strat.to_string(), g1.antiwindup_strat_.to_string()); From 60c0a1c61ffa2614bfc02931df13d5d85ada155e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 11:51:28 +0200 Subject: [PATCH 11/44] deprecate the antiwindup_ member variable --- 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 428b9084..4e8f988e 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -357,8 +357,8 @@ class Pid double i_min_ = 0.0; /**< 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. */ - AntiwindupStrategy antiwindup_strat_ = + [[deprecated("Use antiwindup_strat_ instead.")]] + bool antiwindup_ = false; /**< Anti-windup. */ AntiwindupStrategy::UNDEFINED; /**< Anti-windup strategy. */ }; From fcb5b3a6d87762b8ee9df4116451e20841cc6f04 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 11:52:27 +0200 Subject: [PATCH 12/44] Remove default AntiwindupStrategy initialization --- 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 4e8f988e..ee3b7e08 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -359,7 +359,7 @@ class Pid double u_min_ = -std::numeric_limits::infinity(); /**< Minimum allowable output. */ [[deprecated("Use antiwindup_strat_ instead.")]] bool antiwindup_ = false; /**< Anti-windup. */ - AntiwindupStrategy::UNDEFINED; /**< Anti-windup strategy. */ + AntiwindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ }; /*! From 5a9fee856d72e6b5cc1f958cf257f41d70fba847 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 11:52:36 +0200 Subject: [PATCH 13/44] Update docs --- control_toolbox/include/control_toolbox/pid.hpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index ee3b7e08..e265686c 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -44,6 +44,23 @@ namespace control_toolbox { +/** + * \brief Antiwindup strategy for PID controllers. + * + * This class defines various antiwindup strategies that can be used in PID controllers. + * It allows setting the type of antiwindup strategy and validates the parameters accordingly. + * + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set + * to 0.0 when this strategy is selected, a recommended default value will be applied. + * \param legacy_antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + */ struct AntiwindupStrategy { public: From fad998bdcd5656cfd0a892f9e9d51f07bd08b715 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 11:54:52 +0200 Subject: [PATCH 14/44] update the default constructor --- control_toolbox/include/control_toolbox/pid.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index e265686c..62a5de79 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -74,8 +74,10 @@ struct AntiwindupStrategy CONDITIONAL_INTEGRATION }; - constexpr AntiwindupStrategy() : type(LEGACY) {} - constexpr AntiwindupStrategy(Value v) : type(v) {} // NOLINT(runtime/explicit) + AntiwindupStrategy() + : type(UNDEFINED), i_min(0.0), i_max(0.0), trk_tc(0.0), legacy_antiwindup(false) + { + } void set_type(const std::string & s) { From 2ec2d25eecc644404f4d59682ba90cbda2ece039 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 14:02:31 +0200 Subject: [PATCH 15/44] Change the default antiwindup strategy to declare to "legacy" --- control_toolbox/src/pid_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 71f8e2f8..350ab69c 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -220,7 +220,7 @@ bool PidROS::initialize_from_ros_parameters() u_max = UMAX_INFINITY; u_min = -UMAX_INFINITY; bool antiwindup = false; - std::string antiwindup_strat_str = "none"; + std::string antiwindup_strat_str = "legacy"; bool all_params_available = true; all_params_available &= get_double_param(param_prefix_ + "p", p); From 2ead217864e5fd9071bb8a1b67975334bd66f456 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 14:07:44 +0200 Subject: [PATCH 16/44] Change the default to NaNs in the AntiwindupStrategy --- .../include/control_toolbox/pid.hpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 62a5de79..6e66a85d 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -75,7 +75,11 @@ struct AntiwindupStrategy }; AntiwindupStrategy() - : type(UNDEFINED), i_min(0.0), i_max(0.0), trk_tc(0.0), legacy_antiwindup(false) + : type(UNDEFINED), + i_min(std::numeric_limits::quiet_NaN()), + i_max(std::numeric_limits::quiet_NaN()), + trk_tc(0.0), + legacy_antiwindup(false) { } @@ -120,15 +124,18 @@ struct AntiwindupStrategy "AntiwindupStrategy 'back_calculation' requires a valid positive tracking time constant " "(trk_tc)."); } - if (type == INTEGRATOR_CLAMPING && (i_min >= i_max)) + if ( + type == INTEGRATOR_CLAMPING && + ((i_min >= i_max) || !std::isfinite(i_min) || !std::isfinite(i_max))) { throw std::invalid_argument( - "AntiwindupStrategy 'integrator_clamping' requires i_min < i_max."); + "AntiwindupStrategy 'integrator_clamping' requires i_min < i_max and to be finite."); } - if (type == LEGACY && (i_min >= i_max)) + if (type == LEGACY && ((i_min >= i_max) || !std::isfinite(i_min) || !std::isfinite(i_max))) { // throw std::invalid_argument("AntiwindupStrategy 'legacy' requires i_min < i_max."); - std::cerr << "AntiwindupStrategy 'legacy' requires i_min < i_max." << std::endl; + std::cerr << "AntiwindupStrategy 'legacy' requires i_min < i_max and to be finite." + << std::endl; } } @@ -158,8 +165,8 @@ struct AntiwindupStrategy } Value type = UNDEFINED; - double i_min = 0.0; /**< Minimum allowable integral term. */ - double i_max = 0.0; /**< Maximum allowable integral term. */ + double i_min = std::numeric_limits::quiet_NaN(); /**< Minimum allowable integral term. */ + double i_max = std::numeric_limits::quiet_NaN(); /**< Maximum allowable integral term. */ bool legacy_antiwindup = false; /**< Use legacy anti-windup strategy. */ From 39d5325bf1a484881d5c489eeb108eee6f90f7ac Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 14:08:03 +0200 Subject: [PATCH 17/44] Add more checks and fix the missing .type --- control_toolbox/src/pid.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 2245babd..c25dab2b 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -232,7 +232,7 @@ void Pid::set_gains(const Gains & gains_in) { Gains gains = gains_in; - if (gains.antiwindup_strat_ == AntiwindupStrategy::BACK_CALCULATION) + if (gains.antiwindup_strat_.type == AntiwindupStrategy::BACK_CALCULATION) { if (is_zero(gains.antiwindup_strat_.trk_tc) && !is_zero(gains.d_gain_)) { @@ -336,17 +336,24 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) // Calculate derivative contribution to command d_term = gains.d_gain_ * d_error_; + if (gains.antiwindup_strat_.type == AntiwindupStrategy::UNDEFINED) + { + throw std::runtime_error("PID: Antiwindup strategy cannot be UNDEFINED. " + "Please set a valid antiwindup strategy."); + } + // Calculate integral contribution to command if ( - gains.antiwindup_strat_.legacy_antiwindup && - gains.antiwindup_strat_ == AntiwindupStrategy::LEGACY) + (gains.antiwindup_strat_.legacy_antiwindup && + gains.antiwindup_strat_.type == AntiwindupStrategy::LEGACY) || + gains.antiwindup_strat_.type == AntiwindupStrategy::INTEGRATOR_CLAMPING) { // 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 if ( !gains.antiwindup_strat_.legacy_antiwindup && - gains.antiwindup_strat_ == AntiwindupStrategy::LEGACY) + gains.antiwindup_strat_.type == AntiwindupStrategy::LEGACY) { i_term_ += gains.i_gain_ * dt_s * p_error_; } @@ -354,7 +361,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) // Compute the command if ( !gains.antiwindup_strat_.legacy_antiwindup && - gains.antiwindup_strat_ == AntiwindupStrategy::LEGACY) + gains.antiwindup_strat_.type == AntiwindupStrategy::LEGACY) { // Limit i_term so that the limit is meaningful in the output cmd_unsat_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; From 956578af07dda51b1b3962b50642ae7c3ec8385e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 14:09:55 +0200 Subject: [PATCH 18/44] disallow the undefined state of the antiwindup strategy --- control_toolbox/src/pid.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index c25dab2b..af5eccd3 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -228,6 +228,11 @@ void Pid::set_gains(const Gains & gains_in) { std::cout << "Received NaN for u_min or u_max, skipping new gains" << std::endl; } + else if (gains.antiwindup_strat_.type == AntiwindupStrategy::UNDEFINED) + { + std::cout << "PID: Antiwindup strategy cannot be UNDEFINED. " + << "Please set a valid antiwindup strategy." << std::endl; + } else { Gains gains = gains_in; @@ -338,8 +343,8 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) if (gains.antiwindup_strat_.type == AntiwindupStrategy::UNDEFINED) { - throw std::runtime_error("PID: Antiwindup strategy cannot be UNDEFINED. " - "Please set a valid antiwindup strategy."); + throw std::runtime_error( + "PID: Antiwindup strategy cannot be UNDEFINED. Please set a valid antiwindup strategy."); } // Calculate integral contribution to command From f4244eb635a1ce408992410d655e4cfa49ffe5af Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 14:14:53 +0200 Subject: [PATCH 19/44] fix typo --- control_toolbox/src/pid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index af5eccd3..52f0aa98 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -228,7 +228,7 @@ void Pid::set_gains(const Gains & gains_in) { std::cout << "Received NaN for u_min or u_max, skipping new gains" << std::endl; } - else if (gains.antiwindup_strat_.type == AntiwindupStrategy::UNDEFINED) + else if (gains_in.antiwindup_strat_.type == AntiwindupStrategy::UNDEFINED) { std::cout << "PID: Antiwindup strategy cannot be UNDEFINED. " << "Please set a valid antiwindup strategy." << std::endl; From cb4c4b670d5040115258e6bfc1f257368c7c477c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 16:41:30 +0200 Subject: [PATCH 20/44] Add more checks to validate --- control_toolbox/include/control_toolbox/pid.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 6e66a85d..7c31226e 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -137,6 +137,12 @@ struct AntiwindupStrategy std::cerr << "AntiwindupStrategy 'legacy' requires i_min < i_max and to be finite." << std::endl; } + if ( + type != NONE && type != UNDEFINED && type != LEGACY && type != INTEGRATOR_CLAMPING && + type != BACK_CALCULATION && type != CONDITIONAL_INTEGRATION) + { + throw std::runtime_error("AntiwindupStrategy has an invalid type."); + } } operator std::string() const { return to_string(); } From 1cae28a38867ce9dbd1d7428b91f0b22d7ab994e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 16:42:11 +0200 Subject: [PATCH 21/44] Add return value boolean to check if the return is valid value or not --- .../include/control_toolbox/pid.hpp | 16 +- .../include/control_toolbox/pid_ros.hpp | 16 +- control_toolbox/src/pid.cpp | 42 +++-- control_toolbox/src/pid_ros.cpp | 145 +++++++++++------- 4 files changed, 137 insertions(+), 82 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 7c31226e..e581d061 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -456,11 +456,11 @@ class Pid the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. - * + * \return True if all parameters are successfully set, False otherwise. * \note New gains are not applied if i_min_ > i_max_ */ [[deprecated("Use initialize with AntiwindupStrategy instead.")]] - void initialize( + bool initialize( double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! @@ -476,10 +476,11 @@ class Pid * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the 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 i_min_ > i_max_ or u_min > u_max */ - void initialize( + bool initialize( double p, double i, double d, double u_max, double u_min, const AntiwindupStrategy & antiwindup_strat); @@ -559,11 +560,12 @@ class Pid the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if i_min > i_max */ [[deprecated("Use set_gains with AntiwindupStrategy instead.")]] - void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + bool set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! * \brief Set PID gains for the controller. @@ -576,20 +578,22 @@ class Pid * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the 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 i_min_ > i_max_ or u_min > u_max */ - void set_gains( + bool set_gains( double p, double i, double d, double u_max, double u_min, const AntiwindupStrategy & antiwindup_strat); /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if gains.i_min_ > gains.i_max_ */ - void set_gains(const Gains & gains); + bool set_gains(const Gains & gains); /*! * \brief Set the PID error and compute the PID command with nonuniform time diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index 17464cc3..69324096 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -96,11 +96,12 @@ class PidROS the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if i_min_ > i_max_ */ [[deprecated("Use initialize_from_args with AntiwindupStrategy instead.")]] - void initialize_from_args( + bool initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup); /*! @@ -115,11 +116,12 @@ class PidROS integral contribution to the control output. i_max and i_min are applied in both scenarios. * \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 i_min_ > i_max_ */ [[deprecated("Use initialize_from_args with AntiwindupStrategy instead.")]] - void initialize_from_args( + bool initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term); /*! @@ -145,6 +147,7 @@ class PidROS /*! * \brief Initialize the PID controller based on already set parameters * \return True if all parameters are set (p, i, d, i_max, i_min, u_max, u_min and trk_tc), False otherwise + * \return False if the parameters are not set or if the parameters are invalid */ bool initialize_from_ros_parameters(); @@ -204,11 +207,12 @@ class PidROS the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if i_min > i_max */ [[deprecated("Use set_gains with AntiwindupStrategy instead.")]] - void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + bool set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! * \brief Set PID gains for the controller (preferred). @@ -221,20 +225,22 @@ class PidROS * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the 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 u_min_ > u_max_. */ - void set_gains( + bool set_gains( double p, double i, double d, double u_max, double u_min, const AntiwindupStrategy & antiwindup_strat); /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if gains.i_min_ > gains.i_max_ */ - void set_gains(const Pid::Gains & gains); + bool set_gains(const Pid::Gains & gains); /*! * \brief Set current command for this PID controller diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 52f0aa98..27e9d6c3 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -102,23 +102,29 @@ Pid::Pid(const Pid & source) Pid::~Pid() {} -void Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) +bool Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - set_gains(p, i, d, i_max, i_min, antiwindup); + if (set_gains(p, i, d, i_max, i_min, antiwindup)) + { + reset(); + return true; + } #pragma GCC diagnostic pop - - reset(); + return false; } -void Pid::initialize( +bool Pid::initialize( double p, double i, double d, double u_max, double u_min, const AntiwindupStrategy & antiwindup_strat) { - set_gains(p, i, d, u_max, u_min, antiwindup_strat); - - reset(); + if (set_gains(p, i, d, u_max, u_min, antiwindup_strat)) + { + reset(); + return true; + } + return false; } void Pid::reset() { reset(false); } @@ -179,24 +185,28 @@ Pid::Gains Pid::get_gains() { return *gains_buffer_.readFromRT(); } #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" -void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) +bool Pid::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { try { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" Gains gains(p, i, d, i_max, i_min, antiwindup); - set_gains(gains); #pragma GCC diagnostic pop + if (set_gains(gains)) + { + return true; + } } catch (const std::exception & e) { std::cerr << e.what() << '\n'; } + return false; } #pragma GCC diagnostic pop -void Pid::set_gains( +bool Pid::set_gains( double p, double i, double d, double u_max, double u_min, const AntiwindupStrategy & antiwindup_strat) { @@ -206,15 +216,19 @@ void Pid::set_gains( #pragma GCC diagnostic ignored "-Wdeprecated-declarations" Gains gains(p, i, d, u_max, u_min, antiwindup_strat); #pragma GCC diagnostic pop - set_gains(gains); + if (set_gains(gains)) + { + return true; + } } catch (const std::exception & e) { std::cerr << e.what() << '\n'; } + return false; } -void Pid::set_gains(const Gains & gains_in) +bool Pid::set_gains(const Gains & gains_in) { if (gains_in.i_min_ > gains_in.i_max_) { @@ -251,7 +265,9 @@ void Pid::set_gains(const Gains & gains_in) } } gains_buffer_.writeFromNonRT(gains); + return true; } + return false; } double Pid::compute_command(double error, const double & dt_s) diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 350ab69c..7999cfd7 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -265,9 +265,12 @@ bool PidROS::initialize_from_ros_parameters() return false; } - pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat); + if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat)) + { + return all_params_available; + } - return all_params_available; + return false; } void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValue param_value) @@ -278,7 +281,7 @@ void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValu } } -void PidROS::initialize_from_args( +bool PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup) { AntiwindupStrategy antiwindup_strat; @@ -293,16 +296,16 @@ void PidROS::initialize_from_args( catch (const std::runtime_error & e) { RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); - return; + return false; } #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - initialize_from_args(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, false); + return initialize_from_args(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, false); #pragma GCC diagnostic pop } -void PidROS::initialize_from_args( +bool PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term) { AntiwindupStrategy antiwindup_strat; @@ -317,10 +320,11 @@ void PidROS::initialize_from_args( catch (const std::runtime_error & e) { RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); - return; + return false; } - initialize_from_args(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, save_i_term); + return initialize_from_args( + p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, save_i_term); } bool PidROS::initialize_from_args( @@ -330,17 +334,19 @@ bool PidROS::initialize_from_args( if (u_min > u_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); - return false; } else if (std::isnan(u_min) || std::isnan(u_max)) { RCLCPP_ERROR(node_logging_->get_logger(), "u_min or u_max is NaN, skip new gains"); - return false; } else if (std::isnan(p) || std::isnan(i) || std::isnan(d)) { RCLCPP_ERROR(node_logging_->get_logger(), "p or i or d is NaN, skip new gains"); - return false; + } + else if (antiwindup_strat.type == AntiwindupStrategy::UNDEFINED) + { + RCLCPP_ERROR( + node_logging_->get_logger(), "Antiwindup strategy is set to 'undefined', skip new gains"); } else { @@ -351,36 +357,43 @@ bool PidROS::initialize_from_args( catch (const std::exception & e) { RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); - return false; } - pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat); - const Pid::Gains gains = pid_.get_gains(); - - declare_param(param_prefix_ + "p", rclcpp::ParameterValue(gains.p_gain_)); - declare_param(param_prefix_ + "i", rclcpp::ParameterValue(gains.i_gain_)); - declare_param(param_prefix_ + "d", rclcpp::ParameterValue(gains.d_gain_)); - declare_param( - param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(gains.antiwindup_strat_.i_max)); - declare_param( - param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(gains.antiwindup_strat_.i_min)); - declare_param(param_prefix_ + "u_clamp_max", rclcpp::ParameterValue(gains.u_max_)); - declare_param(param_prefix_ + "u_clamp_min", rclcpp::ParameterValue(gains.u_min_)); - declare_param( - param_prefix_ + "antiwindup", - rclcpp::ParameterValue(gains.antiwindup_strat_.legacy_antiwindup)); - // @todo(sai) add other parameters or optimize it - declare_param( - param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(antiwindup_strat.trk_tc)); - declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(true)); - declare_param( - param_prefix_ + "antiwindup_strategy", - rclcpp::ParameterValue(gains.antiwindup_strat_.to_string())); - declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); - - set_parameter_event_callback(); + if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat)) + { + const Pid::Gains gains = pid_.get_gains(); + + declare_param(param_prefix_ + "p", rclcpp::ParameterValue(gains.p_gain_)); + declare_param(param_prefix_ + "i", rclcpp::ParameterValue(gains.i_gain_)); + declare_param(param_prefix_ + "d", rclcpp::ParameterValue(gains.d_gain_)); + declare_param( + param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(gains.antiwindup_strat_.i_max)); + declare_param( + param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(gains.antiwindup_strat_.i_min)); + declare_param(param_prefix_ + "u_clamp_max", rclcpp::ParameterValue(gains.u_max_)); + declare_param(param_prefix_ + "u_clamp_min", rclcpp::ParameterValue(gains.u_min_)); + declare_param( + param_prefix_ + "antiwindup", + rclcpp::ParameterValue(gains.antiwindup_strat_.legacy_antiwindup)); + // @todo(sai) add other parameters or optimize it + declare_param( + param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(antiwindup_strat.trk_tc)); + declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(true)); + declare_param( + param_prefix_ + "antiwindup_strategy", + rclcpp::ParameterValue(gains.antiwindup_strat_.to_string())); + declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); + + set_parameter_event_callback(); + return true; + } + else + { + RCLCPP_ERROR(node_logging_->get_logger(), "Failed to initialize PID controller"); + return false; + } } - return true; + return false; } void PidROS::reset() @@ -413,7 +426,7 @@ double PidROS::compute_command(double error, double error_dot, const rclcpp::Dur Pid::Gains PidROS::get_gains() { return pid_.get_gains(); } -void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) +bool PidROS::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { AntiwindupStrategy antiwindup_strat; antiwindup_strat.type = AntiwindupStrategy::LEGACY; @@ -427,12 +440,12 @@ void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, catch (const std::runtime_error & e) { RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); - return; + return false; } - set_gains(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat); + return set_gains(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat); } -void PidROS::set_gains( +bool PidROS::set_gains( double p, double i, double d, double u_max, double u_min, const AntiwindupStrategy & antiwindup_strat) { @@ -449,26 +462,30 @@ void PidROS::set_gains( catch (const std::exception & e) { RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); - return; + return false; } - pid_.set_gains(p, i, d, u_max, u_min, antiwindup_strat); - const Pid::Gains gains = pid_.get_gains(); - // @todo(sai) add other parameters or optimize it - node_params_->set_parameters( - {rclcpp::Parameter(param_prefix_ + "p", gains.p_gain_), - rclcpp::Parameter(param_prefix_ + "i", gains.i_gain_), - rclcpp::Parameter(param_prefix_ + "d", gains.d_gain_), - rclcpp::Parameter(param_prefix_ + "u_clamp_max", gains.u_max_), - rclcpp::Parameter(param_prefix_ + "u_clamp_min", gains.u_min_), - rclcpp::Parameter(param_prefix_ + "tracking_time_constant", antiwindup_strat.trk_tc), - rclcpp::Parameter(param_prefix_ + "saturation", true), - rclcpp::Parameter( - param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string())}); + if (pid_.set_gains(p, i, d, u_max, u_min, antiwindup_strat)) + { + const Pid::Gains gains = pid_.get_gains(); + // @todo(sai) add other parameters or optimize it + node_params_->set_parameters( + {rclcpp::Parameter(param_prefix_ + "p", gains.p_gain_), + rclcpp::Parameter(param_prefix_ + "i", gains.i_gain_), + rclcpp::Parameter(param_prefix_ + "d", gains.d_gain_), + rclcpp::Parameter(param_prefix_ + "u_clamp_max", gains.u_max_), + rclcpp::Parameter(param_prefix_ + "u_clamp_min", gains.u_min_), + rclcpp::Parameter(param_prefix_ + "tracking_time_constant", antiwindup_strat.trk_tc), + rclcpp::Parameter(param_prefix_ + "saturation", true), + rclcpp::Parameter( + param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string())}); + return true; + } } + return false; } -void PidROS::set_gains(const Pid::Gains & gains) +bool PidROS::set_gains(const Pid::Gains & gains) { if (gains.i_min_ > gains.i_max_) { @@ -478,10 +495,22 @@ void PidROS::set_gains(const Pid::Gains & gains) { RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); } + else if (std::isnan(gains.u_min_) || std::isnan(gains.u_max_)) + { + RCLCPP_ERROR( + node_logging_->get_logger(), "Received NaN for u_min or u_max, skipping new gains"); + } + else if (gains.antiwindup_strat_.type == AntiwindupStrategy::UNDEFINED) + { + RCLCPP_ERROR( + node_logging_->get_logger(), + "PID: Antiwindup strategy cannot be UNDEFINED. Please set a valid antiwindup strategy."); + } else { - pid_.set_gains(gains); + return pid_.set_gains(gains); } + return false; } void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt) From 04f8242f7417ba7f06552e52deba0cfd498bb2e9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 21:59:36 +0200 Subject: [PATCH 22/44] Fix docs --- control_toolbox/include/control_toolbox/pid.hpp | 5 ----- control_toolbox/include/control_toolbox/pid_ros.hpp | 2 +- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index e581d061..f86a818d 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -233,9 +233,6 @@ inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) \param u_clamp Minimum and maximum bounds for the controller output. The clamp is applied to the \f$command\f$. - \param trk_tc Tracking time constant for the 'back_calculation' strategy. - - \section Usage To use the Pid class, you should first call some version of init() @@ -471,8 +468,6 @@ class Pid * \param d The derivative gain. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index 69324096..9afa63b3 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -146,7 +146,7 @@ class PidROS /*! * \brief Initialize the PID controller based on already set parameters - * \return True if all parameters are set (p, i, d, i_max, i_min, u_max, u_min and trk_tc), False otherwise + * \return True if all parameters are set (p, i, d, i_max, i_min, u_max, u_min), False otherwise * \return False if the parameters are not set or if the parameters are invalid */ bool initialize_from_ros_parameters(); From 13e489b8540d55bec54b427ae1d6b1c619990df5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 12 Jun 2025 23:10:34 +0200 Subject: [PATCH 23/44] Add `validate` method to validate the Gains and to reuse checks --- .../include/control_toolbox/pid.hpp | 61 ++++++-- control_toolbox/src/pid.cpp | 20 +-- control_toolbox/src/pid_ros.cpp | 136 ++++-------------- .../test/pid_ros_parameters_tests.cpp | 2 +- 4 files changed, 83 insertions(+), 136 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index f86a818d..84919da0 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -39,6 +39,7 @@ #include #include +#include "fmt/format.h" #include "rclcpp/duration.hpp" #include "realtime_tools/realtime_buffer.hpp" @@ -116,32 +117,37 @@ struct AntiwindupStrategy { if (type == UNDEFINED) { - throw std::runtime_error("AntiwindupStrategy is UNDEFINED. Please set a valid type."); + throw std::invalid_argument("AntiwindupStrategy is UNDEFINED. Please set a valid type"); } if (type == BACK_CALCULATION && (trk_tc < 0.0 || !std::isfinite(trk_tc))) { - throw std::runtime_error( + throw std::invalid_argument( "AntiwindupStrategy 'back_calculation' requires a valid positive tracking time constant " - "(trk_tc)."); + "(trk_tc)"); } if ( type == INTEGRATOR_CLAMPING && ((i_min >= i_max) || !std::isfinite(i_min) || !std::isfinite(i_max))) { throw std::invalid_argument( - "AntiwindupStrategy 'integrator_clamping' requires i_min < i_max and to be finite."); + fmt::format( + "AntiwindupStrategy 'integrator_clamping' requires i_min < i_max and to be finite " + "(i_min: {}, i_max: {})", + i_min, i_max)); } - if (type == LEGACY && ((i_min >= i_max) || !std::isfinite(i_min) || !std::isfinite(i_max))) + if (type == LEGACY && ((i_min > i_max) || !std::isfinite(i_min) || !std::isfinite(i_max))) { - // throw std::invalid_argument("AntiwindupStrategy 'legacy' requires i_min < i_max."); - std::cerr << "AntiwindupStrategy 'legacy' requires i_min < i_max and to be finite." - << std::endl; + 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 != NONE && type != UNDEFINED && type != LEGACY && type != INTEGRATOR_CLAMPING && type != BACK_CALCULATION && type != CONDITIONAL_INTEGRATION) { - throw std::runtime_error("AntiwindupStrategy has an invalid type."); + throw std::invalid_argument("AntiwindupStrategy has an invalid type"); } } @@ -288,7 +294,6 @@ class Pid antiwindup_strat_.i_max = i_max; antiwindup_strat_.i_min = i_min; antiwindup_strat_.legacy_antiwindup = false; - antiwindup_strat_.validate(); } /*! @@ -320,7 +325,6 @@ class Pid antiwindup_strat_.i_max = i_max; antiwindup_strat_.i_min = i_min; antiwindup_strat_.legacy_antiwindup = antiwindup; - antiwindup_strat_.validate(); } /*! @@ -362,6 +366,41 @@ 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_) + { + error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_); + return false; + } + else if (std::isnan(u_min_) || std::isnan(u_max_)) + { + error_msg = "Gains: u_min or u_max must not be NaN"; + return false; + } + else if (antiwindup_strat_.type == AntiwindupStrategy::UNDEFINED) + { + error_msg = + "Gains: Antiwindup strategy cannot be UNDEFINED. Please set a valid antiwindup strategy."; + return false; + } + try + { + antiwindup_strat_.validate(); + } + catch (const std::exception & e) + { + error_msg = e.what(); + return false; + } + return true; + } + // Default constructor [[deprecated( "Use constructor with AntiwindupStrategy only. The default constructor might be deleted in " diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 27e9d6c3..4a071915 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -61,7 +61,6 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwind antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.legacy_antiwindup = antiwindup; - antiwindup_strat.validate(); set_gains(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat); // Initialize saved i-term values @@ -230,22 +229,11 @@ bool Pid::set_gains( bool Pid::set_gains(const Gains & gains_in) { - if (gains_in.i_min_ > gains_in.i_max_) + std::string error_msg = ""; + if (!gains_in.validate(error_msg)) { - std::cout << "Received i_min > i_max, skip new gains" << std::endl; - } - else if (gains_in.u_min_ > gains_in.u_max_) - { - std::cout << "Received u_min > u_max, skip new gains" << std::endl; - } - else if (std::isnan(gains_in.u_min_) || std::isnan(gains_in.u_max_)) - { - std::cout << "Received NaN for u_min or u_max, skipping new gains" << std::endl; - } - else if (gains_in.antiwindup_strat_.type == AntiwindupStrategy::UNDEFINED) - { - std::cout << "PID: Antiwindup strategy cannot be UNDEFINED. " - << "Please set a valid antiwindup strategy." << std::endl; + std::cout << "PID: Invalid gains: " << error_msg << ". SKipping new gains." << std::endl; + return false; } else { diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 7999cfd7..ed49a297 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -241,7 +241,7 @@ bool PidROS::initialize_from_ros_parameters() set_parameter_event_callback(); } - if (antiwindup_strat_str == "none") + if (antiwindup_strat_str == "legacy") { std::cout << "Old anti-windup technique is deprecated. " "This option will be removed by the ROS 2 Kilted Kaiju release." @@ -259,7 +259,7 @@ bool PidROS::initialize_from_ros_parameters() { antiwindup_strat.validate(); } - catch (const std::runtime_error & e) + catch (const std::exception & e) { RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); return false; @@ -289,15 +289,6 @@ bool PidROS::initialize_from_args( antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.legacy_antiwindup = antiwindup; - try - { - antiwindup_strat.validate(); - } - catch (const std::runtime_error & e) - { - RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); - return false; - } #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" @@ -313,15 +304,6 @@ bool PidROS::initialize_from_args( antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.legacy_antiwindup = antiwindup; - try - { - antiwindup_strat.validate(); - } - catch (const std::runtime_error & e) - { - RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); - return false; - } return initialize_from_args( p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, save_i_term); @@ -331,34 +313,17 @@ bool PidROS::initialize_from_args( double p, double i, double d, double u_max, double u_min, const AntiwindupStrategy & antiwindup_strat, bool save_i_term) { - if (u_min > u_max) - { - RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); - } - else if (std::isnan(u_min) || std::isnan(u_max)) - { - RCLCPP_ERROR(node_logging_->get_logger(), "u_min or u_max is NaN, skip new gains"); - } - else if (std::isnan(p) || std::isnan(i) || std::isnan(d)) - { - RCLCPP_ERROR(node_logging_->get_logger(), "p or i or d is NaN, skip new gains"); - } - else if (antiwindup_strat.type == AntiwindupStrategy::UNDEFINED) + Pid::Gains verify_gains(p, i, d, u_max, u_min, antiwindup_strat); + std::string error_msg = ""; + if (!verify_gains.validate(error_msg)) { RCLCPP_ERROR( - node_logging_->get_logger(), "Antiwindup strategy is set to 'undefined', skip new gains"); + node_logging_->get_logger(), "Received invalid gains: %s. Skipping new gains.", + error_msg.c_str()); + return false; } else { - try - { - antiwindup_strat.validate(); - } - catch (const std::exception & e) - { - RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); - } - if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat)) { const Pid::Gains gains = pid_.get_gains(); @@ -433,15 +398,6 @@ 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; - try - { - antiwindup_strat.validate(); - } - catch (const std::runtime_error & e) - { - RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); - return false; - } return set_gains(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat); } @@ -449,33 +405,36 @@ bool PidROS::set_gains( double p, double i, double d, double u_max, double u_min, const AntiwindupStrategy & antiwindup_strat) { - if (u_min > u_max) + Pid::Gains gains(p, i, d, u_max, u_min, antiwindup_strat); + + return set_gains(gains); +} + +bool PidROS::set_gains(const Pid::Gains & gains) +{ + std::string error_msg = ""; + if (!gains.validate(error_msg)) { - RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + RCLCPP_ERROR( + node_logging_->get_logger(), "Received invalid gains: %s. Skipping new gains.", + error_msg.c_str()); + return false; } else { - try - { - antiwindup_strat.validate(); - } - catch (const std::exception & e) - { - RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); - return false; - } - - if (pid_.set_gains(p, i, d, u_max, u_min, antiwindup_strat)) + if (pid_.set_gains(gains)) { - const Pid::Gains gains = pid_.get_gains(); - // @todo(sai) add other parameters or optimize it node_params_->set_parameters( {rclcpp::Parameter(param_prefix_ + "p", gains.p_gain_), rclcpp::Parameter(param_prefix_ + "i", gains.i_gain_), rclcpp::Parameter(param_prefix_ + "d", gains.d_gain_), + rclcpp::Parameter(param_prefix_ + "i_clamp_max", gains.antiwindup_strat_.i_max), + rclcpp::Parameter(param_prefix_ + "i_clamp_min", gains.antiwindup_strat_.i_min), rclcpp::Parameter(param_prefix_ + "u_clamp_max", gains.u_max_), rclcpp::Parameter(param_prefix_ + "u_clamp_min", gains.u_min_), - rclcpp::Parameter(param_prefix_ + "tracking_time_constant", antiwindup_strat.trk_tc), + rclcpp::Parameter( + param_prefix_ + "tracking_time_constant", gains.antiwindup_strat_.trk_tc), + rclcpp::Parameter(param_prefix_ + "antiwindup", gains.antiwindup_strat_.legacy_antiwindup), rclcpp::Parameter(param_prefix_ + "saturation", true), rclcpp::Parameter( param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string())}); @@ -485,34 +444,6 @@ bool PidROS::set_gains( return false; } -bool PidROS::set_gains(const Pid::Gains & gains) -{ - if (gains.i_min_ > gains.i_max_) - { - RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); - } - else if (gains.u_min_ > gains.u_max_) - { - RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); - } - else if (std::isnan(gains.u_min_) || std::isnan(gains.u_max_)) - { - RCLCPP_ERROR( - node_logging_->get_logger(), "Received NaN for u_min or u_max, skipping new gains"); - } - else if (gains.antiwindup_strat_.type == AntiwindupStrategy::UNDEFINED) - { - RCLCPP_ERROR( - node_logging_->get_logger(), - "PID: Antiwindup strategy cannot be UNDEFINED. Please set a valid antiwindup strategy."); - } - else - { - return pid_.set_gains(gains); - } - return false; -} - void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt) { Pid::Gains gains = pid_.get_gains(); @@ -705,18 +636,7 @@ void PidROS::set_parameter_event_callback() if (changed) { - if (gains.i_min_ > gains.i_max_) - { - RCLCPP_ERROR(node_logging_->get_logger(), "Received i_min > i_max, skip new gains"); - } - else if (gains.u_min_ > gains.u_max_) - { - RCLCPP_ERROR(node_logging_->get_logger(), "Received u_min > u_max, skip new gains"); - } - else - { - pid_.set_gains(gains); - } + pid_.set_gains(gains); } return result; diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 7a413ef7..09f38623 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -595,7 +595,7 @@ TEST(PidParametersTest, GetParametersFromParams) TestablePidROS pid(node); - ASSERT_TRUE(pid.initialize_from_ros_parameters()); + ASSERT_FALSE(pid.initialize_from_ros_parameters()); rclcpp::Parameter param_p; ASSERT_TRUE(node->get_parameter("p", param_p)); From e6d311d60b380a3992ea62c6ab8e00b6288bdf44 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 15 Jun 2025 02:09:22 +0200 Subject: [PATCH 24/44] Rename AntiwindupStrategy to AntiWindupStrategy --- .../include/control_toolbox/pid.hpp | 44 +++++++------- .../include/control_toolbox/pid_ros.hpp | 10 ++-- control_toolbox/src/pid.cpp | 32 +++++----- control_toolbox/src/pid_ros.cpp | 20 +++---- .../test/pid_ros_parameters_tests.cpp | 42 +++++++------- .../test/pid_ros_publisher_tests.cpp | 10 ++-- control_toolbox/test/pid_tests.cpp | 58 +++++++++---------- 7 files changed, 108 insertions(+), 108 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 84919da0..9a03cd06 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -62,7 +62,7 @@ namespace control_toolbox integral contribution to the control output. i_max and i_min are applied in both scenarios. */ -struct AntiwindupStrategy +struct AntiWindupStrategy { public: enum Value : int8_t @@ -75,7 +75,7 @@ struct AntiwindupStrategy CONDITIONAL_INTEGRATION }; - AntiwindupStrategy() + AntiWindupStrategy() : type(UNDEFINED), i_min(std::numeric_limits::quiet_NaN()), i_max(std::numeric_limits::quiet_NaN()), @@ -117,12 +117,12 @@ struct AntiwindupStrategy { if (type == UNDEFINED) { - throw std::invalid_argument("AntiwindupStrategy is UNDEFINED. Please set a valid type"); + throw std::invalid_argument("AntiWindupStrategy is UNDEFINED. Please set a valid type"); } if (type == BACK_CALCULATION && (trk_tc < 0.0 || !std::isfinite(trk_tc))) { throw std::invalid_argument( - "AntiwindupStrategy 'back_calculation' requires a valid positive tracking time constant " + "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant " "(trk_tc)"); } if ( @@ -131,7 +131,7 @@ struct AntiwindupStrategy { throw std::invalid_argument( fmt::format( - "AntiwindupStrategy 'integrator_clamping' requires i_min < i_max and to be finite " + "AntiWindupStrategy 'integrator_clamping' requires i_min < i_max and to be finite " "(i_min: {}, i_max: {})", i_min, i_max)); } @@ -139,7 +139,7 @@ struct AntiwindupStrategy { throw std::invalid_argument( fmt::format( - "AntiwindupStrategy 'legacy' requires i_min < i_max and to be finite (i_min: {}, i_max: " + "AntiWindupStrategy 'legacy' requires i_min < i_max and to be finite (i_min: {}, i_max: " "{})", i_min, i_max)); } @@ -147,7 +147,7 @@ struct AntiwindupStrategy type != NONE && type != UNDEFINED && type != LEGACY && type != INTEGRATOR_CLAMPING && type != BACK_CALCULATION && type != CONDITIONAL_INTEGRATION) { - throw std::invalid_argument("AntiwindupStrategy has an invalid type"); + throw std::invalid_argument("AntiWindupStrategy has an invalid type"); } } @@ -279,7 +279,7 @@ class Pid * \param i_min Lower integral clamp. * */ - [[deprecated("Use constructor with AntiwindupStrategy instead.")]] + [[deprecated("Use constructor with AntiWindupStrategy instead.")]] Gains(double p, double i, double d, double i_max, double i_min) : p_gain_(p), i_gain_(i), @@ -290,7 +290,7 @@ class Pid u_min_(-std::numeric_limits::infinity()), antiwindup_(false) { - antiwindup_strat_.type = AntiwindupStrategy::LEGACY; + antiwindup_strat_.type = AntiWindupStrategy::LEGACY; antiwindup_strat_.i_max = i_max; antiwindup_strat_.i_min = i_min; antiwindup_strat_.legacy_antiwindup = false; @@ -310,7 +310,7 @@ class Pid i_min are applied in both scenarios. * */ - [[deprecated("Use constructor with AntiwindupStrategy instead.")]] + [[deprecated("Use constructor with AntiWindupStrategy instead.")]] Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) : p_gain_(p), i_gain_(i), @@ -321,7 +321,7 @@ class Pid u_min_(-std::numeric_limits::infinity()), antiwindup_(antiwindup) { - antiwindup_strat_.type = AntiwindupStrategy::LEGACY; + antiwindup_strat_.type = AntiWindupStrategy::LEGACY; antiwindup_strat_.i_max = i_max; antiwindup_strat_.i_min = i_min; antiwindup_strat_.legacy_antiwindup = antiwindup; @@ -342,7 +342,7 @@ class Pid */ Gains( double p, double i, double d, double u_max, double u_min, - const AntiwindupStrategy & antiwindup_strat) + const AntiWindupStrategy & antiwindup_strat) : p_gain_(p), i_gain_(i), d_gain_(d), @@ -383,7 +383,7 @@ class Pid error_msg = "Gains: u_min or u_max must not be NaN"; return false; } - else if (antiwindup_strat_.type == AntiwindupStrategy::UNDEFINED) + else if (antiwindup_strat_.type == AntiWindupStrategy::UNDEFINED) { error_msg = "Gains: Antiwindup strategy cannot be UNDEFINED. Please set a valid antiwindup strategy."; @@ -403,7 +403,7 @@ class Pid // Default constructor [[deprecated( - "Use constructor with AntiwindupStrategy only. The default constructor might be deleted in " + "Use constructor with AntiWindupStrategy only. The default constructor might be deleted in " "future")]] Gains() { } @@ -427,7 +427,7 @@ class Pid double u_min_ = -std::numeric_limits::infinity(); /**< Minimum allowable output. */ [[deprecated("Use antiwindup_strat_ instead.")]] bool antiwindup_ = false; /**< Anti-windup. */ - AntiwindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ + AntiWindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ }; /*! @@ -446,7 +446,7 @@ class Pid * * \throws An std::invalid_argument exception is thrown if i_min > i_max */ - [[deprecated("Use constructor with AntiwindupStrategy only.")]] + [[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); @@ -467,7 +467,7 @@ class Pid */ Pid( double p, double i, double d, double u_max, double u_min, - const AntiwindupStrategy & antiwindup_strat); + const AntiWindupStrategy & antiwindup_strat); /*! * \brief Copy constructor required for preventing mutexes from being copied @@ -495,7 +495,7 @@ class Pid * \return True if all parameters are successfully set, False otherwise. * \note New gains are not applied if i_min_ > i_max_ */ - [[deprecated("Use initialize with AntiwindupStrategy instead.")]] + [[deprecated("Use initialize with AntiWindupStrategy instead.")]] bool initialize( double p, double i, double d, double i_max, double i_min, bool antiwindup = false); @@ -516,7 +516,7 @@ class Pid */ bool initialize( double p, double i, double d, double u_max, double u_min, - const AntiwindupStrategy & antiwindup_strat); + const AntiWindupStrategy & antiwindup_strat); /*! * \brief Reset the state of this PID controller @@ -575,7 +575,7 @@ class Pid */ void get_gains( double & p, double & i, double & d, double & u_max, double & u_min, - AntiwindupStrategy & antiwindup_strat); + AntiWindupStrategy & antiwindup_strat); /*! * \brief Get PID gains for the controller. @@ -598,7 +598,7 @@ class Pid * * \note New gains are not applied if i_min > i_max */ - [[deprecated("Use set_gains with AntiwindupStrategy instead.")]] + [[deprecated("Use set_gains with AntiWindupStrategy instead.")]] bool set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! @@ -618,7 +618,7 @@ class Pid */ bool set_gains( double p, double i, double d, double u_max, double u_min, - const AntiwindupStrategy & antiwindup_strat); + const AntiWindupStrategy & antiwindup_strat); /*! * \brief Set PID gains for the controller. diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index 9afa63b3..3cd06578 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -100,7 +100,7 @@ class PidROS * * \note New gains are not applied if i_min_ > i_max_ */ - [[deprecated("Use initialize_from_args with AntiwindupStrategy instead.")]] + [[deprecated("Use initialize_from_args with AntiWindupStrategy instead.")]] bool initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup); @@ -120,7 +120,7 @@ class PidROS * * \note New gains are not applied if i_min_ > i_max_ */ - [[deprecated("Use initialize_from_args with AntiwindupStrategy instead.")]] + [[deprecated("Use initialize_from_args with AntiWindupStrategy instead.")]] bool initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term); @@ -142,7 +142,7 @@ class PidROS */ bool initialize_from_args( double p, double i, double d, double u_max, double u_min, - const AntiwindupStrategy & antiwindup_strat, bool save_i_term); + const AntiWindupStrategy & antiwindup_strat, bool save_i_term); /*! * \brief Initialize the PID controller based on already set parameters @@ -211,7 +211,7 @@ class PidROS * * \note New gains are not applied if i_min > i_max */ - [[deprecated("Use set_gains with AntiwindupStrategy instead.")]] + [[deprecated("Use set_gains with AntiWindupStrategy instead.")]] bool set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! @@ -231,7 +231,7 @@ class PidROS */ bool set_gains( double p, double i, double d, double u_max, double u_min, - const AntiwindupStrategy & antiwindup_strat); + const AntiWindupStrategy & antiwindup_strat); /*! * \brief Set PID gains for the controller. diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 4a071915..723587ad 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -56,8 +56,8 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwind { throw std::invalid_argument("received i_min > i_max"); } - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.legacy_antiwindup = antiwindup; @@ -72,7 +72,7 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwind Pid::Pid( double p, double i, double d, double u_max, double u_min, - const AntiwindupStrategy & antiwindup_strat) + const AntiWindupStrategy & antiwindup_strat) : gains_buffer_() { if (u_min > u_max) @@ -116,7 +116,7 @@ bool Pid::initialize(double p, double i, double d, double i_max, double i_min, b bool Pid::initialize( double p, double i, double d, double u_max, double u_min, - const AntiwindupStrategy & antiwindup_strat) + const AntiWindupStrategy & antiwindup_strat) { if (set_gains(p, i, d, u_max, u_min, antiwindup_strat)) { @@ -148,7 +148,7 @@ void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & { double u_max; double u_min; - AntiwindupStrategy antiwindup_strat; + AntiWindupStrategy antiwindup_strat; get_gains(p, i, d, u_max, u_min, antiwindup_strat); i_max = antiwindup_strat.i_max; i_min = antiwindup_strat.i_min; @@ -159,7 +159,7 @@ void Pid::get_gains( { double u_max; double u_min; - AntiwindupStrategy antiwindup_strat; + AntiWindupStrategy antiwindup_strat; get_gains(p, i, d, u_max, u_min, antiwindup_strat); i_max = antiwindup_strat.i_max; i_min = antiwindup_strat.i_min; @@ -168,7 +168,7 @@ void Pid::get_gains( void Pid::get_gains( double & p, double & i, double & d, double & u_max, double & u_min, - AntiwindupStrategy & antiwindup_strat) + AntiWindupStrategy & antiwindup_strat) { Gains gains = *gains_buffer_.readFromRT(); @@ -207,7 +207,7 @@ bool Pid::set_gains(double p, double i, double d, double i_max, double i_min, bo bool Pid::set_gains( double p, double i, double d, double u_max, double u_min, - const AntiwindupStrategy & antiwindup_strat) + const AntiWindupStrategy & antiwindup_strat) { try { @@ -239,7 +239,7 @@ bool Pid::set_gains(const Gains & gains_in) { Gains gains = gains_in; - if (gains.antiwindup_strat_.type == AntiwindupStrategy::BACK_CALCULATION) + if (gains.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION) { if (is_zero(gains.antiwindup_strat_.trk_tc) && !is_zero(gains.d_gain_)) { @@ -345,7 +345,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) // Calculate derivative contribution to command d_term = gains.d_gain_ * d_error_; - if (gains.antiwindup_strat_.type == AntiwindupStrategy::UNDEFINED) + if (gains.antiwindup_strat_.type == AntiWindupStrategy::UNDEFINED) { throw std::runtime_error( "PID: Antiwindup strategy cannot be UNDEFINED. Please set a valid antiwindup strategy."); @@ -354,15 +354,15 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) // Calculate integral contribution to command if ( (gains.antiwindup_strat_.legacy_antiwindup && - gains.antiwindup_strat_.type == AntiwindupStrategy::LEGACY) || - gains.antiwindup_strat_.type == AntiwindupStrategy::INTEGRATOR_CLAMPING) + gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY) || + gains.antiwindup_strat_.type == AntiWindupStrategy::INTEGRATOR_CLAMPING) { // 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 if ( !gains.antiwindup_strat_.legacy_antiwindup && - gains.antiwindup_strat_.type == AntiwindupStrategy::LEGACY) + gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY) { i_term_ += gains.i_gain_ * dt_s * p_error_; } @@ -370,7 +370,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) // Compute the command if ( !gains.antiwindup_strat_.legacy_antiwindup && - gains.antiwindup_strat_.type == AntiwindupStrategy::LEGACY) + gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY) { // Limit i_term so that the limit is meaningful in the output cmd_unsat_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; @@ -398,12 +398,12 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) } if ( - gains.antiwindup_strat_.type == AntiwindupStrategy::BACK_CALCULATION && !is_zero(gains.i_gain_)) + gains.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && !is_zero(gains.i_gain_)) { i_term_ += dt_s * (gains.i_gain_ * error + 1 / gains.antiwindup_strat_.trk_tc * (cmd_ - cmd_unsat_)); } - else if (gains.antiwindup_strat_.type == AntiwindupStrategy::CONDITIONAL_INTEGRATION) + else if (gains.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION) { if (!(!iszero(cmd_unsat_ - cmd_) && error * cmd_unsat_ > 0)) { diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index ed49a297..7cebbc95 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -248,7 +248,7 @@ bool PidROS::initialize_from_ros_parameters() << std::endl; } - AntiwindupStrategy antiwindup_strat; + AntiWindupStrategy antiwindup_strat; antiwindup_strat.set_type(antiwindup_strat_str); antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; @@ -284,8 +284,8 @@ void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValu bool PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup) { - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.legacy_antiwindup = antiwindup; @@ -299,8 +299,8 @@ bool PidROS::initialize_from_args( bool PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term) { - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.legacy_antiwindup = antiwindup; @@ -311,7 +311,7 @@ bool PidROS::initialize_from_args( bool PidROS::initialize_from_args( double p, double i, double d, double u_max, double u_min, - const AntiwindupStrategy & antiwindup_strat, bool save_i_term) + const AntiWindupStrategy & antiwindup_strat, bool save_i_term) { Pid::Gains verify_gains(p, i, d, u_max, u_min, antiwindup_strat); std::string error_msg = ""; @@ -393,8 +393,8 @@ Pid::Gains PidROS::get_gains() { return pid_.get_gains(); } bool PidROS::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.legacy_antiwindup = antiwindup; @@ -403,7 +403,7 @@ bool PidROS::set_gains(double p, double i, double d, double i_max, double i_min, bool PidROS::set_gains( double p, double i, double d, double u_max, double u_min, - const AntiwindupStrategy & antiwindup_strat) + const AntiWindupStrategy & antiwindup_strat) { Pid::Gains gains(p, i, d, u_max, u_min, antiwindup_strat); @@ -624,7 +624,7 @@ void PidROS::set_parameter_event_callback() else if (param_name == param_prefix_ + "antiwindup_strategy") { // @todo decide if this can be changed in the first place - // gains.antiwindup_strat_ = AntiwindupStrategy(parameter.get_value()); + // gains.antiwindup_strat_ = AntiWindupStrategy(parameter.get_value()); changed = true; } } diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 09f38623..38b5fa93 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -24,7 +24,7 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" -using control_toolbox::AntiwindupStrategy; +using control_toolbox::AntiWindupStrategy; using rclcpp::executors::MultiThreadedExecutor; class TestablePidROS : public control_toolbox::PidROS @@ -63,8 +63,8 @@ void check_set_parameters( const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - AntiwindupStrategy ANTIWINDUP_STRAT; - ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.trk_tc = TRK_TC; @@ -123,7 +123,7 @@ void check_set_parameters( ASSERT_EQ(gains.u_min_, U_MIN); ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_TRUE(gains.antiwindup_); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::LEGACY); + ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } TEST(PidParametersTest, InitPidTest) @@ -153,8 +153,8 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double U_MIN_BAD = 10.0; const double TRK_TC = 4.0; - AntiwindupStrategy ANTIWINDUP_STRAT; - ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX_BAD; ANTIWINDUP_STRAT.i_min = I_MIN_BAD; ANTIWINDUP_STRAT.trk_tc = TRK_TC; @@ -188,7 +188,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.trk_tc, 0.0); ASSERT_FALSE(gains.antiwindup_); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::LEGACY); + ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot) @@ -273,8 +273,8 @@ TEST(PidParametersTest, SetParametersTest) const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - AntiwindupStrategy ANTIWINDUP_STRAT; - ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.trk_tc = TRK_TC; @@ -331,7 +331,7 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.u_min_, U_MIN); ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::LEGACY); + ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } TEST(PidParametersTest, SetBadParametersTest) @@ -355,8 +355,8 @@ TEST(PidParametersTest, SetBadParametersTest) const bool SATURATION = false; const bool ANTIWINDUP = true; - AntiwindupStrategy ANTIWINDUP_STRAT; - ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.trk_tc = TRK_TC; @@ -411,7 +411,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::LEGACY); + ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); // Set the good gains @@ -434,7 +434,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::LEGACY); + ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); // Now re-enabling it should have the old gains back ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", true))); @@ -454,7 +454,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(updated_gains.u_min_, U_MIN); ASSERT_EQ(updated_gains.antiwindup_strat_.trk_tc, TRK_TC); ASSERT_EQ(updated_gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(updated_gains.antiwindup_strat_, AntiwindupStrategy::LEGACY); + ASSERT_EQ(updated_gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } TEST(PidParametersTest, GetParametersTest) @@ -475,8 +475,8 @@ TEST(PidParametersTest, GetParametersTest) const bool SATURATION = true; const bool ANTIWINDUP = true; - AntiwindupStrategy ANTIWINDUP_STRAT; - ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.trk_tc = TRK_TC; @@ -539,8 +539,8 @@ TEST(PidParametersTest, GetParametersTest) const double TRK_TC = 4.0; const bool ANTIWINDUP = true; - AntiwindupStrategy ANTIWINDUP_STRAT; - ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.trk_tc = TRK_TC; @@ -645,8 +645,8 @@ TEST(PidParametersTest, MultiplePidInstances) const double U_MAX = 10.0; const double U_MIN = -10.0; const double TRK_TC = 4.0; - AntiwindupStrategy ANTIWINDUP_STRAT; - ANTIWINDUP_STRAT.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.trk_tc = TRK_TC; diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index 2d44d42e..d8e3f4ac 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -30,7 +30,7 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" -using control_toolbox::AntiwindupStrategy; +using control_toolbox::AntiWindupStrategy; using PidStateMsg = control_msgs::msg::PidState; using rclcpp::executors::MultiThreadedExecutor; @@ -43,8 +43,8 @@ TEST(PidPublisherTest, PublishTest) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.legacy_antiwindup = false; @@ -86,8 +86,8 @@ TEST(PidPublisherTest, PublishTestLifecycle) std::dynamic_pointer_cast>( pid_ros.get_pid_state_publisher()); - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.legacy_antiwindup = false; diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index d5d2a9d3..afd556bf 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -41,7 +41,7 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" -using control_toolbox::AntiwindupStrategy; +using control_toolbox::AntiWindupStrategy; using control_toolbox::Pid; using namespace std::chrono_literals; @@ -52,9 +52,9 @@ TEST(ParameterTest, UTermBadIBoundsTestConstructor) "This test checks if an error is thrown for bad u_bounds specification (i.e. u_min > u_max)."); // Pid(double p, double i, double d, double u_max, double u_min, - // AntiwindupStrategy antiwindup_strat); - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::LEGACY; + // AntiWindupStrategy antiwindup_strat); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; EXPECT_THROW(Pid pid(1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat), std::invalid_argument); @@ -67,9 +67,9 @@ TEST(ParameterTest, UTermBadIBoundsTest) "This test checks if gains remain for bad u_bounds specification (i.e. u_min > u_max)."); // Pid(double p, double i, double d, double u_max, double u_min, - // AntiwindupStrategy antiwindup_strat); - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::LEGACY; + // AntiWindupStrategy antiwindup_strat); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat); @@ -89,9 +89,9 @@ TEST(ParameterTest, outputClampTest) "description", "This test succeeds if the output is clamped when the saturation is active."); // Pid(double p, double i, double d, double u_max, double u_min, - // AntiwindupStrategy antiwindup_strat); - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::BACK_CALCULATION; + // AntiWindupStrategy antiwindup_strat); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; antiwindup_strat.trk_tc = 0.0; // Set to 0.0 to use the default value // Setting u_max = 1.0 and u_min = -1.0 to test clamping Pid pid(1.0, 0.0, 0.0, 1.0, -1.0, antiwindup_strat); @@ -145,9 +145,9 @@ TEST(ParameterTest, noOutputClampTest) "description", "This test succeeds if the output isn't clamped when the saturation is false."); // Pid(double p, double i, double d, double u_max, double u_min, - // AntiwindupStrategy antiwindup_strat); - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::BACK_CALCULATION; + // AntiWindupStrategy antiwindup_strat); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; antiwindup_strat.trk_tc = 0.0; // Set to 0.0 to use the default value // Setting u_max = INF and u_min = -INF to disable clamping Pid pid( @@ -205,9 +205,9 @@ TEST(ParameterTest, integrationBackCalculationZeroGainTest) "the back calculation technique."); // Pid(double p, double i, double d, double u_max, double u_min, - // AntiwindupStrategy antiwindup_strat); - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::BACK_CALCULATION; + // AntiWindupStrategy antiwindup_strat); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; antiwindup_strat.trk_tc = 0.0; // Set to 0.0 to use the default value Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, antiwindup_strat); @@ -255,9 +255,9 @@ TEST(ParameterTest, integrationConditionalIntegrationZeroGainTest) "the conditional integration technique."); // Pid(double p, double i, double d, double u_max, double u_min, - // AntiwindupStrategy antiwindup_strat); - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::CONDITIONAL_INTEGRATION; + // AntiWindupStrategy antiwindup_strat); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, antiwindup_strat); double cmd = 0.0; @@ -443,8 +443,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double u_min = -1 * u_max; double trk_tc = std::rand() % 100; bool antiwindup = false; - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::LEGACY; + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.trk_tc = trk_tc; @@ -457,7 +457,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, u_min_return, trk_tc_return; bool antiwindup_return; - AntiwindupStrategy antiwindup_strat_return; + AntiWindupStrategy antiwindup_strat_return; pid1.get_gains( p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, @@ -486,7 +486,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) u_min = -1 * u_max; trk_tc = std::rand() % 100; antiwindup = false; - antiwindup_strat.type = AntiwindupStrategy::LEGACY; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.trk_tc = trk_tc; @@ -751,10 +751,10 @@ TEST(CommandTest, backCalculationPIDTest) "back calculation technique."); // Pid(double p, double i, double d, double u_max, double u_min, - // AntiwindupStrategy antiwindup_strat); + // AntiWindupStrategy antiwindup_strat); // Setting u_max = 5.0 and u_min = -5.0 to test clamping - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::BACK_CALCULATION; + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; antiwindup_strat.trk_tc = 1.0; // Set to 0.0 to use the default value Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); @@ -812,10 +812,10 @@ TEST(CommandTest, conditionalIntegrationPIDTest) "conditional integration technique."); // Pid(double p, double i, double d, double u_max, double u_min, - // AntiwindupStrategy antiwindup_strat); + // AntiWindupStrategy antiwindup_strat); // Setting u_max = 5.0 and u_min = -5.0 to test clamping - AntiwindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiwindupStrategy::CONDITIONAL_INTEGRATION; + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; antiwindup_strat.trk_tc = 1.0; Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); From 0cb8dbce237c9ed8d83731ceed604e6d65ef9353 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 15 Jun 2025 02:15:18 +0200 Subject: [PATCH 25/44] Rename trk_tc to tracking_time_constant --- .../include/control_toolbox/pid.hpp | 17 ++++++----- control_toolbox/src/pid.cpp | 12 ++++---- control_toolbox/src/pid_ros.cpp | 19 +++++++------ .../test/pid_ros_parameters_tests.cpp | 26 ++++++++--------- .../test/pid_ros_publisher_tests.cpp | 4 +-- control_toolbox/test/pid_tests.cpp | 28 +++++++++---------- 6 files changed, 56 insertions(+), 50 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 9a03cd06..d1f256d8 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -55,7 +55,7 @@ namespace control_toolbox * \param i_min Lower integral clamp. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set + * \param tracking_time_constant Specifies the tracking time constant for the 'back_calculation' strategy. If set * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param legacy_antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the @@ -79,7 +79,7 @@ struct AntiWindupStrategy : type(UNDEFINED), i_min(std::numeric_limits::quiet_NaN()), i_max(std::numeric_limits::quiet_NaN()), - trk_tc(0.0), + tracking_time_constant(0.0), legacy_antiwindup(false) { } @@ -119,11 +119,13 @@ struct AntiWindupStrategy { throw std::invalid_argument("AntiWindupStrategy is UNDEFINED. Please set a valid type"); } - if (type == BACK_CALCULATION && (trk_tc < 0.0 || !std::isfinite(trk_tc))) + if ( + type == BACK_CALCULATION && + (tracking_time_constant < 0.0 || !std::isfinite(tracking_time_constant))) { throw std::invalid_argument( "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant " - "(trk_tc)"); + "(tracking_time_constant)"); } if ( type == INTEGRATOR_CLAMPING && @@ -182,9 +184,10 @@ struct AntiWindupStrategy bool legacy_antiwindup = false; /**< Use legacy anti-windup strategy. */ - // trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. - // If set to 0.0 when this strategy is selected, a recommended default value will be applied. - double trk_tc = 0.0; /**< Tracking time constant for back_calculation strategy. */ + // tracking_time_constant Specifies the tracking time constant for the 'back_calculation' + // strategy. If set to 0.0 when this strategy is selected, a recommended default value + // will be applied. + double tracking_time_constant = 0.0; /**< Tracking time constant for back_calculation strategy. */ }; template diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 723587ad..6c6740fb 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -241,15 +241,15 @@ bool Pid::set_gains(const Gains & gains_in) if (gains.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION) { - if (is_zero(gains.antiwindup_strat_.trk_tc) && !is_zero(gains.d_gain_)) + if (is_zero(gains.antiwindup_strat_.tracking_time_constant) && !is_zero(gains.d_gain_)) { // Default value for tracking time constant for back calculation technique - gains.antiwindup_strat_.trk_tc = std::sqrt(gains.d_gain_ / gains.i_gain_); + gains.antiwindup_strat_.tracking_time_constant = std::sqrt(gains.d_gain_ / gains.i_gain_); } - else if (is_zero(gains.antiwindup_strat_.trk_tc) && is_zero(gains.d_gain_)) + else if (is_zero(gains.antiwindup_strat_.tracking_time_constant) && is_zero(gains.d_gain_)) { // Default value for tracking time constant for back calculation technique - gains.antiwindup_strat_.trk_tc = gains.p_gain_ / gains.i_gain_; + gains.antiwindup_strat_.tracking_time_constant = gains.p_gain_ / gains.i_gain_; } } gains_buffer_.writeFromNonRT(gains); @@ -400,8 +400,8 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) if ( gains.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && !is_zero(gains.i_gain_)) { - i_term_ += - dt_s * (gains.i_gain_ * error + 1 / gains.antiwindup_strat_.trk_tc * (cmd_ - cmd_unsat_)); + i_term_ += dt_s * (gains.i_gain_ * error + + 1 / gains.antiwindup_strat_.tracking_time_constant * (cmd_ - cmd_unsat_)); } else if (gains.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION) { diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 7cebbc95..47cf4dcb 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -215,8 +215,8 @@ bool PidROS::get_string_param(const std::string & param_name, std::string & valu bool PidROS::initialize_from_ros_parameters() { - double p, i, d, i_max, i_min, u_max, u_min, trk_tc; - p = i = d = i_max = i_min = trk_tc = std::numeric_limits::quiet_NaN(); + double p, i, d, i_max, i_min, u_max, u_min, tracking_time_constant; + p = i = d = i_max = i_min = tracking_time_constant = std::numeric_limits::quiet_NaN(); u_max = UMAX_INFINITY; u_min = -UMAX_INFINITY; bool antiwindup = false; @@ -230,7 +230,8 @@ bool PidROS::initialize_from_ros_parameters() all_params_available &= get_double_param(param_prefix_ + "i_clamp_min", i_min); all_params_available &= get_double_param(param_prefix_ + "u_clamp_max", u_max); all_params_available &= get_double_param(param_prefix_ + "u_clamp_min", u_min); - all_params_available &= get_double_param(param_prefix_ + "tracking_time_constant", trk_tc); + all_params_available &= + get_double_param(param_prefix_ + "tracking_time_constant", tracking_time_constant); get_boolean_param(param_prefix_ + "antiwindup", antiwindup); get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat_str); @@ -252,7 +253,7 @@ bool PidROS::initialize_from_ros_parameters() antiwindup_strat.set_type(antiwindup_strat_str); antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; - antiwindup_strat.trk_tc = trk_tc; + antiwindup_strat.tracking_time_constant = tracking_time_constant; antiwindup_strat.legacy_antiwindup = antiwindup; try @@ -342,7 +343,8 @@ bool PidROS::initialize_from_args( rclcpp::ParameterValue(gains.antiwindup_strat_.legacy_antiwindup)); // @todo(sai) add other parameters or optimize it declare_param( - param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(antiwindup_strat.trk_tc)); + param_prefix_ + "tracking_time_constant", + rclcpp::ParameterValue(antiwindup_strat.tracking_time_constant)); declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(true)); declare_param( param_prefix_ + "antiwindup_strategy", @@ -433,7 +435,8 @@ bool PidROS::set_gains(const Pid::Gains & gains) rclcpp::Parameter(param_prefix_ + "u_clamp_max", gains.u_max_), rclcpp::Parameter(param_prefix_ + "u_clamp_min", gains.u_min_), rclcpp::Parameter( - param_prefix_ + "tracking_time_constant", gains.antiwindup_strat_.trk_tc), + param_prefix_ + "tracking_time_constant", + gains.antiwindup_strat_.tracking_time_constant), rclcpp::Parameter(param_prefix_ + "antiwindup", gains.antiwindup_strat_.legacy_antiwindup), rclcpp::Parameter(param_prefix_ + "saturation", true), rclcpp::Parameter( @@ -497,7 +500,7 @@ void PidROS::print_values() << " I Min: " << gains.i_min_ << "\n" << " U_Max: " << gains.u_max_ << "\n" << " U_Min: " << gains.u_min_ << "\n" - << " Tracking_Time_Constant: " << gains.antiwindup_strat_.trk_tc << "\n" + << " Tracking_Time_Constant: " << gains.antiwindup_strat_.tracking_time_constant << "\n" << " Antiwindup: " << gains.antiwindup_strat_.legacy_antiwindup << "\n" << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\n" << "\n" @@ -613,7 +616,7 @@ void PidROS::set_parameter_event_callback() } else if (param_name == param_prefix_ + "tracking_time_constant") { - gains.antiwindup_strat_.trk_tc = parameter.get_value(); + gains.antiwindup_strat_.tracking_time_constant = parameter.get_value(); changed = true; } else if (param_name == param_prefix_ + "antiwindup") diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 38b5fa93..b9b991c9 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -67,7 +67,7 @@ void check_set_parameters( ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; - ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; const bool SAVE_I_TERM = true; @@ -121,7 +121,7 @@ void check_set_parameters( ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_EQ(gains.u_max_, U_MAX); ASSERT_EQ(gains.u_min_, U_MIN); - ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); + ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_TRUE(gains.antiwindup_); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } @@ -157,7 +157,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX_BAD; ANTIWINDUP_STRAT.i_min = I_MIN_BAD; - ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; ANTIWINDUP_STRAT.legacy_antiwindup = false; ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); @@ -186,7 +186,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.i_min_, 0.0); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); - ASSERT_EQ(gains.antiwindup_strat_.trk_tc, 0.0); + ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, 0.0); ASSERT_FALSE(gains.antiwindup_); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } @@ -277,7 +277,7 @@ TEST(PidParametersTest, SetParametersTest) ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; - ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; const bool SAVE_I_TERM = false; @@ -329,7 +329,7 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_EQ(gains.u_max_, U_MAX); ASSERT_EQ(gains.u_min_, U_MIN); - ASSERT_EQ(gains.antiwindup_strat_.trk_tc, TRK_TC); + ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } @@ -359,7 +359,7 @@ TEST(PidParametersTest, SetBadParametersTest) ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; - ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false); @@ -409,7 +409,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.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_.trk_tc, TRK_TC); + ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); @@ -432,7 +432,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.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_.trk_tc, TRK_TC); + ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); @@ -452,7 +452,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(updated_gains.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_.trk_tc, TRK_TC); + ASSERT_EQ(updated_gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(updated_gains.antiwindup_, ANTIWINDUP); ASSERT_EQ(updated_gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } @@ -479,7 +479,7 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; - ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false); @@ -543,7 +543,7 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; - ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false); @@ -649,7 +649,7 @@ TEST(PidParametersTest, MultiplePidInstances) ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; - ANTIWINDUP_STRAT.trk_tc = TRK_TC; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; ANTIWINDUP_STRAT.legacy_antiwindup = false; ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index d8e3f4ac..2752d5b3 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -48,7 +48,7 @@ TEST(PidPublisherTest, PublishTest) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.legacy_antiwindup = false; - antiwindup_strat.trk_tc = 1.0; + antiwindup_strat.tracking_time_constant = 1.0; pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); bool callback_called = false; @@ -91,7 +91,7 @@ TEST(PidPublisherTest, PublishTestLifecycle) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.legacy_antiwindup = false; - antiwindup_strat.trk_tc = 1.0; + antiwindup_strat.tracking_time_constant = 1.0; pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); bool callback_called = false; diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index afd556bf..7334778d 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -92,7 +92,7 @@ TEST(ParameterTest, outputClampTest) // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; - antiwindup_strat.trk_tc = 0.0; // Set to 0.0 to use the default value + antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value // Setting u_max = 1.0 and u_min = -1.0 to test clamping Pid pid(1.0, 0.0, 0.0, 1.0, -1.0, antiwindup_strat); @@ -148,7 +148,7 @@ TEST(ParameterTest, noOutputClampTest) // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; - antiwindup_strat.trk_tc = 0.0; // Set to 0.0 to use the default value + antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value // Setting u_max = INF and u_min = -INF to disable clamping Pid pid( 1.0, 0.0, 0.0, std::numeric_limits::infinity(), @@ -208,7 +208,7 @@ TEST(ParameterTest, integrationBackCalculationZeroGainTest) // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; - antiwindup_strat.trk_tc = 0.0; // Set to 0.0 to use the default value + antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, antiwindup_strat); double cmd = 0.0; @@ -441,13 +441,13 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double i_min = -1 * std::rand() % 100; double u_max = std::numeric_limits::infinity(); double u_min = -1 * u_max; - double trk_tc = std::rand() % 100; + double tracking_time_constant = std::rand() % 100; bool antiwindup = false; AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::LEGACY; antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; - antiwindup_strat.trk_tc = trk_tc; + antiwindup_strat.tracking_time_constant = tracking_time_constant; antiwindup_strat.legacy_antiwindup = antiwindup; // Initialize the default way @@ -455,7 +455,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Test return values ------------------------------------------------- double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, - u_min_return, trk_tc_return; + u_min_return, tracking_time_constant_return; bool antiwindup_return; AntiWindupStrategy antiwindup_strat_return; @@ -468,7 +468,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(d_gain, d_gain_return); EXPECT_EQ(u_max, u_max_return); EXPECT_EQ(u_min, u_min_return); - EXPECT_EQ(trk_tc, antiwindup_strat_return.trk_tc); + EXPECT_EQ(tracking_time_constant, antiwindup_strat_return.tracking_time_constant); EXPECT_EQ(i_min, antiwindup_strat_return.i_min); EXPECT_EQ(i_max, antiwindup_strat_return.i_max); EXPECT_EQ(antiwindup, antiwindup_strat_return.legacy_antiwindup); @@ -484,12 +484,12 @@ TEST(ParameterTest, gainSettingCopyPIDTest) i_min = -1 * std::rand() % 100; u_max = std::numeric_limits::infinity(); u_min = -1 * u_max; - trk_tc = std::rand() % 100; + tracking_time_constant = std::rand() % 100; antiwindup = false; antiwindup_strat.type = AntiWindupStrategy::LEGACY; antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; - antiwindup_strat.trk_tc = trk_tc; + antiwindup_strat.tracking_time_constant = tracking_time_constant; antiwindup_strat.legacy_antiwindup = antiwindup; pid1.set_gains(p_gain, i_gain, d_gain, u_max, u_min, antiwindup_strat); @@ -503,7 +503,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); EXPECT_EQ(antiwindup, g1.antiwindup_); - EXPECT_EQ(trk_tc, g1.antiwindup_strat_.trk_tc); + EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant); EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max); EXPECT_EQ(i_min, g1.antiwindup_strat_.i_min); EXPECT_EQ(antiwindup, g1.antiwindup_strat_.legacy_antiwindup); @@ -527,7 +527,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); - EXPECT_EQ(trk_tc, g1.antiwindup_strat_.trk_tc); + EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant); 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(antiwindup, g1.antiwindup_strat_.legacy_antiwindup); @@ -555,7 +555,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); - EXPECT_EQ(trk_tc, g1.antiwindup_strat_.trk_tc); + EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant); 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(antiwindup, g1.antiwindup_strat_.legacy_antiwindup); @@ -755,7 +755,7 @@ TEST(CommandTest, backCalculationPIDTest) // Setting u_max = 5.0 and u_min = -5.0 to test clamping AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; - antiwindup_strat.trk_tc = 1.0; // Set to 0.0 to use the default value + antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); double cmd = 0.0; @@ -816,7 +816,7 @@ TEST(CommandTest, conditionalIntegrationPIDTest) // Setting u_max = 5.0 and u_min = -5.0 to test clamping AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; - antiwindup_strat.trk_tc = 1.0; + antiwindup_strat.tracking_time_constant = 1.0; Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); double cmd = 0.0; From e59b1cdc31612f57422eccf5d236559684f89c3f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 15 Jun 2025 21:45:06 +0200 Subject: [PATCH 26/44] remove the INTEGRATOR_CLAMPING method --- .../include/control_toolbox/pid.hpp | 21 ++----------------- control_toolbox/src/pid.cpp | 6 ++---- 2 files changed, 4 insertions(+), 23 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index d1f256d8..288fce33 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -70,7 +70,6 @@ struct AntiWindupStrategy UNDEFINED = -1, NONE, LEGACY, - INTEGRATOR_CLAMPING, BACK_CALCULATION, CONDITIONAL_INTEGRATION }; @@ -94,10 +93,6 @@ struct AntiWindupStrategy { type = CONDITIONAL_INTEGRATION; } - else if (s == "integrator_clamping") - { - type = INTEGRATOR_CLAMPING; - } else if (s == "legacy") { type = LEGACY; @@ -127,16 +122,6 @@ struct AntiWindupStrategy "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant " "(tracking_time_constant)"); } - if ( - type == INTEGRATOR_CLAMPING && - ((i_min >= i_max) || !std::isfinite(i_min) || !std::isfinite(i_max))) - { - throw std::invalid_argument( - fmt::format( - "AntiWindupStrategy 'integrator_clamping' requires i_min < i_max and to be finite " - "(i_min: {}, i_max: {})", - i_min, i_max)); - } if (type == LEGACY && ((i_min > i_max) || !std::isfinite(i_min) || !std::isfinite(i_max))) { throw std::invalid_argument( @@ -146,8 +131,8 @@ struct AntiWindupStrategy i_min, i_max)); } if ( - type != NONE && type != UNDEFINED && type != LEGACY && type != INTEGRATOR_CLAMPING && - type != BACK_CALCULATION && type != CONDITIONAL_INTEGRATION) + type != NONE && type != UNDEFINED && type != LEGACY && type != BACK_CALCULATION && + type != CONDITIONAL_INTEGRATION) { throw std::invalid_argument("AntiWindupStrategy has an invalid type"); } @@ -166,8 +151,6 @@ struct AntiWindupStrategy return "back_calculation"; case CONDITIONAL_INTEGRATION: return "conditional_integration"; - case INTEGRATOR_CLAMPING: - return "integrator_clamping"; case LEGACY: return "legacy"; case NONE: diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 6c6740fb..9003f7dc 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -352,10 +352,8 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) } // Calculate integral contribution to command - if ( - (gains.antiwindup_strat_.legacy_antiwindup && - gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY) || - gains.antiwindup_strat_.type == AntiWindupStrategy::INTEGRATOR_CLAMPING) + if ((gains.antiwindup_strat_.legacy_antiwindup && + gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY)) { // 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_); From 75b9239b18a65ca8fbb58efd24426f2b3f254d18 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 10:38:31 +0200 Subject: [PATCH 27/44] Address PR comments --- control_toolbox/src/pid.cpp | 6 +++--- control_toolbox/src/pid_ros.cpp | 14 +++++--------- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 9003f7dc..1c4a79eb 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -232,7 +232,7 @@ bool Pid::set_gains(const Gains & gains_in) std::string error_msg = ""; if (!gains_in.validate(error_msg)) { - std::cout << "PID: Invalid gains: " << error_msg << ". SKipping new gains." << std::endl; + std::cerr << "PID: Invalid gains: " << error_msg << ". SKipping new gains." << std::endl; return false; } else @@ -273,7 +273,7 @@ double Pid::compute_command(double error, const double & dt_s) // don't reset controller but return NaN if (!std::isfinite(error)) { - std::cout << "Received a non-finite error value\n"; + std::cerr << "Received a non-finite error value\n"; return cmd_ = std::numeric_limits::quiet_NaN(); } @@ -335,7 +335,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) // Don't reset controller but return NaN if (!std::isfinite(error) || !std::isfinite(error_dot)) { - std::cout << "Received a non-finite error/error_dot value\n"; + std::cerr << "Received a non-finite error/error_dot value\n"; return cmd_ = std::numeric_limits::quiet_NaN(); } diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 47cf4dcb..d306b218 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -242,12 +242,10 @@ bool PidROS::initialize_from_ros_parameters() set_parameter_event_callback(); } - if (antiwindup_strat_str == "legacy") - { - std::cout << "Old anti-windup technique is deprecated. " - "This option will be removed by the ROS 2 Kilted Kaiju release." - << std::endl; - } + RCLCPP_WARN_EXPRESSION( + node_logging_->get_logger(), antiwindup_strat_str == "legacy", + "Using the legacy anti-windup technique is deprecated. This option will be removed by the ROS " + "2 Kilted Kaiju release."); AntiWindupStrategy antiwindup_strat; antiwindup_strat.set_type(antiwindup_strat_str); @@ -341,7 +339,6 @@ bool PidROS::initialize_from_args( declare_param( param_prefix_ + "antiwindup", rclcpp::ParameterValue(gains.antiwindup_strat_.legacy_antiwindup)); - // @todo(sai) add other parameters or optimize it declare_param( param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(antiwindup_strat.tracking_time_constant)); @@ -626,8 +623,7 @@ void PidROS::set_parameter_event_callback() } else if (param_name == param_prefix_ + "antiwindup_strategy") { - // @todo decide if this can be changed in the first place - // gains.antiwindup_strat_ = AntiWindupStrategy(parameter.get_value()); + // @todo (saikishor) decide if this can be changed in the first place changed = true; } } From 3f6f505e758007b73dc6f8d25d978c0e29b8280f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 10:42:04 +0200 Subject: [PATCH 28/44] Simplify the if condition in compute_command --- control_toolbox/src/pid.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 1c4a79eb..53476ac0 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -352,17 +352,17 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) } // Calculate integral contribution to command - if ((gains.antiwindup_strat_.legacy_antiwindup && - gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY)) + if (gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY) { - // 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 if ( - !gains.antiwindup_strat_.legacy_antiwindup && - gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY) - { - i_term_ += gains.i_gain_ * dt_s * p_error_; + if (gains.antiwindup_strat_.legacy_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_; + } } // Compute the command From 48a51419063ecb22dea9f49c4b867758ffdc73e4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 10:45:45 +0200 Subject: [PATCH 29/44] throw exception on invalid antiwindup type --- control_toolbox/include/control_toolbox/pid.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 288fce33..4cbf1afb 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -104,7 +104,10 @@ struct AntiWindupStrategy else { type = UNDEFINED; - std::cerr << "Unknown antiwindup strategy: '" << s << "'. Using UNDEFINED." << std::endl; + throw std::invalid_argument( + "AntiWindupStrategy: Unknown antiwindup strategy : '" + s + + "'. Valid strategies are: 'back_calculation', 'conditional_integration', 'legacy', " + "'none'."); } } From 74aa005a9cacdd182a1452d8d1840271c46a6e2f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 14:59:26 +0200 Subject: [PATCH 30/44] Add error_deadband option to the AntiWindupStrategy --- .../include/control_toolbox/pid.hpp | 12 +++++++- control_toolbox/src/pid.cpp | 28 +++++++++++-------- control_toolbox/src/pid_ros.cpp | 14 +++++++++- 3 files changed, 41 insertions(+), 13 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 4cbf1afb..be34aacf 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -61,6 +61,12 @@ namespace control_toolbox the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. + * \param error_deadband Error deadband is used to avoid integration when the error is within a small range. + * \param type Specifies the antiwindup strategy type. Valid values are: + * - `NONE`: No antiwindup strategy applied. + * - `LEGACY`: Legacy antiwindup strategy, which limits the integral term to prevent windup. + * - `BACK_CALCULATION`: Back calculation antiwindup strategy, which uses a tracking time constant. + * - `CONDITIONAL_INTEGRATION`: Conditional integration antiwindup strategy, which integrates only when certain conditions are met. */ struct AntiWindupStrategy { @@ -78,8 +84,9 @@ struct AntiWindupStrategy : type(UNDEFINED), i_min(std::numeric_limits::quiet_NaN()), i_max(std::numeric_limits::quiet_NaN()), + legacy_antiwindup(false), tracking_time_constant(0.0), - legacy_antiwindup(false) + error_deadband(std::numeric_limits::epsilon()) { } @@ -174,6 +181,9 @@ struct AntiWindupStrategy // strategy. If set to 0.0 when this strategy is selected, a recommended default value // will be applied. double tracking_time_constant = 0.0; /**< Tracking time constant for back_calculation strategy. */ + + double error_deadband = + std::numeric_limits::epsilon(); /**< Error deadband to avoid integration. */ }; template diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 53476ac0..da7a6f13 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -352,7 +352,9 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) } // Calculate integral contribution to command - if (gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY) + const bool is_error_in_deadband_zone = + control_toolbox::is_zero(error, gains.antiwindup_strat_.error_deadband); + if (!is_error_in_deadband_zone && gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY) { if (gains.antiwindup_strat_.legacy_antiwindup) { @@ -367,7 +369,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) // Compute the command if ( - !gains.antiwindup_strat_.legacy_antiwindup && + !is_error_in_deadband_zone && !gains.antiwindup_strat_.legacy_antiwindup && gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY) { // Limit i_term so that the limit is meaningful in the output @@ -395,17 +397,21 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) cmd_ = cmd_unsat_; } - if ( - gains.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && !is_zero(gains.i_gain_)) - { - i_term_ += dt_s * (gains.i_gain_ * error + - 1 / gains.antiwindup_strat_.tracking_time_constant * (cmd_ - cmd_unsat_)); - } - else if (gains.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION) + if (!is_error_in_deadband_zone) { - if (!(!iszero(cmd_unsat_ - cmd_) && error * cmd_unsat_ > 0)) + if ( + gains.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && + !is_zero(gains.i_gain_)) { - i_term_ += dt_s * gains.i_gain_ * error; + i_term_ += dt_s * (gains.i_gain_ * error + + 1 / gains.antiwindup_strat_.tracking_time_constant * (cmd_ - cmd_unsat_)); + } + else if (gains.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION) + { + if (!(!is_zero(cmd_unsat_ - cmd_) && error * cmd_unsat_ > 0)) + { + i_term_ += dt_s * gains.i_gain_ * error; + } } } diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index d306b218..14f9c2fa 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -215,8 +215,9 @@ bool PidROS::get_string_param(const std::string & param_name, std::string & valu bool PidROS::initialize_from_ros_parameters() { - double p, i, d, i_max, i_min, u_max, u_min, tracking_time_constant; + 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; bool antiwindup = false; @@ -230,6 +231,7 @@ bool PidROS::initialize_from_ros_parameters() all_params_available &= get_double_param(param_prefix_ + "i_clamp_min", i_min); all_params_available &= get_double_param(param_prefix_ + "u_clamp_max", u_max); all_params_available &= get_double_param(param_prefix_ + "u_clamp_min", u_min); + all_params_available &= get_double_param(param_prefix_ + "error_deadband", error_deadband); all_params_available &= get_double_param(param_prefix_ + "tracking_time_constant", tracking_time_constant); @@ -253,6 +255,7 @@ bool PidROS::initialize_from_ros_parameters() antiwindup_strat.i_min = i_min; antiwindup_strat.tracking_time_constant = tracking_time_constant; antiwindup_strat.legacy_antiwindup = antiwindup; + antiwindup_strat.error_deadband = error_deadband; try { @@ -342,6 +345,8 @@ bool PidROS::initialize_from_args( declare_param( param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(antiwindup_strat.tracking_time_constant)); + declare_param( + param_prefix_ + "error_deadband", rclcpp::ParameterValue(antiwindup_strat.error_deadband)); declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(true)); declare_param( param_prefix_ + "antiwindup_strategy", @@ -435,6 +440,8 @@ bool PidROS::set_gains(const Pid::Gains & gains) param_prefix_ + "tracking_time_constant", gains.antiwindup_strat_.tracking_time_constant), rclcpp::Parameter(param_prefix_ + "antiwindup", gains.antiwindup_strat_.legacy_antiwindup), + rclcpp::Parameter( + param_prefix_ + "error_deadband", gains.antiwindup_strat_.error_deadband), rclcpp::Parameter(param_prefix_ + "saturation", true), rclcpp::Parameter( param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string())}); @@ -621,6 +628,11 @@ void PidROS::set_parameter_event_callback() gains.antiwindup_strat_.legacy_antiwindup = parameter.get_value(); changed = true; } + else if (param_name == param_prefix_ + "error_deadband") + { + gains.antiwindup_strat_.error_deadband = parameter.get_value(); + changed = true; + } else if (param_name == param_prefix_ + "antiwindup_strategy") { // @todo (saikishor) decide if this can be changed in the first place From 723c60de49430e7d40c623c459c9819ff5ffdc81 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 15:21:28 +0200 Subject: [PATCH 31/44] remove changing anti-windup strategy at runtime --- control_toolbox/src/pid_ros.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 14f9c2fa..2669304f 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -633,11 +633,6 @@ void PidROS::set_parameter_event_callback() gains.antiwindup_strat_.error_deadband = parameter.get_value(); changed = true; } - else if (param_name == param_prefix_ + "antiwindup_strategy") - { - // @todo (saikishor) decide if this can be changed in the first place - changed = true; - } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { From dafcd3048ab044eca3398b02eaa5ca9a7bbcaca1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 22:18:37 +0200 Subject: [PATCH 32/44] Fix GCC pragma's --- control_toolbox/src/pid.cpp | 12 +++--------- control_toolbox/src/pid_ros.cpp | 6 +++--- 2 files changed, 6 insertions(+), 12 deletions(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index da7a6f13..ed51a6b1 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -101,18 +101,18 @@ Pid::Pid(const Pid & source) Pid::~Pid() {} -bool Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) -{ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" +bool Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) +{ if (set_gains(p, i, d, i_max, i_min, antiwindup)) { reset(); return true; } -#pragma GCC diagnostic pop return false; } +#pragma GCC diagnostic pop bool Pid::initialize( double p, double i, double d, double u_max, double u_min, @@ -188,10 +188,7 @@ bool Pid::set_gains(double p, double i, double d, double i_max, double i_min, bo { try { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" Gains gains(p, i, d, i_max, i_min, antiwindup); -#pragma GCC diagnostic pop if (set_gains(gains)) { return true; @@ -211,10 +208,7 @@ bool Pid::set_gains( { try { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" Gains gains(p, i, d, u_max, u_min, antiwindup_strat); -#pragma GCC diagnostic pop if (set_gains(gains)) { return true; diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 2669304f..29d900de 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -283,6 +283,8 @@ void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValu } } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" bool PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup) { @@ -292,11 +294,9 @@ bool PidROS::initialize_from_args( antiwindup_strat.i_min = i_min; antiwindup_strat.legacy_antiwindup = antiwindup; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" return initialize_from_args(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, false); -#pragma GCC diagnostic pop } +#pragma GCC diagnostic pop bool PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term) From ae372d39ca2ffdf3342967914597c30125c743f7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 22:27:48 +0200 Subject: [PATCH 33/44] Address MR review comments --- control_toolbox/include/control_toolbox/pid.hpp | 9 +++------ control_toolbox/test/pid_tests.cpp | 4 +--- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index be34aacf..65d9fc60 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -103,6 +103,9 @@ struct AntiWindupStrategy else if (s == "legacy") { type = LEGACY; + std::cout << "Using the legacy anti-windup technique is deprecated. This option will be " + "removed by the ROS 2 Kilted Kaiju release." + << std::endl; } else if (s == "none") { @@ -382,12 +385,6 @@ class Pid error_msg = "Gains: u_min or u_max must not be NaN"; return false; } - else if (antiwindup_strat_.type == AntiWindupStrategy::UNDEFINED) - { - error_msg = - "Gains: Antiwindup strategy cannot be UNDEFINED. Please set a valid antiwindup strategy."; - return false; - } try { antiwindup_strat_.validate(); diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 7334778d..175009a2 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -454,9 +454,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid1(p_gain, i_gain, d_gain, u_max, u_min, antiwindup_strat); // Test return values ------------------------------------------------- - double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, - u_min_return, tracking_time_constant_return; - bool antiwindup_return; + double p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return; AntiWindupStrategy antiwindup_strat_return; pid1.get_gains( From ccefe79554b207260aabe8e312a7ea3ba8a75ea5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 22:29:15 +0200 Subject: [PATCH 34/44] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- control_toolbox/include/control_toolbox/pid.hpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 65d9fc60..3dc65df6 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -61,10 +61,10 @@ namespace control_toolbox the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. - * \param error_deadband Error deadband is used to avoid integration when the error is within a small range. + * \param error_deadband Error deadband is used to stop integration when the error is within the given range. * \param type Specifies the antiwindup strategy type. Valid values are: * - `NONE`: No antiwindup strategy applied. - * - `LEGACY`: Legacy antiwindup strategy, which limits the integral term to prevent windup. + * - `LEGACY`: Legacy antiwindup strategy, which limits the integral term to prevent windup (deprecated: This option will be removed in a future release). * - `BACK_CALCULATION`: Back calculation antiwindup strategy, which uses a tracking time constant. * - `CONDITIONAL_INTEGRATION`: Conditional integration antiwindup strategy, which integrates only when certain conditions are met. */ @@ -181,8 +181,7 @@ struct AntiWindupStrategy bool legacy_antiwindup = false; /**< Use legacy anti-windup strategy. */ // tracking_time_constant Specifies the tracking time constant for the 'back_calculation' - // strategy. If set to 0.0 when this strategy is selected, a recommended default value - // will be applied. + // strategy. If set to 0.0 a recommended default value will be applied. double tracking_time_constant = 0.0; /**< Tracking time constant for back_calculation strategy. */ double error_deadband = @@ -415,13 +414,10 @@ class Pid double p_gain_ = 0.0; /**< Proportional gain. */ double i_gain_ = 0.0; /**< Integral gain. */ double d_gain_ = 0.0; /**< Derivative gain. */ - [[deprecated("Use antiwindup_strat_.i_max instead.")]] double i_max_ = 0.0; /**< Maximum allowable integral term. */ - [[deprecated("Use antiwindup_strat_.i_min instead.")]] double i_min_ = 0.0; /**< Minimum allowable integral term. */ double u_max_ = std::numeric_limits::infinity(); /**< Maximum allowable output. */ double u_min_ = -std::numeric_limits::infinity(); /**< Minimum allowable output. */ - [[deprecated("Use antiwindup_strat_ instead.")]] bool antiwindup_ = false; /**< Anti-windup. */ AntiWindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ }; From 9c2a4a2ec0229b337877f896fc033fdf2e0768ba Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 22:29:39 +0200 Subject: [PATCH 35/44] Fix pre-commit --- control_toolbox/include/control_toolbox/pid.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 3dc65df6..2974b8af 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -414,12 +414,12 @@ 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_ = 0.0; /**< Maximum allowable integral term. */ + double i_min_ = 0.0; /**< 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. */ - AntiWindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ + bool antiwindup_ = false; /**< Anti-windup. */ + AntiWindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ }; /*! From b79acda90011436a3ec6adfc4e5702d046c1bed5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 22:54:15 +0200 Subject: [PATCH 36/44] Update control_toolbox/include/control_toolbox/pid.hpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- 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 2974b8af..b5877278 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -294,7 +294,7 @@ class Pid antiwindup_strat_.type = AntiWindupStrategy::LEGACY; antiwindup_strat_.i_max = i_max; antiwindup_strat_.i_min = i_min; - antiwindup_strat_.legacy_antiwindup = false; + antiwindup_strat_.legacy_antiwindup = true; } /*! From edc4ef0cb017391b4c9b0d7c22ecb82b96e6d5b8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 22:58:56 +0200 Subject: [PATCH 37/44] Update control_toolbox/src/pid.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- control_toolbox/src/pid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index ed51a6b1..19bce26c 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -363,7 +363,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) // Compute the command if ( - !is_error_in_deadband_zone && !gains.antiwindup_strat_.legacy_antiwindup && + !gains.antiwindup_strat_.legacy_antiwindup && gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY) { // Limit i_term so that the limit is meaningful in the output From b21533aab48bbc99cd7c4b0ec9948c31cc4e6dfc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 23:03:41 +0200 Subject: [PATCH 38/44] Address MR review comments --- control_toolbox/include/control_toolbox/pid.hpp | 2 +- control_toolbox/src/pid_ros.cpp | 10 ---------- 2 files changed, 1 insertion(+), 11 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index b5877278..da97274c 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -550,7 +550,7 @@ class Pid integral contribution to the control output. i_max and i_min are applied in both scenarios. */ - [[deprecated("Use get_gains overload without bool antiwindup.")]] + [[deprecated("Use get_gains overload with AntiWindupStrategy argument.")]] void get_gains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup); diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 29d900de..6329093f 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -257,16 +257,6 @@ bool PidROS::initialize_from_ros_parameters() antiwindup_strat.legacy_antiwindup = antiwindup; antiwindup_strat.error_deadband = error_deadband; - try - { - antiwindup_strat.validate(); - } - catch (const std::exception & e) - { - RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); - return false; - } - if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat)) { return all_params_available; From 4ba22199c87eeba05622d0e938690c2953742092 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 23:14:18 +0200 Subject: [PATCH 39/44] add deprecation for valid i_min and i_max --- control_toolbox/include/control_toolbox/pid.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index da97274c..4cc0f94b 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -143,6 +143,13 @@ struct AntiWindupStrategy "{})", i_min, i_max)); } + if (std::isfinite(i_min) || std::isfinite(i_max)) + { + throw std::invalid_argument( + "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."); + } if ( type != NONE && type != UNDEFINED && type != LEGACY && type != BACK_CALCULATION && type != CONDITIONAL_INTEGRATION) From df2ff65ffd5daa0e32b2e64ffc1e9f4b4d457f13 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 23:14:22 +0200 Subject: [PATCH 40/44] add deprecation for valid i_min and i_max --- control_toolbox/include/control_toolbox/pid.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 4cc0f94b..a68c7df8 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -145,10 +145,10 @@ struct AntiWindupStrategy } if (std::isfinite(i_min) || std::isfinite(i_max)) { - throw std::invalid_argument( - "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::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; } if ( type != NONE && type != UNDEFINED && type != LEGACY && type != BACK_CALCULATION && From 3c40f427e9b46b81969b5289671c68698a1b4cb7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Jun 2025 23:18:03 +0200 Subject: [PATCH 41/44] readd validation --- control_toolbox/src/pid_ros.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 6329093f..29d900de 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -257,6 +257,16 @@ bool PidROS::initialize_from_ros_parameters() antiwindup_strat.legacy_antiwindup = antiwindup; antiwindup_strat.error_deadband = error_deadband; + try + { + antiwindup_strat.validate(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); + return false; + } + if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat)) { return all_params_available; From 3955eacb670592f79eb45d71551bb0b6dc0385c7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 17 Jun 2025 10:20:58 +0200 Subject: [PATCH 42/44] Apply suggestions from code review Co-authored-by: Felix Exner (fexner) --- control_toolbox/include/control_toolbox/pid.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index a68c7df8..4717716f 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -376,19 +376,19 @@ class Pid bool validate(std::string & error_msg) const { - if (i_min_ > i_max_) + 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_) + else if (u_min_ >= u_max_) { error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_); return false; } else if (std::isnan(u_min_) || std::isnan(u_max_)) { - error_msg = "Gains: u_min or u_max must not be NaN"; + error_msg = "Gains: u_min and u_max must not be NaN"; return false; } try From 17a98ec345cf8f42e596c72386ce2afc15c60ce4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 17 Jun 2025 10:50:43 +0200 Subject: [PATCH 43/44] readd the > condition for backward compatibility --- 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 4717716f..6443b34f 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -376,7 +376,7 @@ class Pid bool validate(std::string & error_msg) const { - if (i_min_ >= i_max_) + if (i_min_ > i_max_) { error_msg = fmt::format("Gains: i_min ({}) must be less than i_max ({})", i_min_, i_max_); return false; From 4d3fec52bb76753dcf760f8b1690dee309275c18 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 17 Jun 2025 10:58:20 +0200 Subject: [PATCH 44/44] Fix tests for the updated condition --- control_toolbox/test/pid_ros_parameters_tests.cpp | 8 ++++++-- control_toolbox/test/pid_tests.cpp | 4 ++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index b9b991c9..0712d6f0 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -482,7 +482,9 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; - pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false); + ASSERT_FALSE(pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false)) + << "Zero u_min and u_max are not valid so initialization should fail"; + ASSERT_TRUE(pid.initialize_from_args(0, 0, 0, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); std::cout << "Setting gains with set_gains()" << std::endl; pid.set_gains(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT); @@ -546,7 +548,9 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; - pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false); + ASSERT_FALSE(pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false)) + << "Zero u_min and u_max are not valid so initialization should fail"; + ASSERT_TRUE(pid.initialize_from_args(0, 0, 0, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); pid.set_gains(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT); rclcpp::Parameter param; diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 175009a2..868674d6 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -209,7 +209,7 @@ TEST(ParameterTest, integrationBackCalculationZeroGainTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value - Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, antiwindup_strat); + Pid pid(0.0, 0.0, 0.0, 20.0, -20.0, antiwindup_strat); double cmd = 0.0; double pe, ie, de; @@ -258,7 +258,7 @@ TEST(ParameterTest, integrationConditionalIntegrationZeroGainTest) // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; - Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, antiwindup_strat); + Pid pid(0.0, 0.0, 0.0, 20.0, -20.0, antiwindup_strat); double cmd = 0.0; double pe, ie, de;