Skip to content

Add three new anti-windup techniques and a Saturation feature #298

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.

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 40 commits into from
Jun 6, 2025
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
40 commits
Select commit Hold shift + click to select a range
d0698b0
Resolving conflicts
ViktorCVS Apr 24, 2025
172d008
Add unit tests
ViktorCVS Mar 13, 2025
a62ed65
Resolving conflicts
ViktorCVS Apr 24, 2025
3805a65
Add unit tests
ViktorCVS Mar 13, 2025
deca4e4
Adjust unit tests for new parameters
ViktorCVS Mar 13, 2025
1c04553
Modify cmd eq. to keep the anti-windup behavior
ViktorCVS Mar 18, 2025
6977d98
Fix license comments
ViktorCVS Apr 28, 2025
b026fd8
Separate i_bounds and u_bounds conditions
ViktorCVS Apr 28, 2025
c7c6919
Fix code style: use consistent brace styles
ViktorCVS Apr 28, 2025
f728af1
Remove and update comments
ViktorCVS Apr 28, 2025
9d915bd
Separate i_bounds and u_bounds conditions
ViktorCVS Apr 28, 2025
8e517ba
Move comment closer to corresponding equation
ViktorCVS Apr 28, 2025
246384f
Add helper function for zero comparisons
ViktorCVS Apr 28, 2025
c4c2ce6
Discard unused comments and fix comment formatting
ViktorCVS Apr 28, 2025
927a19b
Explicitly initialize variables to 0 in header
ViktorCVS Apr 28, 2025
437b555
Remove deprecated comment
ViktorCVS Apr 29, 2025
a5b8bda
add Doxygen trailing comments to Gains members
ViktorCVS Apr 29, 2025
d198843
add Doxygen trailing comments to Gains members
ViktorCVS Apr 29, 2025
78b0545
Add comment for gains_buffer_ in pid.hpp
ViktorCVS Apr 29, 2025
1138785
Add comment for gains_buffer_ in pid.hpp
ViktorCVS Apr 29, 2025
97f4d8d
Add comment for gains_buffer_ in pid.hpp
ViktorCVS Apr 29, 2025
28f9939
Add comment for gains_buffer_ in pid.hpp
ViktorCVS Apr 29, 2025
f4abce3
Add default value for trk_tc
ViktorCVS Apr 29, 2025
d603abc
remove is_not_zero() function
ViktorCVS Apr 29, 2025
ad16f36
Refactor PID antiwindup strategy variable
ViktorCVS May 5, 2025
c5096d5
Add deprecated message for the old anti-windup
ViktorCVS May 5, 2025
60f35fb
Add deprecated message for the old anti-windup
ViktorCVS May 5, 2025
560276e
Calculate tracking time constant in set_gains
ViktorCVS May 5, 2025
98f7ad7
Discard unused comments and fix comment formatting
ViktorCVS Apr 28, 2025
dd9be8c
Add comment for gains_buffer_ in pid.hpp
ViktorCVS Apr 29, 2025
7d7311a
Add comment for gains_buffer_ in pid.hpp
ViktorCVS Apr 29, 2025
47b8639
Add comment for gains_buffer_ in pid.hpp
ViktorCVS Apr 29, 2025
9610d41
Remove CONDITIONING_TECHNIQUE from all files
ViktorCVS May 29, 2025
5b3b630
Remove CONDITIONING_TECHNIQUE from all files
ViktorCVS May 30, 2025
a61cf71
Use iszero(x) to compare variables
ViktorCVS May 30, 2025
46f0e46
Add release note for this pull request
ViktorCVS May 30, 2025
1c15588
Add release note for this pull request
ViktorCVS May 30, 2025
7cc756b
Update doc/release_notes.rst
christophfroehlich May 30, 2025
4706ede
Merge branch 'ros2-master' into ros2-master
ViktorCVS Jun 3, 2025
257fb57
Deprecate old APIs and silence warnings
ViktorCVS Jun 4, 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
440 changes: 414 additions & 26 deletions control_toolbox/include/control_toolbox/pid.hpp

Large diffs are not rendered by default.

131 changes: 128 additions & 3 deletions control_toolbox/include/control_toolbox/pid_ros.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -92,13 +92,14 @@ class PidROS
* \param d The derivative gain.
* \param i_max Upper integral clamp.
* \param i_min Lower integral clamp.
* \param antiwindup Antiwindup functionality. When set to true, limits
* \param antiwindup Anti-windup functionality. When set to true, limits
the integral error to prevent windup; otherwise, constrains the
integral contribution to the control output. i_max and
i_min are applied in both scenarios.
*
* \note New gains are not applied if i_min_ > i_max_
*/
[[deprecated("Use initialize_from_args with AntiwindupStrategy instead.")]]
void initialize_from_args(
double p, double i, double d, double i_max, double i_min, bool antiwindup);

Expand All @@ -109,17 +110,80 @@ class PidROS
* \param d The derivative gain.
* \param i_max The max integral windup.
* \param i_min The min integral windup.
* \param antiwindup antiwindup.
* \param antiwindup Anti-windup functionality. When set to true, limits
the integral error to prevent windup; otherwise, constrains the
integral contribution to the control output. i_max and
i_min are applied in both scenarios.
* \param save_i_term save integrator output between resets.
*
* \note New gains are not applied if i_min_ > i_max_
*/
[[deprecated("Use initialize_from_args with AntiwindupStrategy instead.")]]
void initialize_from_args(
double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term);

/*!
* \brief Initialize the PID controller and set the parameters
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param i_max The max integral windup.
* \param i_min The min integral windup.
* \param u_max Upper output clamp.
* \param u_min Lower output clamp.
* \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set
* to 0.0 when this strategy is selected, a recommended default value will be applied.
* \param saturation Enables output saturation. When true, the controller output is
clamped between u_max and u_min.
* \param antiwindup Anti-windup functionality. When set to true, limits
the integral error to prevent windup; otherwise, constrains the
integral contribution to the control output. i_max and
i_min are applied in both scenarios.
* \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation',
'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the
tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other
than 'none' is selected, it will override the controller's default anti-windup behavior.
* \deprecated{only when `antiwindup_strat == AntiwindupStrategy::NONE`:}
* Old anti-windup technique is deprecated and will be removed by
* the ROS 2 Kilted Kaiju release.
* \warning{If you pass `AntiwindupStrategy::NONE`, at runtime a warning will be printed:}
* `"Old anti-windup technique is deprecated. This option will be removed by the ROS 2 Kilted Kaiju release."`
* \param save_i_term save integrator output between resets.
*
* \note New gains are not applied if i_min_ > i_max_ or if u_min_ > u_max_.
*/
[[deprecated("Use initialize_from_args with AntiwindupStrategy only.")]]
void initialize_from_args(
double p, double i, double d, double i_max, double i_min, double u_max, double u_min,
double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat,
bool save_i_term);

/*!
* \brief Initialize the PID controller and set the parameters.
*
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param u_max Upper output clamp.
* \param u_min Lower output clamp.
* \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set
* to 0.0 when this strategy is selected, a recommended default value will be applied.
* \param saturation Enables output saturation. When true, the controller output is
clamped between u_max and u_min.
* \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation',
'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the
tracking_time_constant parameter to tune the anti-windup behavior.
* \param save_i_term save integrator output between resets.
*
* \note New gains are not applied if u_min_ > u_max_.
*/
void initialize_from_args(
double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation,
AntiwindupStrategy antiwindup_strat, bool save_i_term);

/*!
* \brief Initialize the PID controller based on already set parameters
* \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise
* \return True if all parameters are set (p, i, d, i_max, i_min, u_max, u_min and trk_tc), False otherwise
*/
bool initialize_from_ros_parameters();

Expand Down Expand Up @@ -182,8 +246,67 @@ class PidROS
*
* \note New gains are not applied if i_min > i_max
*/
[[deprecated("Use set_gains with AntiwindupStrategy instead.")]]
void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);

/*!
* \brief Initialize the PID controller and set the parameters
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param i_max The max integral windup.
* \param i_min The min integral windup.
* \param u_max Upper output clamp.
* \param u_min Lower output clamp.
* \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set
* to 0.0 when this strategy is selected, a recommended default value will be applied.
* \param saturation Enables output saturation. When true, the controller output is
clamped between u_max and u_min.
* \param antiwindup Anti-windup functionality. When set to true, limits
the integral error to prevent windup; otherwise, constrains the
integral contribution to the control output. i_max and
i_min are applied in both scenarios.
* \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation',
'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the
tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other
than 'none' is selected, it will override the controller's default anti-windup behavior.
* \deprecated{only when `antiwindup_strat == AntiwindupStrategy::NONE`:}
* Old anti-windup technique is deprecated and will be removed by
* the ROS 2 Kilted Kaiju release.
* \warning{If you pass `AntiwindupStrategy::NONE`, at runtime a warning will be printed:}
* `"Old anti-windup technique is deprecated. This option will be removed by
* the ROS 2 Kilted Kaiju release."`
*
* \note New gains are not applied if i_min > i_max or if u_min_ > u_max_.
*/
[[deprecated("Use set_gains with AntiwindupStrategy only.")]]
void set_gains(
double p, double i, double d, double i_max, double i_min, double u_max, double u_min,
double trk_tc = 0.0, bool saturation = false, bool antiwindup = false,
AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE);

/*!
* \brief Set PID gains for the controller (preferred).
*
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param u_max Upper output clamp.
* \param u_min Lower output clamp.
* \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set
* to 0.0 when this strategy is selected, a recommended default value will be applied.
* \param saturation Enables output saturation. When true, the controller output is
clamped between u_max and u_min.
* \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation',
'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the
tracking_time_constant parameter to tune the anti-windup behavior.
*
* \note New gains are not applied if u_min_ > u_max_.
*/
void set_gains(
double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation,
AntiwindupStrategy antiwindup_strat);

/*!
* \brief Set PID gains for the controller.
* \param gains A struct of the PID gain values
Expand Down Expand Up @@ -248,6 +371,8 @@ class PidROS

bool get_boolean_param(const std::string & param_name, bool & value);

bool get_string_param(const std::string & param_name, std::string & value);

/*!
* \brief Set prefix for topic and parameter names
* \param[in] topic_prefix prefix to add to the pid parameters.
Expand Down
Loading
Loading