diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 13f8553a..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() @@ -113,18 +123,27 @@ class CONTROL_TOOLBOX_PUBLIC Pid */ struct Gains { - // Optional constructor for passing in values without antiwindup + // 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) + : 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 + // 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) + : 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 + 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 +152,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 +171,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 +196,19 @@ 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 Clear the saved integrator output of this controller + */ + void clear_saved_iterm(); + /*! * \brief Get PID gains for the controller. * \param p The proportional gain. @@ -194,6 +220,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 +240,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. diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index cf55eb3c..1333dce5 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 = false); /*! * \brief Initialize the PID controller based on already set parameters @@ -153,10 +155,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..cea3f442 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -46,13 +46,16 @@ 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); + setGains(p, i, d, i_max, i_min, antiwindup, save_iterm); + + // Initialize saved i-term values + clear_saved_iterm(); reset(); } @@ -62,15 +65,19 @@ 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(); } @@ -79,19 +86,35 @@ void Pid::reset() { p_error_last_ = 0.0; p_error_ = 0.0; - i_error_ = 0.0; d_error_ = 0.0; cmd_ = 0.0; + + // 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(); + // Check to see if we should reset integral error here + if (!gains.save_iterm_) i_error_ = 0.0; +} + +void Pid::clear_saved_iterm() +{ + i_error_ = 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 +124,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); } diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 58d0f609..1c69d374 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(); } @@ -237,7 +241,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,9 +252,10 @@ 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); } } @@ -309,6 +315,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 +363,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..9bb3eeef 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 = true; - 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_TRUE(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 = true; - 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;