diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h index cb3d63b70..75412ada7 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h @@ -433,8 +433,16 @@ class VelocityJointSaturationHandle using internal::saturate; // Velocity bounds - double vel_low; - double vel_high; + double vel_low = -limits_.max_velocity; + double vel_high = limits_.max_velocity; + + if (limits_.has_position_limits) + { + // Velocity bounds depend on the velocity limit and the proximity to the position limit. + const double pos = jh_.getPosition(); + vel_low = saturate(limits_.min_position - pos, -limits_.max_velocity, limits_.max_velocity); + vel_high = saturate(limits_.max_position - pos, -limits_.max_velocity, limits_.max_velocity); + } if (limits_.has_acceleration_limits) { @@ -442,13 +450,8 @@ class VelocityJointSaturationHandle const double vel = jh_.getVelocity(); const double dt = period.toSec(); - vel_low = std::max(vel - limits_.max_acceleration * dt, -limits_.max_velocity); - vel_high = std::min(vel + limits_.max_acceleration * dt, limits_.max_velocity); - } - else - { - vel_low = -limits_.max_velocity; - vel_high = limits_.max_velocity; + vel_low = std::max(vel - limits_.max_acceleration * dt, vel_low); + vel_high = std::min(vel + limits_.max_acceleration * dt, vel_high); } // Saturate velocity command according to limits