diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 7d151e69..e007386b 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -135,11 +135,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 != LEGACY && type != BACK_CALCULATION && @@ -149,6 +153,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; } @@ -361,27 +372,11 @@ class Pid antiwindup_(antiwindup_strat.legacy_antiwindup), 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; @@ -413,9 +408,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: " << antiwindup_ - << ", 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. */ @@ -465,7 +459,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, double i, double d, double u_max, double u_min, diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index 77b6f3c3..9ccc4513 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -238,7 +238,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 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, @@ -369,7 +369,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 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 c8e23f5d..a1016651 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -77,9 +77,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); @@ -431,7 +431,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 6ef66d93..eb54de4e 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -555,13 +555,13 @@ 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" + << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\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" + << " 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" @@ -648,12 +648,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 50d583cf..7d223bec 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -113,8 +113,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); @@ -156,7 +156,10 @@ TEST(PidParametersTest, InitPidTestBadParameter) 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)); + 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; @@ -178,13 +181,26 @@ 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_FALSE(gains.antiwindup_); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); + + // 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) @@ -303,8 +319,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); @@ -383,8 +399,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); @@ -406,8 +422,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); @@ -426,8 +442,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 3ccfd06b..60b018cf 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -496,8 +496,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(antiwindup, g1.antiwindup_); @@ -521,8 +521,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); @@ -549,8 +549,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);