diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 708c354636..e8e501305f 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -82,6 +82,12 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) + + ament_add_gmock(test_traction_limiter + test/test_traction_limiter.cpp) + target_link_libraries(test_traction_limiter + tricycle_controller + ) endif() install( diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp index ea0bb16025..437f6cd4c2 100644 --- a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -28,14 +28,25 @@ class TractionLimiter public: /** * \brief Constructor + * + * Parameters are applied symmetrically for both directions, i.e., are applied + * to the absolute value of the corresponding quantity. + * + * \warning + * - Setting min_velocity: the robot can't stand still + * + * - Setting min_deceleration/min_acceleration: the robot can't move with constant velocity + * + * - Setting min_jerk: the robot can't move with constant acceleration + * * \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], usually <= 0 - * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3] + * \param [in] max_jerk Maximum jerk [m/s^3] */ TractionLimiter( double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index a8019714e1..e31e5e1e82 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -44,13 +44,13 @@ TractionLimiter::TractionLimiter( 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_acceleration_)) min_deceleration_ = 0.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 " + " 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) @@ -58,19 +58,39 @@ TractionLimiter::TractionLimiter( 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 cannot be negative." + error); + 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 cannot be negative." + error); + throw std::invalid_argument("Deceleration limits cannot be negative." + error); + } + + if (min_deceleration_ > max_deceleration_) + { + throw std::invalid_argument("Min deceleration cannot be greater than max deceleration."); } if (min_jerk_ < 0 || max_jerk_ < 0) { - throw std::invalid_argument("Jerk cannot be negative." + error); + throw std::invalid_argument("Jerk limits cannot be negative." + error); + } + + if (min_jerk_ > max_jerk_) + { + throw std::invalid_argument("Min jerk cannot be greater than max jerk."); } } diff --git a/tricycle_controller/test/test_traction_limiter.cpp b/tricycle_controller/test/test_traction_limiter.cpp new file mode 100644 index 0000000000..4afed7f217 --- /dev/null +++ b/tricycle_controller/test/test_traction_limiter.cpp @@ -0,0 +1,546 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// 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 + +#include "tricycle_controller/traction_limiter.hpp" + +TEST(SpeedLimiterTest, testWrongParams) +{ + EXPECT_NO_THROW(tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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( + tricycle_controller::TractionLimiter 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) +{ + tricycle_controller::TractionLimiter limiter; + 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); +} + +TEST(SpeedLimiterTest, testVelocityLimits) +{ + tricycle_controller::TractionLimiter 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 / -0.1); + } + + { + 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.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testVelocityNoLimits) +{ + { + tricycle_controller::TractionLimiter 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 + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + } + + { + tricycle_controller::TractionLimiter 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.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } + + { + tricycle_controller::TractionLimiter 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, -2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testAccelerationLimits) +{ + { + // test max_acceleration + tricycle_controller::TractionLimiter 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_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 + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + 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 + 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? + tricycle_controller::TractionLimiter 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); + } + + { + tricycle_controller::TractionLimiter 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); + // 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); + // 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, testDecelerationLimits) +{ + { + // test max_deceleration + tricycle_controller::TractionLimiter 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 = 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? + tricycle_controller::TractionLimiter 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); + } + { + tricycle_controller::TractionLimiter 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 = 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 + tricycle_controller::TractionLimiter 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 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 -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? + tricycle_controller::TractionLimiter 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); + } + { + tricycle_controller::TractionLimiter 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.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); + // 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); + } +}