Skip to content

Commit 1f4772d

Browse files
christophfroehlichchristophfroehlich
committed
Improve PID parameter validation (#510)
1 parent 44af78e commit 1f4772d

File tree

6 files changed

+63
-53
lines changed

6 files changed

+63
-53
lines changed

‎control_toolbox/include/control_toolbox/pid.hpp‎

Lines changed: 18 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -135,11 +135,15 @@ struct AntiWindupStrategy
135135
"AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant "
136136
"(tracking_time_constant)");
137137
}
138-
if (i_min > i_max)
138+
if (i_min > 0)
139139
{
140140
throw std::invalid_argument(
141-
fmt::format(
142-
"PID requires i_min <= i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max));
141+
fmt::format("PID requires i_min to be smaller or equal to 0 (i_min: {})", i_min));
142+
}
143+
if (i_max < 0)
144+
{
145+
throw std::invalid_argument(
146+
fmt::format("PID requires i_max to be greater or equal to 0 (i_max: {})", i_max));
143147
}
144148
if (
145149
type != NONE && type != UNDEFINED && type != LEGACY && type != BACK_CALCULATION &&
@@ -149,6 +153,13 @@ struct AntiWindupStrategy
149153
}
150154
}
151155

156+
void print() const
157+
{
158+
std::cout << "antiwindup_strat: " << to_string() << "\ti_max: " << i_max << ", i_min: " << i_min
159+
<< "\ttracking_time_constant: " << tracking_time_constant
160+
<< "\terror_deadband: " << error_deadband << std::endl;
161+
}
162+
152163
operator std::string() const { return to_string(); }
153164

154165
constexpr bool operator==(Value other) const { return type == other; }
@@ -361,27 +372,11 @@ class Pid
361372
antiwindup_(antiwindup_strat.legacy_antiwindup),
362373
antiwindup_strat_(antiwindup_strat)
363374
{
364-
if (std::isnan(u_min) || std::isnan(u_max))
365-
{
366-
throw std::invalid_argument("Gains: u_min and u_max must not be NaN");
367-
}
368-
if (u_min > u_max)
369-
{
370-
std::cout << "Received invalid u_min and u_max values: " << "u_min: " << u_min
371-
<< ", u_max: " << u_max << ". Setting saturation to false." << std::endl;
372-
u_max_ = std::numeric_limits<double>::infinity();
373-
u_min_ = -std::numeric_limits<double>::infinity();
374-
}
375375
}
376376

377377
bool validate(std::string & error_msg) const
378378
{
379-
if (i_min_ > i_max_)
380-
{
381-
error_msg = fmt::format("Gains: i_min ({}) must be less than i_max ({})", i_min_, i_max_);
382-
return false;
383-
}
384-
else if (u_min_ >= u_max_)
379+
if (u_min_ >= u_max_) // is false if any value is nan
385380
{
386381
error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_);
387382
return false;
@@ -413,9 +408,8 @@ class Pid
413408
void print() const
414409
{
415410
std::cout << "Gains: p: " << p_gain_ << ", i: " << i_gain_ << ", d: " << d_gain_
416-
<< ", i_max: " << i_max_ << ", i_min: " << i_min_ << ", u_max: " << u_max_
417-
<< ", u_min: " << u_min_ << ", antiwindup: " << antiwindup_
418-
<< ", antiwindup_strat: " << antiwindup_strat_.to_string() << std::endl;
411+
<< ", u_max: " << u_max_ << ", u_min: " << u_min_ << std::endl;
412+
antiwindup_strat_.print();
419413
}
420414

421415
double p_gain_ = 0.0; /**< Proportional gain. */
@@ -465,7 +459,7 @@ class Pid
465459
'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the
466460
tracking_time_constant parameter to tune the anti-windup behavior.
467461
*
468-
* \throws An std::invalid_argument exception is thrown if u_min > u_max
462+
* \throws An std::invalid_argument exception is thrown if u_min >= u_max.
469463
*/
470464
Pid(
471465
double p, double i, double d, double u_max, double u_min,

‎control_toolbox/include/control_toolbox/pid_ros.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -238,7 +238,7 @@ class PidROS
238238
* \param save_i_term save integrator output between resets.
239239
* \return True if all parameters are successfully set, False otherwise.
240240
*
241-
* \note New gains are not applied if u_min_ > u_max_.
241+
* \note New gains are not applied if antiwindup_strat.i_min > 0, antiwindup_strat.i_max < 0 or u_min > u_max.
242242
*/
243243
bool initialize_from_args(
244244
double p, double i, double d, double u_max, double u_min,
@@ -369,7 +369,7 @@ class PidROS
369369
tracking_time_constant parameter to tune the anti-windup behavior.
370370
* \return True if all parameters are successfully set, False otherwise.
371371
*
372-
* \note New gains are not applied if u_min_ > u_max_.
372+
* \note New gains are not applied if antiwindup_strat.i_min > 0, antiwindup_strat.i_max < 0 or u_min > u_max.
373373
* \note This method is not RT safe
374374
*/
375375
bool set_gains(

‎control_toolbox/src/pid.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -77,9 +77,9 @@ Pid::Pid(
7777
double p, double i, double d, double u_max, double u_min,
7878
const AntiWindupStrategy & antiwindup_strat)
7979
{
80-
if (u_min > u_max)
80+
if (u_min >= u_max)
8181
{
82-
throw std::invalid_argument("received u_min > u_max");
82+
throw std::invalid_argument("received u_min >= u_max");
8383
}
8484
set_gains(p, i, d, u_max, u_min, antiwindup_strat);
8585

@@ -431,7 +431,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
431431
}
432432
}
433433

434-
i_term_ = std::clamp(i_term_, gains_.i_min_, gains_.i_max_);
434+
i_term_ = std::clamp(i_term_, gains_.antiwindup_strat_.i_min, gains_.antiwindup_strat_.i_max);
435435

436436
return cmd_;
437437
}

‎control_toolbox/src/pid_ros.cpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -555,13 +555,13 @@ void PidROS::print_values()
555555
<< " P Gain: " << gains.p_gain_ << "\n"
556556
<< " I Gain: " << gains.i_gain_ << "\n"
557557
<< " D Gain: " << gains.d_gain_ << "\n"
558-
<< " I Max: " << gains.i_max_ << "\n"
559-
<< " I Min: " << gains.i_min_ << "\n"
560558
<< " U_Max: " << gains.u_max_ << "\n"
561559
<< " U_Min: " << gains.u_min_ << "\n"
560+
<< " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\n"
562561
<< " Tracking_Time_Constant: " << gains.antiwindup_strat_.tracking_time_constant << "\n"
563562
<< " Antiwindup: " << gains.antiwindup_strat_.legacy_antiwindup << "\n"
564-
<< " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\n"
563+
<< " I Max: " << gains.antiwindup_strat_.i_max << "\n"
564+
<< " I Min: " << gains.antiwindup_strat_.i_min << "\n"
565565
<< "\n"
566566
<< " P Error: " << p_error << "\n"
567567
<< " I Term: " << i_term << "\n"
@@ -648,12 +648,12 @@ void PidROS::set_parameter_event_callback()
648648
}
649649
else if (param_name == param_prefix_ + "i_clamp_max")
650650
{
651-
gains.i_max_ = parameter.get_value<double>();
651+
gains.antiwindup_strat_.i_max = parameter.get_value<double>();
652652
changed = true;
653653
}
654654
else if (param_name == param_prefix_ + "i_clamp_min")
655655
{
656-
gains.i_min_ = parameter.get_value<double>();
656+
gains.antiwindup_strat_.i_min = parameter.get_value<double>();
657657
changed = true;
658658
}
659659
else if (param_name == param_prefix_ + "u_clamp_max")

‎control_toolbox/test/pid_ros_parameters_tests.cpp‎

Lines changed: 29 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -113,8 +113,8 @@ void check_set_parameters(
113113
ASSERT_EQ(gains.p_gain_, P);
114114
ASSERT_EQ(gains.i_gain_, I);
115115
ASSERT_EQ(gains.d_gain_, D);
116-
ASSERT_EQ(gains.i_max_, I_MAX);
117-
ASSERT_EQ(gains.i_min_, I_MIN);
116+
ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX);
117+
ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN);
118118
ASSERT_EQ(gains.u_max_, U_MAX);
119119
ASSERT_EQ(gains.u_min_, U_MIN);
120120
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
@@ -156,7 +156,10 @@ TEST(PidParametersTest, InitPidTestBadParameter)
156156
ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC;
157157
ANTIWINDUP_STRAT.legacy_antiwindup = false;
158158

159-
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false));
159+
bool ret;
160+
ASSERT_NO_THROW(
161+
ret = pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false));
162+
ASSERT_FALSE(ret);
160163

161164
rclcpp::Parameter param;
162165

@@ -178,13 +181,26 @@ TEST(PidParametersTest, InitPidTestBadParameter)
178181
ASSERT_EQ(gains.p_gain_, 0.0);
179182
ASSERT_EQ(gains.i_gain_, 0.0);
180183
ASSERT_EQ(gains.d_gain_, 0.0);
181-
ASSERT_EQ(gains.i_max_, std::numeric_limits<double>::infinity());
182-
ASSERT_EQ(gains.i_min_, -std::numeric_limits<double>::infinity());
184+
ASSERT_EQ(gains.antiwindup_strat_.i_max, std::numeric_limits<double>::infinity());
185+
ASSERT_EQ(gains.antiwindup_strat_.i_min, -std::numeric_limits<double>::infinity());
183186
ASSERT_EQ(gains.u_max_, std::numeric_limits<double>::infinity());
184187
ASSERT_EQ(gains.u_min_, -std::numeric_limits<double>::infinity());
185188
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, 0.0);
186189
ASSERT_FALSE(gains.antiwindup_);
187190
ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY);
191+
192+
// Try other invalid combinations
193+
ANTIWINDUP_STRAT.i_max = 10.;
194+
ANTIWINDUP_STRAT.i_min = 5.;
195+
ASSERT_NO_THROW(
196+
ret = pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false));
197+
ASSERT_FALSE(ret);
198+
199+
ANTIWINDUP_STRAT.i_max = -5.;
200+
ANTIWINDUP_STRAT.i_min = 10.;
201+
ASSERT_NO_THROW(
202+
ret = pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false));
203+
ASSERT_FALSE(ret);
188204
}
189205

190206
TEST(PidParametersTest, InitPid_param_prefix_only)
@@ -303,8 +319,8 @@ TEST(PidParametersTest, SetParametersTest)
303319
ASSERT_EQ(gains.p_gain_, P);
304320
ASSERT_EQ(gains.i_gain_, I);
305321
ASSERT_EQ(gains.d_gain_, D);
306-
ASSERT_EQ(gains.i_max_, I_MAX);
307-
ASSERT_EQ(gains.i_min_, I_MIN);
322+
ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX);
323+
ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN);
308324
ASSERT_EQ(gains.u_max_, U_MAX);
309325
ASSERT_EQ(gains.u_min_, U_MIN);
310326
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
@@ -383,8 +399,8 @@ TEST(PidParametersTest, SetBadParametersTest)
383399
ASSERT_EQ(gains.p_gain_, P);
384400
ASSERT_EQ(gains.i_gain_, I);
385401
ASSERT_EQ(gains.d_gain_, D);
386-
ASSERT_EQ(gains.i_max_, I_MAX);
387-
ASSERT_EQ(gains.i_min_, I_MIN);
402+
ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX);
403+
ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN);
388404
ASSERT_EQ(gains.u_max_, std::numeric_limits<double>::infinity());
389405
ASSERT_EQ(gains.u_min_, -std::numeric_limits<double>::infinity());
390406
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
@@ -406,8 +422,8 @@ TEST(PidParametersTest, SetBadParametersTest)
406422
ASSERT_EQ(gains.p_gain_, P);
407423
ASSERT_EQ(gains.i_gain_, I);
408424
ASSERT_EQ(gains.d_gain_, D);
409-
ASSERT_EQ(gains.i_max_, I_MAX);
410-
ASSERT_EQ(gains.i_min_, I_MIN);
425+
ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX);
426+
ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN);
411427
ASSERT_EQ(gains.u_max_, std::numeric_limits<double>::infinity());
412428
ASSERT_EQ(gains.u_min_, -std::numeric_limits<double>::infinity());
413429
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
@@ -426,8 +442,8 @@ TEST(PidParametersTest, SetBadParametersTest)
426442
ASSERT_EQ(updated_gains.p_gain_, P);
427443
ASSERT_EQ(updated_gains.i_gain_, I);
428444
ASSERT_EQ(updated_gains.d_gain_, D);
429-
ASSERT_EQ(updated_gains.i_max_, I_MAX);
430-
ASSERT_EQ(updated_gains.i_min_, I_MIN);
445+
ASSERT_EQ(updated_gains.antiwindup_strat_.i_max, I_MAX);
446+
ASSERT_EQ(updated_gains.antiwindup_strat_.i_min, I_MIN);
431447
ASSERT_EQ(updated_gains.u_max_, U_MAX);
432448
ASSERT_EQ(updated_gains.u_min_, U_MIN);
433449
ASSERT_EQ(updated_gains.antiwindup_strat_.tracking_time_constant, TRK_TC);

‎control_toolbox/test/pid_tests.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -496,8 +496,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
496496
EXPECT_EQ(p_gain, g1.p_gain_);
497497
EXPECT_EQ(i_gain, g1.i_gain_);
498498
EXPECT_EQ(d_gain, g1.d_gain_);
499-
EXPECT_EQ(i_max, g1.i_max_);
500-
EXPECT_EQ(i_min, g1.i_min_);
499+
EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max);
500+
EXPECT_EQ(i_min, g1.antiwindup_strat_.i_min);
501501
EXPECT_EQ(u_max, g1.u_max_);
502502
EXPECT_EQ(u_min, g1.u_min_);
503503
EXPECT_EQ(antiwindup, g1.antiwindup_);
@@ -521,8 +521,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
521521
EXPECT_EQ(p_gain_return, g1.p_gain_);
522522
EXPECT_EQ(i_gain_return, g1.i_gain_);
523523
EXPECT_EQ(d_gain_return, g1.d_gain_);
524-
EXPECT_EQ(antiwindup_strat_return.i_max, g1.i_max_);
525-
EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_);
524+
EXPECT_EQ(antiwindup_strat_return.i_max, g1.antiwindup_strat_.i_max);
525+
EXPECT_EQ(antiwindup_strat_return.i_min, g1.antiwindup_strat_.i_min);
526526
EXPECT_EQ(u_max, g1.u_max_);
527527
EXPECT_EQ(u_min, g1.u_min_);
528528
EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant);
@@ -549,8 +549,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
549549
EXPECT_EQ(p_gain_return, g1.p_gain_);
550550
EXPECT_EQ(i_gain_return, g1.i_gain_);
551551
EXPECT_EQ(d_gain_return, g1.d_gain_);
552-
EXPECT_EQ(antiwindup_strat_return.i_max, g1.i_max_);
553-
EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_);
552+
EXPECT_EQ(antiwindup_strat_return.i_max, g1.antiwindup_strat_.i_max);
553+
EXPECT_EQ(antiwindup_strat_return.i_min, g1.antiwindup_strat_.i_min);
554554
EXPECT_EQ(u_max, g1.u_max_);
555555
EXPECT_EQ(u_min, g1.u_min_);
556556
EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant);

0 commit comments

Comments
 (0)