Skip to content

Commit f3b0b0e

Browse files
christophfroehlichmergify[bot]
christophfroehlich
authored and
mergify[bot]
committed
Improve PID parameter validation (#510)
(cherry picked from commit 84a0c5c) # Conflicts: # control_toolbox/include/control_toolbox/pid.hpp # control_toolbox/include/control_toolbox/pid_ros.hpp # control_toolbox/src/pid_ros.cpp # control_toolbox/test/pid_ros_parameters_tests.cpp
1 parent 44af78e commit f3b0b0e

File tree

6 files changed

+92
-45
lines changed

6 files changed

+92
-45
lines changed

‎control_toolbox/include/control_toolbox/pid.hpp‎

Lines changed: 34 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -135,11 +135,23 @@ struct AntiWindupStrategy
135135
"AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant "
136136
"(tracking_time_constant)");
137137
}
138+
<<<<<<< HEAD
138139
if (i_min > i_max)
139140
{
140141
throw std::invalid_argument(
141142
fmt::format(
142143
"PID requires i_min <= i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max));
144+
=======
145+
if (i_min > 0)
146+
{
147+
throw std::invalid_argument(
148+
fmt::format("PID requires i_min to be smaller or equal to 0 (i_min: {})", i_min));
149+
}
150+
if (i_max < 0)
151+
{
152+
throw std::invalid_argument(
153+
fmt::format("PID requires i_max to be greater or equal to 0 (i_max: {})", i_max));
154+
>>>>>>> 84a0c5c (Improve PID parameter validation (#510))
143155
}
144156
if (
145157
type != NONE && type != UNDEFINED && type != LEGACY && type != BACK_CALCULATION &&
@@ -149,6 +161,13 @@ struct AntiWindupStrategy
149161
}
150162
}
151163

164+
void print() const
165+
{
166+
std::cout << "antiwindup_strat: " << to_string() << "\ti_max: " << i_max << ", i_min: " << i_min
167+
<< "\ttracking_time_constant: " << tracking_time_constant
168+
<< "\terror_deadband: " << error_deadband << std::endl;
169+
}
170+
152171
operator std::string() const { return to_string(); }
153172

154173
constexpr bool operator==(Value other) const { return type == other; }
@@ -354,34 +373,21 @@ class Pid
354373
: p_gain_(p),
355374
i_gain_(i),
356375
d_gain_(d),
357-
i_max_(antiwindup_strat.i_max),
358-
i_min_(antiwindup_strat.i_min),
359376
u_max_(u_max),
360377
u_min_(u_min),
378+
<<<<<<< HEAD
361379
antiwindup_(antiwindup_strat.legacy_antiwindup),
380+
=======
381+
i_max_(antiwindup_strat.i_max),
382+
i_min_(antiwindup_strat.i_min),
383+
>>>>>>> 84a0c5c (Improve PID parameter validation (#510))
362384
antiwindup_strat_(antiwindup_strat)
363385
{
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-
}
375386
}
376387

377388
bool validate(std::string & error_msg) const
378389
{
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_)
390+
if (u_min_ >= u_max_) // is false if any value is nan
385391
{
386392
error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_);
387393
return false;
@@ -413,9 +419,14 @@ class Pid
413419
void print() const
414420
{
415421
std::cout << "Gains: p: " << p_gain_ << ", i: " << i_gain_ << ", d: " << d_gain_
422+
<<<<<<< HEAD
416423
<< ", i_max: " << i_max_ << ", i_min: " << i_min_ << ", u_max: " << u_max_
417424
<< ", u_min: " << u_min_ << ", antiwindup: " << antiwindup_
418425
<< ", antiwindup_strat: " << antiwindup_strat_.to_string() << std::endl;
426+
=======
427+
<< ", u_max: " << u_max_ << ", u_min: " << u_min_ << std::endl;
428+
antiwindup_strat_.print();
429+
>>>>>>> 84a0c5c (Improve PID parameter validation (#510))
419430
}
420431

421432
double p_gain_ = 0.0; /**< Proportional gain. */
@@ -465,7 +476,11 @@ class Pid
465476
'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the
466477
tracking_time_constant parameter to tune the anti-windup behavior.
467478
*
479+
<<<<<<< HEAD
468480
* \throws An std::invalid_argument exception is thrown if u_min > u_max
481+
=======
482+
* \throws An std::invalid_argument exception is thrown if u_min >= u_max.
483+
>>>>>>> 84a0c5c (Improve PID parameter validation (#510))
469484
*/
470485
Pid(
471486
double p, double i, double d, double u_max, double u_min,

‎control_toolbox/include/control_toolbox/pid_ros.hpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -238,7 +238,11 @@ 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+
<<<<<<< HEAD
241242
* \note New gains are not applied if u_min_ > u_max_.
243+
=======
244+
* \note New gains are not applied if antiwindup_strat.i_min > 0, antiwindup_strat.i_max < 0 or u_min > u_max.
245+
>>>>>>> 84a0c5c (Improve PID parameter validation (#510))
242246
*/
243247
bool initialize_from_args(
244248
double p, double i, double d, double u_max, double u_min,
@@ -369,7 +373,11 @@ class PidROS
369373
tracking_time_constant parameter to tune the anti-windup behavior.
370374
* \return True if all parameters are successfully set, False otherwise.
371375
*
376+
<<<<<<< HEAD
372377
* \note New gains are not applied if u_min_ > u_max_.
378+
=======
379+
* \note New gains are not applied if antiwindup_strat.i_min > 0, antiwindup_strat.i_max < 0 or u_min > u_max.
380+
>>>>>>> 84a0c5c (Improve PID parameter validation (#510))
373381
* \note This method is not RT safe
374382
*/
375383
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: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -555,13 +555,17 @@ 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+
<<<<<<< HEAD
562561
<< " Tracking_Time_Constant: " << gains.antiwindup_strat_.tracking_time_constant << "\n"
563562
<< " Antiwindup: " << gains.antiwindup_strat_.legacy_antiwindup << "\n"
563+
=======
564+
>>>>>>> 84a0c5c (Improve PID parameter validation (#510))
564565
<< " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\n"
566+
<< " Tracking_Time_Constant: " << gains.antiwindup_strat_.tracking_time_constant << "\n"
567+
<< " I Max: " << gains.antiwindup_strat_.i_max << "\n"
568+
<< " I Min: " << gains.antiwindup_strat_.i_min << "\n"
565569
<< "\n"
566570
<< " P Error: " << p_error << "\n"
567571
<< " I Term: " << i_term << "\n"
@@ -648,12 +652,12 @@ void PidROS::set_parameter_event_callback()
648652
}
649653
else if (param_name == param_prefix_ + "i_clamp_max")
650654
{
651-
gains.i_max_ = parameter.get_value<double>();
655+
gains.antiwindup_strat_.i_max = parameter.get_value<double>();
652656
changed = true;
653657
}
654658
else if (param_name == param_prefix_ + "i_clamp_min")
655659
{
656-
gains.i_min_ = parameter.get_value<double>();
660+
gains.antiwindup_strat_.i_min = parameter.get_value<double>();
657661
changed = true;
658662
}
659663
else if (param_name == param_prefix_ + "u_clamp_max")

‎control_toolbox/test/pid_ros_parameters_tests.cpp‎

Lines changed: 33 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,30 @@ 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);
189+
<<<<<<< HEAD
186190
ASSERT_FALSE(gains.antiwindup_);
187191
ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY);
192+
=======
193+
ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE);
194+
195+
// Try other invalid combinations
196+
ANTIWINDUP_STRAT.i_max = 10.;
197+
ANTIWINDUP_STRAT.i_min = 5.;
198+
ASSERT_NO_THROW(
199+
ret = pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false));
200+
ASSERT_FALSE(ret);
201+
202+
ANTIWINDUP_STRAT.i_max = -5.;
203+
ANTIWINDUP_STRAT.i_min = 10.;
204+
ASSERT_NO_THROW(
205+
ret = pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false));
206+
ASSERT_FALSE(ret);
207+
>>>>>>> 84a0c5c (Improve PID parameter validation (#510))
188208
}
189209

190210
TEST(PidParametersTest, InitPid_param_prefix_only)
@@ -303,8 +323,8 @@ TEST(PidParametersTest, SetParametersTest)
303323
ASSERT_EQ(gains.p_gain_, P);
304324
ASSERT_EQ(gains.i_gain_, I);
305325
ASSERT_EQ(gains.d_gain_, D);
306-
ASSERT_EQ(gains.i_max_, I_MAX);
307-
ASSERT_EQ(gains.i_min_, I_MIN);
326+
ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX);
327+
ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN);
308328
ASSERT_EQ(gains.u_max_, U_MAX);
309329
ASSERT_EQ(gains.u_min_, U_MIN);
310330
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
@@ -383,8 +403,8 @@ TEST(PidParametersTest, SetBadParametersTest)
383403
ASSERT_EQ(gains.p_gain_, P);
384404
ASSERT_EQ(gains.i_gain_, I);
385405
ASSERT_EQ(gains.d_gain_, D);
386-
ASSERT_EQ(gains.i_max_, I_MAX);
387-
ASSERT_EQ(gains.i_min_, I_MIN);
406+
ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX);
407+
ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN);
388408
ASSERT_EQ(gains.u_max_, std::numeric_limits<double>::infinity());
389409
ASSERT_EQ(gains.u_min_, -std::numeric_limits<double>::infinity());
390410
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
@@ -406,8 +426,8 @@ TEST(PidParametersTest, SetBadParametersTest)
406426
ASSERT_EQ(gains.p_gain_, P);
407427
ASSERT_EQ(gains.i_gain_, I);
408428
ASSERT_EQ(gains.d_gain_, D);
409-
ASSERT_EQ(gains.i_max_, I_MAX);
410-
ASSERT_EQ(gains.i_min_, I_MIN);
429+
ASSERT_EQ(gains.antiwindup_strat_.i_max, I_MAX);
430+
ASSERT_EQ(gains.antiwindup_strat_.i_min, I_MIN);
411431
ASSERT_EQ(gains.u_max_, std::numeric_limits<double>::infinity());
412432
ASSERT_EQ(gains.u_min_, -std::numeric_limits<double>::infinity());
413433
ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC);
@@ -426,8 +446,8 @@ TEST(PidParametersTest, SetBadParametersTest)
426446
ASSERT_EQ(updated_gains.p_gain_, P);
427447
ASSERT_EQ(updated_gains.i_gain_, I);
428448
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);
449+
ASSERT_EQ(updated_gains.antiwindup_strat_.i_max, I_MAX);
450+
ASSERT_EQ(updated_gains.antiwindup_strat_.i_min, I_MIN);
431451
ASSERT_EQ(updated_gains.u_max_, U_MAX);
432452
ASSERT_EQ(updated_gains.u_min_, U_MIN);
433453
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)