Skip to content

Commit

Permalink
update backward compatibility on visual and collisions (#47)
Browse files Browse the repository at this point in the history 
* add test to build robot model from python urdf classes

* add more tests on python urdf build

* update backward compatibility

Link(visual=...) does not set <visual> on <link> since 0.4.0, breaks backward compatibility

* Robot accepts version kwarg

Signed-off-by: Shane Loretz <[email protected]>
Co-authored-by: Shane Loretz <[email protected]>
  • Loading branch information
@k-okada @sloretz @AdrianZw
2 people authored and AdrianZw committed Jul 11, 2023
1 parent dc5ebf9 commit a67c4a7
Show file tree
Hide file tree
Showing 2 changed files with 149 additions and 2 deletions.
12 changes: 11 additions & 1 deletion src/urdf_parser_py/urdf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -355,8 +355,12 @@ def __init__(self, name=None, visual=None, inertial=None, collision=None,
self.aggregate_init()
self.name = name
self.visuals = []
if visual:
self.visual = visual
self.inertial = inertial
self.collisions = []
if collision:
self.collision = collision
self.origin = origin

def __get_visual(self):
Expand All @@ -370,6 +374,8 @@ def __set_visual(self, visual):
self.visuals[0] = visual
else:
self.visuals.append(visual)
if visual:
self.add_aggregate('visual', visual)

def __get_collision(self):
"""Return the first collision or None."""
Expand All @@ -382,6 +388,8 @@ def __set_collision(self, collision):
self.collisions[0] = collision
else:
self.collisions.append(collision)
if collision:
self.add_aggregate('collision', collision)

# Properties for backwards compatibility
visual = property(__get_visual, __set_visual)
Expand Down Expand Up @@ -471,10 +479,12 @@ def check_valid(self):


class Robot(xmlr.Object):
def __init__(self, name=None):
def __init__(self, name=None, version=None):
self.aggregate_init()

self.name = name
if version is not None:
self.version = version
self.joints = []
self.links = []
self.materials = []
Expand Down
139 changes: 138 additions & 1 deletion test/test_urdf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,12 @@ def parse_and_compare(self, orig):
rewritten = minidom.parseString(robot.to_xml_string())
self.assertTrue(xml_matches(xml, rewritten))

def xml_and_compare(self, robot, xml):
robot_xml_string = robot.to_xml_string()
robot_xml = minidom.parseString(robot_xml_string)
orig_xml = minidom.parseString(xml)
self.assertTrue(xml_matches(robot_xml, orig_xml))

def test_new_transmission(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
Expand All @@ -46,6 +52,18 @@ def test_new_transmission(self):
</robot>'''
self.parse_and_compare(xml)

robot = urdf.Robot(name='test', version='1.0')
trans = urdf.Transmission(name='simple_trans')
trans.type = 'transmission_interface/SimpleTransmission'
joint = urdf.TransmissionJoint(name='foo_joint')
joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
trans.add_aggregate('joint', joint)
actuator = urdf.Actuator(name='foo_motor')
actuator.mechanicalReduction = 50.0
trans.add_aggregate('actuator', actuator)
robot.add_aggregate('transmission', trans)
self.xml_and_compare(robot, xml)

def test_new_transmission_multiple_joints(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
Expand All @@ -65,6 +83,22 @@ def test_new_transmission_multiple_joints(self):
</robot>'''
self.parse_and_compare(xml)

robot = urdf.Robot(name='test', version='1.0')
trans = urdf.Transmission(name='simple_trans')
trans.type = 'transmission_interface/SimpleTransmission'
joint = urdf.TransmissionJoint(name='foo_joint')
joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
trans.add_aggregate('joint', joint)
joint = urdf.TransmissionJoint(name='bar_joint')
joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
trans.add_aggregate('joint', joint)
actuator = urdf.Actuator(name='foo_motor')
actuator.mechanicalReduction = 50.0
trans.add_aggregate('actuator', actuator)
robot.add_aggregate('transmission', trans)
self.xml_and_compare(robot, xml)

def test_new_transmission_multiple_actuators(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
Expand All @@ -81,6 +115,20 @@ def test_new_transmission_multiple_actuators(self):
</robot>'''
self.parse_and_compare(xml)

robot = urdf.Robot(name='test', version='1.0')
trans = urdf.Transmission(name='simple_trans')
trans.type = 'transmission_interface/SimpleTransmission'
joint = urdf.TransmissionJoint(name='foo_joint')
joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
trans.add_aggregate('joint', joint)
actuator = urdf.Actuator(name='foo_motor')
actuator.mechanicalReduction = 50.0
trans.add_aggregate('actuator', actuator)
actuator = urdf.Actuator(name='bar_motor')
trans.add_aggregate('actuator', actuator)
robot.add_aggregate('transmission', trans)
self.xml_and_compare(robot, xml)

def test_new_transmission_missing_joint(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
Expand Down Expand Up @@ -113,6 +161,11 @@ def test_old_transmission(self):
</robot>'''
self.parse_and_compare(xml)

robot = urdf.Robot(name='test', version='1.0')
trans = urdf.PR2Transmission(name='PR2_trans', joint='foo_joint', actuator='foo_motor', type='SimpleTransmission', mechanicalReduction=1.0)
robot.add_aggregate('transmission', trans)
self.xml_and_compare(robot, xml)

def test_link_material_missing_color_and_texture(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
Expand All @@ -127,6 +180,20 @@ def test_link_material_missing_color_and_texture(self):
</robot>'''
self.parse_and_compare(xml)

robot = urdf.Robot(name='test', version='1.0')
link = urdf.Link(name='link',
visual=urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
material=urdf.Material(name='mat')))
robot.add_link(link)
self.xml_and_compare(robot, xml)

robot = urdf.Robot(name='test', version='1.0')
link = urdf.Link(name='link')
link.visual = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
material=urdf.Material(name='mat'))
robot.add_link(link)
self.xml_and_compare(robot, xml)

def test_robot_material(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
Expand All @@ -136,6 +203,11 @@ def test_robot_material(self):
</robot>'''
self.parse_and_compare(xml)

robot = urdf.Robot(name='test', version='1.0')
material = urdf.Material(name='mat', color=urdf.Color([0.0, 0.0, 0.0, 1.0]))
robot.add_aggregate('material', material)
self.xml_and_compare(robot, xml)

def test_robot_material_missing_color_and_texture(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
Expand Down Expand Up @@ -163,6 +235,29 @@ def test_link_multiple_visual(self):
</robot>'''
self.parse_and_compare(xml)

robot = urdf.Robot(name='test', version='1.0')
link = urdf.Link(name='link')
link.visual = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
material=urdf.Material(name='mat'))
link.visual = urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5),
material=urdf.Material(name='mat2'))
robot.add_link(link)
self.xml_and_compare(robot, xml)

def test_visual_with_name(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
<link name="link">
<visual name="alice">
<geometry>
<cylinder length="1" radius="1"/>
</geometry>
<material name="mat"/>
</visual>
</link>
</robot>'''
self.parse_and_compare(xml)

def test_link_multiple_collision(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
Expand All @@ -181,6 +276,13 @@ def test_link_multiple_collision(self):
</robot>'''
self.parse_and_compare(xml)

robot = urdf.Robot(name='test', version='1.0')
link = urdf.Link(name='link')
link.collision = urdf.Collision(geometry=urdf.Cylinder(length=1, radius=1))
link.collision = urdf.Collision(geometry=urdf.Cylinder(length=4, radius=0.5))
robot.add_link(link)
self.xml_and_compare(robot, xml)

def test_version_attribute_not_enough_dots(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1">
Expand Down Expand Up @@ -293,7 +395,7 @@ def test_robot_link_defaults_xyz_set(self):
class LinkMultiVisualsAndCollisionsTest(unittest.TestCase):

xml = '''<?xml version="1.0"?>
<robot name="test">
<robot name="test" version="1.0">
<link name="link">
<visual>
<geometry>
Expand Down Expand Up @@ -345,6 +447,41 @@ def test_multi_collision_access(self):
robot.links[0].collision = dummyObject
self.assertEqual(id(dummyObject), id(robot.links[0].collisions[0]))

def test_xml_and_urdfdom_robot_compatible_with_kinetic(self):
robot = urdf.Robot(name='test', version='1.0')
link = urdf.Link(name='link')
link.visual = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
material=urdf.Material(name='mat'))
link.visual = urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5),
material=urdf.Material(name='mat2'))
link.collision = urdf.Collision(geometry=urdf.Cylinder(length=1, radius=1))
link.collision = urdf.Collision(geometry=urdf.Cylinder(length=4, radius=0.5))
robot.add_link(link)
link = urdf.Link(name='link2')
robot.add_link(link)
#
robot_xml_string = robot.to_xml_string()
robot_xml = minidom.parseString(robot_xml_string)
orig_xml = minidom.parseString(self.xml)
self.assertTrue(xml_matches(robot_xml, orig_xml))

def test_xml_and_urdfdom_robot_only_supported_since_melodic(self):
robot = urdf.Robot(name='test', version='1.0')
link = urdf.Link(name='link')
link.add_aggregate('visual', urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
material=urdf.Material(name='mat')))
link.add_aggregate('visual', urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5),
material=urdf.Material(name='mat2')))
link.add_aggregate('collision', urdf.Collision(geometry=urdf.Cylinder(length=1, radius=1)))
link.add_aggregate('collision', urdf.Collision(geometry=urdf.Cylinder(length=4, radius=0.5)))
robot.add_link(link)
link = urdf.Link(name='link2')
robot.add_link(link)
#
robot_xml_string = robot.to_xml_string()
robot_xml = minidom.parseString(robot_xml_string)
orig_xml = minidom.parseString(self.xml)
self.assertTrue(xml_matches(robot_xml, orig_xml))

if __name__ == '__main__':
unittest.main()

0 comments on commit a67c4a7

Please sign in to comment.