update the loguic to check if the actual position is within bounds an…

…d it's tolerance
ros-controls · Sep 16, 2024 · f088099 · f088099
1 parent aaa920d
commit f088099
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 13 deletions.
diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp
@@ -24,6 +24,10 @@
 
 namespace joint_limits
 {
+namespace internal
+{
+constexpr double POSITION_BOUNDS_TOLERANCE = 0.002;
+}
 
 /**
  * @brief Checks if a value is limited by the given limits.

diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp
@@ -67,22 +67,42 @@ std::pair<double, double> compute_velocity_limits(
   std::pair<double, double> vel_limits({-max_vel, max_vel});
   if (limits.has_position_limits && act_pos.has_value())
   {
-    const double max_vel_with_pos_limits = (limits.max_position - act_pos.value()) / dt;
-    const double min_vel_with_pos_limits = (limits.min_position - act_pos.value()) / dt;
+    const double actual_pos = act_pos.value();
+    const double max_vel_with_pos_limits = (limits.max_position - actual_pos) / dt;
+    const double min_vel_with_pos_limits = (limits.min_position - actual_pos) / dt;
     vel_limits.first = std::max(min_vel_with_pos_limits, vel_limits.first);
     vel_limits.second = std::min(max_vel_with_pos_limits, vel_limits.second);
-    if (
-      (act_pos.value() > limits.max_position && desired_vel >= 0.0) ||
-      (act_pos.value() < limits.min_position && desired_vel <= 0.0))
+
+    if (actual_pos > limits.max_position || actual_pos < limits.min_position)
     {
-      RCLCPP_WARN_EXPRESSION(
-        rclcpp::get_logger("joint_limiter_interface"),
-        prev_command_vel.has_value() && prev_command_vel.value() != 0.0,
-        "Joint position %.5f is out of bounds[%.5f, %.5f] for the joint and we want to move "
-        "further into bounds with vel %.5f: '%s'. Joint velocity limits will be "
-        "restrictred to zero.",
-        act_pos.value(), limits.min_position, limits.max_position, desired_vel, joint_name.c_str());
-      vel_limits = {0.0, 0.0};
+      if (
+        (actual_pos < (limits.max_position + internal::POSITION_BOUNDS_TOLERANCE) &&
+         (actual_pos > limits.min_position) && desired_vel >= 0.0) ||
+        (actual_pos > (limits.min_position - internal::POSITION_BOUNDS_TOLERANCE) &&
+         (actual_pos < limits.max_position) && desired_vel <= 0.0))
+      {
+        RCLCPP_WARN_EXPRESSION(
+          rclcpp::get_logger("joint_limiter_interface"),
+          prev_command_vel.has_value() && prev_command_vel.value() != 0.0,
+          "Joint position %.5f is out of bounds[%.5f, %.5f] for the joint and we want to move "
+          "further into bounds with vel %.5f: '%s'. Joint velocity limits will be "
+          "restrictred to zero.",
+          actual_pos, limits.min_position, limits.max_position, desired_vel, joint_name.c_str());
+        vel_limits = {0.0, 0.0};
+      }
+      // If the joint reports a position way out of bounds, then it would mean something is
+      // extremely wrong, so no velocity command should be allowed as it might damage the robot
+      else if (
+        (actual_pos > (limits.max_position + internal::POSITION_BOUNDS_TOLERANCE)) ||
+        (actual_pos < (limits.min_position - internal::POSITION_BOUNDS_TOLERANCE)))
+      {
+        RCLCPP_ERROR_ONCE(
+          rclcpp::get_logger("joint_limiter_interface"),
+          "Joint position is out of bounds for the joint : '%s'. Joint velocity limits will be "
+          "restricted to zero.",
+          joint_name.c_str());
+        vel_limits = {0.0, 0.0};
+      }
     }
   }
   if (limits.has_acceleration_limits && prev_command_vel.has_value())

diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp
@@ -15,6 +15,7 @@
 /// \author Sai Kishor Kothakota
 
 #include <limits>
+#include "joint_limits/joint_limits_helpers.hpp"
 #include "test_joint_limiter.hpp"
 
 TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded)
@@ -252,6 +253,25 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases)
   test_limit_enforcing(-6.0, -1.0, 0.0, true);
   test_limit_enforcing(-6.0, -2.0, 0.0, true);
   test_limit_enforcing(-6.0, 1.0, 0.0, true);
+  // If the reported actual position is within the limits and the tolerance, then the velocity is
+  // allowed to move into the range, but not away from the range
+  test_limit_enforcing(
+    -5.0 - joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, -3.0, 0.0, true);
+  test_limit_enforcing(
+    -5.0 - joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, 1.0, 1.0, false);
+  test_limit_enforcing(
+    -5.0 - joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, 0.2, 0.2, false);
+  test_limit_enforcing(
+    -5.0 - joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, 2.0, 1.0, true);
+
+  test_limit_enforcing(
+    5.0 + joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, 3.0, 0.0, true);
+  test_limit_enforcing(
+    5.0 + joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, -1.0, -1.0, false);
+  test_limit_enforcing(
+    5.0 + joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, -0.2, -0.2, false);
+  test_limit_enforcing(
+    5.0 + joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, -2.0, -1.0, true);
 
   // Now remove the position limits and only test with acceleration limits
   limits.has_position_limits = false;