diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 8d05af1e..e78792b5 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -121,11 +121,15 @@ struct AntiWindupStrategy "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant " "(tracking_time_constant)"); } - if (i_min >= i_max) + if (i_min > 0) { throw std::invalid_argument( - fmt::format( - "PID requires i_min < i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max)); + fmt::format("PID requires i_min to be smaller or equal to 0 (i_min: {})", i_min)); + } + if (i_max < 0) + { + throw std::invalid_argument( + fmt::format("PID requires i_max to be greater or equal to 0 (i_max: {})", i_max)); } if ( type != NONE && type != UNDEFINED && type != BACK_CALCULATION && @@ -135,6 +139,13 @@ struct AntiWindupStrategy } } + void print() const + { + std::cout << "antiwindup_strat: " << to_string() << "\ti_max: " << i_max << ", i_min: " << i_min + << "\ttracking_time_constant: " << tracking_time_constant + << "\terror_deadband: " << error_deadband << std::endl; + } + operator std::string() const { return to_string(); } constexpr bool operator==(Value other) const { return type == other; } @@ -278,33 +289,17 @@ class Pid : p_gain_(p), i_gain_(i), d_gain_(d), - i_max_(antiwindup_strat.i_max), - i_min_(antiwindup_strat.i_min), u_max_(u_max), u_min_(u_min), + i_max_(antiwindup_strat.i_max), + i_min_(antiwindup_strat.i_min), antiwindup_strat_(antiwindup_strat) { - if (std::isnan(u_min) || std::isnan(u_max)) - { - throw std::invalid_argument("Gains: u_min and u_max must not be NaN"); - } - if (u_min > u_max) - { - std::cout << "Received invalid u_min and u_max values: " << "u_min: " << u_min - << ", u_max: " << u_max << ". Setting saturation to false." << std::endl; - u_max_ = std::numeric_limits::infinity(); - u_min_ = -std::numeric_limits::infinity(); - } } bool validate(std::string & error_msg) const { - if (i_min_ > i_max_) - { - error_msg = fmt::format("Gains: i_min ({}) must be less than i_max ({})", i_min_, i_max_); - return false; - } - else if (u_min_ >= u_max_) + if (u_min_ >= u_max_) // is false if any value is nan { error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_); return false; @@ -329,9 +324,8 @@ class Pid void print() const { std::cout << "Gains: p: " << p_gain_ << ", i: " << i_gain_ << ", d: " << d_gain_ - << ", i_max: " << i_max_ << ", i_min: " << i_min_ << ", u_max: " << u_max_ - << ", u_min: " << u_min_ << ", antiwindup_strat: " << antiwindup_strat_.to_string() - << std::endl; + << ", u_max: " << u_max_ << ", u_min: " << u_min_ << std::endl; + antiwindup_strat_.print(); } double p_gain_ = 0.0; /**< Proportional gain. */ @@ -358,7 +352,7 @@ class Pid 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. * - * \throws An std::invalid_argument exception is thrown if u_min > u_max. + * \throws An std::invalid_argument exception is thrown if u_min >= u_max. */ Pid( double p = 0.0, double i = 0.0, double d = 0.0, diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index 95dbf146..6c961d27 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -137,7 +137,7 @@ class PidROS * \param save_i_term save integrator output between resets. * \return True if all parameters are successfully set, False otherwise. * - * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max. + * \note New gains are not applied if antiwindup_strat.i_min > 0, antiwindup_strat.i_max < 0 or u_min > u_max. */ bool initialize_from_args( double p, double i, double d, double u_max, double u_min, @@ -210,7 +210,7 @@ class PidROS tracking_time_constant parameter to tune the anti-windup behavior. * \return True if all parameters are successfully set, False otherwise. * - * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max. + * \note New gains are not applied if antiwindup_strat.i_min > 0, antiwindup_strat.i_max < 0 or u_min > u_max. * \note This method is not RT safe */ bool set_gains( diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 8a2495d1..306b9991 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -50,9 +50,9 @@ Pid::Pid( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) { - if (u_min > u_max) + if (u_min >= u_max) { - throw std::invalid_argument("received u_min > u_max"); + throw std::invalid_argument("received u_min >= u_max"); } set_gains(p, i, d, u_max, u_min, antiwindup_strat); @@ -321,7 +321,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) } } - i_term_ = std::clamp(i_term_, gains_.i_min_, gains_.i_max_); + i_term_ = std::clamp(i_term_, gains_.antiwindup_strat_.i_min, gains_.antiwindup_strat_.i_max); return cmd_; } diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 710c217d..dc8c6b83 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -410,12 +410,12 @@ void PidROS::print_values() << " P Gain: " << gains.p_gain_ << "\n" << " I Gain: " << gains.i_gain_ << "\n" << " D Gain: " << gains.d_gain_ << "\n" - << " I Max: " << gains.i_max_ << "\n" - << " I Min: " << gains.i_min_ << "\n" << " U_Max: " << gains.u_max_ << "\n" << " U_Min: " << gains.u_min_ << "\n" - << " Tracking_Time_Constant: " << gains.antiwindup_strat_.tracking_time_constant << "\n" << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\n" + << " Tracking_Time_Constant: " << gains.antiwindup_strat_.tracking_time_constant << "\n" + << " I Max: " << gains.antiwindup_strat_.i_max << "\n" + << " I Min: " << gains.antiwindup_strat_.i_min << "\n" << "\n" << " P Error: " << p_error << "\n" << " I Term: " << i_term << "\n" @@ -502,12 +502,12 @@ void PidROS::set_parameter_event_callback() } else if (param_name == param_prefix_ + "i_clamp_max") { - gains.i_max_ = parameter.get_value(); + gains.antiwindup_strat_.i_max = parameter.get_value(); changed = true; } else if (param_name == param_prefix_ + "i_clamp_min") { - gains.i_min_ = parameter.get_value(); + gains.antiwindup_strat_.i_min = parameter.get_value(); changed = true; } else if (param_name == param_prefix_ + "u_clamp_max") diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 62e483bc..8cf4b34c 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -105,8 +105,8 @@ void check_set_parameters( ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); - ASSERT_EQ(gains.i_max_, I_MAX); - ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX); + ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN); ASSERT_EQ(gains.u_max_, U_MAX); ASSERT_EQ(gains.u_min_, U_MIN); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); @@ -146,7 +146,10 @@ TEST(PidParametersTest, InitPidTestBadParameter) ANTIWINDUP_STRAT.i_min = I_MIN_BAD; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); + bool ret; + ASSERT_NO_THROW( + ret = pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); + ASSERT_FALSE(ret); rclcpp::Parameter param; @@ -167,12 +170,25 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.p_gain_, 0.0); ASSERT_EQ(gains.i_gain_, 0.0); ASSERT_EQ(gains.d_gain_, 0.0); - ASSERT_EQ(gains.i_max_, std::numeric_limits::infinity()); - ASSERT_EQ(gains.i_min_, -std::numeric_limits::infinity()); + ASSERT_EQ(gains.antiwindup_strat_.i_max, std::numeric_limits::infinity()); + ASSERT_EQ(gains.antiwindup_strat_.i_min, -std::numeric_limits::infinity()); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, 0.0); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); + + // Try other invalid combinations + ANTIWINDUP_STRAT.i_max = 10.; + ANTIWINDUP_STRAT.i_min = 5.; + ASSERT_NO_THROW( + ret = pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); + ASSERT_FALSE(ret); + + ANTIWINDUP_STRAT.i_max = -5.; + ANTIWINDUP_STRAT.i_min = 10.; + ASSERT_NO_THROW( + ret = pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); + ASSERT_FALSE(ret); } TEST(PidParametersTest, InitPid_param_prefix_only) @@ -287,8 +303,8 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); - ASSERT_EQ(gains.i_max_, I_MAX); - ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX); + ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN); ASSERT_EQ(gains.u_max_, U_MAX); ASSERT_EQ(gains.u_min_, U_MIN); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); @@ -362,8 +378,8 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); - ASSERT_EQ(gains.i_max_, I_MAX); - ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX); + ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); @@ -384,8 +400,8 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); - ASSERT_EQ(gains.i_max_, I_MAX); - ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX); + ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); @@ -403,8 +419,8 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(updated_gains.p_gain_, P); ASSERT_EQ(updated_gains.i_gain_, I); ASSERT_EQ(updated_gains.d_gain_, D); - ASSERT_EQ(updated_gains.i_max_, I_MAX); - ASSERT_EQ(updated_gains.i_min_, I_MIN); + ASSERT_EQ(updated_gains.antiwindup_strat_.i_max, I_MAX); + ASSERT_EQ(updated_gains.antiwindup_strat_.i_min, I_MIN); ASSERT_EQ(updated_gains.u_max_, U_MAX); ASSERT_EQ(updated_gains.u_min_, U_MIN); ASSERT_EQ(updated_gains.antiwindup_strat_.tracking_time_constant, TRK_TC); diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 5629cd5f..de2fb680 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -420,8 +420,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(p_gain, g1.p_gain_); EXPECT_EQ(i_gain, g1.i_gain_); EXPECT_EQ(d_gain, g1.d_gain_); - EXPECT_EQ(i_max, g1.i_max_); - EXPECT_EQ(i_min, g1.i_min_); + EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max); + EXPECT_EQ(i_min, g1.antiwindup_strat_.i_min); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant); @@ -443,8 +443,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(p_gain_return, g1.p_gain_); EXPECT_EQ(i_gain_return, g1.i_gain_); EXPECT_EQ(d_gain_return, g1.d_gain_); - EXPECT_EQ(antiwindup_strat_return.i_max, g1.i_max_); - EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_); + EXPECT_EQ(antiwindup_strat_return.i_max, g1.antiwindup_strat_.i_max); + EXPECT_EQ(antiwindup_strat_return.i_min, g1.antiwindup_strat_.i_min); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant); @@ -470,8 +470,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(p_gain_return, g1.p_gain_); EXPECT_EQ(i_gain_return, g1.i_gain_); EXPECT_EQ(d_gain_return, g1.d_gain_); - EXPECT_EQ(antiwindup_strat_return.i_max, g1.i_max_); - EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_); + EXPECT_EQ(antiwindup_strat_return.i_max, g1.antiwindup_strat_.i_max); + EXPECT_EQ(antiwindup_strat_return.i_min, g1.antiwindup_strat_.i_min); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant);