Fix negative gains issue and add tests; update gains setting through …

…DynamicReconfig

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. */
   };
 
   /*!
@@ -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.

src/pid.cpp

@@ -48,7 +48,7 @@ static const std::string DEFAULT_NAMESPACE = "pid"; // \todo better default pref
 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();
 }
@@ -147,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();
@@ -182,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();
 
@@ -191,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);
 }
@@ -223,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);
 }
@@ -242,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);
 }
@@ -263,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)
@@ -366,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

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

test/pid_tests.cpp

@@ -78,9 +78,12 @@ TEST(ParameterTest, integrationClampZeroGainTest)
 
 TEST(ParameterTest, integrationAntiwindupTest)
 {
-  RecordProperty("description","This test succeeds if the integral error is prevented from winding up.");
+  RecordProperty("description","This test succeeds if the integral error is prevented from winding up when i_gain > 0");
 
-  Pid pid(0.0, 2.0, 0.0, 1.0, -1.0, true);
+  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;
@@ -98,6 +101,31 @@ TEST(ParameterTest, integrationAntiwindupTest)
   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.");
@@ -108,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 -------------------------------------------------
 
@@ -130,14 +161,15 @@ 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_);
   EXPECT_EQ(i_gain, g1.i_gain_);
   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() -------------------------------------------------
 
@@ -152,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;
@@ -171,13 +204,14 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
   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;