Custom PID error rate (gazebosim#525)

* Custom PID error rate

Signed-off-by: Nate Koenig <natekoenig@gmail.com>

* added test

Signed-off-by: Nate Koenig <natekoenig@gmail.com>

---------

Signed-off-by: Nate Koenig <natekoenig@gmail.com>

include/gz/math/PID.hh

@@ -157,6 +157,21 @@ namespace ignition
       /// \return The offset value
       public: double CmdOffset() const;
 
+      /// \brief Update the Pid loop with nonuniform time step size.
+      /// \param[in] _error  Error since last call (p_state - p_target).
+      /// \param[in] _errorRate Estimate of error rate, that can be used
+      /// when a smoother estimate is available than the finite difference
+      /// used by Update(const double _error,
+      /// const std::chrono::duration<double> &_dt)
+      /// \param[in] _dt Change in time since last update call.
+      /// Normally, this is called at every time step,
+      /// The return value is an updated command to be passed
+      /// to the object being controlled.
+      /// \return the command value
+      public: double Update(const double _error,
+                            double _errorRate,
+                            const std::chrono::duration<double> &_dt);
+
       /// \brief Update the Pid loop with nonuniform time step size.
       /// \param[in] _error  Error since last call (p_state - p_target).
       /// \param[in] _dt Change in time since last update call.

src/PID.cc

@@ -141,6 +141,22 @@ double PID::Update(const double _error,
     return 0.0;
   }
 
+  // Pass in the derivative error
+  return this->Update(_error, (_error - this->pErrLast) / _dt.count(), _dt);
+}
+
+/////////////////////////////////////////////////
+double PID::Update(const double _error,
+                   double _errorRate,
+                   const std::chrono::duration<double> &_dt)
+{
+  if (_dt == std::chrono::duration<double>(0) ||
+      isnan(_error) || std::isinf(_error) ||
+      isnan(_errorRate) || std::isinf(_errorRate))
+  {
+    return 0.0;
+  }
+
   double pTerm, dTerm;
   this->pErr = _error;
 
@@ -156,12 +172,9 @@ double PID::Update(const double _error,
   if (this->iMax >= this->iMin)
     this->iErr = clamp(this->iErr, this->iMin, this->iMax);
 
-  // Calculate the derivative error
-  if (_dt != std::chrono::duration<double>(0))
-  {
-    this->dErr = (this->pErr - this->pErrLast) / _dt.count();
-    this->pErrLast = this->pErr;
-  }
+  // Use the provided error rate
+  this->dErr = _errorRate;
+  this->pErrLast = this->pErr;
 
   // Calculate derivative contribution to command
   dTerm = this->dGain * this->dErr;

src/PID_TEST.cc

@@ -172,6 +172,15 @@ TEST(PidTest, Update)
   EXPECT_DOUBLE_EQ(pe, 5);
   EXPECT_DOUBLE_EQ(ie, 1.4);
   EXPECT_DOUBLE_EQ(de, 0.0);
+
+  pid.Reset();
+  pid.SetIGain(0.0);
+  pid.SetIMin(0.0);
+  result = pid.Update(5.0, 1.0, std::chrono::duration<double>(10.0));
+  EXPECT_DOUBLE_EQ(result, -5.5);
+
+  result = pid.Update(5.0, 1.0, std::chrono::duration<double>(0.0));
+  EXPECT_DOUBLE_EQ(result, 0);
 }
 
 /////////////////////////////////////////////////

src/python_pybind11/src/PID.cc

@@ -102,7 +102,13 @@ void defineMathPID(py::module &m, const std::string &typestr)
         &Class::CmdOffset,
         "Get the offset value for the command.")
    .def("update",
-        &Class::Update,
+        py::overload_cast<const double, double,
+        const std::chrono::duration<double> &>(&Class::Update),
+        "Update the Pid loop with nonuniform time step size, and custom error "
+        "rate.")
+   .def("update",
+        py::overload_cast<const double,
+        const std::chrono::duration<double> &>(&Class::Update),
         "Update the Pid loop with nonuniform time step size.")
    .def("set_cmd",
         &Class::SetCmd,

src/ruby/PID.i

@@ -83,6 +83,11 @@ namespace ignition
       double Update(const double error, const double dt) {
         return (*$self).Update(error, std::chrono::duration<double>(dt));
       }
+      double Update(const double error, double errorRate, const double dt) {
+        return (*$self).Update(error, errorRate,
+            std::chrono::duration<double>(dt));
+      }
+
     }
   }
 }