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

hardware_interface/src/resource_manager.cpp

@@ -718,7 +718,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())
@@ -1003,7 +1003,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)
             {

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(

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.

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(

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;
 }

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;
 }

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 :