8 sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__),
'.')))
9 sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__),
12 from xml.dom
import minidom
13 from xml_matching
import xml_matches
14 from urdf_parser_py
import urdf
19 super(ParseException, self).
__init__(e, path)
23 @mock.patch(
'urdf_parser_py.xml_reflection.on_error',
24 mock.Mock(side_effect=ParseException))
26 return urdf.Robot.from_xml_string(xml)
29 xml = minidom.parseString(orig)
30 robot = urdf.Robot.from_xml_string(orig)
31 rewritten = minidom.parseString(robot.to_xml_string())
35 robot_xml_string = robot.to_xml_string()
36 robot_xml = minidom.parseString(robot_xml_string)
37 orig_xml = minidom.parseString(xml)
41 xml =
'''<?xml version="1.0"?>
42 <robot name="test" version="1.0">
43 <transmission name="simple_trans">
44 <type>transmission_interface/SimpleTransmission</type>
45 <joint name="foo_joint">
46 <hardwareInterface>EffortJointInterface</hardwareInterface>
48 <actuator name="foo_motor">
49 <mechanicalReduction>50.0</mechanicalReduction>
55 robot = urdf.Robot(name=
'test', version=
'1.0')
56 trans = urdf.Transmission(name=
'simple_trans')
57 trans.type =
'transmission_interface/SimpleTransmission'
58 joint = urdf.TransmissionJoint(name=
'foo_joint')
59 joint.add_aggregate(
'hardwareInterface',
'EffortJointInterface')
60 trans.add_aggregate(
'joint', joint)
61 actuator = urdf.Actuator(name=
'foo_motor')
62 actuator.mechanicalReduction = 50.0
63 trans.add_aggregate(
'actuator', actuator)
64 robot.add_aggregate(
'transmission', trans)
68 xml =
'''<?xml version="1.0"?>
69 <robot name="test" version="1.0">
70 <transmission name="simple_trans">
71 <type>transmission_interface/SimpleTransmission</type>
72 <joint name="foo_joint">
73 <hardwareInterface>EffortJointInterface</hardwareInterface>
75 <joint name="bar_joint">
76 <hardwareInterface>EffortJointInterface</hardwareInterface>
77 <hardwareInterface>EffortJointInterface</hardwareInterface>
79 <actuator name="foo_motor">
80 <mechanicalReduction>50.0</mechanicalReduction>
86 robot = urdf.Robot(name=
'test', version=
'1.0')
87 trans = urdf.Transmission(name=
'simple_trans')
88 trans.type =
'transmission_interface/SimpleTransmission'
89 joint = urdf.TransmissionJoint(name=
'foo_joint')
90 joint.add_aggregate(
'hardwareInterface',
'EffortJointInterface')
91 trans.add_aggregate(
'joint', joint)
92 joint = urdf.TransmissionJoint(name=
'bar_joint')
93 joint.add_aggregate(
'hardwareInterface',
'EffortJointInterface')
94 joint.add_aggregate(
'hardwareInterface',
'EffortJointInterface')
95 trans.add_aggregate(
'joint', joint)
96 actuator = urdf.Actuator(name=
'foo_motor')
97 actuator.mechanicalReduction = 50.0
98 trans.add_aggregate(
'actuator', actuator)
99 robot.add_aggregate(
'transmission', trans)
103 xml =
'''<?xml version="1.0"?>
104 <robot name="test" version="1.0">
105 <transmission name="simple_trans">
106 <type>transmission_interface/SimpleTransmission</type>
107 <joint name="foo_joint">
108 <hardwareInterface>EffortJointInterface</hardwareInterface>
110 <actuator name="foo_motor">
111 <mechanicalReduction>50.0</mechanicalReduction>
113 <actuator name="bar_motor"/>
118 robot = urdf.Robot(name=
'test', version=
'1.0')
119 trans = urdf.Transmission(name=
'simple_trans')
120 trans.type =
'transmission_interface/SimpleTransmission'
121 joint = urdf.TransmissionJoint(name=
'foo_joint')
122 joint.add_aggregate(
'hardwareInterface',
'EffortJointInterface')
123 trans.add_aggregate(
'joint', joint)
124 actuator = urdf.Actuator(name=
'foo_motor')
125 actuator.mechanicalReduction = 50.0
126 trans.add_aggregate(
'actuator', actuator)
127 actuator = urdf.Actuator(name=
'bar_motor')
128 trans.add_aggregate(
'actuator', actuator)
129 robot.add_aggregate(
'transmission', trans)
133 xml =
'''<?xml version="1.0"?>
134 <robot name="test" version="1.0">
135 <transmission name="simple_trans">
136 <type>transmission_interface/SimpleTransmission</type>
139 self.assertRaises(xmlr.core.ParseError, self.
parse, xml)
142 xml =
'''<?xml version="1.0"?>
143 <robot name="test" version="1.0">
144 <transmission name="simple_trans">
145 <type>transmission_interface/SimpleTransmission</type>
146 <joint name="foo_joint">
147 <hardwareInterface>EffortJointInterface</hardwareInterface>
151 self.assertRaises(xmlr.core.ParseError, self.
parse, xml)
154 xml =
'''<?xml version="1.0"?>
155 <robot name="test" version="1.0">
156 <transmission name="PR2_trans" type="SimpleTransmission">
157 <joint name="foo_joint"/>
158 <actuator name="foo_motor"/>
159 <mechanicalReduction>1.0</mechanicalReduction>
164 robot = urdf.Robot(name=
'test', version=
'1.0')
165 trans = urdf.PR2Transmission(name=
'PR2_trans', joint=
'foo_joint', actuator=
'foo_motor', type=
'SimpleTransmission', mechanicalReduction=1.0)
166 robot.add_aggregate(
'transmission', trans)
170 xml =
'''<?xml version="1.0"?>
171 <robot name="test" version="1.0">
175 <cylinder length="1" radius="1"/>
177 <material name="mat"/>
183 robot = urdf.Robot(name=
'test', version=
'1.0')
184 link = urdf.Link(name=
'link',
185 visual=urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
186 material=urdf.Material(name=
'mat')))
190 robot = urdf.Robot(name=
'test', version=
'1.0')
191 link = urdf.Link(name=
'link')
192 link.visual = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
193 material=urdf.Material(name=
'mat'))
198 xml =
'''<?xml version="1.0"?>
199 <robot name="test" version="1.0">
200 <material name="mat">
201 <color rgba="0.0 0.0 0.0 1.0"/>
206 robot = urdf.Robot(name=
'test', version=
'1.0')
207 material = urdf.Material(name=
'mat', color=urdf.Color([0.0, 0.0, 0.0, 1.0]))
208 robot.add_aggregate(
'material', material)
212 xml =
'''<?xml version="1.0"?>
213 <robot name="test" version="1.0">
214 <material name="mat"/>
216 self.assertRaises(xmlr.core.ParseError, self.
parse, xml)
219 xml =
'''<?xml version="1.0"?>
220 <robot name="test" version="1.0">
224 <cylinder length="1" radius="1"/>
226 <material name="mat"/>
230 <cylinder length="4" radius="0.5"/>
232 <material name="mat2"/>
238 robot = urdf.Robot(name=
'test', version=
'1.0')
239 link = urdf.Link(name=
'link')
240 link.visual = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
241 material=urdf.Material(name=
'mat'))
242 link.visual = urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5),
243 material=urdf.Material(name=
'mat2'))
248 xml =
'''<?xml version="1.0"?>
249 <robot name="test" version="1.0">
251 <visual name="alice">
253 <cylinder length="1" radius="1"/>
255 <material name="mat"/>
262 xml =
'''<?xml version="1.0"?>
263 <robot name="test" version="1.0">
267 <cylinder length="1" radius="1"/>
272 <cylinder length="4" radius="0.5"/>
279 robot = urdf.Robot(name=
'test', version=
'1.0')
280 link = urdf.Link(name=
'link')
281 link.collision = urdf.Collision(geometry=urdf.Cylinder(length=1, radius=1))
282 link.collision = urdf.Collision(geometry=urdf.Cylinder(length=4, radius=0.5))
287 xml =
'''<?xml version="1.0"?>
288 <robot name="test" version="1.0">
290 <collision name="alice">
292 <cylinder length="1" radius="1"/>
300 xml =
'''<?xml version="1.0"?>
301 <robot name="test" version="1">
303 self.assertRaises(ValueError, self.
parse, xml)
306 xml =
'''<?xml version="1.0"?>
307 <robot name="test" version="1.0.0">
309 self.assertRaises(ValueError, self.
parse, xml)
312 xml =
'''<?xml version="1.0"?>
313 <robot name="test" version="1.">
315 self.assertRaises(ValueError, self.
parse, xml)
318 xml =
'''<?xml version="1.0"?>
319 <robot name="test" version=".0">
321 self.assertRaises(ValueError, self.
parse, xml)
324 xml =
'''<?xml version="1.0"?>
325 <robot name="test" version="-1.0">
327 self.assertRaises(ValueError, self.
parse, xml)
330 xml =
'''<?xml version="1.0"?>
331 <robot name="test" version="1.-0">
333 self.assertRaises(ValueError, self.
parse, xml)
336 xml =
'''<?xml version="1.0"?>
337 <robot name="test" version="a.c">
339 self.assertRaises(ValueError, self.
parse, xml)
342 xml =
'''<?xml version="1.0"?>
343 <robot name="test" version="1.c">
345 self.assertRaises(ValueError, self.
parse, xml)
348 xml =
'''<?xml version="1.0"?>
349 <robot name="test" version="1.0~pre6">
351 self.assertRaises(ValueError, self.
parse, xml)
354 xml =
'''<?xml version="1.0"?>
355 <robot name="test" version="1.0">
360 xml =
'''<?xml version="1.0"?>
361 <robot name="test" version="foo">
363 self.assertRaises(ValueError, self.
parse, xml)
366 xml =
'''<?xml version="1.0"?>
367 <robot name="test" version="2.0">
369 self.assertRaises(ValueError, self.
parse, xml)
372 @mock.patch(
'urdf_parser_py.xml_reflection.on_error',
373 mock.Mock(side_effect=ParseException))
375 return urdf.Robot.from_xml_string(xml)
378 xml =
'''<?xml version="1.0"?>
380 <link name="test_link">
387 robot = self.
parse(xml)
388 origin = robot.links[0].inertial.origin
389 self.assertEqual(origin.xyz, [0, 0, 0])
390 self.assertEqual(origin.rpy, [0, 0, 0])
393 xml =
'''<?xml version="1.0"?>
395 <link name="test_link">
398 <origin xyz="1 2 3"/>
402 robot = self.
parse(xml)
403 origin = robot.links[0].inertial.origin
404 self.assertEqual(origin.xyz, [1, 2, 3])
405 self.assertEqual(origin.rpy, [0, 0, 0])
410 xml =
'''<?xml version="1.0"?>
411 <robot name="test" version="1.0">
415 <cylinder length="1" radius="1"/>
417 <material name="mat"/>
421 <cylinder length="4" radius="0.5"/>
423 <material name="mat2"/>
427 <cylinder length="1" radius="1"/>
432 <cylinder length="4" radius="0.5"/>
440 robot = urdf.Robot.from_xml_string(self.
xml)
441 self.assertEqual(2, len(robot.links[0].visuals))
443 id(robot.links[0].visuals[0]), id(robot.links[0].visual))
445 self.assertEqual(
None, robot.links[1].visual)
448 robot.links[0].visual = dummyObject
449 self.assertEqual(id(dummyObject), id(robot.links[0].visuals[0]))
452 robot = urdf.Robot.from_xml_string(self.
xml)
453 self.assertEqual(2, len(robot.links[0].collisions))
455 id(robot.links[0].collisions[0]), id(robot.links[0].collision))
457 self.assertEqual(
None, robot.links[1].collision)
460 robot.links[0].collision = dummyObject
461 self.assertEqual(id(dummyObject), id(robot.links[0].collisions[0]))
464 robot = urdf.Robot(name=
'test', version=
'1.0')
465 link = urdf.Link(name=
'link')
466 link.visual = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
467 material=urdf.Material(name=
'mat'))
468 link.visual = urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5),
469 material=urdf.Material(name=
'mat2'))
470 link.collision = urdf.Collision(geometry=urdf.Cylinder(length=1, radius=1))
471 link.collision = urdf.Collision(geometry=urdf.Cylinder(length=4, radius=0.5))
473 link = urdf.Link(name=
'link2')
476 robot_xml_string = robot.to_xml_string()
477 robot_xml = minidom.parseString(robot_xml_string)
478 orig_xml = minidom.parseString(self.
xml)
482 robot = urdf.Robot(name=
'test', version=
'1.0')
483 link = urdf.Link(name=
'link')
484 link.add_aggregate(
'visual', urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
485 material=urdf.Material(name=
'mat')))
486 link.add_aggregate(
'visual', urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5),
487 material=urdf.Material(name=
'mat2')))
488 link.add_aggregate(
'collision', urdf.Collision(geometry=urdf.Cylinder(length=1, radius=1)))
489 link.add_aggregate(
'collision', urdf.Collision(geometry=urdf.Cylinder(length=4, radius=0.5)))
491 link = urdf.Link(name=
'link2')
494 robot_xml_string = robot.to_xml_string()
495 robot_xml = minidom.parseString(robot_xml_string)
496 orig_xml = minidom.parseString(self.
xml)
501 testcase = urdf.URDF(
'robot_name').to_xml()
502 self.assertTrue(
'name' in testcase.keys())
503 self.assertTrue(
'version' in testcase.keys())
504 self.assertEqual(testcase.get(
'name'),
'robot_name')
505 self.assertEqual(testcase.get(
'version'),
'1.0')
508 testcase = urdf.URDF(
'robot_name',
'1.0').to_xml()
509 self.assertTrue(
'name' in testcase.keys())
510 self.assertTrue(
'version' in testcase.keys())
511 self.assertEqual(testcase.get(
'name'),
'robot_name')
512 self.assertEqual(testcase.get(
'version'),
'1.0')
515 if __name__ ==
'__main__':