diff --git a/src/urdf_parser_py/urdf.py b/src/urdf_parser_py/urdf.py
index 41fa2b7..d526d53 100644
--- a/src/urdf_parser_py/urdf.py
+++ b/src/urdf_parser_py/urdf.py
@@ -124,6 +124,17 @@ 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):
@@ -131,7 +142,8 @@ def __init__(self):
'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),
diff --git a/test/test_urdf.py b/test/test_urdf.py
index b78432d..859c4a0 100644
--- a/test/test_urdf.py
+++ b/test/test_urdf.py
@@ -253,6 +253,12 @@ def test_version_attribute_invalid_version(self):
'''
self.assertRaises(ValueError, self.parse, xml)
+ def test_version_attribute_1_1_valid(self):
+ xml = '''
+
+'''
+ 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 = '''
+
+
+
+
+
+
+
+
+'''
+ self.assertRaises(xmlr.core.ParseError, self.parse, xml)
+
+ def test_capsule_visual_geometry(self):
+ xml = '''
+
+
+
+
+
+
+
+
+'''
+ 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 = '''
+
+
+
+
+
+
+
+
+'''
+ 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 = '''
+
+
+
+
+
+
+
+
+
+
+
+
+
+'''
+ 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 = '''
+
+
+
+
+
+
+
+
+'''
+ 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 = '''
+
+
+
+
+
+
+
+
+
+
+
+
+
+'''
+ self.parse_and_compare(xml)
+
+ def test_capsule_multiple_visuals(self):
+ xml = '''
+
+
+
+
+
+
+
+
+
+
+
+
+
+'''
+ 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 = '''
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+'''
+ 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()