From dfd2370c6a8dd8e59f8934577f16292a48dffedb Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 25 Nov 2024 17:51:04 +0100 Subject: [PATCH 1/8] fix rate limiter --- include/control_toolbox/rate_limiter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/control_toolbox/rate_limiter.hpp b/include/control_toolbox/rate_limiter.hpp index bbac2fd3..45a25eca 100644 --- a/include/control_toolbox/rate_limiter.hpp +++ b/include/control_toolbox/rate_limiter.hpp @@ -297,7 +297,7 @@ 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; + const T dt2 = dt * dt; const T da_min = min_second_derivative_ * dt2; const T da_max = max_second_derivative_ * dt2; From 0b690561bd9460c59fc7f4de3a665c01d120cc0d Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Tue, 26 Nov 2024 09:57:17 +0100 Subject: [PATCH 2/8] fix tests - increase jerk limits when not testing it explicitly --- test/rate_limiter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/rate_limiter.cpp b/test/rate_limiter.cpp index f333e07a..adc69850 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; @@ -240,7 +240,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 ); { From 9a6ffe712321c8e0e99ce108168a38f2ec315162 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Tue, 26 Nov 2024 09:57:46 +0100 Subject: [PATCH 3/8] =?UTF-8?q?make=20tests=20consistent=20with=20dt2=20?= =?UTF-8?q?=3D=20dt=C2=B2=20(vs=202=20*=20dt=C2=B2)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- test/rate_limiter.cpp | 30 ++++++++++++++++-------------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/test/rate_limiter.cpp b/test/rate_limiter.cpp index adc69850..e7409b0c 100644 --- a/test/rate_limiter.cpp +++ b/test/rate_limiter.cpp @@ -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); } { @@ -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 From 4ab7b76e510248c27b5198691eb9f824af32f893 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Wed, 27 Nov 2024 08:58:23 +0100 Subject: [PATCH 4/8] reorder clipping functions (velocity first, jerk last) --- include/control_toolbox/rate_limiter.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/control_toolbox/rate_limiter.hpp b/include/control_toolbox/rate_limiter.hpp index 45a25eca..f5ff8734 100644 --- a/include/control_toolbox/rate_limiter.hpp +++ b/include/control_toolbox/rate_limiter.hpp @@ -236,9 +236,9 @@ T RateLimiter::limit(T & v, T v0, T v1, T dt) { const T tmp = v; - limit_second_derivative(v, v0, v1, dt); - limit_first_derivative(v, v0, dt); limit_value(v); + limit_first_derivative(v, v0, dt); + limit_second_derivative(v, v0, v1, dt); return tmp != static_cast(0.0) ? v / tmp : static_cast(1.0); } From 4bea82e6760066a05b5418c4bb456a4e7ff8a908 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sun, 1 Dec 2024 13:49:06 +0100 Subject: [PATCH 5/8] Revert "reorder clipping functions (velocity first, jerk last)" This reverts commit 4ab7b76e510248c27b5198691eb9f824af32f893. --- include/control_toolbox/rate_limiter.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/control_toolbox/rate_limiter.hpp b/include/control_toolbox/rate_limiter.hpp index f5ff8734..45a25eca 100644 --- a/include/control_toolbox/rate_limiter.hpp +++ b/include/control_toolbox/rate_limiter.hpp @@ -236,9 +236,9 @@ T RateLimiter::limit(T & v, T v0, T v1, T dt) { const T tmp = v; - limit_value(v); - limit_first_derivative(v, v0, dt); limit_second_derivative(v, v0, v1, dt); + limit_first_derivative(v, v0, dt); + limit_value(v); return tmp != static_cast(0.0) ? v / tmp : static_cast(1.0); } From 7649782860f79ef9ecada2ee2af63b20b344238d Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sun, 1 Dec 2024 14:00:24 +0100 Subject: [PATCH 6/8] only apply jerk limit when acceleration (or "reverse" accelerating) --- include/control_toolbox/rate_limiter.hpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/include/control_toolbox/rate_limiter.hpp b/include/control_toolbox/rate_limiter.hpp index 45a25eca..47bd9c96 100644 --- a/include/control_toolbox/rate_limiter.hpp +++ b/include/control_toolbox/rate_limiter.hpp @@ -297,14 +297,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 = 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); From c467026f1773b47f4a0cad0b94d1a66d6d6abfc8 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sun, 1 Dec 2024 14:03:45 +0100 Subject: [PATCH 7/8] strictly accelerating only --- include/control_toolbox/rate_limiter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/control_toolbox/rate_limiter.hpp b/include/control_toolbox/rate_limiter.hpp index 47bd9c96..df99c3fe 100644 --- a/include/control_toolbox/rate_limiter.hpp +++ b/include/control_toolbox/rate_limiter.hpp @@ -300,7 +300,7 @@ T RateLimiter::limit_second_derivative(T & v, T v0, T v1, T 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) + if ((dv - dv0) * (v - v0) > 0) { const T dt2 = dt * dt; From 4aaaa70de5462375a685f88471af1790a41cb479 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 2 Dec 2024 16:13:28 +0100 Subject: [PATCH 8/8] add disclaimer about jerk limits in docstring --- include/control_toolbox/rate_limiter.hpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/include/control_toolbox/rate_limiter.hpp b/include/control_toolbox/rate_limiter.hpp index df99c3fe..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);