diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 1e7bf7ef..fed0369e 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -34,12 +34,75 @@ #define CONTROL_TOOLBOX__PID_HPP_ #include +#include +#include #include "rclcpp/duration.hpp" #include "realtime_tools/realtime_buffer.hpp" namespace control_toolbox { +class AntiwindupStrategy +{ +public: + enum Value : int8_t + { + NONE = 0, + BACK_CALCULATION, + CONDITIONAL_INTEGRATION + }; + + constexpr AntiwindupStrategy() : value_(NONE) {} + constexpr AntiwindupStrategy(Value v) : value_(v) {} // NOLINT(runtime/explicit) + + explicit AntiwindupStrategy(const std::string & s) + { + if (s == "back_calculation") + { + value_ = BACK_CALCULATION; + } + else if (s == "conditional_integration") + { + value_ = CONDITIONAL_INTEGRATION; + } + else + { + value_ = NONE; + } + } + + operator std::string() const { return to_string(); } + + constexpr bool operator==(AntiwindupStrategy other) const { return value_ == other.value_; } + constexpr bool operator!=(AntiwindupStrategy other) const { return value_ != other.value_; } + + constexpr bool operator==(Value other) const { return value_ == other; } + constexpr bool operator!=(Value other) const { return value_ != other; } + + std::string to_string() const + { + switch (value_) + { + case BACK_CALCULATION: + return "back_calculation"; + case CONDITIONAL_INTEGRATION: + return "conditional_integration"; + case NONE: + default: + return "none"; + } + } + +private: + Value value_; +}; + +template +inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) +{ + return std::abs(value) <= tolerance; +} + /***************************************************/ /*! \class Pid \brief A basic pid class. @@ -82,7 +145,12 @@ namespace control_toolbox \param i Integral gain - \param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$ + \param i_clamp Minimum and maximum bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$ + + \param u_clamp Minimum and maximum bounds for the controller output. The clamp is applied to the \f$command\f$. + + \param trk_tc Tracking time constant for the 'back_calculation' strategy. + \section Usage @@ -115,7 +183,7 @@ class Pid struct Gains { /*! - * \brief Optional constructor for passing in values without antiwindup + * \brief Optional constructor for passing in values without antiwindup and saturation * * \param p The proportional gain. * \param i The integral gain. @@ -124,65 +192,231 @@ class Pid * \param i_min Lower integral clamp. * */ + [[deprecated("Use constructor with AntiwindupStrategy instead.")]] Gains(double p, double i, double d, double i_max, double i_min) - : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(true) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(0.0), + u_min_(0.0), + trk_tc_(0.0), + saturation_(false), + antiwindup_(true), + antiwindup_strat_(AntiwindupStrategy::NONE) { } /*! - * \brief Optional constructor for passing in values + * \brief Optional constructor for passing in values without saturation * * \param p The proportional gain. * \param i The integral gain. * \param d The derivative gain. * \param i_max Upper integral clamp. * \param i_min Lower integral clamp. - * \param antiwindup Antiwindup functionality. When set to true, limits + * \param antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. * */ + [[deprecated("Use constructor with AntiwindupStrategy instead.")]] Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) - : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(0.0), + u_min_(0.0), + trk_tc_(0.0), + saturation_(false), + antiwindup_(antiwindup), + antiwindup_strat_(AntiwindupStrategy::NONE) + { + } + + /*! + * \brief Optional constructor for passing in values + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set + * to 0.0 when this strategy is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other + than 'none' is selected, it will override the controller's default anti-windup behavior. + * + */ + [[deprecated("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 saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(u_max), + u_min_(u_min), + trk_tc_(trk_tc), + saturation_(saturation), + antiwindup_(antiwindup), + antiwindup_strat_(antiwindup_strat) + { + } + + /*! + * \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 saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * + */ + Gains( + double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation, + AntiwindupStrategy antiwindup_strat) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(0.0), + i_min_(0.0), + u_max_(u_max), + u_min_(u_min), + trk_tc_(trk_tc), + saturation_(saturation), + antiwindup_(false), + antiwindup_strat_(antiwindup_strat) { } // Default constructor - Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0), antiwindup_(false) + Gains() + : p_gain_(0.0), + i_gain_(0.0), + d_gain_(0.0), + i_max_(0.0), + i_min_(0.0), + u_max_(0.0), + u_min_(0.0), + trk_tc_(0.0), + saturation_(false), + antiwindup_(false), + antiwindup_strat_(AntiwindupStrategy::NONE) { } - double p_gain_; /**< Proportional gain. */ - double i_gain_; /**< Integral gain. */ - double d_gain_; /**< Derivative gain. */ - double i_max_; /**< Maximum allowable integral term. */ - double i_min_; /**< Minimum allowable integral term. */ - bool antiwindup_; /**< Antiwindup. */ + double p_gain_; /**< Proportional gain. */ + double i_gain_; /**< Integral gain. */ + double d_gain_; /**< Derivative gain. */ + double i_max_; /**< Maximum allowable integral term. */ + double i_min_; /**< Minimum allowable integral term. */ + double u_max_; /**< Maximum allowable output. */ + double u_min_; /**< Minimum allowable output. */ + double trk_tc_; /**< Tracking time constant. */ + bool saturation_; /**< Saturation. */ + bool antiwindup_; /**< Anti-windup. */ + AntiwindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ }; /*! * \brief Constructor, zeros out Pid values when created and * initialize Pid-gains and integral term limits. - * Does not initialize dynamic reconfigure for PID gains * * \param p The proportional gain. * \param i The integral gain. * \param d The derivative gain. * \param i_max Upper integral clamp. * \param i_min Lower integral clamp. - * \param antiwindup Antiwindup functionality. When set to true, limits + * \param antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. * * \throws An std::invalid_argument exception is thrown if i_min > i_max */ + [[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 saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other + than 'none' is selected, it will override the controller's default anti-windup behavior. + * + * \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 saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + + /*! + * \brief Constructor, initialize Pid-gains and term limits. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set + * to 0.0 when this strategy is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * + * \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, bool saturation, + AntiwindupStrategy antiwindup_strat); + + /*! * \brief Copy constructor required for preventing mutexes from being copied * \param source - Pid to copy */ @@ -194,24 +428,76 @@ class Pid ~Pid(); /*! - * \brief Zeros out Pid values and initialize Pid-gains and integral term limits - * Does not initialize the node's parameter interface for PID gains + * \brief Zeros out Pid values and initialize Pid-gains and term limits * * \param p The proportional gain. * \param i The integral gain. * \param d The derivative gain. * \param i_max Upper integral clamp. * \param i_min Lower integral clamp. - * \param antiwindup Antiwindup functionality. When set to true, limits + * \param antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. * * \note New gains are not applied if i_min_ > i_max_ */ + [[deprecated("Use initialize with AntiwindupStrategy instead.")]] void 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 saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other + than 'none' is selected, it will override the controller's default anti-windup behavior. + * + * \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 saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + + /*! + * \brief Initialize Pid-gains and term limits. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set + * to 0.0 when this strategy is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * + * \note New gains are not applied if 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, bool saturation, + AntiwindupStrategy antiwindup_strat); + /*! * \brief Reset the state of this PID controller * @note The integral term is not retained and it is reset to zero @@ -247,14 +533,62 @@ class Pid * \param d The derivative gain. * \param i_max Upper integral clamp. * \param i_min Lower integral clamp. - * \param antiwindup Antiwindup functionality. When set to true, limits + * \param antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. */ + [[deprecated("Use get_gains overload without bool antiwindup.")]] 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 saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other + than 'none' is selected, it will override the controller's default anti-windup behavior. + */ + [[deprecated("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 & saturation, bool & antiwindup, + AntiwindupStrategy & antiwindup_strat); + + /*! + * \brief Get PID gains for the controller (preferred). + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set + * to 0.0 when this strategy is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + */ + void get_gains( + double & p, double & i, double & d, double & u_max, double & u_min, double & trk_tc, + bool & saturation, AntiwindupStrategy & antiwindup_strat); + /*! * \brief Get PID gains for the controller. * \return gains A struct of the PID gain values @@ -268,15 +602,68 @@ class Pid * \param d The derivative gain. * \param i_max Upper integral clamp. * \param i_min Lower integral clamp. - * \param antiwindup Antiwindup functionality. When set to true, limits + * \param antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. * * \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 saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other + than 'none' is selected, it will override the controller's default anti-windup behavior. + * + * \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 saturation = false, bool antiwindup = false, + AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE); + + /*! + * \brief Set PID gains for the controller. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set + * to 0.0 when this strategy is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * + * \note New gains are not applied if 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, bool saturation, + AntiwindupStrategy antiwindup_strat); + /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values @@ -430,11 +817,12 @@ class Pid // blocking the realtime update loop realtime_tools::RealtimeBuffer gains_buffer_; - double p_error_last_; /** Save state for derivative state calculation. */ - double p_error_; /** Error. */ - double d_error_; /** Derivative of error. */ - double i_term_; /** Integrator state. */ - double cmd_; /** Command to send. */ + double p_error_last_ = 0; /** Save state for derivative state calculation. */ + double p_error_ = 0; /** Error. */ + double d_error_ = 0; /** Derivative of error. */ + double i_term_ = 0; /** Integrator state. */ + double cmd_ = 0; /** Command to send. */ + double cmd_unsat_ = 0; /** command without saturation. */ }; } // namespace control_toolbox diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index c9996e13..1a9fac86 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -92,13 +92,14 @@ class PidROS * \param d The derivative gain. * \param i_max Upper integral clamp. * \param i_min Lower integral clamp. - * \param antiwindup Antiwindup functionality. When set to true, limits + * \param antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. * * \note New gains are not applied if i_min_ > i_max_ */ + [[deprecated("Use initialize_from_args with AntiwindupStrategy instead.")]] void initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup); @@ -109,17 +110,80 @@ class PidROS * \param d The derivative gain. * \param i_max The max integral windup. * \param i_min The min integral windup. - * \param antiwindup antiwindup. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. * \param save_i_term save integrator output between resets. * * \note New gains are not applied if i_min_ > i_max_ */ + [[deprecated("Use initialize_from_args with AntiwindupStrategy instead.")]] void initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term); + /*! + * \brief Initialize the PID controller and set the parameters + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max The max integral windup. + * \param i_min The min integral windup. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set + * to 0.0 when this strategy is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other + than 'none' is selected, it will override the controller's default anti-windup behavior. + * \deprecated{only when `antiwindup_strat == AntiwindupStrategy::NONE`:} + * Old anti-windup technique is deprecated and will be removed by + * the ROS 2 Kilted Kaiju release. + * \warning{If you pass `AntiwindupStrategy::NONE`, at runtime a warning will be printed:} + * `"Old anti-windup technique is deprecated. This option will be removed by the ROS 2 Kilted Kaiju release."` + * \param save_i_term save integrator output between resets. + * + * \note New gains are not applied if i_min_ > i_max_ or if u_min_ > u_max_. + */ + [[deprecated("Use initialize_from_args with AntiwindupStrategy only.")]] + void initialize_from_args( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat, + bool save_i_term); + + /*! + * \brief Initialize the PID controller and set the parameters. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set + * to 0.0 when this strategy is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * \param save_i_term save integrator output between resets. + * + * \note New gains are not applied if u_min_ > u_max_. + */ + void initialize_from_args( + double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation, + AntiwindupStrategy antiwindup_strat, bool save_i_term); + /*! * \brief Initialize the PID controller based on already set parameters - * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise + * \return True if all parameters are set (p, i, d, i_max, i_min, u_max, u_min and trk_tc), False otherwise */ bool initialize_from_ros_parameters(); @@ -182,8 +246,67 @@ class PidROS * * \note New gains are not applied if i_min > i_max */ + [[deprecated("Use set_gains with AntiwindupStrategy instead.")]] void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + /*! + * \brief Initialize the PID controller and set the parameters + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max The max integral windup. + * \param i_min The min integral windup. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set + * to 0.0 when this strategy is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other + than 'none' is selected, it will override the controller's default anti-windup behavior. + * \deprecated{only when `antiwindup_strat == AntiwindupStrategy::NONE`:} + * Old anti-windup technique is deprecated and will be removed by + * the ROS 2 Kilted Kaiju release. + * \warning{If you pass `AntiwindupStrategy::NONE`, at runtime a warning will be printed:} + * `"Old anti-windup technique is deprecated. This option will be removed by + * the ROS 2 Kilted Kaiju release."` + * + * \note New gains are not applied if i_min > i_max or if u_min_ > u_max_. + */ + [[deprecated("Use set_gains with AntiwindupStrategy only.")]] + void set_gains( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc = 0.0, bool saturation = false, bool antiwindup = false, + AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE); + + /*! + * \brief Set PID gains for the controller (preferred). + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' strategy. If set + * to 0.0 when this strategy is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * + * \note New gains are not applied if u_min_ > u_max_. + */ + void set_gains( + double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation, + AntiwindupStrategy antiwindup_strat); + /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values @@ -248,6 +371,8 @@ class PidROS bool get_boolean_param(const std::string & param_name, bool & value); + bool get_string_param(const std::string & param_name, std::string & value); + /*! * \brief Set prefix for topic and parameter names * \param[in] topic_prefix prefix to add to the pid parameters. diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 54621b15..0760eef2 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -39,22 +39,54 @@ #include #include #include -#include #include -#include #include "control_toolbox/pid.hpp" namespace control_toolbox { +#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, 0.0, 0.0, 0.0, false, 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 saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) : gains_buffer_() { if (i_min > i_max) { throw std::invalid_argument("received i_min > i_max"); } - set_gains(p, i, d, i_max, i_min, antiwindup); + 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, saturation, antiwindup, antiwindup_strat); +#pragma GCC diagnostic pop + + // Initialize saved i-term values + clear_saved_iterm(); + + reset(); +} + +Pid::Pid( + double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation, + 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, saturation, antiwindup_strat); // Initialize saved i-term values clear_saved_iterm(); @@ -78,7 +110,31 @@ Pid::~Pid() {} void Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - set_gains(p, i, d, i_max, i_min, antiwindup); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + initialize(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, 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 saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) +{ +#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, saturation, antiwindup, antiwindup_strat); +#pragma GCC diagnostic pop + + reset(); +} + +void Pid::initialize( + double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation, + AntiwindupStrategy antiwindup_strat) +{ + set_gains(p, i, d, u_max, u_min, trk_tc, saturation, antiwindup_strat); reset(); } @@ -103,12 +159,36 @@ void Pid::clear_saved_iterm() { i_term_ = 0.0; } void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & i_min) { + double u_max; + double u_min; + double trk_tc; + bool saturation; bool antiwindup; - get_gains(p, i, d, i_max, i_min, antiwindup); + 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, saturation, antiwindup, antiwindup_strat); +#pragma GCC diagnostic pop } void Pid::get_gains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup) +{ + double u_max; + double u_min; + double trk_tc; + bool saturation; + 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, saturation, antiwindup, antiwindup_strat); +#pragma GCC diagnostic pop +} + +void Pid::get_gains( + double & p, double & i, double & d, double & i_max, double & i_min, double & u_max, + double & u_min, double & trk_tc, bool & saturation, bool & antiwindup, + AntiwindupStrategy & antiwindup_strat) { Gains gains = *gains_buffer_.readFromRT(); @@ -117,33 +197,104 @@ void Pid::get_gains( d = gains.d_gain_; i_max = gains.i_max_; i_min = gains.i_min_; + u_max = gains.u_max_; + u_min = gains.u_min_; + trk_tc = gains.trk_tc_; + saturation = gains.saturation_; antiwindup = gains.antiwindup_; + antiwindup_strat = gains.antiwindup_strat_; +} + +void Pid::get_gains( + double & p, double & i, double & d, double & u_max, double & u_min, double & trk_tc, + bool & saturation, 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_; + saturation = gains.saturation_; + antiwindup_strat = gains.antiwindup_strat_; } 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) { - Gains gains(p, i, d, i_max, i_min, antiwindup); + set_gains(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, 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 saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) +{ + Gains gains( + p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); + + set_gains(gains); +} +#pragma GCC diagnostic pop + +void Pid::set_gains( + double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation, + 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, saturation, antiwindup, antiwindup_strat); +#pragma GCC diagnostic pop set_gains(gains); } -void Pid::set_gains(const Gains & gains) +void Pid::set_gains(const Gains & gains_in) { - if (gains.i_min_ > gains.i_max_) + if (gains_in.i_min_ > gains_in.i_max_) { std::cout << "received i_min > i_max, skip new gains\n"; } + else if (gains_in.u_min_ > gains_in.u_max_) + { + std::cout << "received u_min > u_max, skip new gains\n"; + } else { + Gains gains = gains_in; + + if (gains.antiwindup_strat_ == AntiwindupStrategy::BACK_CALCULATION) + { + if (is_zero(gains.trk_tc_) && !is_zero(gains.d_gain_)) + { + // Default value for tracking time constant for back calculation technique + gains.trk_tc_ = std::sqrt(gains.d_gain_ / gains.i_gain_); + } + else if (is_zero(gains.trk_tc_) && is_zero(gains.d_gain_)) + { + // Default value for tracking time constant for back calculation technique + gains.trk_tc_ = gains.p_gain_ / gains.i_gain_; + } + } gains_buffer_.writeFromNonRT(gains); } } double Pid::compute_command(double error, const double & dt_s) { - if (std::abs(dt_s) <= std::numeric_limits::epsilon()) + if (is_zero(dt_s)) { // don't update anything return cmd_; @@ -199,9 +350,9 @@ double Pid::compute_command(double error, double error_dot, const std::chrono::n double Pid::compute_command(double error, double error_dot, const double & dt_s) { - if (std::abs(dt_s) <= std::numeric_limits::epsilon()) + if (is_zero(dt_s)) { - // don't update anything + // Don't update anything return cmd_; } else if (dt_s < 0.0) @@ -212,36 +363,65 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) Gains gains = *gains_buffer_.readFromRT(); double p_term, d_term; - p_error_ = error; // this is error = target - state - d_error_ = error_dot; + p_error_ = error; // This is error = target - state + d_error_ = error_dot; // This is the derivative of error - // don't reset controller but return NaN + // Don't reset controller but return NaN if (!std::isfinite(error) || !std::isfinite(error_dot)) { std::cout << "Received a non-finite error/error_dot value\n"; - return cmd_ = std::numeric_limits::quiet_NaN(); + return cmd_ = std::numeric_limits::quiet_NaN(); } // Calculate proportional contribution to command p_term = gains.p_gain_ * p_error_; + // Calculate derivative contribution to command + d_term = gains.d_gain_ * d_error_; + // Calculate integral contribution to command - if (gains.antiwindup_) + if (gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) { // Prevent i_term_ from climbing higher than permitted by i_max_/i_min_ i_term_ = std::clamp(i_term_ + gains.i_gain_ * dt_s * p_error_, gains.i_min_, gains.i_max_); } - else + else if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) { i_term_ += gains.i_gain_ * dt_s * p_error_; } - // Calculate derivative contribution to command - d_term = gains.d_gain_ * d_error_; - // Compute the command - // Limit i_term so that the limit is meaningful in the output - cmd_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; + if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) + { + // Limit i_term so that the limit is meaningful in the output + cmd_unsat_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; + } + else + { + cmd_unsat_ = p_term + i_term_ + d_term; + } + + if (gains.saturation_ == true) + { + // Limit cmd_ if saturation is enabled + cmd_ = std::clamp(cmd_unsat_, gains.u_min_, gains.u_max_); + } + else + { + cmd_ = cmd_unsat_; + } + + if (gains.antiwindup_strat_ == 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 (!(!iszero(cmd_unsat_ - cmd_) && error * cmd_unsat_ > 0)) + { + i_term_ += dt_s * gains.i_gain_ * error; + } + } return cmd_; } diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 63952487..257aaadf 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -30,8 +30,6 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include -#include #include #include #include @@ -42,7 +40,8 @@ namespace control_toolbox { - +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" PidROS::PidROS( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, @@ -97,6 +96,7 @@ PidROS::PidROS( rt_state_pub_.reset( new realtime_tools::RealtimePublisher(state_pub_)); } +#pragma GCC diagnostic pop void PidROS::set_prefixes(const std::string & topic_prefix) { @@ -182,19 +182,57 @@ bool PidROS::get_double_param(const std::string & param_name, double & value) } } +bool PidROS::get_string_param(const std::string & param_name, std::string & value) +{ + declare_param(param_name, rclcpp::ParameterValue(value)); + rclcpp::Parameter param; + if (node_params_->has_parameter(param_name)) + { + node_params_->get_parameter(param_name, param); + if (rclcpp::PARAMETER_STRING != param.get_type()) + { + RCLCPP_ERROR( + node_logging_->get_logger(), "Wrong parameter type '%s', not string", param_name.c_str()); + return false; + } + value = param.as_string(); + RCLCPP_DEBUG_STREAM( + node_logging_->get_logger(), "parameter '" << param_name << "' in node '" + << node_base_->get_name() << "' value is " << value + << std::endl); + return true; + } + else + { + RCLCPP_ERROR_STREAM( + node_logging_->get_logger(), "parameter '" << param_name << "' in node '" + << node_base_->get_name() << "' does not exists" + << std::endl); + return false; + } +} + bool PidROS::initialize_from_ros_parameters() { - double p, i, d, i_min, i_max; - p = i = d = i_min = i_max = std::numeric_limits::quiet_NaN(); + double p, i, d, i_max, i_min, u_max, u_min, trk_tc; + p = i = d = i_max = i_min = u_max = u_min = trk_tc = std::numeric_limits::quiet_NaN(); + bool saturation = false; bool antiwindup = false; + std::string antiwindup_strat_str = "none"; bool all_params_available = true; + all_params_available &= get_double_param(param_prefix_ + "p", p); all_params_available &= get_double_param(param_prefix_ + "i", i); all_params_available &= get_double_param(param_prefix_ + "d", d); all_params_available &= get_double_param(param_prefix_ + "i_clamp_max", i_max); all_params_available &= get_double_param(param_prefix_ + "i_clamp_min", i_min); + all_params_available &= get_double_param(param_prefix_ + "u_clamp_max", u_max); + all_params_available &= get_double_param(param_prefix_ + "u_clamp_min", u_min); + all_params_available &= get_double_param(param_prefix_ + "tracking_time_constant", trk_tc); + get_boolean_param(param_prefix_ + "saturation", saturation); get_boolean_param(param_prefix_ + "antiwindup", antiwindup); + get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat_str); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(false)); if (all_params_available) @@ -202,7 +240,20 @@ bool PidROS::initialize_from_ros_parameters() set_parameter_event_callback(); } - pid_.initialize(p, i, d, i_max, i_min, antiwindup); + if (antiwindup_strat_str == "none") + { + std::cout << "Old anti-windup technique is deprecated. " + "This option will be removed by the ROS 2 Kilted Kaiju release." + << std::endl; + } + + AntiwindupStrategy antiwindup_strat(antiwindup_strat_str); + +#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, saturation, antiwindup, antiwindup_strat); +#pragma GCC diagnostic pop return all_params_available; } @@ -218,26 +269,90 @@ void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValu void PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup) { - initialize_from_args(p, i, d, i_max, i_min, antiwindup, false); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + initialize_from_args( + p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE, false); +#pragma GCC diagnostic pop } void 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, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE, save_i_term); +#pragma GCC diagnostic pop +} + +void PidROS::initialize_from_args( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation, bool antiwindup, 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) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + } else { - pid_.initialize(p, i, d, i_max, i_min, antiwindup); + 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_.initialize( + p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); +#pragma GCC diagnostic pop declare_param(param_prefix_ + "p", rclcpp::ParameterValue(p)); declare_param(param_prefix_ + "i", rclcpp::ParameterValue(i)); declare_param(param_prefix_ + "d", rclcpp::ParameterValue(d)); declare_param(param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(i_max)); declare_param(param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(i_min)); + declare_param(param_prefix_ + "u_clamp_max", rclcpp::ParameterValue(u_max)); + declare_param(param_prefix_ + "u_clamp_min", rclcpp::ParameterValue(u_min)); + declare_param(param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(trk_tc)); + declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(saturation)); declare_param(param_prefix_ + "antiwindup", rclcpp::ParameterValue(antiwindup)); + declare_param( + param_prefix_ + "antiwindup_strategy", rclcpp::ParameterValue(antiwindup_strat.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, bool saturation, + 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, saturation, antiwindup_strat); + + declare_param(param_prefix_ + "p", rclcpp::ParameterValue(p)); + declare_param(param_prefix_ + "i", rclcpp::ParameterValue(i)); + declare_param(param_prefix_ + "d", rclcpp::ParameterValue(d)); + declare_param(param_prefix_ + "u_clamp_max", rclcpp::ParameterValue(u_max)); + declare_param(param_prefix_ + "u_clamp_min", rclcpp::ParameterValue(u_min)); + declare_param(param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(trk_tc)); + declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(saturation)); + declare_param( + param_prefix_ + "antiwindup_strategy", rclcpp::ParameterValue(antiwindup_strat.to_string())); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); set_parameter_event_callback(); @@ -275,21 +390,90 @@ 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) +{ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + set_gains(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE); +#pragma GCC diagnostic pop +} + +void PidROS::set_gains( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation, bool antiwindup, 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; + } + node_params_->set_parameters( {rclcpp::Parameter(param_prefix_ + "p", p), rclcpp::Parameter(param_prefix_ + "i", i), rclcpp::Parameter(param_prefix_ + "d", d), rclcpp::Parameter(param_prefix_ + "i_clamp_max", i_max), rclcpp::Parameter(param_prefix_ + "i_clamp_min", i_min), - rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup)}); + rclcpp::Parameter(param_prefix_ + "u_clamp_max", u_max), + rclcpp::Parameter(param_prefix_ + "u_clamp_min", u_min), + rclcpp::Parameter(param_prefix_ + "tracking_time_constant", trk_tc), + rclcpp::Parameter(param_prefix_ + "saturation", saturation), + rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup), + rclcpp::Parameter(param_prefix_ + "antiwindup_strategy", antiwindup_strat.to_string())}); + +#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, saturation, antiwindup, antiwindup_strat); +#pragma GCC diagnostic pop + } +} + +void PidROS::set_gains( + double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation, + AntiwindupStrategy antiwindup_strat) +{ + if (u_min > u_max) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + } + else + { + node_params_->set_parameters( + {rclcpp::Parameter(param_prefix_ + "p", p), rclcpp::Parameter(param_prefix_ + "i", i), + rclcpp::Parameter(param_prefix_ + "d", d), + rclcpp::Parameter(param_prefix_ + "u_clamp_max", u_max), + rclcpp::Parameter(param_prefix_ + "u_clamp_min", u_min), + rclcpp::Parameter(param_prefix_ + "tracking_time_constant", trk_tc), + rclcpp::Parameter(param_prefix_ + "saturation", saturation), + rclcpp::Parameter(param_prefix_ + "antiwindup_strategy", antiwindup_strat.to_string())}); + + pid_.set_gains(p, i, d, u_max, u_min, trk_tc, saturation, antiwindup_strat); + } +} - pid_.set_gains(p, i, d, i_max, i_min, antiwindup); +void PidROS::set_gains(const Pid::Gains & gains) +{ + if (gains.i_min_ > gains.i_max_) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); + } + else if (gains.u_min_ > gains.u_max_) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + } + else + { + pid_.set_gains(gains); } } @@ -339,30 +523,25 @@ 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" - << " Antiwindup: " << gains.antiwindup_ - << "\n" - << " P_Error: " << p_error << "\n" - << " i_term: " << i_term << "\n" - << " D_Error: " << d_error << "\n" - << " Command: " << get_current_cmd();); -} - -void PidROS::set_gains(const Pid::Gains & gains) -{ - if (gains.i_min_ > gains.i_max_) - { - RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); - } - else - { - pid_.set_gains(gains); - } + RCLCPP_INFO_STREAM(node_logging_->get_logger(), + "Current Values of PID template:\n" + << " P Gain: " << gains.p_gain_ << "\n" + << " I Gain: " << gains.i_gain_ << "\n" + << " D Gain: " << gains.d_gain_ << "\n" + << " I Max: " << gains.i_max_ << "\n" + << " I Min: " << gains.i_min_ << "\n" + << " U_Max: " << gains.u_max_ << "\n" + << " U_Min: " << gains.u_min_ << "\n" + << " Tracking_Time_Constant: " << gains.trk_tc_ << "\n" + << " Saturation: " << gains.saturation_ << "\n" + << " Antiwindup: " << gains.antiwindup_ << "\n" + << " Antiwindup_Strategy: " << gains.antiwindup_strat_.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() @@ -406,11 +585,36 @@ void PidROS::set_parameter_event_callback() gains.i_min_ = parameter.get_value(); changed = true; } + else if (param_name == param_prefix_ + "u_clamp_max") + { + gains.u_max_ = parameter.get_value(); + changed = true; + } + else if (param_name == param_prefix_ + "u_clamp_min") + { + gains.u_min_ = parameter.get_value(); + changed = true; + } + else if (param_name == param_prefix_ + "tracking_time_constant") + { + gains.trk_tc_ = parameter.get_value(); + changed = true; + } + else if (param_name == param_prefix_ + "saturation") + { + gains.saturation_ = parameter.get_value(); + changed = true; + } else if (param_name == param_prefix_ + "antiwindup") { gains.antiwindup_ = parameter.get_value(); changed = true; } + else if (param_name == param_prefix_ + "antiwindup_strategy") + { + gains.antiwindup_strat_ = AntiwindupStrategy(parameter.get_value()); + changed = true; + } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { @@ -420,11 +624,14 @@ void PidROS::set_parameter_event_callback() if (changed) { - /// @note don't call set_gains() from inside a callback if (gains.i_min_ > gains.i_max_) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } + else if (gains.u_min_ > gains.u_max_) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + } else { pid_.set_gains(gains); diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 24c7b282..79aad83d 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -21,6 +21,10 @@ #include "rclcpp/parameter.hpp" #include "rclcpp/utilities.hpp" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + +using control_toolbox::AntiwindupStrategy; using rclcpp::executors::MultiThreadedExecutor; class TestablePidROS : public control_toolbox::PidROS @@ -54,10 +58,17 @@ void check_set_parameters( const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; + const bool SATURATION = true; const bool ANTIWINDUP = true; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; const bool SAVE_I_TERM = true; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM)); + ASSERT_NO_THROW(pid.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT, + SAVE_I_TERM)); rclcpp::Parameter param; @@ -77,9 +88,24 @@ void check_set_parameters( ASSERT_TRUE(node->get_parameter(prefix + "i_clamp_min", param)); ASSERT_EQ(param.get_value(), I_MIN); + ASSERT_TRUE(node->get_parameter(prefix + "u_clamp_max", param)); + ASSERT_EQ(param.get_value(), U_MAX); + + ASSERT_TRUE(node->get_parameter(prefix + "u_clamp_min", param)); + ASSERT_EQ(param.get_value(), U_MIN); + + ASSERT_TRUE(node->get_parameter(prefix + "tracking_time_constant", param)); + ASSERT_EQ(param.get_value(), TRK_TC); + + ASSERT_TRUE(node->get_parameter(prefix + "saturation", param)); + ASSERT_EQ(param.get_value(), SATURATION); + ASSERT_TRUE(node->get_parameter(prefix + "antiwindup", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP); + ASSERT_TRUE(node->get_parameter(prefix + "antiwindup_strategy", param)); + ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); + ASSERT_TRUE(node->get_parameter(prefix + "save_i_term", param)); ASSERT_EQ(param.get_value(), SAVE_I_TERM); @@ -90,7 +116,12 @@ void check_set_parameters( ASSERT_EQ(gains.d_gain_, D); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.u_max_, U_MAX); + ASSERT_EQ(gains.u_min_, U_MIN); + ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_TRUE(gains.saturation_); ASSERT_TRUE(gains.antiwindup_); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); } TEST(PidParametersTest, InitPidTest) @@ -116,8 +147,13 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double D = 3.0; const double I_MAX_BAD = -10.0; const double I_MIN_BAD = 10.0; + const double U_MAX_BAD = -10.0; + const double U_MIN_BAD = 10.0; + const double TRK_TC = 4.0; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX_BAD, I_MIN_BAD, false)); + ASSERT_NO_THROW(pid.initialize_from_args( + P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD, TRK_TC, false, false, + AntiwindupStrategy::NONE, false)); rclcpp::Parameter param; @@ -127,7 +163,12 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_FALSE(node->get_parameter("d", param)); ASSERT_FALSE(node->get_parameter("i_clamp_max", param)); ASSERT_FALSE(node->get_parameter("i_clamp_min", param)); + ASSERT_FALSE(node->get_parameter("u_clamp_max", param)); + ASSERT_FALSE(node->get_parameter("u_clamp_min", param)); + ASSERT_FALSE(node->get_parameter("tracking_time_constant", param)); + ASSERT_FALSE(node->get_parameter("saturation", param)); ASSERT_FALSE(node->get_parameter("antiwindup", param)); + ASSERT_FALSE(node->get_parameter("antiwindup_strategy", param)); // check gains were NOT set control_toolbox::Pid::Gains gains = pid.get_gains(); @@ -136,7 +177,12 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.d_gain_, 0.0); ASSERT_EQ(gains.i_max_, 0.0); ASSERT_EQ(gains.i_min_, 0.0); + ASSERT_EQ(gains.u_max_, 0.0); + ASSERT_EQ(gains.u_min_, 0.0); + ASSERT_EQ(gains.trk_tc_, 0.0); + ASSERT_FALSE(gains.saturation_); ASSERT_FALSE(gains.antiwindup_); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); } TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot) @@ -216,10 +262,17 @@ TEST(PidParametersTest, SetParametersTest) const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; + const bool SATURATION = true; const bool ANTIWINDUP = true; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; const bool SAVE_I_TERM = false; - pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM); + pid.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT, + SAVE_I_TERM); rcl_interfaces::msg::SetParametersResult set_result; @@ -238,8 +291,20 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_max", U_MAX))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_min", U_MIN))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("tracking_time_constant", TRK_TC))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", SATURATION))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("antiwindup_strategy", ANTIWINDUP_STRAT))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("save_i_term", SAVE_I_TERM))); ASSERT_TRUE(set_result.successful); @@ -253,7 +318,12 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.d_gain_, D); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.u_max_, U_MAX); + ASSERT_EQ(gains.u_min_, U_MIN); + ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_TRUE(gains.saturation_); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); } TEST(PidParametersTest, SetBadParametersTest) @@ -269,9 +339,17 @@ TEST(PidParametersTest, SetBadParametersTest) const double I_MIN = -10.0; const double I_MAX_BAD = -20.0; const double I_MIN_BAD = 20.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double U_MAX_BAD = -20.0; + const double U_MIN_BAD = 20.0; + const double TRK_TC = 4.0; + const bool SATURATION = true; const bool ANTIWINDUP = true; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; - pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT, false); rcl_interfaces::msg::SetParametersResult set_result; @@ -290,8 +368,20 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN_BAD))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_max", U_MAX_BAD))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_min", U_MIN_BAD))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("tracking_time_constant", TRK_TC))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", SATURATION))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("antiwindup_strategy", ANTIWINDUP_STRAT))); + ASSERT_TRUE(set_result.successful); // process callbacks rclcpp::spin_some(node->get_node_base_interface()); @@ -303,7 +393,12 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.d_gain_, D); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.u_max_, U_MAX); + ASSERT_EQ(gains.u_min_, U_MIN); + ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_TRUE(gains.saturation_); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); } TEST(PidParametersTest, GetParametersTest) @@ -317,10 +412,17 @@ TEST(PidParametersTest, GetParametersTest) const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; + const bool SATURATION = true; const bool ANTIWINDUP = true; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; - pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, false, false); - pid.set_gains(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.initialize_from_args( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, AntiwindupStrategy::NONE, false); + pid.set_gains( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT); rclcpp::Parameter param; @@ -339,9 +441,24 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("i_clamp_min", param)); ASSERT_EQ(param.get_value(), I_MIN); + ASSERT_TRUE(node->get_parameter("u_clamp_max", param)); + ASSERT_EQ(param.get_value(), U_MAX); + + ASSERT_TRUE(node->get_parameter("u_clamp_min", param)); + ASSERT_EQ(param.get_value(), U_MIN); + + ASSERT_TRUE(node->get_parameter("tracking_time_constant", param)); + ASSERT_EQ(param.get_value(), TRK_TC); + + ASSERT_TRUE(node->get_parameter("saturation", param)); + ASSERT_EQ(param.get_value(), SATURATION); + ASSERT_TRUE(node->get_parameter("antiwindup", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP); + ASSERT_TRUE(node->get_parameter("antiwindup_strategy", param)); + ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); + ASSERT_TRUE(node->get_parameter("save_i_term", param)); ASSERT_EQ(param.get_value(), false); } @@ -373,6 +490,18 @@ TEST(PidParametersTest, GetParametersFromParams) rclcpp::Parameter param_i_clamp_min; ASSERT_TRUE(node->get_parameter("i_clamp_min", param_i_clamp_min)); ASSERT_TRUE(std::isnan(param_i_clamp_min.get_value())); + + rclcpp::Parameter param_u_clamp_max; + ASSERT_TRUE(node->get_parameter("u_clamp_max", param_u_clamp_max)); + ASSERT_TRUE(std::isnan(param_u_clamp_max.get_value())); + + rclcpp::Parameter param_u_clamp_min; + ASSERT_TRUE(node->get_parameter("u_clamp_min", param_u_clamp_min)); + ASSERT_TRUE(std::isnan(param_u_clamp_min.get_value())); + + rclcpp::Parameter param_tracking_time_constant; + ASSERT_TRUE(node->get_parameter("tracking_time_constant", param_tracking_time_constant)); + ASSERT_TRUE(std::isnan(param_tracking_time_constant.get_value())); } TEST(PidParametersTest, MultiplePidInstances) @@ -387,9 +516,14 @@ TEST(PidParametersTest, MultiplePidInstances) const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; - ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, I_MAX, I_MIN, false, false)); - ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, I_MAX, I_MIN, true, false)); + ASSERT_NO_THROW(pid_1.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false, false, AntiwindupStrategy::NONE, false)); + ASSERT_NO_THROW(pid_2.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true, true, AntiwindupStrategy::NONE, false)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); @@ -406,3 +540,5 @@ int main(int argc, char ** argv) rclcpp::shutdown(); return result; } + +#pragma GCC diagnostic pop diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index 046ce683..548d1cd2 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -27,6 +27,10 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + +using control_toolbox::AntiwindupStrategy; using PidStateMsg = control_msgs::msg::PidState; using rclcpp::executors::MultiThreadedExecutor; @@ -39,7 +43,8 @@ TEST(PidPublisherTest, PublishTest) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, false, false); + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, false, AntiwindupStrategy::NONE, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -76,7 +81,8 @@ TEST(PidPublisherTest, PublishTestLifecycle) std::dynamic_pointer_cast>( pid_ros.get_pid_state_publisher()); - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, false, false); + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, false, AntiwindupStrategy::NONE, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -109,3 +115,5 @@ int main(int argc, char ** argv) rclcpp::shutdown(); return result; } + +#pragma GCC diagnostic pop diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index c2a75ee8..56f452d2 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -32,14 +32,255 @@ #include #include +#include #include "control_toolbox/pid.hpp" #include "gmock/gmock.h" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + +using control_toolbox::AntiwindupStrategy; using control_toolbox::Pid; using namespace std::chrono_literals; +TEST(ParameterTest, UTermBadIBoundsTestConstructor) +{ + RecordProperty( + "description", + "This test checks if an error is thrown for bad u_bounds specification (i.e. u_min > u_max)."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + EXPECT_THROW( + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, false, AntiwindupStrategy::NONE), + std::invalid_argument); +} + +TEST(ParameterTest, UTermBadIBoundsTest) +{ + RecordProperty( + "description", + "This test checks if gains remain for bad u_bounds specification (i.e. u_min > u_max)."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 0.0, false, false, AntiwindupStrategy::NONE); + auto gains = pid.get_gains(); + EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); + EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); + // Try to set bad u-bounds, i.e. u_min > u_max + EXPECT_NO_THROW(pid.set_gains( + 1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, false, AntiwindupStrategy::NONE)); + // Check if gains were not updated because u-bounds are bad, i.e. u_min > u_max + EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); + EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); +} + +TEST(ParameterTest, outputClampTest) +{ + RecordProperty( + "description", "This test succeeds if the output is clamped when the saturation is active."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, true, false, AntiwindupStrategy::BACK_CALCULATION); + + double cmd = 0.0; + + // ***** TEST UPPER LIMIT ***** + + cmd = pid.compute_command(0.5, 1.0); + EXPECT_EQ(0.5, cmd); + + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(2.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(50.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(100.0, 1.0); + EXPECT_EQ(1.0, cmd); + + // ***** TEST LOWER LIMIT ***** + + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-0.5, cmd); + + cmd = pid.compute_command(-1, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-2, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-10, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-50, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-100, 1.0); + EXPECT_EQ(-1.0, cmd); +} + +TEST(ParameterTest, noOutputClampTest) +{ + RecordProperty( + "description", "This test succeeds if the output isn't clamped when the saturation is false."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, false, false, AntiwindupStrategy::BACK_CALCULATION); + + double cmd = 0.0; + + // ***** TEST UPPER LIMIT ***** + + cmd = pid.compute_command(0.5, 1.0); + EXPECT_EQ(0.5, cmd); + + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(2.0, 1.0); + EXPECT_EQ(2.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + EXPECT_EQ(10.0, cmd); + + cmd = pid.compute_command(50.0, 1.0); + EXPECT_EQ(50.0, cmd); + + cmd = pid.compute_command(100.0, 1.0); + EXPECT_EQ(100.0, cmd); + + // ***** TEST LOWER LIMIT ***** + + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-0.5, cmd); + + cmd = pid.compute_command(-1, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-2, 1.0); + EXPECT_EQ(-2.0, cmd); + + cmd = pid.compute_command(-10, 1.0); + EXPECT_EQ(-10.0, cmd); + + cmd = pid.compute_command(-50, 1.0); + EXPECT_EQ(-50.0, cmd); + + cmd = pid.compute_command(-100, 1.0); + EXPECT_EQ(-100.0, cmd); +} + +TEST(ParameterTest, integrationBackCalculationZeroGainTest) +{ + RecordProperty( + "description", + "This test succeeds if the integral contribution is clamped when the integral gain is zero for " + "the back calculation technique."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, AntiwindupStrategy::BACK_CALCULATION); + + double cmd = 0.0; + double pe, ie, de; + + // back_calculation + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); +} + +TEST(ParameterTest, integrationConditionalIntegrationZeroGainTest) +{ + RecordProperty( + "description", + "This test succeeds if the integral contribution is clamped when the integral gain is zero for " + "the conditional integration technique."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, + AntiwindupStrategy::CONDITIONAL_INTEGRATION); + + double cmd = 0.0; + double pe, ie, de; + + // back_calculation + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); +} + TEST(ParameterTest, ITermBadIBoundsTestConstructor) { RecordProperty( @@ -183,23 +424,39 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double d_gain = std::rand() % 100; double i_max = std::rand() % 100; double i_min = -1 * std::rand() % 100; + double u_max = std::rand() % 100; + double u_min = -1 * std::rand() % 100; + double trk_tc = std::rand() % 100; + bool saturation = false; bool antiwindup = false; + AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE; // Initialize the default way - Pid pid1(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); + Pid pid1( + p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, + antiwindup_strat); // Test return values ------------------------------------------------- - double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return; - bool antiwindup_return; + double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, + u_min_return, trk_tc_return; + bool saturation_return, antiwindup_return; + AntiwindupStrategy antiwindup_strat_return; + pid1.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); + p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, + u_min_return, trk_tc_return, saturation_return, antiwindup_return, antiwindup_strat_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); EXPECT_EQ(d_gain, d_gain_return); EXPECT_EQ(i_max, i_max_return); EXPECT_EQ(i_min, i_min_return); + EXPECT_EQ(u_max, u_max_return); + EXPECT_EQ(u_min, u_min_return); + EXPECT_EQ(trk_tc, trk_tc_return); + EXPECT_EQ(saturation, saturation_return); EXPECT_EQ(antiwindup, antiwindup_return); + EXPECT_EQ(antiwindup_strat, antiwindup_strat_return); // Test return values using struct ------------------------------------------------- @@ -209,7 +466,16 @@ TEST(ParameterTest, gainSettingCopyPIDTest) d_gain = std::rand() % 100; i_max = std::rand() % 100; i_min = -1 * std::rand() % 100; - pid1.set_gains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); + u_max = std::rand() % 100; + u_min = -1 * std::rand() % 100; + trk_tc = std::rand() % 100; + saturation = false; + antiwindup = false; + antiwindup_strat = AntiwindupStrategy::NONE; + + pid1.set_gains( + p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, + antiwindup_strat); Pid::Gains g1 = pid1.get_gains(); EXPECT_EQ(p_gain, g1.p_gain_); @@ -217,11 +483,12 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(d_gain, g1.d_gain_); EXPECT_EQ(i_max, g1.i_max_); EXPECT_EQ(i_min, g1.i_min_); + EXPECT_EQ(u_max, g1.u_max_); + EXPECT_EQ(u_min, g1.u_min_); + EXPECT_EQ(trk_tc, g1.trk_tc_); + EXPECT_EQ(saturation, g1.saturation_); EXPECT_EQ(antiwindup, g1.antiwindup_); - - // \todo test initParam() ------------------------------------------------- - - // \todo test bool init(const ros::NodeHandle &n); ----------------------------------- + EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); // Send update command to populate errors ------------------------------------------------- pid1.set_current_cmd(10); @@ -231,14 +498,20 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid2(pid1); pid2.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); + p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, + u_min_return, trk_tc_return, saturation_return, antiwindup_return, antiwindup_strat_return); - EXPECT_EQ(p_gain, p_gain_return); - EXPECT_EQ(i_gain, i_gain_return); - EXPECT_EQ(d_gain, d_gain_return); - EXPECT_EQ(i_max, i_max_return); - EXPECT_EQ(i_min, i_min_return); - EXPECT_EQ(antiwindup, antiwindup_return); + EXPECT_EQ(p_gain, g1.p_gain_); + EXPECT_EQ(i_gain, g1.i_gain_); + EXPECT_EQ(d_gain, g1.d_gain_); + EXPECT_EQ(i_max, g1.i_max_); + EXPECT_EQ(i_min, g1.i_min_); + EXPECT_EQ(u_max, g1.u_max_); + EXPECT_EQ(u_min, g1.u_min_); + EXPECT_EQ(trk_tc, g1.trk_tc_); + EXPECT_EQ(saturation, g1.saturation_); + EXPECT_EQ(antiwindup, g1.antiwindup_); + EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); // Test that errors are zero double pe2, ie2, de2; @@ -252,14 +525,20 @@ TEST(ParameterTest, gainSettingCopyPIDTest) pid3 = pid1; pid3.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); + p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, + u_min_return, trk_tc_return, saturation_return, antiwindup_return, antiwindup_strat_return); - EXPECT_EQ(p_gain, p_gain_return); - EXPECT_EQ(i_gain, i_gain_return); - EXPECT_EQ(d_gain, d_gain_return); - EXPECT_EQ(i_max, i_max_return); - EXPECT_EQ(i_min, i_min_return); - EXPECT_EQ(antiwindup, antiwindup_return); + EXPECT_EQ(p_gain, g1.p_gain_); + EXPECT_EQ(i_gain, g1.i_gain_); + EXPECT_EQ(d_gain, g1.d_gain_); + EXPECT_EQ(i_max, g1.i_max_); + EXPECT_EQ(i_min, g1.i_min_); + EXPECT_EQ(u_max, g1.u_max_); + EXPECT_EQ(u_min, g1.u_min_); + EXPECT_EQ(trk_tc, g1.trk_tc_); + EXPECT_EQ(saturation, g1.saturation_); + EXPECT_EQ(antiwindup, g1.antiwindup_); + EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); // Test that errors are zero double pe3, ie3, de3; @@ -443,6 +722,123 @@ TEST(CommandTest, completePIDTest) EXPECT_EQ(-3.5, cmd); } +TEST(CommandTest, backCalculationPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "back calculation technique."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, AntiwindupStrategy::BACK_CALCULATION); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(1.0, ie); + EXPECT_EQ(0.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(3.0, ie); + EXPECT_EQ(1.0, cmd); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(3.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(7.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(4.0, cmd); +} + +TEST(CommandTest, conditionalIntegrationPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "conditional integration technique."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, + AntiwindupStrategy::CONDITIONAL_INTEGRATION); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(1.0, ie); + EXPECT_EQ(0.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(3.0, ie); + EXPECT_EQ(1.0, cmd); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(3.0, cmd); + + // Saturation applied, conditional integration now holds the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, conditional integration now holds the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(0.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(5.0, cmd); +} + TEST(CommandTest, timeArgumentTest) { RecordProperty("description", "Tests different dt argument type methods."); @@ -513,3 +909,5 @@ int main(int argc, char ** argv) testing::InitGoogleMock(&argc, argv); return RUN_ALL_TESTS(); } + +#pragma GCC diagnostic pop diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 55af86c1..5e34fc1e 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -3,3 +3,7 @@ Release Notes: Jazzy to Kilted ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes the changes between Jazzy (previous) and Kilted (current) releases. + +Pid/PidRos +*********************************************************** +* Added a saturation feature to PID output and two anti-windup techniques (back calculation and conditional integration) (`#298 `_).