Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 30 additions & 3 deletions src/urdf_parser_py/urdf.py
Original file line number Diff line number Diff line change
Expand Up @@ -257,18 +257,25 @@ def __init__(self, rising=None, falling=None):


class JointLimit(xmlr.Object):
def __init__(self, effort=None, velocity=None, lower=None, upper=None):
def __init__(self, effort=None, velocity=None, lower=None, upper=None,
acceleration=None, deceleration=None, jerk=None):
self.effort = effort
self.velocity = velocity
self.lower = lower
self.upper = upper
self.acceleration = acceleration
self.deceleration = deceleration
self.jerk = jerk


xmlr.reflect(JointLimit, tag='limit', params=[
xmlr.Attribute('effort', float),
xmlr.Attribute('lower', float, False, 0),
xmlr.Attribute('upper', float, False, 0),
xmlr.Attribute('velocity', float)
xmlr.Attribute('velocity', float),
xmlr.Attribute('acceleration', float, False, float('inf')),
xmlr.Attribute('deceleration', float, False, float('inf')),
xmlr.Attribute('jerk', float, False, float('inf'))
])

# FIXME: we are missing __str__ here.
Expand Down Expand Up @@ -471,7 +478,7 @@ def check_valid(self):


class Robot(xmlr.Object):
SUPPORTED_VERSIONS = ["1.0"]
SUPPORTED_VERSIONS = ["1.0", "1.1", "1.2"]

def __init__(self, name=None, version="1.0"):
self.aggregate_init()
Expand Down Expand Up @@ -556,6 +563,26 @@ 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)))

# Validate that acceleration, deceleration, and jerk are only used in version 1.2+
major = int(split[0])
minor = int(split[1])

if major == 1 and minor < 2:
for joint in self.joints:
if joint.limit is not None:
if joint.limit.acceleration is not None and joint.limit.acceleration != float('inf'):
raise ValueError(
"Joint '%s': acceleration attribute requires URDF version 1.2, "
"but version %s was specified" % (joint.name, self.version))
if joint.limit.deceleration is not None and joint.limit.deceleration != float('inf'):
raise ValueError(
"Joint '%s': deceleration attribute requires URDF version 1.2, "
"but version %s was specified" % (joint.name, self.version))
if joint.limit.jerk is not None and joint.limit.jerk != float('inf'):
raise ValueError(
"Joint '%s': jerk attribute requires URDF version 1.2, "
"but version %s was specified" % (joint.name, self.version))


xmlr.reflect(Robot, tag='robot', params=[
xmlr.Attribute('name', str),
Expand Down
198 changes: 198 additions & 0 deletions test/test_urdf.py
Original file line number Diff line number Diff line change
Expand Up @@ -377,5 +377,203 @@ def test_new_urdf_with_version(self):
self.assertEqual(testcase.get('version'), '1.0')


class TestJointLimitVersioning(unittest.TestCase):
"""Tests for version-based validation of acceleration, deceleration, and jerk."""

@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 test_version_1_0_with_acceleration_fails(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10" acceleration="5.0"/>
</joint>
</robot>'''
self.assertRaises(ValueError, self.parse, xml)

def test_version_1_0_with_deceleration_fails(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10" deceleration="5.0"/>
</joint>
</robot>'''
self.assertRaises(ValueError, self.parse, xml)

def test_version_1_0_with_jerk_fails(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10" jerk="100.0"/>
</joint>
</robot>'''
self.assertRaises(ValueError, self.parse, xml)

def test_version_1_1_with_acceleration_fails(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.1">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10" acceleration="5.0"/>
</joint>
</robot>'''
self.assertRaises(ValueError, self.parse, xml)

def test_version_1_1_with_deceleration_fails(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.1">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10" deceleration="5.0"/>
</joint>
</robot>'''
self.assertRaises(ValueError, self.parse, xml)

def test_version_1_1_with_jerk_fails(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.1">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10" jerk="100.0"/>
</joint>
</robot>'''
self.assertRaises(ValueError, self.parse, xml)

def test_version_1_0_without_new_limits_succeeds(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10"/>
</joint>
</robot>'''
robot = self.parse(xml)
self.assertIsNotNone(robot)
self.assertEqual(robot.joints[0].limit.effort, 100)
self.assertEqual(robot.joints[0].limit.velocity, 10)

def test_version_1_1_without_new_limits_succeeds(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.1">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10"/>
</joint>
</robot>'''
robot = self.parse(xml)
self.assertIsNotNone(robot)
self.assertEqual(robot.joints[0].limit.effort, 100)
self.assertEqual(robot.joints[0].limit.velocity, 10)

def test_version_1_2_with_acceleration_succeeds(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.2">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10" acceleration="5.0"/>
</joint>
</robot>'''
robot = self.parse(xml)
self.assertIsNotNone(robot)
self.assertEqual(robot.joints[0].limit.acceleration, 5.0)

def test_version_1_2_with_deceleration_succeeds(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.2">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10" deceleration="3.0"/>
</joint>
</robot>'''
robot = self.parse(xml)
self.assertIsNotNone(robot)
self.assertEqual(robot.joints[0].limit.deceleration, 3.0)

def test_version_1_2_with_jerk_succeeds(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.2">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10" jerk="200.0"/>
</joint>
</robot>'''
robot = self.parse(xml)
self.assertIsNotNone(robot)
self.assertEqual(robot.joints[0].limit.jerk, 200.0)

def test_version_1_2_with_all_new_limits_succeeds(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.2">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10" acceleration="5.0" deceleration="3.0" jerk="200.0"/>
</joint>
</robot>'''
robot = self.parse(xml)
self.assertIsNotNone(robot)
self.assertEqual(robot.joints[0].limit.acceleration, 5.0)
self.assertEqual(robot.joints[0].limit.deceleration, 3.0)
self.assertEqual(robot.joints[0].limit.jerk, 200.0)

def test_version_1_2_without_new_limits_uses_defaults(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.2">
<link name="l1"/>
<link name="l2"/>
<joint name="j1" type="fixed">
<parent link="l1"/>
<child link="l2"/>
<limit effort="100" velocity="10"/>
</joint>
</robot>'''
robot = self.parse(xml)
self.assertIsNotNone(robot)
self.assertEqual(robot.joints[0].limit.acceleration, float('inf'))
self.assertEqual(robot.joints[0].limit.deceleration, float('inf'))
self.assertEqual(robot.joints[0].limit.jerk, float('inf'))


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