From 64f292cfb9bfa77367e02f3ef0f472573f89dbe5 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 30 Apr 2019 18:42:28 +0900 Subject: [PATCH 1/7] add test to build robot model from python urdf classes --- test/test_urdf.py | 72 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 72 insertions(+) diff --git a/test/test_urdf.py b/test/test_urdf.py index 55344b4..bf27799 100644 --- a/test/test_urdf.py +++ b/test/test_urdf.py @@ -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 = ''' @@ -46,6 +52,18 @@ def test_new_transmission(self): ''' self.parse_and_compare(xml) + robot = urdf.Robot(name = 'test') + 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 = ''' @@ -65,6 +83,22 @@ def test_new_transmission_multiple_joints(self): ''' self.parse_and_compare(xml) + robot = urdf.Robot(name = 'test') + 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 = ''' @@ -81,6 +115,20 @@ def test_new_transmission_multiple_actuators(self): ''' self.parse_and_compare(xml) + robot = urdf.Robot(name = 'test') + 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 = ''' @@ -113,6 +161,11 @@ def test_old_transmission(self): ''' self.parse_and_compare(xml) + robot = urdf.Robot(name = 'test') + 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 = ''' @@ -127,6 +180,20 @@ def test_link_material_missing_color_and_texture(self): ''' self.parse_and_compare(xml) + robot = urdf.Robot(name = 'test') + 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') + 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 = ''' @@ -136,6 +203,11 @@ def test_robot_material(self): ''' self.parse_and_compare(xml) + robot = urdf.Robot(name = 'test') + 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 = ''' From 34f130c11c9df61c1eb5b5f09898043b767ea375 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 30 Apr 2019 20:53:18 +0900 Subject: [PATCH 2/7] add more tests on python urdf build --- test/test_urdf.py | 51 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) diff --git a/test/test_urdf.py b/test/test_urdf.py index bf27799..8b3333f 100644 --- a/test/test_urdf.py +++ b/test/test_urdf.py @@ -235,6 +235,15 @@ def test_link_multiple_visual(self): ''' self.parse_and_compare(xml) + robot = urdf.Robot(name = 'test') + 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_link_multiple_collision(self): xml = ''' @@ -253,6 +262,13 @@ def test_link_multiple_collision(self): ''' self.parse_and_compare(xml) + robot = urdf.Robot(name = 'test') + link = urdf.Link(name = 'link') + link.collision = urdf.Visual(geometry = urdf.Cylinder(length = 1, radius = 1)) + link.collision = urdf.Visual(geometry = urdf.Cylinder(length = 4, radius = 0.5)) + robot.add_link(link) + self.xml_and_compare(robot, xml) + class LinkOriginTestCase(unittest.TestCase): @mock.patch('urdf_parser_py.xml_reflection.on_error', @@ -346,6 +362,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') + 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') + 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() From f0bc4700756a64292763894bb74ea6da0b00285d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 30 Apr 2019 20:54:31 +0900 Subject: [PATCH 3/7] update backward compatibility Link(visual=...) does not set on since 0.4.0, breaks backward compatibility --- src/urdf_parser_py/urdf.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/urdf_parser_py/urdf.py b/src/urdf_parser_py/urdf.py index 2b08437..b4519ad 100644 --- a/src/urdf_parser_py/urdf.py +++ b/src/urdf_parser_py/urdf.py @@ -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): @@ -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.""" @@ -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) From 977fb1e4aab29f47135c81a8fdc2dfbe47c248be Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 9 Mar 2020 16:15:42 -0700 Subject: [PATCH 4/7] Robot accepts version kwarg Signed-off-by: Shane Loretz --- src/urdf_parser_py/urdf.py | 4 +++- test/test_urdf.py | 24 ++++++++++++------------ 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/urdf_parser_py/urdf.py b/src/urdf_parser_py/urdf.py index ab0d946..817f2c8 100644 --- a/src/urdf_parser_py/urdf.py +++ b/src/urdf_parser_py/urdf.py @@ -481,10 +481,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 = [] diff --git a/test/test_urdf.py b/test/test_urdf.py index b3bff6c..069ee73 100644 --- a/test/test_urdf.py +++ b/test/test_urdf.py @@ -52,7 +52,7 @@ def test_new_transmission(self): ''' self.parse_and_compare(xml) - robot = urdf.Robot(name = 'test') + 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') @@ -83,7 +83,7 @@ def test_new_transmission_multiple_joints(self): ''' self.parse_and_compare(xml) - robot = urdf.Robot(name = 'test') + 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') @@ -115,7 +115,7 @@ def test_new_transmission_multiple_actuators(self): ''' self.parse_and_compare(xml) - robot = urdf.Robot(name = 'test') + 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') @@ -161,7 +161,7 @@ def test_old_transmission(self): ''' self.parse_and_compare(xml) - robot = urdf.Robot(name = 'test') + 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) @@ -180,14 +180,14 @@ def test_link_material_missing_color_and_texture(self): ''' self.parse_and_compare(xml) - robot = urdf.Robot(name = 'test') + 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') + 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')) @@ -203,7 +203,7 @@ def test_robot_material(self): ''' self.parse_and_compare(xml) - robot = urdf.Robot(name = 'test') + 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) @@ -235,7 +235,7 @@ def test_link_multiple_visual(self): ''' self.parse_and_compare(xml) - robot = urdf.Robot(name = 'test') + 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')) @@ -276,7 +276,7 @@ def test_link_multiple_collision(self): ''' self.parse_and_compare(xml) - robot = urdf.Robot(name = 'test') + robot = urdf.Robot(name='test', version='1.0') link = urdf.Link(name = 'link') link.collision = urdf.Visual(geometry = urdf.Cylinder(length = 1, radius = 1)) link.collision = urdf.Visual(geometry = urdf.Cylinder(length = 4, radius = 0.5)) @@ -396,7 +396,7 @@ def test_robot_link_defaults_xyz_set(self): class LinkMultiVisualsAndCollisionsTest(unittest.TestCase): xml = ''' - + @@ -449,7 +449,7 @@ def test_multi_collision_access(self): 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') + 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')) @@ -467,7 +467,7 @@ def test_xml_and_urdfdom_robot_compatible_with_kinetic(self): self.assertTrue(xml_matches(robot_xml, orig_xml)) def test_xml_and_urdfdom_robot_only_supported_since_melodic(self): - robot = urdf.Robot(name = 'test') + 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'))) From c4eda83c9834dc4bfc4e84605e7c5a238aae3c9a Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 9 Mar 2020 16:21:59 -0700 Subject: [PATCH 5/7] kwarg ' = ' -> '=' Signed-off-by: Shane Loretz --- test/test_urdf.py | 86 +++++++++++++++++++++++------------------------ 1 file changed, 43 insertions(+), 43 deletions(-) diff --git a/test/test_urdf.py b/test/test_urdf.py index 069ee73..c3be453 100644 --- a/test/test_urdf.py +++ b/test/test_urdf.py @@ -53,12 +53,12 @@ def test_new_transmission(self): self.parse_and_compare(xml) robot = urdf.Robot(name='test', version='1.0') - trans = urdf.Transmission(name = 'simple_trans') + trans = urdf.Transmission(name='simple_trans') trans.type = 'transmission_interface/SimpleTransmission' - joint = urdf.TransmissionJoint(name = 'foo_joint') + joint = urdf.TransmissionJoint(name='foo_joint') joint.add_aggregate('hardwareInterface', 'EffortJointInterface') trans.add_aggregate('joint', joint) - actuator = urdf.Actuator(name = 'foo_motor') + actuator = urdf.Actuator(name='foo_motor') actuator.mechanicalReduction = 50.0 trans.add_aggregate('actuator', actuator) robot.add_aggregate('transmission', trans) @@ -84,16 +84,16 @@ def test_new_transmission_multiple_joints(self): self.parse_and_compare(xml) robot = urdf.Robot(name='test', version='1.0') - trans = urdf.Transmission(name = 'simple_trans') + trans = urdf.Transmission(name='simple_trans') trans.type = 'transmission_interface/SimpleTransmission' - joint = urdf.TransmissionJoint(name = 'foo_joint') + joint = urdf.TransmissionJoint(name='foo_joint') joint.add_aggregate('hardwareInterface', 'EffortJointInterface') trans.add_aggregate('joint', joint) - joint = urdf.TransmissionJoint(name = 'bar_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 = urdf.Actuator(name='foo_motor') actuator.mechanicalReduction = 50.0 trans.add_aggregate('actuator', actuator) robot.add_aggregate('transmission', trans) @@ -116,15 +116,15 @@ def test_new_transmission_multiple_actuators(self): self.parse_and_compare(xml) robot = urdf.Robot(name='test', version='1.0') - trans = urdf.Transmission(name = 'simple_trans') + trans = urdf.Transmission(name='simple_trans') trans.type = 'transmission_interface/SimpleTransmission' - joint = urdf.TransmissionJoint(name = 'foo_joint') + joint = urdf.TransmissionJoint(name='foo_joint') joint.add_aggregate('hardwareInterface', 'EffortJointInterface') trans.add_aggregate('joint', joint) - actuator = urdf.Actuator(name = 'foo_motor') + actuator = urdf.Actuator(name='foo_motor') actuator.mechanicalReduction = 50.0 trans.add_aggregate('actuator', actuator) - actuator = urdf.Actuator(name = 'bar_motor') + actuator = urdf.Actuator(name='bar_motor') trans.add_aggregate('actuator', actuator) robot.add_aggregate('transmission', trans) self.xml_and_compare(robot, xml) @@ -162,7 +162,7 @@ def test_old_transmission(self): 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) + 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) @@ -181,16 +181,16 @@ def test_link_material_missing_color_and_texture(self): 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'))) + 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')) + 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) @@ -204,7 +204,7 @@ def test_robot_material(self): 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])) + 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) @@ -236,11 +236,11 @@ def test_link_multiple_visual(self): 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')) + 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) @@ -277,9 +277,9 @@ def test_link_multiple_collision(self): self.parse_and_compare(xml) robot = urdf.Robot(name='test', version='1.0') - link = urdf.Link(name = 'link') - link.collision = urdf.Visual(geometry = urdf.Cylinder(length = 1, radius = 1)) - link.collision = urdf.Visual(geometry = urdf.Cylinder(length = 4, radius = 0.5)) + link = urdf.Link(name='link') + link.collision = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1)) + link.collision = urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5)) robot.add_link(link) self.xml_and_compare(robot, xml) @@ -450,15 +450,15 @@ def test_multi_collision_access(self): 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)) + 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') + link = urdf.Link(name='link2') robot.add_link(link) # robot_xml_string = robot.to_xml_string() @@ -468,15 +468,15 @@ def test_xml_and_urdfdom_robot_compatible_with_kinetic(self): 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))) + 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') + link = urdf.Link(name='link2') robot.add_link(link) # robot_xml_string = robot.to_xml_string() From a99c7632ef3a0d15d0b21651d1e7df805a58ceb4 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 9 Mar 2020 16:23:59 -0700 Subject: [PATCH 6/7] collision = Collision( not Visual( Signed-off-by: Shane Loretz --- test/test_urdf.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/test_urdf.py b/test/test_urdf.py index c3be453..4a9bc46 100644 --- a/test/test_urdf.py +++ b/test/test_urdf.py @@ -278,8 +278,8 @@ def test_link_multiple_collision(self): robot = urdf.Robot(name='test', version='1.0') link = urdf.Link(name='link') - link.collision = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1)) - link.collision = urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5)) + 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) From 1b65603f31c51c2f4bf8ea59f0efbeede79d0265 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 9 Mar 2020 16:24:36 -0700 Subject: [PATCH 7/7] Undo whitespace change Signed-off-by: Shane Loretz --- test/test_urdf.py | 1 - 1 file changed, 1 deletion(-) diff --git a/test/test_urdf.py b/test/test_urdf.py index 4a9bc46..c554343 100644 --- a/test/test_urdf.py +++ b/test/test_urdf.py @@ -355,7 +355,6 @@ def test_version_attribute_invalid_version(self): ''' self.assertRaises(ValueError, self.parse, xml) - class LinkOriginTestCase(unittest.TestCase): @mock.patch('urdf_parser_py.xml_reflection.on_error', mock.Mock(side_effect=ParseException))