Warn user when implicit vel exceeds max vel and recal effective acc limits

joint_limits/include/joint_limits/joint_saturation_limiter.hpp

@@ -50,6 +50,7 @@ class JointSaturationLimiter : public JointLimiterInterface<JointLimitsStateData
   bool on_configure(const JointLimitsStateDataType & current_joint_states) override
   {
     prev_command_ = current_joint_states;
+    impl_vel_check_ = false;
     return true;
   }
 
@@ -85,12 +86,17 @@ class JointSaturationLimiter : public JointLimiterInterface<JointLimitsStateData
   {
     std::lock_guard<std::mutex> lock(mutex_);
     prev_command_ = JointLimitsStateDataType();
+    impl_vel_check_ = false;
   }
 
 protected:
   rclcpp::Clock::SharedPtr clock_;
   JointLimitsStateDataType prev_command_;
   std::mutex mutex_;
+
+private:
+  bool impl_vel_check_;  // this flag is used to check if implicit vel exceeds max vel, then update
+                         // max acc/dec to effective acc and dec
 };
 
 template <typename JointLimitsStateDataType>

joint_limits/src/joint_saturation_limiter.cpp

@@ -40,9 +40,59 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
     return false;
   }
 
-  // TODO(gwalck) compute if the max are not implicitly violated with the given dt
-  // e.g. for max vel 2.0 and max acc 5.0, with dt >0.4
-  // velocity max is implicitly already violated due to max_acc * dt > 2.0
+  // check if implicit vel exceeds max velocity, then calculate the
+  // max reduction among all joints
+  if (!impl_vel_check_)
+  {
+    double max_reduction_factor = 1.0f;
+    for (size_t i = 0; i < number_of_joints_; ++i)
+    {
+      if (joint_limits_[i].has_velocity_limits && joint_limits_[i].has_acceleration_limits)
+      {
+        const double implicit_vel = joint_limits_[i].max_acceleration * dt_seconds;
+        if (implicit_vel > joint_limits_[i].max_velocity)
+        {
+          RCLCPP_WARN_THROTTLE(
+            node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+            "Joint %s: dt (%f) is too large for max_acceleration (%f) and max_velocity (%f); "
+            "max_acc * dt = %f exceeds max_velocity",
+            joint_names_[i].c_str(), dt_seconds, joint_limits_[i].max_acceleration,
+            joint_limits_[i].max_velocity, implicit_vel);
+          const double reduction_factor = joint_limits_[i].max_velocity / implicit_vel;
+          max_reduction_factor = std::min(reduction_factor, max_reduction_factor);
+        }
+      }
+      if (joint_limits_[i].has_velocity_limits && joint_limits_[i].has_deceleration_limits)
+      {
+        const double implicit_vel = joint_limits_[i].max_deceleration * dt_seconds;
+        if (implicit_vel > joint_limits_[i].max_velocity)
+        {
+          RCLCPP_WARN_THROTTLE(
+            node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+            "Joint %s: dt (%f) is too large for max_deceleration (%f) and max_velocity (%f); "
+            "max_dec * dt = %f exceeds max_velocity",
+            joint_names_[i].c_str(), dt_seconds, joint_limits_[i].max_deceleration,
+            joint_limits_[i].max_velocity, implicit_vel);
+          const double reduction_factor = joint_limits_[i].max_velocity / implicit_vel;
+          max_reduction_factor = std::min(reduction_factor, max_reduction_factor);
+        }
+      }
+    }
+    // Scale down effective acc/dec for all joints
+    if (max_reduction_factor < 1.0)
+    {
+      RCLCPP_WARN_THROTTLE(
+        node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+        "Scaling all joints acceleration and deceleration limits by factor: %f",
+        max_reduction_factor);
+      for (size_t i = 0; i < number_of_joints_; ++i)
+      {
+        joint_limits_[i].max_acceleration *= max_reduction_factor;
+        joint_limits_[i].max_deceleration *= max_reduction_factor;
+      }
+    }
+    impl_vel_check_ = true;
+  }
 
   // check for required inputs combination
   const bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_);
@@ -257,7 +307,7 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
       }
       else if (joint_limits_[index].has_acceleration_limits)
       {
-        stopping_deccel = joint_limits_[index].max_acceleration;
+        stopping_deccel = joint_limits_[index].max_deceleration;
       }
 
       double stopping_distance =
@@ -374,9 +424,9 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
       ostr << jnt << " ";
     }
     ostr << "\b \b";  // erase last character
-    RCLCPP_WARN_STREAM_THROTTLE(
+    RCLCPP_WARN_THROTTLE(
       node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
-      "Joint(s) [" << ostr.str().c_str() << "] would exceed velocity limits, limiting");
+      "Joint(s) [%s] would exceed velocity limits, limiting", ostr.str().c_str());
   }
 
   if (limited_jnts_acc.size() > 0)
@@ -387,9 +437,9 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
       ostr << jnt << " ";
     }
     ostr << "\b \b";  // erase last character
-    RCLCPP_WARN_STREAM_THROTTLE(
+    RCLCPP_WARN_THROTTLE(
       node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
-      "Joint(s) [" << ostr.str().c_str() << "] would exceed acceleration limits, limiting");
+      "Joint(s) [%s] would exceed acceleration limits, limiting", ostr.str().c_str());
   }
 
   if (limited_jnts_dec.size() > 0)

joint_limits/test/test_joint_saturation_limiter.cpp

@@ -157,16 +157,45 @@ TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied_w
 
     // within limits
     desired_joint_states_.positions[0] = 1.0;
-    desired_joint_states_.velocities[0] = 1.5;     // valid pos derivative as well
-    desired_joint_states_.accelerations[0] = 2.9;  // valid pos derivative as well
+    desired_joint_states_.velocities[0] = 1.5;       // valid pos derivative as well
+    desired_joint_states_.accelerations[0] = 1.333;  // valid pos derivative as well
     ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
 
     // check if no limits applied
     CHECK_STATE_SINGLE_JOINT(
       desired_joint_states_, 0,
-      1.0,  // pos unchanged
-      1.5,  // vel unchanged
-      2.9   // acc = vel / 1.0
+      1.0,   // pos unchanged
+      1.5,   // vel unchanged
+      1.333  // acc = vel / 1.0
+    );
+  }
+}
+
+TEST_F(JointSaturationLimiterTest, when_derivative_too_high_expect_lower_effective_limits)
+{
+  SetupNode("joint_saturation_limiter");
+  Load();
+
+  if (joint_limiter_)
+  {
+    Init();
+    Configure();
+
+    rclcpp::Duration period(1.0, 0.0);  // 1 second
+    // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5
+
+    // within limits
+    desired_joint_states_.positions[0] = 1.0;
+    desired_joint_states_.velocities[0] = 1.5;     // valid pos derivative as well
+    desired_joint_states_.accelerations[0] = 2.9;  // valid pos derivative as well
+    ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+    // check if limits applied
+    CHECK_STATE_SINGLE_JOINT(
+      desired_joint_states_, 0,
+      0.6665,  // pos = 0.0 + 0.5 * 1.333 * 1.0^2
+      1.333,   // vel = 0.0 + 1.333 * 1.0
+      1.333    // acc = 5*0.266 = 1.333 (reduction ratio = max_vel/implicit_vel -> 2.0/7.5 = 0.266)
     );
   }
 }
@@ -193,9 +222,9 @@ TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limi
     // check if limits applied
     CHECK_STATE_SINGLE_JOINT(
       desired_joint_states_, 0,
-      0.0,       // pos = pos + max_vel * dt
-      2.0,       // vel limited to max_vel
-      2.0 / 1.0  // acc set to vel change/DT
+      -1.333,  // pos = = 0.0 + 0.5 * 1.333 * 1.0^2 (since min position is 0)
+      1.333,   // vel = 0.0 + 1.333 * 1.0
+      1.333    // acc = 5*0.266 = 1.333 (reduction ratio = max_vel/implicit_vel -> 2.0/7.5 = 0.266)
     );
 
     // check opposite velocity direction (sign copy)
@@ -208,9 +237,9 @@ TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limi
     // check if vel and acc limits applied
     CHECK_STATE_SINGLE_JOINT(
       desired_joint_states_, 0,
-      0.0,        // pos = pos - max_vel * dt
-      -2.0,       // vel limited to -max_vel
-      -2.0 / 1.0  // acc set to vel change/DT
+      0.0,   // pos = 1.0 - 0.5 * 2.0 * 1.0^2 (since max pos is 1)
+      -2.0,  // vel = 0.0 - 2 * 1.0
+      -2.0   // dec = 7.5*0.266 = 2.0 (reduction ratio = max_vel/implicit_vel -> 2.0/7.5 = 0.266)
     );
   }
 }
@@ -517,6 +546,35 @@ TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_exp
   }
 }
 
+TEST_F(JointSaturationLimiterTest, when_implicit_vel_exceeds_max_vel_expect_vel_enforced)
+{
+  SetupNode("joint_saturation_limiter_nodeclimit");
+  Load();
+
+  if (joint_limiter_)
+  {
+    Init();
+    Configure();
+
+    rclcpp::Duration period(0, 500000000);  // 0.5 seconds
+    // Joint limits: max_vel = 2.0, max_acc = 5.0, no decel limits
+    // implicit_vel = max_acc * dt = 5.0 * 0.5 = 2.5 > max_vel (2.0)
+    // effective_max_acc = max_vel / dt = 2.0 / 0.5 = 4.0
+
+    current_joint_states_.positions[0] = 0.0;
+    current_joint_states_.velocities[0] = -0.5;
+    desired_joint_states_.positions.clear();
+    desired_joint_states_.velocities[0] = 3.0;  // exceeds max_vel -> clamped to 2.0
+    // After vel clamping: desired_acc = (2.0 - (-0.5)) / 0.5 = 5.0 > effective_max_acc (4.0)
+    ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+    // Acc limited to effective_max_acc = 4.0 (not raw max_acc = 5.0)
+    // Vel recomputed: -0.5 + 4.0 * 0.5 = 1.5
+    ASSERT_NEAR(desired_joint_states_.velocities[0], 1.5, COMMON_THRESHOLD);
+    ASSERT_NEAR(desired_joint_states_.accelerations[0], 4.0, COMMON_THRESHOLD);
+  }
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleMock(&argc, argv);