diff --git a/include/control_toolbox/speed_limiter.hpp b/include/control_toolbox/speed_limiter.hpp index b77adad3..de071538 100644 --- a/include/control_toolbox/speed_limiter.hpp +++ b/include/control_toolbox/speed_limiter.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 PAL Robotics S.L. +// Copyright 2022 Pixel Robotics. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,7 +13,7 @@ // limitations under the License. /* - * Author: Enrique Fernández + * Author: Tony Najjar */ #ifndef CONTROL_TOOLBOX__SPEED_LIMITER_HPP_ @@ -28,27 +28,25 @@ class SpeedLimiter public: /** * \brief Constructor - * \param [in] has_velocity_limits if true, applies velocity limits - * \param [in] has_acceleration_limits if true, applies acceleration limits - * \param [in] has_jerk_limits if true, applies jerk limits - * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 - * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 - * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 - * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 - * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 - * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] + * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] + * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] + * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] + * \param [in] min_deceleration Minimum deceleration [m/s^2] or [rad/s^2] + * \param [in] max_deceleration Maximum deceleration [m/s^2] or [rad/s^2] + * \param [in] min_jerk Minimum jerk [m/s^3] (applied for both signs of jerk) + * \param [in] max_jerk Maximum jerk [m/s^3] (applied for both signs of jerk) */ SpeedLimiter( - bool has_velocity_limits = false, bool has_acceleration_limits = false, - bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN, - double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN, - double max_jerk = NAN); + double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, + double max_acceleration = NAN, double min_deceleration = NAN, double max_deceleration = NAN, + double min_jerk = NAN, double max_jerk = NAN); /** * \brief Limit the velocity and acceleration - * \param [in, out] v Velocity [m/s] - * \param [in] v0 Previous velocity to v [m/s] - * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity to v [m/s] or [rad/s] + * \param [in] v1 Previous velocity to v0 [m/s] or [rad/s] * \param [in] dt Time step [s] * \return Limiting factor (1.0 if none) */ @@ -56,15 +54,15 @@ class SpeedLimiter /** * \brief Limit the velocity - * \param [in, out] v Velocity [m/s] + * \param [in, out] v Velocity [m/s] or [rad/s] * \return Limiting factor (1.0 if none) */ double limit_velocity(double & v); /** * \brief Limit the acceleration - * \param [in, out] v Velocity [m/s] - * \param [in] v0 Previous velocity [m/s] + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity [m/s] or [rad/s] * \param [in] dt Time step [s] * \return Limiting factor (1.0 if none) */ @@ -72,9 +70,9 @@ class SpeedLimiter /** * \brief Limit the jerk - * \param [in, out] v Velocity [m/s] - * \param [in] v0 Previous velocity to v [m/s] - * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity to v [m/s] or [rad/s] + * \param [in] v1 Previous velocity to v0 [m/s] or [rad/s] * \param [in] dt Time step [s] * \return Limiting factor (1.0 if none) * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control @@ -82,11 +80,6 @@ class SpeedLimiter double limit_jerk(double & v, double v0, double v1, double dt); private: - // Enable/Disable velocity/acceleration/jerk limits: - bool has_velocity_limits_; - bool has_acceleration_limits_; - bool has_jerk_limits_; - // Velocity limits: double min_velocity_; double max_velocity_; @@ -95,6 +88,10 @@ class SpeedLimiter double min_acceleration_; double max_acceleration_; + // Deceleration limits: + double min_deceleration_; + double max_deceleration_; + // Jerk limits: double min_jerk_; double max_jerk_; diff --git a/src/speed_limiter.cpp b/src/speed_limiter.cpp index 422813b7..dbc6e6e1 100644 --- a/src/speed_limiter.cpp +++ b/src/speed_limiter.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 PAL Robotics S.L. +// Copyright 2022 Pixel Robotics. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,64 +13,84 @@ // limitations under the License. /* - * Author: Enrique Fernández + * Author: Tony Najjar */ #include #include +#include #include "control_toolbox/speed_limiter.hpp" namespace control_toolbox { SpeedLimiter::SpeedLimiter( - bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, double min_velocity, - double max_velocity, double min_acceleration, double max_acceleration, double min_jerk, - double max_jerk) -: has_velocity_limits_(has_velocity_limits), - has_acceleration_limits_(has_acceleration_limits), - has_jerk_limits_(has_jerk_limits), - min_velocity_(min_velocity), + double min_velocity, double max_velocity, double min_acceleration, double max_acceleration, + double min_deceleration, double max_deceleration, double min_jerk, double max_jerk) +: min_velocity_(min_velocity), max_velocity_(max_velocity), min_acceleration_(min_acceleration), max_acceleration_(max_acceleration), + min_deceleration_(min_deceleration), + max_deceleration_(max_deceleration), min_jerk_(min_jerk), max_jerk_(max_jerk) { - // Check if limits are valid, max must be specified, min defaults to -max if unspecified - if (has_velocity_limits_) + if (!std::isnan(min_velocity_) && std::isnan(max_velocity_)) + max_velocity_ = 1000.0; // Arbitrarily big number + if (!std::isnan(max_velocity_) && std::isnan(min_velocity_)) min_velocity_ = 0.0; + + if (!std::isnan(min_acceleration_) && std::isnan(max_acceleration_)) max_acceleration_ = 1000.0; + if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; + + if (!std::isnan(min_deceleration_) && std::isnan(max_deceleration_)) max_deceleration_ = 1000.0; + if (!std::isnan(max_deceleration_) && std::isnan(min_deceleration_)) min_deceleration_ = 0.0; + + if (!std::isnan(min_jerk_) && std::isnan(max_jerk_)) max_jerk_ = 1000.0; + if (!std::isnan(max_jerk_) && std::isnan(min_jerk_)) min_jerk_ = 0.0; + + const std::string error = + " The positive limit will be applied to both directions. Setting different limits for positive " + "and negative directions is not supported. Actuators are " + "assumed to have the same constraints in both directions"; + if (min_velocity_ < 0 || max_velocity_ < 0) + { + throw std::invalid_argument("Velocity cannot be negative." + error); + } + + if (min_velocity_ > max_velocity_) + { + throw std::invalid_argument("Min velocity cannot be greater than max velocity."); + } + + if (min_acceleration_ < 0 || max_acceleration_ < 0) + { + throw std::invalid_argument("Acceleration limits cannot be negative." + error); + } + + if (min_acceleration_ > max_acceleration_) + { + throw std::invalid_argument("Min acceleration cannot be greater than max acceleration."); + } + + if (min_deceleration_ < 0 || max_deceleration_ < 0) + { + throw std::invalid_argument("Deceleration limits cannot be negative." + error); + } + + if (min_deceleration_ > max_deceleration_) { - if (std::isnan(max_velocity_)) - { - throw std::runtime_error("Cannot apply velocity limits if max_velocity is not specified"); - } - if (std::isnan(min_velocity_)) - { - min_velocity_ = -max_velocity_; - } + throw std::invalid_argument("Min deceleration cannot be greater than max deceleration."); } - if (has_acceleration_limits_) + + if (min_jerk_ < 0 || max_jerk_ < 0) { - if (std::isnan(max_acceleration_)) - { - throw std::runtime_error( - "Cannot apply acceleration limits if max_acceleration is not specified"); - } - if (std::isnan(min_acceleration_)) - { - min_acceleration_ = -max_acceleration_; - } + throw std::invalid_argument("Jerk limits cannot be negative." + error); } - if (has_jerk_limits_) + + if (min_jerk_ > max_jerk_) { - if (std::isnan(max_jerk_)) - { - throw std::runtime_error("Cannot apply jerk limits if max_jerk is not specified"); - } - if (std::isnan(min_jerk_)) - { - min_jerk_ = -max_jerk_; - } + throw std::invalid_argument("Min jerk cannot be greater than max jerk."); } } @@ -78,9 +98,10 @@ double SpeedLimiter::limit(double & v, double v0, double v1, double dt) { const double tmp = v; - limit_jerk(v, v0, v1, dt); - limit_acceleration(v, v0, dt); - limit_velocity(v); + if (!std::isnan(min_jerk_) && !std::isnan(max_jerk_)) limit_jerk(v, v0, v1, dt); + if (!std::isnan(min_acceleration_) && !std::isnan(max_acceleration_)) + limit_acceleration(v, v0, dt); + if (!std::isnan(min_velocity_) && !std::isnan(max_velocity_)) limit_velocity(v); return tmp != 0.0 ? v / tmp : 1.0; } @@ -89,11 +110,9 @@ double SpeedLimiter::limit_velocity(double & v) { const double tmp = v; - if (has_velocity_limits_) - { - v = std::clamp(v, min_velocity_, max_velocity_); - } + v = std::clamp(std::fabs(v), min_velocity_, max_velocity_); + v *= tmp >= 0 ? 1 : -1; return tmp != 0.0 ? v / tmp : 1.0; } @@ -101,15 +120,21 @@ double SpeedLimiter::limit_acceleration(double & v, double v0, double dt) { const double tmp = v; - if (has_acceleration_limits_) + double dv_min; + double dv_max; + if (std::fabs(v) >= std::fabs(v0)) { - const double dv_min = min_acceleration_ * dt; - const double dv_max = max_acceleration_ * dt; - - const double dv = std::clamp(v - v0, dv_min, dv_max); - - v = v0 + dv; + dv_min = min_acceleration_ * dt; + dv_max = max_acceleration_ * dt; + } + else + { + dv_min = min_deceleration_ * dt; + dv_max = max_deceleration_ * dt; } + double dv = std::clamp(std::fabs(v - v0), dv_min, dv_max); + dv *= (v - v0 >= 0 ? 1 : -1); + v = v0 + dv; return tmp != 0.0 ? v / tmp : 1.0; } @@ -118,20 +143,17 @@ double SpeedLimiter::limit_jerk(double & v, double v0, double v1, double dt) { const double tmp = v; - if (has_jerk_limits_) - { - const double dv = v - v0; - const double dv0 = v0 - v1; - - const double dt2 = 2. * dt * dt; + const double dv = v - v0; + const double dv0 = v0 - v1; - const double da_min = min_jerk_ * dt2; - const double da_max = max_jerk_ * dt2; + const double dt2 = 2. * dt * dt; - const double da = std::clamp(dv - dv0, da_min, da_max); + const double da_min = min_jerk_ * dt2; + const double da_max = max_jerk_ * dt2; - v = v0 + dv0 + da; - } + double da = std::clamp(std::fabs(dv - dv0), da_min, da_max); + da *= (dv - dv0 >= 0 ? 1 : -1); + v = v0 + dv0 + da; return tmp != 0.0 ? v / tmp : 1.0; } diff --git a/test/speed_limiter.cpp b/test/speed_limiter.cpp index 8668629a..13332986 100644 --- a/test/speed_limiter.cpp +++ b/test/speed_limiter.cpp @@ -20,54 +20,204 @@ TEST(SpeedLimiterTest, testWrongParams) { - EXPECT_THROW(control_toolbox::SpeedLimiter limiter(true, true, true, - -1.0, std::numeric_limits::quiet_NaN(), - -1.0, 1.0, -1.0, 1.0), - std::runtime_error); - EXPECT_THROW(control_toolbox::SpeedLimiter limiter(true, true, true, - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - -1.0, 1.0, -1.0, 1.0), - std::runtime_error); - EXPECT_NO_THROW(control_toolbox::SpeedLimiter limiter(true, true, true, - std::numeric_limits::quiet_NaN(), -1.0, - -1.0, 1.0, -1.0, 1.0)); - EXPECT_NO_THROW(control_toolbox::SpeedLimiter limiter(false, true, true, - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - -1.0, 1.0, -1.0, 1.0)); - - EXPECT_THROW(control_toolbox::SpeedLimiter limiter(true, true, true, - -1.0, 1.0, - -1.0, std::numeric_limits::quiet_NaN(), - -1.0, 1.0), - std::runtime_error); - EXPECT_THROW(control_toolbox::SpeedLimiter limiter(true, true, true, - -1.0, 1.0, - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - -1.0, 1.0), - std::runtime_error); - EXPECT_NO_THROW(control_toolbox::SpeedLimiter limiter(true, true, true, - -1.0, 1.0, - std::numeric_limits::quiet_NaN(), -1.0, - -1.0, 1.0)); - EXPECT_NO_THROW(control_toolbox::SpeedLimiter limiter(true, false, true, - -1.0, 1.0, - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - -1.0, 1.0)); - - EXPECT_THROW(control_toolbox::SpeedLimiter limiter(true, true, true, - -1.0, 1.0, -1.0, 1.0, - -1.0, std::numeric_limits::quiet_NaN()), - std::runtime_error); - EXPECT_THROW(control_toolbox::SpeedLimiter limiter(true, true, true, - -1.0, 1.0, -1.0, 1.0, - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()), - std::runtime_error); - EXPECT_NO_THROW(control_toolbox::SpeedLimiter limiter(true, true, true, - -1.0, 1.0, -1.0, 1.0, - std::numeric_limits::quiet_NaN(), 1.0)); - EXPECT_NO_THROW(control_toolbox::SpeedLimiter limiter(true, true, false, - -1.0, 1.0, -1.0, 1.0, - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN())); + EXPECT_NO_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + )); + + // velocity + { + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + -10., // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + -10., // min_velocity + -20., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + -10., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + 20., // min_velocity + 10., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + } + + // acceleration + { + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + -10., // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + -10., // min_acceleration + -20., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + -10., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + 20., // min_acceleration + 10., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + } + + // deceleration + { + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + -10., // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + -10., // min_deceleration + -20., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + -10., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + 20., // min_deceleration + 10., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + } + + // jerk + { + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + -10., // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), std::invalid_argument); + + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + -10., // min_jerk + -20. // max_jerk + ), std::invalid_argument); + + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + -10. // max_jerk + ), std::invalid_argument); + + EXPECT_THROW(control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + 20., // min_jerk + 10. // max_jerk + ), std::invalid_argument); + } } TEST(SpeedLimiterTest, testNoLimits) @@ -87,19 +237,38 @@ TEST(SpeedLimiterTest, testNoLimits) TEST(SpeedLimiterTest, testVelocityLimits) { - control_toolbox::SpeedLimiter limiter(true, true, true, -0.5, 1.0, -0.5, 1.0, -0.5, 5.0); - + control_toolbox::SpeedLimiter limiter(0.5, 1.0, 0.5, 1.0, 2.0, 3.0, 0.5, 5.0); { double v = 10.0; double limiting_factor = limiter.limit_velocity(v); // check if the robot speed is now 1.0 m.s-1, the limit EXPECT_DOUBLE_EQ(v, 1.0); EXPECT_DOUBLE_EQ(limiting_factor, 0.1); + + v = 0.1; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/0.1); + + // TODO(christophfroehlich): does this behavior make sense? + v = 0.0; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + v = -10.0; limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now -1.0 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, -1.0); + EXPECT_DOUBLE_EQ(limiting_factor, -1.0/-10.0); + + v = -0.1; + limiting_factor = limiter.limit_velocity(v); // check if the robot speed is now -0.5 m.s-1, the limit EXPECT_DOUBLE_EQ(v, -0.5); - EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + EXPECT_DOUBLE_EQ(limiting_factor, -0.5/-0.1); } { @@ -109,24 +278,28 @@ TEST(SpeedLimiterTest, testVelocityLimits) // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s EXPECT_DOUBLE_EQ(v, 0.5); EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + v = -10.0; limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); // acceleration is now limiting, not velocity // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s - EXPECT_DOUBLE_EQ(v, -0.25); - EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); } } TEST(SpeedLimiterTest, testVelocityNoLimits) { { - control_toolbox::SpeedLimiter limiter(false, true, true, -0.5, 1.0, -0.5, 1.0, -0.5, 5.0); + control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + 0.5, 1.0, 2.0, 3.0, 0.5, 5.0); double v = 10.0; double limiting_factor = limiter.limit_velocity(v); // check if the velocity is not limited EXPECT_DOUBLE_EQ(v, 10.0); EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + v = -10.0; limiting_factor = limiter.limit_velocity(v); // check if the velocity is not limited @@ -135,55 +308,52 @@ TEST(SpeedLimiterTest, testVelocityNoLimits) } { - control_toolbox::SpeedLimiter limiter(false, true, true, -0.5, 1.0, -0.5, 1.0, -0.5, 5.0); + control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + 0.5, 1.0, 2.0, 3.0, 0.5, 5.0); double v = 10.0; double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); // acceleration is now limiting, not velocity // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s EXPECT_DOUBLE_EQ(v, 0.5); EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + v = -10.0; limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); // acceleration is now limiting, not velocity // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s - EXPECT_DOUBLE_EQ(v, -0.25); - EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); } { - control_toolbox::SpeedLimiter limiter(false, false, true, -0.5, 1.0, -0.5, 1.0, -0.5, 5.0); + control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + 0.5, 5.0); double v = 10.0; double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); // jerk is now limiting, not velocity EXPECT_DOUBLE_EQ(v, 2.5); EXPECT_DOUBLE_EQ(limiting_factor, 2.5/10.0); - v = -10.0; - limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); - // jerk is now limiting, not velocity - EXPECT_DOUBLE_EQ(v, -0.25); - EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); - } - { - control_toolbox::SpeedLimiter limiter(false, false, false, -0.5, 1.0, -0.5, 1.0, -0.5, 5.0); - double v = 10.0; - double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); - // check if the velocity is not limited - EXPECT_DOUBLE_EQ(v, 10.0); - EXPECT_DOUBLE_EQ(limiting_factor, 1.0); v = -10.0; limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); - // check if the velocity is not limited - EXPECT_DOUBLE_EQ(v, -10.0); - EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + // jerk is now limiting, not velocity + EXPECT_DOUBLE_EQ(v, -2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5/10.0); } } TEST(SpeedLimiterTest, testAccelerationLimits) { - control_toolbox::SpeedLimiter limiter(true, true, true, -0.5, 1.0, -0.5, 1.0, -0.5, 5.0); + control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + 0.5, 1.0, 2.0, 3.0, 0.5, 5.0); { + // test max_acceleration double v = 10.0; double limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s @@ -192,9 +362,27 @@ TEST(SpeedLimiterTest, testAccelerationLimits) v = -10.0; limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); - // check if the robot speed is now -0.25 m.s-1, which is -0.5m.s-2 * 0.5s + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + } + { + // test min_acceleration + // TODO(christophfroehlich): does this behavior make sense? + control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + 0.5, 1.0, 2.0, 3.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + double v = 0.0; + double limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now 0.25m.s-1 = 0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -std::numeric_limits::epsilon(); + limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now -0.25m.s-1 = -0.5m.s-2 * 0.5s EXPECT_DOUBLE_EQ(v, -0.25); - EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); } { @@ -206,39 +394,120 @@ TEST(SpeedLimiterTest, testAccelerationLimits) v = -10.0; limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); - // check if the robot speed is now -0.25 m.s-1, which is -0.5m.s-2 * 0.5s - EXPECT_DOUBLE_EQ(v, -0.25); - EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); } } -TEST(SpeedLimiterTest, testJerkLimits) +TEST(SpeedLimiterTest, testDecelerationLimits) { - control_toolbox::SpeedLimiter limiter(true, true, true, -0.5, 1.0, -0.5, 1.0, -1.0, 1.0); + control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + 0.5, 1.0, 2.0, 3.0, 0.5, 5.0); + + { + // test max_deceleration + double v = 0.0; + double limiting_factor = limiter.limit_acceleration(v, 10.0, 0.5); + // check if the robot speed is now 8.5 m.s-1, which is 10.0 - 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = 0.0; + limiting_factor = limiter.limit_acceleration(v, -10.0, 0.5); + // check if the robot speed is now -8.5 m.s-1, which is -10.0 + 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + } + { + // test min_deceleration + // TODO(christophfroehlich): does this behavior make sense? + control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + 0.5, 1.0, 2.0, 3.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + double v = 9.9; + limiter.limit_acceleration(v, 10.0, 0.5); + // check if the robot speed is now 9.0m.s-1 = 10 - 2.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 9.0); + + v = -9.9; + limiter.limit_acceleration(v, -10., 0.5); + // check if the robot speed is now -9.0m.s-1 = -10 + 2.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -9.0); + } + { + double v = 0.0; + double limiting_factor = limiter.limit(v, 10.0, 10.0, 0.5); + // check if the robot speed is now 8.5 m.s-1, which is 10.0 - 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + v = 0.0; + limiting_factor = limiter.limit(v, -10.0, -10.0, 0.5); + // check if the robot speed is now -8.5 m.s-1, which is -10.0 + 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + } +} + +TEST(SpeedLimiterTest, testJerkLimits) +{ { + // test max_jerk + control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + 0.5, 5.0); double v = 10.0; double limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); - // check if the robot speed is now 0.5m.s-1 = 1.0m.s-3 * 2 * 0.5s * 0.5s - EXPECT_DOUBLE_EQ(v, 0.5); - EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + // check if the robot speed is now 2.5m.s-1 = 5.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5/10.0); + v = -10.0; limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); - // check if the robot speed is now -0.5m.s-1 = -1.0m.s-3 * 2 * 0.5s * 0.5s - EXPECT_DOUBLE_EQ(v, -0.5); - EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + // check if the robot speed is now -2.5m.s-1 = -5.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5/10.0); } { + // test min_jerk + // TODO(christophfroehlich): does this behavior make sense? + control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + 0.5, 5.0); + double v = 0.0; + double limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.25m.s-1 = 0.5m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -std::numeric_limits::epsilon(); + limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.25m.s-1 = -0.5m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + } + { + control_toolbox::SpeedLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + 0.5, 1.0, 2.0, 3.0, 0.5, 5.0); + // acceleration is limiting, not jerk + double v = 10.0; double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); - // check if the robot speed is now 0.5m.s-1 = 1.0m.s-3 * 2 * 0.5s * 0.5s + // check if the robot speed is now 0.5 m.s-1, which is -1.0m.s-2 * 0.5s EXPECT_DOUBLE_EQ(v, 0.5); EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + v = -10.0; limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); - // acceleration is limiting, not jerk - // check if the robot speed is now -0.25 m.s-1, which is -0.5m.s-2 * 0.5s - EXPECT_DOUBLE_EQ(v, -0.25); - EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); } }