diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index e9ede97d9b..10a16729e1 100644 --- 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 index f98752336f..ed5eca07b6 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -67,22 +67,42 @@ std::pair compute_velocity_limits( std::pair 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 index 4571c8ced5..083646d84f 100644 --- 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 +#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;