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,11 @@ class JointSaturationLimiter : public JointLimiterInterface<JointLimitsStateData
   bool on_configure(const JointLimitsStateDataType & current_joint_states) override
   {
     prev_command_ = current_joint_states;
+    const auto num_joints = this->number_of_joints_;
+    prev_dt_seconds_ = 0.0;
+    effective_max_acc_.assign(num_joints, 0.0);
+    effective_max_dec_.assign(num_joints, 0.0);
+
     return true;
   }
 
@@ -91,6 +96,11 @@ class JointSaturationLimiter : public JointLimiterInterface<JointLimitsStateData
   rclcpp::Clock::SharedPtr clock_;
   JointLimitsStateDataType prev_command_;
   std::mutex mutex_;
+
+private:
+  std::vector<double> effective_max_acc_;
+  std::vector<double> effective_max_dec_;
+  double prev_dt_seconds_;
 };
 
 template <typename JointLimitsStateDataType>

joint_limits/src/joint_saturation_limiter.cpp

@@ -40,9 +40,53 @@ 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
+  // if the dt is changed then recalculate the max_acceleration/max_deceleration if implicit vel
+  // exceeds max vel
+  if (std::fabs(dt_seconds - prev_dt_seconds_) > 1e-9)
+  {
+    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_STREAM_THROTTLE(
+            node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+            "Joint '" << joint_names_[i] << "': dt (" << dt_seconds
+                      << ") is too large for max_acceleration ("
+                      << joint_limits_[i].max_acceleration << ") and max_velocity ("
+                      << joint_limits_[i].max_velocity << "); max_acc * dt = " << implicit_vel
+                      << " exceeds max_velocity");
+          effective_max_acc_[i] = joint_limits_[i].max_velocity / dt_seconds;
+        }
+        else
+        {
+          effective_max_acc_[i] = joint_limits_[i].max_acceleration;
+        }
+      }
+      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_STREAM_THROTTLE(
+            node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+            "Joint '" << joint_names_[i] << "': dt (" << dt_seconds
+                      << ") is too large for max_deceleration ("
+                      << joint_limits_[i].max_deceleration << ") and max_velocity ("
+                      << joint_limits_[i].max_velocity << "); max_dec * dt = " << implicit_vel
+                      << " exceeds max_velocity");
+          effective_max_dec_[i] = joint_limits_[i].max_velocity / dt_seconds;
+        }
+        else
+        {
+          effective_max_dec_[i] = joint_limits_[i].max_deceleration;
+        }
+      }
+    }
+    prev_dt_seconds_ = dt_seconds;
+  }
 
   // check for required inputs combination
   const bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_);
@@ -185,17 +229,17 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
           // limit deceleration
           if (joint_limits_[index].has_deceleration_limits)
           {
-            limit_applied = apply_acc_or_dec_limit(
-              joint_limits_[index].max_deceleration, desired_acc, limited_jnts_dec);
+            limit_applied =
+              apply_acc_or_dec_limit(effective_max_dec_[index], desired_acc, limited_jnts_dec);
             deceleration_limit_applied = true;
           }
         }
 
         // limit acceleration (fallback to acceleration if no deceleration limits)
         if (joint_limits_[index].has_acceleration_limits && !deceleration_limit_applied)
         {
-          limit_applied = apply_acc_or_dec_limit(
-            joint_limits_[index].max_acceleration, desired_acc, limited_jnts_acc);
+          limit_applied =
+            apply_acc_or_dec_limit(effective_max_acc_[index], desired_acc, limited_jnts_acc);
         }
 
         if (limit_applied)
@@ -253,11 +297,11 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
       double stopping_deccel = std::fabs(desired_vel[index] / dt_seconds);
       if (joint_limits_[index].has_deceleration_limits)
       {
-        stopping_deccel = joint_limits_[index].max_deceleration;
+        stopping_deccel = effective_max_dec_[index];
       }
       else if (joint_limits_[index].has_acceleration_limits)
       {
-        stopping_deccel = joint_limits_[index].max_acceleration;
+        stopping_deccel = effective_max_acc_[index];
       }
 
       double stopping_distance =

joint_limits/test/test_joint_saturation_limiter.cpp

@@ -158,15 +158,15 @@ 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_.accelerations[0] = 2.0;  // 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
+      2.0   // acc = vel / 1.0
     );
   }
 }
@@ -517,6 +517,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);