Add comprehensive test cases for URDF root link changer (#568)

- Add test for fixed joints without axis elements
- Add test for joint/link naming conflicts with mimic joints
- Add test for None element handling during XML parsing
- Add test for complex kinematic tree root link changes
- Ensure robustness against various URDF structures

Author: iory

skrobot/urdf/xml_root_link_changer.py

@@ -318,12 +318,18 @@ def _reverse_joint_transform(self, joint, prev_joint_xyz, prev_joint_rpy, path_t
 
         # Invert axis direction
         axis = joint.find('axis')
-        axis_xyz_str = axis.get('xyz', '0 0 0')
-        inv_axis_xyz = [-float(x) for x in axis_xyz_str.split()]
-        axis.set('xyz', ' '.join(map(str, inv_axis_xyz)))
+        if axis is not None:
+            axis_xyz_str = axis.get('xyz', '0 0 0')
+            inv_axis_xyz = [-float(x) for x in axis_xyz_str.split()]
+            axis.set('xyz', ' '.join(map(str, inv_axis_xyz)))
 
         # Adjust visual and collision origin
-        parent_name = joint.find('parent').get('link')
+        parent_elem = joint.find('parent')
+        if parent_elem is None:
+            return
+        parent_name = parent_elem.get('link')
+        if parent_name not in self.links:
+            return
         parent = self.links[parent_name]
         parent_visual = parent.find('visual')
         if parent_visual is not None:
@@ -395,6 +401,8 @@ def _matrix_to_pose(self, T):
         return xyz.tolist(), rpy.tolist()
 
     def _get_all_children_links(self, parent_link):
+        if parent_link is None:
+            return []
         parent_link_name = parent_link.get('link')
         for link_name, info in self.joint_tree.items():
             if link_name == parent_link_name:
@@ -404,22 +412,35 @@ def _get_all_children_links(self, parent_link):
 
     def _get_all_children_joints(self, parent_joint):
         children_joints = []
-        link_name = parent_joint.find('child').get('link')
+        child_elem = parent_joint.find('child')
+        if child_elem is None:
+            return children_joints
+        link_name = child_elem.get('link')
+        if link_name is None:
+            return children_joints
         for joint_name, joint in self.joints.items():
-            parent_link_name = joint.find('parent').get('link')
+            parent_elem = joint.find('parent')
+            if parent_elem is None:
+                continue
+            parent_link_name = parent_elem.get('link')
             if parent_link_name == link_name:
                 children_joints.append(joint)
         return children_joints
 
     def _get_all_children_joints_of_link(self, link_name):
         children_joints = []
         for joint_name, joint in self.joints.items():
-            parent_link_name = joint.find('parent').get('link')
+            parent_elem = joint.find('parent')
+            if parent_elem is None:
+                continue
+            parent_link_name = parent_elem.get('link')
             if parent_link_name == link_name:
                 children_joints.append(joint)
         return children_joints
 
     def _link_is_included_in_path(self, target_link, path):
+        if target_link is None:
+            return False
         target_link_name = target_link.get('name')
         for elem in path:
             parent_link_name = elem[0]
@@ -429,6 +450,8 @@ def _link_is_included_in_path(self, target_link, path):
         return False
 
     def _joint_is_included_in_path(self, target_joint, path):
+        if target_joint is None:
+            return False
         target_joint_name = target_joint.get('name')
         for elem in path:
             joint_name = elem[2]

tests/skrobot_tests/urdf_tests/test_xml_root_link_changer.py

@@ -108,6 +108,200 @@ def test_find_path_to_link(self):
                     self.assertIsInstance(item, tuple)
                     self.assertEqual(len(item), 3)
 
+    def test_change_root_link_with_fixed_joints(self):
+        """Test root link changing with fixed joints (no axis elements)."""
+        fixed_joint_urdf_content = '''<?xml version="1.0"?>
+<robot name="test_robot">
+  <link name="world"/>
+  <link name="base_link"/>
+  <link name="end_link"/>
+
+  <joint name="world_to_base" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <joint name="base_to_end" type="revolute">
+    <parent link="base_link"/>
+    <child link="end_link"/>
+    <origin xyz="1 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.57" upper="1.57" effort="10" velocity="1"/>
+  </joint>
+</robot>'''
+
+        test_urdf_path = os.path.join(self.temp_dir, "fixed_joint_test.urdf")
+        with open(test_urdf_path, 'w') as f:
+            f.write(fixed_joint_urdf_content)
+
+        changer = URDFXMLRootLinkChanger(test_urdf_path)
+
+        output_path = os.path.join(self.temp_dir, "fixed_joint_changed.urdf")
+        changer.change_root_link("base_link", output_path)
+
+        self.assertTrue(os.path.exists(output_path))
+
+        new_changer = URDFXMLRootLinkChanger(output_path)
+        actual_new_root = new_changer.get_current_root_link()
+        self.assertEqual(actual_new_root, "base_link")
+
+    def test_change_root_link_with_mimic_joints(self):
+        """Test root link changing with mimic joints and naming conflicts."""
+        mimic_joint_urdf_content = '''<?xml version="1.0"?>
+<robot name="test_robot">
+  <link name="world"/>
+  <link name="base_link"/>
+  <link name="joint1"/>
+  <link name="joint2"/>
+
+  <joint name="world_to_base_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <joint name="joint1" type="revolute">
+    <parent link="base_link"/>
+    <child link="joint1"/>
+    <origin xyz="1 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.57" upper="1.57" effort="10" velocity="1"/>
+  </joint>
+
+  <joint name="joint2_joint" type="revolute">
+    <parent link="joint1"/>
+    <child link="joint2"/>
+    <origin xyz="1 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.57" upper="1.57" effort="10" velocity="1"/>
+    <mimic joint="joint1" multiplier="2.0"/>
+  </joint>
+</robot>'''
+
+        test_urdf_path = os.path.join(self.temp_dir, "mimic_joint_test.urdf")
+        with open(test_urdf_path, 'w') as f:
+            f.write(mimic_joint_urdf_content)
+
+        changer = URDFXMLRootLinkChanger(test_urdf_path)
+
+        output_path = os.path.join(self.temp_dir, "mimic_joint_changed.urdf")
+        changer.change_root_link("base_link", output_path)
+
+        self.assertTrue(os.path.exists(output_path))
+
+        new_changer = URDFXMLRootLinkChanger(output_path)
+        actual_new_root = new_changer.get_current_root_link()
+        self.assertEqual(actual_new_root, "base_link")
+
+    def test_none_element_handling(self):
+        """Test handling of None elements during XML parsing."""
+        malformed_urdf_content = '''<?xml version="1.0"?>
+<robot name="test_robot">
+  <link name="world"/>
+  <link name="base_link"/>
+
+  <joint name="world_to_base" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+</robot>'''
+
+        test_urdf_path = os.path.join(self.temp_dir, "malformed_test.urdf")
+        with open(test_urdf_path, 'w') as f:
+            f.write(malformed_urdf_content)
+
+        changer = URDFXMLRootLinkChanger(test_urdf_path)
+
+        output_path = os.path.join(self.temp_dir, "malformed_changed.urdf")
+
+        try:
+            changer.change_root_link("base_link", output_path)
+            self.assertTrue(os.path.exists(output_path))
+        except AttributeError as e:
+            if "'NoneType' object has no attribute 'get'" in str(e):
+                self.fail("NoneType error not properly handled: {}".format(e))
+            else:
+                raise
+
+    def test_complex_kinematic_tree_root_change(self):
+        """Test root link change in complex kinematic trees with multiple branches."""
+        complex_urdf_content = '''<?xml version="1.0"?>
+<robot name="complex_robot">
+  <link name="world"/>
+  <link name="base_link"/>
+  <link name="arm1_link"/>
+  <link name="arm2_link"/>
+  <link name="gripper_base"/>
+  <link name="gripper_finger1"/>
+  <link name="gripper_finger2"/>
+
+  <joint name="world_to_base_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <joint name="base_to_arm1_joint" type="revolute">
+    <parent link="base_link"/>
+    <child link="arm1_link"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.57" upper="1.57" effort="10" velocity="1"/>
+  </joint>
+
+  <joint name="arm1_to_arm2_joint" type="revolute">
+    <parent link="arm1_link"/>
+    <child link="arm2_link"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-1.57" upper="1.57" effort="10" velocity="1"/>
+  </joint>
+
+  <joint name="arm2_to_gripper_joint" type="fixed">
+    <parent link="arm2_link"/>
+    <child link="gripper_base"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <joint name="gripper_finger1_joint" type="revolute">
+    <parent link="gripper_base"/>
+    <child link="gripper_finger1"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="0" upper="0.5" effort="5" velocity="0.5"/>
+  </joint>
+
+  <joint name="gripper_finger2_joint" type="revolute">
+    <parent link="gripper_base"/>
+    <child link="gripper_finger2"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="0" upper="0.5" effort="5" velocity="0.5"/>
+    <mimic joint="gripper_finger1_joint" multiplier="-1.0"/>
+  </joint>
+</robot>'''
+
+        test_urdf_path = os.path.join(self.temp_dir, "complex_test.urdf")
+        with open(test_urdf_path, 'w') as f:
+            f.write(complex_urdf_content)
+
+        changer = URDFXMLRootLinkChanger(test_urdf_path)
+
+        output_path = os.path.join(self.temp_dir, "complex_changed.urdf")
+        changer.change_root_link("gripper_base", output_path)
+
+        self.assertTrue(os.path.exists(output_path))
+
+        new_changer = URDFXMLRootLinkChanger(output_path)
+        actual_new_root = new_changer.get_current_root_link()
+        self.assertEqual(actual_new_root, "gripper_base")
+
+        new_links = new_changer.list_links()
+        self.assertIn("gripper_finger1", new_links)
+        self.assertIn("gripper_finger2", new_links)
+
 
 if __name__ == '__main__':
     unittest.main()