enable moving out of position limit if velocity is in "right" direction

StoglRobotics-forks · Jul 8, 2024 · 9d1db8d · 9d1db8d
1 parent 8dcfff1
commit 9d1db8d
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 7 deletions.
diff --git 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<double, double> compute_position_limits(
  */
 std::pair<double, double> compute_velocity_limits(
   const std::string & joint_name, const joint_limits::JointLimits & limits,
-  const std::optional<double> & act_pos, const std::optional<double> & prev_command_vel, double dt);
+  const double & desired_vel, const std::optional<double> & act_pos,
+  const std::optional<double> & 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
@@ -59,7 +59,8 @@ std::pair<double, double> compute_position_limits(
 
 std::pair<double, double> compute_velocity_limits(
   const std::string & joint_name, const joint_limits::JointLimits & limits,
-  const std::optional<double> & act_pos, const std::optional<double> & prev_command_vel, double dt)
+  const double & desired_vel, const std::optional<double> & act_pos,
+  const std::optional<double> & prev_command_vel, double dt)
 {
   const double max_vel =
     limits.has_velocity_limits ? limits.max_velocity : std::numeric_limits<double>::infinity();
@@ -70,13 +71,16 @@ std::pair<double, double> 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
@@ -121,7 +121,8 @@ bool JointSaturationLimiter<JointControlInterfacesData>::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
@@ -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())
     {