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

…d it's tolerance

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.

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())

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;