diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index 32b9afbd76..e9ede97d9b 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -57,7 +57,8 @@ std::pair compute_position_limits( */ std::pair compute_velocity_limits( const std::string & joint_name, const joint_limits::JointLimits & limits, - const std::optional & act_pos, const std::optional & prev_command_vel, double dt); + const double & desired_vel, const std::optional & act_pos, + const std::optional & prev_command_vel, double dt); /** * @brief Computes the effort limits based on the position and velocity limits. diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index f356e92b5e..fdf1c84ee3 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -59,7 +59,8 @@ std::pair compute_position_limits( std::pair compute_velocity_limits( const std::string & joint_name, const joint_limits::JointLimits & limits, - const std::optional & act_pos, const std::optional & prev_command_vel, double dt) + const double & desired_vel, const std::optional & act_pos, + const std::optional & prev_command_vel, double dt) { const double max_vel = limits.has_velocity_limits ? limits.max_velocity : std::numeric_limits::infinity(); @@ -70,13 +71,16 @@ std::pair compute_velocity_limits( const double min_vel_with_pos_limits = (limits.min_position - act_pos.value()) / 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 || act_pos.value() < limits.min_position) + if ( + (act_pos.value() > limits.max_position && desired_vel >= 0.0) || + (act_pos.value() < limits.min_position && desired_vel <= 0.0)) { RCLCPP_ERROR_ONCE( rclcpp::get_logger("joint_limiter_interface"), - "Joint position is out of bounds for the joint : '%s'. Joint velocity limits will be " + "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.", - joint_name.c_str()); + act_pos.value(), limits.min_position, limits.max_position, desired_vel, joint_name.c_str()); vel_limits = {0.0, 0.0}; } } diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index bd293d8d6f..3adc30d2e0 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -121,7 +121,8 @@ bool JointSaturationLimiter::on_enforce( if (desired.has_velocity()) { const auto limits = compute_velocity_limits( - joint_name, joint_limits, actual.position, prev_command_.velocity, dt_seconds); + joint_name, joint_limits, desired.velocity.value(), actual.position, prev_command_.velocity, + dt_seconds); limits_enforced = is_limited(desired.velocity.value(), limits.first, limits.second) || limits_enforced; desired.velocity = std::clamp(desired.velocity.value(), limits.first, limits.second); diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp index b0ccbb5dd2..fdac151557 100644 --- a/joint_limits/src/joint_soft_limiter.cpp +++ b/joint_limits/src/joint_soft_limiter.cpp @@ -166,7 +166,8 @@ bool JointSoftLimiter::on_enforce( if (desired.has_velocity()) { const auto velocity_limits = compute_velocity_limits( - joint_name, hard_limits, actual.position, prev_command_.velocity, dt_seconds); + joint_name, hard_limits, desired.velocity.value(), actual.position, prev_command_.velocity, + dt_seconds); if (hard_limits.has_acceleration_limits && actual.has_velocity()) {