From 092e8e8605e6a2ac783c18b422b39ecb0dbf45b1 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Thu, 24 Apr 2025 17:49:02 +0000 Subject: [PATCH 01/28] Resolving conflicts --- .../include/control_toolbox/pid.hpp | 209 ++++++++++++++++-- .../include/control_toolbox/pid_ros.hpp | 75 ++++++- control_toolbox/src/pid.cpp | 107 +++++++-- control_toolbox/src/pid_ros.cpp | 184 ++++++++++----- 4 files changed, 475 insertions(+), 100 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 1e7bf7ef..3120019d 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -34,6 +34,7 @@ #define CONTROL_TOOLBOX__PID_HPP_ #include +#include #include "rclcpp/duration.hpp" #include "realtime_tools/realtime_buffer.hpp" @@ -82,7 +83,12 @@ namespace control_toolbox \param i Integral gain - \param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$ + \param i_clamp Minimum and maximum bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$ + + \param u_clamp Minimum and maximum bounds for the controller output. The clamp is applied to the \f$command\f$. + + \param trk_tc Tracking time constant for the 'back_calculation' and 'conditioning_technique' strategies. + \section Usage @@ -114,8 +120,8 @@ class Pid */ struct Gains { - /*! - * \brief Optional constructor for passing in values without antiwindup + /*! + * \brief Optional constructor for passing in values without antiwindup and saturation * * \param p The proportional gain. * \param i The integral gain. @@ -125,7 +131,28 @@ class Pid * */ Gains(double p, double i, double d, double i_max, double i_min) - : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(true) + : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), u_max_(0.0), u_min_(0.0), + trk_tc_(0.0), saturation_(false), antiwindup_(true), antiwindup_strat_("none") + { + } + + /*! + * \brief Optional constructor for passing in values without saturation + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \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. + * + */ + Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) + : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), u_max_(0.0), u_min_(0.0), + trk_tc_(0.0), saturation_(false), antiwindup_(antiwindup), antiwindup_strat_("none") { } @@ -137,28 +164,49 @@ class Pid * \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 u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies 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 Choose the anti-windup strategy. Options: 'back_calculation', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies 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. * */ - Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) - : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup) + Gains(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, std::string antiwindup_strat) + : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), u_max_(u_max), + u_min_(u_min), trk_tc_(trk_tc), saturation_(saturation), antiwindup_(antiwindup), + antiwindup_strat_(antiwindup_strat) { } // Default constructor - Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0), antiwindup_(false) + Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0), u_max_(0.0), + u_min_(0.0), trk_tc_(0.0), saturation_(false), antiwindup_(false), antiwindup_strat_("none") { } - double p_gain_; /**< Proportional gain. */ - double i_gain_; /**< Integral gain. */ - double d_gain_; /**< Derivative gain. */ - double i_max_; /**< Maximum allowable integral term. */ - double i_min_; /**< Minimum allowable integral term. */ - bool antiwindup_; /**< Antiwindup. */ + double p_gain_; /**< Proportional gain. */ + double i_gain_; /**< Integral gain. */ + double d_gain_; /**< Derivative gain. */ + double i_max_; /**< Maximum allowable integral term. */ + double i_min_; /**< Minimum allowable integral term. */ + double u_max_; /**< Maximum allowable output. */ + double u_min_; /**< Minimum allowable output. */ + double trk_tc_; /**< Tracking time constant. */ + bool saturation_; /**< Saturation. */ + bool antiwindup_; /**< Anti-windup. */ + std::string antiwindup_strat_; /**< Anti-windup strategy. */ }; /*! @@ -171,7 +219,7 @@ class Pid * \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. @@ -182,6 +230,39 @@ class Pid double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0, bool antiwindup = false); + /*! + * \brief Constructor, zeros out Pid values when created and + * initialize Pid-gains and integral term limits. + * Does not initialize dynamic reconfigure for PID gains + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies 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 Choose the anti-windup strategy. Options: 'back_calculation', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies 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. + * + * \throws An std::invalid_argument exception is thrown if i_min > i_max or u_min > u_max + */ + Pid(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, std::string antiwindup_strat); + /** * \brief Copy constructor required for preventing mutexes from being copied * \param source - Pid to copy @@ -202,7 +283,7 @@ class Pid * \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. @@ -212,6 +293,37 @@ class Pid void initialize( double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + /*! + * \brief Optional constructor for passing in values + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies 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 Choose the anti-windup strategy. Options: 'back_calculation', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies 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. + * + * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max + */ + void initialize( + 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, std::string antiwindup_strat); + /*! * \brief Reset the state of this PID controller * @note The integral term is not retained and it is reset to zero @@ -247,7 +359,7 @@ class Pid * \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. @@ -255,6 +367,35 @@ class Pid void get_gains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup); + /*! + * \brief Get PID gains for the controller. + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies 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 Choose the anti-windup strategy. Options: 'back_calculation', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies 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. + */ + void get_gains( + 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, + std::string & antiwindup_strat); + /*! * \brief Get PID gains for the controller. * \return gains A struct of the PID gain values @@ -268,7 +409,7 @@ class Pid * \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. @@ -277,6 +418,37 @@ class Pid */ void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + /*! + * \brief Optional constructor for passing in values + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies 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 Choose the anti-windup strategy. Options: 'back_calculation', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies 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. + * + * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max + */ + void set_gains(double p, double i, double d, double i_max, double i_min, double u_max, + double u_min, double trk_tc, bool saturation = false, bool antiwindup = false, + std::string antiwindup_strat = "none"); + /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values @@ -435,6 +607,7 @@ class Pid double d_error_; /** Derivative of error. */ double i_term_; /** Integrator state. */ double cmd_; /** Command to send. */ + double cmd_unsat_; /** command without saturation. */ }; } // namespace control_toolbox diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index c9996e13..86f7ffb5 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -92,7 +92,7 @@ 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. @@ -109,7 +109,10 @@ 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_ @@ -117,9 +120,42 @@ class PidROS 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' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies 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', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies 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. + * \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_. + */ + 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, std::string 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(); @@ -184,6 +220,37 @@ class PidROS */ 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' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies 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', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies 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. + * + * \note New gains are not applied if i_min > i_max or if u_min_ > u_max_. + */ + void set_gains(double p, double i, double d, double i_max, double i_min, double u_max, + double u_min, double trk_tc, bool saturation = false, bool antiwindup = false, + std::string antiwindup_strat = "none"); + /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values @@ -248,6 +315,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. diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 54621b15..36083dc5 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -48,13 +48,19 @@ namespace control_toolbox { Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) + : Pid(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none") +{ +} + +Pid::Pid(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, std::string antiwindup_strat) : gains_buffer_() { - if (i_min > i_max) - { - throw std::invalid_argument("received i_min > i_max"); + if (i_min > i_max || u_min > u_max) { + throw std::invalid_argument("received i_min > i_max or u_min > u_max"); } - set_gains(p, i, d, i_max, i_min, antiwindup); + set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, + antiwindup, antiwindup_strat); // Initialize saved i-term values clear_saved_iterm(); @@ -78,7 +84,15 @@ Pid::~Pid() {} void Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - set_gains(p, i, d, i_max, i_min, antiwindup); + set_gains(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none"); + + reset(); +} + +void Pid::initialize(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, std::string antiwindup_strat) +{ + set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); reset(); } @@ -103,12 +117,30 @@ void Pid::clear_saved_iterm() { i_term_ = 0.0; } void Pid::get_gains(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; - get_gains(p, i, d, i_max, i_min, antiwindup); + std::string antiwindup_strat; + get_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); } void Pid::get_gains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup) +{ + double u_max; + double u_min; + double trk_tc; + bool saturation; + std::string antiwindup_strat; + get_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); +} + +void Pid::get_gains( + 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, + std::string & antiwindup_strat) { Gains gains = *gains_buffer_.readFromRT(); @@ -117,7 +149,12 @@ void Pid::get_gains( d = gains.d_gain_; i_max = gains.i_max_; i_min = gains.i_min_; + u_max = gains.u_max_; + u_min = gains.u_min_; + trk_tc = gains.trk_tc_; + saturation = gains.saturation_; antiwindup = gains.antiwindup_; + antiwindup_strat = gains.antiwindup_strat_; } Pid::Gains Pid::get_gains() { return *gains_buffer_.readFromRT(); } @@ -129,14 +166,20 @@ void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bo set_gains(gains); } +void Pid::set_gains(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, std::string antiwindup_strat) +{ + Gains gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, + antiwindup_strat); + + set_gains(gains); +} + void Pid::set_gains(const Gains & gains) { - if (gains.i_min_ > gains.i_max_) - { - std::cout << "received i_min > i_max, skip new gains\n"; - } - else - { + if (gains.i_min_ > gains.i_max_ || gains.u_min_ > gains.u_max_) { + std::cout << "received i_min > i_max or u_min > u_max_, skip new gains\n"; + } else { gains_buffer_.writeFromNonRT(gains); } } @@ -225,9 +268,11 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) // Calculate proportional contribution to command p_term = gains.p_gain_ * p_error_; + // Calculate derivative contribution to command + d_term = gains.d_gain_ * d_error_; + // Calculate integral contribution to command - if (gains.antiwindup_) - { + if (gains.antiwindup_ == true && gains.antiwindup_strat_ == "none") { // Prevent i_term_ from climbing higher than permitted by i_max_/i_min_ i_term_ = std::clamp(i_term_ + gains.i_gain_ * dt_s * p_error_, gains.i_min_, gains.i_max_); } @@ -236,12 +281,38 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) i_term_ += gains.i_gain_ * dt_s * p_error_; } - // Calculate derivative contribution to command - d_term = gains.d_gain_ * d_error_; - // Compute the command // Limit i_term so that the limit is meaningful in the output - cmd_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; + // Compute the command + cmd_unsat_ = p_term + i_term_ + d_term; + + if (gains.saturation_ == true) { + // Limit cmd_ if saturation is enabled + cmd_ = std::clamp(cmd_unsat_, gains.u_min_, gains.u_max_); + } else { + cmd_ = cmd_unsat_; + } + + if (gains.antiwindup_strat_ == "back_calculation") { + if (gains.trk_tc_ == 0.0 && gains.d_gain_ != 0.0) { + // Default value for tracking time constant for back calculation technique + gains.trk_tc_ = std::sqrt(gains.d_gain_/gains.i_gain_); + } else if (gains.trk_tc_ == 0.0 && gains.d_gain_ == 0.0) { + // Default value for tracking time constant for back calculation technique + gains.trk_tc_ = gains.p_gain_/gains.i_gain_; + } + i_term_ += dt_s * (gains.i_gain_ * error + 1/gains.trk_tc_ * (cmd_ - cmd_unsat_)); + } else if (gains.antiwindup_strat_ == "conditioning_technique") { + if (gains.trk_tc_ == 0.0) { + // Default value for tracking time constant for conditioning technique + gains.trk_tc_ = gains.p_gain_; + } + i_term_ += dt_s * gains.i_gain_ * (error + 1/gains.trk_tc_ * (cmd_ - cmd_unsat_)); + } else if (gains.antiwindup_strat_ == "conditional_integration") { + if (!(cmd_unsat_ != cmd_ && error * cmd_unsat_ > 0)) { + i_term_ += dt_s * gains.i_gain_ * error; + } + } return cmd_; } diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 63952487..70c5a8fa 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -20,18 +20,8 @@ // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include -#include +// FOR A PARTICULAR git config pull.rebase true + #include #include #include @@ -182,19 +172,53 @@ bool PidROS::get_double_param(const std::string & param_name, double & value) } } +bool PidROS::get_string_param(const std::string & param_name, std::string & value) +{ + declare_param(param_name, rclcpp::ParameterValue(value)); + rclcpp::Parameter param; + if (node_params_->has_parameter(param_name)) { + node_params_->get_parameter(param_name, param); + if (rclcpp::PARAMETER_STRING != param.get_type()) { + RCLCPP_ERROR( + node_logging_->get_logger(), "Wrong parameter type '%s', not string", param_name.c_str()); + return false; + } + value = param.as_string(); + RCLCPP_DEBUG_STREAM( + node_logging_->get_logger(), "parameter '" << param_name << "' in node '" + << node_base_->get_name() << "' value is " << value + << std::endl); + return true; + } else { + RCLCPP_ERROR_STREAM( + node_logging_->get_logger(), "parameter '" << param_name << "' in node '" + << node_base_->get_name() << "' does not exists" + << std::endl); + return false; + } +} + bool PidROS::initialize_from_ros_parameters() { - double p, i, d, i_min, i_max; - p = i = d = i_min = i_max = std::numeric_limits::quiet_NaN(); + double p, i, d, i_max, i_min, u_max, u_min, trk_tc; + p = i = d = i_max = i_min = u_max = u_min = trk_tc = std::numeric_limits::quiet_NaN(); + bool saturation = false; bool antiwindup = false; + std::string antiwindup_strat = "none"; bool all_params_available = true; + all_params_available &= get_double_param(param_prefix_ + "p", p); all_params_available &= get_double_param(param_prefix_ + "i", i); all_params_available &= get_double_param(param_prefix_ + "d", d); all_params_available &= get_double_param(param_prefix_ + "i_clamp_max", i_max); all_params_available &= get_double_param(param_prefix_ + "i_clamp_min", i_min); + all_params_available &= get_double_param(param_prefix_ + "u_clamp_max", u_max); + all_params_available &= get_double_param(param_prefix_ + "u_clamp_min", u_min); + all_params_available &= get_double_param(param_prefix_ + "tracking_time_constant", trk_tc); + get_boolean_param(param_prefix_ + "saturation", saturation); get_boolean_param(param_prefix_ + "antiwindup", antiwindup); + get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(false)); if (all_params_available) @@ -202,7 +226,8 @@ bool PidROS::initialize_from_ros_parameters() set_parameter_event_callback(); } - pid_.initialize(p, i, d, i_max, i_min, antiwindup); + pid_.initialize(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, + antiwindup, antiwindup_strat); return all_params_available; } @@ -218,26 +243,38 @@ void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValu void PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup) { - initialize_from_args(p, i, d, i_max, i_min, antiwindup, false); + initialize_from_args(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none", false); } void PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term) { - if (i_min > i_max) - { - RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); - } - else - { - pid_.initialize(p, i, d, i_max, i_min, antiwindup); + initialize_from_args(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, + false, antiwindup, "none", save_i_term); +} + +void PidROS::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, + std::string antiwindup_strat, bool save_i_term) +{ + if (i_min > i_max || u_min > u_max) { + RCLCPP_ERROR(node_logging_->get_logger(), + "received i_min > i_max or u_min > u_max, skip new gains"); + } else { + pid_.initialize(p, i, d, i_max, i_min, u_max, u_min, trk_tc, + saturation, antiwindup, antiwindup_strat); declare_param(param_prefix_ + "p", rclcpp::ParameterValue(p)); declare_param(param_prefix_ + "i", rclcpp::ParameterValue(i)); declare_param(param_prefix_ + "d", rclcpp::ParameterValue(d)); declare_param(param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(i_max)); declare_param(param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(i_min)); + declare_param(param_prefix_ + "u_clamp_max", rclcpp::ParameterValue(u_max)); + declare_param(param_prefix_ + "u_clamp_min", rclcpp::ParameterValue(u_min)); + declare_param(param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(trk_tc)); + declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(saturation)); declare_param(param_prefix_ + "antiwindup", rclcpp::ParameterValue(antiwindup)); + declare_param(param_prefix_ + "antiwindup_strategy", rclcpp::ParameterValue(antiwindup_strat)); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); set_parameter_event_callback(); @@ -276,20 +313,31 @@ Pid::Gains PidROS::get_gains() { return pid_.get_gains(); } void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - if (i_min > i_max) - { - RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); - } - else - { + set_gains(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none"); +} + +void PidROS::set_gains(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, std::string antiwindup_strat) +{ + if (i_min > i_max || u_min > u_max) { + RCLCPP_ERROR(node_logging_->get_logger(), + "received i_min > i_max or u_min > u_max, skip new gains"); + } else { node_params_->set_parameters( - {rclcpp::Parameter(param_prefix_ + "p", p), rclcpp::Parameter(param_prefix_ + "i", i), + {rclcpp::Parameter(param_prefix_ + "p", p), + rclcpp::Parameter(param_prefix_ + "i", i), rclcpp::Parameter(param_prefix_ + "d", d), rclcpp::Parameter(param_prefix_ + "i_clamp_max", i_max), rclcpp::Parameter(param_prefix_ + "i_clamp_min", i_min), - rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup)}); - - pid_.set_gains(p, i, d, i_max, i_min, antiwindup); + rclcpp::Parameter(param_prefix_ + "u_clamp_max", u_max), + rclcpp::Parameter(param_prefix_ + "u_clamp_min", u_min), + rclcpp::Parameter(param_prefix_ + "tracking_time_constant", trk_tc), + rclcpp::Parameter(param_prefix_ + "saturation", saturation), + rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup), + rclcpp::Parameter(param_prefix_ + "antiwindup_strategy", antiwindup_strat)}); + + pid_.set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, + antiwindup, antiwindup_strat); } } @@ -340,28 +388,31 @@ void PidROS::print_values() get_current_pid_errors(p_error, i_term, d_error); RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Current Values of PID template:\n" - << " 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" - << " Antiwindup: " << gains.antiwindup_ - << "\n" - << " P_Error: " << p_error << "\n" - << " i_term: " << i_term << "\n" - << " D_Error: " << d_error << "\n" - << " Command: " << get_current_cmd();); + << " 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.trk_tc_ << "\n" + << " Saturation: " << gains.saturation_ << "\n" + << " Antiwindup: " << gains.antiwindup_ << "\n" + << " Antiwindup_Strategy: " << gains.antiwindup_strat_ << "\n" + << "\n" + << " P Error: " << p_error << "\n" + << " I Term: " << i_term << "\n" + << " D Error: " << d_error << "\n" + << " Command: " << get_current_cmd();); } void PidROS::set_gains(const Pid::Gains & gains) { - if (gains.i_min_ > gains.i_max_) - { - RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); - } - else - { - pid_.set_gains(gains); + if (gains.i_min_ > gains.i_max_ || gains.u_min_ > gains.u_max_) { + RCLCPP_ERROR(node_logging_->get_logger(), + "received i_min > i_max or u_min > u_max, skip new gains"); + } else { + pid_.set_gains(gains); } } @@ -405,11 +456,24 @@ void PidROS::set_parameter_event_callback() { gains.i_min_ = parameter.get_value(); changed = true; - } - else if (param_name == param_prefix_ + "antiwindup") - { + } else if (param_name == param_prefix_ + "u_clamp_max") { + gains.u_max_ = parameter.get_value(); + changed = true; + } else if (param_name == param_prefix_ + "u_clamp_min") { + gains.u_min_ = parameter.get_value(); + changed = true; + } else if (param_name == param_prefix_ + "tracking_time_constant") { + gains.trk_tc_ = parameter.get_value(); + changed = true; + } else if (param_name == param_prefix_ + "saturation") { + gains.saturation_ = parameter.get_value(); + changed = true; + } else if (param_name == param_prefix_ + "antiwindup") { gains.antiwindup_ = parameter.get_value(); changed = true; + } else if (param_name == param_prefix_ + "antiwindup_strategy") { + gains.antiwindup_strat_ = parameter.get_value(); + changed = true; } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) @@ -421,13 +485,11 @@ void PidROS::set_parameter_event_callback() if (changed) { /// @note don't call set_gains() from inside a callback - if (gains.i_min_ > gains.i_max_) - { - RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); - } - else - { - pid_.set_gains(gains); + if (gains.i_min_ > gains.i_max_ || gains.u_min_ > gains.u_max_) { + RCLCPP_ERROR(node_logging_->get_logger(), + "received i_min > i_max or u_min > u_max, skip new gains"); + } else { + pid_.set_gains(gains); } } From 9fb43675456eb863db7e5ec9ae1b354713629233 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Thu, 13 Mar 2025 12:19:20 +0000 Subject: [PATCH 02/28] Add unit tests Add 3 unit tests for saturation feature. --- control_toolbox/test/pid_tests.cpp | 84 ++++++++++++++++++++++++++++++ 1 file changed, 84 insertions(+) diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index c2a75ee8..c2d5f04d 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -40,6 +40,90 @@ using control_toolbox::Pid; using namespace std::chrono_literals; +TEST(ParameterTest, UTermBadIBoundsTestConstructor) +{ + RecordProperty( + "description", + "This test checks if an error is thrown for bad u_bounds specification (i.e. u_min > u_max)."); + + // Pid(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, std::string antiwindup_strat); + EXPECT_THROW(Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, false, "none"), + std::invalid_argument); +} + +TEST(ParameterTest, UTermBadIBoundsTest) +{ + RecordProperty( + "description", + "This test checks if gains remain for bad u_bounds specification (i.e. u_min > u_max)."); + + // Pid(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, std::string antiwindup_strat); + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 0.0, false, false, "none"); + auto gains = pid.get_gains(); + EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); + EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); + // Try to set bad u-bounds, i.e. u_min > u_max + EXPECT_NO_THROW(pid.set_gains(1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, false, "none")); + // Check if gains were not updated because u-bounds are bad, i.e. u_min > u_max + EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); + EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); +} + +TEST(ParameterTest, outputClampTest) +{ + RecordProperty( + "description", + "This test succeeds if the output is clamped when the saturation is active."); + + // Pid(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, std::string antiwindup_strat); + Pid pid(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, true, false, "back_calculation"); + + double cmd = 0.0; + + // ***** TEST UPPER LIMIT ***** + + cmd = pid.compute_command(0.5, 1.0); + EXPECT_EQ(0.5, cmd); + + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(2.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(50.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(100.0, 1.0); + EXPECT_EQ(1.0, cmd); + + // ***** TEST LOWER LIMIT ***** + + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-0.5, cmd); + + cmd = pid.compute_command(-1, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-2, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-10, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-50, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-100, 1.0); + EXPECT_EQ(-1.0, cmd); +} + TEST(ParameterTest, ITermBadIBoundsTestConstructor) { RecordProperty( From 307b2cf282b71609ad1da7df7397b6f68ddc24f9 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Thu, 24 Apr 2025 17:50:05 +0000 Subject: [PATCH 03/28] Resolving conflicts --- control_toolbox/include/control_toolbox/pid.hpp | 4 ++-- control_toolbox/src/pid.cpp | 13 ++++++------- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 3120019d..afad4d04 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -605,9 +605,9 @@ class Pid double p_error_last_; /** Save state for derivative state calculation. */ double p_error_; /** Error. */ double d_error_; /** Derivative of error. */ - double i_term_; /** Integrator state. */ + double i_term_{0}; /** Integrator state. */ double cmd_; /** Command to send. */ - double cmd_unsat_; /** command without saturation. */ + double cmd_unsat_; /** command without saturation. */ }; } // namespace control_toolbox diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 36083dc5..0acb52f5 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -272,18 +272,17 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) d_term = gains.d_gain_ * d_error_; // Calculate integral contribution to command - if (gains.antiwindup_ == true && gains.antiwindup_strat_ == "none") { + if (gains.antiwindup_ && gains.antiwindup_strat_ == "none") { // Prevent i_term_ from climbing higher than permitted by i_max_/i_min_ - i_term_ = std::clamp(i_term_ + gains.i_gain_ * dt_s * p_error_, gains.i_min_, gains.i_max_); - } - else - { + i_term_ = std::clamp(i_term_ + gains.i_gain_ * dt_s * p_error_, + gains.i_min_, gains.i_max_); + } else if (!gains.antiwindup_ && gains.antiwindup_strat_ == "none") { i_term_ += gains.i_gain_ * dt_s * p_error_; + i_term_ = std::clamp(i_term_, gains.i_min_, gains.i_max_); } // Compute the command // Limit i_term so that the limit is meaningful in the output - // Compute the command cmd_unsat_ = p_term + i_term_ + d_term; if (gains.saturation_ == true) { @@ -293,7 +292,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) cmd_ = cmd_unsat_; } - if (gains.antiwindup_strat_ == "back_calculation") { + if (gains.antiwindup_strat_ == "back_calculation" && gains.i_gain_ != 0) { if (gains.trk_tc_ == 0.0 && gains.d_gain_ != 0.0) { // Default value for tracking time constant for back calculation technique gains.trk_tc_ = std::sqrt(gains.d_gain_/gains.i_gain_); From c8c7c69059b08d9e9440786c6cd6bebb05f343f8 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Thu, 13 Mar 2025 16:56:21 +0000 Subject: [PATCH 04/28] Add unit tests Add 7 unit tests for saturation and anti-windup feature. --- control_toolbox/src/pid.cpp | 2 +- control_toolbox/test/pid_tests.cpp | 444 +++++++++++++++++++++++++++-- 2 files changed, 426 insertions(+), 20 deletions(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 0acb52f5..1276af8d 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -301,7 +301,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) gains.trk_tc_ = gains.p_gain_/gains.i_gain_; } i_term_ += dt_s * (gains.i_gain_ * error + 1/gains.trk_tc_ * (cmd_ - cmd_unsat_)); - } else if (gains.antiwindup_strat_ == "conditioning_technique") { + } else if (gains.antiwindup_strat_ == "conditioning_technique" && gains.i_gain_ != 0) { if (gains.trk_tc_ == 0.0) { // Default value for tracking time constant for conditioning technique gains.trk_tc_ = gains.p_gain_; diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index c2d5f04d..c87bb7fd 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -32,6 +32,7 @@ #include #include +#include #include "control_toolbox/pid.hpp" @@ -124,6 +125,200 @@ TEST(ParameterTest, outputClampTest) EXPECT_EQ(-1.0, cmd); } +TEST(ParameterTest, noOutputClampTest) +{ + RecordProperty( + "description", + "This test succeeds if the output isn't clamped when the saturation is false."); + + // Pid(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, std::string antiwindup_strat); + Pid pid(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, false, false, "back_calculation"); + + double cmd = 0.0; + + // ***** TEST UPPER LIMIT ***** + + cmd = pid.compute_command(0.5, 1.0); + EXPECT_EQ(0.5, cmd); + + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(2.0, 1.0); + EXPECT_EQ(2.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + EXPECT_EQ(10.0, cmd); + + cmd = pid.compute_command(50.0, 1.0); + EXPECT_EQ(50.0, cmd); + + cmd = pid.compute_command(100.0, 1.0); + EXPECT_EQ(100.0, cmd); + + // ***** TEST LOWER LIMIT ***** + + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-0.5, cmd); + + cmd = pid.compute_command(-1, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-2, 1.0); + EXPECT_EQ(-2.0, cmd); + + cmd = pid.compute_command(-10, 1.0); + EXPECT_EQ(-10.0, cmd); + + cmd = pid.compute_command(-50, 1.0); + EXPECT_EQ(-50.0, cmd); + + cmd = pid.compute_command(-100, 1.0); + EXPECT_EQ(-100.0, cmd); +} + +TEST(ParameterTest, integrationBackCalculationZeroGainTest) +{ + RecordProperty( + "description", + "This test succeeds if the integral contribution is clamped when the integral gain is zero for " + "the back calculation technique."); + + // Pid(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, std::string antiwindup_strat); + Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, "back_calculation"); + + double cmd = 0.0; + double pe, ie, de; + + // back_calculation + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); +} + +TEST(ParameterTest, integrationConditioningTechniqueZeroGainTest) +{ + RecordProperty( + "description", + "This test succeeds if the integral contribution is clamped when the integral gain is zero for " + "the conditioning technique."); + + // Pid(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, std::string antiwindup_strat); + Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, "conditioning_technique"); + + double cmd = 0.0; + double pe, ie, de; + + // back_calculation + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); +} + +TEST(ParameterTest, integrationConditionalIntegrationZeroGainTest) +{ + RecordProperty( + "description", + "This test succeeds if the integral contribution is clamped when the integral gain is zero for " + "the conditional integration technique."); + + // Pid(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, std::string antiwindup_strat); + Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, "conditional_integration"); + + double cmd = 0.0; + double pe, ie, de; + + // back_calculation + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); +} + TEST(ParameterTest, ITermBadIBoundsTestConstructor) { RecordProperty( @@ -267,23 +462,38 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double d_gain = std::rand() % 100; double i_max = std::rand() % 100; double i_min = -1 * std::rand() % 100; + double u_max = std::rand() % 100; + double u_min = -1 * std::rand() % 100; + double trk_tc = std::rand() % 100; + bool saturation = false; bool antiwindup = false; + std::string antiwindup_strat = "none"; // Initialize the default way - Pid pid1(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); + Pid pid1(p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, saturation, + antiwindup, antiwindup_strat); // Test return values ------------------------------------------------- - double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return; - bool antiwindup_return; + double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, + u_min_return, trk_tc_return; + bool saturation_return, antiwindup_return; + std::string antiwindup_strat_return; + pid1.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); + p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, + u_min_return, trk_tc_return, saturation_return, antiwindup_return, antiwindup_strat_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); EXPECT_EQ(d_gain, d_gain_return); EXPECT_EQ(i_max, i_max_return); EXPECT_EQ(i_min, i_min_return); + EXPECT_EQ(u_max, u_max_return); + EXPECT_EQ(u_min, u_min_return); + EXPECT_EQ(trk_tc, trk_tc_return); + EXPECT_EQ(saturation, saturation_return); EXPECT_EQ(antiwindup, antiwindup_return); + EXPECT_EQ(antiwindup_strat, antiwindup_strat_return); // Test return values using struct ------------------------------------------------- @@ -293,7 +503,15 @@ TEST(ParameterTest, gainSettingCopyPIDTest) d_gain = std::rand() % 100; i_max = std::rand() % 100; i_min = -1 * std::rand() % 100; - pid1.set_gains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); + u_max = std::rand() % 100; + u_min = -1 * std::rand() % 100; + trk_tc = std::rand() % 100; + saturation = false; + antiwindup = false; + antiwindup_strat = "none"; + + pid1.set_gains(p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, saturation, + antiwindup, antiwindup_strat); Pid::Gains g1 = pid1.get_gains(); EXPECT_EQ(p_gain, g1.p_gain_); @@ -301,7 +519,12 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(d_gain, g1.d_gain_); EXPECT_EQ(i_max, g1.i_max_); EXPECT_EQ(i_min, g1.i_min_); + EXPECT_EQ(u_max, g1.u_max_); + EXPECT_EQ(u_min, g1.u_min_); + EXPECT_EQ(trk_tc, g1.trk_tc_); + EXPECT_EQ(saturation, g1.saturation_); EXPECT_EQ(antiwindup, g1.antiwindup_); + EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); // \todo test initParam() ------------------------------------------------- @@ -315,14 +538,20 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid2(pid1); pid2.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); + p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, + u_min_return, trk_tc_return, saturation_return, antiwindup_return, antiwindup_strat_return); - EXPECT_EQ(p_gain, p_gain_return); - EXPECT_EQ(i_gain, i_gain_return); - EXPECT_EQ(d_gain, d_gain_return); - EXPECT_EQ(i_max, i_max_return); - EXPECT_EQ(i_min, i_min_return); - EXPECT_EQ(antiwindup, antiwindup_return); + 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(u_max, g1.u_max_); + EXPECT_EQ(u_min, g1.u_min_); + EXPECT_EQ(trk_tc, g1.trk_tc_); + EXPECT_EQ(saturation, g1.saturation_); + EXPECT_EQ(antiwindup, g1.antiwindup_); + EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); // Test that errors are zero double pe2, ie2, de2; @@ -336,14 +565,20 @@ TEST(ParameterTest, gainSettingCopyPIDTest) pid3 = pid1; pid3.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); + p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, + u_min_return, trk_tc_return, saturation_return, antiwindup_return, antiwindup_strat_return); - EXPECT_EQ(p_gain, p_gain_return); - EXPECT_EQ(i_gain, i_gain_return); - EXPECT_EQ(d_gain, d_gain_return); - EXPECT_EQ(i_max, i_max_return); - EXPECT_EQ(i_min, i_min_return); - EXPECT_EQ(antiwindup, antiwindup_return); + 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(u_max, g1.u_max_); + EXPECT_EQ(u_min, g1.u_min_); + EXPECT_EQ(trk_tc, g1.trk_tc_); + EXPECT_EQ(saturation, g1.saturation_); + EXPECT_EQ(antiwindup, g1.antiwindup_); + EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); // Test that errors are zero double pe3, ie3, de3; @@ -527,6 +762,177 @@ TEST(CommandTest, completePIDTest) EXPECT_EQ(-3.5, cmd); } +TEST(CommandTest, backCalculationPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "back calculation technique."); + + // Pid(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, std::string antiwindup_strat); + Pid pid(0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, "back_calculation"); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(1.0, ie); + EXPECT_EQ(0.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(3.0, ie); + EXPECT_EQ(1.0, cmd); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(3.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(7.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(4.0, cmd); +} + +TEST(CommandTest, conditioningTechniquePIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "conditioning technique."); + + // Pid(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, std::string antiwindup_strat); + Pid pid(0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, "conditioning_technique"); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(1.0, ie); + EXPECT_EQ(0.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(3.0, ie); + EXPECT_EQ(1.0, cmd); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(3.0, cmd); + + // Saturation applied, conditioning technique now reduces the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, conditioning technique now reduces the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(7.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, conditioning technique now reduces the integral term + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(4.0, cmd); +} + +TEST(CommandTest, conditionalIntegrationPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "conditional integration technique."); + + // Pid(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, std::string antiwindup_strat); + Pid pid(0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, "conditional_integration"); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(1.0, ie); + EXPECT_EQ(0.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(3.0, ie); + EXPECT_EQ(1.0, cmd); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(3.0, cmd); + + // Saturation applied, conditional integration now holds the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, conditional integration now holds the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(0.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(5.0, cmd); +} + TEST(CommandTest, timeArgumentTest) { RecordProperty("description", "Tests different dt argument type methods."); From a37452054e3baa5616e9ff58c8352b0e5557d793 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Thu, 13 Mar 2025 18:05:18 +0000 Subject: [PATCH 05/28] Adjust unit tests for new parameters Add new parameters to the existing unit tests in the package. --- .../test/pid_ros_parameters_tests.cpp | 144 ++++++++++++++++-- .../test/pid_ros_publisher_tests.cpp | 6 +- 2 files changed, 139 insertions(+), 11 deletions(-) diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 24c7b282..f0cd39be 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -54,10 +54,16 @@ void check_set_parameters( const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; + const bool SATURATION = true; const bool ANTIWINDUP = true; + const std::string ANTIWINDUP_STRAT = "none"; const bool SAVE_I_TERM = true; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM)); + ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, + ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM)); rclcpp::Parameter param; @@ -77,9 +83,24 @@ void check_set_parameters( ASSERT_TRUE(node->get_parameter(prefix + "i_clamp_min", param)); ASSERT_EQ(param.get_value(), I_MIN); + ASSERT_TRUE(node->get_parameter(prefix + "u_clamp_max", param)); + ASSERT_EQ(param.get_value(), U_MAX); + + ASSERT_TRUE(node->get_parameter(prefix + "u_clamp_min", param)); + ASSERT_EQ(param.get_value(), U_MIN); + + ASSERT_TRUE(node->get_parameter(prefix + "tracking_time_constant", param)); + ASSERT_EQ(param.get_value(), TRK_TC); + + ASSERT_TRUE(node->get_parameter(prefix + "saturation", param)); + ASSERT_EQ(param.get_value(), SATURATION); + ASSERT_TRUE(node->get_parameter(prefix + "antiwindup", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP); + ASSERT_TRUE(node->get_parameter(prefix + "antiwindup_strategy", param)); + ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT); + ASSERT_TRUE(node->get_parameter(prefix + "save_i_term", param)); ASSERT_EQ(param.get_value(), SAVE_I_TERM); @@ -90,7 +111,12 @@ void check_set_parameters( ASSERT_EQ(gains.d_gain_, D); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.u_max_, U_MAX); + ASSERT_EQ(gains.u_min_, U_MIN); + ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_TRUE(gains.saturation_); ASSERT_TRUE(gains.antiwindup_); + ASSERT_EQ(gains.antiwindup_strat_, "none"); } TEST(PidParametersTest, InitPidTest) @@ -116,8 +142,12 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double D = 3.0; const double I_MAX_BAD = -10.0; const double I_MIN_BAD = 10.0; + const double U_MAX_BAD = -10.0; + const double U_MIN_BAD = 10.0; + const double TRK_TC = 4.0; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX_BAD, I_MIN_BAD, false)); + ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD, + TRK_TC, false, false, "none", false)); rclcpp::Parameter param; @@ -127,7 +157,12 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_FALSE(node->get_parameter("d", param)); ASSERT_FALSE(node->get_parameter("i_clamp_max", param)); ASSERT_FALSE(node->get_parameter("i_clamp_min", param)); + ASSERT_FALSE(node->get_parameter("u_clamp_max", param)); + ASSERT_FALSE(node->get_parameter("u_clamp_min", param)); + ASSERT_FALSE(node->get_parameter("tracking_time_constant", param)); + ASSERT_FALSE(node->get_parameter("saturation", param)); ASSERT_FALSE(node->get_parameter("antiwindup", param)); + ASSERT_FALSE(node->get_parameter("antiwindup_strategy", param)); // check gains were NOT set control_toolbox::Pid::Gains gains = pid.get_gains(); @@ -136,7 +171,12 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.d_gain_, 0.0); ASSERT_EQ(gains.i_max_, 0.0); ASSERT_EQ(gains.i_min_, 0.0); + ASSERT_EQ(gains.u_max_, 0.0); + ASSERT_EQ(gains.u_min_, 0.0); + ASSERT_EQ(gains.trk_tc_, 0.0); + ASSERT_FALSE(gains.saturation_); ASSERT_FALSE(gains.antiwindup_); + ASSERT_EQ(gains.antiwindup_strat_, "none"); } TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot) @@ -216,10 +256,16 @@ TEST(PidParametersTest, SetParametersTest) const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; + const bool SATURATION = true; const bool ANTIWINDUP = true; + const std::string ANTIWINDUP_STRAT = "none"; const bool SAVE_I_TERM = false; - pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM); + pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, + ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM); rcl_interfaces::msg::SetParametersResult set_result; @@ -238,8 +284,20 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_max", U_MAX))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_min", U_MIN))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter( + "tracking_time_constant", TRK_TC))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", SATURATION))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter( + "antiwindup_strategy", ANTIWINDUP_STRAT))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("save_i_term", SAVE_I_TERM))); ASSERT_TRUE(set_result.successful); @@ -253,7 +311,12 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.d_gain_, D); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.u_max_, U_MAX); + ASSERT_EQ(gains.u_min_, U_MIN); + ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_TRUE(gains.saturation_); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); + ASSERT_EQ(gains.antiwindup_strat_, "none"); } TEST(PidParametersTest, SetBadParametersTest) @@ -269,9 +332,17 @@ TEST(PidParametersTest, SetBadParametersTest) const double I_MIN = -10.0; const double I_MAX_BAD = -20.0; const double I_MIN_BAD = 20.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double U_MAX_BAD = -20.0; + const double U_MIN_BAD = 20.0; + const double TRK_TC = 4.0; + const bool SATURATION = true; const bool ANTIWINDUP = true; + const std::string ANTIWINDUP_STRAT = "none"; - pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, + ANTIWINDUP, ANTIWINDUP_STRAT, false); rcl_interfaces::msg::SetParametersResult set_result; @@ -290,8 +361,20 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN_BAD))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_max", U_MAX_BAD))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_min", U_MIN_BAD))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter( + "tracking_time_constant", TRK_TC))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", SATURATION))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter( + "antiwindup_strategy", ANTIWINDUP_STRAT))); + ASSERT_TRUE(set_result.successful); // process callbacks rclcpp::spin_some(node->get_node_base_interface()); @@ -303,7 +386,12 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.d_gain_, D); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.u_max_, U_MAX); + ASSERT_EQ(gains.u_min_, U_MIN); + ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_TRUE(gains.saturation_); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); + ASSERT_EQ(gains.antiwindup_strat_, "none"); } TEST(PidParametersTest, GetParametersTest) @@ -317,10 +405,16 @@ TEST(PidParametersTest, GetParametersTest) const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; + const bool SATURATION = true; const bool ANTIWINDUP = true; + const std::string ANTIWINDUP_STRAT = "none"; - pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, false, false); - pid.set_gains(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, "none", false); + pid.set_gains(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, + ANTIWINDUP, ANTIWINDUP_STRAT); rclcpp::Parameter param; @@ -339,9 +433,24 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("i_clamp_min", param)); ASSERT_EQ(param.get_value(), I_MIN); + ASSERT_TRUE(node->get_parameter("u_clamp_max", param)); + ASSERT_EQ(param.get_value(), U_MAX); + + ASSERT_TRUE(node->get_parameter("u_clamp_min", param)); + ASSERT_EQ(param.get_value(), U_MIN); + + ASSERT_TRUE(node->get_parameter("tracking_time_constant", param)); + ASSERT_EQ(param.get_value(), TRK_TC); + + ASSERT_TRUE(node->get_parameter("saturation", param)); + ASSERT_EQ(param.get_value(), SATURATION); + ASSERT_TRUE(node->get_parameter("antiwindup", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP); + ASSERT_TRUE(node->get_parameter("antiwindup_strategy", param)); + ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT); + ASSERT_TRUE(node->get_parameter("save_i_term", param)); ASSERT_EQ(param.get_value(), false); } @@ -373,6 +482,18 @@ TEST(PidParametersTest, GetParametersFromParams) rclcpp::Parameter param_i_clamp_min; ASSERT_TRUE(node->get_parameter("i_clamp_min", param_i_clamp_min)); ASSERT_TRUE(std::isnan(param_i_clamp_min.get_value())); + + rclcpp::Parameter param_u_clamp_max; + ASSERT_TRUE(node->get_parameter("u_clamp_max", param_u_clamp_max)); + ASSERT_TRUE(std::isnan(param_u_clamp_max.get_value())); + + rclcpp::Parameter param_u_clamp_min; + ASSERT_TRUE(node->get_parameter("u_clamp_min", param_u_clamp_min)); + ASSERT_TRUE(std::isnan(param_u_clamp_min.get_value())); + + rclcpp::Parameter param_tracking_time_constant; + ASSERT_TRUE(node->get_parameter("tracking_time_constant", param_tracking_time_constant)); + ASSERT_TRUE(std::isnan(param_tracking_time_constant.get_value())); } TEST(PidParametersTest, MultiplePidInstances) @@ -387,9 +508,14 @@ TEST(PidParametersTest, MultiplePidInstances) const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; - - ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, I_MAX, I_MIN, false, false)); - ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, I_MAX, I_MIN, true, false)); + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; + + ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false, + false, "none", false)); + ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true, + true, "none", false)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index 046ce683..baca589a 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -39,7 +39,8 @@ TEST(PidPublisherTest, PublishTest) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, false, false); + pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, + false, false, "none", false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -76,7 +77,8 @@ TEST(PidPublisherTest, PublishTestLifecycle) std::dynamic_pointer_cast>( pid_ros.get_pid_state_publisher()); - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, false, false); + pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, + false, false, "none", false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; From eab8116acb6e0020cdbce867192e2457a163de20 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Tue, 18 Mar 2025 11:00:02 +0000 Subject: [PATCH 06/28] Modify cmd eq. to keep the anti-windup behavior --- control_toolbox/src/pid.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 1276af8d..a70cb430 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -278,12 +278,15 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) gains.i_min_, gains.i_max_); } else if (!gains.antiwindup_ && gains.antiwindup_strat_ == "none") { i_term_ += gains.i_gain_ * dt_s * p_error_; - i_term_ = std::clamp(i_term_, gains.i_min_, gains.i_max_); } // Compute the command // Limit i_term so that the limit is meaningful in the output - cmd_unsat_ = p_term + i_term_ + d_term; + if (!gains.antiwindup_ && gains.antiwindup_strat_ == "none") { + cmd_unsat_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; + } else { + cmd_unsat_ = p_term + i_term_ + d_term; + } if (gains.saturation_ == true) { // Limit cmd_ if saturation is enabled From 3d42b917ed66afe47f987b472b1f4463299b6076 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 28 Apr 2025 10:24:28 +0000 Subject: [PATCH 07/28] Fix license comments --- control_toolbox/src/pid_ros.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 70c5a8fa..6e776bcb 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -20,7 +20,15 @@ // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR git config pull.rebase true +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. #include #include From 1fa121de763aeeca73106083c04a20be1d54b3d1 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 28 Apr 2025 11:07:45 +0000 Subject: [PATCH 08/28] Separate i_bounds and u_bounds conditions The i_bounds and u_bounds conditions were combined using an 'or' operator. I have updated them to use separate if else statements. --- control_toolbox/src/pid_ros.cpp | 28 ++++++++++++++++++++-------- 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 6e776bcb..47d3469f 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -265,9 +265,12 @@ void PidROS::initialize_from_args(double p, double i, double d, double i_max, do double u_max, double u_min, double trk_tc, bool saturation, bool antiwindup, std::string antiwindup_strat, bool save_i_term) { - if (i_min > i_max || u_min > u_max) { + if (i_min > i_max) { RCLCPP_ERROR(node_logging_->get_logger(), - "received i_min > i_max or u_min > u_max, skip new gains"); + "received i_min > i_max, skip new gains"); + } else if (u_min > u_max) { + RCLCPP_ERROR(node_logging_->get_logger(), + "received u_min > u_max, skip new gains"); } else { pid_.initialize(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); @@ -327,9 +330,12 @@ void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, void PidROS::set_gains(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, std::string antiwindup_strat) { - if (i_min > i_max || u_min > u_max) { + if (i_min > i_max) { + RCLCPP_ERROR(node_logging_->get_logger(), + "received i_min > i_max, skip new gains"); + } else if (u_min > u_max) { RCLCPP_ERROR(node_logging_->get_logger(), - "received i_min > i_max or u_min > u_max, skip new gains"); + "received u_min > u_max, skip new gains"); } else { node_params_->set_parameters( {rclcpp::Parameter(param_prefix_ + "p", p), @@ -416,9 +422,12 @@ void PidROS::print_values() void PidROS::set_gains(const Pid::Gains & gains) { - if (gains.i_min_ > gains.i_max_ || gains.u_min_ > gains.u_max_) { + if (gains.i_min_ > gains.i_max_) { RCLCPP_ERROR(node_logging_->get_logger(), - "received i_min > i_max or u_min > u_max, skip new gains"); + "received i_min > i_max, skip new gains"); + } else if (gains.u_min_ > gains.u_max_) { + RCLCPP_ERROR(node_logging_->get_logger(), + "received u_min > u_max, skip new gains"); } else { pid_.set_gains(gains); } @@ -493,9 +502,12 @@ void PidROS::set_parameter_event_callback() if (changed) { /// @note don't call set_gains() from inside a callback - if (gains.i_min_ > gains.i_max_ || gains.u_min_ > gains.u_max_) { + if (gains.i_min_ > gains.i_max_) { + RCLCPP_ERROR(node_logging_->get_logger(), + "received i_min > i_max, skip new gains"); + } else if (gains.u_min_ > gains.u_max_) { RCLCPP_ERROR(node_logging_->get_logger(), - "received i_min > i_max or u_min > u_max, skip new gains"); + "received u_min > u_max, skip new gains"); } else { pid_.set_gains(gains); } From 2850d8c2531204fb2f44787e465ae92b52cc602e Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 28 Apr 2025 11:51:08 +0000 Subject: [PATCH 09/28] Fix code style: use consistent brace styles --- .../include/control_toolbox/pid.hpp | 74 ++++++-- .../include/control_toolbox/pid_ros.hpp | 9 +- control_toolbox/src/pid.cpp | 94 ++++++---- control_toolbox/src/pid_ros.cpp | 171 ++++++++++-------- .../test/pid_ros_parameters_tests.cpp | 46 ++--- .../test/pid_ros_publisher_tests.cpp | 8 +- control_toolbox/test/pid_tests.cpp | 22 +-- 7 files changed, 259 insertions(+), 165 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index afad4d04..53ee9c1e 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -120,7 +120,7 @@ class Pid */ struct Gains { - /*! + /*! * \brief Optional constructor for passing in values without antiwindup and saturation * * \param p The proportional gain. @@ -131,12 +131,21 @@ class Pid * */ Gains(double p, double i, double d, double i_max, double i_min) - : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), u_max_(0.0), u_min_(0.0), - trk_tc_(0.0), saturation_(false), antiwindup_(true), antiwindup_strat_("none") + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(0.0), + u_min_(0.0), + trk_tc_(0.0), + saturation_(false), + antiwindup_(true), + antiwindup_strat_("none") { } - /*! + /*! * \brief Optional constructor for passing in values without saturation * * \param p The proportional gain. @@ -151,8 +160,17 @@ class Pid * */ Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) - : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), u_max_(0.0), u_min_(0.0), - trk_tc_(0.0), saturation_(false), antiwindup_(antiwindup), antiwindup_strat_("none") + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(0.0), + u_min_(0.0), + trk_tc_(0.0), + saturation_(false), + antiwindup_(antiwindup), + antiwindup_strat_("none") { } @@ -182,17 +200,36 @@ class Pid other than 'none' is selected, it will override the controller's default anti-windup behavior. * */ - Gains(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, std::string antiwindup_strat) - : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), u_max_(u_max), - u_min_(u_min), trk_tc_(trk_tc), saturation_(saturation), antiwindup_(antiwindup), - antiwindup_strat_(antiwindup_strat) + Gains( + 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, std::string antiwindup_strat) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(u_max), + u_min_(u_min), + trk_tc_(trk_tc), + saturation_(saturation), + antiwindup_(antiwindup), + antiwindup_strat_(antiwindup_strat) { } // Default constructor - Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0), u_max_(0.0), - u_min_(0.0), trk_tc_(0.0), saturation_(false), antiwindup_(false), antiwindup_strat_("none") + Gains() + : p_gain_(0.0), + i_gain_(0.0), + d_gain_(0.0), + i_max_(0.0), + i_min_(0.0), + u_max_(0.0), + u_min_(0.0), + trk_tc_(0.0), + saturation_(false), + antiwindup_(false), + antiwindup_strat_("none") { } @@ -259,9 +296,9 @@ class Pid * * \throws An std::invalid_argument exception is thrown if i_min > i_max or u_min > u_max */ - Pid(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, std::string antiwindup_strat); + Pid( + 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, std::string antiwindup_strat); /** * \brief Copy constructor required for preventing mutexes from being copied @@ -445,8 +482,9 @@ class Pid * * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max */ - void set_gains(double p, double i, double d, double i_max, double i_min, double u_max, - double u_min, double trk_tc, bool saturation = false, bool antiwindup = false, + void set_gains( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation = false, bool antiwindup = false, std::string antiwindup_strat = "none"); /*! diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index 86f7ffb5..df2ee662 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -148,7 +148,7 @@ class PidROS * * \note New gains are not applied if i_min_ > i_max_ or if u_min_ > u_max_. */ - void initialize_from_args( + 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, std::string antiwindup_strat, bool save_i_term); @@ -220,7 +220,7 @@ class PidROS */ 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. @@ -247,8 +247,9 @@ class PidROS * * \note New gains are not applied if i_min > i_max or if u_min_ > u_max_. */ - void set_gains(double p, double i, double d, double i_max, double i_min, double u_max, - double u_min, double trk_tc, bool saturation = false, bool antiwindup = false, + void set_gains( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation = false, bool antiwindup = false, std::string antiwindup_strat = "none"); /*! diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index a70cb430..f2f4bc24 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -48,19 +48,20 @@ namespace control_toolbox { Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) - : Pid(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none") +: Pid(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none") { } -Pid::Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, +Pid::Pid( + 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, std::string antiwindup_strat) : gains_buffer_() { - if (i_min > i_max || u_min > u_max) { + if (i_min > i_max || u_min > u_max) + { throw std::invalid_argument("received i_min > i_max or u_min > u_max"); } - set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, - antiwindup, antiwindup_strat); + set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); // Initialize saved i-term values clear_saved_iterm(); @@ -89,8 +90,9 @@ void Pid::initialize(double p, double i, double d, double i_max, double i_min, b reset(); } -void Pid::initialize(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, std::string antiwindup_strat) +void Pid::initialize( + 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, std::string antiwindup_strat) { set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); @@ -166,20 +168,24 @@ void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bo set_gains(gains); } -void Pid::set_gains(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, std::string antiwindup_strat) +void Pid::set_gains( + 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, std::string antiwindup_strat) { - Gains gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, - antiwindup_strat); + Gains gains( + p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); set_gains(gains); } void Pid::set_gains(const Gains & gains) { - if (gains.i_min_ > gains.i_max_ || gains.u_min_ > gains.u_max_) { + if (gains.i_min_ > gains.i_max_ || gains.u_min_ > gains.u_max_) + { std::cout << "received i_min > i_max or u_min > u_max_, skip new gains\n"; - } else { + } + else + { gains_buffer_.writeFromNonRT(gains); } } @@ -272,46 +278,64 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) d_term = gains.d_gain_ * d_error_; // Calculate integral contribution to command - if (gains.antiwindup_ && gains.antiwindup_strat_ == "none") { + if (gains.antiwindup_ && gains.antiwindup_strat_ == "none") + { // Prevent i_term_ from climbing higher than permitted by i_max_/i_min_ - i_term_ = std::clamp(i_term_ + gains.i_gain_ * dt_s * p_error_, - gains.i_min_, gains.i_max_); - } else if (!gains.antiwindup_ && gains.antiwindup_strat_ == "none") { + i_term_ = std::clamp(i_term_ + gains.i_gain_ * dt_s * p_error_, gains.i_min_, gains.i_max_); + } + else if (!gains.antiwindup_ && gains.antiwindup_strat_ == "none") + { i_term_ += gains.i_gain_ * dt_s * p_error_; } // Compute the command // Limit i_term so that the limit is meaningful in the output - if (!gains.antiwindup_ && gains.antiwindup_strat_ == "none") { + if (!gains.antiwindup_ && gains.antiwindup_strat_ == "none") + { cmd_unsat_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; - } else { + } + else + { cmd_unsat_ = p_term + i_term_ + d_term; } - if (gains.saturation_ == true) { + if (gains.saturation_ == true) + { // Limit cmd_ if saturation is enabled cmd_ = std::clamp(cmd_unsat_, gains.u_min_, gains.u_max_); - } else { + } + else + { cmd_ = cmd_unsat_; } - if (gains.antiwindup_strat_ == "back_calculation" && gains.i_gain_ != 0) { - if (gains.trk_tc_ == 0.0 && gains.d_gain_ != 0.0) { + if (gains.antiwindup_strat_ == "back_calculation" && gains.i_gain_ != 0) + { + if (gains.trk_tc_ == 0.0 && gains.d_gain_ != 0.0) + { // Default value for tracking time constant for back calculation technique - gains.trk_tc_ = std::sqrt(gains.d_gain_/gains.i_gain_); - } else if (gains.trk_tc_ == 0.0 && gains.d_gain_ == 0.0) { + gains.trk_tc_ = std::sqrt(gains.d_gain_ / gains.i_gain_); + } + else if (gains.trk_tc_ == 0.0 && gains.d_gain_ == 0.0) + { // Default value for tracking time constant for back calculation technique - gains.trk_tc_ = gains.p_gain_/gains.i_gain_; + gains.trk_tc_ = gains.p_gain_ / gains.i_gain_; } - i_term_ += dt_s * (gains.i_gain_ * error + 1/gains.trk_tc_ * (cmd_ - cmd_unsat_)); - } else if (gains.antiwindup_strat_ == "conditioning_technique" && gains.i_gain_ != 0) { - if (gains.trk_tc_ == 0.0) { - // Default value for tracking time constant for conditioning technique - gains.trk_tc_ = gains.p_gain_; - } - i_term_ += dt_s * gains.i_gain_ * (error + 1/gains.trk_tc_ * (cmd_ - cmd_unsat_)); - } else if (gains.antiwindup_strat_ == "conditional_integration") { - if (!(cmd_unsat_ != cmd_ && error * cmd_unsat_ > 0)) { + i_term_ += dt_s * (gains.i_gain_ * error + 1 / gains.trk_tc_ * (cmd_ - cmd_unsat_)); + } + else if (gains.antiwindup_strat_ == "conditioning_technique" && gains.i_gain_ != 0) + { + if (gains.trk_tc_ == 0.0) + { + // Default value for tracking time constant for conditioning technique + gains.trk_tc_ = gains.p_gain_; + } + i_term_ += dt_s * gains.i_gain_ * (error + 1 / gains.trk_tc_ * (cmd_ - cmd_unsat_)); + } + else if (gains.antiwindup_strat_ == "conditional_integration") + { + if (!(cmd_unsat_ != cmd_ && error * cmd_unsat_ > 0)) + { i_term_ += dt_s * gains.i_gain_ * error; } } diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 47d3469f..51819a86 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -184,9 +184,11 @@ bool PidROS::get_string_param(const std::string & param_name, std::string & valu { declare_param(param_name, rclcpp::ParameterValue(value)); rclcpp::Parameter param; - if (node_params_->has_parameter(param_name)) { + if (node_params_->has_parameter(param_name)) + { node_params_->get_parameter(param_name, param); - if (rclcpp::PARAMETER_STRING != param.get_type()) { + if (rclcpp::PARAMETER_STRING != param.get_type()) + { RCLCPP_ERROR( node_logging_->get_logger(), "Wrong parameter type '%s', not string", param_name.c_str()); return false; @@ -197,7 +199,9 @@ bool PidROS::get_string_param(const std::string & param_name, std::string & valu << node_base_->get_name() << "' value is " << value << std::endl); return true; - } else { + } + else + { RCLCPP_ERROR_STREAM( node_logging_->get_logger(), "parameter '" << param_name << "' in node '" << node_base_->get_name() << "' does not exists" @@ -234,8 +238,8 @@ bool PidROS::initialize_from_ros_parameters() set_parameter_event_callback(); } - pid_.initialize(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, - antiwindup, antiwindup_strat); + pid_.initialize( + p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); return all_params_available; } @@ -257,23 +261,26 @@ void PidROS::initialize_from_args( void PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term) { - initialize_from_args(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, - false, antiwindup, "none", save_i_term); + initialize_from_args( + p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none", save_i_term); } -void PidROS::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, - std::string antiwindup_strat, bool save_i_term) +void PidROS::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, std::string antiwindup_strat, bool save_i_term) { - if (i_min > i_max) { - RCLCPP_ERROR(node_logging_->get_logger(), - "received i_min > i_max, skip new gains"); - } else if (u_min > u_max) { - RCLCPP_ERROR(node_logging_->get_logger(), - "received u_min > u_max, skip new gains"); - } else { - pid_.initialize(p, i, d, i_max, i_min, u_max, u_min, trk_tc, - saturation, antiwindup, antiwindup_strat); + if (i_min > i_max) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); + } + else if (u_min > u_max) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + } + else + { + pid_.initialize( + p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); declare_param(param_prefix_ + "p", rclcpp::ParameterValue(p)); declare_param(param_prefix_ + "i", rclcpp::ParameterValue(i)); @@ -327,19 +334,22 @@ void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, set_gains(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none"); } -void PidROS::set_gains(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, std::string antiwindup_strat) +void PidROS::set_gains( + 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, std::string antiwindup_strat) { - if (i_min > i_max) { - RCLCPP_ERROR(node_logging_->get_logger(), - "received i_min > i_max, skip new gains"); - } else if (u_min > u_max) { - RCLCPP_ERROR(node_logging_->get_logger(), - "received u_min > u_max, skip new gains"); - } else { + if (i_min > i_max) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); + } + else if (u_min > u_max) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + } + else + { node_params_->set_parameters( - {rclcpp::Parameter(param_prefix_ + "p", p), - rclcpp::Parameter(param_prefix_ + "i", i), + {rclcpp::Parameter(param_prefix_ + "p", p), rclcpp::Parameter(param_prefix_ + "i", i), rclcpp::Parameter(param_prefix_ + "d", d), rclcpp::Parameter(param_prefix_ + "i_clamp_max", i_max), rclcpp::Parameter(param_prefix_ + "i_clamp_min", i_min), @@ -350,8 +360,8 @@ void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup), rclcpp::Parameter(param_prefix_ + "antiwindup_strategy", antiwindup_strat)}); - pid_.set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, - antiwindup, antiwindup_strat); + pid_.set_gains( + p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); } } @@ -401,35 +411,39 @@ void PidROS::print_values() double p_error, i_term, d_error; get_current_pid_errors(p_error, i_term, d_error); - RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Current Values of PID template:\n" - << " 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.trk_tc_ << "\n" - << " Saturation: " << gains.saturation_ << "\n" - << " Antiwindup: " << gains.antiwindup_ << "\n" - << " Antiwindup_Strategy: " << gains.antiwindup_strat_ << "\n" - << "\n" - << " P Error: " << p_error << "\n" - << " I Term: " << i_term << "\n" - << " D Error: " << d_error << "\n" - << " Command: " << get_current_cmd();); + RCLCPP_INFO_STREAM(node_logging_->get_logger(), + "Current Values of PID template:\n" + << " 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.trk_tc_ << "\n" + << " Saturation: " << gains.saturation_ << "\n" + << " Antiwindup: " << gains.antiwindup_ << "\n" + << " Antiwindup_Strategy: " << gains.antiwindup_strat_ << "\n" + << "\n" + << " P Error: " << p_error << "\n" + << " I Term: " << i_term << "\n" + << " D Error: " << d_error << "\n" + << " Command: " << get_current_cmd();); } void PidROS::set_gains(const Pid::Gains & gains) { - if (gains.i_min_ > gains.i_max_) { - RCLCPP_ERROR(node_logging_->get_logger(), - "received i_min > i_max, skip new gains"); - } else if (gains.u_min_ > gains.u_max_) { - RCLCPP_ERROR(node_logging_->get_logger(), - "received u_min > u_max, skip new gains"); - } else { - pid_.set_gains(gains); + if (gains.i_min_ > gains.i_max_) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); + } + else if (gains.u_min_ > gains.u_max_) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + } + else + { + pid_.set_gains(gains); } } @@ -473,22 +487,34 @@ void PidROS::set_parameter_event_callback() { gains.i_min_ = parameter.get_value(); changed = true; - } else if (param_name == param_prefix_ + "u_clamp_max") { + } + else if (param_name == param_prefix_ + "u_clamp_max") + { gains.u_max_ = parameter.get_value(); changed = true; - } else if (param_name == param_prefix_ + "u_clamp_min") { + } + else if (param_name == param_prefix_ + "u_clamp_min") + { gains.u_min_ = parameter.get_value(); changed = true; - } else if (param_name == param_prefix_ + "tracking_time_constant") { + } + else if (param_name == param_prefix_ + "tracking_time_constant") + { gains.trk_tc_ = parameter.get_value(); changed = true; - } else if (param_name == param_prefix_ + "saturation") { + } + else if (param_name == param_prefix_ + "saturation") + { gains.saturation_ = parameter.get_value(); changed = true; - } else if (param_name == param_prefix_ + "antiwindup") { + } + else if (param_name == param_prefix_ + "antiwindup") + { gains.antiwindup_ = parameter.get_value(); changed = true; - } else if (param_name == param_prefix_ + "antiwindup_strategy") { + } + else if (param_name == param_prefix_ + "antiwindup_strategy") + { gains.antiwindup_strat_ = parameter.get_value(); changed = true; } @@ -502,14 +528,17 @@ void PidROS::set_parameter_event_callback() if (changed) { /// @note don't call set_gains() from inside a callback - if (gains.i_min_ > gains.i_max_) { - RCLCPP_ERROR(node_logging_->get_logger(), - "received i_min > i_max, skip new gains"); - } else if (gains.u_min_ > gains.u_max_) { - RCLCPP_ERROR(node_logging_->get_logger(), - "received u_min > u_max, skip new gains"); - } else { - pid_.set_gains(gains); + if (gains.i_min_ > gains.i_max_) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); + } + else if (gains.u_min_ > gains.u_max_) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + } + else + { + pid_.set_gains(gains); } } diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index f0cd39be..8037c1fd 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -62,8 +62,9 @@ void check_set_parameters( const std::string ANTIWINDUP_STRAT = "none"; const bool SAVE_I_TERM = true; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, - ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM)); + ASSERT_NO_THROW(pid.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT, + SAVE_I_TERM)); rclcpp::Parameter param; @@ -146,8 +147,8 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double U_MIN_BAD = 10.0; const double TRK_TC = 4.0; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD, - TRK_TC, false, false, "none", false)); + ASSERT_NO_THROW(pid.initialize_from_args( + P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD, TRK_TC, false, false, "none", false)); rclcpp::Parameter param; @@ -264,8 +265,9 @@ TEST(PidParametersTest, SetParametersTest) const std::string ANTIWINDUP_STRAT = "none"; const bool SAVE_I_TERM = false; - pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, - ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM); + pid.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT, + SAVE_I_TERM); rcl_interfaces::msg::SetParametersResult set_result; @@ -288,15 +290,15 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_min", U_MIN))); ASSERT_TRUE(set_result.successful); - ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter( - "tracking_time_constant", TRK_TC))); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("tracking_time_constant", TRK_TC))); ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", SATURATION))); ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP))); ASSERT_TRUE(set_result.successful); - ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter( - "antiwindup_strategy", ANTIWINDUP_STRAT))); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("antiwindup_strategy", ANTIWINDUP_STRAT))); ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("save_i_term", SAVE_I_TERM))); ASSERT_TRUE(set_result.successful); @@ -341,8 +343,8 @@ TEST(PidParametersTest, SetBadParametersTest) const bool ANTIWINDUP = true; const std::string ANTIWINDUP_STRAT = "none"; - pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, - ANTIWINDUP, ANTIWINDUP_STRAT, false); + pid.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT, false); rcl_interfaces::msg::SetParametersResult set_result; @@ -365,15 +367,15 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_min", U_MIN_BAD))); ASSERT_TRUE(set_result.successful); - ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter( - "tracking_time_constant", TRK_TC))); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("tracking_time_constant", TRK_TC))); ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", SATURATION))); ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP))); ASSERT_TRUE(set_result.successful); - ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter( - "antiwindup_strategy", ANTIWINDUP_STRAT))); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("antiwindup_strategy", ANTIWINDUP_STRAT))); ASSERT_TRUE(set_result.successful); // process callbacks @@ -413,8 +415,8 @@ TEST(PidParametersTest, GetParametersTest) const std::string ANTIWINDUP_STRAT = "none"; pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, "none", false); - pid.set_gains(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, - ANTIWINDUP, ANTIWINDUP_STRAT); + pid.set_gains( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT); rclcpp::Parameter param; @@ -512,10 +514,10 @@ TEST(PidParametersTest, MultiplePidInstances) const double U_MIN = -10.0; const double TRK_TC = 4.0; - ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false, - false, "none", false)); - ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true, - true, "none", false)); + ASSERT_NO_THROW(pid_1.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false, false, "none", false)); + ASSERT_NO_THROW(pid_2.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true, true, "none", false)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index baca589a..e0c556e7 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -39,8 +39,8 @@ TEST(PidPublisherTest, PublishTest) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, - false, false, "none", false); + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, false, "none", false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -77,8 +77,8 @@ TEST(PidPublisherTest, PublishTestLifecycle) std::dynamic_pointer_cast>( pid_ros.get_pid_state_publisher()); - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, - false, false, "none", false); + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, false, "none", false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index c87bb7fd..d04eaeb5 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -49,8 +49,8 @@ TEST(ParameterTest, UTermBadIBoundsTestConstructor) // Pid(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, std::string antiwindup_strat); - EXPECT_THROW(Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, false, "none"), - std::invalid_argument); + EXPECT_THROW( + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, false, "none"), std::invalid_argument); } TEST(ParameterTest, UTermBadIBoundsTest) @@ -75,8 +75,7 @@ TEST(ParameterTest, UTermBadIBoundsTest) TEST(ParameterTest, outputClampTest) { RecordProperty( - "description", - "This test succeeds if the output is clamped when the saturation is active."); + "description", "This test succeeds if the output is clamped when the saturation is active."); // Pid(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, std::string antiwindup_strat); @@ -128,8 +127,7 @@ TEST(ParameterTest, outputClampTest) TEST(ParameterTest, noOutputClampTest) { RecordProperty( - "description", - "This test succeeds if the output isn't clamped when the saturation is false."); + "description", "This test succeeds if the output isn't clamped when the saturation is false."); // Pid(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, std::string antiwindup_strat); @@ -470,12 +468,13 @@ TEST(ParameterTest, gainSettingCopyPIDTest) std::string antiwindup_strat = "none"; // Initialize the default way - Pid pid1(p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, saturation, - antiwindup, antiwindup_strat); + Pid pid1( + p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, + antiwindup_strat); // Test return values ------------------------------------------------- double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, - u_min_return, trk_tc_return; + u_min_return, trk_tc_return; bool saturation_return, antiwindup_return; std::string antiwindup_strat_return; @@ -510,8 +509,9 @@ TEST(ParameterTest, gainSettingCopyPIDTest) antiwindup = false; antiwindup_strat = "none"; - pid1.set_gains(p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, saturation, - antiwindup, antiwindup_strat); + pid1.set_gains( + p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, + antiwindup_strat); Pid::Gains g1 = pid1.get_gains(); EXPECT_EQ(p_gain, g1.p_gain_); From ad95a6b8d99ad2a89f54874348ab7824f1f1d038 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 28 Apr 2025 12:37:34 +0000 Subject: [PATCH 10/28] Remove and update comments Delete comments related to dynamic reconfigure and ROS, and update several other comments. --- .../include/control_toolbox/pid.hpp | 35 ++++++++----------- 1 file changed, 15 insertions(+), 20 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 53ee9c1e..7dbba4b4 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -233,23 +233,22 @@ class Pid { } - double p_gain_; /**< Proportional gain. */ - double i_gain_; /**< Integral gain. */ - double d_gain_; /**< Derivative gain. */ - double i_max_; /**< Maximum allowable integral term. */ - double i_min_; /**< Minimum allowable integral term. */ - double u_max_; /**< Maximum allowable output. */ - double u_min_; /**< Minimum allowable output. */ - double trk_tc_; /**< Tracking time constant. */ - bool saturation_; /**< Saturation. */ - bool antiwindup_; /**< Anti-windup. */ - std::string antiwindup_strat_; /**< Anti-windup strategy. */ + double p_gain_; + double i_gain_; + double d_gain_; + double i_max_; + double i_min_; + double u_max_; + double u_min_; + double trk_tc_; + bool saturation_; + bool antiwindup_; + std::string antiwindup_strat_; }; /*! * \brief Constructor, zeros out Pid values when created and * initialize Pid-gains and integral term limits. - * Does not initialize dynamic reconfigure for PID gains * * \param p The proportional gain. * \param i The integral gain. @@ -268,9 +267,7 @@ class Pid bool antiwindup = false); /*! - * \brief Constructor, zeros out Pid values when created and - * initialize Pid-gains and integral term limits. - * Does not initialize dynamic reconfigure for PID gains + * \brief Constructor, initialize Pid-gains and term limits. * * \param p The proportional gain. * \param i The integral gain. @@ -312,8 +309,7 @@ class Pid ~Pid(); /*! - * \brief Zeros out Pid values and initialize Pid-gains and integral term limits - * Does not initialize the node's parameter interface for PID gains + * \brief Zeros out Pid values and initialize Pid-gains and term limits * * \param p The proportional gain. * \param i The integral gain. @@ -331,7 +327,7 @@ class Pid double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! - * \brief Optional constructor for passing in values + * \brief Initialize Pid-gains and term limits * * \param p The proportional gain. * \param i The integral gain. @@ -456,8 +452,7 @@ class Pid void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! - * \brief Optional constructor for passing in values - * + * \brief Set PID gains for the controller. * \param p The proportional gain. * \param i The integral gain. * \param d The derivative gain. From 0bd47d8065e34a4f7e560e43d5ccac605c584fcb Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 28 Apr 2025 12:50:25 +0000 Subject: [PATCH 11/28] Separate i_bounds and u_bounds conditions The i_bounds and u_bounds conditions were combined using an 'or' operator. I have updated them to use separate if else statements. --- control_toolbox/src/pid.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index f2f4bc24..a50f4500 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -57,9 +57,13 @@ Pid::Pid( double trk_tc, bool saturation, bool antiwindup, std::string antiwindup_strat) : gains_buffer_() { - if (i_min > i_max || u_min > u_max) + if (i_min > i_max) { - throw std::invalid_argument("received i_min > i_max or u_min > u_max"); + throw std::invalid_argument("received i_min > i_max"); + } + else if (u_min > u_max) + { + throw std::invalid_argument("received u_min > u_max"); } set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); @@ -180,9 +184,13 @@ void Pid::set_gains( void Pid::set_gains(const Gains & gains) { - if (gains.i_min_ > gains.i_max_ || gains.u_min_ > gains.u_max_) + if (gains.i_min_ > gains.i_max_) + { + std::cout << "received i_min > i_max, skip new gains\n"; + } + else if (gains.u_min_ > gains.u_max_) { - std::cout << "received i_min > i_max or u_min > u_max_, skip new gains\n"; + std::cout << "received u_min > u_max, skip new gains\n"; } else { From fc218faca45228c300be3fabd703b17edabe912d Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 28 Apr 2025 13:01:58 +0000 Subject: [PATCH 12/28] Move comment closer to corresponding equation --- control_toolbox/src/pid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index a50f4500..134c301b 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -297,9 +297,9 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) } // Compute the command - // Limit i_term so that the limit is meaningful in the output if (!gains.antiwindup_ && gains.antiwindup_strat_ == "none") { + // Limit i_term so that the limit is meaningful in the output cmd_unsat_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; } else From 1a223ebded495335d082a8a06d605682f61ae429 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 28 Apr 2025 14:29:53 +0000 Subject: [PATCH 13/28] Add helper function for zero comparisons This commit adds a helper function for comparing values to zero, improving the readability of the code. --- .../include/control_toolbox/pid.hpp | 13 ++++++++++++ control_toolbox/src/pid.cpp | 21 +++++++++---------- 2 files changed, 23 insertions(+), 11 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 7dbba4b4..b9b56e76 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -34,6 +34,7 @@ #define CONTROL_TOOLBOX__PID_HPP_ #include +#include #include #include "rclcpp/duration.hpp" @@ -41,6 +42,18 @@ namespace control_toolbox { +template +inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) +{ + return std::abs(value) <= tolerance; +} + +template +inline bool is_not_zero(T value, T tolerance = std::numeric_limits::epsilon()) +{ + return !is_zero(value, tolerance); +} + /***************************************************/ /*! \class Pid \brief A basic pid class. diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 134c301b..a13c08fa 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -39,7 +39,6 @@ #include #include #include -#include #include #include @@ -200,7 +199,7 @@ void Pid::set_gains(const Gains & gains) double Pid::compute_command(double error, const double & dt_s) { - if (std::abs(dt_s) <= std::numeric_limits::epsilon()) + if (is_zero(dt_s)) { // don't update anything return cmd_; @@ -256,9 +255,9 @@ double Pid::compute_command(double error, double error_dot, const std::chrono::n double Pid::compute_command(double error, double error_dot, const double & dt_s) { - if (std::abs(dt_s) <= std::numeric_limits::epsilon()) + if (is_zero(dt_s)) { - // don't update anything + // Don't update anything return cmd_; } else if (dt_s < 0.0) @@ -269,10 +268,10 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) Gains gains = *gains_buffer_.readFromRT(); double p_term, d_term; - p_error_ = error; // this is error = target - state + p_error_ = error; // This is error = target - state d_error_ = error_dot; - // don't reset controller but return NaN + // Don't reset controller but return NaN if (!std::isfinite(error) || !std::isfinite(error_dot)) { std::cout << "Received a non-finite error/error_dot value\n"; @@ -317,23 +316,23 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) cmd_ = cmd_unsat_; } - if (gains.antiwindup_strat_ == "back_calculation" && gains.i_gain_ != 0) + if (gains.antiwindup_strat_ == "back_calculation" && is_not_zero(gains.i_gain_)) { - if (gains.trk_tc_ == 0.0 && gains.d_gain_ != 0.0) + if (is_zero(gains.trk_tc_) && is_not_zero(gains.d_gain_)) { // Default value for tracking time constant for back calculation technique gains.trk_tc_ = std::sqrt(gains.d_gain_ / gains.i_gain_); } - else if (gains.trk_tc_ == 0.0 && gains.d_gain_ == 0.0) + else if (is_zero(gains.trk_tc_) && is_zero(gains.d_gain_)) { // Default value for tracking time constant for back calculation technique gains.trk_tc_ = gains.p_gain_ / gains.i_gain_; } i_term_ += dt_s * (gains.i_gain_ * error + 1 / gains.trk_tc_ * (cmd_ - cmd_unsat_)); } - else if (gains.antiwindup_strat_ == "conditioning_technique" && gains.i_gain_ != 0) + else if (gains.antiwindup_strat_ == "conditioning_technique" && is_not_zero(gains.i_gain_)) { - if (gains.trk_tc_ == 0.0) + if (is_zero(gains.trk_tc_)) { // Default value for tracking time constant for conditioning technique gains.trk_tc_ = gains.p_gain_; From 8fc04bed3dd77dbace0ef6d40baaebc02f16795f Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 28 Apr 2025 16:16:00 +0000 Subject: [PATCH 14/28] Discard unused comments and fix comment formatting --- control_toolbox/include/control_toolbox/pid.hpp | 4 +--- control_toolbox/test/pid_tests.cpp | 4 ---- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index b9b56e76..45326d04 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -310,7 +310,7 @@ class Pid 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, std::string antiwindup_strat); - /** + /*! * \brief Copy constructor required for preventing mutexes from being copied * \param source - Pid to copy */ @@ -644,8 +644,6 @@ class Pid } protected: - // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without - // blocking the realtime update loop realtime_tools::RealtimeBuffer gains_buffer_; double p_error_last_; /** Save state for derivative state calculation. */ diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index d04eaeb5..964a5d5c 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -526,10 +526,6 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(antiwindup, g1.antiwindup_); EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); - // \todo test initParam() ------------------------------------------------- - - // \todo test bool init(const ros::NodeHandle &n); ----------------------------------- - // Send update command to populate errors ------------------------------------------------- pid1.set_current_cmd(10); (void)pid1.compute_command(20, 1.0); From 72783233ef67c7d97b123d2bd7c90b398168a8ec Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 28 Apr 2025 16:20:10 +0000 Subject: [PATCH 15/28] Explicitly initialize variables to 0 in header --- control_toolbox/include/control_toolbox/pid.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 45326d04..d0997c81 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -646,12 +646,12 @@ class Pid protected: realtime_tools::RealtimeBuffer gains_buffer_; - double p_error_last_; /** Save state for derivative state calculation. */ - double p_error_; /** Error. */ - double d_error_; /** Derivative of error. */ - double i_term_{0}; /** Integrator state. */ - double cmd_; /** Command to send. */ - double cmd_unsat_; /** command without saturation. */ + double p_error_last_ = 0; /** Save state for derivative state calculation. */ + double p_error_ = 0; /** Error. */ + double d_error_ = 0; /** Derivative of error. */ + double i_term_ = 0; /** Integrator state. */ + double cmd_ = 0; /** Command to send. */ + double cmd_unsat_ = 0; /** command without saturation. */ }; } // namespace control_toolbox From d8e07d24ab966d5a5e9286b7899cb5c5f3478bce Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Tue, 29 Apr 2025 10:36:42 +0000 Subject: [PATCH 16/28] Remove deprecated comment --- control_toolbox/src/pid_ros.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 51819a86..55af0034 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -527,7 +527,6 @@ void PidROS::set_parameter_event_callback() if (changed) { - /// @note don't call set_gains() from inside a callback if (gains.i_min_ > gains.i_max_) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); From 18f8d1d5b360d228fa5411971e3d10688e9a1c76 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Tue, 29 Apr 2025 10:52:14 +0000 Subject: [PATCH 17/28] add Doxygen trailing comments to Gains members --- .../include/control_toolbox/pid.hpp | 42 +++++++------------ 1 file changed, 15 insertions(+), 27 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index d0997c81..13187a0a 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -154,29 +154,17 @@ class Pid trk_tc_(0.0), saturation_(false), antiwindup_(true), - antiwindup_strat_("none") - { - } + antiwindup_strat_("none"){} - /*! + /*! * \brief Optional constructor for passing in values without saturation * * \param p The proportional gain. * \param i The integral gain. * \param d The derivative gain. * \param i_max Upper integral clamp. - * \param i_min Lower integral clamp. - * \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. - * - */ - Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) - : p_gain_(p), - i_gain_(i), - d_gain_(d), - i_max_(i_max), + * \param i_min L/**< Anti-windup strategy. */ + , i_min_(i_min), u_max_(0.0), u_min_(0.0), @@ -246,17 +234,17 @@ class Pid { } - double p_gain_; - double i_gain_; - double d_gain_; - double i_max_; - double i_min_; - double u_max_; - double u_min_; - double trk_tc_; - bool saturation_; - bool antiwindup_; - std::string antiwindup_strat_; + double p_gain_; /**< Proportional gain. */ + double i_gain_; /**< Integral gain. */ + double d_gain_; /**< Derivative gain. */ + double i_max_; /**< Maximum allowable integral term. */ + double i_min_; /**< Minimum allowable integral term. */ + double u_max_; /**< Maximum allowable output. */ + double u_min_; /**< Minimum allowable output. */ + double trk_tc_; /**< Tracking time constant. */ + bool saturation_; /**< Saturation. */ + bool antiwindup_; /**< Anti-windup. */ + std::string antiwindup_strat_; /**< Anti-windup strategy. */ }; /*! From 9c740998ad1947e2834bde6b70d50da5711825f4 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Tue, 29 Apr 2025 10:54:36 +0000 Subject: [PATCH 18/28] add Doxygen trailing comments to Gains members --- .../include/control_toolbox/pid.hpp | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 13187a0a..142c39bc 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -154,17 +154,29 @@ class Pid trk_tc_(0.0), saturation_(false), antiwindup_(true), - antiwindup_strat_("none"){} + antiwindup_strat_("none") + { + } - /*! + /*! * \brief Optional constructor for passing in values without saturation * * \param p The proportional gain. * \param i The integral gain. * \param d The derivative gain. * \param i_max Upper integral clamp. - * \param i_min L/**< Anti-windup strategy. */ - , + * \param i_min Lower integral clamp. + * \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. + * + */ + Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), i_min_(i_min), u_max_(0.0), u_min_(0.0), From b43e8eabcff986a46c736dfe4f29d9283290f2cb Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Tue, 29 Apr 2025 11:01:32 +0000 Subject: [PATCH 19/28] Add comment for gains_buffer_ in pid.hpp --- control_toolbox/include/control_toolbox/pid.hpp | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 142c39bc..425c1a96 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -263,17 +263,8 @@ class Pid * \brief Constructor, zeros out Pid values when created and * initialize Pid-gains and integral term limits. * - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. - * \param i_max Upper integral clamp. - * \param i_min Lower integral clamp. - * \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. - * - * \throws An std::invalid_argument exception is thrown if i_min > i_max + * \param p The proportional gain. // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without + // blocking the realtime update loop_min > i_max */ Pid( double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0, @@ -644,6 +635,8 @@ class Pid } protected: + // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without + // blocking the realtime update loop realtime_tools::RealtimeBuffer gains_buffer_; double p_error_last_ = 0; /** Save state for derivative state calculation. */ From b459b704cb284c39655862ef8c875f7f3d38af5d Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Tue, 29 Apr 2025 11:02:52 +0000 Subject: [PATCH 20/28] Add comment for gains_buffer_ in pid.hpp --- control_toolbox/include/control_toolbox/pid.hpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 425c1a96..142c39bc 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -263,8 +263,17 @@ class Pid * \brief Constructor, zeros out Pid values when created and * initialize Pid-gains and integral term limits. * - * \param p The proportional gain. // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without - // blocking the realtime update loop_min > i_max + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \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. + * + * \throws An std::invalid_argument exception is thrown if i_min > i_max */ Pid( double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0, @@ -635,8 +644,6 @@ class Pid } protected: - // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without - // blocking the realtime update loop realtime_tools::RealtimeBuffer gains_buffer_; double p_error_last_ = 0; /** Save state for derivative state calculation. */ From d76cb7442544557bf8c71bdbe1b052728c1f3f9d Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Tue, 29 Apr 2025 11:03:57 +0000 Subject: [PATCH 21/28] Add comment for gains_buffer_ in pid.hpp --- control_toolbox/include/control_toolbox/pid.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 142c39bc..087aa26b 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -644,6 +644,8 @@ class Pid } protected: + // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without + // blocking the realtime update loop realtime_tools::RealtimeBuffer gains_buffer_; double p_error_last_ = 0; /** Save state for derivative state calculation. */ From ebdae9502575a0dc5627157d0e1196420cf0d278 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Tue, 29 Apr 2025 11:08:17 +0000 Subject: [PATCH 22/28] Add comment for gains_buffer_ in pid.hpp --- control_toolbox/include/control_toolbox/pid_ros.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index df2ee662..3e68b1ec 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -249,7 +249,7 @@ class PidROS */ void set_gains( double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool saturation = false, bool antiwindup = false, + double trk_tc = 0.0, bool saturation = false, bool antiwindup = false, std::string antiwindup_strat = "none"); /*! From 0aa356a6839698383f057038e47a0daeb7ab574d Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Tue, 29 Apr 2025 11:21:20 +0000 Subject: [PATCH 23/28] Add default value for trk_tc --- control_toolbox/include/control_toolbox/pid.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 087aa26b..25ed30b9 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -492,7 +492,7 @@ class Pid */ void set_gains( double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool saturation = false, bool antiwindup = false, + double trk_tc = 0.0, bool saturation = false, bool antiwindup = false, std::string antiwindup_strat = "none"); /*! From f7b8ad450a4aaf2f1380e646f9e8b646502f74d9 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Tue, 29 Apr 2025 11:32:00 +0000 Subject: [PATCH 24/28] remove is_not_zero() function --- control_toolbox/include/control_toolbox/pid.hpp | 6 ------ control_toolbox/src/pid.cpp | 6 +++--- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 25ed30b9..2aa1973f 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -48,12 +48,6 @@ inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) return std::abs(value) <= tolerance; } -template -inline bool is_not_zero(T value, T tolerance = std::numeric_limits::epsilon()) -{ - return !is_zero(value, tolerance); -} - /***************************************************/ /*! \class Pid \brief A basic pid class. diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index a13c08fa..48a0bbbc 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -316,9 +316,9 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) cmd_ = cmd_unsat_; } - if (gains.antiwindup_strat_ == "back_calculation" && is_not_zero(gains.i_gain_)) + if (gains.antiwindup_strat_ == "back_calculation" && !is_zero(gains.i_gain_)) { - if (is_zero(gains.trk_tc_) && is_not_zero(gains.d_gain_)) + if (is_zero(gains.trk_tc_) && !is_zero(gains.d_gain_)) { // Default value for tracking time constant for back calculation technique gains.trk_tc_ = std::sqrt(gains.d_gain_ / gains.i_gain_); @@ -330,7 +330,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) } i_term_ += dt_s * (gains.i_gain_ * error + 1 / gains.trk_tc_ * (cmd_ - cmd_unsat_)); } - else if (gains.antiwindup_strat_ == "conditioning_technique" && is_not_zero(gains.i_gain_)) + else if (gains.antiwindup_strat_ == "conditioning_technique" && !is_zero(gains.i_gain_)) { if (is_zero(gains.trk_tc_)) { From 47f2f6ed979ba2bd93ef94915131a1a9a87f3078 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 5 May 2025 12:47:34 +0000 Subject: [PATCH 25/28] Refactor PID antiwindup strategy variable Refactor antiwindup strategy variable implementation replacing string usage with a dedicated enum improve type safety. --- .../include/control_toolbox/pid.hpp | 100 ++++++++++++++---- .../include/control_toolbox/pid_ros.hpp | 4 +- control_toolbox/src/pid.cpp | 33 +++--- control_toolbox/src/pid_ros.cpp | 28 +++-- .../test/pid_ros_parameters_tests.cpp | 31 +++--- .../test/pid_ros_publisher_tests.cpp | 5 +- control_toolbox/test/pid_tests.cpp | 63 ++++++----- 7 files changed, 176 insertions(+), 88 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 2aa1973f..7e9a83a3 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -42,6 +42,68 @@ namespace control_toolbox { +class AntiwindupStrategy +{ +public: + enum Value : int8_t + { + NONE = 0, + BACK_CALCULATION, + CONDITIONING_TECHNIQUE, + CONDITIONAL_INTEGRATION + }; + + constexpr AntiwindupStrategy() : value_(NONE) {} + constexpr AntiwindupStrategy(Value v) : value_(v) {} // NOLINT(runtime/explicit) + + explicit AntiwindupStrategy(const std::string & s) + { + if (s == "back_calculation") + { + value_ = BACK_CALCULATION; + } + else if (s == "conditioning_technique") + { + value_ = CONDITIONING_TECHNIQUE; + } + else if (s == "conditional_integration") + { + value_ = CONDITIONAL_INTEGRATION; + } + else + { + value_ = NONE; + } + } + + operator std::string() const { return to_string(); } + + constexpr bool operator==(AntiwindupStrategy other) const { return value_ == other.value_; } + constexpr bool operator!=(AntiwindupStrategy other) const { return value_ != other.value_; } + + constexpr bool operator==(Value other) const { return value_ == other; } + constexpr bool operator!=(Value other) const { return value_ != other; } + + std::string to_string() const + { + switch (value_) + { + case BACK_CALCULATION: + return "back_calculation"; + case CONDITIONING_TECHNIQUE: + return "conditioning_technique"; + case CONDITIONAL_INTEGRATION: + return "conditional_integration"; + case NONE: + default: + return "none"; + } + } + +private: + Value value_; +}; + template inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) { @@ -148,7 +210,7 @@ class Pid trk_tc_(0.0), saturation_(false), antiwindup_(true), - antiwindup_strat_("none") + antiwindup_strat_(AntiwindupStrategy::NONE) { } @@ -177,7 +239,7 @@ class Pid trk_tc_(0.0), saturation_(false), antiwindup_(antiwindup), - antiwindup_strat_("none") + antiwindup_strat_(AntiwindupStrategy::NONE) { } @@ -209,7 +271,7 @@ class Pid */ Gains( 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, std::string antiwindup_strat) + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) : p_gain_(p), i_gain_(i), d_gain_(d), @@ -236,21 +298,21 @@ class Pid trk_tc_(0.0), saturation_(false), antiwindup_(false), - antiwindup_strat_("none") + antiwindup_strat_(AntiwindupStrategy::NONE) { } - double p_gain_; /**< Proportional gain. */ - double i_gain_; /**< Integral gain. */ - double d_gain_; /**< Derivative gain. */ - double i_max_; /**< Maximum allowable integral term. */ - double i_min_; /**< Minimum allowable integral term. */ - double u_max_; /**< Maximum allowable output. */ - double u_min_; /**< Minimum allowable output. */ - double trk_tc_; /**< Tracking time constant. */ - bool saturation_; /**< Saturation. */ - bool antiwindup_; /**< Anti-windup. */ - std::string antiwindup_strat_; /**< Anti-windup strategy. */ + double p_gain_; /**< Proportional gain. */ + double i_gain_; /**< Integral gain. */ + double d_gain_; /**< Derivative gain. */ + double i_max_; /**< Maximum allowable integral term. */ + double i_min_; /**< Minimum allowable integral term. */ + double u_max_; /**< Maximum allowable output. */ + double u_min_; /**< Minimum allowable output. */ + double trk_tc_; /**< Tracking time constant. */ + bool saturation_; /**< Saturation. */ + bool antiwindup_; /**< Anti-windup. */ + AntiwindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ }; /*! @@ -302,7 +364,7 @@ class Pid */ Pid( 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, std::string antiwindup_strat); + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); /*! * \brief Copy constructor required for preventing mutexes from being copied @@ -362,7 +424,7 @@ class Pid */ void initialize( 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, std::string antiwindup_strat); + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); /*! * \brief Reset the state of this PID controller @@ -434,7 +496,7 @@ class Pid void get_gains( 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, - std::string & antiwindup_strat); + AntiwindupStrategy & antiwindup_strat); /*! * \brief Get PID gains for the controller. @@ -487,7 +549,7 @@ class Pid 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, - std::string antiwindup_strat = "none"); + AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE); /*! * \brief Set PID gains for the controller. diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index 3e68b1ec..7f8687c4 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -150,7 +150,7 @@ class PidROS */ 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, std::string antiwindup_strat, + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat, bool save_i_term); /*! @@ -250,7 +250,7 @@ class PidROS 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, - std::string antiwindup_strat = "none"); + AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE); /*! * \brief Set PID gains for the controller. diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 48a0bbbc..cd75fc9a 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -40,20 +40,19 @@ #include #include #include -#include #include "control_toolbox/pid.hpp" namespace control_toolbox { Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) -: Pid(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none") +: Pid(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE) { } Pid::Pid( 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, std::string antiwindup_strat) + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) : gains_buffer_() { if (i_min > i_max) @@ -88,14 +87,14 @@ Pid::~Pid() {} void Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - set_gains(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none"); + set_gains(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE); reset(); } void Pid::initialize( 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, std::string antiwindup_strat) + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) { set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); @@ -127,7 +126,7 @@ void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & double trk_tc; bool saturation; bool antiwindup; - std::string antiwindup_strat; + AntiwindupStrategy antiwindup_strat; get_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); } @@ -138,14 +137,14 @@ void Pid::get_gains( double u_min; double trk_tc; bool saturation; - std::string antiwindup_strat; + AntiwindupStrategy antiwindup_strat; get_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); } void Pid::get_gains( 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, - std::string & antiwindup_strat) + AntiwindupStrategy & antiwindup_strat) { Gains gains = *gains_buffer_.readFromRT(); @@ -173,7 +172,7 @@ void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bo void Pid::set_gains( 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, std::string antiwindup_strat) + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) { Gains gains( p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); @@ -275,7 +274,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) if (!std::isfinite(error) || !std::isfinite(error_dot)) { std::cout << "Received a non-finite error/error_dot value\n"; - return cmd_ = std::numeric_limits::quiet_NaN(); + return cmd_ = std::numeric_limits::quiet_NaN(); } // Calculate proportional contribution to command @@ -285,18 +284,18 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) d_term = gains.d_gain_ * d_error_; // Calculate integral contribution to command - if (gains.antiwindup_ && gains.antiwindup_strat_ == "none") + if (gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) { // Prevent i_term_ from climbing higher than permitted by i_max_/i_min_ i_term_ = std::clamp(i_term_ + gains.i_gain_ * dt_s * p_error_, gains.i_min_, gains.i_max_); } - else if (!gains.antiwindup_ && gains.antiwindup_strat_ == "none") + else if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) { i_term_ += gains.i_gain_ * dt_s * p_error_; } // Compute the command - if (!gains.antiwindup_ && gains.antiwindup_strat_ == "none") + if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) { // Limit i_term so that the limit is meaningful in the output cmd_unsat_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; @@ -316,7 +315,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) cmd_ = cmd_unsat_; } - if (gains.antiwindup_strat_ == "back_calculation" && !is_zero(gains.i_gain_)) + if (gains.antiwindup_strat_ == AntiwindupStrategy::BACK_CALCULATION && !is_zero(gains.i_gain_)) { if (is_zero(gains.trk_tc_) && !is_zero(gains.d_gain_)) { @@ -330,7 +329,9 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) } i_term_ += dt_s * (gains.i_gain_ * error + 1 / gains.trk_tc_ * (cmd_ - cmd_unsat_)); } - else if (gains.antiwindup_strat_ == "conditioning_technique" && !is_zero(gains.i_gain_)) + else if ( + gains.antiwindup_strat_ == AntiwindupStrategy::CONDITIONING_TECHNIQUE && + !is_zero(gains.i_gain_)) { if (is_zero(gains.trk_tc_)) { @@ -339,7 +340,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) } i_term_ += dt_s * gains.i_gain_ * (error + 1 / gains.trk_tc_ * (cmd_ - cmd_unsat_)); } - else if (gains.antiwindup_strat_ == "conditional_integration") + else if (gains.antiwindup_strat_ == AntiwindupStrategy::CONDITIONAL_INTEGRATION) { if (!(cmd_unsat_ != cmd_ && error * cmd_unsat_ > 0)) { diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 55af0034..f64d701e 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -216,7 +216,7 @@ bool PidROS::initialize_from_ros_parameters() p = i = d = i_max = i_min = u_max = u_min = trk_tc = std::numeric_limits::quiet_NaN(); bool saturation = false; bool antiwindup = false; - std::string antiwindup_strat = "none"; + std::string antiwindup_strat_str = "none"; bool all_params_available = true; all_params_available &= get_double_param(param_prefix_ + "p", p); @@ -230,7 +230,7 @@ bool PidROS::initialize_from_ros_parameters() get_boolean_param(param_prefix_ + "saturation", saturation); get_boolean_param(param_prefix_ + "antiwindup", antiwindup); - get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat); + get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat_str); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(false)); if (all_params_available) @@ -238,6 +238,8 @@ bool PidROS::initialize_from_ros_parameters() set_parameter_event_callback(); } + AntiwindupStrategy antiwindup_strat(antiwindup_strat_str); + pid_.initialize( p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); @@ -255,19 +257,21 @@ void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValu void PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup) { - initialize_from_args(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none", false); + initialize_from_args( + p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE, false); } void PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term) { initialize_from_args( - p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none", save_i_term); + p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE, save_i_term); } void PidROS::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, std::string antiwindup_strat, bool save_i_term) + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat, + bool save_i_term) { if (i_min > i_max) { @@ -292,7 +296,8 @@ void PidROS::initialize_from_args( declare_param(param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(trk_tc)); declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(saturation)); declare_param(param_prefix_ + "antiwindup", rclcpp::ParameterValue(antiwindup)); - declare_param(param_prefix_ + "antiwindup_strategy", rclcpp::ParameterValue(antiwindup_strat)); + declare_param( + param_prefix_ + "antiwindup_strategy", rclcpp::ParameterValue(antiwindup_strat.to_string())); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); set_parameter_event_callback(); @@ -331,12 +336,12 @@ Pid::Gains PidROS::get_gains() { return pid_.get_gains(); } void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - set_gains(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, "none"); + set_gains(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE); } void PidROS::set_gains( 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, std::string antiwindup_strat) + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) { if (i_min > i_max) { @@ -358,7 +363,7 @@ void PidROS::set_gains( rclcpp::Parameter(param_prefix_ + "tracking_time_constant", trk_tc), rclcpp::Parameter(param_prefix_ + "saturation", saturation), rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup), - rclcpp::Parameter(param_prefix_ + "antiwindup_strategy", antiwindup_strat)}); + rclcpp::Parameter(param_prefix_ + "antiwindup_strategy", antiwindup_strat.to_string())}); pid_.set_gains( p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); @@ -423,7 +428,8 @@ void PidROS::print_values() << " Tracking_Time_Constant: " << gains.trk_tc_ << "\n" << " Saturation: " << gains.saturation_ << "\n" << " Antiwindup: " << gains.antiwindup_ << "\n" - << " Antiwindup_Strategy: " << gains.antiwindup_strat_ << "\n" + << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() + << "\n" << "\n" << " P Error: " << p_error << "\n" << " I Term: " << i_term << "\n" @@ -515,7 +521,7 @@ void PidROS::set_parameter_event_callback() } else if (param_name == param_prefix_ + "antiwindup_strategy") { - gains.antiwindup_strat_ = parameter.get_value(); + gains.antiwindup_strat_ = AntiwindupStrategy(parameter.get_value()); changed = true; } } diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 8037c1fd..4c82cb6e 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -21,6 +21,7 @@ #include "rclcpp/parameter.hpp" #include "rclcpp/utilities.hpp" +using control_toolbox::AntiwindupStrategy; using rclcpp::executors::MultiThreadedExecutor; class TestablePidROS : public control_toolbox::PidROS @@ -59,7 +60,7 @@ void check_set_parameters( const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - const std::string ANTIWINDUP_STRAT = "none"; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; const bool SAVE_I_TERM = true; ASSERT_NO_THROW(pid.initialize_from_args( @@ -100,7 +101,7 @@ void check_set_parameters( ASSERT_EQ(param.get_value(), ANTIWINDUP); ASSERT_TRUE(node->get_parameter(prefix + "antiwindup_strategy", param)); - ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT); + ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); ASSERT_TRUE(node->get_parameter(prefix + "save_i_term", param)); ASSERT_EQ(param.get_value(), SAVE_I_TERM); @@ -117,7 +118,7 @@ void check_set_parameters( ASSERT_EQ(gains.trk_tc_, TRK_TC); ASSERT_TRUE(gains.saturation_); ASSERT_TRUE(gains.antiwindup_); - ASSERT_EQ(gains.antiwindup_strat_, "none"); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); } TEST(PidParametersTest, InitPidTest) @@ -148,7 +149,8 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double TRK_TC = 4.0; ASSERT_NO_THROW(pid.initialize_from_args( - P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD, TRK_TC, false, false, "none", false)); + P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD, TRK_TC, false, false, + AntiwindupStrategy::NONE, false)); rclcpp::Parameter param; @@ -177,7 +179,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.trk_tc_, 0.0); ASSERT_FALSE(gains.saturation_); ASSERT_FALSE(gains.antiwindup_); - ASSERT_EQ(gains.antiwindup_strat_, "none"); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); } TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot) @@ -262,7 +264,7 @@ TEST(PidParametersTest, SetParametersTest) const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - const std::string ANTIWINDUP_STRAT = "none"; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; const bool SAVE_I_TERM = false; pid.initialize_from_args( @@ -318,7 +320,7 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.trk_tc_, TRK_TC); ASSERT_TRUE(gains.saturation_); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, "none"); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); } TEST(PidParametersTest, SetBadParametersTest) @@ -341,7 +343,7 @@ TEST(PidParametersTest, SetBadParametersTest) const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - const std::string ANTIWINDUP_STRAT = "none"; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; pid.initialize_from_args( P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT, false); @@ -393,7 +395,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.trk_tc_, TRK_TC); ASSERT_TRUE(gains.saturation_); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, "none"); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); } TEST(PidParametersTest, GetParametersTest) @@ -412,9 +414,10 @@ TEST(PidParametersTest, GetParametersTest) const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - const std::string ANTIWINDUP_STRAT = "none"; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; - pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, "none", false); + pid.initialize_from_args( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, AntiwindupStrategy::NONE, false); pid.set_gains( P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT); @@ -451,7 +454,7 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_EQ(param.get_value(), ANTIWINDUP); ASSERT_TRUE(node->get_parameter("antiwindup_strategy", param)); - ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT); + ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); ASSERT_TRUE(node->get_parameter("save_i_term", param)); ASSERT_EQ(param.get_value(), false); @@ -515,9 +518,9 @@ TEST(PidParametersTest, MultiplePidInstances) const double TRK_TC = 4.0; ASSERT_NO_THROW(pid_1.initialize_from_args( - P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false, false, "none", false)); + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false, false, AntiwindupStrategy::NONE, false)); ASSERT_NO_THROW(pid_2.initialize_from_args( - P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true, true, "none", false)); + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true, true, AntiwindupStrategy::NONE, false)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index e0c556e7..b5931062 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -27,6 +27,7 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +using control_toolbox::AntiwindupStrategy; using PidStateMsg = control_msgs::msg::PidState; using rclcpp::executors::MultiThreadedExecutor; @@ -40,7 +41,7 @@ TEST(PidPublisherTest, PublishTest) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); pid_ros.initialize_from_args( - 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, false, "none", false); + 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, false, AntiwindupStrategy::NONE, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -78,7 +79,7 @@ TEST(PidPublisherTest, PublishTestLifecycle) pid_ros.get_pid_state_publisher()); pid_ros.initialize_from_args( - 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, false, "none", false); + 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, false, AntiwindupStrategy::NONE, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 964a5d5c..e67f4a79 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -38,6 +38,7 @@ #include "gmock/gmock.h" +using control_toolbox::AntiwindupStrategy; using control_toolbox::Pid; using namespace std::chrono_literals; @@ -48,9 +49,10 @@ TEST(ParameterTest, UTermBadIBoundsTestConstructor) "This test checks if an error is thrown for bad u_bounds specification (i.e. u_min > u_max)."); // Pid(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, std::string antiwindup_strat); + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); EXPECT_THROW( - Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, false, "none"), std::invalid_argument); + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, false, AntiwindupStrategy::NONE), + std::invalid_argument); } TEST(ParameterTest, UTermBadIBoundsTest) @@ -60,13 +62,14 @@ TEST(ParameterTest, UTermBadIBoundsTest) "This test checks if gains remain for bad u_bounds specification (i.e. u_min > u_max)."); // Pid(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, std::string antiwindup_strat); - Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 0.0, false, false, "none"); + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 0.0, false, false, AntiwindupStrategy::NONE); auto gains = pid.get_gains(); EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); // Try to set bad u-bounds, i.e. u_min > u_max - EXPECT_NO_THROW(pid.set_gains(1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, false, "none")); + EXPECT_NO_THROW(pid.set_gains( + 1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, false, AntiwindupStrategy::NONE)); // Check if gains were not updated because u-bounds are bad, i.e. u_min > u_max EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); @@ -78,8 +81,9 @@ TEST(ParameterTest, outputClampTest) "description", "This test succeeds if the output is clamped when the saturation is active."); // Pid(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, std::string antiwindup_strat); - Pid pid(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, true, false, "back_calculation"); + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, true, false, AntiwindupStrategy::BACK_CALCULATION); double cmd = 0.0; @@ -130,8 +134,9 @@ TEST(ParameterTest, noOutputClampTest) "description", "This test succeeds if the output isn't clamped when the saturation is false."); // Pid(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, std::string antiwindup_strat); - Pid pid(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, false, false, "back_calculation"); + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, false, false, AntiwindupStrategy::BACK_CALCULATION); double cmd = 0.0; @@ -184,8 +189,9 @@ TEST(ParameterTest, integrationBackCalculationZeroGainTest) "the back calculation technique."); // Pid(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, std::string antiwindup_strat); - Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, "back_calculation"); + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, AntiwindupStrategy::BACK_CALCULATION); double cmd = 0.0; double pe, ie, de; @@ -231,8 +237,10 @@ TEST(ParameterTest, integrationConditioningTechniqueZeroGainTest) "the conditioning technique."); // Pid(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, std::string antiwindup_strat); - Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, "conditioning_technique"); + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, + AntiwindupStrategy::CONDITIONING_TECHNIQUE); double cmd = 0.0; double pe, ie, de; @@ -278,8 +286,10 @@ TEST(ParameterTest, integrationConditionalIntegrationZeroGainTest) "the conditional integration technique."); // Pid(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, std::string antiwindup_strat); - Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, "conditional_integration"); + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, + AntiwindupStrategy::CONDITIONAL_INTEGRATION); double cmd = 0.0; double pe, ie, de; @@ -465,7 +475,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double trk_tc = std::rand() % 100; bool saturation = false; bool antiwindup = false; - std::string antiwindup_strat = "none"; + AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE; // Initialize the default way Pid pid1( @@ -476,7 +486,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, u_min_return, trk_tc_return; bool saturation_return, antiwindup_return; - std::string antiwindup_strat_return; + AntiwindupStrategy antiwindup_strat_return; pid1.get_gains( p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, @@ -507,7 +517,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) trk_tc = std::rand() % 100; saturation = false; antiwindup = false; - antiwindup_strat = "none"; + antiwindup_strat = AntiwindupStrategy::NONE; pid1.set_gains( p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, @@ -766,8 +776,9 @@ TEST(CommandTest, backCalculationPIDTest) "back calculation technique."); // Pid(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, std::string antiwindup_strat); - Pid pid(0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, "back_calculation"); + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, AntiwindupStrategy::BACK_CALCULATION); double cmd = 0.0; double pe, ie, de; @@ -823,8 +834,10 @@ TEST(CommandTest, conditioningTechniquePIDTest) "conditioning technique."); // Pid(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, std::string antiwindup_strat); - Pid pid(0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, "conditioning_technique"); + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, + AntiwindupStrategy::CONDITIONING_TECHNIQUE); double cmd = 0.0; double pe, ie, de; @@ -880,8 +893,10 @@ TEST(CommandTest, conditionalIntegrationPIDTest) "conditional integration technique."); // Pid(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, std::string antiwindup_strat); - Pid pid(0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, "conditional_integration"); + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, + AntiwindupStrategy::CONDITIONAL_INTEGRATION); double cmd = 0.0; double pe, ie, de; From 9c14cb89c4690c9285ed4e889b59e5884f1d05ef Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 5 May 2025 13:57:08 +0000 Subject: [PATCH 26/28] Add deprecated message for the old anti-windup --- control_toolbox/src/pid_ros.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index f64d701e..74cdb854 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -238,6 +238,13 @@ bool PidROS::initialize_from_ros_parameters() set_parameter_event_callback(); } + if (antiwindup_strat_str == "none") + { + std::cout << "Old anti-windup technique is deprecated. " + "This option will be removed by the ROS 2 Kilted Kaiju release." + << std::endl; + } + AntiwindupStrategy antiwindup_strat(antiwindup_strat_str); pid_.initialize( @@ -283,6 +290,13 @@ void PidROS::initialize_from_args( } else { + if (antiwindup_strat == AntiwindupStrategy::NONE) + { + std::cout << "Old anti-windup technique is deprecated. " + "This option will be removed by the ROS 2 Kilted Kaiju release." + << std::endl; + } + pid_.initialize( p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); @@ -353,6 +367,13 @@ void PidROS::set_gains( } else { + if (antiwindup_strat == AntiwindupStrategy::NONE) + { + std::cout << "Old anti-windup technique is deprecated. " + "This option will be removed by the ROS 2 Kilted Kaiju release." + << std::endl; + } + node_params_->set_parameters( {rclcpp::Parameter(param_prefix_ + "p", p), rclcpp::Parameter(param_prefix_ + "i", i), rclcpp::Parameter(param_prefix_ + "d", d), From db75c8343b1e3fe8e362486f5c3d93e8c7332627 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 5 May 2025 16:18:17 +0000 Subject: [PATCH 27/28] Add deprecated message for the old anti-windup --- control_toolbox/include/control_toolbox/pid_ros.hpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index 7f8687c4..3b53dc90 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -144,6 +144,11 @@ class PidROS 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_. @@ -244,6 +249,12 @@ class PidROS 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_. */ From 3058121e57bf747fc19a02659943008972c87e8e Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Mon, 5 May 2025 16:41:17 +0000 Subject: [PATCH 28/28] Calculate tracking time constant in set_gains --- control_toolbox/src/pid.cpp | 44 ++++++++++++++++++++++--------------- 1 file changed, 26 insertions(+), 18 deletions(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index cd75fc9a..3bbb4c8e 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -180,18 +180,41 @@ void Pid::set_gains( set_gains(gains); } -void Pid::set_gains(const Gains & gains) +void Pid::set_gains(const Gains & gains_in) { - if (gains.i_min_ > gains.i_max_) + if (gains_in.i_min_ > gains_in.i_max_) { std::cout << "received i_min > i_max, skip new gains\n"; } - else if (gains.u_min_ > gains.u_max_) + else if (gains_in.u_min_ > gains_in.u_max_) { std::cout << "received u_min > u_max, skip new gains\n"; } else { + Gains gains = gains_in; + + if (gains.antiwindup_strat_ == AntiwindupStrategy::BACK_CALCULATION) + { + if (is_zero(gains.trk_tc_) && !is_zero(gains.d_gain_)) + { + // Default value for tracking time constant for back calculation technique + gains.trk_tc_ = std::sqrt(gains.d_gain_ / gains.i_gain_); + } + else if (is_zero(gains.trk_tc_) && is_zero(gains.d_gain_)) + { + // Default value for tracking time constant for back calculation technique + gains.trk_tc_ = gains.p_gain_ / gains.i_gain_; + } + } + else if (gains.antiwindup_strat_ == AntiwindupStrategy::CONDITIONING_TECHNIQUE) + { + if (is_zero(gains.trk_tc_)) + { + // Default value for tracking time constant for conditioning technique + gains.trk_tc_ = gains.p_gain_; + } + } gains_buffer_.writeFromNonRT(gains); } } @@ -317,27 +340,12 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) if (gains.antiwindup_strat_ == AntiwindupStrategy::BACK_CALCULATION && !is_zero(gains.i_gain_)) { - if (is_zero(gains.trk_tc_) && !is_zero(gains.d_gain_)) - { - // Default value for tracking time constant for back calculation technique - gains.trk_tc_ = std::sqrt(gains.d_gain_ / gains.i_gain_); - } - else if (is_zero(gains.trk_tc_) && is_zero(gains.d_gain_)) - { - // Default value for tracking time constant for back calculation technique - gains.trk_tc_ = gains.p_gain_ / gains.i_gain_; - } i_term_ += dt_s * (gains.i_gain_ * error + 1 / gains.trk_tc_ * (cmd_ - cmd_unsat_)); } else if ( gains.antiwindup_strat_ == AntiwindupStrategy::CONDITIONING_TECHNIQUE && !is_zero(gains.i_gain_)) { - if (is_zero(gains.trk_tc_)) - { - // Default value for tracking time constant for conditioning technique - gains.trk_tc_ = gains.p_gain_; - } i_term_ += dt_s * gains.i_gain_ * (error + 1 / gains.trk_tc_ * (cmd_ - cmd_unsat_)); } else if (gains.antiwindup_strat_ == AntiwindupStrategy::CONDITIONAL_INTEGRATION)