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

pal-robotics-forks · May 6, 2024 · b5683c1 · b5683c1
1 parent 64d0bbc
commit b5683c1
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 6 deletions.
diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/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;

diff --git a/joint_limits/test/test_joint_saturation_limiter.cpp b/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");