diff --git a/control_toolbox/CHANGELOG.rst b/control_toolbox/CHANGELOG.rst index 7c73a139..a235f2d6 100644 --- a/control_toolbox/CHANGELOG.rst +++ b/control_toolbox/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package control_toolbox ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +5.3.2 (2025-05-05) +------------------ +* fix deprecated tf2 header (`#361 `_) +* Contributors: Bence Magyar + 5.3.1 (2025-04-25) ------------------ * Minor filter fixes + clang-format rules update (`#347 `_) diff --git a/control_toolbox/include/control_toolbox/filter_traits.hpp b/control_toolbox/include/control_toolbox/filter_traits.hpp new file mode 100644 index 00000000..0c6994d0 --- /dev/null +++ b/control_toolbox/include/control_toolbox/filter_traits.hpp @@ -0,0 +1,243 @@ +// Copyright (c) 2025, ros2_control development team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_TOOLBOX__FILTER_TRAITS_HPP_ +#define CONTROL_TOOLBOX__FILTER_TRAITS_HPP_ + +#define EIGEN_INITIALIZE_MATRICES_BY_NAN + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/wrench_stamped.hpp" + +namespace control_toolbox +{ + +template +struct FilterTraits; + +// Wrapper around std::vector to be used as +// the std::vector StorageType specialization. +// This is a workaround for the fact that +// std::vector's operator* and operator+ cannot be overloaded. +template +struct FilterVector +{ + std::vector data; + + FilterVector() = default; + + explicit FilterVector(const std::vector & vec) : data(vec) {} + + explicit FilterVector(size_t size, const U & initial_value = U{}) : data(size, initial_value) {} + + FilterVector operator*(const U & scalar) const + { + FilterVector result = *this; + for (auto & val : result.data) + { + val *= scalar; + } + return result; + } + + FilterVector operator+(const FilterVector & other) const + { + assert(data.size() == other.data.size() && "Vectors must be of the same size for addition"); + FilterVector result = *this; + for (size_t i = 0; i < data.size(); ++i) + { + result.data[i] += other.data[i]; + } + return result; + } + + size_t size() const { return data.size(); } +}; + +// Enable scalar * FilterVector +template +inline FilterVector operator*(const U & scalar, const FilterVector & vec) +{ + return vec * scalar; +} + +template +struct FilterTraits +{ + using StorageType = T; + + static void initialize(StorageType & storage) + { + storage = T{std::numeric_limits::quiet_NaN()}; + } + + static bool is_nan(const StorageType & storage) { return std::isnan(storage); } + + static bool is_finite(const StorageType & storage) { return std::isfinite(storage); } + + static bool is_empty(const StorageType & storage) + { + (void)storage; + return false; + } + + static void assign(StorageType & storage, const StorageType & data_in) { storage = data_in; } + + static void validate_input(const T & data_in, const StorageType & filtered_value, T & data_out) + { + (void)data_in; + (void)filtered_value; + (void)data_out; // Suppress unused warnings + } + + static void add_metadata(StorageType & storage, const StorageType & data_in) + { + (void)storage; + (void)data_in; + } +}; + +template <> +struct FilterTraits +{ + using StorageType = Eigen::Matrix; + using DataType = geometry_msgs::msg::WrenchStamped; + + static void initialize(StorageType & storage) + { + // Evocation of the default constructor through EIGEN_INITIALIZE_MATRICES_BY_NAN + storage = StorageType(); + } + + static bool is_nan(const StorageType & storage) { return storage.hasNaN(); } + + static bool is_finite(const DataType & data) + { + return std::isfinite(data.wrench.force.x) && std::isfinite(data.wrench.force.y) && + std::isfinite(data.wrench.force.z) && std::isfinite(data.wrench.torque.x) && + std::isfinite(data.wrench.torque.y) && std::isfinite(data.wrench.torque.z); + } + + static bool is_empty(const StorageType & storage) + { + (void)storage; + return false; + } + + static void assign(DataType & data_in, const StorageType & storage) + { + data_in.wrench.force.x = storage[0]; + data_in.wrench.force.y = storage[1]; + data_in.wrench.force.z = storage[2]; + data_in.wrench.torque.x = storage[3]; + data_in.wrench.torque.y = storage[4]; + data_in.wrench.torque.z = storage[5]; + } + + static void assign(StorageType & storage, const DataType & data_in) + { + storage[0] = data_in.wrench.force.x; + storage[1] = data_in.wrench.force.y; + storage[2] = data_in.wrench.force.z; + storage[3] = data_in.wrench.torque.x; + storage[4] = data_in.wrench.torque.y; + storage[5] = data_in.wrench.torque.z; + } + + static void assign(StorageType & storage, const StorageType & data_in) { storage = data_in; } + + static void validate_input( + const DataType & data_in, const StorageType & filtered_value, DataType & data_out) + { + (void)filtered_value; // Not used here + + // Compare new input's frame_id with previous output's frame_id + assert( + data_in.header.frame_id == data_out.header.frame_id && + "Frame ID changed between filter updates"); + } + + static void add_metadata(DataType & data_out, const DataType & data_in) + { + data_out.header = data_in.header; + } +}; + +template +struct FilterTraits> +{ + using StorageType = FilterVector; + using DataType = std::vector; + + static void initialize(StorageType & storage) { (void)storage; } + + static bool is_finite(const StorageType & storage) + { + return std::all_of( + storage.data.begin(), storage.data.end(), [](U val) { return std::isfinite(val); }); + } + + static bool is_finite(const DataType & storage) + { + return std::all_of(storage.begin(), storage.end(), [](U val) { return std::isfinite(val); }); + } + + static bool is_empty(const StorageType & storage) { return storage.data.empty(); } + + static bool is_nan(const StorageType & storage) + { + for (const auto & val : storage.data) + { + if (std::isnan(val)) + { + return true; + } + } + + return false; + } + + static void assign(StorageType & storage, const DataType & data_in) { storage.data = data_in; } + + static void assign(DataType & storage, const StorageType & data_in) { storage = data_in.data; } + + static void assign(StorageType & storage, const StorageType & data_in) + { + storage.data = data_in.data; + } + + static void validate_input( + const DataType & data_in, const StorageType & filtered_value, DataType & data_out) + { + assert( + data_in.size() == filtered_value.size() && + "Input vector size does not match internal state size"); + assert(data_out.size() == data_in.size() && "Input and output vectors must be the same size"); + } + + static void add_metadata(DataType & storage, const DataType & data_in) + { + (void)storage; + (void)data_in; + } +}; + +} // namespace control_toolbox + +#endif // CONTROL_TOOLBOX__FILTER_TRAITS_HPP_ diff --git a/control_toolbox/include/control_toolbox/low_pass_filter.hpp b/control_toolbox/include/control_toolbox/low_pass_filter.hpp index 82d40b15..f5d54c86 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -15,15 +15,16 @@ #ifndef CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ #define CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ -#include - #include #include #include #include #include +#include #include +#include "control_toolbox/filter_traits.hpp" + #include "geometry_msgs/msg/wrench_stamped.hpp" namespace control_toolbox @@ -123,10 +124,13 @@ class LowPassFilter // Filter parameters double a1_; /** feedbackward coefficient. */ double b1_; /** feedforward coefficient. */ - /** internal data storage of template type. */ - T filtered_value, filtered_old_value, old_value; - /** internal data storage (wrench). */ - Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; + + // Define the storage type based on T + using Traits = FilterTraits; + using StorageType = typename Traits::StorageType; + + StorageType filtered_value_, filtered_old_value_, old_value_; + bool configured_ = false; }; @@ -143,66 +147,15 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() { - // Initialize storage Vectors - filtered_value = filtered_old_value = old_value = std::numeric_limits::quiet_NaN(); - // TODO(destogl): make the size parameterizable and more intelligent is using complex types - for (Eigen::Index i = 0; i < 6; ++i) - { - msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = std::numeric_limits::quiet_NaN(); - } + Traits::initialize(filtered_value_); + Traits::initialize(filtered_old_value_); + Traits::initialize(old_value_); return configured_ = true; } -template <> -inline bool LowPassFilter::update( - const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) -{ - if (!configured_) - { - throw std::runtime_error("Filter is not configured"); - } - // If this is the first call to update initialize the filter at the current state - // so that we dont apply an impulse to the data. - if (msg_filtered.hasNaN()) - { - msg_filtered[0] = data_in.wrench.force.x; - msg_filtered[1] = data_in.wrench.force.y; - msg_filtered[2] = data_in.wrench.force.z; - msg_filtered[3] = data_in.wrench.torque.x; - msg_filtered[4] = data_in.wrench.torque.y; - msg_filtered[5] = data_in.wrench.torque.z; - msg_filtered_old = msg_filtered; - msg_old = msg_filtered; - } - - // IIR Filter - msg_filtered = b1_ * msg_old + a1_ * msg_filtered_old; - msg_filtered_old = msg_filtered; - - // TODO(destogl): use wrenchMsgToEigen - msg_old[0] = data_in.wrench.force.x; - msg_old[1] = data_in.wrench.force.y; - msg_old[2] = data_in.wrench.force.z; - msg_old[3] = data_in.wrench.torque.x; - msg_old[4] = data_in.wrench.torque.y; - msg_old[5] = data_in.wrench.torque.z; - - data_out.wrench.force.x = msg_filtered[0]; - data_out.wrench.force.y = msg_filtered[1]; - data_out.wrench.force.z = msg_filtered[2]; - data_out.wrench.torque.x = msg_filtered[3]; - data_out.wrench.torque.y = msg_filtered[4]; - data_out.wrench.torque.z = msg_filtered[5]; - - // copy the header - data_out.header = data_in.header; - return true; -} - -template <> -inline bool LowPassFilter>::update( - const std::vector & data_in, std::vector & data_out) +template +bool LowPassFilter::update(const T & data_in, T & data_out) { if (!configured_) { @@ -210,62 +163,36 @@ inline bool LowPassFilter>::update( } // If this is the first call to update initialize the filter at the current state // so that we dont apply an impulse to the data. - // This also sets the size of the member variables to match the input data. - if (filtered_value.empty()) + if (Traits::is_nan(filtered_value_) || Traits::is_empty(filtered_value_)) { - if (std::any_of(data_in.begin(), data_in.end(), [](double val) { return !std::isfinite(val); })) + if (!Traits::is_finite(data_in)) { return false; } - filtered_value = data_in; - filtered_old_value = data_in; - old_value = data_in; + + Traits::assign(filtered_value_, data_in); + Traits::assign(filtered_old_value_, data_in); + Traits::assign(old_value_, data_in); } else { - assert( - data_in.size() == filtered_value.size() && - "Internal data and the data_in doesn't hold the same size"); - assert(data_out.size() == data_in.size() && "data_in and data_out doesn't hold same size"); + // Generic validation for all types + Traits::validate_input(data_in, filtered_value_, data_out); } - // Filter each value in the vector - for (std::size_t i = 0; i < data_in.size(); i++) - { - data_out[i] = b1_ * old_value[i] + a1_ * filtered_old_value[i]; - filtered_old_value[i] = data_out[i]; - if (std::isfinite(data_in[i])) - { - old_value[i] = data_in[i]; - } - } + // Filter + filtered_value_ = old_value_ * b1_ + filtered_old_value_ * a1_; + filtered_old_value_ = filtered_value_; - return true; -} + Traits::assign(old_value_, data_in); + Traits::assign(data_out, filtered_value_); -template -bool LowPassFilter::update(const T & data_in, T & data_out) -{ - if (!configured_) - { - throw std::runtime_error("Filter is not configured"); - } - // If this is the first call to update initialize the filter at the current state - // so that we dont apply an impulse to the data. - if (std::isnan(filtered_value)) + if (Traits::is_finite(data_in)) { - filtered_value = data_in; - filtered_old_value = data_in; - old_value = data_in; + Traits::assign(old_value_, data_in); } - // Filter - data_out = b1_ * old_value + a1_ * filtered_old_value; - filtered_old_value = data_out; - if (std::isfinite(data_in)) - { - old_value = data_in; - } + Traits::add_metadata(data_out, data_in); return true; } diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 1e7bf7ef..7e9a83a3 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -34,12 +34,82 @@ #define CONTROL_TOOLBOX__PID_HPP_ #include +#include +#include #include "rclcpp/duration.hpp" #include "realtime_tools/realtime_buffer.hpp" namespace control_toolbox { +class AntiwindupStrategy +{ +public: + enum Value : int8_t + { + NONE = 0, + BACK_CALCULATION, + CONDITIONING_TECHNIQUE, + CONDITIONAL_INTEGRATION + }; + + constexpr AntiwindupStrategy() : value_(NONE) {} + constexpr AntiwindupStrategy(Value v) : value_(v) {} // NOLINT(runtime/explicit) + + explicit AntiwindupStrategy(const std::string & s) + { + if (s == "back_calculation") + { + value_ = BACK_CALCULATION; + } + else if (s == "conditioning_technique") + { + value_ = CONDITIONING_TECHNIQUE; + } + else if (s == "conditional_integration") + { + value_ = CONDITIONAL_INTEGRATION; + } + else + { + value_ = NONE; + } + } + + operator std::string() const { return to_string(); } + + constexpr bool operator==(AntiwindupStrategy other) const { return value_ == other.value_; } + constexpr bool operator!=(AntiwindupStrategy other) const { return value_ != other.value_; } + + constexpr bool operator==(Value other) const { return value_ == other; } + constexpr bool operator!=(Value other) const { return value_ != other; } + + std::string to_string() const + { + switch (value_) + { + case BACK_CALCULATION: + return "back_calculation"; + case CONDITIONING_TECHNIQUE: + return "conditioning_technique"; + case CONDITIONAL_INTEGRATION: + return "conditional_integration"; + case NONE: + default: + return "none"; + } + } + +private: + Value value_; +}; + +template +inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) +{ + return std::abs(value) <= tolerance; +} + /***************************************************/ /*! \class Pid \brief A basic pid class. @@ -82,7 +152,12 @@ namespace control_toolbox \param i Integral gain - \param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$ + \param i_clamp Minimum and maximum bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$ + + \param u_clamp Minimum and maximum bounds for the controller output. The clamp is applied to the \f$command\f$. + + \param trk_tc Tracking time constant for the 'back_calculation' and 'conditioning_technique' strategies. + \section Usage @@ -115,7 +190,7 @@ class Pid struct Gains { /*! - * \brief Optional constructor for passing in values without antiwindup + * \brief Optional constructor for passing in values without antiwindup and saturation * * \param p The proportional gain. * \param i The integral gain. @@ -125,53 +200,131 @@ class Pid * */ Gains(double p, double i, double d, double i_max, double i_min) - : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(true) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(0.0), + u_min_(0.0), + trk_tc_(0.0), + saturation_(false), + antiwindup_(true), + antiwindup_strat_(AntiwindupStrategy::NONE) { } /*! - * \brief Optional constructor for passing in values + * \brief Optional constructor for passing in values without saturation * * \param p The proportional gain. * \param i The integral gain. * \param d The derivative gain. * \param i_max Upper integral clamp. * \param i_min Lower integral clamp. - * \param antiwindup Antiwindup functionality. When set to true, limits + * \param antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. * */ Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) - : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(0.0), + u_min_(0.0), + trk_tc_(0.0), + saturation_(false), + antiwindup_(antiwindup), + antiwindup_strat_(AntiwindupStrategy::NONE) + { + } + + /*! + * \brief Optional constructor for passing in values + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Choose the anti-windup strategy. Options: 'back_calculation', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy + other than 'none' is selected, it will override the controller's default anti-windup behavior. + * + */ + Gains( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min), + u_max_(u_max), + u_min_(u_min), + trk_tc_(trk_tc), + saturation_(saturation), + antiwindup_(antiwindup), + antiwindup_strat_(antiwindup_strat) { } // Default constructor - Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0), antiwindup_(false) + Gains() + : p_gain_(0.0), + i_gain_(0.0), + d_gain_(0.0), + i_max_(0.0), + i_min_(0.0), + u_max_(0.0), + u_min_(0.0), + trk_tc_(0.0), + saturation_(false), + antiwindup_(false), + antiwindup_strat_(AntiwindupStrategy::NONE) { } - double p_gain_; /**< Proportional gain. */ - double i_gain_; /**< Integral gain. */ - double d_gain_; /**< Derivative gain. */ - double i_max_; /**< Maximum allowable integral term. */ - double i_min_; /**< Minimum allowable integral term. */ - bool antiwindup_; /**< Antiwindup. */ + double p_gain_; /**< Proportional gain. */ + double i_gain_; /**< Integral gain. */ + double d_gain_; /**< Derivative gain. */ + double i_max_; /**< Maximum allowable integral term. */ + double i_min_; /**< Minimum allowable integral term. */ + double u_max_; /**< Maximum allowable output. */ + double u_min_; /**< Minimum allowable output. */ + double trk_tc_; /**< Tracking time constant. */ + bool saturation_; /**< Saturation. */ + bool antiwindup_; /**< Anti-windup. */ + AntiwindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ }; /*! * \brief Constructor, zeros out Pid values when created and * initialize Pid-gains and integral term limits. - * Does not initialize dynamic reconfigure for PID gains * * \param p The proportional gain. * \param i The integral gain. * \param d The derivative gain. * \param i_max Upper integral clamp. * \param i_min Lower integral clamp. - * \param antiwindup Antiwindup functionality. When set to true, limits + * \param antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. @@ -182,7 +335,38 @@ class Pid double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0, bool antiwindup = false); - /** + /*! + * \brief Constructor, initialize Pid-gains and term limits. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Choose the anti-windup strategy. Options: 'back_calculation', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy + other than 'none' is selected, it will override the controller's default anti-windup behavior. + * + * \throws An std::invalid_argument exception is thrown if i_min > i_max or u_min > u_max + */ + Pid( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + + /*! * \brief Copy constructor required for preventing mutexes from being copied * \param source - Pid to copy */ @@ -194,15 +378,14 @@ class Pid ~Pid(); /*! - * \brief Zeros out Pid values and initialize Pid-gains and integral term limits - * Does not initialize the node's parameter interface for PID gains + * \brief Zeros out Pid values and initialize Pid-gains and term limits * * \param p The proportional gain. * \param i The integral gain. * \param d The derivative gain. * \param i_max Upper integral clamp. * \param i_min Lower integral clamp. - * \param antiwindup Antiwindup functionality. When set to true, limits + * \param antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. @@ -212,6 +395,37 @@ class Pid void initialize( double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + /*! + * \brief Initialize Pid-gains and term limits + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Choose the anti-windup strategy. Options: 'back_calculation', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy + other than 'none' is selected, it will override the controller's default anti-windup behavior. + * + * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max + */ + void initialize( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + /*! * \brief Reset the state of this PID controller * @note The integral term is not retained and it is reset to zero @@ -247,7 +461,7 @@ class Pid * \param d The derivative gain. * \param i_max Upper integral clamp. * \param i_min Lower integral clamp. - * \param antiwindup Antiwindup functionality. When set to true, limits + * \param antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. @@ -255,6 +469,35 @@ class Pid void get_gains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup); + /*! + * \brief Get PID gains for the controller. + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Choose the anti-windup strategy. Options: 'back_calculation', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy + other than 'none' is selected, it will override the controller's default anti-windup behavior. + */ + void get_gains( + double & p, double & i, double & d, double & i_max, double & i_min, double & u_max, + double & u_min, double & trk_tc, bool & saturation, bool & antiwindup, + AntiwindupStrategy & antiwindup_strat); + /*! * \brief Get PID gains for the controller. * \return gains A struct of the PID gain values @@ -268,7 +511,7 @@ class Pid * \param d The derivative gain. * \param i_max Upper integral clamp. * \param i_min Lower integral clamp. - * \param antiwindup Antiwindup functionality. When set to true, limits + * \param antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. @@ -277,6 +520,37 @@ class Pid */ void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + /*! + * \brief Set PID gains for the controller. + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max Upper integral clamp. + * \param i_min Lower integral clamp. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Choose the anti-windup strategy. Options: 'back_calculation', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy + other than 'none' is selected, it will override the controller's default anti-windup behavior. + * + * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max + */ + void set_gains( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc = 0.0, bool saturation = false, bool antiwindup = false, + AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE); + /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values @@ -430,11 +704,12 @@ class Pid // blocking the realtime update loop realtime_tools::RealtimeBuffer gains_buffer_; - double p_error_last_; /** Save state for derivative state calculation. */ - double p_error_; /** Error. */ - double d_error_; /** Derivative of error. */ - double i_term_; /** Integrator state. */ - double cmd_; /** Command to send. */ + double p_error_last_ = 0; /** Save state for derivative state calculation. */ + double p_error_ = 0; /** Error. */ + double d_error_ = 0; /** Derivative of error. */ + double i_term_ = 0; /** Integrator state. */ + double cmd_ = 0; /** Command to send. */ + double cmd_unsat_ = 0; /** command without saturation. */ }; } // namespace control_toolbox diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index c9996e13..3b53dc90 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -92,7 +92,7 @@ class PidROS * \param d The derivative gain. * \param i_max Upper integral clamp. * \param i_min Lower integral clamp. - * \param antiwindup Antiwindup functionality. When set to true, limits + * \param antiwindup Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios. @@ -109,7 +109,10 @@ class PidROS * \param d The derivative gain. * \param i_max The max integral windup. * \param i_min The min integral windup. - * \param antiwindup antiwindup. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. * \param save_i_term save integrator output between resets. * * \note New gains are not applied if i_min_ > i_max_ @@ -117,9 +120,47 @@ class PidROS void initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term); + /*! + * \brief Initialize the PID controller and set the parameters + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max The max integral windup. + * \param i_min The min integral windup. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy + other than 'none' is selected, it will override the controller's default + anti-windup behavior. + * \deprecated{only when `antiwindup_strat == AntiwindupStrategy::NONE`:} + * Old anti-windup technique is deprecated and will be removed by + * the ROS 2 Kilted Kaiju release. + * \warning{If you pass `AntiwindupStrategy::NONE`, at runtime a warning will be printed:} + * `"Old anti-windup technique is deprecated. This option will be removed by the ROS 2 Kilted Kaiju release."` + * \param save_i_term save integrator output between resets. + * + * \note New gains are not applied if i_min_ > i_max_ or if u_min_ > u_max_. + */ + void initialize_from_args( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat, + bool save_i_term); + /*! * \brief Initialize the PID controller based on already set parameters - * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise + * \return True if all parameters are set (p, i, d, i_max, i_min, u_max, u_min and trk_tc), False otherwise */ bool initialize_from_ros_parameters(); @@ -184,6 +225,44 @@ class PidROS */ void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + /*! + * \brief Initialize the PID controller and set the parameters + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max The max integral windup. + * \param i_min The min integral windup. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param trk_tc Specifies the tracking time constant for the 'back_calculation' + and 'conditioning_technique' strategies. If set to 0.0 when one of these + strategies is selected, a recommended default value will be applied. + * \param saturation Enables output saturation. When true, the controller output is + clamped between u_max and u_min. + * \param antiwindup Anti-windup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_max and + i_min are applied in both scenarios. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditioning_technique', 'conditional_integration', or 'none'. Note that + the 'back_calculation' and 'conditioning_technique' strategies use the + tracking_time_constant parameter to tune the anti-windup behavior. When a strategy + other than 'none' is selected, it will override the controller's default + anti-windup behavior. + * \deprecated{only when `antiwindup_strat == AntiwindupStrategy::NONE`:} + * Old anti-windup technique is deprecated and will be removed by + * the ROS 2 Kilted Kaiju release. + * \warning{If you pass `AntiwindupStrategy::NONE`, at runtime a warning will be printed:} + * `"Old anti-windup technique is deprecated. This option will be removed by + * the ROS 2 Kilted Kaiju release."` + * + * \note New gains are not applied if i_min > i_max or if u_min_ > u_max_. + */ + void set_gains( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc = 0.0, bool saturation = false, bool antiwindup = false, + AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE); + /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values @@ -248,6 +327,8 @@ class PidROS bool get_boolean_param(const std::string & param_name, bool & value); + bool get_string_param(const std::string & param_name, std::string & value); + /*! * \brief Set prefix for topic and parameter names * \param[in] topic_prefix prefix to add to the pid parameters. diff --git a/control_toolbox/package.xml b/control_toolbox/package.xml index 1e3f3d06..46bb67d2 100644 --- a/control_toolbox/package.xml +++ b/control_toolbox/package.xml @@ -1,7 +1,7 @@ control_toolbox - 5.3.1 + 5.3.2 The control toolbox contains modules that are useful across all controllers. Bence Magyar diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 54621b15..3bbb4c8e 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -39,22 +39,31 @@ #include #include #include -#include #include -#include #include "control_toolbox/pid.hpp" namespace control_toolbox { Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) +: Pid(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE) +{ +} + +Pid::Pid( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) : gains_buffer_() { if (i_min > i_max) { throw std::invalid_argument("received i_min > i_max"); } - set_gains(p, i, d, i_max, i_min, antiwindup); + else if (u_min > u_max) + { + throw std::invalid_argument("received u_min > u_max"); + } + set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); // Initialize saved i-term values clear_saved_iterm(); @@ -78,7 +87,16 @@ Pid::~Pid() {} void Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - set_gains(p, i, d, i_max, i_min, antiwindup); + set_gains(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE); + + reset(); +} + +void Pid::initialize( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) +{ + set_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); reset(); } @@ -103,12 +121,30 @@ void Pid::clear_saved_iterm() { i_term_ = 0.0; } void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & i_min) { + double u_max; + double u_min; + double trk_tc; + bool saturation; bool antiwindup; - get_gains(p, i, d, i_max, i_min, antiwindup); + AntiwindupStrategy antiwindup_strat; + get_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); } void Pid::get_gains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup) +{ + double u_max; + double u_min; + double trk_tc; + bool saturation; + AntiwindupStrategy antiwindup_strat; + get_gains(p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); +} + +void Pid::get_gains( + double & p, double & i, double & d, double & i_max, double & i_min, double & u_max, + double & u_min, double & trk_tc, bool & saturation, bool & antiwindup, + AntiwindupStrategy & antiwindup_strat) { Gains gains = *gains_buffer_.readFromRT(); @@ -117,7 +153,12 @@ void Pid::get_gains( d = gains.d_gain_; i_max = gains.i_max_; i_min = gains.i_min_; + u_max = gains.u_max_; + u_min = gains.u_min_; + trk_tc = gains.trk_tc_; + saturation = gains.saturation_; antiwindup = gains.antiwindup_; + antiwindup_strat = gains.antiwindup_strat_; } Pid::Gains Pid::get_gains() { return *gains_buffer_.readFromRT(); } @@ -129,21 +170,58 @@ void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bo set_gains(gains); } -void Pid::set_gains(const Gains & gains) +void Pid::set_gains( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) +{ + Gains gains( + p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); + + set_gains(gains); +} + +void Pid::set_gains(const Gains & gains_in) { - if (gains.i_min_ > gains.i_max_) + if (gains_in.i_min_ > gains_in.i_max_) { std::cout << "received i_min > i_max, skip new gains\n"; } + else if (gains_in.u_min_ > gains_in.u_max_) + { + std::cout << "received u_min > u_max, skip new gains\n"; + } else { + Gains gains = gains_in; + + if (gains.antiwindup_strat_ == AntiwindupStrategy::BACK_CALCULATION) + { + if (is_zero(gains.trk_tc_) && !is_zero(gains.d_gain_)) + { + // Default value for tracking time constant for back calculation technique + gains.trk_tc_ = std::sqrt(gains.d_gain_ / gains.i_gain_); + } + else if (is_zero(gains.trk_tc_) && is_zero(gains.d_gain_)) + { + // Default value for tracking time constant for back calculation technique + gains.trk_tc_ = gains.p_gain_ / gains.i_gain_; + } + } + else if (gains.antiwindup_strat_ == AntiwindupStrategy::CONDITIONING_TECHNIQUE) + { + if (is_zero(gains.trk_tc_)) + { + // Default value for tracking time constant for conditioning technique + gains.trk_tc_ = gains.p_gain_; + } + } gains_buffer_.writeFromNonRT(gains); } } double Pid::compute_command(double error, const double & dt_s) { - if (std::abs(dt_s) <= std::numeric_limits::epsilon()) + if (is_zero(dt_s)) { // don't update anything return cmd_; @@ -199,9 +277,9 @@ double Pid::compute_command(double error, double error_dot, const std::chrono::n double Pid::compute_command(double error, double error_dot, const double & dt_s) { - if (std::abs(dt_s) <= std::numeric_limits::epsilon()) + if (is_zero(dt_s)) { - // don't update anything + // Don't update anything return cmd_; } else if (dt_s < 0.0) @@ -212,36 +290,71 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) Gains gains = *gains_buffer_.readFromRT(); double p_term, d_term; - p_error_ = error; // this is error = target - state + p_error_ = error; // This is error = target - state d_error_ = error_dot; - // don't reset controller but return NaN + // Don't reset controller but return NaN if (!std::isfinite(error) || !std::isfinite(error_dot)) { std::cout << "Received a non-finite error/error_dot value\n"; - return cmd_ = std::numeric_limits::quiet_NaN(); + return cmd_ = std::numeric_limits::quiet_NaN(); } // Calculate proportional contribution to command p_term = gains.p_gain_ * p_error_; + // Calculate derivative contribution to command + d_term = gains.d_gain_ * d_error_; + // Calculate integral contribution to command - if (gains.antiwindup_) + if (gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) { // Prevent i_term_ from climbing higher than permitted by i_max_/i_min_ i_term_ = std::clamp(i_term_ + gains.i_gain_ * dt_s * p_error_, gains.i_min_, gains.i_max_); } - else + else if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) { i_term_ += gains.i_gain_ * dt_s * p_error_; } - // Calculate derivative contribution to command - d_term = gains.d_gain_ * d_error_; - // Compute the command - // Limit i_term so that the limit is meaningful in the output - cmd_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; + if (!gains.antiwindup_ && gains.antiwindup_strat_ == AntiwindupStrategy::NONE) + { + // Limit i_term so that the limit is meaningful in the output + cmd_unsat_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; + } + else + { + cmd_unsat_ = p_term + i_term_ + d_term; + } + + if (gains.saturation_ == true) + { + // Limit cmd_ if saturation is enabled + cmd_ = std::clamp(cmd_unsat_, gains.u_min_, gains.u_max_); + } + else + { + cmd_ = cmd_unsat_; + } + + if (gains.antiwindup_strat_ == AntiwindupStrategy::BACK_CALCULATION && !is_zero(gains.i_gain_)) + { + i_term_ += dt_s * (gains.i_gain_ * error + 1 / gains.trk_tc_ * (cmd_ - cmd_unsat_)); + } + else if ( + gains.antiwindup_strat_ == AntiwindupStrategy::CONDITIONING_TECHNIQUE && + !is_zero(gains.i_gain_)) + { + i_term_ += dt_s * gains.i_gain_ * (error + 1 / gains.trk_tc_ * (cmd_ - cmd_unsat_)); + } + else if (gains.antiwindup_strat_ == AntiwindupStrategy::CONDITIONAL_INTEGRATION) + { + if (!(cmd_unsat_ != cmd_ && error * cmd_unsat_ > 0)) + { + i_term_ += dt_s * gains.i_gain_ * error; + } + } return cmd_; } diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 63952487..74cdb854 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -30,8 +30,6 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include -#include #include #include #include @@ -182,19 +180,57 @@ bool PidROS::get_double_param(const std::string & param_name, double & value) } } +bool PidROS::get_string_param(const std::string & param_name, std::string & value) +{ + declare_param(param_name, rclcpp::ParameterValue(value)); + rclcpp::Parameter param; + if (node_params_->has_parameter(param_name)) + { + node_params_->get_parameter(param_name, param); + if (rclcpp::PARAMETER_STRING != param.get_type()) + { + RCLCPP_ERROR( + node_logging_->get_logger(), "Wrong parameter type '%s', not string", param_name.c_str()); + return false; + } + value = param.as_string(); + RCLCPP_DEBUG_STREAM( + node_logging_->get_logger(), "parameter '" << param_name << "' in node '" + << node_base_->get_name() << "' value is " << value + << std::endl); + return true; + } + else + { + RCLCPP_ERROR_STREAM( + node_logging_->get_logger(), "parameter '" << param_name << "' in node '" + << node_base_->get_name() << "' does not exists" + << std::endl); + return false; + } +} + bool PidROS::initialize_from_ros_parameters() { - double p, i, d, i_min, i_max; - p = i = d = i_min = i_max = std::numeric_limits::quiet_NaN(); + double p, i, d, i_max, i_min, u_max, u_min, trk_tc; + p = i = d = i_max = i_min = u_max = u_min = trk_tc = std::numeric_limits::quiet_NaN(); + bool saturation = false; bool antiwindup = false; + std::string antiwindup_strat_str = "none"; bool all_params_available = true; + all_params_available &= get_double_param(param_prefix_ + "p", p); all_params_available &= get_double_param(param_prefix_ + "i", i); all_params_available &= get_double_param(param_prefix_ + "d", d); all_params_available &= get_double_param(param_prefix_ + "i_clamp_max", i_max); all_params_available &= get_double_param(param_prefix_ + "i_clamp_min", i_min); + all_params_available &= get_double_param(param_prefix_ + "u_clamp_max", u_max); + all_params_available &= get_double_param(param_prefix_ + "u_clamp_min", u_min); + all_params_available &= get_double_param(param_prefix_ + "tracking_time_constant", trk_tc); + get_boolean_param(param_prefix_ + "saturation", saturation); get_boolean_param(param_prefix_ + "antiwindup", antiwindup); + get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat_str); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(false)); if (all_params_available) @@ -202,7 +238,17 @@ bool PidROS::initialize_from_ros_parameters() set_parameter_event_callback(); } - pid_.initialize(p, i, d, i_max, i_min, antiwindup); + if (antiwindup_strat_str == "none") + { + std::cout << "Old anti-windup technique is deprecated. " + "This option will be removed by the ROS 2 Kilted Kaiju release." + << std::endl; + } + + AntiwindupStrategy antiwindup_strat(antiwindup_strat_str); + + pid_.initialize( + p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); return all_params_available; } @@ -218,26 +264,54 @@ void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValu void PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup) { - initialize_from_args(p, i, d, i_max, i_min, antiwindup, false); + initialize_from_args( + p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE, false); } void PidROS::initialize_from_args( double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term) +{ + initialize_from_args( + p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE, save_i_term); +} + +void PidROS::initialize_from_args( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat, + bool save_i_term) { if (i_min > i_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } + else if (u_min > u_max) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + } else { - pid_.initialize(p, i, d, i_max, i_min, antiwindup); + if (antiwindup_strat == AntiwindupStrategy::NONE) + { + std::cout << "Old anti-windup technique is deprecated. " + "This option will be removed by the ROS 2 Kilted Kaiju release." + << std::endl; + } + + pid_.initialize( + p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); declare_param(param_prefix_ + "p", rclcpp::ParameterValue(p)); declare_param(param_prefix_ + "i", rclcpp::ParameterValue(i)); declare_param(param_prefix_ + "d", rclcpp::ParameterValue(d)); declare_param(param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(i_max)); declare_param(param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(i_min)); + declare_param(param_prefix_ + "u_clamp_max", rclcpp::ParameterValue(u_max)); + declare_param(param_prefix_ + "u_clamp_min", rclcpp::ParameterValue(u_min)); + declare_param(param_prefix_ + "tracking_time_constant", rclcpp::ParameterValue(trk_tc)); + declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(saturation)); declare_param(param_prefix_ + "antiwindup", rclcpp::ParameterValue(antiwindup)); + declare_param( + param_prefix_ + "antiwindup_strategy", rclcpp::ParameterValue(antiwindup_strat.to_string())); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); set_parameter_event_callback(); @@ -275,21 +349,45 @@ double PidROS::compute_command(double error, double error_dot, const rclcpp::Dur Pid::Gains PidROS::get_gains() { return pid_.get_gains(); } void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) +{ + set_gains(p, i, d, i_max, i_min, 0.0, 0.0, 0.0, false, antiwindup, AntiwindupStrategy::NONE); +} + +void PidROS::set_gains( + double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat) { if (i_min > i_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } + else if (u_min > u_max) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + } else { + if (antiwindup_strat == AntiwindupStrategy::NONE) + { + std::cout << "Old anti-windup technique is deprecated. " + "This option will be removed by the ROS 2 Kilted Kaiju release." + << std::endl; + } + node_params_->set_parameters( {rclcpp::Parameter(param_prefix_ + "p", p), rclcpp::Parameter(param_prefix_ + "i", i), rclcpp::Parameter(param_prefix_ + "d", d), rclcpp::Parameter(param_prefix_ + "i_clamp_max", i_max), rclcpp::Parameter(param_prefix_ + "i_clamp_min", i_min), - rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup)}); - - pid_.set_gains(p, i, d, i_max, i_min, antiwindup); + rclcpp::Parameter(param_prefix_ + "u_clamp_max", u_max), + rclcpp::Parameter(param_prefix_ + "u_clamp_min", u_min), + rclcpp::Parameter(param_prefix_ + "tracking_time_constant", trk_tc), + rclcpp::Parameter(param_prefix_ + "saturation", saturation), + rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup), + rclcpp::Parameter(param_prefix_ + "antiwindup_strategy", antiwindup_strat.to_string())}); + + pid_.set_gains( + p, i, d, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, antiwindup_strat); } } @@ -339,18 +437,25 @@ void PidROS::print_values() double p_error, i_term, d_error; get_current_pid_errors(p_error, i_term, d_error); - RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Current Values of PID template:\n" - << " P Gain: " << gains.p_gain_ << "\n" - << " I Gain: " << gains.i_gain_ << "\n" - << " D Gain: " << gains.d_gain_ << "\n" - << " I_Max: " << gains.i_max_ << "\n" - << " I_Min: " << gains.i_min_ << "\n" - << " Antiwindup: " << gains.antiwindup_ - << "\n" - << " P_Error: " << p_error << "\n" - << " i_term: " << i_term << "\n" - << " D_Error: " << d_error << "\n" - << " Command: " << get_current_cmd();); + RCLCPP_INFO_STREAM(node_logging_->get_logger(), + "Current Values of PID template:\n" + << " P Gain: " << gains.p_gain_ << "\n" + << " I Gain: " << gains.i_gain_ << "\n" + << " D Gain: " << gains.d_gain_ << "\n" + << " I Max: " << gains.i_max_ << "\n" + << " I Min: " << gains.i_min_ << "\n" + << " U_Max: " << gains.u_max_ << "\n" + << " U_Min: " << gains.u_min_ << "\n" + << " Tracking_Time_Constant: " << gains.trk_tc_ << "\n" + << " Saturation: " << gains.saturation_ << "\n" + << " Antiwindup: " << gains.antiwindup_ << "\n" + << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() + << "\n" + << "\n" + << " P Error: " << p_error << "\n" + << " I Term: " << i_term << "\n" + << " D Error: " << d_error << "\n" + << " Command: " << get_current_cmd();); } void PidROS::set_gains(const Pid::Gains & gains) @@ -359,6 +464,10 @@ void PidROS::set_gains(const Pid::Gains & gains) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } + else if (gains.u_min_ > gains.u_max_) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + } else { pid_.set_gains(gains); @@ -406,11 +515,36 @@ void PidROS::set_parameter_event_callback() gains.i_min_ = parameter.get_value(); changed = true; } + else if (param_name == param_prefix_ + "u_clamp_max") + { + gains.u_max_ = parameter.get_value(); + changed = true; + } + else if (param_name == param_prefix_ + "u_clamp_min") + { + gains.u_min_ = parameter.get_value(); + changed = true; + } + else if (param_name == param_prefix_ + "tracking_time_constant") + { + gains.trk_tc_ = parameter.get_value(); + changed = true; + } + else if (param_name == param_prefix_ + "saturation") + { + gains.saturation_ = parameter.get_value(); + changed = true; + } else if (param_name == param_prefix_ + "antiwindup") { gains.antiwindup_ = parameter.get_value(); changed = true; } + else if (param_name == param_prefix_ + "antiwindup_strategy") + { + gains.antiwindup_strat_ = AntiwindupStrategy(parameter.get_value()); + changed = true; + } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { @@ -420,11 +554,14 @@ void PidROS::set_parameter_event_callback() if (changed) { - /// @note don't call set_gains() from inside a callback if (gains.i_min_ > gains.i_max_) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } + else if (gains.u_min_ > gains.u_max_) + { + RCLCPP_ERROR(node_logging_->get_logger(), "received u_min > u_max, skip new gains"); + } else { pid_.set_gains(gains); diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 24c7b282..4c82cb6e 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -21,6 +21,7 @@ #include "rclcpp/parameter.hpp" #include "rclcpp/utilities.hpp" +using control_toolbox::AntiwindupStrategy; using rclcpp::executors::MultiThreadedExecutor; class TestablePidROS : public control_toolbox::PidROS @@ -54,10 +55,17 @@ void check_set_parameters( const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; + const bool SATURATION = true; const bool ANTIWINDUP = true; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; const bool SAVE_I_TERM = true; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM)); + ASSERT_NO_THROW(pid.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT, + SAVE_I_TERM)); rclcpp::Parameter param; @@ -77,9 +85,24 @@ void check_set_parameters( ASSERT_TRUE(node->get_parameter(prefix + "i_clamp_min", param)); ASSERT_EQ(param.get_value(), I_MIN); + ASSERT_TRUE(node->get_parameter(prefix + "u_clamp_max", param)); + ASSERT_EQ(param.get_value(), U_MAX); + + ASSERT_TRUE(node->get_parameter(prefix + "u_clamp_min", param)); + ASSERT_EQ(param.get_value(), U_MIN); + + ASSERT_TRUE(node->get_parameter(prefix + "tracking_time_constant", param)); + ASSERT_EQ(param.get_value(), TRK_TC); + + ASSERT_TRUE(node->get_parameter(prefix + "saturation", param)); + ASSERT_EQ(param.get_value(), SATURATION); + ASSERT_TRUE(node->get_parameter(prefix + "antiwindup", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP); + ASSERT_TRUE(node->get_parameter(prefix + "antiwindup_strategy", param)); + ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); + ASSERT_TRUE(node->get_parameter(prefix + "save_i_term", param)); ASSERT_EQ(param.get_value(), SAVE_I_TERM); @@ -90,7 +113,12 @@ void check_set_parameters( ASSERT_EQ(gains.d_gain_, D); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.u_max_, U_MAX); + ASSERT_EQ(gains.u_min_, U_MIN); + ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_TRUE(gains.saturation_); ASSERT_TRUE(gains.antiwindup_); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); } TEST(PidParametersTest, InitPidTest) @@ -116,8 +144,13 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double D = 3.0; const double I_MAX_BAD = -10.0; const double I_MIN_BAD = 10.0; + const double U_MAX_BAD = -10.0; + const double U_MIN_BAD = 10.0; + const double TRK_TC = 4.0; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX_BAD, I_MIN_BAD, false)); + ASSERT_NO_THROW(pid.initialize_from_args( + P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD, TRK_TC, false, false, + AntiwindupStrategy::NONE, false)); rclcpp::Parameter param; @@ -127,7 +160,12 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_FALSE(node->get_parameter("d", param)); ASSERT_FALSE(node->get_parameter("i_clamp_max", param)); ASSERT_FALSE(node->get_parameter("i_clamp_min", param)); + ASSERT_FALSE(node->get_parameter("u_clamp_max", param)); + ASSERT_FALSE(node->get_parameter("u_clamp_min", param)); + ASSERT_FALSE(node->get_parameter("tracking_time_constant", param)); + ASSERT_FALSE(node->get_parameter("saturation", param)); ASSERT_FALSE(node->get_parameter("antiwindup", param)); + ASSERT_FALSE(node->get_parameter("antiwindup_strategy", param)); // check gains were NOT set control_toolbox::Pid::Gains gains = pid.get_gains(); @@ -136,7 +174,12 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.d_gain_, 0.0); ASSERT_EQ(gains.i_max_, 0.0); ASSERT_EQ(gains.i_min_, 0.0); + ASSERT_EQ(gains.u_max_, 0.0); + ASSERT_EQ(gains.u_min_, 0.0); + ASSERT_EQ(gains.trk_tc_, 0.0); + ASSERT_FALSE(gains.saturation_); ASSERT_FALSE(gains.antiwindup_); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); } TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot) @@ -216,10 +259,17 @@ TEST(PidParametersTest, SetParametersTest) const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; + const bool SATURATION = true; const bool ANTIWINDUP = true; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; const bool SAVE_I_TERM = false; - pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM); + pid.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT, + SAVE_I_TERM); rcl_interfaces::msg::SetParametersResult set_result; @@ -238,8 +288,20 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_max", U_MAX))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_min", U_MIN))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("tracking_time_constant", TRK_TC))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", SATURATION))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("antiwindup_strategy", ANTIWINDUP_STRAT))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("save_i_term", SAVE_I_TERM))); ASSERT_TRUE(set_result.successful); @@ -253,7 +315,12 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.d_gain_, D); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.u_max_, U_MAX); + ASSERT_EQ(gains.u_min_, U_MIN); + ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_TRUE(gains.saturation_); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); } TEST(PidParametersTest, SetBadParametersTest) @@ -269,9 +336,17 @@ TEST(PidParametersTest, SetBadParametersTest) const double I_MIN = -10.0; const double I_MAX_BAD = -20.0; const double I_MIN_BAD = 20.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double U_MAX_BAD = -20.0; + const double U_MIN_BAD = 20.0; + const double TRK_TC = 4.0; + const bool SATURATION = true; const bool ANTIWINDUP = true; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; - pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT, false); rcl_interfaces::msg::SetParametersResult set_result; @@ -290,8 +365,20 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN_BAD))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_max", U_MAX_BAD))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_min", U_MIN_BAD))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("tracking_time_constant", TRK_TC))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", SATURATION))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("antiwindup_strategy", ANTIWINDUP_STRAT))); + ASSERT_TRUE(set_result.successful); // process callbacks rclcpp::spin_some(node->get_node_base_interface()); @@ -303,7 +390,12 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.d_gain_, D); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); + ASSERT_EQ(gains.u_max_, U_MAX); + ASSERT_EQ(gains.u_min_, U_MIN); + ASSERT_EQ(gains.trk_tc_, TRK_TC); + ASSERT_TRUE(gains.saturation_); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); + ASSERT_EQ(gains.antiwindup_strat_, AntiwindupStrategy::NONE); } TEST(PidParametersTest, GetParametersTest) @@ -317,10 +409,17 @@ TEST(PidParametersTest, GetParametersTest) const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; + const bool SATURATION = true; const bool ANTIWINDUP = true; + const AntiwindupStrategy ANTIWINDUP_STRAT = AntiwindupStrategy::NONE; - pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, false, false); - pid.set_gains(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.initialize_from_args( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, AntiwindupStrategy::NONE, false); + pid.set_gains( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION, ANTIWINDUP, ANTIWINDUP_STRAT); rclcpp::Parameter param; @@ -339,9 +438,24 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("i_clamp_min", param)); ASSERT_EQ(param.get_value(), I_MIN); + ASSERT_TRUE(node->get_parameter("u_clamp_max", param)); + ASSERT_EQ(param.get_value(), U_MAX); + + ASSERT_TRUE(node->get_parameter("u_clamp_min", param)); + ASSERT_EQ(param.get_value(), U_MIN); + + ASSERT_TRUE(node->get_parameter("tracking_time_constant", param)); + ASSERT_EQ(param.get_value(), TRK_TC); + + ASSERT_TRUE(node->get_parameter("saturation", param)); + ASSERT_EQ(param.get_value(), SATURATION); + ASSERT_TRUE(node->get_parameter("antiwindup", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP); + ASSERT_TRUE(node->get_parameter("antiwindup_strategy", param)); + ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); + ASSERT_TRUE(node->get_parameter("save_i_term", param)); ASSERT_EQ(param.get_value(), false); } @@ -373,6 +487,18 @@ TEST(PidParametersTest, GetParametersFromParams) rclcpp::Parameter param_i_clamp_min; ASSERT_TRUE(node->get_parameter("i_clamp_min", param_i_clamp_min)); ASSERT_TRUE(std::isnan(param_i_clamp_min.get_value())); + + rclcpp::Parameter param_u_clamp_max; + ASSERT_TRUE(node->get_parameter("u_clamp_max", param_u_clamp_max)); + ASSERT_TRUE(std::isnan(param_u_clamp_max.get_value())); + + rclcpp::Parameter param_u_clamp_min; + ASSERT_TRUE(node->get_parameter("u_clamp_min", param_u_clamp_min)); + ASSERT_TRUE(std::isnan(param_u_clamp_min.get_value())); + + rclcpp::Parameter param_tracking_time_constant; + ASSERT_TRUE(node->get_parameter("tracking_time_constant", param_tracking_time_constant)); + ASSERT_TRUE(std::isnan(param_tracking_time_constant.get_value())); } TEST(PidParametersTest, MultiplePidInstances) @@ -387,9 +513,14 @@ TEST(PidParametersTest, MultiplePidInstances) const double D = 3.0; const double I_MAX = 10.0; const double I_MIN = -10.0; - - ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, I_MAX, I_MIN, false, false)); - ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, I_MAX, I_MIN, true, false)); + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; + + ASSERT_NO_THROW(pid_1.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false, false, AntiwindupStrategy::NONE, false)); + ASSERT_NO_THROW(pid_2.initialize_from_args( + P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true, true, AntiwindupStrategy::NONE, false)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index 046ce683..b5931062 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -27,6 +27,7 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +using control_toolbox::AntiwindupStrategy; using PidStateMsg = control_msgs::msg::PidState; using rclcpp::executors::MultiThreadedExecutor; @@ -39,7 +40,8 @@ TEST(PidPublisherTest, PublishTest) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, false, false); + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, false, AntiwindupStrategy::NONE, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -76,7 +78,8 @@ TEST(PidPublisherTest, PublishTestLifecycle) std::dynamic_pointer_cast>( pid_ros.get_pid_state_publisher()); - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, false, false); + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0, false, false, AntiwindupStrategy::NONE, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index c2a75ee8..e67f4a79 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -32,14 +32,301 @@ #include #include +#include #include "control_toolbox/pid.hpp" #include "gmock/gmock.h" +using control_toolbox::AntiwindupStrategy; using control_toolbox::Pid; using namespace std::chrono_literals; +TEST(ParameterTest, UTermBadIBoundsTestConstructor) +{ + RecordProperty( + "description", + "This test checks if an error is thrown for bad u_bounds specification (i.e. u_min > u_max)."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + EXPECT_THROW( + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, false, AntiwindupStrategy::NONE), + std::invalid_argument); +} + +TEST(ParameterTest, UTermBadIBoundsTest) +{ + RecordProperty( + "description", + "This test checks if gains remain for bad u_bounds specification (i.e. u_min > u_max)."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 0.0, false, false, AntiwindupStrategy::NONE); + auto gains = pid.get_gains(); + EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); + EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); + // Try to set bad u-bounds, i.e. u_min > u_max + EXPECT_NO_THROW(pid.set_gains( + 1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, 0.0, false, false, AntiwindupStrategy::NONE)); + // Check if gains were not updated because u-bounds are bad, i.e. u_min > u_max + EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); + EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); +} + +TEST(ParameterTest, outputClampTest) +{ + RecordProperty( + "description", "This test succeeds if the output is clamped when the saturation is active."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, true, false, AntiwindupStrategy::BACK_CALCULATION); + + double cmd = 0.0; + + // ***** TEST UPPER LIMIT ***** + + cmd = pid.compute_command(0.5, 1.0); + EXPECT_EQ(0.5, cmd); + + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(2.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(50.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(100.0, 1.0); + EXPECT_EQ(1.0, cmd); + + // ***** TEST LOWER LIMIT ***** + + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-0.5, cmd); + + cmd = pid.compute_command(-1, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-2, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-10, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-50, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-100, 1.0); + EXPECT_EQ(-1.0, cmd); +} + +TEST(ParameterTest, noOutputClampTest) +{ + RecordProperty( + "description", "This test succeeds if the output isn't clamped when the saturation is false."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, false, false, AntiwindupStrategy::BACK_CALCULATION); + + double cmd = 0.0; + + // ***** TEST UPPER LIMIT ***** + + cmd = pid.compute_command(0.5, 1.0); + EXPECT_EQ(0.5, cmd); + + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(1.0, cmd); + + cmd = pid.compute_command(2.0, 1.0); + EXPECT_EQ(2.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + EXPECT_EQ(10.0, cmd); + + cmd = pid.compute_command(50.0, 1.0); + EXPECT_EQ(50.0, cmd); + + cmd = pid.compute_command(100.0, 1.0); + EXPECT_EQ(100.0, cmd); + + // ***** TEST LOWER LIMIT ***** + + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-0.5, cmd); + + cmd = pid.compute_command(-1, 1.0); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.compute_command(-2, 1.0); + EXPECT_EQ(-2.0, cmd); + + cmd = pid.compute_command(-10, 1.0); + EXPECT_EQ(-10.0, cmd); + + cmd = pid.compute_command(-50, 1.0); + EXPECT_EQ(-50.0, cmd); + + cmd = pid.compute_command(-100, 1.0); + EXPECT_EQ(-100.0, cmd); +} + +TEST(ParameterTest, integrationBackCalculationZeroGainTest) +{ + RecordProperty( + "description", + "This test succeeds if the integral contribution is clamped when the integral gain is zero for " + "the back calculation technique."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, AntiwindupStrategy::BACK_CALCULATION); + + double cmd = 0.0; + double pe, ie, de; + + // back_calculation + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); +} + +TEST(ParameterTest, integrationConditioningTechniqueZeroGainTest) +{ + RecordProperty( + "description", + "This test succeeds if the integral contribution is clamped when the integral gain is zero for " + "the conditioning technique."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, + AntiwindupStrategy::CONDITIONING_TECHNIQUE); + + double cmd = 0.0; + double pe, ie, de; + + // back_calculation + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); +} + +TEST(ParameterTest, integrationConditionalIntegrationZeroGainTest) +{ + RecordProperty( + "description", + "This test succeeds if the integral contribution is clamped when the integral gain is zero for " + "the conditional integration technique."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, + AntiwindupStrategy::CONDITIONAL_INTEGRATION); + + double cmd = 0.0; + double pe, ie, de; + + // back_calculation + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + cmd = pid.compute_command(10.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); +} + TEST(ParameterTest, ITermBadIBoundsTestConstructor) { RecordProperty( @@ -183,23 +470,39 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double d_gain = std::rand() % 100; double i_max = std::rand() % 100; double i_min = -1 * std::rand() % 100; + double u_max = std::rand() % 100; + double u_min = -1 * std::rand() % 100; + double trk_tc = std::rand() % 100; + bool saturation = false; bool antiwindup = false; + AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE; // Initialize the default way - Pid pid1(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); + Pid pid1( + p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, + antiwindup_strat); // Test return values ------------------------------------------------- - double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return; - bool antiwindup_return; + double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, + u_min_return, trk_tc_return; + bool saturation_return, antiwindup_return; + AntiwindupStrategy antiwindup_strat_return; + pid1.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); + p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, + u_min_return, trk_tc_return, saturation_return, antiwindup_return, antiwindup_strat_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); EXPECT_EQ(d_gain, d_gain_return); EXPECT_EQ(i_max, i_max_return); EXPECT_EQ(i_min, i_min_return); + EXPECT_EQ(u_max, u_max_return); + EXPECT_EQ(u_min, u_min_return); + EXPECT_EQ(trk_tc, trk_tc_return); + EXPECT_EQ(saturation, saturation_return); EXPECT_EQ(antiwindup, antiwindup_return); + EXPECT_EQ(antiwindup_strat, antiwindup_strat_return); // Test return values using struct ------------------------------------------------- @@ -209,7 +512,16 @@ TEST(ParameterTest, gainSettingCopyPIDTest) d_gain = std::rand() % 100; i_max = std::rand() % 100; i_min = -1 * std::rand() % 100; - pid1.set_gains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); + u_max = std::rand() % 100; + u_min = -1 * std::rand() % 100; + trk_tc = std::rand() % 100; + saturation = false; + antiwindup = false; + antiwindup_strat = AntiwindupStrategy::NONE; + + pid1.set_gains( + p_gain, i_gain, d_gain, i_max, i_min, u_max, u_min, trk_tc, saturation, antiwindup, + antiwindup_strat); Pid::Gains g1 = pid1.get_gains(); EXPECT_EQ(p_gain, g1.p_gain_); @@ -217,11 +529,12 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(d_gain, g1.d_gain_); EXPECT_EQ(i_max, g1.i_max_); EXPECT_EQ(i_min, g1.i_min_); + EXPECT_EQ(u_max, g1.u_max_); + EXPECT_EQ(u_min, g1.u_min_); + EXPECT_EQ(trk_tc, g1.trk_tc_); + EXPECT_EQ(saturation, g1.saturation_); EXPECT_EQ(antiwindup, g1.antiwindup_); - - // \todo test initParam() ------------------------------------------------- - - // \todo test bool init(const ros::NodeHandle &n); ----------------------------------- + EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); // Send update command to populate errors ------------------------------------------------- pid1.set_current_cmd(10); @@ -231,14 +544,20 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid2(pid1); pid2.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); + p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, + u_min_return, trk_tc_return, saturation_return, antiwindup_return, antiwindup_strat_return); - EXPECT_EQ(p_gain, p_gain_return); - EXPECT_EQ(i_gain, i_gain_return); - EXPECT_EQ(d_gain, d_gain_return); - EXPECT_EQ(i_max, i_max_return); - EXPECT_EQ(i_min, i_min_return); - EXPECT_EQ(antiwindup, antiwindup_return); + EXPECT_EQ(p_gain, g1.p_gain_); + EXPECT_EQ(i_gain, g1.i_gain_); + EXPECT_EQ(d_gain, g1.d_gain_); + EXPECT_EQ(i_max, g1.i_max_); + EXPECT_EQ(i_min, g1.i_min_); + EXPECT_EQ(u_max, g1.u_max_); + EXPECT_EQ(u_min, g1.u_min_); + EXPECT_EQ(trk_tc, g1.trk_tc_); + EXPECT_EQ(saturation, g1.saturation_); + EXPECT_EQ(antiwindup, g1.antiwindup_); + EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); // Test that errors are zero double pe2, ie2, de2; @@ -252,14 +571,20 @@ TEST(ParameterTest, gainSettingCopyPIDTest) pid3 = pid1; pid3.get_gains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); + p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, u_max_return, + u_min_return, trk_tc_return, saturation_return, antiwindup_return, antiwindup_strat_return); - EXPECT_EQ(p_gain, p_gain_return); - EXPECT_EQ(i_gain, i_gain_return); - EXPECT_EQ(d_gain, d_gain_return); - EXPECT_EQ(i_max, i_max_return); - EXPECT_EQ(i_min, i_min_return); - EXPECT_EQ(antiwindup, antiwindup_return); + EXPECT_EQ(p_gain, g1.p_gain_); + EXPECT_EQ(i_gain, g1.i_gain_); + EXPECT_EQ(d_gain, g1.d_gain_); + EXPECT_EQ(i_max, g1.i_max_); + EXPECT_EQ(i_min, g1.i_min_); + EXPECT_EQ(u_max, g1.u_max_); + EXPECT_EQ(u_min, g1.u_min_); + EXPECT_EQ(trk_tc, g1.trk_tc_); + EXPECT_EQ(saturation, g1.saturation_); + EXPECT_EQ(antiwindup, g1.antiwindup_); + EXPECT_EQ(antiwindup_strat, g1.antiwindup_strat_); // Test that errors are zero double pe3, ie3, de3; @@ -443,6 +768,182 @@ TEST(CommandTest, completePIDTest) EXPECT_EQ(-3.5, cmd); } +TEST(CommandTest, backCalculationPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "back calculation technique."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, AntiwindupStrategy::BACK_CALCULATION); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(1.0, ie); + EXPECT_EQ(0.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(3.0, ie); + EXPECT_EQ(1.0, cmd); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(3.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(7.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(4.0, cmd); +} + +TEST(CommandTest, conditioningTechniquePIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "conditioning technique."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, + AntiwindupStrategy::CONDITIONING_TECHNIQUE); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(1.0, ie); + EXPECT_EQ(0.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(3.0, ie); + EXPECT_EQ(1.0, cmd); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(3.0, cmd); + + // Saturation applied, conditioning technique now reduces the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, conditioning technique now reduces the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(7.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, conditioning technique now reduces the integral term + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(4.0, cmd); +} + +TEST(CommandTest, conditionalIntegrationPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "conditional integration technique."); + + // Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, + // double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat); + Pid pid( + 0.0, 1.0, 0.0, 0.0, 0.0, 5.0, -5.0, 1.0, true, false, + AntiwindupStrategy::CONDITIONAL_INTEGRATION); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(1.0, ie); + EXPECT_EQ(0.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(3.0, ie); + EXPECT_EQ(1.0, cmd); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(3.0, cmd); + + // Saturation applied, conditional integration now holds the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, conditional integration now holds the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(0.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(5.0, cmd); +} + TEST(CommandTest, timeArgumentTest) { RecordProperty("description", "Tests different dt argument type methods.");