Fix the crashing joint limiters when used with multiple interfaces (#2371)

saikishor · web-flow · commit 7061ac416de1 · 2025-07-16T18:56:52.000+01:00
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -740,7 +740,7 @@ class ResourceStorage
         std::vector<joint_limits::SoftJointLimits> soft_limits;
         const std::vector<joint_limits::JointLimits> hard_limits{limits};
         joint_limits::JointInterfacesCommandLimiterData data;
-        data.joint_name = joint_name;
+        data.set_joint_name(joint_name);
         limiters_data_.insert({joint_name, data});
         // If the joint limits is found in the softlimits, then extract it
         if (hw_info.soft_limits.find(joint_name) != hw_info.soft_limits.end())
@@ -1025,7 +1025,7 @@ class ResourceStorage
           {
             is_limited = false;
             joint_limits::JointInterfacesCommandLimiterData data;
-            data.joint_name = joint_name;
+            data.set_joint_name(joint_name);
             update_joint_limiters_data(data.joint_name, state_interface_map_, data.actual);
             if (interface_name == hardware_interface::HW_IF_POSITION)
             {
diff --git a/joint_limits/include/joint_limits/data_structures.hpp b/joint_limits/include/joint_limits/data_structures.hpp
@@ -70,6 +70,7 @@ struct JointControlInterfacesData
   std::string to_string() const
   {
     std::string str;
+    str += "Joint: '" + joint_name + "', ";
     if (has_position())
     {
       str += "position: " + std::to_string(position.value()) + ", ";
@@ -117,6 +118,15 @@ struct JointInterfacesCommandLimiterData
 
   bool has_limited_data() const { return limited.has_data(); }
 
+  void set_joint_name(const std::string & name)
+  {
+    joint_name = name;
+    actual.joint_name = name;
+    command.joint_name = name;
+    limited.joint_name = name;
+    prev_command.joint_name = name;
+  }
+
   std::string to_string() const
   {
     return fmt::format(
diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp
@@ -32,6 +32,14 @@ constexpr double POSITION_BOUNDS_TOLERANCE = 0.002;
 constexpr double OUT_OF_BOUNDS_EXCEPTION_TOLERANCE = 0.0087;
 }  // namespace internal
 
+/**
+ * @brief Updates the previous command with the desired command.
+ * @param prev_command The previous command to update.
+ * @param desired The desired command which is limited.
+ */
+void update_prev_command(
+  const JointControlInterfacesData & desired, JointControlInterfacesData & prev_command);
+
 /**
  * @brief Checks if a value is limited by the given limits.
  * @param value The value to check.
diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp
@@ -72,6 +72,32 @@ void verify_actual_position_within_limits(
 }
 }  // namespace internal
 
+void update_prev_command(
+  const JointControlInterfacesData & desired, JointControlInterfacesData & prev_command)
+{
+  if (desired.has_position())
+  {
+    prev_command.position = desired.position;
+  }
+  if (desired.has_velocity())
+  {
+    prev_command.velocity = desired.velocity;
+  }
+  if (desired.has_effort())
+  {
+    prev_command.effort = desired.effort;
+  }
+  if (desired.has_acceleration())
+  {
+    prev_command.acceleration = desired.acceleration;
+  }
+  if (desired.has_jerk())
+  {
+    prev_command.jerk = desired.jerk;
+  }
+  prev_command.joint_name = desired.joint_name;
+}
+
 bool is_limited(double value, double min, double max) { return value < min || value > max; }
 
 PositionLimits compute_position_limits(
diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp
@@ -61,45 +61,26 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
   // The following conditional filling is needed for cases of having certain information missing
   if (!prev_command_.has_data())
   {
-    if (actual.has_position())
+    if (desired.has_position())
     {
-      prev_command_.position = actual.position;
+      prev_command_.position = actual.has_position() ? actual.position : desired.position;
     }
-    else if (desired.has_position())
+    if (desired.has_velocity())
     {
-      prev_command_.position = desired.position;
+      prev_command_.velocity = actual.has_velocity() ? actual.velocity : desired.velocity;
     }
-    if (actual.has_velocity())
+    if (desired.has_effort())
     {
-      prev_command_.velocity = actual.velocity;
+      prev_command_.effort = actual.has_effort() ? actual.effort : desired.effort;
     }
-    else if (desired.has_velocity())
+    if (desired.has_acceleration())
     {
-      prev_command_.velocity = desired.velocity;
+      prev_command_.acceleration =
+        actual.has_acceleration() ? actual.acceleration : desired.acceleration;
     }
-    if (actual.has_effort())
+    if (desired.has_jerk())
     {
-      prev_command_.effort = actual.effort;
-    }
-    else if (desired.has_effort())
-    {
-      prev_command_.effort = desired.effort;
-    }
-    if (actual.has_acceleration())
-    {
-      prev_command_.acceleration = actual.acceleration;
-    }
-    else if (desired.has_acceleration())
-    {
-      prev_command_.acceleration = desired.acceleration;
-    }
-    if (actual.has_jerk())
-    {
-      prev_command_.jerk = actual.jerk;
-    }
-    else if (desired.has_jerk())
-    {
-      prev_command_.jerk = desired.jerk;
+      prev_command_.jerk = actual.has_jerk() ? actual.jerk : desired.jerk;
     }
     if (actual.has_data())
     {
@@ -159,7 +140,7 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
     desired.jerk = std::clamp(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk);
   }
 
-  prev_command_ = desired;
+  update_prev_command(desired, prev_command_);
 
   return limits_enforced;
 }
diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp
@@ -44,45 +44,26 @@ bool JointSoftLimiter::on_enforce(
 
   if (!prev_command_.has_data())
   {
-    if (actual.has_position())
+    if (desired.has_position())
     {
-      prev_command_.position = actual.position;
+      prev_command_.position = actual.has_position() ? actual.position : desired.position;
     }
-    else if (desired.has_position())
+    if (desired.has_velocity())
     {
-      prev_command_.position = desired.position;
+      prev_command_.velocity = actual.has_velocity() ? actual.velocity : desired.velocity;
     }
-    if (actual.has_velocity())
+    if (desired.has_effort())
     {
-      prev_command_.velocity = actual.velocity;
+      prev_command_.effort = actual.has_effort() ? actual.effort : desired.effort;
     }
-    else if (desired.has_velocity())
+    if (desired.has_acceleration())
     {
-      prev_command_.velocity = desired.velocity;
+      prev_command_.acceleration =
+        actual.has_acceleration() ? actual.acceleration : desired.acceleration;
     }
-    if (actual.has_effort())
+    if (desired.has_jerk())
     {
-      prev_command_.effort = actual.effort;
-    }
-    else if (desired.has_effort())
-    {
-      prev_command_.effort = desired.effort;
-    }
-    if (actual.has_acceleration())
-    {
-      prev_command_.acceleration = actual.acceleration;
-    }
-    else if (desired.has_acceleration())
-    {
-      prev_command_.acceleration = desired.acceleration;
-    }
-    if (actual.has_jerk())
-    {
-      prev_command_.jerk = actual.jerk;
-    }
-    else if (desired.has_jerk())
-    {
-      prev_command_.jerk = desired.jerk;
+      prev_command_.jerk = actual.has_jerk() ? actual.jerk : desired.jerk;
     }
     if (actual.has_data())
     {
@@ -266,7 +247,7 @@ bool JointSoftLimiter::on_enforce(
     limits_enforced = true;
   }
 
-  prev_command_ = desired;
+  update_prev_command(desired, prev_command_);
 
   return limits_enforced;
 }
diff --git a/joint_limits/test/test_joint_soft_limiter.cpp b/joint_limits/test/test_joint_soft_limiter.cpp
@@ -548,6 +548,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases)
   limits.max_effort = 200.0;
   joint_limits::SoftJointLimits soft_limits;
   ASSERT_TRUE(Init(limits, soft_limits));
+  last_commanded_state_ = {};
   ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
 
   // Reset the desired and actual states
@@ -651,6 +652,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases)
   // Check with high values of k_velocity (hard limits should be considered)
   soft_limits.k_velocity = 5000.0;
   ASSERT_TRUE(Init(limits, soft_limits));
+  last_commanded_state_ = {};
   ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
 
   for (auto act_pos :
@@ -674,6 +676,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases)
   // Check with low values of k_velocity
   soft_limits.k_velocity = 300.0;
   ASSERT_TRUE(Init(limits, soft_limits));
+  last_commanded_state_ = {};
   ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
 
   for (auto act_pos :
@@ -702,6 +705,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases)
   soft_limits.min_position = -4.0;
   soft_limits.max_position = 4.0;
   ASSERT_TRUE(Init(limits, soft_limits));
+  last_commanded_state_ = {};
   ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
 
   test_limit_enforcing(0.0, 0.5, 20.0, 20.0, false);
@@ -762,6 +766,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases)
   soft_limits.min_position = -10.0;
   soft_limits.max_position = 10.0;
   ASSERT_TRUE(Init(limits, soft_limits));
+  last_commanded_state_ = {};
   ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
 
   for (auto act_pos :
@@ -788,6 +793,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases)
   soft_limits.min_position = -10.0;
   soft_limits.max_position = 10.0;
   ASSERT_TRUE(Init(limits, soft_limits));
+  last_commanded_state_ = {};
   ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
 
   for (auto act_pos :

Original file line number	Diff line number	Diff line change
`@@ -61,45 +61,26 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(`
`61`	`61`	`// The following conditional filling is needed for cases of having certain information missing`
`62`	`62`	`if (!prev_command_.has_data())`
`63`	`63`	`{`
`64`		`- if (actual.has_position())`
	`64`	`+ if (desired.has_position())`
`65`	`65`	`{`
`66`		`- prev_command_.position = actual.position;`
	`66`	`+ prev_command_.position = actual.has_position() ? actual.position : desired.position;`
`67`	`67`	`}`
`68`		`- else if (desired.has_position())`
	`68`	`+ if (desired.has_velocity())`
`69`	`69`	`{`
`70`		`- prev_command_.position = desired.position;`
	`70`	`+ prev_command_.velocity = actual.has_velocity() ? actual.velocity : desired.velocity;`
`71`	`71`	`}`
`72`		`- if (actual.has_velocity())`
	`72`	`+ if (desired.has_effort())`
`73`	`73`	`{`
`74`		`- prev_command_.velocity = actual.velocity;`
	`74`	`+ prev_command_.effort = actual.has_effort() ? actual.effort : desired.effort;`
`75`	`75`	`}`
`76`		`- else if (desired.has_velocity())`
	`76`	`+ if (desired.has_acceleration())`
`77`	`77`	`{`
`78`		`- prev_command_.velocity = desired.velocity;`
	`78`	`+ prev_command_.acceleration =`
	`79`	`+ actual.has_acceleration() ? actual.acceleration : desired.acceleration;`
`79`	`80`	`}`
`80`		`- if (actual.has_effort())`
	`81`	`+ if (desired.has_jerk())`
`81`	`82`	`{`
`82`		`- prev_command_.effort = actual.effort;`
`83`		`- }`
`84`		`- else if (desired.has_effort())`
`85`		`- {`
`86`		`- prev_command_.effort = desired.effort;`
`87`		`- }`
`88`		`- if (actual.has_acceleration())`
`89`		`- {`
`90`		`- prev_command_.acceleration = actual.acceleration;`
`91`		`- }`
`92`		`- else if (desired.has_acceleration())`
`93`		`- {`
`94`		`- prev_command_.acceleration = desired.acceleration;`
`95`		`- }`
`96`		`- if (actual.has_jerk())`
`97`		`- {`
`98`		`- prev_command_.jerk = actual.jerk;`
`99`		`- }`
`100`		`- else if (desired.has_jerk())`
`101`		`- {`
`102`		`- prev_command_.jerk = desired.jerk;`
	`83`	`+ prev_command_.jerk = actual.has_jerk() ? actual.jerk : desired.jerk;`
`103`	`84`	`}`
`104`	`85`	`if (actual.has_data())`
`105`	`86`	`{`
`@@ -159,7 +140,7 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(`
`159`	`140`	`desired.jerk = std::clamp(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk);`
`160`	`141`	`}`
`161`	`142`
`162`		`- prev_command_ = desired;`
	`143`	`+ update_prev_command(desired, prev_command_);`
`163`	`144`
`164`	`145`	`return limits_enforced;`
`165`	`146`	`}`
Original file line number	Diff line number	Diff line change
`@@ -44,45 +44,26 @@ bool JointSoftLimiter::on_enforce(`
`44`	`44`
`45`	`45`	`if (!prev_command_.has_data())`
`46`	`46`	`{`
`47`		`- if (actual.has_position())`
	`47`	`+ if (desired.has_position())`
`48`	`48`	`{`
`49`		`- prev_command_.position = actual.position;`
	`49`	`+ prev_command_.position = actual.has_position() ? actual.position : desired.position;`
`50`	`50`	`}`
`51`		`- else if (desired.has_position())`
	`51`	`+ if (desired.has_velocity())`
`52`	`52`	`{`
`53`		`- prev_command_.position = desired.position;`
	`53`	`+ prev_command_.velocity = actual.has_velocity() ? actual.velocity : desired.velocity;`
`54`	`54`	`}`
`55`		`- if (actual.has_velocity())`
	`55`	`+ if (desired.has_effort())`
`56`	`56`	`{`
`57`		`- prev_command_.velocity = actual.velocity;`
	`57`	`+ prev_command_.effort = actual.has_effort() ? actual.effort : desired.effort;`
`58`	`58`	`}`
`59`		`- else if (desired.has_velocity())`
	`59`	`+ if (desired.has_acceleration())`
`60`	`60`	`{`
`61`		`- prev_command_.velocity = desired.velocity;`
	`61`	`+ prev_command_.acceleration =`
	`62`	`+ actual.has_acceleration() ? actual.acceleration : desired.acceleration;`
`62`	`63`	`}`
`63`		`- if (actual.has_effort())`
	`64`	`+ if (desired.has_jerk())`
`64`	`65`	`{`
`65`		`- prev_command_.effort = actual.effort;`
`66`		`- }`
`67`		`- else if (desired.has_effort())`
`68`		`- {`
`69`		`- prev_command_.effort = desired.effort;`
`70`		`- }`
`71`		`- if (actual.has_acceleration())`
`72`		`- {`
`73`		`- prev_command_.acceleration = actual.acceleration;`
`74`		`- }`
`75`		`- else if (desired.has_acceleration())`
`76`		`- {`
`77`		`- prev_command_.acceleration = desired.acceleration;`
`78`		`- }`
`79`		`- if (actual.has_jerk())`
`80`		`- {`
`81`		`- prev_command_.jerk = actual.jerk;`
`82`		`- }`
`83`		`- else if (desired.has_jerk())`
`84`		`- {`
`85`		`- prev_command_.jerk = desired.jerk;`
	`66`	`+ prev_command_.jerk = actual.has_jerk() ? actual.jerk : desired.jerk;`
`86`	`67`	`}`
`87`	`68`	`if (actual.has_data())`
`88`	`69`	`{`
`@@ -266,7 +247,7 @@ bool JointSoftLimiter::on_enforce(`
`266`	`247`	`limits_enforced = true;`
`267`	`248`	`}`
`268`	`249`
`269`		`- prev_command_ = desired;`
	`250`	`+ update_prev_command(desired, prev_command_);`
`270`	`251`
`271`	`252`	`return limits_enforced;`
`272`	`253`	`}`