Added service for reading PIDs as well pid initialisation with weight…

CMakeLists.txt

@@ -23,6 +23,7 @@ generate_dynamic_reconfigure_options(
 add_service_files(
   FILES
   SetPidGains.srv
+  GetPidGains.srv
   )
 
 generate_messages(

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"))
 

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> 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. */

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<Pid*> pids_;
 };
 

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,16 +192,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);
 }
@@ -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"

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;
 }
 

srv/GetPidGains.srv

@@ -0,0 +1,7 @@
+---
+float64 p
+float64 i
+float64 d
+float64 i_clamp_max
+float64 i_clamp_min
+bool antiwindup

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
 ---