diff --git a/include/ignition/math/SpeedLimiter.hh b/include/ignition/math/SpeedLimiter.hh new file mode 100644 index 000000000..0376ff7b4 --- /dev/null +++ b/include/ignition/math/SpeedLimiter.hh @@ -0,0 +1,149 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_MATH_SYSTEMS_SPEEDLIMITER_HH_ +#define IGNITION_MATH_SYSTEMS_SPEEDLIMITER_HH_ + +#include +#include +#include +#include "ignition/math/Helpers.hh" + +namespace ignition +{ +namespace math +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // Forward declaration. + class SpeedLimiterPrivate; + + /// \brief Class to limit velocity, acceleration and jerk. + class IGNITION_MATH_VISIBLE SpeedLimiter + { + /// \brief Constructor. + /// There are no limits by default. + public: SpeedLimiter(); + + /// \brief Destructor. + public: ~SpeedLimiter(); + + /// \brief Set minimum velocity limit in m/s, usually <= 0. + /// \param[in] _lim Minimum velocity. + public: void SetMinVelocity(double _lim); + + /// \brief Get minimum velocity limit, defaults to negative infinity. + /// \return Minimum velocity. + public: double MinVelocity() const; + + /// \brief Set maximum velocity limit in m/s, usually >= 0. + /// \param[in] _lim Maximum velocity. + public: void SetMaxVelocity(double _lim); + + /// \brief Get maximum velocity limit, defaults to positive infinity. + /// \return Maximum velocity. + public: double MaxVelocity() const; + + /// \brief Set minimum acceleration limit in m/s^2, usually <= 0. + /// \param[in] _lim Minimum acceleration. + public: void SetMinAcceleration(double _lim); + + /// \brief Get minimum acceleration limit, defaults to negative infinity. + /// \return Minimum acceleration. + public: double MinAcceleration() const; + + /// \brief Set maximum acceleration limit in m/s^2, usually >= 0. + /// \param[in] _lim Maximum acceleration. + public: void SetMaxAcceleration(double _lim); + + /// \brief Get maximum acceleration limit, defaults to positive infinity. + /// \return Maximum acceleration. + public: double MaxAcceleration() const; + + /// \brief Set minimum jerk limit in m/s^3, usually <= 0. + /// \param[in] _lim Minimum jerk. + public: void SetMinJerk(double _lim); + + /// \brief Get minimum jerk limit, defaults to negative infinity. + /// \return Minimum jerk. + public: double MinJerk() const; + + /// \brief Set maximum jerk limit in m/s^3, usually >= 0. + /// \param[in] _lim Maximum jerk. + public: void SetMaxJerk(double _lim); + + /// \brief Get maximum jerk limit, defaults to positive infinity. + /// \return Maximum jerk. + public: double MaxJerk() const; + + /// \brief Limit velocity, acceleration and jerk. + /// \param [in, out] _vel Velocity to limit [m/s]. + /// \param [in] _prevVel Previous velocity to _vel [m/s]. + /// \param [in] _prevPrevVel Previous velocity to _prevVel [m/s]. + /// \param [in] _dt Time step. + /// \return Limiting difference, which is (out _vel - in _vel). + public: double Limit(double &_vel, + double _prevVel, + double _prevPrevVel, + std::chrono::steady_clock::duration _dt) const; + + /// \brief Limit the velocity. + /// \param [in, out] _vel Velocity to limit [m/s]. + /// \return Limiting difference, which is (out _vel - in _vel). + public: double LimitVelocity(double &_vel) const; + + /// \brief Limit the acceleration using a first-order backward difference + /// method. + /// \param [in, out] _vel Velocity [m/s]. + /// \param [in] _prevVel Previous velocity [m/s]. + /// \param [in] _dt Time step. + /// \return Limiting difference, which is (out _vel - in _vel). + public: double LimitAcceleration( + double &_vel, + double _prevVel, + std::chrono::steady_clock::duration _dt) const; + + /// \brief Limit the jerk using a second-order backward difference method. + /// \param [in, out] _vel Velocity to limit [m/s]. + /// \param [in] _prevVel Previous velocity to v [m/s]. + /// \param [in] _prevPrevVel Previous velocity to prevVel [m/s]. + /// \param [in] _dt Time step. + /// \return Limiting difference, which is (out _vel - in _vel). + /// \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control. + public: double LimitJerk( + double &_vel, + double _prevVel, + double _prevPrevVel, + std::chrono::steady_clock::duration _dt) const; + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + } +} +} + +#endif diff --git a/src/SpeedLimiter.cc b/src/SpeedLimiter.cc new file mode 100644 index 000000000..267c95117 --- /dev/null +++ b/src/SpeedLimiter.cc @@ -0,0 +1,195 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "ignition/math/Helpers.hh" +#include "ignition/math/SpeedLimiter.hh" + +using namespace ignition; +using namespace math; + +/// \brief Private SpeedLimiter data class. +class ignition::math::SpeedLimiterPrivate +{ + /// \brief Minimum velocity limit. + public: double minVelocity{-std::numeric_limits::infinity()}; + + /// \brief Maximum velocity limit. + public: double maxVelocity{std::numeric_limits::infinity()}; + + /// \brief Minimum acceleration limit. + public: double minAcceleration{-std::numeric_limits::infinity()}; + + /// \brief Maximum acceleration limit. + public: double maxAcceleration{std::numeric_limits::infinity()}; + + /// \brief Minimum jerk limit. + public: double minJerk{-std::numeric_limits::infinity()}; + + /// \brief Maximum jerk limit. + public: double maxJerk{std::numeric_limits::infinity()}; +}; + +////////////////////////////////////////////////// +SpeedLimiter::SpeedLimiter() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +SpeedLimiter::~SpeedLimiter() = default; + +////////////////////////////////////////////////// +void SpeedLimiter::SetMinVelocity(double _lim) +{ + this->dataPtr->minVelocity = _lim; +} + +////////////////////////////////////////////////// +double SpeedLimiter::MinVelocity() const +{ + return this->dataPtr->minVelocity; +} + +////////////////////////////////////////////////// +void SpeedLimiter::SetMaxVelocity(double _lim) +{ + this->dataPtr->maxVelocity = _lim; +} + +////////////////////////////////////////////////// +double SpeedLimiter::MaxVelocity() const +{ + return this->dataPtr->maxVelocity; +} + +////////////////////////////////////////////////// +void SpeedLimiter::SetMinAcceleration(double _lim) +{ + this->dataPtr->minAcceleration = _lim; +} + +////////////////////////////////////////////////// +double SpeedLimiter::MinAcceleration() const +{ + return this->dataPtr->minAcceleration; +} + +////////////////////////////////////////////////// +void SpeedLimiter::SetMaxAcceleration(double _lim) +{ + this->dataPtr->maxAcceleration = _lim; +} + +////////////////////////////////////////////////// +double SpeedLimiter::MaxAcceleration() const +{ + return this->dataPtr->maxAcceleration; +} + +////////////////////////////////////////////////// +void SpeedLimiter::SetMinJerk(double _lim) +{ + this->dataPtr->minJerk = _lim; +} + +////////////////////////////////////////////////// +double SpeedLimiter::MinJerk() const +{ + return this->dataPtr->minJerk; +} + +////////////////////////////////////////////////// +void SpeedLimiter::SetMaxJerk(double _lim) +{ + this->dataPtr->maxJerk = _lim; +} + +////////////////////////////////////////////////// +double SpeedLimiter::MaxJerk() const +{ + return this->dataPtr->maxJerk; +} + +////////////////////////////////////////////////// +double SpeedLimiter::Limit(double &_vel, double _prevVel, double _prevPrevVel, + std::chrono::steady_clock::duration _dt) const +{ + const double vUnclamped = _vel; + + this->LimitJerk(_vel, _prevVel, _prevPrevVel, _dt); + this->LimitAcceleration(_vel, _prevVel, _dt); + this->LimitVelocity(_vel); + + return _vel - vUnclamped; +} + +////////////////////////////////////////////////// +double SpeedLimiter::LimitVelocity(double &_vel) const +{ + const double vUnclamped = _vel; + + _vel = ignition::math::clamp( + _vel, this->dataPtr->minVelocity, this->dataPtr->maxVelocity); + + return _vel - vUnclamped; +} + +////////////////////////////////////////////////// +double SpeedLimiter::LimitAcceleration(double &_vel, double _prevVel, + std::chrono::steady_clock::duration _dt) const +{ + const double vUnclamped = _vel; + + const double dtSec = std::chrono::duration(_dt).count(); + + if (equal(dtSec, 0.0)) + return 0.0; + + const double accUnclamped = (_vel - _prevVel) / dtSec; + + const double accClamped = ignition::math::clamp(accUnclamped, + this->dataPtr->minAcceleration, this->dataPtr->maxAcceleration); + + _vel = _prevVel + accClamped * dtSec; + + return _vel - vUnclamped; +} + +////////////////////////////////////////////////// +double SpeedLimiter::LimitJerk(double &_vel, double _prevVel, + double _prevPrevVel, std::chrono::steady_clock::duration _dt) const +{ + const double vUnclamped = _vel; + + const double dtSec = std::chrono::duration(_dt).count(); + + if (equal(dtSec, 0.0)) + return 0.0; + + const double accUnclamped = (_vel - _prevVel) / dtSec; + const double accPrev = (_prevVel - _prevPrevVel) / dtSec; + const double jerkUnclamped = (accUnclamped - accPrev) / dtSec; + + const double jerkClamped = ignition::math::clamp(jerkUnclamped, + this->dataPtr->minJerk, this->dataPtr->maxJerk); + + const double accClamped = accPrev + jerkClamped * dtSec; + + _vel = _prevVel + accClamped * dtSec; + + return _vel - vUnclamped; +} diff --git a/src/SpeedLimiter_TEST.cc b/src/SpeedLimiter_TEST.cc new file mode 100644 index 000000000..08cb39685 --- /dev/null +++ b/src/SpeedLimiter_TEST.cc @@ -0,0 +1,258 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include "ignition/math/Helpers.hh" +#include "ignition/math/SpeedLimiter.hh" + +using namespace ignition; +using namespace math; +using namespace std::literals::chrono_literals; + +///////////////////////////////////////////////// +TEST(SpeedLimiterTest, Default) +{ + // Unlimited by default, the velocity shouldn't change. + SpeedLimiter limiter; + + EXPECT_DOUBLE_EQ(-std::numeric_limits::infinity(), + limiter.MinVelocity()); + EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), + limiter.MaxVelocity()); + EXPECT_DOUBLE_EQ(-std::numeric_limits::infinity(), + limiter.MinAcceleration()); + EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), + limiter.MaxAcceleration()); + EXPECT_DOUBLE_EQ(-std::numeric_limits::infinity(), + limiter.MinJerk()); + EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), + limiter.MaxJerk()); + + double vel{5.0}; + EXPECT_DOUBLE_EQ(0.0, limiter.Limit(vel, 4.0, 3.0, 1ms)); + EXPECT_DOUBLE_EQ(5.0, vel); + + EXPECT_DOUBLE_EQ(0.0, limiter.LimitVelocity(vel)); + EXPECT_DOUBLE_EQ(5.0, vel); + + EXPECT_DOUBLE_EQ(0.0, limiter.LimitAcceleration(vel, 4.0, 1ms)); + EXPECT_DOUBLE_EQ(5.0, vel); + + EXPECT_DOUBLE_EQ(0.0, limiter.LimitJerk(vel, 4.0, 3.0, 1ms)); + EXPECT_DOUBLE_EQ(5.0, vel); + + vel = 0.0; + EXPECT_DOUBLE_EQ(0.0, limiter.Limit(vel, 4.0, 3.0, 1ms)); + EXPECT_DOUBLE_EQ(0.0, vel); +} + +///////////////////////////////////////////////// +TEST(SpeedLimiterTest, LimitVelocity) +{ + double minVel = -1.0; + double maxVel = 4.0; + + SpeedLimiter limiter; + limiter.SetMinVelocity(minVel); + limiter.SetMaxVelocity(maxVel); + + EXPECT_DOUBLE_EQ(minVel, limiter.MinVelocity()); + EXPECT_DOUBLE_EQ(maxVel, limiter.MaxVelocity()); + + // Within bounds + double vel{1.0}; + EXPECT_DOUBLE_EQ(0.0, limiter.LimitVelocity(vel)); + EXPECT_DOUBLE_EQ(1.0, vel); + + // Above upper bound + vel = 5.0; + EXPECT_DOUBLE_EQ(-1.0, limiter.LimitVelocity(vel)); + EXPECT_DOUBLE_EQ(maxVel, vel); + + // Under lower bound + vel = -2.0; + EXPECT_DOUBLE_EQ(1.0, limiter.LimitVelocity(vel)); + EXPECT_DOUBLE_EQ(minVel, vel); +} + +///////////////////////////////////////////////// +TEST(SpeedLimiterTest, LimitAcceleration) +{ + double minAcc = -2.0; + double maxAcc = 4.0; + + SpeedLimiter limiter; + limiter.SetMinAcceleration(minAcc); + limiter.SetMaxAcceleration(maxAcc); + + EXPECT_DOUBLE_EQ(minAcc, limiter.MinAcceleration()); + EXPECT_DOUBLE_EQ(maxAcc, limiter.MaxAcceleration()); + + auto dt = 1s; + const double dtSec = std::chrono::duration(dt).count(); + + // Within bounds + double vel{2.0}; + double velPrev = 1.0; + EXPECT_GT(vel - velPrev, minAcc * dtSec); + EXPECT_LT(vel - velPrev, maxAcc * dtSec); + + EXPECT_DOUBLE_EQ(0.0, limiter.LimitAcceleration(vel, velPrev, dt)); + EXPECT_DOUBLE_EQ(2.0, vel); + + // Above upper bound + vel = 10.0; + velPrev = 1.0; + EXPECT_GT(vel - velPrev, minAcc * dtSec); + EXPECT_GT(vel - velPrev, maxAcc * dtSec); + + EXPECT_DOUBLE_EQ(-5.0, limiter.LimitAcceleration(vel, velPrev, dt)); + EXPECT_DOUBLE_EQ(5.0, vel); + EXPECT_DOUBLE_EQ(vel - velPrev, maxAcc * dtSec); + + // Under lower bound + vel = -8.0; + velPrev = -2.0; + EXPECT_LT(vel - velPrev, minAcc * dtSec); + EXPECT_LT(vel - velPrev, maxAcc * dtSec); + + EXPECT_DOUBLE_EQ(4.0, limiter.LimitAcceleration(vel, velPrev, dt)); + EXPECT_DOUBLE_EQ(-4.0, vel); + EXPECT_DOUBLE_EQ(vel - velPrev, minAcc * dtSec); +} + +///////////////////////////////////////////////// +TEST(SpeedLimiterTest, LimitJerk) +{ + double minJerk = -1.0; + double maxJerk = 1.0; + + SpeedLimiter limiter; + limiter.SetMinJerk(minJerk); + limiter.SetMaxJerk(maxJerk); + + EXPECT_DOUBLE_EQ(minJerk, limiter.MinJerk()); + EXPECT_DOUBLE_EQ(maxJerk, limiter.MaxJerk()); + + auto dt = 1s; + const double dtSec = std::chrono::duration(dt).count(); + + // Within bounds + double vel{2.0}; + double velPrev = 1.0; + double velPrevPrev = 0.5; + double acc = (vel - velPrev) / dtSec; + double accPrev = (velPrev - velPrevPrev) / dtSec; + EXPECT_GT(acc - accPrev, minJerk * dtSec); + EXPECT_LT(acc - accPrev, maxJerk * dtSec); + + EXPECT_DOUBLE_EQ(0.0, limiter.LimitJerk(vel, velPrev, velPrevPrev, dt)); + EXPECT_DOUBLE_EQ(2.0, vel); + + // Above upper bound (desired: accel 4.0, jerk 3.0) + vel = 6.0; + velPrev = 2.0; + velPrevPrev = 1.0; + acc = (vel - velPrev) / dtSec; + accPrev = (velPrev - velPrevPrev) / dtSec; + EXPECT_GT(acc - accPrev, minJerk * dtSec); + EXPECT_GT(acc - accPrev, maxJerk * dtSec); + + EXPECT_DOUBLE_EQ(-2.0, limiter.LimitJerk(vel, velPrev, velPrevPrev, dt)); + EXPECT_DOUBLE_EQ(4.0, vel); + acc = (vel - velPrev) / dtSec; + EXPECT_DOUBLE_EQ(acc - accPrev, maxJerk * dtSec); + + // Under lower bound + vel = -6.0; + velPrev = -2.0; + velPrevPrev = -1.0; + acc = (vel - velPrev) / dtSec; + accPrev = (velPrev - velPrevPrev) / dtSec; + EXPECT_LT(acc - accPrev, minJerk * dtSec); + EXPECT_LT(acc - accPrev, maxJerk * dtSec); + + EXPECT_DOUBLE_EQ(2.0, limiter.LimitJerk(vel, velPrev, velPrevPrev, dt)); + EXPECT_DOUBLE_EQ(-4.0, vel); + + acc = vel - velPrev; + EXPECT_DOUBLE_EQ(acc - accPrev, minJerk * dtSec); +} + +///////////////////////////////////////////////// +TEST(SpeedLimiterTest, LimitAll) +{ + double minVel = -4.0; + double maxVel = 4.0; + double minAcc = -2.0; + double maxAcc = 2.0; + double minJerk = -1.0; + double maxJerk = 1.0; + + SpeedLimiter limiter; + limiter.SetMinVelocity(minVel); + limiter.SetMaxVelocity(maxVel); + limiter.SetMinAcceleration(minAcc); + limiter.SetMaxAcceleration(maxAcc); + limiter.SetMinJerk(minJerk); + limiter.SetMaxJerk(maxJerk); + + EXPECT_DOUBLE_EQ(minVel, limiter.MinVelocity()); + EXPECT_DOUBLE_EQ(maxVel, limiter.MaxVelocity()); + EXPECT_DOUBLE_EQ(minAcc, limiter.MinAcceleration()); + EXPECT_DOUBLE_EQ(maxAcc, limiter.MaxAcceleration()); + EXPECT_DOUBLE_EQ(minJerk, limiter.MinJerk()); + EXPECT_DOUBLE_EQ(maxJerk, limiter.MaxJerk()); + + auto dt = 1s; + + // Within bounds + double vel{2.0}; + double velPrev = 1.0; + double velPrevPrev = 0.5; + EXPECT_DOUBLE_EQ(0.0, limiter.Limit(vel, velPrev, velPrevPrev, dt)); + EXPECT_DOUBLE_EQ(2.0, vel); + + // Above upper velocity bound + vel = 5.0; + velPrev = 2.0; + velPrevPrev = 1.0; + EXPECT_DOUBLE_EQ(-1.0, limiter.Limit(vel, velPrev, velPrevPrev, dt)); + EXPECT_DOUBLE_EQ(4.0, vel); + + // Above upper acceleration bound (desired accel: 3.0) + vel = 4.0; + velPrev = 1.0; + velPrevPrev = 0.0; + EXPECT_DOUBLE_EQ(-1.0, limiter.Limit(vel, velPrev, velPrevPrev, dt)); + EXPECT_DOUBLE_EQ(3.0, vel); + + // Above upper jerk bound (desired jerk: 1.5, accel: 1.8) + vel = 2.1; + velPrev = 0.3; + velPrevPrev = 0.0; + EXPECT_DOUBLE_EQ(-0.5, limiter.Limit(vel, velPrev, velPrevPrev, dt)); + EXPECT_DOUBLE_EQ(1.6, vel); + + // No change in zero time + dt = 0s; + vel = 2.1; + + EXPECT_DOUBLE_EQ(0.0, limiter.Limit(vel, velPrev, velPrevPrev, dt)); + EXPECT_DOUBLE_EQ(2.1, vel); +}