Add first draft of parsing mimic info from URDF

hardware_interface/CMakeLists.txt

@@ -14,6 +14,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rcutils
   TinyXML2
   tinyxml2_vendor
+  urdf
 )
 
 find_package(ament_cmake REQUIRED)

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -44,6 +44,15 @@ struct InterfaceInfo
   int size;
 };
 
+/// @brief This structure stores information about a joint that is mimicking another joint
+struct MimicJoint
+{
+  std::size_t joint_index;
+  std::size_t mimicked_joint_index;
+  double multiplier = 1.0;
+  double offset = 0.0;
+};
+
 /**
  * This structure stores information about components defined for a specific hardware
  * in robot's URDF.
@@ -55,6 +64,8 @@ struct ComponentInfo
   /// Type of the component: sensor, joint, or GPIO.
   std::string type;
 
+  ///  If the component is a mimic joint
+  bool is_mimic;
   /**
    * Name of the command interfaces that can be set, e.g. "position", "velocity", etc.
    * Used by joints and GPIOs.
@@ -116,22 +127,26 @@ struct HardwareInfo
   /// (Optional) Key-value pairs for hardware parameters.
   std::unordered_map<std::string, std::string> hardware_parameters;
   /**
-   * Map of joints provided by the hardware where the key is the joint name.
+   * Vector of joints provided by the hardware.
    * Required for Actuator and System Hardware.
    */
   std::vector<ComponentInfo> joints;
   /**
-   * Map of sensors provided by the hardware where the key is the joint or link name.
+   * Vector of mimic joints.
+   */
+  std::vector<MimicJoint> mimic_joints;
+  /**
+   * Vector of sensors provided by the hardware.
    * Required for Sensor and optional for System Hardware.
    */
   std::vector<ComponentInfo> sensors;
   /**
-   * Map of GPIO provided by the hardware where the key is a descriptive name of the GPIO.
+   * Vector of GPIOs provided by the hardware.
    * Optional for any hardware components.
    */
   std::vector<ComponentInfo> gpios;
   /**
-   * Map of transmissions to calculate ration between joints and physical actuators.
+   * Vector of transmissions to calculate ratio between joints and physical actuators.
    * Optional for Actuator and System Hardware.
    */
   std::vector<TransmissionInfo> transmissions;

hardware_interface/package.xml

@@ -15,6 +15,7 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>rcpputils</depend>
   <depend>tinyxml2_vendor</depend>
+  <depend>urdf</depend>
 
   <build_depend>rcutils</build_depend>
   <exec_depend>rcutils</exec_depend>

hardware_interface/src/component_parser.cpp

@@ -21,6 +21,8 @@
 #include <unordered_map>
 #include <vector>
 
+#include "urdf/model.h"
+
 #include "hardware_interface/component_parser.hpp"
 #include "hardware_interface/hardware_info.hpp"
 
@@ -41,6 +43,7 @@ constexpr const auto kStateInterfaceTag = "state_interface";
 constexpr const auto kMinTag = "min";
 constexpr const auto kMaxTag = "max";
 constexpr const auto kInitialValueTag = "initial_value";
+constexpr const auto kMimicAttribute = "mimic";
 constexpr const auto kDataTypeAttribute = "data_type";
 constexpr const auto kSizeAttribute = "size";
 constexpr const auto kNameAttribute = "name";
@@ -98,6 +101,29 @@ std::string get_attribute_value(
   return element_it->Attribute(attribute_name);
 }
 
+/// Gets value of the attribute on an XMLelement.
+/**
+ * If parameter is not found, returns specified default value
+ *
+ * \param[in] element_it XMLElement iterator to search for the attribute
+ * \param[in] attribute_name attribute name to search for and return value
+ * \param[in] default_value When the attribute is not found, this value is returned instead
+ * \return attribute value
+ * \throws std::runtime_error if attribute is not found
+ */
+std::string get_attribute_value_or(
+  const tinyxml2::XMLElement * element_it, const char * attribute_name,
+  const std::string default_value)
+{
+  const tinyxml2::XMLAttribute * attr;
+  attr = element_it->FindAttribute(attribute_name);
+  if (!attr)
+  {
+    return default_value;
+  }
+  return element_it->Attribute(attribute_name);
+}
+
 /// Gets value of the attribute on an XMLelement.
 /**
  * If attribute is not found throws an error.
@@ -307,7 +333,7 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml(
  * \param[in] component_it pointer to the iterator where component
  * info should be found
  * \return ComponentInfo filled with information about component
- * \throws std::runtime_error if a component attribute or tag is not found
+ * \throws std::runtime_error if a component attribute or tag is not found or false configuration
  */
 ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it)
 {
@@ -317,8 +343,20 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it
   component.type = component_it->Name();
   component.name = get_attribute_value(component_it, kNameAttribute, component.type);
 
+  if (!component.type.compare(kJointTag))
+  {
+    std::string mimic_str = get_attribute_value_or(component_it, kMimicAttribute, "false");
+    component.is_mimic = mimic_str.compare("true") == 0;
+  }
+
   // Parse all command interfaces
   const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag);
+  if (component.is_mimic && command_interfaces_it)
+  {
+    throw std::runtime_error(
+      "Component " + std::string(component.name) +
+      " has mimic attribute set to true: Mimic joints cannot have command interfaces.");
+  }
   while (command_interfaces_it)
   {
     component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it));
@@ -613,6 +651,52 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
     ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag);
   }
 
+  // parse full URDF for mimic options
+  urdf::Model model;
+  if (!model.initString(urdf))
+  {
+    throw std::runtime_error("Failed to parse URDF file");
+  }
+  for (auto & hw_info : hardware_info)
+  {
+    for (auto joint : hw_info.joints)
+    {
+      auto urdf_joint = model.getJoint(joint.name);
+      if (!urdf_joint)
+      {
+        throw std::runtime_error("Joint " + joint.name + " not found in URDF");
+      }
+      if (joint.is_mimic)
+      {
+        if (urdf_joint->mimic)
+        {
+          auto find_joint = [&hw_info](const std::string & name)
+          {
+            auto it = std::find_if(
+              hw_info.joints.begin(), hw_info.joints.end(),
+              [&name](const auto & j) { return j.name == name; });
+            if (it == hw_info.joints.end())
+            {
+              throw std::runtime_error("Joint `" + name + "` not found in hw_info.joints");
+            }
+            return std::distance(hw_info.joints.begin(), it);
+          };
+
+          MimicJoint mimic_joint;
+          mimic_joint.joint_index = find_joint(joint.name);
+          mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name);
+          mimic_joint.multiplier = urdf_joint->mimic->multiplier;
+          mimic_joint.offset = urdf_joint->mimic->offset;
+          hw_info.mimic_joints.push_back(mimic_joint);
+        }
+        else
+        {
+          throw std::runtime_error("Joint `" + joint.name + "` has no mimic information in URDF");
+        }
+      }
+    }
+  }
+
   return hardware_info;
 }
 

hardware_interface/test/test_component_parser.cpp

@@ -674,3 +674,76 @@ TEST_F(TestComponentParser, transmission_given_too_many_joints_throws_error)
     ros2_control_test_assets::urdf_tail;
   ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
 }
+
+TEST_F(TestComponentParser, mimic_true_valid_config)
+{
+  const auto urdf_to_test =
+    std::string(ros2_control_test_assets::gripper_urdf_head) +
+    std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) +
+    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(1));
+  ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1));
+  EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 1.0);
+  EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 0.0);
+  EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0);
+  EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1);
+}
+
+TEST_F(TestComponentParser, mimic_with_unknown_joint_throws_error)
+{
+  const auto urdf_to_test =
+    std::string(ros2_control_test_assets::gripper_urdf_head_unknown_joint) +
+    std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) +
+    std::string(ros2_control_test_assets::urdf_tail);
+  ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
+}
+
+TEST_F(TestComponentParser, mimic_true_without_mimic_info_throws_error)
+{
+  const auto urdf_to_test =
+    std::string(ros2_control_test_assets::gripper_urdf_head_no_mimic) +
+    std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) +
+    std::string(ros2_control_test_assets::urdf_tail);
+  ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
+}
+
+TEST_F(TestComponentParser, mimic_true_invalid_config_throws_error)
+{
+  const auto urdf_to_test =
+    std::string(ros2_control_test_assets::gripper_urdf_head) +
+    std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_command_if) +
+    std::string(ros2_control_test_assets::urdf_tail);
+  ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
+}
+
+TEST_F(TestComponentParser, mimic_false_valid_config)
+{
+  const auto urdf_to_test =
+    std::string(ros2_control_test_assets::gripper_urdf_head) +
+    std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_false_command_if) +
+    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(1));
+  ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(0));
+}
+
+TEST_F(TestComponentParser, urdf_two_base_links_throws_error)
+{
+  const auto urdf_to_test =
+    std::string(ros2_control_test_assets::gripper_urdf_head_two_base_links) +
+    std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) +
+    std::string(ros2_control_test_assets::urdf_tail);
+  ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
+}
+
+TEST_F(TestComponentParser, urdf_incomplete_throws_error)
+{
+  const auto urdf_to_test =
+    std::string(ros2_control_test_assets::gripper_urdf_head_incomplete) +
+    std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) +
+    std::string(ros2_control_test_assets::urdf_tail);
+  ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
+}