Fix disable velocity and effort limiting feature

joint_limits/include/joint_limits/joint_limits_helpers.hpp

@@ -80,12 +80,14 @@ VelocityLimits compute_velocity_limits(
   const std::optional<double> & prev_command_vel, double dt);
 
 /**
- * @brief Compute the effort limits based on the position and velocity limits.
+ * @brief Computes the effort limits based on the position and velocity limits.
+ *
  * @param limits The joint limits.
  * @param act_pos The actual position of the joint.
  * @param act_vel The actual velocity of the joint.
- * @param dt The time step.
- * @return The effort limits, first is the lower limit and second is the upper limit.
+ * @param dt The time step (currently unused).
+ * @return The effort limits: lower_limit is the minimum allowed effort, upper_limit is the maximum.
+ * @note When the effort limits are disabled, this method returns [-inf, inf]. 
  */
 EffortLimits compute_effort_limits(
   const joint_limits::JointLimits & limits, const std::optional<double> & act_pos,

joint_limits/src/joint_limits_helpers.cpp

@@ -197,9 +197,13 @@ EffortLimits compute_effort_limits(
   const joint_limits::JointLimits & limits, const std::optional<double> & act_pos,
   const std::optional<double> & act_vel, double /*dt*/)
 {
-  const double max_effort =
-    limits.has_effort_limits ? limits.max_effort : std::numeric_limits<double>::infinity();
-  EffortLimits eff_limits(-max_effort, max_effort);
+  // When effort limits are disabled the effort command must pass through untouched
+  if (!limits.has_effort_limits)
+  {
+    return EffortLimits(
+      -std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity());
+  }
+  EffortLimits eff_limits(-limits.max_effort, limits.max_effort);
   if (limits.has_position_limits && act_pos.has_value() && act_vel.has_value())
   {
     if ((act_pos.value() <= limits.min_position) && (act_vel.value() <= 0.0))

joint_limits/test/test_joint_range_limiter.cpp

@@ -452,6 +452,85 @@ TEST_F(JointSaturationLimiterTest, check_desired_effort_only_cases)
   test_limit_enforcing(-5.0, -0.2, 30.0, 30.0, false);
 }
 
+TEST_F(JointSaturationLimiterTest, when_effort_limits_disabled_expect_no_effort_enforcement)
+{
+  SetupNode("joint_saturation_limiter");
+  ASSERT_TRUE(Load());
+
+  // effort limits are OFF, but position and velocity limits are still active
+  joint_limits::JointLimits limits;
+  limits.has_position_limits = true;
+  limits.min_position = -5.0;
+  limits.max_position = 5.0;
+  limits.has_velocity_limits = true;
+  limits.max_velocity = 1.0;
+  limits.has_effort_limits = false;  // <-- disabled
+  ASSERT_TRUE(Init(limits));
+  ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
+
+  rclcpp::Duration period(1, 0);  // 1 second
+
+  auto test_effort_passes_through = [&](
+                                      const std::optional<double> & actual_position,
+                                      const std::optional<double> & actual_velocity,
+                                      double desired_effort)
+  {
+    desired_state_ = {};
+    actual_state_ = {};
+    const double act_pos = actual_position.has_value() ? actual_position.value()
+                                                       : std::numeric_limits<double>::quiet_NaN();
+    const double act_vel = actual_velocity.has_value() ? actual_velocity.value()
+                                                       : std::numeric_limits<double>::quiet_NaN();
+    SCOPED_TRACE(
+      "Testing effort passthrough for actual position: " + std::to_string(act_pos) +
+      ", actual velocity: " + std::to_string(act_vel) + ", desired effort: " +
+      std::to_string(desired_effort) + " for the joint limits : " + limits.to_string());
+    if (actual_position.has_value())
+    {
+      actual_state_.position = actual_position.value();
+    }
+    if (actual_velocity.has_value())
+    {
+      actual_state_.velocity = actual_velocity.value();
+    }
+    desired_state_.effort = desired_effort;
+    // With effort limits disabled, enforce() must NOT clamp effort regardless of
+    // the position/velocity state it must pass through completely untouched.
+    ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period));
+    EXPECT_TRUE(desired_state_.has_effort());
+    EXPECT_NEAR(desired_state_.effort.value(), desired_effort, COMMON_THRESHOLD);
+  };
+
+  // Normal operating region: any effort must pass through unchanged
+  test_effort_passes_through(0.0, 0.0, 10000.0);
+  test_effort_passes_through(0.0, 0.0, -10000.0);
+
+  // At max position + zero velocity (old code zeroed upper_limit to 0.0 here)
+  test_effort_passes_through(5.0, 0.0, 10000.0);
+  test_effort_passes_through(5.0, 0.0, -10000.0);
+  // Beyond max position
+  test_effort_passes_through(6.0, 0.0, 10000.0);
+  test_effort_passes_through(6.0, 0.0, -10000.0);
+
+  // At max position + positive velocity (moving further out)
+  test_effort_passes_through(5.0, 0.2, 10000.0);
+  test_effort_passes_through(5.0, 0.2, -10000.0);
+
+  // At min position + zero velocity (old code zeroed lower_limit to 0.0 here)
+  test_effort_passes_through(-5.0, 0.0, 10000.0);
+  test_effort_passes_through(-5.0, 0.0, -10000.0);
+  // At min position + negative velocity (moving further out)
+  test_effort_passes_through(-5.0, -0.2, 10000.0);
+  test_effort_passes_through(-5.0, -0.2, -10000.0);
+
+  // Over max velocity (old code zeroed upper_limit to 0.0 here)
+  test_effort_passes_through(0.0, 1.5, 10000.0);
+  test_effort_passes_through(0.0, 1.5, -10000.0);
+  // Over negative max velocity
+  test_effort_passes_through(0.0, -1.5, 10000.0);
+  test_effort_passes_through(0.0, -1.5, -10000.0);
+}
+
 TEST_F(JointSaturationLimiterTest, check_desired_acceleration_only_cases)
 {
   SetupNode("joint_saturation_limiter");

joint_limits/test/test_joint_soft_limiter.cpp

@@ -878,6 +878,97 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases)
   }
 }
 
+TEST_F(JointSoftLimiterTest, when_effort_limits_disabled_expect_no_effort_enforcement)
+{
+  SetupNode("joint_saturation_limiter");
+  ASSERT_TRUE(Load());
+
+  // effort limits are OFF, but position and velocity limits are still active
+  joint_limits::JointLimits limits;
+  limits.has_position_limits = true;
+  limits.min_position = -5.0;
+  limits.max_position = 5.0;
+  limits.has_velocity_limits = true;
+  limits.max_velocity = 1.0;
+  limits.has_effort_limits = false;  // <-- disabled
+  joint_limits::SoftJointLimits soft_limits;
+  ASSERT_TRUE(Init(limits, soft_limits));
+  last_commanded_state_ = {};
+  ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
+
+  rclcpp::Duration period(1, 0);  // 1 second
+
+  auto test_effort_passes_through = [&](
+                                      const std::optional<double> & actual_position,
+                                      const std::optional<double> & actual_velocity,
+                                      double desired_effort)
+  {
+    desired_state_ = {};
+    actual_state_ = {};
+    const double act_pos = actual_position.has_value() ? actual_position.value()
+                                                       : std::numeric_limits<double>::quiet_NaN();
+    const double act_vel = actual_velocity.has_value() ? actual_velocity.value()
+                                                       : std::numeric_limits<double>::quiet_NaN();
+    SCOPED_TRACE(
+      "Testing effort passthrough for actual position: " + std::to_string(act_pos) +
+      ", actual velocity: " + std::to_string(act_vel) + ", desired effort: " +
+      std::to_string(desired_effort) + " for the joint limits : " + limits.to_string());
+    if (actual_position.has_value())
+    {
+      actual_state_.position = actual_position.value();
+    }
+    if (actual_velocity.has_value())
+    {
+      actual_state_.velocity = actual_velocity.value();
+    }
+    desired_state_.effort = desired_effort;
+    // With effort limits disabled, enforce() must NOT clamp effort regardless of
+    // the position/velocity state it must pass through completely untouched.
+    ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period));
+    EXPECT_TRUE(desired_state_.has_effort());
+    EXPECT_NEAR(desired_state_.effort.value(), desired_effort, COMMON_THRESHOLD);
+  };
+
+  // Normal operating region: any effort must pass through unchanged
+  test_effort_passes_through(0.0, 0.0, 10000.0);
+  test_effort_passes_through(0.0, 0.0, -10000.0);
+
+  // At max position + zero velocity (old code zeroed upper_limit to 0.0 here)
+  test_effort_passes_through(5.0, 0.0, 10000.0);
+  test_effort_passes_through(5.0, 0.0, -10000.0);
+  // Beyond max position
+  test_effort_passes_through(6.0, 0.0, 10000.0);
+  test_effort_passes_through(6.0, 0.0, -10000.0);
+
+  // At max position + positive velocity (moving further out)
+  test_effort_passes_through(5.0, 0.2, 10000.0);
+  test_effort_passes_through(5.0, 0.2, -10000.0);
+
+  // At min position + zero velocity (old code zeroed lower_limit to 0.0 here)
+  test_effort_passes_through(-5.0, 0.0, 10000.0);
+  test_effort_passes_through(-5.0, 0.0, -10000.0);
+  // At min position + negative velocity (moving further out)
+  test_effort_passes_through(-5.0, -0.2, 10000.0);
+  test_effort_passes_through(-5.0, -0.2, -10000.0);
+
+  // Over max velocity (old code zeroed upper_limit to 0.0 here)
+  test_effort_passes_through(0.0, 1.5, 10000.0);
+  test_effort_passes_through(0.0, 1.5, -10000.0);
+  // Over negative max velocity
+  test_effort_passes_through(0.0, -1.5, 10000.0);
+  test_effort_passes_through(0.0, -1.5, -10000.0);
+
+  // Also verify with k_velocity set: soft effort shaping must also be skipped
+  soft_limits.k_velocity = 5000.0;
+  ASSERT_TRUE(Init(limits, soft_limits));
+  last_commanded_state_ = {};
+  ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
+  test_effort_passes_through(0.0, 0.5, 10000.0);
+  test_effort_passes_through(0.0, 0.5, -10000.0);
+  test_effort_passes_through(5.0, 0.2, 10000.0);
+  test_effort_passes_through(5.0, 0.2, -10000.0);
+}
+
 TEST_F(JointSoftLimiterTest, check_desired_acceleration_only_cases)
 {
   SetupNode("joint_saturation_limiter");