diff --git a/include/gz/math/PID.hh b/include/gz/math/PID.hh index bf40356c8..1317caf7d 100644 --- a/include/gz/math/PID.hh +++ b/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 &_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 &_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. diff --git a/src/PID.cc b/src/PID.cc index c80989121..4e873182a 100644 --- a/src/PID.cc +++ b/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 &_dt) +{ + if (_dt == std::chrono::duration(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(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; diff --git a/src/PID_TEST.cc b/src/PID_TEST.cc index ff58a25bf..e31b672db 100644 --- a/src/PID_TEST.cc +++ b/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(10.0)); + EXPECT_DOUBLE_EQ(result, -5.5); + + result = pid.Update(5.0, 1.0, std::chrono::duration(0.0)); + EXPECT_DOUBLE_EQ(result, 0); } ///////////////////////////////////////////////// diff --git a/src/python_pybind11/src/PID.cc b/src/python_pybind11/src/PID.cc index 9e273d1a6..f3b81bef2 100644 --- a/src/python_pybind11/src/PID.cc +++ b/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 &>(&Class::Update), + "Update the Pid loop with nonuniform time step size, and custom error " + "rate.") + .def("update", + py::overload_cast &>(&Class::Update), "Update the Pid loop with nonuniform time step size.") .def("set_cmd", &Class::SetCmd, diff --git a/src/ruby/PID.i b/src/ruby/PID.i index 7861cfcc2..b38be1d94 100644 --- a/src/ruby/PID.i +++ b/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(dt)); } + double Update(const double error, double errorRate, const double dt) { + return (*$self).Update(error, errorRate, + std::chrono::duration(dt)); + } + } } }