From b256384df0705ee7d3e850c7d0ad7c8f6a3b1ab3 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Tue, 16 Sep 2025 13:32:45 +0000 Subject: [PATCH 01/12] Add derivative filter and discretization --- .../include/control_toolbox/pid.hpp | 174 ++++++++++++++++-- .../include/control_toolbox/pid_ros.hpp | 47 +++++ control_toolbox/src/pid.cpp | 92 ++++++++- control_toolbox/src/pid_ros.cpp | 54 +++++- .../test/pid_ros_parameters_tests.cpp | 10 +- 5 files changed, 345 insertions(+), 32 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 8d05af1e..7afd8726 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -33,8 +33,10 @@ #ifndef CONTROL_TOOLBOX__PID_HPP_ #define CONTROL_TOOLBOX__PID_HPP_ +#include #include #include +#include #include #include #include @@ -118,14 +120,14 @@ struct AntiWindupStrategy (tracking_time_constant < 0.0 || !std::isfinite(tracking_time_constant))) { throw std::invalid_argument( - "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant " + "AntiWindupStrategy 'back_calculation' requires a non-negative tracking time constant " "(tracking_time_constant)"); } - if (i_min >= i_max) + if (i_min > i_max) { throw std::invalid_argument( fmt::format( - "PID requires i_min < i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max)); + "PID requires i_min <= i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max)); } if ( type != NONE && type != UNDEFINED && type != BACK_CALCULATION && @@ -204,6 +206,7 @@ inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) \param p Proportional gain. Reacts to current error. \param i Integral gain. Accumulates past error to eliminate steady-state error. \param d Derivative gain. Predicts future error to reduce overshoot and settling time. + \param tf Derivative filter time constant. Used to filter high-frequency noise in the derivative term. \param u\_min Minimum bound for the controller output. \param u\_max Maximum bound for the controller output. \param tracking\_time\_constant Tracking time constant for BACK_CALCULATION anti-windup. @@ -216,6 +219,14 @@ inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) and unsaturated outputs using \c tracking\_time\_constant. - CONDITIONAL_INTEGRATION: only integrates when output is not saturated or error drives it away from saturation. + \param integration\_method Method to compute the integral contribution. Options: + - "forward_euler" + - "backward_euler" + - "trapezoidal" + \param derivative\_method Method to compute the derivative contribution. Options: + - "forward_euler" + - "backward_euler" + - "trapezoidal" \section antiwindup Anti-Windup Strategies Without anti-windup, clamping causes integral windup, leading to overshoot and sluggish @@ -275,14 +286,44 @@ class Pid Gains( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) + : Gains( + p, i, d, + /*tf*/ 0.0, u_max, u_min, antiwindup_strat, + /*i_method*/ "forward_euler", + /*d_method*/ "forward_euler") + { + } + + /*! + * \brief Constructor for passing in values. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \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 i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * + */ + Gains( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method) : p_gain_(p), i_gain_(i), d_gain_(d), + tf_(tf), i_max_(antiwindup_strat.i_max), i_min_(antiwindup_strat.i_min), u_max_(u_max), u_min_(u_min), - antiwindup_strat_(antiwindup_strat) + antiwindup_strat_(antiwindup_strat), + i_method_(i_method), + d_method_(d_method) { if (std::isnan(u_min) || std::isnan(u_max)) { @@ -301,12 +342,14 @@ class Pid { if (i_min_ > i_max_) { - error_msg = fmt::format("Gains: i_min ({}) must be less than i_max ({})", i_min_, i_max_); + error_msg = + fmt::format("Gains: i_min ({}) must be less than or equal to i_max ({})", i_min_, i_max_); return false; } - else if (u_min_ >= u_max_) + else if (u_min_ > u_max_) { - error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_); + error_msg = + fmt::format("Gains: u_min ({}) must be less than or equal to u_max ({})", u_min_, u_max_); return false; } else if (std::isnan(u_min_) || std::isnan(u_max_)) @@ -314,6 +357,11 @@ class Pid error_msg = "Gains: u_min and u_max must not be NaN"; return false; } + else if (tf_ < 0.0) + { + error_msg = "Gains: tf (derivative filter time constant) must be non-negative"; + return false; + } try { antiwindup_strat_.validate(); @@ -329,14 +377,16 @@ class Pid void print() const { 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_ << ", antiwindup_strat: " << antiwindup_strat_.to_string() - << std::endl; + << ", tf: " << tf_ << ", i_max: " << i_max_ << ", i_min: " << i_min_ + << ", u_max: " << u_max_ << ", u_min: " << u_min_ + << ", antiwindup_strat: " << antiwindup_strat_.to_string() + << ", i_method: " << i_method_ << ", d_method: " << d_method_ << std::endl; } double p_gain_ = 0.0; /**< Proportional gain. */ double i_gain_ = 0.0; /**< Integral gain. */ double d_gain_ = 0.0; /**< Derivative gain. */ + double tf_ = 0.0; /**< Derivative filter time constant. */ double i_max_ = std::numeric_limits::infinity(); /**< Maximum allowable integral term. */ double i_min_ = @@ -344,6 +394,8 @@ class Pid double u_max_ = std::numeric_limits::infinity(); /**< Maximum allowable output. */ double u_min_ = -std::numeric_limits::infinity(); /**< Minimum allowable output. */ AntiWindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ + std::string i_method_ = "forward_euler"; /**< Integration method. */ + std::string d_method_ = "forward_euler"; /**< Derivative method. */ }; /*! @@ -366,6 +418,27 @@ class Pid double u_min = -std::numeric_limits::infinity(), const AntiWindupStrategy & antiwindup_strat = AntiWindupStrategy()); + /*! + * \brief Constructor, initialize Pid-gains and term limits. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \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 i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * + * \throws An std::invalid_argument exception is thrown if u_min > u_max. + */ + Pid( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method); + /*! * \brief Copy constructor required for preventing mutexes from being copied * \param source - Pid to copy @@ -396,6 +469,28 @@ class Pid double p, double i, double d, double u_max, double u_min, const 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 tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \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 i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * \return True if all parameters are successfully set, False otherwise. + * + * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max. + */ + bool initialize( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method); + /*! * \brief Reset the state of this PID controller * @note The integral term is not retained and it is reset to zero @@ -431,6 +526,26 @@ class Pid double & p, double & i, double & d, double & u_max, double & u_min, 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 tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \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 i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * + * \note This method is not RT safe + */ + void get_gains( + double & p, double & i, double & d, double & tf, double & u_max, double & u_min, + AntiWindupStrategy & antiwindup_strat, std::string & i_method, std::string & d_method); + /*! * \brief Get PID gains for the controller. * \return gains A struct of the PID gain values @@ -467,6 +582,29 @@ class Pid double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat); + /*! + * \brief Set PID gains for the controller. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \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 i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * \return True if all parameters are successfully set, False otherwise. + * + * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max. + * \note This method is not RT safe + */ + bool set_gains( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method); + /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values. @@ -623,19 +761,27 @@ class Pid 0.0, 0.0, 0.0, + 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), - AntiWindupStrategy()}; + AntiWindupStrategy(), + "forward_euler", + "forward_euler"}; // Store the PID gains in a realtime box to allow dynamic reconfigure to update it without // blocking the realtime update loop realtime_tools::RealtimeThreadSafeBox gains_box_{gains_}; double p_error_last_ = 0; /** Save state for derivative state calculation. */ double p_error_ = 0; /** Error. */ + + double d_error_last_ = 0; /** Save state for derivative state calculation. */ + double d_term_last_ = 0; /** Save state for derivative filter. */ 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. */ + + 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 d6856576..ecdf9970 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -175,6 +175,30 @@ class PidROS 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 and set the parameters. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \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 i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * \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 antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max. + */ + bool initialize_from_args( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method, + 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), False otherwise @@ -249,6 +273,29 @@ class PidROS double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat); + /*! + * \brief Set PID gains for the controller. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \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 i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * \return True if all parameters are successfully set, False otherwise. + * + * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max. + * \note This method is not RT safe + */ + bool set_gains( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method); + /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 8a2495d1..6c52ff45 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -49,12 +49,19 @@ namespace control_toolbox Pid::Pid( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) +: Pid(p, i, d, 0.0, u_max, u_min, antiwindup_strat, "forward_euler", "forward_euler") +{ +} + +Pid::Pid( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method) { if (u_min > u_max) { throw std::invalid_argument("received u_min > u_max"); } - set_gains(p, i, d, u_max, u_min, antiwindup_strat); + set_gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method); // Initialize saved i-term values clear_saved_iterm(); @@ -80,7 +87,14 @@ bool Pid::initialize( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) { - if (set_gains(p, i, d, u_max, u_min, antiwindup_strat)) + return initialize(p, i, d, 0.0, u_max, u_min, antiwindup_strat, "forward_euler", "forward_euler"); +} + +bool Pid::initialize( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method) +{ + if (set_gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method)) { reset(); return true; @@ -122,6 +136,22 @@ void Pid::get_gains( antiwindup_strat = gains.antiwindup_strat_; } +void Pid::get_gains( + double & p, double & i, double & d, double & tf, double & u_max, double & u_min, + AntiWindupStrategy & antiwindup_strat, std::string & i_method, std::string & d_method) +{ + Gains gains = get_gains(); + p = gains.p_gain_; + i = gains.i_gain_; + d = gains.d_gain_; + tf = gains.tf_; + u_max = gains.u_max_; + u_min = gains.u_min_; + antiwindup_strat = gains.antiwindup_strat_; + i_method = gains.i_method_; + d_method = gains.d_method_; +} + Pid::Gains Pid::get_gains() { // blocking, as get_gains() is called from non-RT thread @@ -131,10 +161,17 @@ Pid::Gains Pid::get_gains() bool Pid::set_gains( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) +{ + return set_gains(p, i, d, 0.0, u_max, u_min, antiwindup_strat, "forward_euler", "forward_euler"); +} + +bool Pid::set_gains( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method) { try { - Gains gains(p, i, d, u_max, u_min, antiwindup_strat); + Gains gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method); if (set_gains(gains)) { return true; @@ -199,7 +236,19 @@ double Pid::compute_command(double error, const double & dt_s) } // Calculate the derivative error - d_error_ = (error - p_error_last_) / dt_s; + if (gains_.d_method_ == "forward_euler" || gains_.d_method_ == "backward_euler") + { + d_error_ = (error - p_error_last_) / dt_s; + } + else if (gains_.d_method_ == "trapezoidal") + { + d_error_ = 2 * (error - p_error_last_) / dt_s - d_error_last_; + d_error_last_ = d_error_; + } + else + { + throw std::invalid_argument("Unknown derivative method: " + gains_.d_method_); + } p_error_last_ = error; return compute_command(error, d_error_, dt_s); @@ -268,7 +317,37 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) p_term = gains_.p_gain_ * p_error_; // Calculate derivative contribution to command - d_term = gains_.d_gain_ * d_error_; + if (gains_.tf_ > 0.0 && gains_.d_method_ == "forward_euler") + { + // Derivative filter is on + d_term = + ((gains_.tf_ - dt_s) * d_term_last_ + (gains_.d_gain_ * dt_s * d_error_)) / (gains_.tf_); + } + else if (gains_.tf_ > 0.0 && gains_.d_method_ == "backward_euler") + { + // Derivative filter is on + d_term = + ((gains_.tf_) * d_term_last_ + (gains_.d_gain_ * dt_s * d_error_)) / (gains_.tf_ + dt_s); + } + else if (gains_.tf_ > 0.0 && gains_.d_method_ == "trapezoidal") + { + // Derivative filter is on + d_term = ((2 * gains_.tf_ + dt_s) * d_term_last_ + + (gains_.d_gain_ * dt_s * (d_error_ + d_error_last_))) / + (2 * gains_.tf_ + dt_s); + } + else if ( + gains_.tf_ == 0.0 && + (gains_.d_method_ == "forward_euler" || gains_.d_method_ == "backward_euler")) + { + // Derivative filter is off. Since forward doesn't exist for tf=0, use backward + d_term = gains_.d_gain_ * d_error_; + } + else if (gains_.tf_ == 0.0 && gains_.d_method_ == "trapezoidal") + { + // Derivative filter is off + d_term = (-dt_s * d_term_last_ + (gains_.d_gain_ * dt_s * (d_error_ + d_error_last_))) / (dt_s); + } if (gains_.antiwindup_strat_.type == AntiWindupStrategy::UNDEFINED) { @@ -323,6 +402,9 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) i_term_ = std::clamp(i_term_, gains_.i_min_, gains_.i_max_); + d_term_last_ = d_term; + d_error_last_ = d_error_; + return cmd_; } diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index ddf445fa..feef1a4b 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -30,6 +30,7 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include #include #include #include @@ -254,19 +255,22 @@ 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, tracking_time_constant, error_deadband; - p = i = d = tracking_time_constant = std::numeric_limits::quiet_NaN(); + double p, i, d, tf, i_max, i_min, u_max, u_min, tracking_time_constant, error_deadband; + p = i = d = tf = tracking_time_constant = std::numeric_limits::quiet_NaN(); error_deadband = std::numeric_limits::epsilon(); i_max = MAX_INFINITY; i_min = -MAX_INFINITY; u_max = MAX_INFINITY; u_min = -MAX_INFINITY; std::string antiwindup_strat_str = "none"; + std::string i_method = "forward_euler"; + std::string d_method = "forward_euler"; 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_ + "derivative_filter_time", tf); 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); @@ -283,6 +287,9 @@ bool PidROS::initialize_from_ros_parameters() u_min = -MAX_INFINITY; } get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat_str); + get_string_param(param_prefix_ + "integration_method", i_method); + get_string_param(param_prefix_ + "derivative_method", d_method); + declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(false)); declare_param( param_prefix_ + "activate_state_publisher", rclcpp::ParameterValue(rt_state_pub_ != nullptr)); @@ -309,7 +316,7 @@ bool PidROS::initialize_from_ros_parameters() return false; } - if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat)) + if (pid_.initialize(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method)) { return all_params_available; } @@ -329,7 +336,16 @@ 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) { - Pid::Gains verify_gains(p, i, d, u_max, u_min, antiwindup_strat); + return initialize_from_args( + p, i, d, 0.0, u_max, u_min, antiwindup_strat, "forward_euler", "forward_euler", save_i_term); +} + +bool PidROS::initialize_from_args( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method, + bool save_i_term) +{ + Pid::Gains verify_gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method); std::string error_msg = ""; if (!verify_gains.validate(error_msg)) { @@ -340,12 +356,13 @@ bool PidROS::initialize_from_args( } else { - if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat)) + if (pid_.initialize(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method)) { 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_ + "derivative_filter_time", rclcpp::ParameterValue(gains.tf_)); declare_param( param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(gains.antiwindup_strat_.i_max)); declare_param( @@ -363,6 +380,8 @@ bool PidROS::initialize_from_args( declare_param( param_prefix_ + "antiwindup_strategy", rclcpp::ParameterValue(gains.antiwindup_strat_.to_string())); + declare_param(param_prefix_ + "integration_method", rclcpp::ParameterValue(i_method)); + declare_param(param_prefix_ + "derivative_method", rclcpp::ParameterValue(d_method)); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); declare_param( param_prefix_ + "activate_state_publisher", @@ -414,7 +433,14 @@ bool PidROS::set_gains( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) { - Pid::Gains gains(p, i, d, u_max, u_min, antiwindup_strat); + return set_gains(p, i, d, 0.0, u_max, u_min, antiwindup_strat, "forward_euler", "forward_euler"); +} + +bool PidROS::set_gains( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method) +{ + Pid::Gains gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method); return set_gains(gains); } @@ -437,6 +463,7 @@ bool PidROS::set_gains(const Pid::Gains & gains) {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_ + "derivative_filter_time", gains.tf_), 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_), @@ -448,7 +475,9 @@ bool PidROS::set_gains(const Pid::Gains & gains) 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())}); + param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string()), + rclcpp::Parameter(param_prefix_ + "integration_method", gains.i_method_), + rclcpp::Parameter(param_prefix_ + "derivative_method", gains.d_method_)}); return true; } } @@ -504,17 +533,19 @@ void PidROS::print_values() << " P Gain: " << gains.p_gain_ << "\n" << " I Gain: " << gains.i_gain_ << "\n" << " D Gain: " << gains.d_gain_ << "\n" + << " D Filter Time: " << gains.tf_ << "\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_Strategy: " << gains.antiwindup_strat_.to_string() << "\n" - << "\n" + << " integration_method: " << gains.i_method_ << "\n" + << " derivative_method: " << gains.d_method_ << "\n" << " P Error: " << p_error << "\n" << " I Term: " << i_term << "\n" << " D Error: " << d_error << "\n" - << " Command: " << get_current_cmd();); + << " Command: " << get_current_cmd()); } void PidROS::set_parameter_event_callback() @@ -594,6 +625,11 @@ void PidROS::set_parameter_event_callback() gains.d_gain_ = parameter.get_value(); changed = true; } + else if (param_name == param_prefix_ + "derivative_filter_time") + { + gains.tf_ = parameter.get_value(); + changed = true; + } else if (param_name == param_prefix_ + "i_clamp_max") { gains.i_max_ = parameter.get_value(); diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 6839f0cf..a1c661bb 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -434,8 +434,7 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - 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.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false)); 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, U_MAX, U_MIN, ANTIWINDUP_STRAT); @@ -524,8 +523,7 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - 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.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false)); 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); @@ -586,6 +584,10 @@ TEST(PidParametersTest, GetParametersFromParams) ASSERT_TRUE(node->get_parameter("d", param_d)); EXPECT_TRUE(std::isnan(param_d.get_value())); + rclcpp::Parameter param_tf; + ASSERT_TRUE(node->get_parameter("derivative_filter_time", param_tf)); + EXPECT_TRUE(std::isnan(param_tf.get_value())); + rclcpp::Parameter param_i_clamp_max; ASSERT_TRUE(node->get_parameter("i_clamp_max", param_i_clamp_max)); EXPECT_TRUE(std::isinf(param_i_clamp_max.get_value())); From c40689175f8c9fb6154cf75e27166747ac41df4f Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Wed, 17 Sep 2025 02:55:29 +0000 Subject: [PATCH 02/12] Add integrator discretization --- .../include/control_toolbox/pid.hpp | 9 +- control_toolbox/src/pid.cpp | 115 ++++++++++++++---- 2 files changed, 100 insertions(+), 24 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 7afd8726..56b6eb2e 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -778,10 +778,13 @@ class Pid double d_term_last_ = 0; /** Save state for derivative filter. */ double d_error_ = 0; /** Derivative of error. */ - double i_term_ = 0; /** Integrator state. */ + double i_term_ = 0; /** Integrator state. */ + double i_term_last_ = 0; /** Last integrator state. */ + double aw_term_last_ = 0; /** Last anti-windup term. */ - double cmd_ = 0; /** Command to send. */ - double cmd_unsat_ = 0; /** command without saturation. */ + double cmd_ = 0; /** Command to send. */ + double cmd_unsat_ = 0; /** command without saturation. */ + double error_last_ = 0; /** Last error. */ }; } // namespace control_toolbox diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 6c52ff45..17d98dbb 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -111,6 +111,11 @@ void Pid::reset(bool save_i_term) d_error_ = 0.0; cmd_ = 0.0; + d_error_last_ = 0; + d_term_last_ = 0; + error_last_ = 0; + aw_term_last_ = 0; + // Check to see if we should reset integral error here if (!save_i_term) { @@ -121,7 +126,11 @@ void Pid::reset(bool save_i_term) gains_ = gains_box_.get(); } -void Pid::clear_saved_iterm() { i_term_ = 0.0; } +void Pid::clear_saved_iterm() +{ + i_term_ = 0.0; + i_term_last_ = 0.0; +} void Pid::get_gains( double & p, double & i, double & d, double & u_max, double & u_min, @@ -332,22 +341,16 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) else if (gains_.tf_ > 0.0 && gains_.d_method_ == "trapezoidal") { // Derivative filter is on - d_term = ((2 * gains_.tf_ + dt_s) * d_term_last_ + + d_term = ((2 * gains_.tf_ - dt_s) * d_term_last_ + (gains_.d_gain_ * dt_s * (d_error_ + d_error_last_))) / (2 * gains_.tf_ + dt_s); } - else if ( - gains_.tf_ == 0.0 && - (gains_.d_method_ == "forward_euler" || gains_.d_method_ == "backward_euler")) + else if (gains_.tf_ == 0.0) { - // Derivative filter is off. Since forward doesn't exist for tf=0, use backward + // Derivative filter is off. Since forward doesn't exist for tf=0, use backward. + // To avoid artificial states and amplify noise, trapezoidal also falls back to backward. d_term = gains_.d_gain_ * d_error_; } - else if (gains_.tf_ == 0.0 && gains_.d_method_ == "trapezoidal") - { - // Derivative filter is off - d_term = (-dt_s * d_term_last_ + (gains_.d_gain_ * dt_s * (d_error_ + d_error_last_))) / (dt_s); - } if (gains_.antiwindup_strat_.type == AntiWindupStrategy::UNDEFINED) { @@ -358,6 +361,60 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) const bool is_error_in_deadband_zone = control_toolbox::is_zero(error, gains_.antiwindup_strat_.error_deadband); + if (gains_.i_method_ == "forward_euler") + { + i_term_ = gains_.i_gain_ * dt_s * error_last_; + } + else if (gains_.i_method_ == "backward_euler") + { + i_term_ = gains_.i_gain_ * dt_s * error; + } + else if (gains_.i_method_ == "trapezoidal") + { + i_term_ = gains_.i_gain_ * (dt_s * 0.5) * (error + error_last_); + } + else + { + throw std::runtime_error("Pid: invalid integral method"); + } + + double i_proposed = i_term_last_ + i_term_; + if (gains_.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION) + { + // testa se o incremento empurra a saturação + const bool have_limits = std::isfinite(gains_.u_min_) || std::isfinite(gains_.u_max_); + if (have_limits) + { + // commando não-saturado candidato usando i_proposed + const double cmd_unsat_candidate = p_term + d_term + i_proposed; + + const bool sat_high = std::isfinite(gains_.u_max_) && (cmd_unsat_candidate > gains_.u_max_); + const bool sat_low = std::isfinite(gains_.u_min_) && (cmd_unsat_candidate < gains_.u_min_); + + const bool pushing_high = sat_high && (i_term_ > 0.0); + const bool pushing_low = sat_low && (i_term_ < 0.0); + + if (pushing_high || pushing_low) + { + i_term_ = i_term_last_; // congela + } + else + { + i_term_ = i_proposed; // aceita + } + } + else + { + i_term_ = i_proposed; // sem limites -> aceita + } + } + else + { + i_term_ = i_proposed; // sem CI -> integra normalmente (AW vem depois) + } + + i_term_ = std::clamp(i_term_, gains_.i_min_, gains_.i_max_); + // Compute the command cmd_unsat_ = p_term + i_term_ + d_term; @@ -381,10 +438,30 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) { if ( gains_.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && - !is_zero(gains_.i_gain_)) + !is_zero(gains_.i_gain_) && gains_.i_method_ == "forward_euler") + { + i_term_ += dt_s * 1 / gains_.antiwindup_strat_.tracking_time_constant * (cmd_ - cmd_unsat_); + } + else if ( + gains_.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && + !is_zero(gains_.i_gain_) && gains_.i_method_ == "backward_euler") { - i_term_ += dt_s * (gains_.i_gain_ * error + - 1 / gains_.antiwindup_strat_.tracking_time_constant * (cmd_ - cmd_unsat_)); + i_term_ = i_term_ / (1 + dt_s / gains_.antiwindup_strat_.tracking_time_constant) + + dt_s / gains_.antiwindup_strat_.tracking_time_constant * (cmd_ - p_term - d_term) / + (1 + dt_s / gains_.antiwindup_strat_.tracking_time_constant); + } + else if ( + gains_.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && + !is_zero(gains_.i_gain_) && gains_.i_method_ == "trapezoidal") + { + const double num_i_last = + (1.0 - dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant)) * i_term_last_; + const double trap_inc = gains_.i_gain_ * (dt_s * 0.5) * (p_error_ + error_last_); + const double aw_sum = (cmd_ - (p_term + d_term)) + aw_term_last_; + const double denom = (1.0 + dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant)); + i_term_ = (num_i_last + trap_inc + + (dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant)) * aw_sum) / + denom; } else if (gains_.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION) { @@ -393,17 +470,13 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) i_term_ += dt_s * gains_.i_gain_ * error; } } - else if (gains_.antiwindup_strat_.type == AntiWindupStrategy::NONE) - { - // No anti-windup strategy, so just integrate the error - i_term_ += dt_s * gains_.i_gain_ * error; - } } - i_term_ = std::clamp(i_term_, gains_.i_min_, gains_.i_max_); - d_term_last_ = d_term; d_error_last_ = d_error_; + i_term_last_ = i_term_; + aw_term_last_ = cmd_ - p_term - d_term; + error_last_ = error; return cmd_; } From e15f1a2148837e0a159d39b3ae9006d307ac7939 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Thu, 18 Sep 2025 02:18:20 +0000 Subject: [PATCH 03/12] Fix tests --- control_toolbox/src/pid.cpp | 48 ++++++------------------------ control_toolbox/test/pid_tests.cpp | 26 +++++++++------- 2 files changed, 24 insertions(+), 50 deletions(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 17d98dbb..3ee4f86a 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -363,55 +363,32 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) if (gains_.i_method_ == "forward_euler") { - i_term_ = gains_.i_gain_ * dt_s * error_last_; + i_term_ = i_term_last_ + gains_.i_gain_ * dt_s * error_last_; } else if (gains_.i_method_ == "backward_euler") { - i_term_ = gains_.i_gain_ * dt_s * error; + i_term_ = i_term_last_ + gains_.i_gain_ * dt_s * error; } else if (gains_.i_method_ == "trapezoidal") { - i_term_ = gains_.i_gain_ * (dt_s * 0.5) * (error + error_last_); + i_term_ = i_term_last_ + gains_.i_gain_ * (dt_s * 0.5) * (error + error_last_); } else { throw std::runtime_error("Pid: invalid integral method"); } - double i_proposed = i_term_last_ + i_term_; if (gains_.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION) { - // testa se o incremento empurra a saturação - const bool have_limits = std::isfinite(gains_.u_min_) || std::isfinite(gains_.u_max_); - if (have_limits) + if (!is_zero(aw_term_last_ - i_term_last_)) { - // commando não-saturado candidato usando i_proposed - const double cmd_unsat_candidate = p_term + d_term + i_proposed; - - const bool sat_high = std::isfinite(gains_.u_max_) && (cmd_unsat_candidate > gains_.u_max_); - const bool sat_low = std::isfinite(gains_.u_min_) && (cmd_unsat_candidate < gains_.u_min_); - - const bool pushing_high = sat_high && (i_term_ > 0.0); - const bool pushing_low = sat_low && (i_term_ < 0.0); - - if (pushing_high || pushing_low) - { - i_term_ = i_term_last_; // congela - } - else - { - i_term_ = i_proposed; // aceita - } - } - else - { - i_term_ = i_proposed; // sem limites -> aceita + double dI = i_term_ - i_term_last_; + bool sat_high = (cmd_ == gains_.u_max_); + bool sat_low = (cmd_ == gains_.u_min_); + bool move_saturation = (sat_high && dI < 0) || (sat_low && dI > 0); + i_term_ = move_saturation ? i_term_ : i_term_last_; } } - else - { - i_term_ = i_proposed; // sem CI -> integra normalmente (AW vem depois) - } i_term_ = std::clamp(i_term_, gains_.i_min_, gains_.i_max_); @@ -463,13 +440,6 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) (dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant)) * aw_sum) / denom; } - else if (gains_.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION) - { - if (!(!is_zero(cmd_unsat_ - cmd_) && error * cmd_unsat_ > 0)) - { - i_term_ += dt_s * gains_.i_gain_ * error; - } - } } d_term_last_ = d_term; diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 5629cd5f..9a3f4af5 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -699,43 +699,45 @@ TEST(CommandTest, backCalculationPIDTest) // 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); + // Since default discretization is forward Euler, the integral term is not updated at + // the first call (last error = 0) + EXPECT_EQ(0.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, 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, 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, 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, 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, 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, ie); EXPECT_EQ(4.0, cmd); } @@ -762,19 +764,21 @@ TEST(CommandTest, conditionalIntegrationPIDTest) // 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); + // Since default discretization is forward Euler, the integral term is not updated + // at the first call (last error = 0) + EXPECT_EQ(0.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, 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, ie); EXPECT_EQ(3.0, cmd); // Saturation applied, conditional integration now holds the integral term @@ -792,7 +796,7 @@ TEST(CommandTest, conditionalIntegrationPIDTest) // 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(6.0, ie); EXPECT_EQ(5.0, cmd); // PID recover from the windup/saturation From b783ae2495b88b414050ddccb0a80fa326c47adf Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Thu, 18 Sep 2025 03:48:37 +0000 Subject: [PATCH 04/12] Add filtered derivative test --- .../include/control_toolbox/pid.hpp | 5 +- control_toolbox/src/pid.cpp | 40 ++++++++++------ control_toolbox/test/pid_tests.cpp | 47 +++++++++++++++++++ 3 files changed, 75 insertions(+), 17 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 56b6eb2e..8611e962 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -782,9 +782,8 @@ class Pid double i_term_last_ = 0; /** Last integrator state. */ double aw_term_last_ = 0; /** Last anti-windup term. */ - double cmd_ = 0; /** Command to send. */ - double cmd_unsat_ = 0; /** command without saturation. */ - double error_last_ = 0; /** Last error. */ + double cmd_ = 0; /** Command to send. */ + double cmd_unsat_ = 0; /** command without saturation. */ }; } // namespace control_toolbox diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 3ee4f86a..c3202c28 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -109,12 +109,10 @@ void Pid::reset(bool save_i_term) p_error_last_ = 0.0; p_error_ = 0.0; d_error_ = 0.0; - cmd_ = 0.0; - d_error_last_ = 0; d_term_last_ = 0; - error_last_ = 0; aw_term_last_ = 0; + cmd_ = 0.0; // Check to see if we should reset integral error here if (!save_i_term) @@ -244,21 +242,21 @@ double Pid::compute_command(double error, const double & dt_s) return cmd_ = std::numeric_limits::quiet_NaN(); } - // Calculate the derivative error + // Calculate the derivative error based on the selected method if (gains_.d_method_ == "forward_euler" || gains_.d_method_ == "backward_euler") { + // Since \dot{e}[k-1] and \dot{e}[k] are calculated the same way for forward and backward euler, + // we can combine them here d_error_ = (error - p_error_last_) / dt_s; } else if (gains_.d_method_ == "trapezoidal") { d_error_ = 2 * (error - p_error_last_) / dt_s - d_error_last_; - d_error_last_ = d_error_; } else { throw std::invalid_argument("Unknown derivative method: " + gains_.d_method_); } - p_error_last_ = error; return compute_command(error, d_error_, dt_s); } @@ -304,6 +302,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) { throw std::invalid_argument("Pid is called with negative dt"); } + // Get the gain parameters from the realtime box auto gains_opt = gains_box_.try_get(); if (gains_opt.has_value()) @@ -361,9 +360,10 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) const bool is_error_in_deadband_zone = control_toolbox::is_zero(error, gains_.antiwindup_strat_.error_deadband); + // Calculate integral contribution to command if (gains_.i_method_ == "forward_euler") { - i_term_ = i_term_last_ + gains_.i_gain_ * dt_s * error_last_; + i_term_ = i_term_last_ + gains_.i_gain_ * dt_s * p_error_last_; } else if (gains_.i_method_ == "backward_euler") { @@ -371,25 +371,34 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) } else if (gains_.i_method_ == "trapezoidal") { - i_term_ = i_term_last_ + gains_.i_gain_ * (dt_s * 0.5) * (error + error_last_); + i_term_ = i_term_last_ + gains_.i_gain_ * (dt_s * 0.5) * (error + p_error_last_); } else { throw std::runtime_error("Pid: invalid integral method"); } + // Anti-windup via conditional integration if (gains_.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION) { - if (!is_zero(aw_term_last_ - i_term_last_)) + if (!is_zero(cmd_ - cmd_unsat_)) { - double dI = i_term_ - i_term_last_; - bool sat_high = (cmd_ == gains_.u_max_); - bool sat_low = (cmd_ == gains_.u_min_); + // If we are in saturation, don't integrate if it would drive the controller further into + // saturation. + + double dI = i_term_ - i_term_last_; // The variation of the integral term + bool sat_high = (cmd_ == gains_.u_max_); // Check if saturation at upper bound + bool sat_low = (cmd_ == gains_.u_min_); // Check if saturation at lower bound + + // If we are saturated at the upper limit and the variation of the integral term is decreasing + // or if we are saturated at the lower limit and the variation of the integral term is + // increasing, add the variation to the last integral term. bool move_saturation = (sat_high && dI < 0) || (sat_low && dI > 0); i_term_ = move_saturation ? i_term_ : i_term_last_; } } + // Clamp the i_term_. Notice that this clamp occurs before the back-calculation technique. i_term_ = std::clamp(i_term_, gains_.i_min_, gains_.i_max_); // Compute the command @@ -411,6 +420,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) { cmd_ = cmd_unsat_; } + if (!is_error_in_deadband_zone) { if ( @@ -433,20 +443,22 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) { const double num_i_last = (1.0 - dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant)) * i_term_last_; - const double trap_inc = gains_.i_gain_ * (dt_s * 0.5) * (p_error_ + error_last_); + const double trap_inc = gains_.i_gain_ * (dt_s * 0.5) * (p_error_ + p_error_last_); const double aw_sum = (cmd_ - (p_term + d_term)) + aw_term_last_; const double denom = (1.0 + dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant)); + i_term_ = (num_i_last + trap_inc + (dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant)) * aw_sum) / denom; } } + // Update last values for next iteration d_term_last_ = d_term; d_error_last_ = d_error_; i_term_last_ = i_term_; aw_term_last_ = cmd_ - p_term - d_term; - error_last_ = error; + p_error_last_ = error; return cmd_; } diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 9a3f4af5..eaa1a4d8 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -644,6 +644,53 @@ TEST(CommandTest, derivativeOnlyTest) EXPECT_EQ(0.5, cmd); } +TEST(CommandTest, derivativeFilteredForwardTest) +{ + RecordProperty( + "description", + "This test verifies that a command is computed correctly using only the filtered derivative " + "contribution with its own differentiation (NOTE: this test depends on the differentiation " + "scheme)."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::NONE; + // Set only derivative gain and derivative filter time (tf) + Pid pid(0.0, 0.0, 1.0, 2.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "forward_euler"); + double cmd = 0.0; + + // If initial error = 0, d-gain = 1, tf = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_NEAR(-0.25, cmd, EPS); + + // If call again with same error + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_NEAR(-0.125, cmd, EPS); + + // If call again with same error and smaller control period + cmd = pid.compute_command(-0.5, 0.1); + EXPECT_NEAR(-0.11875, cmd, EPS); + + // If the error becomes more negative, with dt = 1 + cmd = pid.compute_command(-1.0, 1.0); + EXPECT_NEAR(-0.309375, cmd, EPS); + + // If the error becomes more negative, with dt = 1 + cmd = pid.compute_command(-2.090625, 1.0); + EXPECT_NEAR(-0.7, cmd, EPS); + + // If error increases, with dt = 1 + cmd = pid.compute_command(0.0, 1.0); + EXPECT_NEAR(0.6953125, cmd, EPS); + + // If error increases, with dt = 1 + cmd = pid.compute_command(3.0, 1.0); + EXPECT_NEAR(1.84765625, cmd, EPS); + + // If error increases, with dt = 1 + cmd = pid.compute_command(5.15234375, 1.0); + EXPECT_NEAR(2, cmd, EPS); +} + TEST(CommandTest, completePIDTest) { RecordProperty( From 2463c65b9855d0114a8dcdf42b62dfd1985f9c99 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Thu, 18 Sep 2025 23:00:29 +0000 Subject: [PATCH 05/12] New tests for derivative filter --- control_toolbox/test/pid_tests.cpp | 47 ++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index eaa1a4d8..8c36bb7a 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -691,6 +691,53 @@ TEST(CommandTest, derivativeFilteredForwardTest) EXPECT_NEAR(2, cmd, EPS); } +TEST(CommandTest, derivativeFilteredBackwardTest) +{ + RecordProperty( + "description", + "This test verifies that a command is computed correctly using only the filtered derivative " + "contribution with its own differentiation (NOTE: this test depends on the differentiation " + "scheme)."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::NONE; + // Set only derivative gain and derivative filter time (tf) + Pid pid(0.0, 0.0, 1.0, 2.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "backward_euler"); + double cmd = 0.0; + + // If initial error = 0, d-gain = 1, tf = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_NEAR(-1.0 / 6.0, cmd, EPS); // -0,1666666666 + + // If call again with same error + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_NEAR(-1.0 / 9.0, cmd, EPS); // -0,1111111111 + + // If call again with same error and smaller control period + cmd = pid.compute_command(-0.5, 0.1); + EXPECT_NEAR(-20.0 / 189.0, cmd, EPS); // -0,1058201058 + + // If the error becomes more negative, with dt = 1 + cmd = pid.compute_command(-1.0, 1.0); + EXPECT_NEAR(-269.0 / 1134.0, cmd, EPS); // -0,2372134038 + + // If the error becomes more negative, with dt = 1 + cmd = pid.compute_command(-2.0, 1.0); + EXPECT_NEAR(-836.0 / 1701.0, cmd, EPS); // -0,4914756025 + + // If error increases, with dt = 1 + cmd = pid.compute_command(0.0, 1.0); + EXPECT_NEAR(1730.0 / 5103.0, cmd, EPS); // 0,3390162649 + + // If error increases, with dt = 1 + cmd = pid.compute_command(3.0, 1.0); + EXPECT_NEAR(18769.0 / 15309.0, cmd, EPS); // 1,2260108432 + + // If error increases, with dt = 1 + cmd = pid.compute_command(100243.0 / 15309.0, 1.0); // 6,5479783134 + EXPECT_NEAR(2, cmd, EPS); +} + TEST(CommandTest, completePIDTest) { RecordProperty( From 9ec502f19e24eabbefc28f8f4b92c323c43feb44 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Fri, 19 Sep 2025 11:54:07 +0000 Subject: [PATCH 06/12] Add new derivative tests --- control_toolbox/test/pid_tests.cpp | 151 ++++++++++++++++++++++++++--- 1 file changed, 139 insertions(+), 12 deletions(-) diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 8c36bb7a..f488acd0 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -604,7 +604,7 @@ TEST(CommandTest, integralOnlyTest) EXPECT_EQ(-0.5, cmd); } -TEST(CommandTest, derivativeOnlyTest) +TEST(CommandTest, derivativeOnlyForwardTest) { RecordProperty( "description", @@ -615,7 +615,7 @@ TEST(CommandTest, derivativeOnlyTest) antiwindup_strat.type = AntiWindupStrategy::NONE; // Set only derivative gain - Pid pid(0.0, 0.0, 1.0, 10.0, -10.0, antiwindup_strat); + Pid pid(0.0, 0.0, 1.0, 0.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "forward_euler"); double cmd = 0.0; // If initial error = 0, d-gain = 1, dt = 1 @@ -633,17 +633,97 @@ TEST(CommandTest, derivativeOnlyTest) // Then expect command = 0 again EXPECT_EQ(0.0, cmd); - // If the error increases, with dt = 1 + // If the error increases in module, with dt = 1 cmd = pid.compute_command(-1.0, 1.0); // Then expect the command = change in dt EXPECT_EQ(-0.5, cmd); - // If error decreases, with dt = 1 + // If error decreases in module, with dt = 1 cmd = pid.compute_command(-0.5, 1.0); // Then expect always the command = change in dt (note the sign flip) EXPECT_EQ(0.5, cmd); } +TEST(CommandTest, derivativeOnlyBackwardTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using the derivative contribution only " + "with own differentiation (ATTENTION: this test depends on the differentiation scheme)."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::NONE; + + // Set only derivative gain + Pid pid(0.0, 0.0, 1.0, 0.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "backward_euler"); + double cmd = 0.0; + + // If initial error = 0, d-gain = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + // Then expect command = error + EXPECT_EQ(-0.5, cmd); + + // If call again with same error + cmd = pid.compute_command(-0.5, 1.0); + // Then expect command = 0 due to no variation on error + EXPECT_EQ(0.0, cmd); + + // If call again with same error and smaller control period + cmd = pid.compute_command(-0.5, 0.1); + // Then expect command = 0 again + EXPECT_EQ(0.0, cmd); + + // If the error increases in module, with dt = 1 + cmd = pid.compute_command(-1.0, 1.0); + // Then expect the command = change in dt + EXPECT_EQ(-0.5, cmd); + + // If error decreases in module, with dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + // Then expect always the command = change in dt (note the sign flip) + EXPECT_EQ(0.5, cmd); +} + +TEST(CommandTest, derivativeOnlyTrapezoidalTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using the derivative contribution only " + "with own differentiation (ATTENTION: this test depends on the differentiation scheme)."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::NONE; + + // Set only derivative gain + Pid pid(0.0, 0.0, 1.0, 0.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "trapezoidal"); + double cmd = 0.0; + + // If initial error = 0, d-gain = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + // Then expect command = error + EXPECT_EQ(-1.0, cmd); + + // If call again with same error + cmd = pid.compute_command(-0.5, 1.0); + // Then expect command = 0 due to no variation on error + EXPECT_EQ(1.0, cmd); + + // If call again with same error and smaller control period + cmd = pid.compute_command(-0.5, 0.1); + // Then expect command = 0 again + EXPECT_EQ(-1.0, cmd); + + // If the error increases in module, with dt = 1 + cmd = pid.compute_command(-1.0, 1.0); + // Then expect the command = change in dt + EXPECT_EQ(0.0, cmd); + + // If error decreases in module, with dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + // Then expect always the command = change in dt (note the sign flip) + EXPECT_EQ(1.0, cmd); +} + TEST(CommandTest, derivativeFilteredForwardTest) { RecordProperty( @@ -707,37 +787,84 @@ TEST(CommandTest, derivativeFilteredBackwardTest) // If initial error = 0, d-gain = 1, tf = 1, dt = 1 cmd = pid.compute_command(-0.5, 1.0); - EXPECT_NEAR(-1.0 / 6.0, cmd, EPS); // -0,1666666666 + EXPECT_NEAR(-1.0 / 6.0, cmd, EPS); // -0.1666666666 // If call again with same error cmd = pid.compute_command(-0.5, 1.0); - EXPECT_NEAR(-1.0 / 9.0, cmd, EPS); // -0,1111111111 + EXPECT_NEAR(-1.0 / 9.0, cmd, EPS); // -0.1111111111 // If call again with same error and smaller control period cmd = pid.compute_command(-0.5, 0.1); - EXPECT_NEAR(-20.0 / 189.0, cmd, EPS); // -0,1058201058 + EXPECT_NEAR(-20.0 / 189.0, cmd, EPS); // -0.1058201058 // If the error becomes more negative, with dt = 1 cmd = pid.compute_command(-1.0, 1.0); - EXPECT_NEAR(-269.0 / 1134.0, cmd, EPS); // -0,2372134038 + EXPECT_NEAR(-269.0 / 1134.0, cmd, EPS); // -0.2372134038 // If the error becomes more negative, with dt = 1 cmd = pid.compute_command(-2.0, 1.0); - EXPECT_NEAR(-836.0 / 1701.0, cmd, EPS); // -0,4914756025 + EXPECT_NEAR(-836.0 / 1701.0, cmd, EPS); // -0.4914756025 // If error increases, with dt = 1 cmd = pid.compute_command(0.0, 1.0); - EXPECT_NEAR(1730.0 / 5103.0, cmd, EPS); // 0,3390162649 + EXPECT_NEAR(1730.0 / 5103.0, cmd, EPS); // 0.3390162649 // If error increases, with dt = 1 cmd = pid.compute_command(3.0, 1.0); - EXPECT_NEAR(18769.0 / 15309.0, cmd, EPS); // 1,2260108432 + EXPECT_NEAR(18769.0 / 15309.0, cmd, EPS); // 1.2260108432 // If error increases, with dt = 1 - cmd = pid.compute_command(100243.0 / 15309.0, 1.0); // 6,5479783134 + cmd = pid.compute_command(100243.0 / 15309.0, 1.0); // 6.5479783134 EXPECT_NEAR(2, cmd, EPS); } +TEST(CommandTest, derivativeFilteredTrapezoidalTest) +{ + RecordProperty( + "description", + "This test verifies that a command is computed correctly using only the filtered derivative " + "contribution with its own differentiation (NOTE: this test depends on the differentiation " + "scheme)."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::NONE; + // Set only derivative gain and derivative filter time (tf) + Pid pid(0.0, 0.0, 1.0, 2.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "trapezoidal"); + double cmd = 0.0; + + // If initial error = 0, d-gain = 1, tf = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_NEAR(-1.0 / 5.0, cmd, EPS); // -0.2 + + // If call again with same error + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_NEAR(-3.0 / 25.0, cmd, EPS); // -0.12 + + // If call again with same error and smaller control period + cmd = pid.compute_command(-0.5, 0.1); + EXPECT_NEAR(-117.0 / 1025.0, cmd, EPS); // -0.114146341 + + // If the error becomes more negative, with dt = 1 + cmd = pid.compute_command(-1.0, 1.0); + EXPECT_NEAR(-1376.0 / 5125.0, cmd, EPS); // -0.268487805 + + // If the error becomes more negative, with dt = 1 + cmd = pid.compute_command(-2.0, 1.0); + EXPECT_NEAR(-14378.0 / 25625.0, cmd, EPS); // -0.561092683 + + // If error increases, with dt = 1 + cmd = pid.compute_command(0.0, 1.0); + EXPECT_NEAR(59366.0 / 128125.0, cmd, EPS); // 0.46334439 + + // If error increases, with dt = 1 + cmd = pid.compute_command(3.0, 1.0); + EXPECT_NEAR(946848.0 / 640625.0, cmd, EPS); // 1.478006634 + + // If error increases, with dt = 1 + cmd = pid.compute_command(4206331.0 / 1281250.0, 1.0); // 3.282990049 + EXPECT_NEAR(1, cmd, EPS); +} + TEST(CommandTest, completePIDTest) { RecordProperty( From 5c473e3f3d7f9e89ff0ffdbe0421386fb2899891 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Sat, 20 Sep 2025 04:13:58 +0000 Subject: [PATCH 07/12] Add integral tests --- control_toolbox/test/pid_tests.cpp | 134 +++++++++++++++++++++++++++++ 1 file changed, 134 insertions(+) diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index f488acd0..39f9f92d 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -604,6 +604,140 @@ TEST(CommandTest, integralOnlyTest) EXPECT_EQ(-0.5, cmd); } +TEST(CommandTest, integralOnlyBackwardTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using the integral contribution only " + "with backward euler discretization (ATTENTION: this test depends on the integration scheme)."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::NONE; + + // Set only integral gains with enough limits to test behavior + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "backward_euler", "forward_euler"); + double cmd = 0.0; + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + // Then expect command = -0.5 + EXPECT_EQ(-0.5, cmd); + + // If call again with same arguments + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-1.0, cmd); + + // Call again with no error + cmd = pid.compute_command(0.0, 1.0); + EXPECT_EQ(-1.0, cmd); + + // Check that the integral contribution keep the previous command + cmd = pid.compute_command(0.0, 1.0); + EXPECT_EQ(-1.0, cmd); + + // Finally call again with positive error to see if the command changes in the opposite direction + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(0.0, cmd); + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(1.0, cmd); + + // after reset without argument (save_i_term=false) + // we expect the command to be 0 if update is called error = 0 + pid.reset(); + cmd = pid.compute_command(0.5, 1.0); + EXPECT_EQ(0.5, cmd); + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(0.0, 1.0); + // Then expect command = 0.5 + EXPECT_EQ(0.5, cmd); + + // after reset with argument (save_i_term=false) + pid.reset(false); + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-0.5, cmd); + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(0.0, 1.0); + // Then expect command = -0.5 + EXPECT_EQ(-0.5, cmd); + // after reset with save_i_term=true + // we expect still the same command if update is called error = 0 + pid.reset(true); + cmd = pid.compute_command(0.0, 1.0); + // Then expect command = -0.5 + EXPECT_EQ(-0.5, cmd); +} + +TEST(CommandTest, integralOnlyTrapezoidalTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using the integral contribution only " + "with trapezoidal discretization (ATTENTION: this test depends on the integration scheme)."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::NONE; + + // Set only integral gains with enough limits to test behavior + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "trapezoidal", "forward_euler"); + double cmd = 0.0; + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + // Since trapezoidal integration is used, we expect the mean value of the error over + // this and the last period, which is -0.25 + EXPECT_EQ(-0.25, cmd); + + // If call again with same arguments + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-0.75, cmd); + + // Call again with no error + cmd = pid.compute_command(0.0, 1.0); + EXPECT_EQ(-1.0, cmd); + + // Check that the integral contribution keep the previous command + cmd = pid.compute_command(0.0, 1.0); + EXPECT_EQ(-1.0, cmd); + + // Finally call again with positive error to see if the command changes in the opposite direction + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(-0.5, cmd); + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(0.5, cmd); + + // after reset without argument (save_i_term=false) + pid.reset(); + cmd = pid.compute_command(0.5, 1.0); + EXPECT_EQ(0.25, cmd); + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(0.0, 1.0); + // Then expect command = 0.5 + EXPECT_EQ(0.5, cmd); + + // after reset with argument (save_i_term=false) + pid.reset(false); + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-0.25, cmd); + + cmd = pid.compute_command(0.0, 1.0); + // Then expect command = -0.5 + EXPECT_EQ(-0.5, cmd); + + // after reset with save_i_term=true + // we expect still the same command if update is called error = 0 + pid.reset(true); + cmd = pid.compute_command(0.0, 1.0); + // Then expect command = -0.5 + EXPECT_EQ(-0.5, cmd); +} + TEST(CommandTest, derivativeOnlyForwardTest) { RecordProperty( From b712b0bbe4cd248367fddd166174e127a6440288 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Sat, 20 Sep 2025 16:54:02 +0000 Subject: [PATCH 08/12] Add new antiwindup trapezoidal test --- control_toolbox/src/pid.cpp | 10 +-- control_toolbox/test/pid_tests.cpp | 140 ++++++++++++++++++++++++++++- 2 files changed, 140 insertions(+), 10 deletions(-) diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index c3202c28..fbdb97f7 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -441,15 +441,13 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) gains_.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && !is_zero(gains_.i_gain_) && gains_.i_method_ == "trapezoidal") { - const double num_i_last = - (1.0 - dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant)) * i_term_last_; + const double alpha = dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant); + const double num_i_last = (1.0 - alpha) * i_term_last_; const double trap_inc = gains_.i_gain_ * (dt_s * 0.5) * (p_error_ + p_error_last_); const double aw_sum = (cmd_ - (p_term + d_term)) + aw_term_last_; - const double denom = (1.0 + dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant)); + const double denom = (1.0 + alpha); - i_term_ = (num_i_last + trap_inc + - (dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant)) * aw_sum) / - denom; + i_term_ = (num_i_last + trap_inc + alpha * aw_sum) / denom; } } diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 39f9f92d..5e5c9f54 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -1031,12 +1031,12 @@ TEST(CommandTest, completePIDTest) EXPECT_EQ(-0.5, cmd); } -TEST(CommandTest, backCalculationPIDTest) +TEST(CommandTest, backCalculationForwardPIDTest) { RecordProperty( "description", - "This test checks that a command is computed correctly using a complete PID controller with " - "back calculation technique."); + "This test checks that a command is computed correctly using a PID controller with " + "back calculation technique and forward discretization."); // Pid(double p, double i, double d, double u_max, double u_min, // AntiWindupStrategy antiwindup_strat); @@ -1046,7 +1046,7 @@ TEST(CommandTest, backCalculationPIDTest) antiwindup_strat.i_max = 10.0; antiwindup_strat.i_min = -10.0; 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); + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); double cmd = 0.0; double pe, ie, de; @@ -1096,6 +1096,138 @@ TEST(CommandTest, backCalculationPIDTest) EXPECT_EQ(4.0, cmd); } +TEST(CommandTest, backCalculationBackwardPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a PID controller with " + "back calculation technique and backward discretization."); + + // 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.i_max = 10.0; + antiwindup_strat.i_min = -10.0; + antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "backward_euler", "forward_euler"); + + 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(1.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(3.0, cmd); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.5, ie); // Reduced from 6.0 (1.0 + 2.0 + 3.0) to 5.5 due to back-calculation + 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(5.75, ie); // Reduced from 6.5 (5.5 + 1.0) to 5.75 due to back-calculation + 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(6.375, ie); // Reduced from 7.75 (5.75 + 2.0) to 6.375 due to back-calculation + 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(5.1875, ie); // Reduced from 5.375 (6.375 - 1.0) to 5.1875 due to back-calculation + 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(4.1875, ie); + EXPECT_EQ(4.1875, cmd); +} + +TEST(CommandTest, backCalculationTrapezoidalPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a PID controller with " + "back calculation technique and trapezoidal discretization."); + + // 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.i_max = 10.0; + antiwindup_strat.i_min = -10.0; + antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "trapezoidal", "forward_euler"); + + 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_NEAR(0.5, ie, EPS); + EXPECT_NEAR(0.5, cmd, EPS); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_NEAR(2.0, ie, EPS); + EXPECT_NEAR(2.0, cmd, EPS); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_NEAR(4.5, ie, EPS); + EXPECT_NEAR(4.5, cmd, EPS); + + // Error to cause saturation + cmd = pid.compute_command(5.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_NEAR(22.0 / 3.0, ie, EPS); // 7.33... + EXPECT_NEAR(5.0, cmd, EPS); + + // 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_NEAR(70.0 / 9.0, ie, EPS); // 7.77... + EXPECT_NEAR(5.0, cmd, EPS); + + // 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_NEAR(187.0 / 27.0, ie, EPS); // 6.92592592 + EXPECT_NEAR(5.0, cmd, EPS); + + // 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_NEAR(457.0 / 81.0, ie, EPS); // 5.12962963 + EXPECT_NEAR(5.0, cmd, EPS); + + // PID recover from the windup/saturation + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_NEAR(1909.0 / 486.0, ie, EPS); // 3.92798353 + EXPECT_NEAR(671.0 / 162.0, cmd, EPS); // 4.14197531 +} + TEST(CommandTest, conditionalIntegrationPIDTest) { RecordProperty( From 66f09df428cb17d1a703162911bb9ea6dd2e261e Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Sat, 20 Sep 2025 18:37:47 +0000 Subject: [PATCH 09/12] Add CI antiwindup tests --- control_toolbox/test/pid_tests.cpp | 142 ++++++++++++++++++++++++++++- 1 file changed, 140 insertions(+), 2 deletions(-) diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 5e5c9f54..4fd1af3e 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -1228,12 +1228,12 @@ TEST(CommandTest, backCalculationTrapezoidalPIDTest) EXPECT_NEAR(671.0 / 162.0, cmd, EPS); // 4.14197531 } -TEST(CommandTest, conditionalIntegrationPIDTest) +TEST(CommandTest, conditionalIntegrationForwardPIDTest) { RecordProperty( "description", "This test checks that a command is computed correctly using a complete PID controller with " - "conditional integration technique."); + "conditional integration technique and forward euler discretization."); // Pid(double p, double i, double d, double u_max, double u_min, // AntiWindupStrategy antiwindup_strat); @@ -1293,6 +1293,144 @@ TEST(CommandTest, conditionalIntegrationPIDTest) EXPECT_EQ(5.0, cmd); } +TEST(CommandTest, conditionalIntegrationBackwardPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "conditional integration technique and backward euler discretization."); + + // 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.i_max = 10.0; + antiwindup_strat.i_min = -10.0; + antiwindup_strat.tracking_time_constant = 1.0; + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "backward_euler", "forward_euler"); + + 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(1.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(3.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(5.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.5, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.5, ie); + EXPECT_EQ(4.5, 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(4.5, ie); + EXPECT_EQ(4.5, cmd); +} + +TEST(CommandTest, conditionalIntegrationTrapezoidalPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "conditional integration technique and trapezoidal discretization."); + + // 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.i_max = 10.0; + antiwindup_strat.i_min = -10.0; + antiwindup_strat.tracking_time_constant = 1.0; + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "trapezoidal", "forward_euler"); + + 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(0.5, ie); + EXPECT_EQ(0.5, 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(2.0, ie); + EXPECT_EQ(2.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.5, ie); + EXPECT_EQ(4.5, cmd); + + // Error to cause saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.5, ie); + EXPECT_EQ(5.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.5, 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.5, ie); + EXPECT_EQ(5.0, cmd); + + // PID recovering from the windup/saturation + cmd = pid.compute_command(-2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.5, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(-2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.5, ie); + EXPECT_EQ(4.5, 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(3.5, ie); + EXPECT_EQ(3.5, cmd); +} + TEST(CommandTest, timeArgumentTest) { RecordProperty("description", "Tests different dt argument type methods."); From 98de888624240dc9f4b88104c422f7cf2cbb64f8 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Sat, 20 Sep 2025 18:55:28 +0000 Subject: [PATCH 10/12] Fix old tests for new parameters --- control_toolbox/test/pid_tests.cpp | 103 ++++++++++++++--------------- 1 file changed, 51 insertions(+), 52 deletions(-) diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 4fd1af3e..79156617 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -48,13 +48,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 u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; 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); + EXPECT_THROW( + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat, "forward_euler", "forward_euler"), + std::invalid_argument); } TEST(ParameterTest, UTermBadIBoundsTest) @@ -63,18 +63,17 @@ 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 u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; 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); + Pid pid(1.0, 1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat, "forward_euler", "forward_euler"); 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, antiwindup_strat)); + EXPECT_NO_THROW(pid.set_gains( + 1.0, 1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat, "forward_euler", "forward_euler")); // 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); @@ -85,13 +84,11 @@ 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 u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; 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, 1.0, -1.0, antiwindup_strat); + Pid pid(1.0, 0.0, 0.0, 1.0, 1.0, -1.0, antiwindup_strat, "forward_euler", "forward_euler"); double cmd = 0.0; @@ -141,15 +138,13 @@ 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 u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; 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, std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), antiwindup_strat); + 1.0, 0.0, 0.0, 1.0, std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), antiwindup_strat, "forward_euler", "forward_euler"); double cmd = 0.0; @@ -201,14 +196,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 u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; 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); + Pid pid(0.0, 0.0, 0.0, 1.0, 20.0, -20.0, antiwindup_strat, "forward_euler", "forward_euler"); double cmd = 0.0; double pe, ie, de; @@ -253,13 +246,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 u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; - Pid pid(0.0, 0.0, 0.0, 20.0, -20.0, antiwindup_strat); + Pid pid(0.0, 0.0, 0.0, 1.0, 20.0, -20.0, antiwindup_strat, "forward_euler", "forward_euler"); double cmd = 0.0; double pe, ie, de; @@ -308,14 +299,15 @@ TEST(ParameterTest, ITermBadIBoundsTest) 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); + Pid pid(1.0, 1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat, "forward_euler", "forward_euler"); auto gains = pid.get_gains(); EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_max, 1.0); EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_min, -1.0); // Try to set bad i-bounds, i.e. i_min > i_max antiwindup_strat.i_max = -1.0; antiwindup_strat.i_min = 1.0; - EXPECT_NO_THROW(pid.set_gains(1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat)); + EXPECT_NO_THROW(pid.set_gains( + 1.0, 1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat, "forward_euler", "forward_euler")); // Check if gains were not updated because i-bounds are bad, i.e. i_min > i_max EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_max, 1.0); EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_min, -1.0); @@ -338,7 +330,7 @@ TEST(ParameterTest, integrationAntiwindupTest) const double u_max = std::numeric_limits::infinity(); const double u_min = -std::numeric_limits::infinity(); - Pid pid(0.0, i_gain, 0.0, u_max, u_min, antiwindup_strat); + Pid pid(0.0, i_gain, 0.0, 1.0, u_max, u_min, antiwindup_strat, "forward_euler", "forward_euler"); double cmd = 0.0; @@ -366,6 +358,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double p_gain = std::rand() % 100; double i_gain = std::rand() % 100; double d_gain = std::rand() % 100; + double tf = std::rand() % 100; double i_max = std::rand() % 100; double i_min = -1 * std::rand() % 100; double u_max = std::numeric_limits::infinity(); @@ -376,27 +369,33 @@ TEST(ParameterTest, gainSettingCopyPIDTest) antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.tracking_time_constant = tracking_time_constant; + std::string i_method = "forward_euler"; + std::string d_method = "forward_euler"; // Initialize the default way - Pid pid1(p_gain, i_gain, d_gain, u_max, u_min, antiwindup_strat); + Pid pid1(p_gain, i_gain, d_gain, tf, u_max, u_min, antiwindup_strat, i_method, d_method); // Test return values ------------------------------------------------- - double p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return; + double p_gain_return, i_gain_return, d_gain_return, tf_return, u_max_return, u_min_return; AntiWindupStrategy antiwindup_strat_return; + std::string i_method_return, d_method_return; pid1.get_gains( - p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, - antiwindup_strat_return); + p_gain_return, i_gain_return, d_gain_return, tf_return, u_max_return, u_min_return, + antiwindup_strat_return, i_method_return, d_method_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); EXPECT_EQ(d_gain, d_gain_return); + EXPECT_EQ(tf, tf_return); EXPECT_EQ(u_max, u_max_return); EXPECT_EQ(u_min, u_min_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_strat.to_string(), antiwindup_strat_return.to_string()); + EXPECT_EQ(i_method, i_method_return); + EXPECT_EQ(d_method, d_method_return); // Test return values using struct ------------------------------------------------- @@ -404,6 +403,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) p_gain = std::rand() % 100; i_gain = std::rand() % 100; d_gain = std::rand() % 100; + tf = std::rand() % 100; i_max = std::rand() % 100; i_min = -1 * std::rand() % 100; u_max = std::numeric_limits::infinity(); @@ -413,13 +413,16 @@ TEST(ParameterTest, gainSettingCopyPIDTest) antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.tracking_time_constant = tracking_time_constant; + i_method = "backward_euler"; + d_method = "backward_euler"; - pid1.set_gains(p_gain, i_gain, d_gain, u_max, u_min, antiwindup_strat); + pid1.set_gains(p_gain, i_gain, d_gain, tf, u_max, u_min, antiwindup_strat, i_method, d_method); Pid::Gains g1 = pid1.get_gains(); EXPECT_EQ(p_gain, g1.p_gain_); EXPECT_EQ(i_gain, g1.i_gain_); EXPECT_EQ(d_gain, g1.d_gain_); + EXPECT_EQ(tf, g1.tf_); EXPECT_EQ(i_max, g1.i_max_); EXPECT_EQ(i_min, g1.i_min_); EXPECT_EQ(u_max, g1.u_max_); @@ -428,6 +431,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max); EXPECT_EQ(i_min, g1.antiwindup_strat_.i_min); EXPECT_EQ(antiwindup_strat.to_string(), g1.antiwindup_strat_.to_string()); + EXPECT_EQ(i_method, g1.i_method_); + EXPECT_EQ(d_method, g1.d_method_); // Send update command to populate errors ------------------------------------------------- pid1.set_current_cmd(10); @@ -437,12 +442,13 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid2(pid1); pid2.get_gains( - p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, - antiwindup_strat_return); + p_gain_return, i_gain_return, d_gain_return, tf_return, u_max_return, u_min_return, + antiwindup_strat_return, i_method_return, d_method_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(tf_return, g1.tf_); 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_); @@ -451,6 +457,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) 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_strat.to_string(), g1.antiwindup_strat_.to_string()); + EXPECT_EQ(i_method_return, g1.i_method_); + EXPECT_EQ(d_method_return, g1.d_method_); // Test that errors are zero double pe2, ie2, de2; @@ -464,12 +472,13 @@ TEST(ParameterTest, gainSettingCopyPIDTest) pid3 = pid1; pid3.get_gains( - p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, - antiwindup_strat_return); + p_gain_return, i_gain_return, d_gain_return, tf_return, u_max_return, u_min_return, + antiwindup_strat_return, i_method_return, d_method_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(tf_return, g1.tf_); 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_); @@ -478,6 +487,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) 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_strat.to_string(), g1.antiwindup_strat_.to_string()); + EXPECT_EQ(i_method_return, g1.i_method_); + EXPECT_EQ(d_method_return, g1.d_method_); // Test that errors are zero double pe3, ie3, de3; @@ -510,7 +521,7 @@ TEST(CommandTest, proportionalOnlyTest) antiwindup_strat.type = AntiWindupStrategy::NONE; // Set only proportional gain - Pid pid(1.0, 0.0, 0.0, 10.0, -10.0, antiwindup_strat); + Pid pid(1.0, 0.0, 0.0, 1.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "forward_euler"); double cmd = 0.0; // If initial error = 0, p-gain = 1, dt = 1 @@ -545,7 +556,7 @@ TEST(CommandTest, integralOnlyTest) antiwindup_strat.type = AntiWindupStrategy::NONE; // Set only integral gains with enough limits to test behavior - Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); + Pid pid(0.0, 1.0, 0.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); double cmd = 0.0; // If initial error = 0, i-gain = 1, dt = 1 @@ -1011,7 +1022,7 @@ TEST(CommandTest, completePIDTest) antiwindup_strat.i_max = 10.0; antiwindup_strat.i_min = -10.0; - Pid pid(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat); + Pid pid(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); double cmd = 0.0; // All contributions are tested, here few tests check that they sum up correctly @@ -1038,8 +1049,6 @@ TEST(CommandTest, backCalculationForwardPIDTest) "This test checks that a command is computed correctly using a PID controller with " "back calculation technique and forward discretization."); - // 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; @@ -1103,8 +1112,6 @@ TEST(CommandTest, backCalculationBackwardPIDTest) "This test checks that a command is computed correctly using a PID controller with " "back calculation technique and backward discretization."); - // 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; @@ -1166,8 +1173,6 @@ TEST(CommandTest, backCalculationTrapezoidalPIDTest) "This test checks that a command is computed correctly using a PID controller with " "back calculation technique and trapezoidal discretization."); - // 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; @@ -1235,15 +1240,13 @@ TEST(CommandTest, conditionalIntegrationForwardPIDTest) "This test checks that a command is computed correctly using a complete PID controller with " "conditional integration technique and forward euler discretization."); - // 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.i_max = 10.0; antiwindup_strat.i_min = -10.0; antiwindup_strat.tracking_time_constant = 1.0; - Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); + Pid pid(0.0, 1.0, 0.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); double cmd = 0.0; double pe, ie, de; @@ -1300,8 +1303,6 @@ TEST(CommandTest, conditionalIntegrationBackwardPIDTest) "This test checks that a command is computed correctly using a complete PID controller with " "conditional integration technique and backward euler discretization."); - // 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; @@ -1363,8 +1364,6 @@ TEST(CommandTest, conditionalIntegrationTrapezoidalPIDTest) "This test checks that a command is computed correctly using a complete PID controller with " "conditional integration technique and trapezoidal discretization."); - // 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; @@ -1441,10 +1440,10 @@ TEST(CommandTest, timeArgumentTest) antiwindup_strat.i_min = -10.0; antiwindup_strat.tracking_time_constant = 1.0; - Pid pid1(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat); - Pid pid2(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat); - Pid pid3(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat); - Pid pid4(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat); + Pid pid1(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); + Pid pid2(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); + Pid pid3(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); + Pid pid4(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); // call without error_dt, dt is used to calculate error_dt auto cmd1 = pid1.compute_command(-0.5, 1.0); From 038fca8539b77fe674974caea66af743b24742f7 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Sat, 20 Sep 2025 19:29:35 +0000 Subject: [PATCH 11/12] Fix old tests --- .../test/pid_ros_parameters_tests.cpp | 131 ++++++++++++++++-- .../test/pid_ros_publisher_tests.cpp | 15 +- 2 files changed, 128 insertions(+), 18 deletions(-) diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index a1c661bb..11180559 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -49,6 +49,7 @@ void check_set_parameters( const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 0.5; const double I_MAX = 10.0; const double I_MIN = -10.0; const double U_MAX = 10.0; @@ -60,9 +61,12 @@ void check_set_parameters( ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + std::string I_METHOD = "forward_euler"; + std::string D_METHOD = "forward_euler"; const bool SAVE_I_TERM = true; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, SAVE_I_TERM)); + ASSERT_NO_THROW(pid.initialize_from_args( + P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, SAVE_I_TERM)); rclcpp::Parameter param; @@ -76,6 +80,9 @@ void check_set_parameters( ASSERT_TRUE(node->get_parameter(prefix + "d", param)); ASSERT_EQ(param.get_value(), D); + ASSERT_TRUE(node->get_parameter(prefix + "derivative_filter_time", param)); + ASSERT_EQ(param.get_value(), TF); + ASSERT_TRUE(node->get_parameter(prefix + "i_clamp_max", param)); ASSERT_EQ(param.get_value(), I_MAX); @@ -97,6 +104,12 @@ void check_set_parameters( ASSERT_TRUE(node->get_parameter(prefix + "antiwindup_strategy", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); + ASSERT_TRUE(node->get_parameter(prefix + "integration_method", param)); + ASSERT_EQ(param.get_value(), I_METHOD); + + ASSERT_TRUE(node->get_parameter(prefix + "derivative_method", param)); + ASSERT_EQ(param.get_value(), D_METHOD); + ASSERT_TRUE(node->get_parameter(prefix + "save_i_term", param)); ASSERT_EQ(param.get_value(), SAVE_I_TERM); @@ -105,12 +118,15 @@ void check_set_parameters( ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); + ASSERT_EQ(gains.tf_, TF); 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.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); + ASSERT_EQ(gains.i_method_, I_METHOD); + ASSERT_EQ(gains.d_method_, D_METHOD); } TEST(PidParametersTest, InitPid_no_prefix) @@ -134,6 +150,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX_BAD = -10.0; const double I_MIN_BAD = 10.0; const double U_MAX_BAD = -10.0; @@ -146,7 +163,11 @@ TEST(PidParametersTest, InitPidTestBadParameter) ANTIWINDUP_STRAT.i_min = I_MIN_BAD; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); + std::string I_METHOD = "forward_euler"; + std::string D_METHOD = "forward_euler"; + + ASSERT_NO_THROW(pid.initialize_from_args( + P, I, D, TF, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); rclcpp::Parameter param; @@ -154,6 +175,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_FALSE(node->get_parameter("p", param)); ASSERT_FALSE(node->get_parameter("i", param)); ASSERT_FALSE(node->get_parameter("d", param)); + ASSERT_FALSE(node->get_parameter("derivative_filter_time", 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)); @@ -161,18 +183,23 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_FALSE(node->get_parameter("tracking_time_constant", param)); ASSERT_FALSE(node->get_parameter("saturation", param)); ASSERT_FALSE(node->get_parameter("antiwindup_strategy", param)); + ASSERT_FALSE(node->get_parameter("integration_method", param)); + ASSERT_FALSE(node->get_parameter("derivative_method", param)); // check gains were NOT set control_toolbox::Pid::Gains gains = pid.get_gains(); ASSERT_EQ(gains.p_gain_, 0.0); ASSERT_EQ(gains.i_gain_, 0.0); ASSERT_EQ(gains.d_gain_, 0.0); + ASSERT_EQ(gains.tf_, 0.0); ASSERT_EQ(gains.i_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.i_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, 0.0); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); + ASSERT_EQ(gains.i_method_, "forward_euler"); + ASSERT_EQ(gains.d_method_, "forward_euler"); } TEST(PidParametersTest, InitPid_param_prefix_only) @@ -229,6 +256,7 @@ TEST(PidParametersTest, SetParametersTest) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX = 10.0; const double I_MIN = -10.0; const double U_MAX = 10.0; @@ -240,9 +268,12 @@ TEST(PidParametersTest, SetParametersTest) ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + std::string I_METHOD = "forward_euler"; + std::string D_METHOD = "forward_euler"; const bool SAVE_I_TERM = false; - pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, SAVE_I_TERM); + pid.initialize_from_args( + P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, SAVE_I_TERM); rcl_interfaces::msg::SetParametersResult set_result; @@ -257,6 +288,9 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("d", D))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("derivative_filter_time", TF))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_max", I_MAX))); ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN))); @@ -273,6 +307,12 @@ TEST(PidParametersTest, SetParametersTest) 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("integration_method", I_METHOD))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("derivative_method", D_METHOD))); + 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); ASSERT_NO_THROW( @@ -287,12 +327,15 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); + ASSERT_EQ(gains.tf_, TF); 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.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); + ASSERT_EQ(gains.i_method_, I_METHOD); + ASSERT_EQ(gains.d_method_, D_METHOD); } TEST(PidParametersTest, SetBadParametersTest) @@ -304,6 +347,7 @@ TEST(PidParametersTest, SetBadParametersTest) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX = 10.0; const double I_MIN = -10.0; const double I_MAX_BAD = -20.0; @@ -321,7 +365,10 @@ TEST(PidParametersTest, SetBadParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false); + std::string I_METHOD = "forward_euler"; + std::string D_METHOD = "forward_euler"; + + pid.initialize_from_args(P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false); rcl_interfaces::msg::SetParametersResult set_result; @@ -336,6 +383,9 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("d", D))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("derivative_filter_time", TF))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_max", I_MAX_BAD))); ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN_BAD))); @@ -352,6 +402,12 @@ TEST(PidParametersTest, SetBadParametersTest) 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("integration_method", I_METHOD))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("derivative_method", D_METHOD))); + ASSERT_TRUE(set_result.successful); // process callbacks rclcpp::spin_some(node->get_node_base_interface()); @@ -362,12 +418,15 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); + ASSERT_EQ(gains.tf_, TF); ASSERT_EQ(gains.i_max_, I_MAX); 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.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); + ASSERT_EQ(gains.i_method_, I_METHOD); + ASSERT_EQ(gains.d_method_, D_METHOD); // Set the good gains @@ -384,12 +443,15 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); + ASSERT_EQ(gains.tf_, TF); ASSERT_EQ(gains.i_max_, I_MAX); 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.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); + ASSERT_EQ(gains.i_method_, I_METHOD); + ASSERT_EQ(gains.d_method_, D_METHOD); // Now re-enabling it should have the old gains back ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", true))); @@ -403,12 +465,15 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(updated_gains.p_gain_, P); ASSERT_EQ(updated_gains.i_gain_, I); ASSERT_EQ(updated_gains.d_gain_, D); + ASSERT_EQ(updated_gains.tf_, TF); ASSERT_EQ(updated_gains.i_max_, I_MAX); 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.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(updated_gains.antiwindup_strat_, AntiWindupStrategy::NONE); + ASSERT_EQ(updated_gains.i_method_, I_METHOD); + ASSERT_EQ(updated_gains.d_method_, D_METHOD); } TEST(PidParametersTest, GetParametersTest) @@ -421,6 +486,7 @@ TEST(PidParametersTest, GetParametersTest) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX = 10.0; const double I_MIN = -10.0; const double U_MAX = 10.0; @@ -434,10 +500,15 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - ASSERT_TRUE(pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false)); - ASSERT_TRUE(pid.initialize_from_args(0, 0, 0, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); + std::string I_METHOD = "forward_euler"; + std::string D_METHOD = "forward_euler"; + + ASSERT_TRUE(pid.initialize_from_args( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, "forward_euler", "forward_euler", false)); + ASSERT_TRUE(pid.initialize_from_args( + 0.0, 0.0, 0.0, 0.0, U_MAX, U_MIN, ANTIWINDUP_STRAT, "forward_euler", "forward_euler", false)); std::cout << "Setting gains with set_gains()" << std::endl; - pid.set_gains(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT); + pid.set_gains(P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD); rclcpp::Parameter param; @@ -450,6 +521,9 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("d", param)); ASSERT_EQ(param.get_value(), D); + ASSERT_TRUE(node->get_parameter("derivative_filter_time", param)); + ASSERT_EQ(param.get_value(), TF); + ASSERT_TRUE(node->get_parameter("i_clamp_max", param)); ASSERT_EQ(param.get_value(), I_MAX); @@ -471,6 +545,12 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("antiwindup_strategy", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); + ASSERT_TRUE(node->get_parameter("integration_method", param)); + ASSERT_EQ(param.get_value(), I_METHOD); + + ASSERT_TRUE(node->get_parameter("derivative_method", param)); + ASSERT_EQ(param.get_value(), D_METHOD); + ASSERT_TRUE(node->get_parameter("save_i_term", param)); ASSERT_EQ(param.get_value(), false); @@ -485,6 +565,7 @@ TEST(PidParametersTest, GetParametersTest) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX = 10.0; const double I_MIN = -10.0; const double U_MAX = 10.0; @@ -497,7 +578,11 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - ASSERT_TRUE(pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); + std::string I_METHOD = "forward_euler"; + std::string D_METHOD = "forward_euler"; + + ASSERT_TRUE(pid.initialize_from_args( + P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); rclcpp::Parameter param; ASSERT_TRUE(node->get_parameter("activate_state_publisher", param)); @@ -511,6 +596,7 @@ TEST(PidParametersTest, GetParametersTest) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX = 10.0; const double I_MIN = -10.0; const double U_MAX = std::numeric_limits::infinity(); @@ -523,9 +609,14 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - ASSERT_TRUE(pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false)); - 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); + std::string I_METHOD = "forward_euler"; + std::string D_METHOD = "forward_euler"; + + ASSERT_TRUE(pid.initialize_from_args( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, "forward_euler", "forward_euler", false)); + ASSERT_TRUE(pid.initialize_from_args( + 0.0, 0.0, 0.0, 0.0, U_MAX, U_MIN, ANTIWINDUP_STRAT, "forward_euler", "forward_euler", false)); + pid.set_gains(P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD); rclcpp::Parameter param; @@ -538,6 +629,9 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("d", param)); ASSERT_EQ(param.get_value(), D); + ASSERT_TRUE(node->get_parameter("derivative_filter_time", param)); + ASSERT_EQ(param.get_value(), TF); + ASSERT_TRUE(node->get_parameter("i_clamp_max", param)); ASSERT_EQ(param.get_value(), I_MAX); @@ -559,6 +653,12 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("antiwindup_strategy", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); + ASSERT_TRUE(node->get_parameter("integration_method", param)); + ASSERT_EQ(param.get_value(), I_METHOD); + + ASSERT_TRUE(node->get_parameter("derivative_method", param)); + ASSERT_EQ(param.get_value(), D_METHOD); + ASSERT_TRUE(node->get_parameter("save_i_term", param)); ASSERT_EQ(param.get_value(), false); } @@ -627,6 +727,7 @@ TEST(PidParametersTest, MultiplePidInstances) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX = 10.0; const double I_MIN = -10.0; const double U_MAX = 10.0; @@ -637,9 +738,13 @@ TEST(PidParametersTest, MultiplePidInstances) ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + std::string I_METHOD = "forward_euler"; + std::string D_METHOD = "forward_euler"; - ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); - ASSERT_NO_THROW(pid_2.initialize_from_args(2 * P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); + ASSERT_NO_THROW(pid_1.initialize_from_args( + P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); + ASSERT_NO_THROW(pid_2.initialize_from_args( + 2 * P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, 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 1274c291..7d9480a1 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -45,7 +45,8 @@ TEST(PidPublisherTest, PublishTest) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; 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); + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler", false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -83,7 +84,8 @@ TEST(PidPublisherTest, PublishTest_start_deactivated) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; 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); + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler", false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -152,7 +154,8 @@ TEST(PidPublisherTest, PublishTest_prefix) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; 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); + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler", false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -190,7 +193,8 @@ TEST(PidPublisherTest, PublishTest_local_prefix) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; 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); + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler", false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -232,7 +236,8 @@ TEST(PidPublisherTest, PublishTestLifecycle) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; 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); + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler", false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; From 2b7a90981d4fd8965e842e9d87080932ccb33e95 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Fri, 26 Sep 2025 02:56:12 +0000 Subject: [PATCH 12/12] Change Discretization to struct --- .../include/control_toolbox/pid.hpp | 113 ++++++++++-- .../include/control_toolbox/pid_ros.hpp | 7 +- control_toolbox/src/pid.cpp | 48 ++--- control_toolbox/src/pid_ros.cpp | 55 ++++-- .../test/pid_ros_parameters_tests.cpp | 57 +++--- .../test/pid_ros_publisher_tests.cpp | 31 +++- control_toolbox/test/pid_tests.cpp | 170 ++++++++++++++---- 7 files changed, 360 insertions(+), 121 deletions(-) diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 8611e962..55f54fd8 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -170,6 +170,87 @@ struct AntiWindupStrategy std::numeric_limits::epsilon(); /**< Error deadband to avoid integration. */ }; +struct DiscretizationMethod +{ +public: + enum Value : int8_t + { + UNDEFINED = -1, + FORWARD_EULER, + BACKWARD_EULER, + TRAPEZOIDAL + }; + + constexpr DiscretizationMethod() : type(FORWARD_EULER) {} + explicit constexpr DiscretizationMethod(Value v) : type(v) {} + explicit DiscretizationMethod(const std::string & s) { set_type(s); } + + void set_type(const std::string & s) + { + if (s == "forward_euler") + { + type = FORWARD_EULER; + } + else if (s == "backward_euler") + { + type = BACKWARD_EULER; + } + else if (s == "trapezoidal") + { + type = TRAPEZOIDAL; + } + else + { + type = UNDEFINED; + throw std::invalid_argument( + "DiscretizationMethod: Unknown discretization method : '" + s + + "'. Valid methods are: 'forward_euler', 'backward_euler', 'trapezoidal'."); + } + } + + void validate() const + { + if (type == UNDEFINED) + { + throw std::invalid_argument("DiscretizationMethod is UNDEFINED. Please set a valid type"); + } + if (type != FORWARD_EULER && type != BACKWARD_EULER && type != TRAPEZOIDAL && type != UNDEFINED) + { + throw std::invalid_argument("DiscretizationMethod has an invalid type"); + } + } + + operator std::string() const { return to_string(); } + + constexpr bool operator==(Value other) const { return type == other; } + constexpr bool operator!=(Value other) const { return type != other; } + constexpr bool operator==(const DiscretizationMethod & other) const { return type == other.type; } + constexpr bool operator!=(const DiscretizationMethod & other) const { return type != other.type; } + constexpr DiscretizationMethod & operator=(Value v) + { + type = v; + return *this; + } + + std::string to_string() const + { + switch (type) + { + case FORWARD_EULER: + return "forward_euler"; + case BACKWARD_EULER: + return "backward_euler"; + case TRAPEZOIDAL: + return "trapezoidal"; + case UNDEFINED: + default: + return "UNDEFINED"; + } + } + + Value type = UNDEFINED; +}; + template inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) { @@ -289,8 +370,8 @@ class Pid : Gains( p, i, d, /*tf*/ 0.0, u_max, u_min, antiwindup_strat, - /*i_method*/ "forward_euler", - /*d_method*/ "forward_euler") + /*i_method*/ DiscretizationMethod(), + /*d_method*/ DiscretizationMethod()) { } @@ -312,7 +393,8 @@ class Pid */ Gains( double p, double i, double d, double tf, double u_max, double u_min, - const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method) + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method) : p_gain_(p), i_gain_(i), d_gain_(d), @@ -365,6 +447,8 @@ class Pid try { antiwindup_strat_.validate(); + i_method_.validate(); + d_method_.validate(); } catch (const std::exception & e) { @@ -380,7 +464,8 @@ class Pid << ", tf: " << tf_ << ", i_max: " << i_max_ << ", i_min: " << i_min_ << ", u_max: " << u_max_ << ", u_min: " << u_min_ << ", antiwindup_strat: " << antiwindup_strat_.to_string() - << ", i_method: " << i_method_ << ", d_method: " << d_method_ << std::endl; + << ", i_method: " << i_method_.to_string() + << ", d_method: " << d_method_.to_string() << std::endl; } double p_gain_ = 0.0; /**< Proportional gain. */ @@ -394,8 +479,8 @@ class Pid double u_max_ = std::numeric_limits::infinity(); /**< Maximum allowable output. */ double u_min_ = -std::numeric_limits::infinity(); /**< Minimum allowable output. */ AntiWindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ - std::string i_method_ = "forward_euler"; /**< Integration method. */ - std::string d_method_ = "forward_euler"; /**< Derivative method. */ + DiscretizationMethod i_method_; /**< Integration method. */ + DiscretizationMethod d_method_; /**< Derivative method. */ }; /*! @@ -437,7 +522,8 @@ class Pid */ Pid( double p, double i, double d, double tf, double u_max, double u_min, - const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method); + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method); /*! * \brief Copy constructor required for preventing mutexes from being copied @@ -489,7 +575,8 @@ class Pid */ bool initialize( double p, double i, double d, double tf, double u_max, double u_min, - const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method); + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method); /*! * \brief Reset the state of this PID controller @@ -544,7 +631,8 @@ class Pid */ void get_gains( double & p, double & i, double & d, double & tf, double & u_max, double & u_min, - AntiWindupStrategy & antiwindup_strat, std::string & i_method, std::string & d_method); + AntiWindupStrategy & antiwindup_strat, DiscretizationMethod & i_method, + DiscretizationMethod & d_method); /*! * \brief Get PID gains for the controller. @@ -603,7 +691,8 @@ class Pid */ bool set_gains( double p, double i, double d, double tf, double u_max, double u_min, - const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method); + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method); /*! * \brief Set PID gains for the controller. @@ -765,8 +854,8 @@ class Pid std::numeric_limits::infinity(), -std::numeric_limits::infinity(), AntiWindupStrategy(), - "forward_euler", - "forward_euler"}; + DiscretizationMethod(), + DiscretizationMethod()}; // Store the PID gains in a realtime box to allow dynamic reconfigure to update it without // blocking the realtime update loop realtime_tools::RealtimeThreadSafeBox gains_box_{gains_}; diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index ecdf9970..1decfa73 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -196,8 +196,8 @@ class PidROS */ bool initialize_from_args( double p, double i, double d, double tf, double u_max, double u_min, - const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method, - bool save_i_term); + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method, bool save_i_term); /*! * \brief Initialize the PID controller based on already set parameters @@ -294,7 +294,8 @@ class PidROS */ bool set_gains( double p, double i, double d, double tf, double u_max, double u_min, - const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method); + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method); /*! * \brief Set PID gains for the controller. diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index fbdb97f7..538b0eed 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -49,13 +49,14 @@ namespace control_toolbox Pid::Pid( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) -: Pid(p, i, d, 0.0, u_max, u_min, antiwindup_strat, "forward_euler", "forward_euler") +: Pid(p, i, d, 0.0, u_max, u_min, antiwindup_strat, DiscretizationMethod(), DiscretizationMethod()) { } Pid::Pid( double p, double i, double d, double tf, double u_max, double u_min, - const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method) + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method) { if (u_min > u_max) { @@ -87,12 +88,14 @@ bool Pid::initialize( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) { - return initialize(p, i, d, 0.0, u_max, u_min, antiwindup_strat, "forward_euler", "forward_euler"); + return initialize( + p, i, d, 0.0, u_max, u_min, antiwindup_strat, DiscretizationMethod(), DiscretizationMethod()); } bool Pid::initialize( double p, double i, double d, double tf, double u_max, double u_min, - const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method) + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method) { if (set_gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method)) { @@ -145,7 +148,8 @@ void Pid::get_gains( void Pid::get_gains( double & p, double & i, double & d, double & tf, double & u_max, double & u_min, - AntiWindupStrategy & antiwindup_strat, std::string & i_method, std::string & d_method) + AntiWindupStrategy & antiwindup_strat, DiscretizationMethod & i_method, + DiscretizationMethod & d_method) { Gains gains = get_gains(); p = gains.p_gain_; @@ -169,12 +173,14 @@ bool Pid::set_gains( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) { - return set_gains(p, i, d, 0.0, u_max, u_min, antiwindup_strat, "forward_euler", "forward_euler"); + return set_gains( + p, i, d, 0.0, u_max, u_min, antiwindup_strat, DiscretizationMethod(), DiscretizationMethod()); } bool Pid::set_gains( double p, double i, double d, double tf, double u_max, double u_min, - const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method) + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method) { try { @@ -243,19 +249,21 @@ double Pid::compute_command(double error, const double & dt_s) } // Calculate the derivative error based on the selected method - if (gains_.d_method_ == "forward_euler" || gains_.d_method_ == "backward_euler") + if ( + gains_.d_method_ == DiscretizationMethod::FORWARD_EULER || + gains_.d_method_ == DiscretizationMethod::BACKWARD_EULER) { // Since \dot{e}[k-1] and \dot{e}[k] are calculated the same way for forward and backward euler, // we can combine them here d_error_ = (error - p_error_last_) / dt_s; } - else if (gains_.d_method_ == "trapezoidal") + else if (gains_.d_method_ == DiscretizationMethod::TRAPEZOIDAL) { d_error_ = 2 * (error - p_error_last_) / dt_s - d_error_last_; } else { - throw std::invalid_argument("Unknown derivative method: " + gains_.d_method_); + throw std::invalid_argument("Unknown derivative method: " + gains_.d_method_.to_string()); } return compute_command(error, d_error_, dt_s); @@ -325,19 +333,19 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) p_term = gains_.p_gain_ * p_error_; // Calculate derivative contribution to command - if (gains_.tf_ > 0.0 && gains_.d_method_ == "forward_euler") + if (gains_.tf_ > 0.0 && gains_.d_method_ == DiscretizationMethod::FORWARD_EULER) { // Derivative filter is on d_term = ((gains_.tf_ - dt_s) * d_term_last_ + (gains_.d_gain_ * dt_s * d_error_)) / (gains_.tf_); } - else if (gains_.tf_ > 0.0 && gains_.d_method_ == "backward_euler") + else if (gains_.tf_ > 0.0 && gains_.d_method_ == DiscretizationMethod::BACKWARD_EULER) { // Derivative filter is on d_term = ((gains_.tf_) * d_term_last_ + (gains_.d_gain_ * dt_s * d_error_)) / (gains_.tf_ + dt_s); } - else if (gains_.tf_ > 0.0 && gains_.d_method_ == "trapezoidal") + else if (gains_.tf_ > 0.0 && gains_.d_method_ == DiscretizationMethod::TRAPEZOIDAL) { // Derivative filter is on d_term = ((2 * gains_.tf_ - dt_s) * d_term_last_ + @@ -361,21 +369,21 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) control_toolbox::is_zero(error, gains_.antiwindup_strat_.error_deadband); // Calculate integral contribution to command - if (gains_.i_method_ == "forward_euler") + if (gains_.i_method_ == DiscretizationMethod::FORWARD_EULER) { i_term_ = i_term_last_ + gains_.i_gain_ * dt_s * p_error_last_; } - else if (gains_.i_method_ == "backward_euler") + else if (gains_.i_method_ == DiscretizationMethod::BACKWARD_EULER) { i_term_ = i_term_last_ + gains_.i_gain_ * dt_s * error; } - else if (gains_.i_method_ == "trapezoidal") + else if (gains_.i_method_ == DiscretizationMethod::TRAPEZOIDAL) { i_term_ = i_term_last_ + gains_.i_gain_ * (dt_s * 0.5) * (error + p_error_last_); } else { - throw std::runtime_error("Pid: invalid integral method"); + throw std::invalid_argument("Unknown integration method: " + gains_.i_method_.to_string()); } // Anti-windup via conditional integration @@ -425,13 +433,13 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) { if ( gains_.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && - !is_zero(gains_.i_gain_) && gains_.i_method_ == "forward_euler") + !is_zero(gains_.i_gain_) && gains_.i_method_ == DiscretizationMethod::FORWARD_EULER) { i_term_ += dt_s * 1 / gains_.antiwindup_strat_.tracking_time_constant * (cmd_ - cmd_unsat_); } else if ( gains_.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && - !is_zero(gains_.i_gain_) && gains_.i_method_ == "backward_euler") + !is_zero(gains_.i_gain_) && gains_.i_method_ == DiscretizationMethod::BACKWARD_EULER) { i_term_ = i_term_ / (1 + dt_s / gains_.antiwindup_strat_.tracking_time_constant) + dt_s / gains_.antiwindup_strat_.tracking_time_constant * (cmd_ - p_term - d_term) / @@ -439,7 +447,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) } else if ( gains_.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && - !is_zero(gains_.i_gain_) && gains_.i_method_ == "trapezoidal") + !is_zero(gains_.i_gain_) && gains_.i_method_ == DiscretizationMethod::TRAPEZOIDAL) { const double alpha = dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant); const double num_i_last = (1.0 - alpha) * i_term_last_; diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index feef1a4b..30f9b93e 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -31,12 +31,15 @@ // POSSIBILITY OF SUCH DAMAGE. #include +#include #include #include #include #include #include +#include "rcl_interfaces/msg/set_parameters_result.hpp" + #include "control_toolbox/pid_ros.hpp" namespace control_toolbox @@ -263,8 +266,8 @@ bool PidROS::initialize_from_ros_parameters() u_max = MAX_INFINITY; u_min = -MAX_INFINITY; std::string antiwindup_strat_str = "none"; - std::string i_method = "forward_euler"; - std::string d_method = "forward_euler"; + std::string i_method_str = "forward_euler"; + std::string d_method_str = "forward_euler"; bool all_params_available = true; all_params_available &= get_double_param(param_prefix_ + "p", p); @@ -287,8 +290,8 @@ bool PidROS::initialize_from_ros_parameters() u_min = -MAX_INFINITY; } get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat_str); - get_string_param(param_prefix_ + "integration_method", i_method); - get_string_param(param_prefix_ + "derivative_method", d_method); + get_string_param(param_prefix_ + "integration_method", i_method_str); + get_string_param(param_prefix_ + "derivative_method", d_method_str); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(false)); declare_param( @@ -306,6 +309,9 @@ bool PidROS::initialize_from_ros_parameters() antiwindup_strat.tracking_time_constant = tracking_time_constant; antiwindup_strat.error_deadband = error_deadband; + DiscretizationMethod i_method{i_method_str}; + DiscretizationMethod d_method{d_method_str}; + try { antiwindup_strat.validate(); @@ -315,6 +321,16 @@ bool PidROS::initialize_from_ros_parameters() RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); return false; } + try + { + i_method.validate(); + d_method.validate(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(node_logging_->get_logger(), "Invalid discretization method: %s", e.what()); + return false; + } if (pid_.initialize(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method)) { @@ -336,14 +352,17 @@ 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) { + DiscretizationMethod i_method; + DiscretizationMethod d_method; + return initialize_from_args( - p, i, d, 0.0, u_max, u_min, antiwindup_strat, "forward_euler", "forward_euler", save_i_term); + p, i, d, 0.0, u_max, u_min, antiwindup_strat, i_method, d_method, save_i_term); } bool PidROS::initialize_from_args( double p, double i, double d, double tf, double u_max, double u_min, - const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method, - bool save_i_term) + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method, bool save_i_term) { Pid::Gains verify_gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method); std::string error_msg = ""; @@ -380,8 +399,10 @@ bool PidROS::initialize_from_args( declare_param( param_prefix_ + "antiwindup_strategy", rclcpp::ParameterValue(gains.antiwindup_strat_.to_string())); - declare_param(param_prefix_ + "integration_method", rclcpp::ParameterValue(i_method)); - declare_param(param_prefix_ + "derivative_method", rclcpp::ParameterValue(d_method)); + declare_param( + param_prefix_ + "integration_method", rclcpp::ParameterValue(gains.i_method_.to_string())); + declare_param( + param_prefix_ + "derivative_method", rclcpp::ParameterValue(gains.d_method_.to_string())); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); declare_param( param_prefix_ + "activate_state_publisher", @@ -433,12 +454,16 @@ bool PidROS::set_gains( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) { - return set_gains(p, i, d, 0.0, u_max, u_min, antiwindup_strat, "forward_euler", "forward_euler"); + DiscretizationMethod i_method; + DiscretizationMethod d_method; + + return set_gains(p, i, d, 0.0, u_max, u_min, antiwindup_strat, i_method, d_method); } bool PidROS::set_gains( double p, double i, double d, double tf, double u_max, double u_min, - const AntiWindupStrategy & antiwindup_strat, std::string i_method, std::string d_method) + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method) { Pid::Gains gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method); @@ -476,8 +501,8 @@ bool PidROS::set_gains(const Pid::Gains & gains) rclcpp::Parameter(param_prefix_ + "saturation", true), rclcpp::Parameter( param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string()), - rclcpp::Parameter(param_prefix_ + "integration_method", gains.i_method_), - rclcpp::Parameter(param_prefix_ + "derivative_method", gains.d_method_)}); + rclcpp::Parameter(param_prefix_ + "integration_method", gains.i_method_.to_string()), + rclcpp::Parameter(param_prefix_ + "derivative_method", gains.d_method_.to_string())}); return true; } } @@ -540,8 +565,8 @@ void PidROS::print_values() << " U_Min: " << gains.u_min_ << "\n" << " Tracking_Time_Constant: " << gains.antiwindup_strat_.tracking_time_constant << "\n" << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\n" - << " integration_method: " << gains.i_method_ << "\n" - << " derivative_method: " << gains.d_method_ << "\n" + << " integration_method: " << gains.i_method_.to_string() << "\n" + << " derivative_method: " << gains.d_method_.to_string() << "\n" << " P Error: " << p_error << "\n" << " I Term: " << i_term << "\n" << " D Error: " << d_error << "\n" diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 11180559..006bf687 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -22,6 +22,7 @@ #include "rclcpp/utilities.hpp" using control_toolbox::AntiWindupStrategy; +using control_toolbox::DiscretizationMethod; using rclcpp::executors::MultiThreadedExecutor; class TestablePidROS : public control_toolbox::PidROS @@ -61,8 +62,8 @@ void check_set_parameters( ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - std::string I_METHOD = "forward_euler"; - std::string D_METHOD = "forward_euler"; + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; const bool SAVE_I_TERM = true; ASSERT_NO_THROW(pid.initialize_from_args( @@ -105,10 +106,10 @@ void check_set_parameters( ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); ASSERT_TRUE(node->get_parameter(prefix + "integration_method", param)); - ASSERT_EQ(param.get_value(), I_METHOD); + ASSERT_EQ(param.get_value(), I_METHOD.to_string()); ASSERT_TRUE(node->get_parameter(prefix + "derivative_method", param)); - ASSERT_EQ(param.get_value(), D_METHOD); + ASSERT_EQ(param.get_value(), D_METHOD.to_string()); ASSERT_TRUE(node->get_parameter(prefix + "save_i_term", param)); ASSERT_EQ(param.get_value(), SAVE_I_TERM); @@ -163,8 +164,8 @@ TEST(PidParametersTest, InitPidTestBadParameter) ANTIWINDUP_STRAT.i_min = I_MIN_BAD; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - std::string I_METHOD = "forward_euler"; - std::string D_METHOD = "forward_euler"; + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; ASSERT_NO_THROW(pid.initialize_from_args( P, I, D, TF, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); @@ -198,8 +199,8 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, 0.0); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); - ASSERT_EQ(gains.i_method_, "forward_euler"); - ASSERT_EQ(gains.d_method_, "forward_euler"); + ASSERT_EQ(gains.i_method_, DiscretizationMethod::FORWARD_EULER); + ASSERT_EQ(gains.d_method_, DiscretizationMethod::FORWARD_EULER); } TEST(PidParametersTest, InitPid_param_prefix_only) @@ -268,8 +269,8 @@ TEST(PidParametersTest, SetParametersTest) ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - std::string I_METHOD = "forward_euler"; - std::string D_METHOD = "forward_euler"; + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; const bool SAVE_I_TERM = false; pid.initialize_from_args( @@ -365,8 +366,8 @@ TEST(PidParametersTest, SetBadParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - std::string I_METHOD = "forward_euler"; - std::string D_METHOD = "forward_euler"; + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; pid.initialize_from_args(P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false); @@ -500,13 +501,13 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - std::string I_METHOD = "forward_euler"; - std::string D_METHOD = "forward_euler"; + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; ASSERT_TRUE(pid.initialize_from_args( - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, "forward_euler", "forward_euler", false)); + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); ASSERT_TRUE(pid.initialize_from_args( - 0.0, 0.0, 0.0, 0.0, U_MAX, U_MIN, ANTIWINDUP_STRAT, "forward_euler", "forward_euler", false)); + 0.0, 0.0, 0.0, 0.0, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); std::cout << "Setting gains with set_gains()" << std::endl; pid.set_gains(P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD); @@ -546,10 +547,10 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); ASSERT_TRUE(node->get_parameter("integration_method", param)); - ASSERT_EQ(param.get_value(), I_METHOD); + ASSERT_EQ(param.get_value(), I_METHOD.to_string()); ASSERT_TRUE(node->get_parameter("derivative_method", param)); - ASSERT_EQ(param.get_value(), D_METHOD); + ASSERT_EQ(param.get_value(), D_METHOD.to_string()); ASSERT_TRUE(node->get_parameter("save_i_term", param)); ASSERT_EQ(param.get_value(), false); @@ -578,8 +579,8 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - std::string I_METHOD = "forward_euler"; - std::string D_METHOD = "forward_euler"; + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; ASSERT_TRUE(pid.initialize_from_args( P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); @@ -609,13 +610,13 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - std::string I_METHOD = "forward_euler"; - std::string D_METHOD = "forward_euler"; + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; ASSERT_TRUE(pid.initialize_from_args( - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, "forward_euler", "forward_euler", false)); + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); ASSERT_TRUE(pid.initialize_from_args( - 0.0, 0.0, 0.0, 0.0, U_MAX, U_MIN, ANTIWINDUP_STRAT, "forward_euler", "forward_euler", false)); + 0.0, 0.0, 0.0, 0.0, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); pid.set_gains(P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD); rclcpp::Parameter param; @@ -654,10 +655,10 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); ASSERT_TRUE(node->get_parameter("integration_method", param)); - ASSERT_EQ(param.get_value(), I_METHOD); + ASSERT_EQ(param.get_value(), I_METHOD.to_string()); ASSERT_TRUE(node->get_parameter("derivative_method", param)); - ASSERT_EQ(param.get_value(), D_METHOD); + ASSERT_EQ(param.get_value(), D_METHOD.to_string()); ASSERT_TRUE(node->get_parameter("save_i_term", param)); ASSERT_EQ(param.get_value(), false); @@ -738,8 +739,8 @@ TEST(PidParametersTest, MultiplePidInstances) ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - std::string I_METHOD = "forward_euler"; - std::string D_METHOD = "forward_euler"; + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; ASSERT_NO_THROW(pid_1.initialize_from_args( P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index 7d9480a1..2cc14d43 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -28,6 +28,7 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" using control_toolbox::AntiWindupStrategy; +using control_toolbox::DiscretizationMethod; using PidStateMsg = control_msgs::msg::PidState; using rclcpp::executors::MultiThreadedExecutor; @@ -45,8 +46,12 @@ TEST(PidPublisherTest, PublishTest) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.tracking_time_constant = 1.0; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + pid_ros.initialize_from_args( - 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler", false); + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -84,8 +89,12 @@ TEST(PidPublisherTest, PublishTest_start_deactivated) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.tracking_time_constant = 1.0; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + pid_ros.initialize_from_args( - 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler", false); + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -154,8 +163,12 @@ TEST(PidPublisherTest, PublishTest_prefix) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.tracking_time_constant = 1.0; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + pid_ros.initialize_from_args( - 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler", false); + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -193,8 +206,12 @@ TEST(PidPublisherTest, PublishTest_local_prefix) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.tracking_time_constant = 1.0; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + pid_ros.initialize_from_args( - 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler", false); + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -236,8 +253,12 @@ TEST(PidPublisherTest, PublishTestLifecycle) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.tracking_time_constant = 1.0; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + pid_ros.initialize_from_args( - 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler", false); + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method, 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 79156617..ef9b850f 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -39,6 +39,7 @@ #include "gmock/gmock.h" using control_toolbox::AntiWindupStrategy; +using control_toolbox::DiscretizationMethod; using control_toolbox::Pid; using namespace std::chrono_literals; @@ -52,8 +53,12 @@ TEST(ParameterTest, UTermBadIBoundsTestConstructor) antiwindup_strat.type = AntiWindupStrategy::NONE; antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + EXPECT_THROW( - Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat, "forward_euler", "forward_euler"), + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat, i_method, d_method), std::invalid_argument); } @@ -67,13 +72,17 @@ TEST(ParameterTest, UTermBadIBoundsTest) antiwindup_strat.type = AntiWindupStrategy::NONE; antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; - Pid pid(1.0, 1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat, "forward_euler", "forward_euler"); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(1.0, 1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat, i_method, d_method); 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, antiwindup_strat, "forward_euler", "forward_euler")); + EXPECT_NO_THROW( + pid.set_gains(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat, i_method, d_method)); // 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); @@ -87,8 +96,12 @@ TEST(ParameterTest, outputClampTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Setting u_max = 1.0 and u_min = -1.0 to test clamping - Pid pid(1.0, 0.0, 0.0, 1.0, 1.0, -1.0, antiwindup_strat, "forward_euler", "forward_euler"); + Pid pid(1.0, 0.0, 0.0, 1.0, 1.0, -1.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; @@ -141,10 +154,14 @@ TEST(ParameterTest, noOutputClampTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Setting u_max = INF and u_min = -INF to disable clamping Pid pid( 1.0, 0.0, 0.0, 1.0, std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), antiwindup_strat, "forward_euler", "forward_euler"); + -std::numeric_limits::infinity(), antiwindup_strat, i_method, d_method); double cmd = 0.0; @@ -201,7 +218,11 @@ TEST(ParameterTest, integrationBackCalculationZeroGainTest) antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value - Pid pid(0.0, 0.0, 0.0, 1.0, 20.0, -20.0, antiwindup_strat, "forward_euler", "forward_euler"); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 0.0, 0.0, 1.0, 20.0, -20.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; double pe, ie, de; @@ -250,7 +271,11 @@ TEST(ParameterTest, integrationConditionalIntegrationZeroGainTest) antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; - Pid pid(0.0, 0.0, 0.0, 1.0, 20.0, -20.0, antiwindup_strat, "forward_euler", "forward_euler"); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 0.0, 0.0, 1.0, 20.0, -20.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; double pe, ie, de; @@ -299,15 +324,18 @@ TEST(ParameterTest, ITermBadIBoundsTest) antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; - Pid pid(1.0, 1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat, "forward_euler", "forward_euler"); + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(1.0, 1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat, i_method, d_method); auto gains = pid.get_gains(); EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_max, 1.0); EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_min, -1.0); // Try to set bad i-bounds, i.e. i_min > i_max antiwindup_strat.i_max = -1.0; antiwindup_strat.i_min = 1.0; - EXPECT_NO_THROW(pid.set_gains( - 1.0, 1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat, "forward_euler", "forward_euler")); + EXPECT_NO_THROW( + pid.set_gains(1.0, 1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat, i_method, d_method)); // Check if gains were not updated because i-bounds are bad, i.e. i_min > i_max EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_max, 1.0); EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_min, -1.0); @@ -330,7 +358,10 @@ TEST(ParameterTest, integrationAntiwindupTest) const double u_max = std::numeric_limits::infinity(); const double u_min = -std::numeric_limits::infinity(); - Pid pid(0.0, i_gain, 0.0, 1.0, u_max, u_min, antiwindup_strat, "forward_euler", "forward_euler"); + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, i_gain, 0.0, 1.0, u_max, u_min, antiwindup_strat, i_method, d_method); double cmd = 0.0; @@ -369,8 +400,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.tracking_time_constant = tracking_time_constant; - std::string i_method = "forward_euler"; - std::string d_method = "forward_euler"; + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; // Initialize the default way Pid pid1(p_gain, i_gain, d_gain, tf, u_max, u_min, antiwindup_strat, i_method, d_method); @@ -378,7 +409,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Test return values ------------------------------------------------- double p_gain_return, i_gain_return, d_gain_return, tf_return, u_max_return, u_min_return; AntiWindupStrategy antiwindup_strat_return; - std::string i_method_return, d_method_return; + DiscretizationMethod i_method_return, d_method_return; pid1.get_gains( p_gain_return, i_gain_return, d_gain_return, tf_return, u_max_return, u_min_return, @@ -413,8 +444,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.tracking_time_constant = tracking_time_constant; - i_method = "backward_euler"; - d_method = "backward_euler"; + i_method = DiscretizationMethod::BACKWARD_EULER; + d_method = DiscretizationMethod::BACKWARD_EULER; pid1.set_gains(p_gain, i_gain, d_gain, tf, u_max, u_min, antiwindup_strat, i_method, d_method); @@ -520,8 +551,11 @@ TEST(CommandTest, proportionalOnlyTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Set only proportional gain - Pid pid(1.0, 0.0, 0.0, 1.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "forward_euler"); + Pid pid(1.0, 0.0, 0.0, 1.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, p-gain = 1, dt = 1 @@ -555,8 +589,11 @@ TEST(CommandTest, integralOnlyTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Set only integral gains with enough limits to test behavior - Pid pid(0.0, 1.0, 0.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); + Pid pid(0.0, 1.0, 0.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, i-gain = 1, dt = 1 @@ -625,8 +662,11 @@ TEST(CommandTest, integralOnlyBackwardTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + DiscretizationMethod i_method{DiscretizationMethod::BACKWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Set only integral gains with enough limits to test behavior - Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "backward_euler", "forward_euler"); + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, i-gain = 1, dt = 1 @@ -692,8 +732,11 @@ TEST(CommandTest, integralOnlyTrapezoidalTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + DiscretizationMethod i_method{DiscretizationMethod::TRAPEZOIDAL}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Set only integral gains with enough limits to test behavior - Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "trapezoidal", "forward_euler"); + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, i-gain = 1, dt = 1 @@ -759,8 +802,11 @@ TEST(CommandTest, derivativeOnlyForwardTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Set only derivative gain - Pid pid(0.0, 0.0, 1.0, 0.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "forward_euler"); + Pid pid(0.0, 0.0, 1.0, 0.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, d-gain = 1, dt = 1 @@ -799,8 +845,11 @@ TEST(CommandTest, derivativeOnlyBackwardTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::BACKWARD_EULER}; + // Set only derivative gain - Pid pid(0.0, 0.0, 1.0, 0.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "backward_euler"); + Pid pid(0.0, 0.0, 1.0, 0.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, d-gain = 1, dt = 1 @@ -839,8 +888,11 @@ TEST(CommandTest, derivativeOnlyTrapezoidalTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::TRAPEZOIDAL}; + // Set only derivative gain - Pid pid(0.0, 0.0, 1.0, 0.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "trapezoidal"); + Pid pid(0.0, 0.0, 1.0, 0.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, d-gain = 1, dt = 1 @@ -879,8 +931,12 @@ TEST(CommandTest, derivativeFilteredForwardTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Set only derivative gain and derivative filter time (tf) - Pid pid(0.0, 0.0, 1.0, 2.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "forward_euler"); + Pid pid(0.0, 0.0, 1.0, 2.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, d-gain = 1, tf = 1, dt = 1 @@ -926,8 +982,12 @@ TEST(CommandTest, derivativeFilteredBackwardTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::BACKWARD_EULER}; + // Set only derivative gain and derivative filter time (tf) - Pid pid(0.0, 0.0, 1.0, 2.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "backward_euler"); + Pid pid(0.0, 0.0, 1.0, 2.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, d-gain = 1, tf = 1, dt = 1 @@ -973,8 +1033,12 @@ TEST(CommandTest, derivativeFilteredTrapezoidalTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::TRAPEZOIDAL}; + // Set only derivative gain and derivative filter time (tf) - Pid pid(0.0, 0.0, 1.0, 2.0, 10.0, -10.0, antiwindup_strat, "forward_euler", "trapezoidal"); + Pid pid(0.0, 0.0, 1.0, 2.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, d-gain = 1, tf = 1, dt = 1 @@ -1022,7 +1086,10 @@ TEST(CommandTest, completePIDTest) antiwindup_strat.i_max = 10.0; antiwindup_strat.i_min = -10.0; - Pid pid(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // All contributions are tested, here few tests check that they sum up correctly @@ -1055,7 +1122,11 @@ TEST(CommandTest, backCalculationForwardPIDTest) antiwindup_strat.i_max = 10.0; antiwindup_strat.i_min = -10.0; antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value - Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; double pe, ie, de; @@ -1118,7 +1189,11 @@ TEST(CommandTest, backCalculationBackwardPIDTest) antiwindup_strat.i_max = 10.0; antiwindup_strat.i_min = -10.0; antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value - Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "backward_euler", "forward_euler"); + + DiscretizationMethod i_method{DiscretizationMethod::BACKWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; double pe, ie, de; @@ -1179,7 +1254,11 @@ TEST(CommandTest, backCalculationTrapezoidalPIDTest) antiwindup_strat.i_max = 10.0; antiwindup_strat.i_min = -10.0; antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value - Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "trapezoidal", "forward_euler"); + + DiscretizationMethod i_method{DiscretizationMethod::TRAPEZOIDAL}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; double pe, ie, de; @@ -1246,7 +1325,11 @@ TEST(CommandTest, conditionalIntegrationForwardPIDTest) antiwindup_strat.i_max = 10.0; antiwindup_strat.i_min = -10.0; antiwindup_strat.tracking_time_constant = 1.0; - Pid pid(0.0, 1.0, 0.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 1.0, 0.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; double pe, ie, de; @@ -1309,7 +1392,11 @@ TEST(CommandTest, conditionalIntegrationBackwardPIDTest) antiwindup_strat.i_max = 10.0; antiwindup_strat.i_min = -10.0; antiwindup_strat.tracking_time_constant = 1.0; - Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "backward_euler", "forward_euler"); + + DiscretizationMethod i_method{DiscretizationMethod::BACKWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; double pe, ie, de; @@ -1370,7 +1457,11 @@ TEST(CommandTest, conditionalIntegrationTrapezoidalPIDTest) antiwindup_strat.i_max = 10.0; antiwindup_strat.i_min = -10.0; antiwindup_strat.tracking_time_constant = 1.0; - Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, "trapezoidal", "forward_euler"); + + DiscretizationMethod i_method{DiscretizationMethod::TRAPEZOIDAL}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; double pe, ie, de; @@ -1440,10 +1531,13 @@ TEST(CommandTest, timeArgumentTest) antiwindup_strat.i_min = -10.0; antiwindup_strat.tracking_time_constant = 1.0; - Pid pid1(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); - Pid pid2(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); - Pid pid3(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); - Pid pid4(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, "forward_euler", "forward_euler"); + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid1(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + Pid pid2(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + Pid pid3(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + Pid pid4(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); // call without error_dt, dt is used to calculate error_dt auto cmd1 = pid1.compute_command(-0.5, 1.0);