Skip to content

Improve PID parameter validation #510

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 6 commits into from
Oct 14, 2025
Merged

Improve PID parameter validation #510

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
52e89ed
Improve validation of i_min/i_max of PID
christophfroehlich Oct 13, 2025
64011ec
Add test for it
christophfroehlich Oct 13, 2025
b419fc3
Remove double check
christophfroehlich Oct 14, 2025
087c395
Cleanup parameter validation
christophfroehlich Oct 14, 2025
c8e6484
Merge branch 'ros2-master' into improve/pid/validation
christophfroehlich Oct 14, 2025
e74ec11
Fix docstring
christophfroehlich Oct 14, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 20 additions & 26 deletions control_toolbox/include/control_toolbox/pid.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 &&
Expand All @@ -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; }
Expand Down Expand Up @@ -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<double>::infinity();
u_min_ = -std::numeric_limits<double>::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;
Expand All @@ -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. */
Expand All @@ -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,
Expand Down
4 changes: 2 additions & 2 deletions control_toolbox/include/control_toolbox/pid_ros.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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(
Expand Down
6 changes: 3 additions & 3 deletions control_toolbox/src/pid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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_;
}
Expand Down
10 changes: 5 additions & 5 deletions control_toolbox/src/pid_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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<double>();
gains.antiwindup_strat_.i_max = parameter.get_value<double>();
changed = true;
}
else if (param_name == param_prefix_ + "i_clamp_min")
{
gains.i_min_ = parameter.get_value<double>();
gains.antiwindup_strat_.i_min = parameter.get_value<double>();
changed = true;
}
else if (param_name == param_prefix_ + "u_clamp_max")
Expand Down
42 changes: 29 additions & 13 deletions control_toolbox/test/pid_ros_parameters_tests.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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;

Expand All @@ -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<double>::infinity());
ASSERT_EQ(gains.i_min_, -std::numeric_limits<double>::infinity());
ASSERT_EQ(gains.antiwindup_strat_.i_max, std::numeric_limits<double>::infinity());
ASSERT_EQ(gains.antiwindup_strat_.i_min, -std::numeric_limits<double>::infinity());
ASSERT_EQ(gains.u_max_, std::numeric_limits<double>::infinity());
ASSERT_EQ(gains.u_min_, -std::numeric_limits<double>::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)
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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<double>::infinity());
ASSERT_EQ(gains.u_min_, -std::numeric_limits<double>::infinity());
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
Expand All @@ -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<double>::infinity());
ASSERT_EQ(gains.u_min_, -std::numeric_limits<double>::infinity());
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
Expand All @@ -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);
Expand Down
12 changes: 6 additions & 6 deletions control_toolbox/test/pid_tests.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand Down
Loading