From d3838fdf928fe42ba17abcb15ad0f4c68762652d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= <denis@stoglrobotics.de>
Date: Thu, 8 Feb 2024 18:31:32 +0100
Subject: [PATCH] Small fixes of calculation when differnet set of inputs are
 provided.

---
 joint_limits/src/joint_saturation_limiter.cpp | 19 ++++++++----
 .../test/test_joint_saturation_limiter.cpp    | 29 +++++++++++++++++++
 2 files changed, 42 insertions(+), 6 deletions(-)

diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp
index 2a0b908b0e..5020cab27b 100644
--- 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
index 3c1353ecff..e78c4b8994 100644
--- 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");