Small fixes of calculation when differnet set of inputs are provided.

joint_limits/src/joint_saturation_limiter.cpp

@@ -121,6 +121,12 @@ bool JointSaturationLimiter<JointLimits>::on_enforce(
  // limit velocity
  if (joint_limits_[index].has_velocity_limits)
  {
+ // if desired velocity is not defined calculate it from positions
+ if (std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_vel[index]))
+ {
+ desired_vel[index] =
+ (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds;
+ }
  // clamp input vel_cmd
  if (std::fabs(desired_vel[index]) > joint_limits_[index].max_velocity)
  {
@@ -135,7 +141,6 @@ bool JointSaturationLimiter<JointLimits>::on_enforce(
  current_joint_states.positions[index] + desired_vel[index] * dt_seconds;
  }
 
- // compute desired_acc when velocity is limited
  desired_acc[index] =
  (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds;
  }
@@ -166,11 +171,13 @@ bool JointSaturationLimiter<JointLimits>::on_enforce(
  }
  };
 
- // limit acc for pos_cmd and/or vel_cmd
-
- // compute desired_acc with desired_vel and vel_state
- desired_acc[index] =
- (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds;
+ // if desired acceleration if not provided compute it from desired_vel and vel_state
+ if (
+ std::fabs(desired_acc[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_acc[index]))
+ {
+ desired_acc[index] =
+ (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds;
+ }
 
  // check if decelerating - if velocity is changing toward 0
  bool deceleration_limit_applied = false;

joint_limits/test/test_joint_saturation_limiter.cpp

@@ -142,6 +142,35 @@ TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied)
  }
 }
 
+TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied_with_acc)
+{
+ 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_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
+ );
+ }
+}
+
 TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_enforced)
 {
  SetupNode("joint_saturation_limiter");