diff --git a/include/control_toolbox/rate_limiter.hpp b/include/control_toolbox/rate_limiter.hpp index bbac2fd3..b847b5e7 100644 --- a/include/control_toolbox/rate_limiter.hpp +++ b/include/control_toolbox/rate_limiter.hpp @@ -47,6 +47,13 @@ class RateLimiter * If max_* values are NAN, the respective limit is deactivated * If min_* values are NAN (unspecified), defaults to -max * If min_first_derivative_pos/max_first_derivative_neg values are NAN, symmetric limits are used + * + * Disclaimer about the jerk limits: + * The jerk limit is only applied when accelerating or reverse_accelerating (i.e., "sign(jerk * accel) > 0"). + * This condition prevents oscillating closed-loop behavior, see discussion details in + * https://github.com/ros-controls/control_toolbox/issues/240. + * if you use this feature, you should perform a test to check that the behavior is really as you expect. + * */ RateLimiter( T min_value = std::numeric_limits::quiet_NaN(), @@ -92,6 +99,8 @@ class RateLimiter * \param [in] dt Time step [s] * \return Limiting factor (1.0 if none) * \see http://en.wikipedia.org/wiki/jerk_%28physics%29#Motion_control + * \note + * The jerk limit is only applied when accelerating or reverse_accelerating (i.e., "sign(jerk * accel) > 0"). */ T limit_second_derivative(T & v, T v0, T v1, T dt); @@ -297,14 +306,20 @@ T RateLimiter::limit_second_derivative(T & v, T v0, T v1, T dt) const T dv = v - v0; const T dv0 = v0 - v1; - const T dt2 = static_cast(2.0) * dt * dt; + // Only limit jerk when accelerating or reverse_accelerating + // Note: this prevents oscillating closed-loop behavior, see discussion + // details in https://github.com/ros-controls/control_toolbox/issues/240. + if ((dv - dv0) * (v - v0) > 0) + { + const T dt2 = dt * dt; - const T da_min = min_second_derivative_ * dt2; - const T da_max = max_second_derivative_ * dt2; + const T da_min = min_second_derivative_ * dt2; + const T da_max = max_second_derivative_ * dt2; - const T da = std::clamp(dv - dv0, da_min, da_max); + const T da = std::clamp(dv - dv0, da_min, da_max); - v = v0 + dv0 + da; + v = v0 + dv0 + da; + } } return tmp != static_cast(0.0) ? v / tmp : static_cast(1.0); diff --git a/test/rate_limiter.cpp b/test/rate_limiter.cpp index f333e07a..e7409b0c 100644 --- a/test/rate_limiter.cpp +++ b/test/rate_limiter.cpp @@ -124,7 +124,7 @@ TEST(RateLimiterTest, testValueLimits) -0.5, 1.0, -0.5, 1.0, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - -0.5, 5.0); + -2.0, 5.0); { double v = 10.0; @@ -181,7 +181,7 @@ TEST(RateLimiterTest, testValueNoLimits) std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), -0.5, 1.0, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - -0.5, 5.0 + -2.0, 2.0 ); double v = 10.0; double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); @@ -207,13 +207,15 @@ TEST(RateLimiterTest, testValueNoLimits) double v = 10.0; double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); // second_derivative is now limiting, not value - EXPECT_DOUBLE_EQ(v, 2.5); - EXPECT_DOUBLE_EQ(limiting_factor, 2.5/10.0); + // check if the robot speed is now 1.25 m.s-1, which is 5.0 m.s-3 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 1.25); + EXPECT_DOUBLE_EQ(limiting_factor, 1.25/10.0); v = -10.0; limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); // second_derivative is now limiting, not value - EXPECT_DOUBLE_EQ(v, -0.25); - EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + // check if the robot speed is now -0.25 m.s-1, which is -0.5m.s-3 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -0.125); + EXPECT_DOUBLE_EQ(limiting_factor, 0.125/10.0); } { @@ -240,7 +242,7 @@ TEST(RateLimiterTest, testFirstDerivativeLimits) control_toolbox::RateLimiter limiter( -0.5, 1.0, -0.5, 1.0, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - -0.5, 5.0 + -2.0, 5.0 ); { @@ -323,21 +325,21 @@ TEST(RateLimiterTest, testSecondDerivativeLimits) { double v = 10.0; double limiting_factor = limiter.limit_second_derivative(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 0.25m.s-1 = 1.0m.s-3 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); v = -10.0; limiting_factor = limiter.limit_second_derivative(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 -0.25m.s-1 = -1.0m.s-3 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.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.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 0.25m.s-1 = 1.0m.s-3 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); v = -10.0; limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); // first_derivative is limiting, not second_derivative