Add support for capsule geometry type

src/urdf_parser_py/urdf.py

@@ -124,14 +124,26 @@ def __init__(self, filename=None, scale=None):
     xmlr.Attribute('scale', 'vector3', required=False)
 ])
 
+class Capsule(xmlr.Object):
+    def __init__(self, radius=0.0, length=0.0):
+        self.radius = radius
+        self.length = length
+
+
+xmlr.reflect(Capsule, tag='capsule', params=[
+    xmlr.Attribute('radius', float),
+    xmlr.Attribute('length', float)
+])
+
 
 class GeometricType(xmlr.ValueType):
     def __init__(self):
         self.factory = xmlr.FactoryType('geometric', {
             'box': Box,
             'cylinder': Cylinder,
             'sphere': Sphere,
-            'mesh': Mesh
+            'mesh': Mesh,
+            'capsule': Capsule
         })
 
     def from_xml(self, node, path):
@@ -471,7 +483,7 @@ def check_valid(self):
 
 
 class Robot(xmlr.Object):
-    SUPPORTED_VERSIONS = ["1.0"]
+    SUPPORTED_VERSIONS = ["1.0", "1.1"]
 
     def __init__(self, name=None, version="1.0"):
         self.aggregate_init()
@@ -539,6 +551,19 @@ def get_root(self):
         assert root is not None, "No roots detected, invalid URDF."
         return root
 
+    def _check_capsule_version(self):
+        """Check that capsule geometry is not used with version 1.0."""
+        if self.version == "1.0":
+            for link in self.links:
+                for visual in link.visuals:
+                    if visual.geometry is not None and isinstance(visual.geometry, Capsule):
+                        xmlr.on_error("Capsule geometry is not supported in URDF version 1.0. "
+                                      "Please use version 1.1 or later.")
+                for collision in link.collisions:
+                    if collision.geometry is not None and isinstance(collision.geometry, Capsule):
+                        xmlr.on_error("Capsule geometry is not supported in URDF version 1.0. "
+                                      "Please use version 1.1 or later.")
+
     def post_read_xml(self):
         if self.version is None:
             self.version = "1.0"
@@ -556,6 +581,7 @@ def post_read_xml(self):
         if self.version not in self.SUPPORTED_VERSIONS:
             raise ValueError("Invalid version; only %s is supported" % (','.join(self.SUPPORTED_VERSIONS)))
 
+        self._check_capsule_version()
 
 xmlr.reflect(Robot, tag='robot', params=[
     xmlr.Attribute('name', str),

test/test_urdf.py

@@ -253,6 +253,12 @@ def test_version_attribute_invalid_version(self):
 </robot>'''
         self.assertRaises(ValueError, self.parse, xml)
 
+    def test_version_attribute_1_1_valid(self):
+        xml = '''<?xml version="1.0"?>
+<robot name="test" version="1.1">
+</robot>'''
+        self.parse_and_compare(xml)
+
 class LinkOriginTestCase(unittest.TestCase):
     @mock.patch('urdf_parser_py.xml_reflection.on_error',
                 mock.Mock(side_effect=ParseException))
@@ -377,5 +383,222 @@ def test_new_urdf_with_version(self):
         self.assertEqual(testcase.get('version'), '1.0')
 
 
+class TestCapsuleGeometry(unittest.TestCase):
+    @mock.patch('urdf_parser_py.xml_reflection.on_error',
+                mock.Mock(side_effect=ParseException))
+    def parse(self, xml):
+        return urdf.Robot.from_xml_string(xml)
+
+    def parse_and_compare(self, orig):
+        xml = minidom.parseString(orig)
+        robot = urdf.Robot.from_xml_string(orig)
+        rewritten = minidom.parseString(robot.to_xml_string())
+        self.assertTrue(xml_matches(xml, rewritten))
+
+    def test_capsule_not_supported_in_version_1_0(self):
+        """Capsule geometry should fail in URDF version 1.0."""
+        xml = '''<?xml version="1.0"?>
+<robot name="capsule_test" version="1.0">
+  <link name="link1">
+    <visual>
+      <geometry>
+        <capsule radius="0.05" length="0.5"/>
+      </geometry>
+    </visual>
+  </link>
+</robot>'''
+        self.assertRaises(xmlr.core.ParseError, self.parse, xml)
+
+    def test_capsule_visual_geometry(self):
+        xml = '''<?xml version="1.0"?>
+<robot name="capsule_test" version="1.1">
+  <link name="link1">
+    <visual>
+      <geometry>
+        <capsule radius="0.05" length="0.5"/>
+      </geometry>
+    </visual>
+  </link>
+</robot>'''
+        robot = self.parse(xml)
+        self.assertEqual("capsule_test", robot.name)
+        self.assertEqual(1, len(robot.links))
+        link = robot.links[0]
+        self.assertIsNotNone(link.visual)
+        self.assertIsInstance(link.visual.geometry, urdf.Capsule)
+        self.assertEqual(0.05, link.visual.geometry.radius)
+        self.assertEqual(0.5, link.visual.geometry.length)
+
+    def test_capsule_collision_geometry(self):
+        xml = '''<?xml version="1.0"?>
+<robot name="capsule_collision_test" version="1.1">
+  <link name="link1">
+    <collision>
+      <geometry>
+        <capsule radius="0.1" length="1.0"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>'''
+        robot = self.parse(xml)
+        self.assertEqual("capsule_collision_test", robot.name)
+        link = robot.links[0]
+        self.assertIsNotNone(link.collision)
+        self.assertIsInstance(link.collision.geometry, urdf.Capsule)
+        self.assertEqual(0.1, link.collision.geometry.radius)
+        self.assertEqual(1.0, link.collision.geometry.length)
+
+    def test_capsule_visual_and_collision_geometry(self):
+        xml = '''<?xml version="1.0"?>
+<robot name="capsule_both_test" version="1.1">
+  <link name="link1">
+    <visual>
+      <geometry>
+        <capsule radius="0.05" length="0.5"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <capsule radius="0.1" length="1.0"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>'''
+        robot = self.parse(xml)
+        link = robot.links[0]
+
+        # Check visual geometry
+        self.assertIsInstance(link.visual.geometry, urdf.Capsule)
+        self.assertEqual(0.05, link.visual.geometry.radius)
+        self.assertEqual(0.5, link.visual.geometry.length)
+
+        # Check collision geometry
+        self.assertIsInstance(link.collision.geometry, urdf.Capsule)
+        self.assertEqual(0.1, link.collision.geometry.radius)
+        self.assertEqual(1.0, link.collision.geometry.length)
+
+    def test_capsule_zero_values(self):
+        xml = '''<?xml version="1.0"?>
+<robot name="capsule_zero_test" version="1.1">
+  <link name="link1">
+    <visual>
+      <geometry>
+        <capsule radius="0.0" length="0.0"/>
+      </geometry>
+    </visual>
+  </link>
+</robot>'''
+        robot = self.parse(xml)
+        link = robot.links[0]
+        capsule = link.visual.geometry
+        self.assertIsInstance(capsule, urdf.Capsule)
+        self.assertEqual(0.0, capsule.radius)
+        self.assertEqual(0.0, capsule.length)
+
+    def test_capsule_parse_and_compare(self):
+        xml = '''<?xml version="1.0"?>
+<robot name="capsule_roundtrip_test" version="1.1">
+  <link name="link1">
+    <visual>
+      <geometry>
+        <capsule length="0.5" radius="0.05"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <capsule length="1.0" radius="0.1"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>'''
+        self.parse_and_compare(xml)
+
+    def test_capsule_multiple_visuals(self):
+        xml = '''<?xml version="1.0"?>
+<robot name="capsule_multi_visual_test" version="1.1">
+  <link name="link1">
+    <visual>
+      <geometry>
+        <capsule radius="0.05" length="0.5"/>
+      </geometry>
+    </visual>
+    <visual>
+      <geometry>
+        <capsule radius="0.1" length="1.0"/>
+      </geometry>
+    </visual>
+  </link>
+</robot>'''
+        robot = self.parse(xml)
+        link = robot.links[0]
+        self.assertEqual(2, len(link.visuals))
+        self.assertIsInstance(link.visuals[0].geometry, urdf.Capsule)
+        self.assertEqual(0.05, link.visuals[0].geometry.radius)
+        self.assertEqual(0.5, link.visuals[0].geometry.length)
+        self.assertIsInstance(link.visuals[1].geometry, urdf.Capsule)
+        self.assertEqual(0.1, link.visuals[1].geometry.radius)
+        self.assertEqual(1.0, link.visuals[1].geometry.length)
+
+    def test_capsule_multiple_visuals_and_collisions(self):
+        """Test multiple capsule geometries in both visual and collision."""
+        xml = '''<?xml version="1.0"?>
+<robot name="multi_capsule_test" version="1.1">
+  <link name="link1">
+    <visual>
+      <geometry>
+        <capsule radius="0.05" length="0.5"/>
+      </geometry>
+    </visual>
+    <visual>
+      <geometry>
+        <capsule radius="0.1" length="1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <capsule radius="0.15" length="1.5"/>
+      </geometry>
+    </collision>
+    <collision>
+      <geometry>
+        <capsule radius="0.2" length="2.0"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>'''
+        robot = self.parse(xml)
+        link = robot.links[0]
+
+        # Check multiple visual geometries
+        self.assertEqual(2, len(link.visuals))
+        self.assertIsInstance(link.visuals[0].geometry, urdf.Capsule)
+        self.assertEqual(0.05, link.visuals[0].geometry.radius)
+        self.assertEqual(0.5, link.visuals[0].geometry.length)
+        self.assertIsInstance(link.visuals[1].geometry, urdf.Capsule)
+        self.assertEqual(0.1, link.visuals[1].geometry.radius)
+        self.assertEqual(1.0, link.visuals[1].geometry.length)
+
+        # Check multiple collision geometries
+        self.assertEqual(2, len(link.collisions))
+        self.assertIsInstance(link.collisions[0].geometry, urdf.Capsule)
+        self.assertEqual(0.15, link.collisions[0].geometry.radius)
+        self.assertEqual(1.5, link.collisions[0].geometry.length)
+        self.assertIsInstance(link.collisions[1].geometry, urdf.Capsule)
+        self.assertEqual(0.2, link.collisions[1].geometry.radius)
+        self.assertEqual(2.0, link.collisions[1].geometry.length)
+
+    def test_capsule_programmatic_creation(self):
+        """Test creating a Capsule programmatically."""
+        capsule = urdf.Capsule(radius=0.05, length=0.5)
+        self.assertEqual(0.05, capsule.radius)
+        self.assertEqual(0.5, capsule.length)
+
+    def test_capsule_default_values(self):
+        """Test Capsule default values."""
+        capsule = urdf.Capsule()
+        self.assertEqual(0.0, capsule.radius)
+        self.assertEqual(0.0, capsule.length)
+
+
 if __name__ == '__main__':
     unittest.main()