Merge pull request ros-controls#52 from clearpathrobotics/antiwindup-fix

Add antiwindup to PID controller

cfg/Parameters.cfg

@@ -45,6 +45,7 @@ def generate(gen):
 	gen.add( "d" ,      double_t, 1,"Derivative gain.",   1.0 , -1000 , 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.",   False)
 	                 # PkgName  #NodeName         #Prefix for generated .h include file, e.g. ParametersConfig.py
 	exit(gen.generate(PACKAGE, "control_toolbox", "Parameters"))
 

include/control_toolbox/pid.h

@@ -124,20 +124,29 @@ 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. */
+    Gains()
+      : p_gain_(0.0),
+        i_gain_(0.0),
+        d_gain_(0.0),
+        i_max_(0.0),
+        i_min_(0.0),
+        antiwindup_(false)
+    {}
+    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. */
   };
 
   /*!
@@ -151,7 +160,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
@@ -174,7 +183,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
@@ -186,7 +195,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
@@ -233,7 +242,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.
@@ -249,7 +258,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.

src/pid.cpp

@@ -41,14 +41,16 @@
 #include <control_toolbox/pid.h>
 #include <tinyxml.h>
 
+#include <boost/algorithm/clamp.hpp>
+
 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)
+Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
   : dynamic_reconfig_initialized_(false)
 {
-  setGains(p,i,d,i_max,i_min);
+  setGains(p,i,d,i_max,i_min,antiwindup);
 
   reset();
 }
@@ -67,19 +69,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();
 }
@@ -123,6 +125,7 @@ bool Pid::init(const ros::NodeHandle &node, const bool quiet)
     nh.param("i_clamp_max", gains.i_max_, gains.i_max_); // use i_clamp_max parameter, otherwise keep i_clamp
     gains.i_max_ = std::abs(gains.i_max_); // make sure the value is >= 0
   }
+  nh.param("antiwindup", gains.antiwindup_, false);
 
   nh.param("publish_state", publish_state_, false);
 
@@ -151,7 +154,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();
@@ -186,7 +190,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();
 
@@ -195,16 +199,17 @@ 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()
 {
   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);
 }
@@ -227,7 +232,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);
 }
@@ -246,6 +251,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);
 }
@@ -267,7 +273,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)
@@ -311,11 +317,22 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
   // Calculate the integral of the position error
   i_error_ += dt.toSec() * p_error_;
 
+  if(gains.antiwindup_)
+  {
+    // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
+    i_error_ = boost::algorithm::clamp(i_error_,
+                                       gains.i_min_ / std::abs(gains.i_gain_),
+                                       gains.i_max_ / std::abs(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(!gains.antiwindup_)
+  {
+    // Limit i_term so that the limit is meaningful in the output
+    i_term = boost::algorithm::clamp(i_term, gains.i_min_, gains.i_max_);
+  }
 
   // Calculate derivative contribution to command
   d_term = gains.d_gain_ * d_error_;
@@ -380,6 +397,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"

src/pid_gains_setter.cpp

@@ -53,11 +53,12 @@ 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);
+    pids_[i]->setGains(req.p, req.i, req.d, req.i_clamp, -req.i_clamp, req.antiwindup);
   node_.setParam("p", req.p);
   node_.setParam("i", req.i);
   node_.setParam("d", req.d);
   node_.setParam("i_clamp", req.i_clamp);
+  node_.setParam("antiwindup", req.antiwindup);
   return true;
 }
 

srv/SetPidGains.srv

@@ -2,4 +2,5 @@ float64 p
 float64 i
 float64 d
 float64 i_clamp
+bool antiwindup
 ---