Fix disabling joint limits via URDF (#2992)

Author: saikishor

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -86,6 +86,9 @@ struct ComponentInfo
   ///  Hold the value of the mimic attribute if given, NOT_SET otherwise
   MimicAttribute is_mimic = MimicAttribute::NOT_SET;
 
+  /// Whether limits are enabled for this component (set at joint level with <limits enable="..."/>)
+  bool enable_limits = true;
+
   /**
    * Name of the command interfaces that can be set, e.g. "position", "velocity", etc.
    * Used by joints and GPIOs.

hardware_interface/src/component_parser.cpp

@@ -454,19 +454,20 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it
   }
 
   // Option enable or disable the interface limits, by default they are enabled
-  bool enable_limits = true;
+  component.enable_limits = true;
   const auto * limits_it = component_it->FirstChildElement(kLimitsTag);
   if (limits_it)
   {
-    enable_limits = parse_bool(get_attribute_value(limits_it, kEnableAttribute, limits_it->Name()));
+    component.enable_limits =
+      parse_bool(get_attribute_value(limits_it, kEnableAttribute, limits_it->Name()));
   }
 
   // Parse all command interfaces
   const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag);
   while (command_interfaces_it)
   {
     InterfaceInfo cmd_info = parse_interfaces_from_xml(command_interfaces_it);
-    cmd_info.enable_limits &= enable_limits;
+    cmd_info.enable_limits &= component.enable_limits;
     component.command_interfaces.push_back(cmd_info);
     command_interfaces_it = command_interfaces_it->NextSiblingElement(kCommandInterfaceTag);
   }
@@ -476,7 +477,7 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it
   while (state_interfaces_it)
   {
     InterfaceInfo state_info = parse_interfaces_from_xml(state_interfaces_it);
-    state_info.enable_limits &= enable_limits;
+    state_info.enable_limits &= component.enable_limits;
     component.state_interfaces.push_back(state_info);
     state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag);
   }
@@ -905,13 +906,18 @@ void set_custom_interface_values(const InterfaceInfo & itr, joint_limits::JointL
 /**
  * @brief Retrieve the limits from ros2_control command interface tags and override URDF limits if
  * restrictive
- * @param interfaces The interfaces to retrieve the limits from.
+ * @param joint The joint component info containing interfaces and joint-level enable_limits.
  * @param limits The joint limits to be set.
  */
-void update_interface_limits(
-  const std::vector<InterfaceInfo> & interfaces, joint_limits::JointLimits & limits)
+void update_interface_limits(const ComponentInfo & joint, joint_limits::JointLimits & limits)
 {
-  for (auto & itr : interfaces)
+  // If limits are disabled at the joint level, disable all limit flags
+  if (!joint.enable_limits)
+  {
+    limits.disable_all_limits();
+    return;
+  }
+  for (auto & itr : joint.command_interfaces)
   {
     if (itr.name == hardware_interface::HW_IF_POSITION)
     {
@@ -1084,7 +1090,7 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
       joint_limits::JointLimits limits;
       getJointLimits(urdf_joint, limits);
       // Take the most restricted one. Also valid for continuous-joint type only
-      detail::update_interface_limits(joint.command_interfaces, limits);
+      detail::update_interface_limits(joint, limits);
       hw_info.limits[joint.name] = limits;
       joint_limits::SoftJointLimits soft_limits;
       if (getSoftJointLimits(urdf_joint, soft_limits))

hardware_interface/src/resource_manager.cpp

@@ -761,10 +761,32 @@ class ResourceStorage
 
   void import_joint_limiters(const std::vector<HardwareInfo> & hardware_infos)
   {
+    const auto are_joint_limits_enabled =
+      [&](
+        const std::vector<hardware_interface::ComponentInfo> & comp_info,
+        const std::string & joint_name) -> bool
+    {
+      for (const auto & joint_component_info : comp_info)
+      {
+        if (joint_component_info.name == joint_name && !joint_component_info.enable_limits)
+        {
+          return false;
+        }
+      }
+      return true;
+    };
     for (const auto & hw_info : hardware_infos)
     {
       for (const auto & [joint_name, limits] : hw_info.limits)
       {
+        if (!are_joint_limits_enabled(hw_info.joints, joint_name))
+        {
+          RCLCPP_INFO(
+            get_logger(), "Joint limits are disabled for joint '%s' in hardware '%s'",
+            joint_name.c_str(), hw_info.name.c_str());
+          continue;
+        }
+
         std::vector<joint_limits::SoftJointLimits> soft_limits;
         hard_joint_limits_.insert({joint_name, limits});
         const std::vector<joint_limits::JointLimits> hard_limits{limits};

hardware_interface/test/test_component_parser.cpp

@@ -1216,8 +1216,12 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in
   EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits);
   EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits);
   EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits);
-  EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5));
-  EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5));
+  EXPECT_THAT(
+    hardware_info.limits.at("joint2").max_velocity,
+    DoubleNear(std::numeric_limits<double>::max(), 1e-5));
+  EXPECT_THAT(
+    hardware_info.limits.at("joint2").max_effort,
+    DoubleNear(std::numeric_limits<double>::max(), 1e-5));
   EXPECT_THAT(hardware_info.soft_limits.at("joint2").max_position, DoubleNear(0.5, 1e-5));
   EXPECT_THAT(hardware_info.soft_limits.at("joint2").min_position, DoubleNear(-1.5, 1e-5));
   EXPECT_THAT(hardware_info.soft_limits.at("joint2").k_position, DoubleNear(10.0, 1e-5));
@@ -1339,8 +1343,12 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_unavailable
   EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits);
   EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits);
   EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits);
-  EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5));
-  EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5));
+  EXPECT_THAT(
+    hardware_info.limits.at("joint2").max_velocity,
+    DoubleNear(std::numeric_limits<double>::max(), 1e-5));
+  EXPECT_THAT(
+    hardware_info.limits.at("joint2").max_effort,
+    DoubleNear(std::numeric_limits<double>::max(), 1e-5));
 
   EXPECT_TRUE(hardware_info.limits.at("joint3").has_position_limits);
   EXPECT_THAT(hardware_info.limits.at("joint3").min_position, DoubleNear(-M_PI, 1e-5));
@@ -1925,6 +1933,152 @@ TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardw
   EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum");
 }
 
+TEST_F(TestComponentParser, successfully_parse_urdf_with_disabled_limits)
+{
+  const auto urdf_to_test =
+    std::string(ros2_control_test_assets::urdf_head) +
+    std::string(ros2_control_test_assets::hardware_resources_with_disabled_limits) +
+    std::string(ros2_control_test_assets::urdf_tail);
+
+  std::vector<hardware_interface::HardwareInfo> hw_info;
+  ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test));
+  ASSERT_THAT(hw_info, SizeIs(3));
+
+  // First hardware: TestActuatorHardware with joint1
+  // joint1 has <limits enable="false"/> at the joint level, so all interfaces should have
+  // enable_limits=false
+  EXPECT_EQ(hw_info[0].name, "TestActuatorHardware");
+  EXPECT_EQ(hw_info[0].type, "actuator");
+  ASSERT_THAT(hw_info[0].joints, SizeIs(1));
+  EXPECT_EQ(hw_info[0].joints[0].name, "joint1");
+
+  // All command interfaces on joint1 should have enable_limits=false
+  ASSERT_THAT(hw_info[0].joints[0].command_interfaces, SizeIs(4));
+  EXPECT_EQ(hw_info[0].joints[0].command_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_FALSE(hw_info[0].joints[0].command_interfaces[0].enable_limits);
+  EXPECT_EQ(hw_info[0].joints[0].command_interfaces[1].name, HW_IF_VELOCITY);
+  EXPECT_FALSE(hw_info[0].joints[0].command_interfaces[1].enable_limits);
+  EXPECT_EQ(hw_info[0].joints[0].command_interfaces[2].name, HW_IF_EFFORT);
+  EXPECT_FALSE(hw_info[0].joints[0].command_interfaces[2].enable_limits);
+  EXPECT_EQ(hw_info[0].joints[0].command_interfaces[3].name, "max_velocity");
+  EXPECT_FALSE(hw_info[0].joints[0].command_interfaces[3].enable_limits);
+
+  // All state interfaces on joint1 should have enable_limits=false
+  ASSERT_THAT(hw_info[0].joints[0].state_interfaces, SizeIs(2));
+  EXPECT_EQ(hw_info[0].joints[0].state_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_FALSE(hw_info[0].joints[0].state_interfaces[0].enable_limits);
+  EXPECT_EQ(hw_info[0].joints[0].state_interfaces[1].name, HW_IF_VELOCITY);
+  EXPECT_FALSE(hw_info[0].joints[0].state_interfaces[1].enable_limits);
+
+  // Verify that joint1 limits are disabled (has_*_limits should be false)
+  ASSERT_THAT(hw_info[0].limits, SizeIs(1));
+  EXPECT_FALSE(hw_info[0].limits.at("joint1").has_position_limits);
+  EXPECT_FALSE(hw_info[0].limits.at("joint1").has_velocity_limits);
+  EXPECT_FALSE(hw_info[0].limits.at("joint1").has_effort_limits);
+  EXPECT_FALSE(hw_info[0].limits.at("joint1").has_acceleration_limits);
+  EXPECT_FALSE(hw_info[0].limits.at("joint1").has_deceleration_limits);
+  EXPECT_FALSE(hw_info[0].limits.at("joint1").has_jerk_limits);
+
+  // Second hardware: TestSensorHardware - no limits element, so enable_limits should be true
+  // (default)
+  EXPECT_EQ(hw_info[1].name, "TestSensorHardware");
+  EXPECT_EQ(hw_info[1].type, "sensor");
+  ASSERT_THAT(hw_info[1].sensors, SizeIs(1));
+  EXPECT_EQ(hw_info[1].sensors[0].name, "sensor1");
+  ASSERT_THAT(hw_info[1].sensors[0].state_interfaces, SizeIs(1));
+  EXPECT_EQ(hw_info[1].sensors[0].state_interfaces[0].name, HW_IF_VELOCITY);
+  EXPECT_TRUE(hw_info[1].sensors[0].state_interfaces[0].enable_limits);
+
+  // Third hardware: TestSystemHardware with joint2 and joint3
+  EXPECT_EQ(hw_info[2].name, "TestSystemHardware");
+  EXPECT_EQ(hw_info[2].type, "system");
+  ASSERT_THAT(hw_info[2].joints, SizeIs(2));
+
+  // joint2 has <limits enable="false"/> at the joint level, so all interfaces should have
+  // enable_limits=false
+  EXPECT_EQ(hw_info[2].joints[0].name, "joint2");
+  ASSERT_THAT(hw_info[2].joints[0].command_interfaces, SizeIs(2));
+  EXPECT_EQ(hw_info[2].joints[0].command_interfaces[0].name, HW_IF_VELOCITY);
+  EXPECT_FALSE(hw_info[2].joints[0].command_interfaces[0].enable_limits);
+  EXPECT_EQ(hw_info[2].joints[0].command_interfaces[1].name, "max_acceleration");
+  EXPECT_FALSE(hw_info[2].joints[0].command_interfaces[1].enable_limits);
+
+  ASSERT_THAT(hw_info[2].joints[0].state_interfaces, SizeIs(3));
+  EXPECT_EQ(hw_info[2].joints[0].state_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_FALSE(hw_info[2].joints[0].state_interfaces[0].enable_limits);
+  EXPECT_EQ(hw_info[2].joints[0].state_interfaces[1].name, HW_IF_VELOCITY);
+  EXPECT_FALSE(hw_info[2].joints[0].state_interfaces[1].enable_limits);
+  EXPECT_EQ(hw_info[2].joints[0].state_interfaces[2].name, HW_IF_ACCELERATION);
+  EXPECT_FALSE(hw_info[2].joints[0].state_interfaces[2].enable_limits);
+
+  // joint3 has <limits enable="false"/> only on the position command interface
+  // Other interfaces should have enable_limits=true (default)
+  EXPECT_EQ(hw_info[2].joints[1].name, "joint3");
+  ASSERT_THAT(hw_info[2].joints[1].command_interfaces, SizeIs(2));
+  // Position command interface should have enable_limits=false (explicitly set)
+  EXPECT_EQ(hw_info[2].joints[1].command_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_FALSE(hw_info[2].joints[1].command_interfaces[0].enable_limits);
+  // Velocity command interface should have enable_limits=true (default, no limits element)
+  EXPECT_EQ(hw_info[2].joints[1].command_interfaces[1].name, HW_IF_VELOCITY);
+  EXPECT_TRUE(hw_info[2].joints[1].command_interfaces[1].enable_limits);
+
+  // All state interfaces on joint3 should have enable_limits=true (default)
+  ASSERT_THAT(hw_info[2].joints[1].state_interfaces, SizeIs(3));
+  EXPECT_EQ(hw_info[2].joints[1].state_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_TRUE(hw_info[2].joints[1].state_interfaces[0].enable_limits);
+  EXPECT_EQ(hw_info[2].joints[1].state_interfaces[1].name, HW_IF_VELOCITY);
+  EXPECT_TRUE(hw_info[2].joints[1].state_interfaces[1].enable_limits);
+  EXPECT_EQ(hw_info[2].joints[1].state_interfaces[2].name, HW_IF_ACCELERATION);
+  EXPECT_TRUE(hw_info[2].joints[1].state_interfaces[2].enable_limits);
+
+  // Verify limits for TestSystemHardware joints
+  ASSERT_THAT(hw_info[2].limits, SizeIs(2));
+  // joint2 has limits disabled at the joint level, but only has velocity command interface, even
+  // then all the limits should be disabled by default
+  EXPECT_FALSE(hw_info[2].limits.at("joint2").has_position_limits);
+  EXPECT_THAT(
+    hw_info[2].limits.at("joint2").max_position,
+    DoubleNear(std::numeric_limits<double>::max(), 1e-5));
+  EXPECT_THAT(
+    hw_info[2].limits.at("joint2").min_position,
+    DoubleNear(-std::numeric_limits<double>::max(), 1e-5));
+  EXPECT_FALSE(hw_info[2].limits.at("joint2").has_velocity_limits);
+  EXPECT_FALSE(hw_info[2].limits.at("joint2").has_effort_limits);
+  EXPECT_FALSE(hw_info[2].limits.at("joint2").has_acceleration_limits);
+  EXPECT_FALSE(hw_info[2].limits.at("joint2").has_deceleration_limits);
+  EXPECT_FALSE(hw_info[2].limits.at("joint2").has_jerk_limits);
+
+  // joint3 has only position command interface limits disabled
+  // position limits should be disabled (has position cmd_if with enable_limits=false)
+  // velocity limits remain enabled (from URDF, velocity cmd_if has enable_limits=true)
+  EXPECT_FALSE(hw_info[2].limits.at("joint3").has_position_limits);
+  EXPECT_TRUE(hw_info[2].limits.at("joint3").has_velocity_limits);
+  EXPECT_THAT(hw_info[2].limits.at("joint3").max_velocity, DoubleNear(0.2, 1e-5));
+  EXPECT_TRUE(hw_info[2].limits.at("joint3").has_effort_limits);
+  EXPECT_THAT(hw_info[2].limits.at("joint3").max_effort, DoubleNear(0.1, 1e-5));
+  EXPECT_FALSE(hw_info[2].limits.at("joint3").has_acceleration_limits);
+  EXPECT_FALSE(hw_info[2].limits.at("joint3").has_deceleration_limits);
+  EXPECT_FALSE(hw_info[2].limits.at("joint3").has_jerk_limits);
+
+  // Verify soft_limits - joint2 has a safety_controller in the URDF but since joint2's
+  // limits are disabled, we should still have soft_limits parsed from the URDF
+  ASSERT_THAT(hw_info[2].soft_limits, SizeIs(1));
+  EXPECT_THAT(hw_info[2].soft_limits.at("joint2").max_position, DoubleNear(0.5, 1e-5));
+  EXPECT_THAT(hw_info[2].soft_limits.at("joint2").min_position, DoubleNear(-1.5, 1e-5));
+  EXPECT_THAT(hw_info[2].soft_limits.at("joint2").k_position, DoubleNear(10.0, 1e-5));
+  EXPECT_THAT(hw_info[2].soft_limits.at("joint2").k_velocity, DoubleNear(20.0, 1e-5));
+
+  // GPIO interfaces - no limits element, so enable_limits should be true (default)
+  ASSERT_THAT(hw_info[2].gpios, SizeIs(1));
+  EXPECT_EQ(hw_info[2].gpios[0].name, "configuration");
+  ASSERT_THAT(hw_info[2].gpios[0].command_interfaces, SizeIs(1));
+  EXPECT_EQ(hw_info[2].gpios[0].command_interfaces[0].name, "max_tcp_jerk");
+  EXPECT_TRUE(hw_info[2].gpios[0].command_interfaces[0].enable_limits);
+  ASSERT_THAT(hw_info[2].gpios[0].state_interfaces, SizeIs(1));
+  EXPECT_EQ(hw_info[2].gpios[0].state_interfaces[0].name, "max_tcp_jerk");
+  EXPECT_TRUE(hw_info[2].gpios[0].state_interfaces[0].enable_limits);
+}
+
 TEST_F(TestComponentParser, successfully_parse_valid_sdf)
 {
   std::string sdf_to_test = ros2_control_test_assets::diff_drive_robot_sdf;

joint_limits/include/joint_limits/joint_limits.hpp

@@ -67,6 +67,23 @@ struct JointLimits
   bool has_effort_limits;
   bool angle_wraparound;
 
+  void disable_all_limits()
+  {
+    has_position_limits = false;
+    has_velocity_limits = false;
+    has_acceleration_limits = false;
+    has_deceleration_limits = false;
+    has_jerk_limits = false;
+    has_effort_limits = false;
+    min_position = -std::numeric_limits<double>::max();
+    max_position = std::numeric_limits<double>::max();
+    max_velocity = std::numeric_limits<double>::max();
+    max_acceleration = std::numeric_limits<double>::max();
+    max_deceleration = std::numeric_limits<double>::max();
+    max_jerk = std::numeric_limits<double>::max();
+    max_effort = std::numeric_limits<double>::max();
+  }
+
   std::string to_string() const
   {
     std::stringstream ss_output;

ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp

@@ -695,6 +695,62 @@ const auto hardware_resources =
   </ros2_control>
 )";
 
+const auto hardware_resources_with_disabled_limits =
+  R"(
+  <ros2_control name="TestActuatorHardware" type="actuator">
+    <hardware>
+      <plugin>test_actuator</plugin>
+    </hardware>
+    <joint name="joint1">
+      <limits enable="false"/>
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <command_interface name="max_velocity" />
+    </joint>
+  </ros2_control>
+  <ros2_control name="TestSensorHardware" type="sensor">
+    <hardware>
+      <plugin>test_sensor</plugin>
+      <param name="example_param_write_for_sec">2</param>
+      <param name="example_param_read_for_sec">2</param>
+    </hardware>
+    <sensor name="sensor1">
+      <state_interface name="velocity"/>
+    </sensor>
+  </ros2_control>
+  <ros2_control name="TestSystemHardware" type="system">
+    <hardware>
+      <plugin>test_system</plugin>
+      <param name="example_param_write_for_sec">2</param>
+      <param name="example_param_read_for_sec">2</param>
+    </hardware>
+    <joint name="joint2">
+      <limits enable="false"/>
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="acceleration"/>
+      <command_interface name="max_acceleration" />
+    </joint>
+    <joint name="joint3">
+      <command_interface name="position">
+        <limits enable="false"/>
+      </command_interface>
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="acceleration"/>
+    </joint>
+    <gpio name="configuration">
+      <command_interface name="max_tcp_jerk"/>
+      <state_interface name="max_tcp_jerk"/>
+    </gpio>
+  </ros2_control>
+)";
+
 const auto async_hardware_resources =
   R"(
   <ros2_control name="TestActuatorHardware" type="actuator" is_async="true" thread_priority="30">