diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 6e9f61ba..6443b34f 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -39,64 +39,160 @@ #include #include +#include "fmt/format.h" #include "rclcpp/duration.hpp" #include "realtime_tools/realtime_buffer.hpp" namespace control_toolbox { -class AntiwindupStrategy +/** + * \brief Antiwindup strategy for PID controllers. + * + * This class defines various antiwindup strategies that can be used in PID controllers. + * It allows setting the type of antiwindup strategy and validates the parameters accordingly. + * + * \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 tracking_time_constant Specifies the tracking time constant for the 'back_calculation' strategy. If set + * to 0.0 when this strategy is selected, a recommended default value will be applied. + * \param legacy_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 error_deadband Error deadband is used to stop integration when the error is within the given range. + * \param type Specifies the antiwindup strategy type. Valid values are: + * - `NONE`: No antiwindup strategy applied. + * - `LEGACY`: Legacy antiwindup strategy, which limits the integral term to prevent windup (deprecated: This option will be removed in a future release). + * - `BACK_CALCULATION`: Back calculation antiwindup strategy, which uses a tracking time constant. + * - `CONDITIONAL_INTEGRATION`: Conditional integration antiwindup strategy, which integrates only when certain conditions are met. + */ +struct AntiWindupStrategy { public: enum Value : int8_t { - NONE = 0, + UNDEFINED = -1, + NONE, + LEGACY, BACK_CALCULATION, CONDITIONAL_INTEGRATION }; - constexpr AntiwindupStrategy() : value_(NONE) {} - constexpr AntiwindupStrategy(Value v) : value_(v) {} // NOLINT(runtime/explicit) + AntiWindupStrategy() + : type(UNDEFINED), + i_min(std::numeric_limits::quiet_NaN()), + i_max(std::numeric_limits::quiet_NaN()), + legacy_antiwindup(false), + tracking_time_constant(0.0), + error_deadband(std::numeric_limits::epsilon()) + { + } - explicit AntiwindupStrategy(const std::string & s) + void set_type(const std::string & s) { if (s == "back_calculation") { - value_ = BACK_CALCULATION; + type = BACK_CALCULATION; } else if (s == "conditional_integration") { - value_ = CONDITIONAL_INTEGRATION; + type = CONDITIONAL_INTEGRATION; + } + else if (s == "legacy") + { + type = LEGACY; + std::cout << "Using the legacy anti-windup technique is deprecated. This option will be " + "removed by the ROS 2 Kilted Kaiju release." + << std::endl; + } + else if (s == "none") + { + type = NONE; } else { - value_ = NONE; + type = UNDEFINED; + throw std::invalid_argument( + "AntiWindupStrategy: Unknown antiwindup strategy : '" + s + + "'. Valid strategies are: 'back_calculation', 'conditional_integration', 'legacy', " + "'none'."); } } - operator std::string() const { return to_string(); } + void validate() const + { + if (type == UNDEFINED) + { + throw std::invalid_argument("AntiWindupStrategy is UNDEFINED. Please set a valid type"); + } + if ( + type == BACK_CALCULATION && + (tracking_time_constant < 0.0 || !std::isfinite(tracking_time_constant))) + { + throw std::invalid_argument( + "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant " + "(tracking_time_constant)"); + } + if (type == LEGACY && ((i_min > i_max) || !std::isfinite(i_min) || !std::isfinite(i_max))) + { + throw std::invalid_argument( + fmt::format( + "AntiWindupStrategy 'legacy' requires i_min < i_max and to be finite (i_min: {}, i_max: " + "{})", + i_min, i_max)); + } + if (std::isfinite(i_min) || std::isfinite(i_max)) + { + std::cout << "Warning: The i_min and i_max are only valid for the deprecated LEGACY " + "antiwindup strategy. Please use the AntiWindupStrategy::set_type() method to " + "set the type of antiwindup strategy you want to use." + << std::endl; + } + if ( + type != NONE && type != UNDEFINED && type != LEGACY && type != BACK_CALCULATION && + type != CONDITIONAL_INTEGRATION) + { + throw std::invalid_argument("AntiWindupStrategy has an invalid type"); + } + } - constexpr bool operator==(AntiwindupStrategy other) const { return value_ == other.value_; } - constexpr bool operator!=(AntiwindupStrategy other) const { return value_ != other.value_; } + operator std::string() const { return to_string(); } - constexpr bool operator==(Value other) const { return value_ == other; } - constexpr bool operator!=(Value other) const { return value_ != other; } + constexpr bool operator==(Value other) const { return type == other; } + constexpr bool operator!=(Value other) const { return type != other; } std::string to_string() const { - switch (value_) + switch (type) { case BACK_CALCULATION: return "back_calculation"; case CONDITIONAL_INTEGRATION: return "conditional_integration"; + case LEGACY: + return "legacy"; case NONE: - default: return "none"; + case UNDEFINED: + default: + return "UNDEFINED"; } } -private: - Value value_; + Value type = UNDEFINED; + double i_min = std::numeric_limits::quiet_NaN(); /**< Minimum allowable integral term. */ + double i_max = std::numeric_limits::quiet_NaN(); /**< Maximum allowable integral term. */ + + bool legacy_antiwindup = false; /**< Use legacy anti-windup strategy. */ + + // tracking_time_constant Specifies the tracking time constant for the 'back_calculation' + // strategy. If set to 0.0 a recommended default value will be applied. + double tracking_time_constant = 0.0; /**< Tracking time constant for back_calculation strategy. */ + + double error_deadband = + std::numeric_limits::epsilon(); /**< Error deadband to avoid integration. */ }; template @@ -151,9 +247,6 @@ inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) \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' strategy. - - \section Usage To use the Pid class, you should first call some version of init() @@ -194,12 +287,21 @@ class Pid * \param i_min Lower integral clamp. * */ - [[deprecated("Use constructor with AntiwindupStrategy instead.")]] + [[deprecated("Use constructor with AntiWindupStrategy instead.")]] Gains(double p, double i, double d, double i_max, double i_min) - : Gains( - p, i, d, i_max, i_min, std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), 0.0, true, AntiwindupStrategy::NONE) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(std::numeric_limits::infinity()), + u_min_(-std::numeric_limits::infinity()), + antiwindup_(false) { + antiwindup_strat_.type = AntiWindupStrategy::LEGACY; + antiwindup_strat_.i_max = i_max; + antiwindup_strat_.i_min = i_min; + antiwindup_strat_.legacy_antiwindup = true; } /*! @@ -216,49 +318,47 @@ class Pid i_min are applied in both scenarios. * */ - [[deprecated("Use constructor with AntiwindupStrategy instead.")]] + [[deprecated("Use constructor with AntiWindupStrategy instead.")]] Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) - : Gains( - p, i, d, i_max, i_min, std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), 0.0, antiwindup, AntiwindupStrategy::NONE) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(std::numeric_limits::infinity()), + u_min_(-std::numeric_limits::infinity()), + antiwindup_(antiwindup) { + antiwindup_strat_.type = AntiWindupStrategy::LEGACY; + antiwindup_strat_.i_max = i_max; + antiwindup_strat_.i_min = i_min; + antiwindup_strat_.legacy_antiwindup = antiwindup; } /*! - * \brief Optional constructor for passing in values + * \brief 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' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. + tracking_time_constant parameter to tune the anti-windup behavior. * */ - [[deprecated("Use constructor with AntiwindupStrategy only.")]] Gains( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat) + double p, double i, double d, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat) : p_gain_(p), i_gain_(i), d_gain_(d), - i_max_(i_max), - i_min_(i_min), + i_max_(antiwindup_strat.i_max), + i_min_(antiwindup_strat.i_min), u_max_(u_max), u_min_(u_min), - trk_tc_(trk_tc), - antiwindup_(antiwindup), + antiwindup_(antiwindup_strat.legacy_antiwindup), antiwindup_strat_(antiwindup_strat) { if (std::isnan(u_min) || std::isnan(u_max)) @@ -274,31 +374,38 @@ class Pid } } - /*! - * \brief Constructor for passing in values. - * - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. - * \param u_max Upper output clamp. - * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. - * - */ - Gains( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat) - : Gains(p, i, d, 0.0, 0.0, u_max, u_min, trk_tc, false, antiwindup_strat) + bool validate(std::string & error_msg) const { + if (i_min_ > i_max_) + { + error_msg = fmt::format("Gains: i_min ({}) must be less than i_max ({})", i_min_, i_max_); + return false; + } + else if (u_min_ >= u_max_) + { + error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_); + return false; + } + else if (std::isnan(u_min_) || std::isnan(u_max_)) + { + error_msg = "Gains: u_min and u_max must not be NaN"; + return false; + } + try + { + antiwindup_strat_.validate(); + } + catch (const std::exception & e) + { + error_msg = e.what(); + return false; + } + return true; } // Default constructor [[deprecated( - "Use constructor with AntiwindupStrategy only. The default constructor might be deleted in " + "Use constructor with AntiWindupStrategy only. The default constructor might be deleted in " "future")]] Gains() { } @@ -307,8 +414,7 @@ class Pid { std::cout << "Gains: p: " << p_gain_ << ", i: " << i_gain_ << ", d: " << d_gain_ << ", i_max: " << i_max_ << ", i_min: " << i_min_ << ", u_max: " << u_max_ - << ", u_min: " << u_min_ << ", trk_tc: " << trk_tc_ - << ", antiwindup: " << antiwindup_ + << ", u_min: " << u_min_ << ", antiwindup: " << antiwindup_ << ", antiwindup_strat: " << antiwindup_strat_.to_string() << std::endl; } @@ -319,9 +425,8 @@ class Pid double i_min_ = 0.0; /**< Minimum allowable integral term. */ double u_max_ = std::numeric_limits::infinity(); /**< Maximum allowable output. */ double u_min_ = -std::numeric_limits::infinity(); /**< Minimum allowable output. */ - double trk_tc_ = 0.0; /**< Tracking time constant. */ bool antiwindup_ = false; /**< Anti-windup. */ - AntiwindupStrategy antiwindup_strat_ = AntiwindupStrategy::NONE; /**< Anti-windup strategy. */ + AntiWindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ }; /*! @@ -340,39 +445,11 @@ class Pid * * \throws An std::invalid_argument exception is thrown if i_min > i_max */ - [[deprecated("Use constructor with AntiwindupStrategy only.")]] + [[deprecated("Use constructor with AntiWindupStrategy only.")]] 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, initialize Pid-gains and 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 u_max Upper output clamp. - * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. - * - * \throws An std::invalid_argument exception is thrown if i_min > i_max or u_min > u_max - */ - [[deprecated("Use constructor with AntiwindupStrategy only.")]] - Pid( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); - /*! * \brief Constructor, initialize Pid-gains and term limits. * @@ -381,8 +458,6 @@ class Pid * \param d The derivative gain. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. @@ -390,8 +465,8 @@ class Pid * \throws An std::invalid_argument exception is thrown if u_min > u_max */ Pid( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat); + double p, double i, double d, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat); /*! * \brief Copy constructor required for preventing mutexes from being copied @@ -416,41 +491,13 @@ class Pid 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. - * + * \return True if all parameters are successfully set, False otherwise. * \note New gains are not applied if i_min_ > i_max_ */ - [[deprecated("Use initialize with AntiwindupStrategy instead.")]] - void initialize( + [[deprecated("Use initialize with AntiWindupStrategy instead.")]] + bool initialize( double p, double i, double d, double i_max, double i_min, bool antiwindup = false); - /*! - * \brief Initialize Pid-gains and 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 u_max Upper output clamp. - * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. - * - * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max - */ - [[deprecated("Use initialize with AntiwindupStrategy only.")]] - void initialize( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat); - /*! * \brief Initialize Pid-gains and term limits. * @@ -459,17 +506,16 @@ class Pid * \param d The derivative gain. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. + * \return True if all parameters are successfully set, False otherwise. * * \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 u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat); + bool initialize( + double p, double i, double d, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat); /*! * \brief Reset the state of this PID controller @@ -511,35 +557,10 @@ class Pid integral contribution to the control output. i_max and i_min are applied in both scenarios. */ - [[deprecated("Use get_gains overload without bool antiwindup.")]] + [[deprecated("Use get_gains overload with AntiWindupStrategy argument.")]] 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' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. - */ - [[deprecated("Use get_gains overload with AntiwindupStrategy only.")]] - 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 & antiwindup, AntiwindupStrategy & antiwindup_strat); - /*! * \brief Get PID gains for the controller (preferred). * \param p The proportional gain. @@ -547,15 +568,13 @@ class Pid * \param d The derivative gain. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. */ void get_gains( - double & p, double & i, double & d, double & u_max, double & u_min, double & trk_tc, - AntiwindupStrategy & antiwindup_strat); + double & p, double & i, double & d, double & u_max, double & u_min, + AntiWindupStrategy & antiwindup_strat); /*! * \brief Get PID gains for the controller. @@ -574,39 +593,12 @@ class Pid 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. + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if i_min > i_max */ - [[deprecated("Use set_gains with AntiwindupStrategy instead.")]] - void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); - - /*! - * \brief Set 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' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. - * - * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max - */ - [[deprecated("Use set_gains with AntiwindupStrategy only.")]] - void set_gains( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc = 0.0, bool antiwindup = false, - AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE); + [[deprecated("Use set_gains with AntiWindupStrategy instead.")]] + bool set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! * \brief Set PID gains for the controller. @@ -616,25 +608,25 @@ class Pid * \param d The derivative gain. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. + * \return True if all parameters are successfully set, False otherwise. * * \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 u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat); + bool set_gains( + double p, double i, double d, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat); /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if gains.i_min_ > gains.i_max_ */ - void set_gains(const Gains & gains); + bool set_gains(const Gains & gains); /*! * \brief Set the PID error and compute the PID command with nonuniform time diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index f3f2dffc..3cd06578 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -96,11 +96,12 @@ class PidROS 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. + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if i_min_ > i_max_ */ - [[deprecated("Use initialize_from_args with AntiwindupStrategy instead.")]] - void initialize_from_args( + [[deprecated("Use initialize_from_args with AntiWindupStrategy instead.")]] + bool initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup); /*! @@ -115,46 +116,14 @@ class PidROS 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. + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if i_min_ > i_max_ */ - [[deprecated("Use initialize_from_args with AntiwindupStrategy instead.")]] - void initialize_from_args( + [[deprecated("Use initialize_from_args with AntiWindupStrategy instead.")]] + bool initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term); - /*! - * \brief Initialize the PID controller and set the parameters - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. - * \param i_max The max integral windup. - * \param i_min The min integral windup. - * \param u_max Upper output clamp. - * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. - * \deprecated{only when `antiwindup_strat == AntiwindupStrategy::NONE`:} - * Old anti-windup technique is deprecated and will be removed by - * the ROS 2 Kilted Kaiju release. - * \warning{If you pass `AntiwindupStrategy::NONE`, at runtime a warning will be printed:} - * `"Old anti-windup technique is deprecated. This option will be removed by the ROS 2 Kilted Kaiju release."` - * \param save_i_term save integrator output between resets. - * - * \note New gains are not applied if i_min_ > i_max_ or if u_min_ > u_max_. - */ - [[deprecated("Use initialize_from_args with AntiwindupStrategy only.")]] - void initialize_from_args( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat, bool save_i_term); - /*! * \brief Initialize the PID controller and set the parameters. * @@ -163,22 +132,22 @@ class PidROS * \param d The derivative gain. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. * \param save_i_term save integrator output between resets. + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if u_min_ > u_max_. */ - void initialize_from_args( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat, bool save_i_term); + bool initialize_from_args( + double p, double i, double d, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, bool save_i_term); /*! * \brief Initialize the PID controller based on already set parameters - * \return True if all parameters are set (p, i, d, i_max, i_min, u_max, u_min and trk_tc), False otherwise + * \return True if all parameters are set (p, i, d, i_max, i_min, u_max, u_min), False otherwise + * \return False if the parameters are not set or if the parameters are invalid */ bool initialize_from_ros_parameters(); @@ -238,45 +207,12 @@ class PidROS 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. + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if i_min > i_max */ - [[deprecated("Use set_gains with AntiwindupStrategy instead.")]] - void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); - - /*! - * \brief Initialize the PID controller and set the parameters - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. - * \param i_max The max integral windup. - * \param i_min The min integral windup. - * \param u_max Upper output clamp. - * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. - * \param antiwindup Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_max and - i_min are applied in both scenarios. - * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the - tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other - than 'none' is selected, it will override the controller's default anti-windup behavior. - * \deprecated{only when `antiwindup_strat == AntiwindupStrategy::NONE`:} - * Old anti-windup technique is deprecated and will be removed by - * the ROS 2 Kilted Kaiju release. - * \warning{If you pass `AntiwindupStrategy::NONE`, at runtime a warning will be printed:} - * `"Old anti-windup technique is deprecated. This option will be removed by - * the ROS 2 Kilted Kaiju release."` - * - * \note New gains are not applied if i_min > i_max or if u_min_ > u_max_. - */ - [[deprecated("Use set_gains with AntiwindupStrategy only.")]] - void set_gains( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc = 0.0, bool antiwindup = false, - AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE); + [[deprecated("Use set_gains with AntiWindupStrategy instead.")]] + bool set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! * \brief Set PID gains for the controller (preferred). @@ -286,25 +222,25 @@ class PidROS * \param d The derivative gain. * \param u_max Upper output clamp. * \param u_min Lower output clamp. - * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set - * to 0.0 when this strategy is selected, a recommended default value will be applied. * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior. + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if u_min_ > u_max_. */ - void set_gains( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat); + bool set_gains( + double p, double i, double d, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat); /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values + * \return True if all parameters are successfully set, False otherwise. * * \note New gains are not applied if gains.i_min_ > gains.i_max_ */ - void set_gains(const Pid::Gains & gains); + bool set_gains(const Pid::Gains & gains); /*! * \brief Set current command for this PID controller diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index dc0246ad..19bce26c 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -50,46 +50,36 @@ constexpr double UMAX_INFINITY = std::numeric_limits::infinity(); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) -: Pid( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, AntiwindupStrategy::NONE) -{ -} -#pragma GCC diagnostic pop - -Pid::Pid( - double p, double i, double d, double i_max, double i_min, double u_max, double u_min, - double trk_tc, bool antiwindup, AntiwindupStrategy antiwindup_strat) : gains_buffer_() { if (i_min > i_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"); - } -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.legacy_antiwindup = antiwindup; + set_gains(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat); // Initialize saved i-term values clear_saved_iterm(); reset(); } +#pragma GCC diagnostic pop Pid::Pid( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat) + double p, double i, double d, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat) : gains_buffer_() { if (u_min > u_max) { throw std::invalid_argument("received u_min > u_max"); } - set_gains(p, i, d, u_max, u_min, trk_tc, antiwindup_strat); + set_gains(p, i, d, u_max, u_min, antiwindup_strat); // Initialize saved i-term values clear_saved_iterm(); @@ -111,37 +101,29 @@ Pid::Pid(const Pid & source) Pid::~Pid() {} -void Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) -{ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - initialize( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, - AntiwindupStrategy::NONE); -#pragma GCC diagnostic pop - - 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 antiwindup, AntiwindupStrategy antiwindup_strat) +bool Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop - - reset(); + if (set_gains(p, i, d, i_max, i_min, antiwindup)) + { + reset(); + return true; + } + return false; } +#pragma GCC diagnostic pop -void Pid::initialize( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat) +bool Pid::initialize( + double p, double i, double d, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat) { - set_gains(p, i, d, u_max, u_min, trk_tc, antiwindup_strat); - - reset(); + if (set_gains(p, i, d, u_max, u_min, antiwindup_strat)) + { + reset(); + return true; + } + return false; } void Pid::reset() { reset(false); } @@ -166,13 +148,10 @@ void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & { double u_max; double u_min; - double trk_tc; - bool antiwindup; - AntiwindupStrategy antiwindup_strat; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - get_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop + AntiWindupStrategy antiwindup_strat; + get_gains(p, i, d, u_max, u_min, antiwindup_strat); + i_max = antiwindup_strat.i_max; + i_min = antiwindup_strat.i_min; } void Pid::get_gains( @@ -180,44 +159,24 @@ void Pid::get_gains( { double u_max; double u_min; - double trk_tc; - AntiwindupStrategy antiwindup_strat; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - get_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop + AntiWindupStrategy antiwindup_strat; + get_gains(p, i, d, u_max, u_min, antiwindup_strat); + i_max = antiwindup_strat.i_max; + i_min = antiwindup_strat.i_min; + antiwindup = antiwindup_strat.legacy_antiwindup; } 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 & antiwindup, AntiwindupStrategy & antiwindup_strat) + double & p, double & i, double & d, double & u_max, double & u_min, + AntiWindupStrategy & antiwindup_strat) { Gains gains = *gains_buffer_.readFromRT(); p = gains.p_gain_; i = gains.i_gain_; 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_; - antiwindup = gains.antiwindup_; - antiwindup_strat = gains.antiwindup_strat_; -} - -void Pid::get_gains( - double & p, double & i, double & d, double & u_max, double & u_min, double & trk_tc, - AntiwindupStrategy & antiwindup_strat) -{ - Gains gains = *gains_buffer_.readFromRT(); - - p = gains.p_gain_; - i = gains.i_gain_; - d = gains.d_gain_; - u_max = gains.u_max_; - u_min = gains.u_min_; - trk_tc = gains.trk_tc_; antiwindup_strat = gains.antiwindup_strat_; } @@ -225,75 +184,72 @@ Pid::Gains Pid::get_gains() { return *gains_buffer_.readFromRT(); } #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" -void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) +bool Pid::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, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, - AntiwindupStrategy::NONE); -} -#pragma GCC diagnostic pop - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" -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 antiwindup, AntiwindupStrategy antiwindup_strat) -{ - Gains gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); - - set_gains(gains); + try + { + Gains gains(p, i, d, i_max, i_min, antiwindup); + if (set_gains(gains)) + { + return true; + } + } + catch (const std::exception & e) + { + std::cerr << e.what() << '\n'; + } + return false; } #pragma GCC diagnostic pop -void Pid::set_gains( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat) +bool Pid::set_gains( + double p, double i, double d, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat) { - double i_min = 0.0; - double i_max = 0.0; - bool antiwindup = false; - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - Gains gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop - - set_gains(gains); -} - -void Pid::set_gains(const Gains & gains_in) -{ - if (gains_in.i_min_ > gains_in.i_max_) + try { - std::cout << "Received i_min > i_max, skip new gains" << std::endl; + Gains gains(p, i, d, u_max, u_min, antiwindup_strat); + if (set_gains(gains)) + { + return true; + } } - else if (gains_in.u_min_ > gains_in.u_max_) + catch (const std::exception & e) { - std::cout << "Received u_min > u_max, skip new gains" << std::endl; + std::cerr << e.what() << '\n'; } - else if (std::isnan(gains_in.u_min_) || std::isnan(gains_in.u_max_)) + return false; +} + +bool Pid::set_gains(const Gains & gains_in) +{ + std::string error_msg = ""; + if (!gains_in.validate(error_msg)) { - std::cout << "Received NaN for u_min or u_max, skipping new gains" << std::endl; + std::cerr << "PID: Invalid gains: " << error_msg << ". SKipping new gains." << std::endl; + return false; } else { Gains gains = gains_in; - if (gains.antiwindup_strat_ == AntiwindupStrategy::BACK_CALCULATION) + if (gains.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION) { - if (is_zero(gains.trk_tc_) && !is_zero(gains.d_gain_)) + if (is_zero(gains.antiwindup_strat_.tracking_time_constant) && !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_); + gains.antiwindup_strat_.tracking_time_constant = std::sqrt(gains.d_gain_ / gains.i_gain_); } - else if (is_zero(gains.trk_tc_) && is_zero(gains.d_gain_)) + else if (is_zero(gains.antiwindup_strat_.tracking_time_constant) && is_zero(gains.d_gain_)) { // Default value for tracking time constant for back calculation technique - gains.trk_tc_ = gains.p_gain_ / gains.i_gain_; + gains.antiwindup_strat_.tracking_time_constant = gains.p_gain_ / gains.i_gain_; } } gains_buffer_.writeFromNonRT(gains); + return true; } + return false; } double Pid::compute_command(double error, const double & dt_s) @@ -311,7 +267,7 @@ double Pid::compute_command(double error, const double & dt_s) // don't reset controller but return NaN if (!std::isfinite(error)) { - std::cout << "Received a non-finite error value\n"; + std::cerr << "Received a non-finite error value\n"; return cmd_ = std::numeric_limits::quiet_NaN(); } @@ -373,7 +329,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) // 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"; + std::cerr << "Received a non-finite error/error_dot value\n"; return cmd_ = std::numeric_limits::quiet_NaN(); } @@ -383,19 +339,32 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) // Calculate derivative contribution to command d_term = gains.d_gain_ * d_error_; - // Calculate integral contribution to command - if (gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) + if (gains.antiwindup_strat_.type == AntiWindupStrategy::UNDEFINED) { - // 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_); + throw std::runtime_error( + "PID: Antiwindup strategy cannot be UNDEFINED. Please set a valid antiwindup strategy."); } - else if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) + + // Calculate integral contribution to command + const bool is_error_in_deadband_zone = + control_toolbox::is_zero(error, gains.antiwindup_strat_.error_deadband); + if (!is_error_in_deadband_zone && gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY) { - i_term_ += gains.i_gain_ * dt_s * p_error_; + if (gains.antiwindup_strat_.legacy_antiwindup) + { + // 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_ += gains.i_gain_ * dt_s * p_error_; + } } // Compute the command - if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) + if ( + !gains.antiwindup_strat_.legacy_antiwindup && + gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY) { // 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; @@ -422,15 +391,21 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) cmd_ = cmd_unsat_; } - if (gains.antiwindup_strat_ == AntiwindupStrategy::BACK_CALCULATION && !is_zero(gains.i_gain_)) - { - i_term_ += dt_s * (gains.i_gain_ * error + 1 / gains.trk_tc_ * (cmd_ - cmd_unsat_)); - } - else if (gains.antiwindup_strat_ == AntiwindupStrategy::CONDITIONAL_INTEGRATION) + if (!is_error_in_deadband_zone) { - if (!(!iszero(cmd_unsat_ - cmd_) && error * cmd_unsat_ > 0)) + if ( + gains.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && + !is_zero(gains.i_gain_)) + { + i_term_ += dt_s * (gains.i_gain_ * error + + 1 / gains.antiwindup_strat_.tracking_time_constant * (cmd_ - cmd_unsat_)); + } + else if (gains.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION) { - i_term_ += dt_s * gains.i_gain_ * error; + if (!(!is_zero(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 33fa9ca8..29d900de 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -215,12 +215,13 @@ bool PidROS::get_string_param(const std::string & param_name, std::string & valu bool PidROS::initialize_from_ros_parameters() { - double p, i, d, i_max, i_min, u_max, u_min, trk_tc; - p = i = d = i_max = i_min = trk_tc = std::numeric_limits::quiet_NaN(); + double p, i, d, i_max, i_min, u_max, u_min, tracking_time_constant, error_deadband; + p = i = d = i_max = i_min = tracking_time_constant = std::numeric_limits::quiet_NaN(); + error_deadband = std::numeric_limits::epsilon(); u_max = UMAX_INFINITY; u_min = -UMAX_INFINITY; bool antiwindup = false; - std::string antiwindup_strat_str = "none"; + std::string antiwindup_strat_str = "legacy"; bool all_params_available = true; all_params_available &= get_double_param(param_prefix_ + "p", p); @@ -230,7 +231,9 @@ bool PidROS::initialize_from_ros_parameters() 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); + all_params_available &= get_double_param(param_prefix_ + "error_deadband", error_deadband); + all_params_available &= + get_double_param(param_prefix_ + "tracking_time_constant", tracking_time_constant); get_boolean_param(param_prefix_ + "antiwindup", antiwindup); get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat_str); @@ -241,21 +244,35 @@ bool PidROS::initialize_from_ros_parameters() set_parameter_event_callback(); } - if (antiwindup_strat_str == "none") + RCLCPP_WARN_EXPRESSION( + node_logging_->get_logger(), antiwindup_strat_str == "legacy", + "Using the legacy anti-windup technique is deprecated. This option will be removed by the ROS " + "2 Kilted Kaiju release."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.set_type(antiwindup_strat_str); + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.tracking_time_constant = tracking_time_constant; + antiwindup_strat.legacy_antiwindup = antiwindup; + antiwindup_strat.error_deadband = error_deadband; + + try { - std::cout << "Old anti-windup technique is deprecated. " - "This option will be removed by the ROS 2 Kilted Kaiju release." - << std::endl; + antiwindup_strat.validate(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); + return false; } - AntiwindupStrategy antiwindup_strat(antiwindup_strat_str); - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - pid_.initialize(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop + if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat)) + { + return all_params_available; + } - return all_params_available; + return false; } void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValue param_value) @@ -266,101 +283,86 @@ 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) -{ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - initialize_from_args( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, AntiwindupStrategy::NONE, - false); -#pragma GCC diagnostic pop +bool PidROS::initialize_from_args( + double p, double i, double d, double i_max, double i_min, bool antiwindup) +{ + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.legacy_antiwindup = antiwindup; + + return initialize_from_args(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, false); } +#pragma GCC diagnostic pop -void PidROS::initialize_from_args( +bool PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term) { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - initialize_from_args( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, AntiwindupStrategy::NONE, - save_i_term); -#pragma GCC diagnostic pop + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.legacy_antiwindup = antiwindup; + + return initialize_from_args( + p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, 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 antiwindup, AntiwindupStrategy antiwindup_strat, bool save_i_term) +bool PidROS::initialize_from_args( + double p, double i, double d, double u_max, double u_min, + const AntiWindupStrategy & 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) + Pid::Gains verify_gains(p, i, d, u_max, u_min, antiwindup_strat); + std::string error_msg = ""; + if (!verify_gains.validate(error_msg)) { - RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + RCLCPP_ERROR( + node_logging_->get_logger(), "Received invalid gains: %s. Skipping new gains.", + error_msg.c_str()); + return false; } else { - if (antiwindup_strat == AntiwindupStrategy::NONE) + if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat)) { - std::cout << "Old anti-windup technique is deprecated. " - "This option will be removed by the ROS 2 Kilted Kaiju release." - << std::endl; + const Pid::Gains gains = pid_.get_gains(); + + declare_param(param_prefix_ + "p", rclcpp::ParameterValue(gains.p_gain_)); + declare_param(param_prefix_ + "i", rclcpp::ParameterValue(gains.i_gain_)); + declare_param(param_prefix_ + "d", rclcpp::ParameterValue(gains.d_gain_)); + declare_param( + param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(gains.antiwindup_strat_.i_max)); + declare_param( + param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(gains.antiwindup_strat_.i_min)); + declare_param(param_prefix_ + "u_clamp_max", rclcpp::ParameterValue(gains.u_max_)); + declare_param(param_prefix_ + "u_clamp_min", rclcpp::ParameterValue(gains.u_min_)); + declare_param( + param_prefix_ + "antiwindup", + rclcpp::ParameterValue(gains.antiwindup_strat_.legacy_antiwindup)); + declare_param( + param_prefix_ + "tracking_time_constant", + rclcpp::ParameterValue(antiwindup_strat.tracking_time_constant)); + declare_param( + param_prefix_ + "error_deadband", rclcpp::ParameterValue(antiwindup_strat.error_deadband)); + declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(true)); + declare_param( + param_prefix_ + "antiwindup_strategy", + rclcpp::ParameterValue(gains.antiwindup_strat_.to_string())); + declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); + + set_parameter_event_callback(); + return true; + } + else + { + RCLCPP_ERROR(node_logging_->get_logger(), "Failed to initialize PID controller"); + return false; } - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - pid_.initialize(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop - const Pid::Gains gains = pid_.get_gains(); - - declare_param(param_prefix_ + "p", rclcpp::ParameterValue(gains.p_gain_)); - declare_param(param_prefix_ + "i", rclcpp::ParameterValue(gains.i_gain_)); - declare_param(param_prefix_ + "d", rclcpp::ParameterValue(gains.d_gain_)); - declare_param(param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(gains.i_max_)); - declare_param(param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(gains.i_min_)); - declare_param(param_prefix_ + "u_clamp_max", rclcpp::ParameterValue(gains.u_max_)); - declare_param(param_prefix_ + "u_clamp_min", rclcpp::ParameterValue(gains.u_min_)); - declare_param(param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(gains.trk_tc_)); - declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(true)); - declare_param(param_prefix_ + "antiwindup", rclcpp::ParameterValue(gains.antiwindup_)); - declare_param( - param_prefix_ + "antiwindup_strategy", - rclcpp::ParameterValue(gains.antiwindup_strat_.to_string())); - declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); - - set_parameter_event_callback(); - } -} - -void PidROS::initialize_from_args( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat, bool save_i_term) -{ - 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, u_max, u_min, trk_tc, antiwindup_strat); - const Pid::Gains gains = pid_.get_gains(); - - declare_param(param_prefix_ + "p", rclcpp::ParameterValue(gains.p_gain_)); - declare_param(param_prefix_ + "i", rclcpp::ParameterValue(gains.i_gain_)); - declare_param(param_prefix_ + "d", rclcpp::ParameterValue(gains.d_gain_)); - declare_param(param_prefix_ + "u_clamp_max", rclcpp::ParameterValue(gains.u_max_)); - declare_param(param_prefix_ + "u_clamp_min", rclcpp::ParameterValue(gains.u_min_)); - declare_param(param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(gains.trk_tc_)); - declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(true)); - declare_param( - param_prefix_ + "antiwindup_strategy", - rclcpp::ParameterValue(gains.antiwindup_strat_.to_string())); - declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); - - set_parameter_event_callback(); } + return false; } void PidROS::reset() @@ -393,98 +395,60 @@ double PidROS::compute_command(double error, double error_dot, const rclcpp::Dur 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) +bool PidROS::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - set_gains( - p, i, d, i_max, i_min, UMAX_INFINITY, -UMAX_INFINITY, 0.0, antiwindup, - AntiwindupStrategy::NONE); -#pragma GCC diagnostic pop + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.legacy_antiwindup = antiwindup; + return set_gains(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, 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 antiwindup, AntiwindupStrategy antiwindup_strat) +bool PidROS::set_gains( + double p, double i, double d, double u_max, double u_min, + const AntiWindupStrategy & 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 (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; - } - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - pid_.set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); -#pragma GCC diagnostic pop - const Pid::Gains gains = pid_.get_gains(); - - node_params_->set_parameters( - {rclcpp::Parameter(param_prefix_ + "p", gains.p_gain_), - rclcpp::Parameter(param_prefix_ + "i", gains.i_gain_), - rclcpp::Parameter(param_prefix_ + "d", gains.d_gain_), - rclcpp::Parameter(param_prefix_ + "i_clamp_max", gains.i_max_), - rclcpp::Parameter(param_prefix_ + "i_clamp_min", gains.i_min_), - rclcpp::Parameter(param_prefix_ + "u_clamp_max", gains.u_max_), - rclcpp::Parameter(param_prefix_ + "u_clamp_min", gains.u_min_), - rclcpp::Parameter(param_prefix_ + "tracking_time_constant", gains.trk_tc_), - rclcpp::Parameter(param_prefix_ + "saturation", true), - rclcpp::Parameter(param_prefix_ + "antiwindup", gains.antiwindup_), - rclcpp::Parameter( - param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string())}); - } -} + Pid::Gains gains(p, i, d, u_max, u_min, antiwindup_strat); -void PidROS::set_gains( - double p, double i, double d, double u_max, double u_min, double trk_tc, - AntiwindupStrategy antiwindup_strat) -{ - if (u_min > u_max) - { - RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); - } - else - { - pid_.set_gains(p, i, d, u_max, u_min, trk_tc, antiwindup_strat); - const Pid::Gains gains = pid_.get_gains(); - node_params_->set_parameters( - {rclcpp::Parameter(param_prefix_ + "p", gains.p_gain_), - rclcpp::Parameter(param_prefix_ + "i", gains.i_gain_), - rclcpp::Parameter(param_prefix_ + "d", gains.d_gain_), - rclcpp::Parameter(param_prefix_ + "u_clamp_max", gains.u_max_), - rclcpp::Parameter(param_prefix_ + "u_clamp_min", gains.u_min_), - rclcpp::Parameter(param_prefix_ + "tracking_time_constant", gains.trk_tc_), - rclcpp::Parameter(param_prefix_ + "saturation", true), - rclcpp::Parameter( - param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string())}); - } + return set_gains(gains); } -void PidROS::set_gains(const Pid::Gains & gains) +bool 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_) + std::string error_msg = ""; + if (!gains.validate(error_msg)) { - RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + RCLCPP_ERROR( + node_logging_->get_logger(), "Received invalid gains: %s. Skipping new gains.", + error_msg.c_str()); + return false; } else { - pid_.set_gains(gains); + if (pid_.set_gains(gains)) + { + node_params_->set_parameters( + {rclcpp::Parameter(param_prefix_ + "p", gains.p_gain_), + rclcpp::Parameter(param_prefix_ + "i", gains.i_gain_), + rclcpp::Parameter(param_prefix_ + "d", gains.d_gain_), + rclcpp::Parameter(param_prefix_ + "i_clamp_max", gains.antiwindup_strat_.i_max), + rclcpp::Parameter(param_prefix_ + "i_clamp_min", gains.antiwindup_strat_.i_min), + rclcpp::Parameter(param_prefix_ + "u_clamp_max", gains.u_max_), + rclcpp::Parameter(param_prefix_ + "u_clamp_min", gains.u_min_), + rclcpp::Parameter( + param_prefix_ + "tracking_time_constant", + gains.antiwindup_strat_.tracking_time_constant), + rclcpp::Parameter(param_prefix_ + "antiwindup", gains.antiwindup_strat_.legacy_antiwindup), + rclcpp::Parameter( + param_prefix_ + "error_deadband", gains.antiwindup_strat_.error_deadband), + rclcpp::Parameter(param_prefix_ + "saturation", true), + rclcpp::Parameter( + param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string())}); + return true; + } } + return false; } void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt) @@ -530,24 +494,24 @@ 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" - << " Antiwindup: " << gains.antiwindup_ << "\n" - << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() - << "\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.antiwindup_strat_.tracking_time_constant << "\n" + << " Antiwindup: " << gains.antiwindup_strat_.legacy_antiwindup << "\n" + << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\n" + << "\n" + << " P Error: " << p_error << "\n" + << " I Term: " << i_term << "\n" + << " D Error: " << d_error << "\n" + << " Command: " << get_current_cmd();); } void PidROS::set_parameter_event_callback() @@ -656,17 +620,17 @@ void PidROS::set_parameter_event_callback() } else if (param_name == param_prefix_ + "tracking_time_constant") { - gains.trk_tc_ = parameter.get_value(); + gains.antiwindup_strat_.tracking_time_constant = parameter.get_value(); changed = true; } else if (param_name == param_prefix_ + "antiwindup") { - gains.antiwindup_ = parameter.get_value(); + gains.antiwindup_strat_.legacy_antiwindup = parameter.get_value(); changed = true; } - else if (param_name == param_prefix_ + "antiwindup_strategy") + else if (param_name == param_prefix_ + "error_deadband") { - gains.antiwindup_strat_ = AntiwindupStrategy(parameter.get_value()); + gains.antiwindup_strat_.error_deadband = parameter.get_value(); changed = true; } } @@ -678,18 +642,7 @@ void PidROS::set_parameter_event_callback() if (changed) { - 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); - } + pid_.set_gains(gains); } return result; diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 2ab043be..0712d6f0 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -24,7 +24,7 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" -using control_toolbox::AntiwindupStrategy; +using control_toolbox::AntiWindupStrategy; using rclcpp::executors::MultiThreadedExecutor; class TestablePidROS : public control_toolbox::PidROS @@ -63,11 +63,15 @@ void check_set_parameters( const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; 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, ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM)); + ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, SAVE_I_TERM)); rclcpp::Parameter param; @@ -117,9 +121,9 @@ void check_set_parameters( 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_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_TRUE(gains.antiwindup_); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); + ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } TEST(PidParametersTest, InitPidTest) @@ -149,9 +153,14 @@ 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, AntiwindupStrategy::NONE, - false)); + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX_BAD; + ANTIWINDUP_STRAT.i_min = I_MIN_BAD; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = false; + + ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); rclcpp::Parameter param; @@ -177,9 +186,9 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.i_min_, 0.0); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); - ASSERT_EQ(gains.trk_tc_, 0.0); + ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, 0.0); ASSERT_FALSE(gains.antiwindup_); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); + ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot) @@ -264,11 +273,15 @@ TEST(PidParametersTest, SetParametersTest) const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; const bool SAVE_I_TERM = false; - pid.initialize_from_args( - P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM); + pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, SAVE_I_TERM); rcl_interfaces::msg::SetParametersResult set_result; @@ -316,9 +329,9 @@ TEST(PidParametersTest, SetParametersTest) 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_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); + ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } TEST(PidParametersTest, SetBadParametersTest) @@ -341,10 +354,15 @@ TEST(PidParametersTest, SetBadParametersTest) const double TRK_TC = 4.0; const bool SATURATION = false; const bool ANTIWINDUP = true; - const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; - pid.initialize_from_args( - P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, ANTIWINDUP, ANTIWINDUP_STRAT, false); + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; + + pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false); rcl_interfaces::msg::SetParametersResult set_result; @@ -391,9 +409,9 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); - ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); + ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); // Set the good gains @@ -414,9 +432,9 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); - ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); + ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); // Now re-enabling it should have the old gains back ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", true))); @@ -434,9 +452,9 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(updated_gains.i_min_, I_MIN); ASSERT_EQ(updated_gains.u_max_, U_MAX); ASSERT_EQ(updated_gains.u_min_, U_MIN); - ASSERT_EQ(updated_gains.trk_tc_, TRK_TC); + ASSERT_EQ(updated_gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(updated_gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(updated_gains.antiwindup_strat_, AntiwindupStrategy::NONE); + ASSERT_EQ(updated_gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } TEST(PidParametersTest, GetParametersTest) @@ -456,12 +474,19 @@ TEST(PidParametersTest, GetParametersTest) const double TRK_TC = 4.0; const bool SATURATION = true; const bool ANTIWINDUP = true; - 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, AntiwindupStrategy::NONE, false); + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; + + ASSERT_FALSE(pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false)) + << "Zero u_min and u_max are not valid so initialization should fail"; + ASSERT_TRUE(pid.initialize_from_args(0, 0, 0, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); std::cout << "Setting gains with set_gains()" << std::endl; - pid.set_gains(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, ANTIWINDUP, ANTIWINDUP_STRAT); + pid.set_gains(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT); rclcpp::Parameter param; @@ -515,11 +540,18 @@ TEST(PidParametersTest, GetParametersTest) const double U_MIN = -std::numeric_limits::infinity(); const double TRK_TC = 4.0; const bool ANTIWINDUP = true; - 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, AntiwindupStrategy::NONE, false); - pid.set_gains(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, ANTIWINDUP, ANTIWINDUP_STRAT); + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; + + ASSERT_FALSE(pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false)) + << "Zero u_min and u_max are not valid so initialization should fail"; + ASSERT_TRUE(pid.initialize_from_args(0, 0, 0, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); + pid.set_gains(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT); rclcpp::Parameter param; @@ -567,7 +599,7 @@ TEST(PidParametersTest, GetParametersFromParams) TestablePidROS pid(node); - ASSERT_TRUE(pid.initialize_from_ros_parameters()); + ASSERT_FALSE(pid.initialize_from_ros_parameters()); rclcpp::Parameter param_p; ASSERT_TRUE(node->get_parameter("p", param_p)); @@ -617,11 +649,16 @@ TEST(PidParametersTest, MultiplePidInstances) 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, 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, AntiwindupStrategy::NONE, false)); + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = false; + + ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); + ANTIWINDUP_STRAT.legacy_antiwindup = true; + ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, 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 9d776800..2752d5b3 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -30,7 +30,7 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" -using control_toolbox::AntiwindupStrategy; +using control_toolbox::AntiWindupStrategy; using PidStateMsg = control_msgs::msg::PidState; using rclcpp::executors::MultiThreadedExecutor; @@ -43,8 +43,13 @@ 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, AntiwindupStrategy::NONE, false); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = 5.0; + antiwindup_strat.i_min = -5.0; + antiwindup_strat.legacy_antiwindup = false; + antiwindup_strat.tracking_time_constant = 1.0; + pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -81,8 +86,13 @@ 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, AntiwindupStrategy::NONE, false); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = 5.0; + antiwindup_strat.i_min = -5.0; + antiwindup_strat.legacy_antiwindup = false; + antiwindup_strat.tracking_time_constant = 1.0; + pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, 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 128cf97c..868674d6 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -41,7 +41,7 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" -using control_toolbox::AntiwindupStrategy; +using control_toolbox::AntiWindupStrategy; using control_toolbox::Pid; using namespace std::chrono_literals; @@ -51,11 +51,13 @@ TEST(ParameterTest, UTermBadIBoundsTestConstructor) "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 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, AntiwindupStrategy::NONE), - std::invalid_argument); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiWindupStrategy antiwindup_strat); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = 1.0; + antiwindup_strat.i_min = -1.0; + EXPECT_THROW(Pid pid(1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat), std::invalid_argument); } TEST(ParameterTest, UTermBadIBoundsTest) @@ -64,15 +66,18 @@ TEST(ParameterTest, UTermBadIBoundsTest) "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 antiwindup, AntiwindupStrategy antiwindup_strat); - Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 0.0, false, AntiwindupStrategy::NONE); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiWindupStrategy antiwindup_strat); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = 1.0; + antiwindup_strat.i_min = -1.0; + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat); 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, AntiwindupStrategy::NONE)); + EXPECT_NO_THROW(pid.set_gains(1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat)); // 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); @@ -83,10 +88,13 @@ 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 antiwindup, AntiwindupStrategy antiwindup_strat); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiWindupStrategy antiwindup_strat); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; + antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value // Setting u_max = 1.0 and u_min = -1.0 to test clamping - Pid pid(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, false, AntiwindupStrategy::BACK_CALCULATION); + Pid pid(1.0, 0.0, 0.0, 1.0, -1.0, antiwindup_strat); double cmd = 0.0; @@ -136,12 +144,15 @@ 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 antiwindup, AntiwindupStrategy antiwindup_strat); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiWindupStrategy antiwindup_strat); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; + antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value // Setting u_max = INF and u_min = -INF to disable clamping Pid pid( - 1.0, 0.0, 0.0, 0.0, 0.0, std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), 0.0, false, AntiwindupStrategy::BACK_CALCULATION); + 1.0, 0.0, 0.0, std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), antiwindup_strat); double cmd = 0.0; @@ -193,9 +204,12 @@ TEST(ParameterTest, integrationBackCalculationZeroGainTest) "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 antiwindup, AntiwindupStrategy antiwindup_strat); - Pid pid(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, AntiwindupStrategy::BACK_CALCULATION); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiWindupStrategy antiwindup_strat); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; + antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value + Pid pid(0.0, 0.0, 0.0, 20.0, -20.0, antiwindup_strat); double cmd = 0.0; double pe, ie, de; @@ -240,10 +254,11 @@ TEST(ParameterTest, integrationConditionalIntegrationZeroGainTest) "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 antiwindup, AntiwindupStrategy antiwindup_strat); - Pid pid( - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, AntiwindupStrategy::CONDITIONAL_INTEGRATION); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiWindupStrategy antiwindup_strat); + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; + Pid pid(0.0, 0.0, 0.0, 20.0, -20.0, antiwindup_strat); double cmd = 0.0; double pe, ie, de; @@ -301,13 +316,13 @@ TEST(ParameterTest, ITermBadIBoundsTest) Pid pid(1.0, 1.0, 1.0, 1.0, -1.0); auto gains = pid.get_gains(); - EXPECT_DOUBLE_EQ(gains.i_min_, -1.0); - EXPECT_DOUBLE_EQ(gains.i_max_, 1.0); + EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_min, -1.0); + EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_max, 1.0); // Try to set bad i-bounds, i.e. i_min > i_max EXPECT_NO_THROW(pid.set_gains(1.0, 1.0, 1.0, -2.0, 2.0)); // Check if gains were not updated because i-bounds are bad, i.e. i_min > i_max - EXPECT_DOUBLE_EQ(gains.i_min_, -1.0); - EXPECT_DOUBLE_EQ(gains.i_max_, 1.0); + EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_min, -1.0); + EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_max, 1.0); } TEST(ParameterTest, integrationClampTest) @@ -426,34 +441,36 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double i_min = -1 * std::rand() % 100; double u_max = std::numeric_limits::infinity(); double u_min = -1 * u_max; - double trk_tc = std::rand() % 100; + double tracking_time_constant = std::rand() % 100; bool antiwindup = false; - AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE; + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.tracking_time_constant = tracking_time_constant; + antiwindup_strat.legacy_antiwindup = antiwindup; // Initialize the default way - Pid pid1( - p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); + Pid pid1(p_gain, i_gain, d_gain, u_max, u_min, 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; - bool antiwindup_return; - AntiwindupStrategy antiwindup_strat_return; + double p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_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, - u_min_return, trk_tc_return, antiwindup_return, antiwindup_strat_return); + p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_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(antiwindup, antiwindup_return); - EXPECT_EQ(antiwindup_strat, antiwindup_strat_return); + EXPECT_EQ(tracking_time_constant, antiwindup_strat_return.tracking_time_constant); + EXPECT_EQ(i_min, antiwindup_strat_return.i_min); + EXPECT_EQ(i_max, antiwindup_strat_return.i_max); + EXPECT_EQ(antiwindup, antiwindup_strat_return.legacy_antiwindup); + EXPECT_EQ(antiwindup_strat.to_string(), antiwindup_strat_return.to_string()); // Test return values using struct ------------------------------------------------- @@ -465,12 +482,15 @@ TEST(ParameterTest, gainSettingCopyPIDTest) i_min = -1 * std::rand() % 100; u_max = std::numeric_limits::infinity(); u_min = -1 * u_max; - trk_tc = std::rand() % 100; + tracking_time_constant = std::rand() % 100; antiwindup = false; - antiwindup_strat = AntiwindupStrategy::NONE; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = i_max; + antiwindup_strat.i_min = i_min; + antiwindup_strat.tracking_time_constant = tracking_time_constant; + antiwindup_strat.legacy_antiwindup = antiwindup; - pid1.set_gains( - p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, antiwindup, antiwindup_strat); + pid1.set_gains(p_gain, i_gain, d_gain, u_max, u_min, antiwindup_strat); Pid::Gains g1 = pid1.get_gains(); EXPECT_EQ(p_gain, g1.p_gain_); @@ -480,9 +500,12 @@ TEST(ParameterTest, gainSettingCopyPIDTest) 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(antiwindup, g1.antiwindup_); - EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); + EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant); + EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max); + EXPECT_EQ(i_min, g1.antiwindup_strat_.i_min); + EXPECT_EQ(antiwindup, g1.antiwindup_strat_.legacy_antiwindup); + EXPECT_EQ(antiwindup_strat.to_string(), g1.antiwindup_strat_.to_string()); // Send update command to populate errors ------------------------------------------------- pid1.set_current_cmd(10); @@ -492,19 +515,21 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid2(pid1); pid2.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, - u_min_return, trk_tc_return, antiwindup_return, antiwindup_strat_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_); + p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, + antiwindup_strat_return); + + EXPECT_EQ(p_gain_return, g1.p_gain_); + EXPECT_EQ(i_gain_return, g1.i_gain_); + EXPECT_EQ(d_gain_return, g1.d_gain_); + EXPECT_EQ(antiwindup_strat_return.i_max, g1.i_max_); + EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); - EXPECT_EQ(trk_tc, g1.trk_tc_); - EXPECT_EQ(antiwindup, g1.antiwindup_); - EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); + EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant); + EXPECT_EQ(antiwindup_strat_return.i_max, g1.antiwindup_strat_.i_max); + EXPECT_EQ(antiwindup_strat_return.i_min, g1.antiwindup_strat_.i_min); + EXPECT_EQ(antiwindup, g1.antiwindup_strat_.legacy_antiwindup); + EXPECT_EQ(antiwindup_strat.to_string(), g1.antiwindup_strat_.to_string()); // Test that errors are zero double pe2, ie2, de2; @@ -518,19 +543,21 @@ TEST(ParameterTest, gainSettingCopyPIDTest) pid3 = pid1; pid3.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, - u_min_return, trk_tc_return, antiwindup_return, antiwindup_strat_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_); + p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, + antiwindup_strat_return); + + EXPECT_EQ(p_gain_return, g1.p_gain_); + EXPECT_EQ(i_gain_return, g1.i_gain_); + EXPECT_EQ(d_gain_return, g1.d_gain_); + EXPECT_EQ(antiwindup_strat_return.i_max, g1.i_max_); + EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_); EXPECT_EQ(u_max, g1.u_max_); EXPECT_EQ(u_min, g1.u_min_); - EXPECT_EQ(trk_tc, g1.trk_tc_); - EXPECT_EQ(antiwindup, g1.antiwindup_); - EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); + EXPECT_EQ(tracking_time_constant, g1.antiwindup_strat_.tracking_time_constant); + EXPECT_EQ(antiwindup_strat_return.i_max, g1.antiwindup_strat_.i_max); + EXPECT_EQ(antiwindup_strat_return.i_min, g1.antiwindup_strat_.i_min); + EXPECT_EQ(antiwindup, g1.antiwindup_strat_.legacy_antiwindup); + EXPECT_EQ(antiwindup_strat.to_string(), g1.antiwindup_strat_.to_string()); // Test that errors are zero double pe3, ie3, de3; @@ -721,9 +748,13 @@ TEST(CommandTest, backCalculationPIDTest) "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 antiwindup, AntiwindupStrategy antiwindup_strat); - Pid pid(0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, false, AntiwindupStrategy::BACK_CALCULATION); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiWindupStrategy antiwindup_strat); + // Setting u_max = 5.0 and u_min = -5.0 to test clamping + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; + antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value + Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); double cmd = 0.0; double pe, ie, de; @@ -778,10 +809,13 @@ TEST(CommandTest, conditionalIntegrationPIDTest) "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 antiwindup, AntiwindupStrategy antiwindup_strat); - Pid pid( - 0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, false, AntiwindupStrategy::CONDITIONAL_INTEGRATION); + // Pid(double p, double i, double d, double u_max, double u_min, + // AntiWindupStrategy antiwindup_strat); + // Setting u_max = 5.0 and u_min = -5.0 to test clamping + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; + antiwindup_strat.tracking_time_constant = 1.0; + Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); double cmd = 0.0; double pe, ie, de;