From 90c5c221fddf7248db732967ff54c7680f33583b Mon Sep 17 00:00:00 2001 From: Alex White Date: Wed, 19 Apr 2023 16:15:10 -0700 Subject: [PATCH 01/13] Added adaptive PID functionality to PID controller Added save_iterm_ to PID gains Added save_iterm() and clear_saved_iterm() methods to update and clear save integral term Cleared saved integral term in constructor Called save_iterm() in reset() method Added saved integral term to PID output --- include/control_toolbox/pid.hpp | 44 ++++++++++++++++++--- include/control_toolbox/pid_ros.hpp | 13 +++++- src/pid.cpp | 61 ++++++++++++++++++++++++----- src/pid_ros.cpp | 26 ++++++++---- test/pid_parameters_tests.cpp | 25 +++++++++--- test/pid_publisher_tests.cpp | 4 +- test/pid_tests.cpp | 18 ++++++--- 7 files changed, 153 insertions(+), 38 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 13f8553a..3ccb7a69 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -113,9 +113,10 @@ class CONTROL_TOOLBOX_PUBLIC Pid */ struct Gains { - // Optional constructor for passing in values without antiwindup + // Optional constructor for passing in values without antiwindup/save i-term 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_(false) + : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(false), + save_iterm_(false) { } // Optional constructor for passing in values @@ -123,8 +124,15 @@ class CONTROL_TOOLBOX_PUBLIC Pid : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup) { } + // Optional constructor for passing in values + Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup, + bool save_iterm) : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), + antiwindup_(antiwindup), save_iterm_(save_iterm) + { + } // 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), + antiwindup_(false), save_iterm_(false) { } double p_gain_; /**< Proportional gain. */ @@ -133,6 +141,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid double i_max_; /**< Maximum allowable integral term. */ double i_min_; /**< Minimum allowable integral term. */ bool antiwindup_; /**< Antiwindup. */ + bool save_iterm_; /**< Save integral term. */ }; /*! @@ -151,7 +160,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid */ 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); + bool antiwindup = false, bool save_iterm = false); /** * \brief Copy constructor required for preventing mutexes from being copied @@ -176,13 +185,24 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \note New gains are not applied if i_min_ > i_max_ */ - void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false, + bool save_iterm = false); /*! * \brief Reset the state of this PID controller */ void reset(); + /*! + * \brief Save the integrator output of this controller + */ + void save_iterm(); + + /*! + * \brief Clear the saved the integrator output of this controller + */ + void clear_saved_iterm(); + /*! * \brief Get PID gains for the controller. * \param p The proportional gain. @@ -194,6 +214,9 @@ class CONTROL_TOOLBOX_PUBLIC Pid void getGains(double & p, double & i, double & d, double & i_max, double & i_min); void getGains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup); + void getGains( + double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup, + bool & save_iterm); /*! * \brief Get PID gains for the controller. @@ -211,7 +234,8 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \note New gains are not applied if i_min > i_max */ - void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false, + bool save_iterm = false); /*! * \brief Set PID gains for the controller. @@ -288,14 +312,22 @@ class CONTROL_TOOLBOX_PUBLIC Pid return *this; } + /*! + * \brief Return saved integral term + */ + double getSavedITerm() {return i_term_saved_; } + protected: // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without // blocking the realtime update loop realtime_tools::RealtimeBuffer gains_buffer_; + double min_i_term_ = 0.01; // Minimum value for saving integral term double p_error_last_; /**< _Save position state for derivative state calculation. */ double p_error_; /**< Position error. */ double i_error_; /**< Integral of position error. */ + double i_term_saved_; /**< Retained integral output */ + double i_term_last_; /**< Last integrator term output */ double d_error_; /**< Derivative of position error. */ double cmd_; /**< Command to send. */ double error_dot_; /**< Derivative error */ diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index cf55eb3c..2b9391d6 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -98,10 +98,12 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * \param i_max The max integral windup. * \param i_min The min integral windup. * \param antiwindup antiwindup. + * \param save_iterm save integrator output between resets. * * \note New gains are not applied if i_min_ > i_max_ */ - void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup); + void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup, + bool save_iterm); /*! * \brief Initialize the PID controller based on already set parameters @@ -114,6 +116,11 @@ class CONTROL_TOOLBOX_PUBLIC PidROS */ void reset(); + /*! + * \brief Save the integrator output of this controller + */ + void save_iterm(); + /*! * \brief Set the PID error and compute the PID command with nonuniform time * step size. The derivative error is computed from the change in the error @@ -153,10 +160,12 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * \param i_max The max integral windup. * \param i_min The min integral windup. * \param antiwindup antiwindup. + * \param save_iterm save integrator output between resets. * * \note New gains are not applied if i_min > i_max */ - void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false, + bool save_iterm = false); /*! * \brief Set PID gains for the controller. diff --git a/src/pid.cpp b/src/pid.cpp index 282aafb2..28e26e54 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -46,13 +46,15 @@ namespace control_toolbox { -Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) +Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_iterm) : gains_buffer_() { if (i_min > i_max) { throw std::invalid_argument("received i_min > i_max"); } - setGains(p, i, d, i_max, i_min, antiwindup); + // Initialize saved i-term values + clear_saved_iterm(); + setGains(p, i, d, i_max, i_min, antiwindup, save_iterm); reset(); } @@ -62,21 +64,27 @@ Pid::Pid(const Pid & source) // Copy the realtime buffer to the new PID class gains_buffer_ = source.gains_buffer_; + // Initialize saved i-term values + clear_saved_iterm(); + // Reset the state of this PID controller reset(); } Pid::~Pid() {} -void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup, + bool save_iterm) { - setGains(p, i, d, i_max, i_min, antiwindup); + setGains(p, i, d, i_max, i_min, antiwindup, save_iterm); reset(); } void Pid::reset() { + save_iterm(); + p_error_last_ = 0.0; p_error_ = 0.0; i_error_ = 0.0; @@ -84,14 +92,39 @@ void Pid::reset() cmd_ = 0.0; } +void Pid::save_iterm() +{ + // If last integral term is less than min_i_term_, just return - don't keep very small terms + if (std::fabs(i_term_last_) < min_i_term_) return; + + // Get the gain parameters from the realtime buffer + Gains gains = *gains_buffer_.readFromRT(); + + if (gains.save_iterm_) { + i_term_saved_ += i_term_last_; + // Limit saved integrator output to max integrator output + i_term_saved_ = std::clamp(i_term_saved_, gains.i_min_, gains.i_max_); + } else { + i_term_saved_ = 0.0; + } + i_term_last_ = 0.0; +} + +void Pid::clear_saved_iterm() +{ + i_term_saved_ = 0.0; + i_term_last_ = 0.0; +} + void Pid::getGains(double & p, double & i, double & d, double & i_max, double & i_min) { - bool antiwindup; - getGains(p, i, d, i_max, i_min, antiwindup); + bool antiwindup, save_iterm; + getGains(p, i, d, i_max, i_min, antiwindup, save_iterm); } void Pid::getGains( - double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup) + double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup, + bool &save_iterm) { Gains gains = *gains_buffer_.readFromRT(); @@ -101,13 +134,15 @@ void Pid::getGains( i_max = gains.i_max_; i_min = gains.i_min_; antiwindup = gains.antiwindup_; + save_iterm = gains.save_iterm_; } Pid::Gains Pid::getGains() { return *gains_buffer_.readFromRT(); } -void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup, + bool save_iterm) { - Gains gains(p, i, d, i_max, i_min, antiwindup); + Gains gains(p, i, d, i_max, i_min, antiwindup, save_iterm); setGains(gains); } @@ -171,12 +206,18 @@ double Pid::computeCommand(double error, double error_dot, uint64_t dt) // Limit i_term so that the limit is meaningful in the output i_term = std::clamp(i_term, gains.i_min_, gains.i_max_); } + i_term_last_ = i_term; + + if (!gains.save_iterm_) { + // reset saved integrator term if disabled + i_term_saved_ = 0.0; + } // Calculate derivative contribution to command d_term = gains.d_gain_ * d_error_; // Compute the command - cmd_ = p_term + i_term + d_term; + cmd_ = p_term + i_term + i_term_saved_ + d_term; return cmd_; } diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 58d0f609..1b84c96d 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -169,6 +169,7 @@ bool PidROS::initPid() double p, i, d, i_min, i_max; p = i = d = i_min = i_max = std::numeric_limits::quiet_NaN(); bool antiwindup = false; + bool save_iterm = false; bool all_params_available = true; all_params_available &= getDoubleParam(param_prefix_ + "p", p); all_params_available &= getDoubleParam(param_prefix_ + "i", i); @@ -177,12 +178,13 @@ bool PidROS::initPid() all_params_available &= getDoubleParam(param_prefix_ + "i_clamp_min", i_min); getBooleanParam(param_prefix_ + "antiwindup", antiwindup); + getBooleanParam(param_prefix_ + "save_iterm", save_iterm); if (all_params_available) { setParameterEventCallback(); } - pid_.initPid(p, i, d, i_max, i_min, antiwindup); + pid_.initPid(p, i, d, i_max, i_min, antiwindup, save_iterm); return all_params_available; } @@ -194,12 +196,13 @@ void PidROS::declareParam(const std::string & param_name, rclcpp::ParameterValue } } -void PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup, + bool save_iterm) { if (i_min > i_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } else { - pid_.initPid(p, i, d, i_max, i_min, antiwindup); + pid_.initPid(p, i, d, i_max, i_min, antiwindup, save_iterm); declareParam(param_prefix_ + "p", rclcpp::ParameterValue(p)); declareParam(param_prefix_ + "i", rclcpp::ParameterValue(i)); @@ -207,6 +210,7 @@ void PidROS::initPid(double p, double i, double d, double i_max, double i_min, b declareParam(param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(i_max)); declareParam(param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(i_min)); declareParam(param_prefix_ + "antiwindup", rclcpp::ParameterValue(antiwindup)); + declareParam(param_prefix_ + "save_iterm", rclcpp::ParameterValue(save_iterm)); setParameterEventCallback(); } @@ -214,6 +218,8 @@ void PidROS::initPid(double p, double i, double d, double i_max, double i_min, b void PidROS::reset() { pid_.reset(); } +void PidROS::save_iterm() { pid_.save_iterm(); } + std::shared_ptr> PidROS::getPidStatePublisher() { return state_pub_; @@ -237,7 +243,8 @@ double PidROS::computeCommand(double error, double error_dot, rclcpp::Duration d Pid::Gains PidROS::getGains() { return pid_.getGains(); } -void PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup, + bool save_iterm) { if (i_min > i_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); @@ -247,11 +254,11 @@ void PidROS::setGains(double p, double i, double d, double i_max, double i_min, rclcpp::Parameter(param_prefix_ + "d", d), rclcpp::Parameter(param_prefix_ + "i_clamp_max", i_max), rclcpp::Parameter(param_prefix_ + "i_clamp_min", i_min), - rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup)}); + rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup), + rclcpp::Parameter(param_prefix_ + "save_iterm", save_iterm)}); - pid_.setGains(p, i, d, i_max, i_min, antiwindup); + pid_.setGains(p, i, d, i_max, i_min, antiwindup, save_iterm); } -} void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt) { @@ -309,6 +316,8 @@ void PidROS::printValues() << " I_Min: " << gains.i_min_ << "\n" << " Antiwindup: " << gains.antiwindup_ << "\n" + << " Save I-Term: " << gains.save_iterm_ + << "\n" << " P_Error: " << p_error_ << "\n" << " I_Error: " << i_error_ << "\n" << " D_Error: " << d_error_ << "\n" @@ -355,6 +364,9 @@ void PidROS::setParameterEventCallback() } else if (param_name == param_prefix_ + "antiwindup") { gains.antiwindup_ = parameter.get_value(); changed = true; + } else if (param_name == param_prefix_ + "save_iterm") { + gains.save_iterm_ = parameter.get_value(); + changed = true; } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Please use the right type: " << e.what()); diff --git a/test/pid_parameters_tests.cpp b/test/pid_parameters_tests.cpp index 9a26d659..7a9d4a81 100644 --- a/test/pid_parameters_tests.cpp +++ b/test/pid_parameters_tests.cpp @@ -57,8 +57,9 @@ void check_set_parameters( const double I_MAX = 10.0; const double I_MIN = -10.0; const bool ANTIWINDUP = true; + const bool SAVE_ITERM = false; - ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP)); + ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM)); rclcpp::Parameter param; @@ -81,6 +82,9 @@ void check_set_parameters( ASSERT_TRUE(node->get_parameter(prefix + "antiwindup", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP); + ASSERT_TRUE(node->get_parameter(prefix + "save_iterm", param)); + ASSERT_EQ(param.get_value(), SAVE_ITERM); + // check gains were set control_toolbox::Pid::Gains gains = pid.getGains(); ASSERT_EQ(gains.p_gain_, P); @@ -89,6 +93,7 @@ void check_set_parameters( ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_TRUE(gains.antiwindup_); + ASSERT_FALSE(gains.save_iterm_); } TEST(PidParametersTest, InitPidTest) @@ -215,8 +220,9 @@ TEST(PidParametersTest, SetParametersTest) const double I_MAX = 10.0; const double I_MIN = -10.0; const bool ANTIWINDUP = true; + const bool SAVE_ITERM = false; - pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM); rcl_interfaces::msg::SetParametersResult set_result; @@ -237,6 +243,8 @@ TEST(PidParametersTest, SetParametersTest) 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("save_iterm", SAVE_ITERM))); + ASSERT_TRUE(set_result.successful); // process callbacks rclcpp::spin_some(node->get_node_base_interface()); @@ -249,6 +257,7 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); + ASSERT_EQ(gains.save_iterm_, SAVE_ITERM); } TEST(PidParametersTest, SetBadParametersTest) @@ -313,9 +322,10 @@ TEST(PidParametersTest, GetParametersTest) const double I_MAX = 10.0; const double I_MIN = -10.0; const bool ANTIWINDUP = true; + const bool SAVE_ITERM = false; - pid.initPid(0.0, 0.0, 0.0, 0.0, 0.0, false); - pid.setGains(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.initPid(0.0, 0.0, 0.0, 0.0, 0.0, false, false); + pid.setGains(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM); rclcpp::Parameter param; @@ -336,6 +346,9 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("antiwindup", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP); + + ASSERT_TRUE(node->get_parameter("save_iterm", param)); + ASSERT_EQ(param.get_value(), SAVE_ITERM); } TEST(PidParametersTest, GetParametersFromParams) @@ -380,8 +393,8 @@ TEST(PidParametersTest, MultiplePidInstances) const double I_MAX = 10.0; const double I_MIN = -10.0; - ASSERT_NO_THROW(pid_1.initPid(P, I, D, I_MAX, I_MIN, false)); - ASSERT_NO_THROW(pid_2.initPid(P, I, D, I_MAX, I_MIN, true)); + ASSERT_NO_THROW(pid_1.initPid(P, I, D, I_MAX, I_MIN, false, false)); + ASSERT_NO_THROW(pid_2.initPid(P, I, D, I_MAX, I_MIN, true, false)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); diff --git a/test/pid_publisher_tests.cpp b/test/pid_publisher_tests.cpp index 1c3de5f1..48abc05b 100644 --- a/test/pid_publisher_tests.cpp +++ b/test/pid_publisher_tests.cpp @@ -39,7 +39,7 @@ TEST(PidPublisherTest, PublishTest) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); - pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false); + pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -77,7 +77,7 @@ TEST(PidPublisherTest, PublishTestLifecycle) pid_ros.getPidStatePublisher()); // state_pub_lifecycle_->on_activate(); - pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false); + pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index b8ef5635..50e3cf55 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -182,15 +182,17 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double i_max = std::rand() % 100; double i_min = -1 * std::rand() % 100; bool antiwindup = false; + bool save_iterm = false; // Initialize the default way Pid pid1(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); // Test return values ------------------------------------------------- double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return; - bool antiwindup_return; + bool antiwindup_return, save_iterm_return; pid1.getGains( - 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, antiwindup_return, + save_iterm_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); @@ -198,6 +200,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_max, i_max_return); EXPECT_EQ(i_min, i_min_return); EXPECT_EQ(antiwindup, antiwindup_return); + EXPECT_EQ(save_iterm, save_iterm_return); // Test return values using struct ------------------------------------------------- @@ -207,7 +210,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) d_gain = std::rand() % 100; i_max = std::rand() % 100; i_min = -1 * std::rand() % 100; - pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); + pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup, save_iterm); Pid::Gains g1 = pid1.getGains(); EXPECT_EQ(p_gain, g1.p_gain_); @@ -216,6 +219,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_max, g1.i_max_); EXPECT_EQ(i_min, g1.i_min_); EXPECT_EQ(antiwindup, g1.antiwindup_); + EXPECT_EQ(save_iterm, g1.save_iterm_); // \todo test initParam() ------------------------------------------------- @@ -229,7 +233,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid2(pid1); pid2.getGains( - 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, antiwindup_return, + save_iterm_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); @@ -237,6 +242,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_max, i_max_return); EXPECT_EQ(i_min, i_min_return); EXPECT_EQ(antiwindup, antiwindup_return); + EXPECT_EQ(save_iterm, save_iterm_return); // Test that errors are zero double pe2, ie2, de2; @@ -250,7 +256,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) pid3 = pid1; pid3.getGains( - 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, antiwindup_return, + save_iterm_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); @@ -258,6 +265,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_max, i_max_return); EXPECT_EQ(i_min, i_min_return); EXPECT_EQ(antiwindup, antiwindup_return); + EXPECT_EQ(save_iterm, save_iterm_return); // Test that errors are zero double pe3, ie3, de3; From 3aaab3806c225a3e1281c6af7f3a764b51eca53b Mon Sep 17 00:00:00 2001 From: Alex White Date: Thu, 20 Apr 2023 08:02:01 -0700 Subject: [PATCH 02/13] Updates from PR comments Simplified saving of integral term --- include/control_toolbox/pid.hpp | 18 +++------------ include/control_toolbox/pid_ros.hpp | 7 +----- src/pid.cpp | 35 +++++++---------------------- src/pid_ros.cpp | 2 -- 4 files changed, 12 insertions(+), 50 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 3ccb7a69..5226a957 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -121,7 +121,8 @@ class CONTROL_TOOLBOX_PUBLIC Pid } // Optional constructor for passing in values 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), antiwindup_(antiwindup), + save_iterm_(false) { } // Optional constructor for passing in values @@ -194,12 +195,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid void reset(); /*! - * \brief Save the integrator output of this controller - */ - void save_iterm(); - - /*! - * \brief Clear the saved the integrator output of this controller + * \brief Clear the saved integrator output of this controller */ void clear_saved_iterm(); @@ -312,22 +308,14 @@ class CONTROL_TOOLBOX_PUBLIC Pid return *this; } - /*! - * \brief Return saved integral term - */ - double getSavedITerm() {return i_term_saved_; } - protected: // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without // blocking the realtime update loop realtime_tools::RealtimeBuffer gains_buffer_; - double min_i_term_ = 0.01; // Minimum value for saving integral term double p_error_last_; /**< _Save position state for derivative state calculation. */ double p_error_; /**< Position error. */ double i_error_; /**< Integral of position error. */ - double i_term_saved_; /**< Retained integral output */ - double i_term_last_; /**< Last integrator term output */ double d_error_; /**< Derivative of position error. */ double cmd_; /**< Command to send. */ double error_dot_; /**< Derivative error */ diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index 2b9391d6..1333dce5 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -103,7 +103,7 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * \note New gains are not applied if i_min_ > i_max_ */ void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup, - bool save_iterm); + bool save_iterm = false); /*! * \brief Initialize the PID controller based on already set parameters @@ -116,11 +116,6 @@ class CONTROL_TOOLBOX_PUBLIC PidROS */ void reset(); - /*! - * \brief Save the integrator output of this controller - */ - void save_iterm(); - /*! * \brief Set the PID error and compute the PID command with nonuniform time * step size. The derivative error is computed from the change in the error diff --git a/src/pid.cpp b/src/pid.cpp index 28e26e54..b654a77b 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -52,9 +52,10 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwind if (i_min > i_max) { throw std::invalid_argument("received i_min > i_max"); } + setGains(p, i, d, i_max, i_min, antiwindup, save_iterm); + // Initialize saved i-term values clear_saved_iterm(); - setGains(p, i, d, i_max, i_min, antiwindup, save_iterm); reset(); } @@ -83,37 +84,23 @@ void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool void Pid::reset() { - save_iterm(); - p_error_last_ = 0.0; p_error_ = 0.0; - i_error_ = 0.0; d_error_ = 0.0; cmd_ = 0.0; -} -void Pid::save_iterm() -{ - // If last integral term is less than min_i_term_, just return - don't keep very small terms - if (std::fabs(i_term_last_) < min_i_term_) return; + // If last integral error is already zero, just return + if (std::fabs(i_error_) < std::numeric_limits::epsilon()) return; // Get the gain parameters from the realtime buffer Gains gains = *gains_buffer_.readFromRT(); - - if (gains.save_iterm_) { - i_term_saved_ += i_term_last_; - // Limit saved integrator output to max integrator output - i_term_saved_ = std::clamp(i_term_saved_, gains.i_min_, gains.i_max_); - } else { - i_term_saved_ = 0.0; - } - i_term_last_ = 0.0; + // Check to see if we should reset integral error here + if (!gains.save_iterm_) i_error_ = 0.0; } void Pid::clear_saved_iterm() { - i_term_saved_ = 0.0; - i_term_last_ = 0.0; + i_error_ = 0.0; } void Pid::getGains(double & p, double & i, double & d, double & i_max, double & i_min) @@ -206,18 +193,12 @@ double Pid::computeCommand(double error, double error_dot, uint64_t dt) // Limit i_term so that the limit is meaningful in the output i_term = std::clamp(i_term, gains.i_min_, gains.i_max_); } - i_term_last_ = i_term; - - if (!gains.save_iterm_) { - // reset saved integrator term if disabled - i_term_saved_ = 0.0; - } // Calculate derivative contribution to command d_term = gains.d_gain_ * d_error_; // Compute the command - cmd_ = p_term + i_term + i_term_saved_ + d_term; + cmd_ = p_term + i_term + d_term; return cmd_; } diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 1b84c96d..23572150 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -218,8 +218,6 @@ void PidROS::initPid(double p, double i, double d, double i_max, double i_min, b void PidROS::reset() { pid_.reset(); } -void PidROS::save_iterm() { pid_.save_iterm(); } - std::shared_ptr> PidROS::getPidStatePublisher() { return state_pub_; From 2fa429a45875eaa4dca7de8a4e1e1e95e544937b Mon Sep 17 00:00:00 2001 From: Whalex451 <101365645+Whalex451@users.noreply.github.com> Date: Thu, 20 Apr 2023 11:19:12 -0700 Subject: [PATCH 03/13] Apply suggestions from code review Co-authored-by: Dr. Denis --- include/control_toolbox/pid.hpp | 2 +- src/pid.cpp | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 5226a957..b858d278 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -113,7 +113,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid */ struct Gains { - // Optional constructor for passing in values without antiwindup/save i-term + // Optional constructor for passing in values without antiwindup and save i-term 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_(false), save_iterm_(false) diff --git a/src/pid.cpp b/src/pid.cpp index b654a77b..63e61ad0 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -90,7 +90,10 @@ void Pid::reset() cmd_ = 0.0; // If last integral error is already zero, just return - if (std::fabs(i_error_) < std::numeric_limits::epsilon()) return; + if (std::fabs(i_error_) < std::numeric_limits::epsilon()) + { + return; + } // Get the gain parameters from the realtime buffer Gains gains = *gains_buffer_.readFromRT(); @@ -111,7 +114,7 @@ void Pid::getGains(double & p, double & i, double & d, double & i_max, double & void Pid::getGains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup, - bool &save_iterm) + bool & save_iterm) { Gains gains = *gains_buffer_.readFromRT(); From 0d196f28034d09f4f009bf3679e3eeda845c51ba Mon Sep 17 00:00:00 2001 From: Alex White Date: Thu, 20 Apr 2023 12:00:58 -0700 Subject: [PATCH 04/13] Applied more suggestions from code review --- include/control_toolbox/pid.hpp | 12 +++++++++++- src/pid.cpp | 2 +- test/pid_parameters_tests.cpp | 6 +++--- 3 files changed, 15 insertions(+), 5 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index b858d278..cb0531b4 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -57,6 +57,14 @@ namespace control_toolbox be subclassed to provide more specific controls based on a particular control loop. + This class also allows for retention of integral + term on reset. This is useful for control loops + that are enabled/disabled with a constant steady-state + external disturbance. Once the integrator cancels + out the external disturbance, disabling/resetting/ + re-enabling closed-loop control does not require + the integrator to wind up again. + In particular, this class implements the standard pid equation: @@ -83,6 +91,8 @@ namespace control_toolbox \param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$ + \param save_iterm boolean indicating if integral term is retained on reset() + \section Usage To use the Pid class, you should first call some version of init() @@ -119,7 +129,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid save_iterm_(false) { } - // Optional constructor for passing in values + // Optional constructor for passing in values without save i-term 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), save_iterm_(false) diff --git a/src/pid.cpp b/src/pid.cpp index 63e61ad0..cea3f442 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -90,7 +90,7 @@ void Pid::reset() cmd_ = 0.0; // If last integral error is already zero, just return - if (std::fabs(i_error_) < std::numeric_limits::epsilon()) + if (std::fabs(i_error_) < std::numeric_limits::epsilon()) { return; } diff --git a/test/pid_parameters_tests.cpp b/test/pid_parameters_tests.cpp index 7a9d4a81..9bb3eeef 100644 --- a/test/pid_parameters_tests.cpp +++ b/test/pid_parameters_tests.cpp @@ -57,7 +57,7 @@ void check_set_parameters( const double I_MAX = 10.0; const double I_MIN = -10.0; const bool ANTIWINDUP = true; - const bool SAVE_ITERM = false; + const bool SAVE_ITERM = true; ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM)); @@ -93,7 +93,7 @@ void check_set_parameters( ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_TRUE(gains.antiwindup_); - ASSERT_FALSE(gains.save_iterm_); + ASSERT_TRUE(gains.save_iterm_); } TEST(PidParametersTest, InitPidTest) @@ -322,7 +322,7 @@ TEST(PidParametersTest, GetParametersTest) const double I_MAX = 10.0; const double I_MIN = -10.0; const bool ANTIWINDUP = true; - const bool SAVE_ITERM = false; + const bool SAVE_ITERM = true; pid.initPid(0.0, 0.0, 0.0, 0.0, 0.0, false, false); pid.setGains(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM); From e67dd314bedef6e674cc1bd1427f7ad411ea3b1b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 12 Feb 2024 11:22:50 +0100 Subject: [PATCH 05/13] Fixup for compilation. --- src/pid_ros.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 23572150..1c69d374 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -257,6 +257,7 @@ void PidROS::setGains(double p, double i, double d, double i_max, double i_min, pid_.setGains(p, i, d, i_max, i_min, antiwindup, save_iterm); } +} void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt) { From b82835330ca8ed20e8fd6b7c65aa78be7c4d3556 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 4 Nov 2024 23:34:18 +0100 Subject: [PATCH 06/13] Update docstring --- include/control_toolbox/pid.hpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 877b3f25..574f95dd 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -148,7 +148,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \param d The derivative gain. * \param i_max The max integral windup. * \param i_min The min integral windup. - * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced + * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced. * * \throws An std::invalid_argument exception is thrown if i_min > i_max */ @@ -158,7 +158,19 @@ class CONTROL_TOOLBOX_PUBLIC Pid { } - // Optional constructor for passing in values + /*! + * \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 The max integral windup. + * \param i_min The min integral windup. + * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced. + * \param save_iterm If true, save integral term when PID is reset. + * + * \throws An std::invalid_argument exception is thrown if i_min > i_max + */ Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_iterm) : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup), save_iterm_(save_iterm) From 92f7d6f5151dc77a79d4645752617de94ab41375 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 11 Jan 2025 21:24:24 +0000 Subject: [PATCH 07/13] Rename PidRos test files --- CMakeLists.txt | 10 +++++----- ...rameters_tests.cpp => pid_ros_parameters_tests.cpp} | 0 ...publisher_tests.cpp => pid_ros_publisher_tests.cpp} | 0 3 files changed, 5 insertions(+), 5 deletions(-) rename test/{pid_parameters_tests.cpp => pid_ros_parameters_tests.cpp} (100%) rename test/{pid_publisher_tests.cpp => pid_ros_publisher_tests.cpp} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index e8bbc3e7..38261158 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -139,12 +139,12 @@ if(BUILD_TESTING) ament_add_gmock(rate_limiter_tests test/rate_limiter.cpp) target_link_libraries(rate_limiter_tests control_toolbox) - ament_add_gtest(pid_parameters_tests test/pid_parameters_tests.cpp) - target_link_libraries(pid_parameters_tests control_toolbox) + ament_add_gtest(pid_ros_parameters_tests test/pid_ros_parameters_tests.cpp) + target_link_libraries(pid_ros_parameters_tests control_toolbox) - ament_add_gtest(pid_publisher_tests test/pid_publisher_tests.cpp) - target_link_libraries(pid_publisher_tests control_toolbox) - ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) + ament_add_gtest(pid_ros_publisher_tests test/pid_ros_publisher_tests.cpp) + target_link_libraries(pid_ros_publisher_tests control_toolbox) + ament_target_dependencies(pid_ros_publisher_tests rclcpp_lifecycle) ## Control Filters # exponential_filter diff --git a/test/pid_parameters_tests.cpp b/test/pid_ros_parameters_tests.cpp similarity index 100% rename from test/pid_parameters_tests.cpp rename to test/pid_ros_parameters_tests.cpp diff --git a/test/pid_publisher_tests.cpp b/test/pid_ros_publisher_tests.cpp similarity index 100% rename from test/pid_publisher_tests.cpp rename to test/pid_ros_publisher_tests.cpp From f917eb0c691401836bb6ffa8a999accc3f597ba5 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 11 Jan 2025 21:24:53 +0000 Subject: [PATCH 08/13] Add test for save_iterm --- test/pid_tests.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index 882a9749..db42f1e4 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -353,6 +353,27 @@ TEST(CommandTest, integralOnlyTest) cmd = pid.computeCommand(1.0, static_cast(1.0 * 1e9)); // Expect that the command is cleared since error = -1 * previous command, i-gain = 1, dt = 1 EXPECT_EQ(0.0, cmd); + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); + // Then expect command = error + EXPECT_EQ(-0.5, cmd); + // after reset we expect the command to be 0 if update is called error = 0 + pid.reset(); + cmd = pid.computeCommand(0.0, static_cast(1.0 * 1e9)); + EXPECT_EQ(0.0, cmd); + + // enable save_iterm + pid.setGains(0.0, 1.0, 0.0, 5.0, -5.0, false, true); + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); + // Then expect command = error + EXPECT_EQ(-0.5, cmd); + // after reset we expect still the same command if update is called error = 0 + pid.reset(); + cmd = pid.computeCommand(0.0, static_cast(1.0 * 1e9)); + EXPECT_EQ(-0.5, cmd); } TEST(CommandTest, derivativeOnlyTest) From 0f92493ccae1d45986995dee68ad309ac83efc4d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 11 Jan 2025 21:26:28 +0000 Subject: [PATCH 09/13] Revert "Rename PidRos test files" This reverts commit 92f7d6f5151dc77a79d4645752617de94ab41375. --- CMakeLists.txt | 10 +++++----- ...s_parameters_tests.cpp => pid_parameters_tests.cpp} | 0 ...ros_publisher_tests.cpp => pid_publisher_tests.cpp} | 0 3 files changed, 5 insertions(+), 5 deletions(-) rename test/{pid_ros_parameters_tests.cpp => pid_parameters_tests.cpp} (100%) rename test/{pid_ros_publisher_tests.cpp => pid_publisher_tests.cpp} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 38261158..e8bbc3e7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -139,12 +139,12 @@ if(BUILD_TESTING) ament_add_gmock(rate_limiter_tests test/rate_limiter.cpp) target_link_libraries(rate_limiter_tests control_toolbox) - ament_add_gtest(pid_ros_parameters_tests test/pid_ros_parameters_tests.cpp) - target_link_libraries(pid_ros_parameters_tests control_toolbox) + ament_add_gtest(pid_parameters_tests test/pid_parameters_tests.cpp) + target_link_libraries(pid_parameters_tests control_toolbox) - ament_add_gtest(pid_ros_publisher_tests test/pid_ros_publisher_tests.cpp) - target_link_libraries(pid_ros_publisher_tests control_toolbox) - ament_target_dependencies(pid_ros_publisher_tests rclcpp_lifecycle) + ament_add_gtest(pid_publisher_tests test/pid_publisher_tests.cpp) + target_link_libraries(pid_publisher_tests control_toolbox) + ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) ## Control Filters # exponential_filter diff --git a/test/pid_ros_parameters_tests.cpp b/test/pid_parameters_tests.cpp similarity index 100% rename from test/pid_ros_parameters_tests.cpp rename to test/pid_parameters_tests.cpp diff --git a/test/pid_ros_publisher_tests.cpp b/test/pid_publisher_tests.cpp similarity index 100% rename from test/pid_ros_publisher_tests.cpp rename to test/pid_publisher_tests.cpp From 16e7dcdcfc5433059b3894938aca2c42aa4eadc1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 13 Jan 2025 21:45:31 +0100 Subject: [PATCH 10/13] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- src/pid.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pid.cpp b/src/pid.cpp index f874da62..9d7db45a 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -90,7 +90,7 @@ void Pid::reset() cmd_ = 0.0; // If last integral error is already zero, just return - if (std::fabs(i_error_) < std::numeric_limits::epsilon()) + if (std::abs(i_error_) < std::numeric_limits::epsilon()) { return; } @@ -98,7 +98,7 @@ void Pid::reset() // Get the gain parameters from the realtime buffer Gains gains = *gains_buffer_.readFromRT(); // Check to see if we should reset integral error here - if (!gains.save_iterm_) i_error_ = 0.0; + if (!gains.save_iterm_) clear_saved_iterm(); } void Pid::clear_saved_iterm() From 36fabbc138bed14d0de8588f31e4d27c60bddec0 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 23 Jan 2025 12:15:08 +0000 Subject: [PATCH 11/13] Make the save_iterm parameter an argument of reset() instead --- include/control_toolbox/pid.hpp | 49 +++++++++-------------------- include/control_toolbox/pid_ros.hpp | 27 +++++++++++++--- src/pid.cpp | 31 +++++++++--------- src/pid_ros.cpp | 36 ++++++++++++--------- test/pid_parameters_tests.cpp | 7 ++--- test/pid_tests.cpp | 37 +++++++++++----------- 6 files changed, 94 insertions(+), 93 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index b6574aa4..3fb6fbd6 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -86,8 +86,6 @@ namespace control_toolbox \param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$ - \param save_iterm boolean indicating if integral term is retained on reset() - \section Usage To use the Pid class, you should first call some version of init() @@ -130,8 +128,7 @@ class Pid * \throws An std::invalid_argument exception is thrown if i_min > i_max */ 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_(false), - save_iterm_(false) + : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(false) { } @@ -148,33 +145,13 @@ class Pid * \throws An std::invalid_argument exception is thrown if i_min > i_max */ 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), - save_iterm_(false) - { - } - - /*! - * \brief Optional constructor for passing in values - * - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. - * \param i_max The max integral windup. - * \param i_min The min integral windup. - * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced. - * \param save_iterm If true, save integral term when PID is reset. - * - * \throws An std::invalid_argument exception is thrown if i_min > i_max - */ - Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup, - bool save_iterm) : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), - antiwindup_(antiwindup), save_iterm_(save_iterm) + : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup) { } // 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), save_iterm_(false) + antiwindup_(false) { } @@ -184,7 +161,6 @@ class Pid double i_max_; /**< Maximum allowable integral term. */ double i_min_; /**< Minimum allowable integral term. */ bool antiwindup_; /**< Antiwindup. */ - bool save_iterm_; /**< Save integral term. */ }; /*! @@ -203,7 +179,7 @@ class Pid */ 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, bool save_iterm = false); + bool antiwindup = false); /** * \brief Copy constructor required for preventing mutexes from being copied @@ -229,14 +205,21 @@ class Pid * * \note New gains are not applied if i_min_ > i_max_ */ - void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false, - bool save_iterm = false); + void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! * \brief Reset the state of this PID controller + * */ void reset(); + /*! + * \brief Reset the state of this PID controller + * + * \param save_iterm boolean indicating if integral term is retained on reset() + */ + void reset(bool save_iterm); + /*! * \brief Clear the saved integrator output of this controller */ @@ -262,9 +245,6 @@ class Pid */ void getGains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup); - void getGains( - double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup, - bool & save_iterm); /*! * \brief Get PID gains for the controller. @@ -283,8 +263,7 @@ class Pid * * \note New gains are not applied if i_min > i_max */ - void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false, - bool save_iterm = false); + void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! * \brief Set PID gains for the controller. diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index 0ede6aba..500ddac8 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -89,6 +89,19 @@ class PidROS rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, std::string prefix = std::string(""), bool prefix_is_for_params = 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 antiwindup antiwindup. + * + * \note New gains are not applied if i_min_ > i_max_ + */ + void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup); + /*! * \brief Initialize the PID controller and set the parameters * \param p The proportional gain. @@ -102,7 +115,7 @@ class PidROS * \note New gains are not applied if i_min_ > i_max_ */ void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup, - bool save_iterm = false); + bool save_iterm); /*! * \brief Initialize the PID controller based on already set parameters @@ -112,9 +125,17 @@ class PidROS /*! * \brief Reset the state of this PID controller + * */ void reset(); + /*! + * \brief Reset the state of this PID controller + * + * \param save_iterm boolean indicating if integral term is retained on reset() + */ + void reset(bool save_iterm = false); + /*! * \brief Set the PID error and compute the PID command with nonuniform time * step size. The derivative error is computed from the change in the error @@ -154,12 +175,10 @@ class PidROS * \param i_max The max integral windup. * \param i_min The min integral windup. * \param antiwindup antiwindup. - * \param save_iterm save integrator output between resets. * * \note New gains are not applied if i_min > i_max */ - void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false, - bool save_iterm = false); + void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! * \brief Set PID gains for the controller. diff --git a/src/pid.cpp b/src/pid.cpp index 9d7db45a..8d0e0a1d 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -46,13 +46,13 @@ namespace control_toolbox { -Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_iterm) +Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) : gains_buffer_() { if (i_min > i_max) { throw std::invalid_argument("received i_min > i_max"); } - setGains(p, i, d, i_max, i_min, antiwindup, save_iterm); + setGains(p, i, d, i_max, i_min, antiwindup); // Initialize saved i-term values clear_saved_iterm(); @@ -74,15 +74,19 @@ Pid::Pid(const Pid & source) Pid::~Pid() {} -void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup, - bool save_iterm) +void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - setGains(p, i, d, i_max, i_min, antiwindup, save_iterm); + setGains(p, i, d, i_max, i_min, antiwindup); reset(); } void Pid::reset() +{ + reset(false); +} + +void Pid::reset(bool save_iterm) { p_error_last_ = 0.0; p_error_ = 0.0; @@ -95,10 +99,8 @@ void Pid::reset() return; } - // Get the gain parameters from the realtime buffer - Gains gains = *gains_buffer_.readFromRT(); // Check to see if we should reset integral error here - if (!gains.save_iterm_) clear_saved_iterm(); + if (!save_iterm) clear_saved_iterm(); } void Pid::clear_saved_iterm() @@ -108,13 +110,12 @@ void Pid::clear_saved_iterm() void Pid::getGains(double & p, double & i, double & d, double & i_max, double & i_min) { - bool antiwindup, save_iterm; - getGains(p, i, d, i_max, i_min, antiwindup, save_iterm); + bool antiwindup; + getGains(p, i, d, i_max, i_min, antiwindup); } void Pid::getGains( - double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup, - bool & save_iterm) + double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup) { Gains gains = *gains_buffer_.readFromRT(); @@ -124,15 +125,13 @@ void Pid::getGains( i_max = gains.i_max_; i_min = gains.i_min_; antiwindup = gains.antiwindup_; - save_iterm = gains.save_iterm_; } Pid::Gains Pid::getGains() { return *gains_buffer_.readFromRT(); } -void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup, - bool save_iterm) +void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - Gains gains(p, i, d, i_max, i_min, antiwindup, save_iterm); + Gains gains(p, i, d, i_max, i_min, antiwindup); setGains(gains); } diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 21f7e62d..4acca59b 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -169,7 +169,6 @@ bool PidROS::initPid() double p, i, d, i_min, i_max; p = i = d = i_min = i_max = std::numeric_limits::quiet_NaN(); bool antiwindup = false; - bool save_iterm = false; bool all_params_available = true; all_params_available &= getDoubleParam(param_prefix_ + "p", p); all_params_available &= getDoubleParam(param_prefix_ + "i", i); @@ -178,13 +177,13 @@ bool PidROS::initPid() all_params_available &= getDoubleParam(param_prefix_ + "i_clamp_min", i_min); getBooleanParam(param_prefix_ + "antiwindup", antiwindup); - getBooleanParam(param_prefix_ + "save_iterm", save_iterm); + declareParam(param_prefix_ + "save_iterm", rclcpp::ParameterValue(false)); if (all_params_available) { setParameterEventCallback(); } - pid_.initPid(p, i, d, i_max, i_min, antiwindup, save_iterm); + pid_.initPid(p, i, d, i_max, i_min, antiwindup); return all_params_available; } @@ -196,13 +195,18 @@ void PidROS::declareParam(const std::string & param_name, rclcpp::ParameterValue } } +void PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup) +{ + initPid(p, i, d, i_max, i_min, antiwindup, false); +} + void PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_iterm) { if (i_min > i_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } else { - pid_.initPid(p, i, d, i_max, i_min, antiwindup, save_iterm); + pid_.initPid(p, i, d, i_max, i_min, antiwindup); declareParam(param_prefix_ + "p", rclcpp::ParameterValue(p)); declareParam(param_prefix_ + "i", rclcpp::ParameterValue(i)); @@ -216,7 +220,16 @@ void PidROS::initPid(double p, double i, double d, double i_max, double i_min, b } } -void PidROS::reset() { pid_.reset(); } +void PidROS::reset() { + bool save_iterm = false; + getBooleanParam(param_prefix_ + "save_iterm", save_iterm); + reset(save_iterm); +} + +void PidROS::reset(bool save_iterm) +{ + pid_.reset(save_iterm); +} std::shared_ptr> PidROS::getPidStatePublisher() { @@ -241,8 +254,7 @@ double PidROS::computeCommand(double error, double error_dot, rclcpp::Duration d Pid::Gains PidROS::getGains() { return pid_.getGains(); } -void PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup, - bool save_iterm) +void PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { if (i_min > i_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); @@ -252,10 +264,9 @@ void PidROS::setGains(double p, double i, double d, double i_max, double i_min, rclcpp::Parameter(param_prefix_ + "d", d), rclcpp::Parameter(param_prefix_ + "i_clamp_max", i_max), rclcpp::Parameter(param_prefix_ + "i_clamp_min", i_min), - rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup), - rclcpp::Parameter(param_prefix_ + "save_iterm", save_iterm)}); + rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup)}); - pid_.setGains(p, i, d, i_max, i_min, antiwindup, save_iterm); + pid_.setGains(p, i, d, i_max, i_min, antiwindup); } } @@ -315,8 +326,6 @@ void PidROS::printValues() << " I_Min: " << gains.i_min_ << "\n" << " Antiwindup: " << gains.antiwindup_ << "\n" - << " Save I-Term: " << gains.save_iterm_ - << "\n" << " P_Error: " << p_error_ << "\n" << " I_Error: " << i_error_ << "\n" << " D_Error: " << d_error_ << "\n" @@ -363,9 +372,6 @@ void PidROS::setParameterEventCallback() } else if (param_name == param_prefix_ + "antiwindup") { gains.antiwindup_ = parameter.get_value(); changed = true; - } else if (param_name == param_prefix_ + "save_iterm") { - gains.save_iterm_ = parameter.get_value(); - changed = true; } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Please use the right type: " << e.what()); diff --git a/test/pid_parameters_tests.cpp b/test/pid_parameters_tests.cpp index c9ccb3dc..7a0c57b7 100644 --- a/test/pid_parameters_tests.cpp +++ b/test/pid_parameters_tests.cpp @@ -93,7 +93,6 @@ void check_set_parameters( ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_TRUE(gains.antiwindup_); - ASSERT_TRUE(gains.save_iterm_); } TEST(PidParametersTest, InitPidTest) @@ -257,7 +256,6 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_EQ(gains.antiwindup_, ANTIWINDUP); - ASSERT_EQ(gains.save_iterm_, SAVE_ITERM); } TEST(PidParametersTest, SetBadParametersTest) @@ -322,10 +320,9 @@ TEST(PidParametersTest, GetParametersTest) const double I_MAX = 10.0; const double I_MIN = -10.0; const bool ANTIWINDUP = true; - const bool SAVE_ITERM = true; pid.initPid(0.0, 0.0, 0.0, 0.0, 0.0, false, false); - pid.setGains(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM); + pid.setGains(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rclcpp::Parameter param; @@ -348,7 +345,7 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_EQ(param.get_value(), ANTIWINDUP); ASSERT_TRUE(node->get_parameter("save_iterm", param)); - ASSERT_EQ(param.get_value(), SAVE_ITERM); + ASSERT_EQ(param.get_value(), false); } TEST(PidParametersTest, GetParametersFromParams) diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index db42f1e4..75d237bd 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -182,17 +182,15 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double i_max = std::rand() % 100; double i_min = -1 * std::rand() % 100; bool antiwindup = false; - bool save_iterm = false; // Initialize the default way Pid pid1(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); // Test return values ------------------------------------------------- double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return; - bool antiwindup_return, save_iterm_return; + bool antiwindup_return; pid1.getGains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return, - save_iterm_return); + p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); @@ -200,7 +198,6 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_max, i_max_return); EXPECT_EQ(i_min, i_min_return); EXPECT_EQ(antiwindup, antiwindup_return); - EXPECT_EQ(save_iterm, save_iterm_return); // Test return values using struct ------------------------------------------------- @@ -210,7 +207,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) d_gain = std::rand() % 100; i_max = std::rand() % 100; i_min = -1 * std::rand() % 100; - pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup, save_iterm); + pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); Pid::Gains g1 = pid1.getGains(); EXPECT_EQ(p_gain, g1.p_gain_); @@ -219,7 +216,6 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_max, g1.i_max_); EXPECT_EQ(i_min, g1.i_min_); EXPECT_EQ(antiwindup, g1.antiwindup_); - EXPECT_EQ(save_iterm, g1.save_iterm_); // \todo test initParam() ------------------------------------------------- @@ -233,8 +229,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid2(pid1); pid2.getGains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return, - save_iterm_return); + p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); @@ -242,7 +237,6 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_max, i_max_return); EXPECT_EQ(i_min, i_min_return); EXPECT_EQ(antiwindup, antiwindup_return); - EXPECT_EQ(save_iterm, save_iterm_return); // Test that errors are zero double pe2, ie2, de2; @@ -256,8 +250,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) pid3 = pid1; pid3.getGains( - p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return, - save_iterm_return); + p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); @@ -265,7 +258,6 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_max, i_max_return); EXPECT_EQ(i_min, i_min_return); EXPECT_EQ(antiwindup, antiwindup_return); - EXPECT_EQ(save_iterm, save_iterm_return); // Test that errors are zero double pe3, ie3, de3; @@ -358,20 +350,29 @@ TEST(CommandTest, integralOnlyTest) cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); // Then expect command = error EXPECT_EQ(-0.5, cmd); - // after reset we expect the command to be 0 if update is called error = 0 + // after reset without argument (save_iterm=false) + // we expect the command to be 0 if update is called error = 0 pid.reset(); cmd = pid.computeCommand(0.0, static_cast(1.0 * 1e9)); EXPECT_EQ(0.0, cmd); - // enable save_iterm - pid.setGains(0.0, 1.0, 0.0, 5.0, -5.0, false, true); + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); + // Then expect command = error + EXPECT_EQ(-0.5, cmd); + // after reset with argument (save_iterm=false) + // we expect the command to be 0 if update is called error = 0 + pid.reset(false); + cmd = pid.computeCommand(0.0, static_cast(1.0 * 1e9)); + EXPECT_EQ(0.0, cmd); // If initial error = 0, i-gain = 1, dt = 1 cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); // Then expect command = error EXPECT_EQ(-0.5, cmd); - // after reset we expect still the same command if update is called error = 0 - pid.reset(); + // after reset with save_iterm=true + // we expect still the same command if update is called error = 0 + pid.reset(true); cmd = pid.computeCommand(0.0, static_cast(1.0 * 1e9)); EXPECT_EQ(-0.5, cmd); } From 8c7a6bc1ebfefa6fa1dcb4aa795bfb39a26024a9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 23 Jan 2025 12:19:09 +0000 Subject: [PATCH 12/13] Remove some unneeded changes --- include/control_toolbox/pid.hpp | 10 ++++------ include/control_toolbox/pid_ros.hpp | 1 - 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 3fb6fbd6..3f431f71 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -117,7 +117,7 @@ class Pid struct Gains { /*! - * \brief Optional constructor for passing in values without antiwindup and save i-term + * \brief Optional constructor for passing in values without antiwindup * * \param p The proportional gain. * \param i The integral gain. @@ -128,12 +128,12 @@ class Pid * \throws An std::invalid_argument exception is thrown if i_min > i_max */ 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_(false) + : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(true) { } /*! - * \brief Optional constructor for passing in values without save i-term + * \brief Optional constructor for passing in values * * \param p The proportional gain. * \param i The integral gain. @@ -150,8 +150,7 @@ class Pid } // 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), antiwindup_(false) { } @@ -209,7 +208,6 @@ class Pid /*! * \brief Reset the state of this PID controller - * */ void reset(); diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index 500ddac8..120d6452 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -125,7 +125,6 @@ class PidROS /*! * \brief Reset the state of this PID controller - * */ void reset(); From 410c61d28d6db519647cb30e95712d9d8b23a3eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 23 Jan 2025 14:33:18 +0100 Subject: [PATCH 13/13] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- include/control_toolbox/pid.hpp | 1 + include/control_toolbox/pid_ros.hpp | 3 ++- src/pid.cpp | 6 ------ 3 files changed, 3 insertions(+), 7 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 3f431f71..38c8a7aa 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -208,6 +208,7 @@ class Pid /*! * \brief Reset the state of this PID controller + * @note The integral term is not retained and it is reset to zero */ void reset(); diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index 120d6452..5f82b029 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -125,6 +125,7 @@ class PidROS /*! * \brief Reset the state of this PID controller + * @note The integral term is not retained and it is reset to zero */ void reset(); @@ -133,7 +134,7 @@ class PidROS * * \param save_iterm boolean indicating if integral term is retained on reset() */ - void reset(bool save_iterm = false); + void reset(bool save_iterm); /*! * \brief Set the PID error and compute the PID command with nonuniform time diff --git a/src/pid.cpp b/src/pid.cpp index 8d0e0a1d..2a0ed340 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -93,12 +93,6 @@ void Pid::reset(bool save_iterm) d_error_ = 0.0; cmd_ = 0.0; - // If last integral error is already zero, just return - if (std::abs(i_error_) < std::numeric_limits::epsilon()) - { - return; - } - // Check to see if we should reset integral error here if (!save_iterm) clear_saved_iterm(); }