Skip to content

Commit

Permalink
update the loguic to check if the actual position is within bounds an…
Browse files Browse the repository at this point in the history
…d it's tolerance
  • Loading branch information
saikishor committed Sep 28, 2024
1 parent b225be4 commit 51a69af
Show file tree
Hide file tree
Showing 3 changed files with 57 additions and 13 deletions.
4 changes: 4 additions & 0 deletions joint_limits/include/joint_limits/joint_limits_helpers.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
46 changes: 33 additions & 13 deletions joint_limits/src/joint_limits_helpers.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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())
Expand Down
20 changes: 20 additions & 0 deletions joint_limits/test/test_joint_range_limiter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 51a69af

Please sign in to comment.