Fix jerk limiter in rate_limiter

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<double>::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<T>::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<T>(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<T>(0.0) ? v / tmp : static_cast<T>(1.0);

test/rate_limiter.cpp

@@ -124,7 +124,7 @@ TEST(RateLimiterTest, testValueLimits)
     -0.5, 1.0,
     -0.5, 1.0,
     std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(),
-    -0.5, 5.0);
+    -2.0, 5.0);
 
   {
     double v = 10.0;
@@ -181,7 +181,7 @@ TEST(RateLimiterTest, testValueNoLimits)
       std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(),
       -0.5, 1.0,
       std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::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<double>::quiet_NaN(), std::numeric_limits<double>::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