diff --git a/CMakeLists.txt b/CMakeLists.txt index adcd0536..1408b451 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,7 @@ generate_dynamic_reconfigure_options( add_service_files( FILES SetPidGains.srv + GetPidGains.srv ) generate_messages( diff --git a/cfg/Parameters.cfg b/cfg/Parameters.cfg index 7f5e4247..4b25f4da 100755 --- a/cfg/Parameters.cfg +++ b/cfg/Parameters.cfg @@ -45,6 +45,7 @@ def generate(gen): gen.add( "d" , double_t, 1,"Derivative gain.", 1.0 , 0 , 1000) gen.add( "i_clamp_min" , double_t, 1,"Min bounds for the integral windup", -10.0 , -1000 , 0) gen.add( "i_clamp_max" , double_t, 1,"Max bounds for the integral windup", 10.0 , 0 , 1000) + gen.add( "antiwindup" , bool_t, 1,"Antiwindup.", True) # PkgName #NodeName #Prefix for generated .h include file, e.g. ParametersConfig.py exit(gen.generate(PACKAGE, "control_toolbox", "Parameters")) diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index d39e45c9..3ffa51ab 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -120,20 +120,22 @@ class Pid struct Gains { // Optional constructor for passing in values - Gains(double p, double i, double d, double i_max, double i_min) + 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) + i_min_(i_min), + antiwindup_(antiwindup) {} // Default constructor Gains() {} - 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 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. */ }; /*! @@ -147,7 +149,7 @@ class Pid * \param i_max The max integral windup. * \param i_min The min integral windup. */ - Pid(double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0); + 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 Copy constructor required for preventing mutexes from being copied @@ -170,7 +172,7 @@ class Pid * \param i_max The max integral windup. * \param i_min The min integral windup. */ - void initPid(double p, double i, double d, double i_max, double i_min); + void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup); /*! * \brief Zeros out Pid values and initialize Pid-gains and integral term limits @@ -182,7 +184,7 @@ class Pid * \param i_max The max integral windup. * \param i_min The min integral windup. */ - void initPid(double p, double i, double d, double i_max, double i_min, const ros::NodeHandle &node); + void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup, const ros::NodeHandle &node); /*! * \brief Initialize PID with the parameters in a namespace @@ -229,7 +231,7 @@ class Pid * \param i_max The max integral windup. * \param i_min The min integral windup. */ - 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); /*! * \brief Get PID gains for the controller. @@ -245,7 +247,7 @@ class Pid * \param i_max The max integral windup. * \param i_min The min integral windup. */ - void setGains(double p, double i, double d, double i_max, double i_min); + void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup); /*! * \brief Set PID gains for the controller. @@ -368,6 +370,8 @@ class Pid // blocking the realtime update loop realtime_tools::RealtimeBuffer gains_buffer_; + bool antiwindup_; + double p_error_last_; /**< _Save position state for derivative state calculation. */ double p_error_; /**< Position error. */ double i_error_; /**< Integral of position error. */ diff --git a/include/control_toolbox/pid_gains_setter.h b/include/control_toolbox/pid_gains_setter.h index b7114b0a..977442e1 100644 --- a/include/control_toolbox/pid_gains_setter.h +++ b/include/control_toolbox/pid_gains_setter.h @@ -39,6 +39,7 @@ #include "ros/node_handle.h" #include "control_toolbox/pid.h" #include "control_toolbox/SetPidGains.h" +#include "control_toolbox/GetPidGains.h" namespace control_toolbox { @@ -87,19 +88,25 @@ class PidGainsSetter /** * \brief Advertises the "set_gains" service, initializing the PidGainsSetter */ - void advertise(const ros::NodeHandle &n); + void advertise(const std::string name_postfix, const ros::NodeHandle &n); /** * \brief Advertises the "set_gains" service, initializing the PidGainsSetter */ - void advertise(const std::string &ns) { advertise(ros::NodeHandle(ns)); } + void advertise(const std::string name_postfix, const std::string &ns) { advertise(name_postfix, ros::NodeHandle(ns)); } bool setGains(control_toolbox::SetPidGains::Request &req, control_toolbox::SetPidGains::Response &resp); + bool getGains(control_toolbox::GetPidGains::Request &req, + control_toolbox::GetPidGains::Response &resp); + private: ros::NodeHandle node_; + ros::ServiceServer serve_set_gains_; + ros::ServiceServer serve_get_gains_; + std::vector pids_; }; diff --git a/src/pid.cpp b/src/pid.cpp index 5c89eab5..3fdf0524 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -45,10 +45,10 @@ namespace control_toolbox { static const std::string DEFAULT_NAMESPACE = "pid"; // \todo better default prefix? -Pid::Pid(double p, double i, double d, double i_max, double i_min) - : dynamic_reconfig_initialized_(false) +Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) + : dynamic_reconfig_initialized_(false), antiwindup_(antiwindup) { - setGains(p,i,d,i_max,i_min); + setGains(p,i,d,i_max,i_min,antiwindup); reset(); } @@ -67,19 +67,19 @@ Pid::~Pid() { } -void Pid::initPid(double p, double i, double d, double i_max, double i_min, +void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup, const ros::NodeHandle &node) { - initPid(p, i, d, i_max, i_min); + initPid(p, i, d, i_max, i_min, antiwindup); // Create node handle for dynamic reconfigure ros::NodeHandle nh(DEFAULT_NAMESPACE); initDynamicReconfig(nh); } -void Pid::initPid(double p, double i, double d, double i_max, double i_min) +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); + setGains(p,i,d,i_max,i_min, antiwindup); reset(); } @@ -124,6 +124,8 @@ bool Pid::init(const ros::NodeHandle &node, const bool quiet) gains.i_max_ = std::abs(gains.i_max_); // make sure the value is >= 0 } + nh.param("antiwindup", antiwindup_, false); + setGains(gains); reset(); @@ -145,7 +147,8 @@ bool Pid::initXml(TiXmlElement *config) config->Attribute("i") ? atof(config->Attribute("i")) : 0.0, config->Attribute("d") ? atof(config->Attribute("d")) : 0.0, std::abs(i_clamp), - -std::abs(i_clamp) + -std::abs(i_clamp), + config->Attribute("antiwindup") ? atof(config->Attribute("antiwindup")) : false ); reset(); @@ -180,7 +183,7 @@ void Pid::reset() cmd_ = 0.0; } -void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min) +void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup) { Gains gains = *gains_buffer_.readFromRT(); @@ -189,6 +192,7 @@ void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min d = gains.d_gain_; i_max = gains.i_max_; i_min = gains.i_min_; + antiwindup = gains.antiwindup_; } Pid::Gains Pid::getGains() @@ -196,9 +200,9 @@ Pid::Gains Pid::getGains() return *gains_buffer_.readFromRT(); } -void Pid::setGains(double p, double i, double d, double i_max, double i_min) +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); + Gains gains(p,i,d,i_max,i_min, antiwindup); setGains(gains); } @@ -221,7 +225,7 @@ void Pid::updateDynamicReconfig() control_toolbox::ParametersConfig config; // Get starting values - getGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min); + getGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min, config.antiwindup); updateDynamicReconfig(config); } @@ -240,6 +244,7 @@ void Pid::updateDynamicReconfig(Gains gains_config) config.d = gains_config.d_gain_; config.i_clamp_max = gains_config.i_max_; config.i_clamp_min = gains_config.i_min_; + config.antiwindup = gains_config.antiwindup_; updateDynamicReconfig(config); } @@ -261,7 +266,7 @@ void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uin ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved."); // Set the gains - setGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min); + setGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min, config.antiwindup); } double Pid::computeCommand(double error, ros::Duration dt) @@ -299,18 +304,26 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt) if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot)) return 0.0; - // Calculate proportional contribution to command p_term = gains.p_gain_ * p_error_; // Calculate the integral of the position error i_error_ += dt.toSec() * p_error_; + if(antiwindup_) + { + // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_ + i_error_ = std::max(gains.i_min_ / std::fabs(gains.i_gain_), std::min(i_error_, gains.i_max_ / std::fabs(gains.i_gain_))); + } + // Calculate integral contribution to command i_term = gains.i_gain_ * i_error_; - // Limit i_term so that the limit is meaningful in the output - i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) ); + if(!antiwindup_) + { + // Limit i_term so that the limit is meaningful in the output + i_term = std::max(gains.i_min_, std::min(i_term, gains.i_max_)); + } // Calculate derivative contribution to command d_term = gains.d_gain_ * d_error_; @@ -356,6 +369,7 @@ void Pid::printValues() << " D Gain: " << gains.d_gain_ << "\n" << " I_Max: " << gains.i_max_ << "\n" << " I_Min: " << gains.i_min_ << "\n" + << " Antiwindup: " << gains.antiwindup_ << "\n" << " P_Error_Last: " << p_error_last_ << "\n" << " P_Error: " << p_error_ << "\n" << " I_Error: " << i_error_ << "\n" diff --git a/src/pid_gains_setter.cpp b/src/pid_gains_setter.cpp index 80a5e746..9b136d72 100644 --- a/src/pid_gains_setter.cpp +++ b/src/pid_gains_setter.cpp @@ -39,25 +39,42 @@ PidGainsSetter::~PidGainsSetter() PidGainsSetter& PidGainsSetter::add(Pid *pid) { assert(pid); + + if(pids_.size() > 0){ + //use existing pid config for new added controller + pid->setGains(pids_.begin().operator *()->getGains()); + } + pids_.push_back(pid); + + return *this; } -void PidGainsSetter::advertise(const ros::NodeHandle &n) +void PidGainsSetter::advertise(const std::string name_postfix, const ros::NodeHandle &n) { node_ = n; - serve_set_gains_ = node_.advertiseService("set_gains", &PidGainsSetter::setGains, this); + serve_set_gains_ = node_.advertiseService(std::string("set_gains_") + name_postfix, &PidGainsSetter::setGains, this); + serve_get_gains_ = node_.advertiseService(std::string("get_gains_")+ name_postfix, &PidGainsSetter::getGains, this); } bool PidGainsSetter::setGains(control_toolbox::SetPidGains::Request &req, control_toolbox::SetPidGains::Response &resp) { - for (size_t i = 0; i < pids_.size(); ++i) - pids_[i]->setGains(req.p, req.i, req.d, req.i_clamp, -req.i_clamp); - node_.setParam("p", req.p); - node_.setParam("i", req.i); - node_.setParam("d", req.d); - node_.setParam("i_clamp", req.i_clamp); + for (size_t i = 0; i < pids_.size(); ++i){ + pids_[i]->setGains(req.p, req.i, req.d, req.i_clamp_max, req.i_clamp_min, req.antiwindup); + } + return true; +} + +bool PidGainsSetter::getGains(control_toolbox::GetPidGains::Request &req, + control_toolbox::GetPidGains::Response &resp) +{ + if(pids_.size()>0){ + bool antiwindup = false; + pids_[0]->getGains(resp.p, resp.i, resp.d ,resp.i_clamp_max, resp.i_clamp_min, antiwindup); + resp.antiwindup = antiwindup; + } return true; } diff --git a/srv/GetPidGains.srv b/srv/GetPidGains.srv new file mode 100644 index 00000000..55d1057e --- /dev/null +++ b/srv/GetPidGains.srv @@ -0,0 +1,7 @@ +--- +float64 p +float64 i +float64 d +float64 i_clamp_max +float64 i_clamp_min +bool antiwindup diff --git a/srv/SetPidGains.srv b/srv/SetPidGains.srv index e0dce4a8..c17a673a 100644 --- a/srv/SetPidGains.srv +++ b/srv/SetPidGains.srv @@ -1,5 +1,7 @@ float64 p float64 i float64 d -float64 i_clamp +float64 i_clamp_max +float64 i_clamp_min +bool antiwindup --- diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index 86be85f3..cb84aed6 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -30,9 +30,9 @@ TEST(ParameterTest, zeroITermBadIBoundsTest) EXPECT_FALSE(boost::math::isnan(cmd)); } -TEST(ParameterTest, integrationWindupTest) +TEST(ParameterTest, integrationClampTest) { - RecordProperty("description","This test succeeds if the integral contribution is prevented from winding up when the integral gain is non-zero."); + RecordProperty("description","This test succeeds if the integral contribution is clamped when the integral gain is non-zero."); Pid pid(0.0, 2.0, 0.0, 1.0, -1.0); @@ -44,11 +44,17 @@ TEST(ParameterTest, integrationWindupTest) cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); EXPECT_EQ(-1.0, cmd); + + cmd = pid.computeCommand(1.0, ros::Duration(1.0)); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); + EXPECT_EQ(-1.0, cmd); } -TEST(ParameterTest, integrationWindupZeroGainTest) +TEST(ParameterTest, integrationClampZeroGainTest) { - RecordProperty("description","This test succeeds if the integral contribution is prevented from winding up when the integral gain is zero. If the integral contribution is allowed to wind up while it is disabled, it can cause sudden jumps to the minimum or maximum bound in control command when re-enabled."); + RecordProperty("description","This test succeeds if the integral contribution is clamped when the integral gain is zero. If the integral contribution is not clamped while it is disabled, it can cause sudden jumps to the minimum or maximum bound in control command when re-enabled."); double i_gain = 0.0; double i_min = -1.0; @@ -70,6 +76,56 @@ TEST(ParameterTest, integrationWindupZeroGainTest) EXPECT_EQ(0.0, cmd); } +TEST(ParameterTest, integrationAntiwindupTest) +{ + RecordProperty("description","This test succeeds if the integral error is prevented from winding up when i_gain > 0"); + + double i_gain = 2.0; + double i_min = -1.0; + double i_max = 1.0; + Pid pid(0.0, i_gain, 0.0, i_max, i_min, true); + + double cmd = 0.0; + double pe,ie,de; + + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); + EXPECT_EQ(-1.0, cmd); + + cmd = pid.computeCommand(0.5, ros::Duration(1.0)); + EXPECT_EQ(0.0, cmd); + + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); + EXPECT_EQ(-1.0, cmd); +} + +TEST(ParameterTest, negativeIntegrationAntiwindupTest) +{ + RecordProperty("description","This test succeeds if the integral error is prevented from winding up when i_gain < 0"); + + double i_gain = -2.0; + double i_min = -1.0; + double i_max = 1.0; + Pid pid(0.0, i_gain, 0.0, i_max, i_min, true); + + double cmd = 0.0; + double pe,ie,de; + + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); + EXPECT_EQ(1.0, cmd); + + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); + EXPECT_EQ(1.0, cmd); + + cmd = pid.computeCommand(0.5, ros::Duration(1.0)); + EXPECT_EQ(0.0, cmd); + + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); + EXPECT_EQ(1.0, cmd); +} + TEST(ParameterTest, gainSettingCopyPIDTest) { RecordProperty("description","This test succeeds if a PID object has its gain set at different points in time then the values are get-ed and still remain the same, as well as when PID is copied."); @@ -80,19 +136,22 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double d_gain = rand() % 100; double i_max = rand() % 100; double i_min = -1 * rand() % 100; + bool antiwindup = false; // Initialize the default way - Pid pid1(p_gain, i_gain, d_gain, i_max, i_min); - + 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; - pid1.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return); + bool antiwindup_return; + pid1.getGains(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); 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); // Test return values using struct ------------------------------------------------- @@ -102,7 +161,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) d_gain = rand() % 100; i_max = rand() % 100; i_min = -1 * rand() % 100; - pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min); + 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_); @@ -110,9 +169,10 @@ 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(antiwindup, g1.antiwindup_); // \todo test initParam() ------------------------------------------------- - + // \todo test bool init(const ros::NodeHandle &n); ----------------------------------- @@ -124,13 +184,14 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Test copy constructor ------------------------------------------------- Pid pid2(pid1); - pid2.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return); + pid2.getGains(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); 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); // Test that errors are zero double pe, ie, de; @@ -138,18 +199,19 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(0.0, pe); EXPECT_EQ(0.0, ie); EXPECT_EQ(0.0, de); - + // Test assignment constructor ------------------------------------------------- Pid pid3; pid3 = pid1; - pid3.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return); + pid3.getGains(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); 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); // Test that errors are zero double pe2, ie2, de2;