def test_xml_input_random(self):
sdf_elements = [
'mass', 'child', 'parent', 'pose', 'box', 'cylinder', 'sphere',
'mesh', 'limit', 'inertial', 'inertia'
]
for sdf_name in sdf_elements:
obj = create_sdf_element(sdf_name)
assert obj is not None
obj.random()
output = subprocess.check_output(
['pcg-sdf2urdf', '--xml',
obj.to_xml_as_str(), '--print'])
urdf = parse_urdf(output.decode('utf-8'))
assert urdf is not None
response_sdf = urdf2sdf(urdf)
assert obj == response_sdf
# Test for dynamics
obj = create_sdf_element('dynamics')
assert obj is not None
obj.reset(with_optional_elements=True)
obj.damping.random()
obj.friction.random()
output = subprocess.check_output(
['pcg-sdf2urdf', '--xml',
obj.to_xml_as_str(), '--print'])
urdf = parse_urdf(output.decode('utf-8'))
assert urdf is not None
response_sdf = urdf2sdf(urdf)
assert obj.damping == response_sdf.damping
assert obj.friction == response_sdf.friction
def test_parse_empty_link(self):
filename = get_urdf_file('link_empty')
obj = parse_urdf(filename)
self.assertTrue(obj.is_valid(), 'Link should be valid')
self.assertEqual(obj.name, 'empty_link',
'Link should have name empty link')
self.assertIsNone(obj.mass, 'Link should have no mass')
self.assertIsNone(obj.center_of_mass,
'Link should have no center of mass')
self.assertIsNone(obj.inertia, 'Link should have no inertia')
self.assertIsNone(obj.inertial, 'Link should have no inertial')
self.assertIsNone(obj.collisions, 'Link should have no collisions')
self.assertIsNone(obj.visuals, 'Link should have no visuals')
def test_parse_gazebo_default_joint(self):
filename = get_urdf_file('gazebo_default_joint')
obj = parse_urdf(filename)
self.assertTrue(obj.is_valid(), 'Gazebo block should be valid')
self.assertEqual(
obj.reference, 'joint',
'Wrong value for reference, received={}, expected={}'.format(
obj.reference, 'joint'))
elems = dict(stopCfm=0.1, stopErp=0.3, provideFeedback=False)
for tag in elems:
self.assertTrue(hasattr(obj, tag))
self.assertEqual(getattr(obj, tag).value, elems[tag])
def test_parse_geometry_mesh(self):
filename = get_urdf_file('geometry_mesh')
obj = parse_urdf(filename)
self.assertTrue(obj.is_valid(), 'Mesh geometry should be valid')
self.assertIsNotNone(obj.mesh, 'Mesh geometry should exist')
self.assertEqual(
obj.mesh.filename, 'mesh',
'Wrong esh filename, received={}, expected={}'.format(
obj.mesh.filename, 'mesh'))
sdf_obj = obj.to_sdf()
self.assertIsNotNone(sdf_obj, 'Convert SDF mesh geometry is invalid')
self.assertEqual(sdf_obj.mesh.uri.value, obj.mesh.filename,
'Converted SDF mesh has different value for mesh')
def test_parse_geometry_box(self):
filename = get_urdf_file('geometry_box')
obj = parse_urdf(filename)
self.assertTrue(obj.is_valid(), 'Box geometry should be valid')
self.assertIsNotNone(obj.box, 'Box geometry should exist')
self.assertEqual(
obj.box.size, [1, 2, 3],
'Wrong box size, received={}, expected={}'.format(
obj.box.size, [1, 2, 3]))
sdf_obj = obj.to_sdf()
self.assertIsNotNone(sdf_obj, 'Convert SDF box geometry is invalid')
self.assertEqual(sdf_obj.box.size.value, obj.box.size,
'Converted SDF box has different value for size')
def test_parse_geometry_sphere(self):
filename = get_urdf_file('geometry_sphere')
obj = parse_urdf(filename)
self.assertTrue(obj.is_valid(), 'Sphere geometry should be valid')
self.assertIsNotNone(obj.sphere, 'Sphere geometry should exist')
self.assertEqual(
obj.sphere.radius, 3.5,
'Wrong sphere radius, received={}, expected={}'.format(
obj.sphere.radius, 3.5))
sdf_obj = obj.to_sdf()
self.assertIsNotNone(sdf_obj, 'Convert SDF sphere geometry is invalid')
self.assertEqual(
sdf_obj.sphere.radius.value, obj.sphere.radius,
'Converted SDF sphere has different value for radius')
def test_parse_gazebo_default_link(self):
filename = get_urdf_file('gazebo_default_link')
obj = parse_urdf(filename)
elems = dict(mu1=1.0,
mu2=1.0,
kp=1000000.0,
kd=100.0,
minDepth=0.001,
maxVel=1.0,
selfCollide=False,
maxContacts=15,
material='Gazebo/Yellow')
for tag in elems:
self.assertTrue(hasattr(obj, tag))
self.assertEqual(getattr(obj, tag).value, elems[tag])
def test_gazebo_material_single_link_single_visual(self):
filename = get_urdf_file('gazebo_material_single_link_single_visual')
obj = parse_urdf(filename)
self.assertIsNotNone(obj)
self.assertTrue(obj.is_valid())
self.assertEqual(obj.xml_element_name, 'robot')
sdf = urdf2sdf(obj)
self.assertIsNotNone(sdf)
self.assertIsNotNone(sdf.links)
self.assertEqual(len(sdf.links), 1)
self.assertIsNotNone(sdf.links[0].visuals)
self.assertEqual(sdf.links[0].visuals[0].material.script.uris[0].value,
'file://media/materials/scripts/gazebo.material')
self.assertEqual(sdf.links[0].visuals[0].material.script.name.value,
'Gazebo/Blue')
def test_xml_input_visual_collision(self):
for sdf_tag in ['visual', 'collision']:
obj = create_sdf_element(sdf_tag)
assert obj is not None
obj.pose = create_sdf_element('pose')
obj.pose.random()
obj.geometry = create_sdf_element('geometry')
geometries = ['box', 'cylinder', 'sphere', 'mesh']
for geo_name in geometries:
obj.geometry.reset(mode=geo_name)
getattr(obj.geometry, geo_name).random()
output = subprocess.check_output(
['pcg-sdf2urdf', '--xml',
obj.to_xml_as_str(), '--print'])
urdf = parse_urdf(output.decode('utf-8'))
assert urdf is not None
response_sdf = urdf2sdf(urdf)
assert obj == response_sdf
def test_parse_geometry_cylinder(self):
filename = get_urdf_file('geometry_cylinder')
obj = parse_urdf(filename)
self.assertTrue(obj.is_valid(), 'Cylinder geometry should be valid')
self.assertIsNotNone(obj.cylinder, 'Cylinder geometry should exist')
self.assertEqual(
obj.cylinder.radius, 3.4,
'Wrong cylinder radius, received={}, expected={}'.format(
obj.cylinder.radius, 3.4))
self.assertEqual(
obj.cylinder.length, 5.6,
'Wrong cylinder radius, received={}, expected={}'.format(
obj.cylinder.length, 5.6))
sdf_obj = obj.to_sdf()
self.assertIsNotNone(sdf_obj,
'Convert SDF cylinder geometry is invalid')
self.assertEqual(
sdf_obj.cylinder.radius.value, obj.cylinder.radius,
'Converted SDF cylinder has different value for radius')
self.assertEqual(
sdf_obj.cylinder.length.value, obj.cylinder.length,
'Converted SDF cylinder has different value for length')
def test_parse_gazebo_imu_sdf_1_6(self):
filename = get_urdf_file('gazebo_imu_1_6')
obj = parse_urdf(filename)
self.assertTrue(obj.is_valid(), 'Gazebo block should be valid')
self.assertIn('sensor', obj.children, 'Sensor block was not loaded')
self.assertEqual(
len(obj.children['sensor']), 1,
'One sensor element must should be in the children sensor list')
# Checking general Gazebo sensor parameters
sensor = obj.children['sensor'][0]
self.assertEqual(
sensor.name, 'custom_imu',
'Wrong value for name, received={}, expected={}'.format(
sensor.name, 'custom_imu'))
self.assertEqual(
sensor.pose.value, [1, 2, 3, 4, 5, 6],
'Wrong value for pose, received={}, expected={}'.format(
sensor.pose.value, [1, 2, 3, 4, 5, 6]))
self.assertEqual(
sensor.visualize.value, 1,
'Wrong value for visualize flag, received={}, expected={}'.format(
sensor.visualize.value, 1))
self.assertEqual(
sensor.topic.value, 'sensor_topic',
'Wrong value for topic name, received={}, expected={}'.format(
sensor.topic.value, 1))
self.assertEqual(
sensor.update_rate.value, 50,
'Wrong value for update rate, received={}, expected={}'.format(
sensor.update_rate.value, 50))
self.assertEqual(
sensor.always_on.value, True,
'Wrong value for always on flag, received={}, expected={}'.format(
sensor.always_on.value, True))
self.assertIsNotNone(sensor.imu, 'IMU elements was not parsed')
# Checking IMU parameters
imu = obj.children['sensor'][0].imu
self.assertEqual(
imu.topic.value, 'imu_topic',
'Wrong value for imu.topic, received={}, expected={}'.format(
imu.topic.value, 'imu_topic'))
self.assertIsNotNone(
imu.orientation_reference_frame.localization,
'No localization element found for orientation_reference_frame')
# Checking the IMU's orientation reference frame description
self.assertEqual(
imu.orientation_reference_frame.localization.value, 'CUSTOM',
'Wrong value for imu.orientation_reference_frame.localization,'
' received={}, expected={}'.format(
imu.orientation_reference_frame.localization.value, 'CUSTOM'))
# Checking noise parameters for angular velocity and linear
# acceleration
for imu_element in ['angular_velocity', 'linear_acceleration']:
self.assertTrue(hasattr(imu, imu_element),
'IMU has no element {}'.format(imu_element))
for component in ['x', 'y', 'z']:
self.assertTrue(
hasattr(getattr(imu, imu_element), component),
'imu.{} has no element {}'.format(imu_element, component))
self.assertTrue(
hasattr(getattr(getattr(imu, imu_element), component),
'noise'), 'imu.{}.{} has no element noise'.format(
imu_element, component))
noise = getattr(getattr(imu, imu_element), component).noise
if component == 'x':
self.assertEqual(
noise.type.value, 'none',
'Wrong value for noise imu.{}.{}.noise.type, '
'received={}, expected={}'.format(
imu_element, component, noise.type.value, 'none'))
elif component == 'y':
self.assertEqual(
noise.type.value, 'gaussian',
'Wrong value for noise imu.{}.{}.noise.type, '
'received={}, expected={}'.format(
imu_element, component, noise.type.value,
'gaussian'))
elif component == 'z':
self.assertEqual(
noise.type.value, 'gaussian_quantized',
'Wrong value for noise imu.{}.{}.noise.type, '
'received={}, expected={}'.format(
imu_element, component, noise.type.value,
'gaussian_quantized'))
self.assertEqual(
noise.mean.value, 0.2,
'Wrong value for noise imu.{}.{}.noise.mean, '
'received={}, expected={}'.format(imu_element, component,
noise.mean.value, 0.2))
self.assertEqual(
noise.stddev.value, 0.02,
'Wrong value for noise imu.{}.{}.noise.stddev, '
'received={}, expected={}'.format(imu_element, component,
noise.stddev.value,
0.02))
self.assertEqual(
noise.bias_mean.value, 4,
'Wrong value for noise imu.{}.{}.noise.bias_mean, '
'received={}, expected={}'.format(imu_element, component,
noise.bias_mean.value,
4))
self.assertEqual(
noise.bias_stddev.value, 4,
'Wrong value for noise imu.{}.{}.noise.bias_stddev, '
'received={}, expected={}'.format(imu_element, component,
noise.bias_stddev.value,
4))
self.assertEqual(
noise.precision.value, 0.5,
'Wrong value for noise imu.{}.{}.noise.precision, '
'received={}, expected={}'.format(imu_element, component,
noise.precision.value,
0.5))
# Checking plugin parameters
self.assertIsNotNone(sensor.plugins, 'No plugin found for sensor')
self.assertEqual(len(sensor.plugins), 1,
'There should be one plugin for sensor')
plugin = sensor.plugins[0]
self.assertEqual(
plugin.name, 'some_imu_plugin',
'Wrong value for plugin.name value (SDF 1.6), '
'received={}, expected={}'.format(plugin.name, 'some_imu_plugin'))
self.assertEqual(
plugin.filename, 'imu_ros_plugin.so',
'Wrong value for plugin.filename value (SDF 1.6), '
'received={}, expected={}'.format(plugin.filename,
'imu_ros_plugin.so'))
self.assertIn('param', plugin.value,
'Plugin parameter <param> not found')
self.assertEqual(
plugin.value['param'], 10,
'Wrong value for plugin.param value (SDF 1.6), '
'received={}, expected={}'.format(plugin.value['param'], 10))
self.assertIn('block', plugin.value,
'Plugin parameter <block> not found')
self.assertIn('subparam', plugin.value['block'],
'Plugin block parameter <subparam> not found')
self.assertEqual(
plugin.value['block']['subparam'], 1.245,
'Wrong value for plugin.block.subparam value'
' (SDF 1.6), received={}, expected={}'.format(
plugin.value['block']['subparam'], 1.245))
def test_parse_gazebo_imu_sdf_1_4(self):
filename = get_urdf_file('gazebo_imu_1_4')
obj = parse_urdf(filename)
self.assertIsNotNone(obj, filename)
self.assertTrue(obj.is_valid(), 'Gazebo block should be valid')
self.assertIn('sensor', obj.children, 'Sensor block was not loaded')
self.assertEqual(
len(obj.children['sensor']), 1,
'One sensor element must should be in the children sensor list')
# Checking general Gazebo sensor parameters
sensor = obj.children['sensor'][0]
self.assertEqual(
sensor.name, 'custom_imu',
'Wrong value for name, received={}, expected={}'.format(
sensor.name, 'custom_imu'))
self.assertEqual(
sensor.pose.value, [1, 2, 3, 4, 5, 6],
'Wrong value for pose, received={}, expected={}'.format(
sensor.pose.value, [1, 2, 3, 4, 5, 6]))
self.assertEqual(
sensor.visualize.value, 1,
'Wrong value for visualize flag, received={}, expected={}'.format(
sensor.visualize.value, 1))
self.assertEqual(
sensor.topic.value, 'sensor_topic',
'Wrong value for topic name, received={}, expected={}'.format(
sensor.topic.value, 1))
self.assertEqual(
sensor.update_rate.value, 50,
'Wrong value for update rate, received={}, expected={}'.format(
sensor.update_rate.value, 50))
self.assertEqual(
sensor.always_on.value, True,
'Wrong value for always on flag, received={}, '
'expected={}'.format(sensor.always_on.value, True))
self.assertIsNotNone(sensor.imu, 'IMU elements was not parsed')
# Checking IMU parameters
imu = obj.children['sensor'][0].imu
self.assertIsNotNone(
imu.noise, 'For SDF version 1.4, noise element should be valid')
self.assertEqual(
imu.noise.type.value, 'gaussian',
'Wrong value for type (SDF 1.4), received={}, '
'expected={}'.format(imu.noise.type.value, True))
self.assertEqual(
imu.noise.accel.mean.value, 0.1,
'Wrong value for accel.mean value (SDF 1.4), '
'received={}, expected={}'.format(imu.noise.accel.mean.value, 0.1))
self.assertEqual(
imu.noise.accel.bias_mean.value, 3,
'Wrong value for accel.bias_mean value (SDF 1.4), '
'received={}, expected={}'.format(imu.noise.accel.bias_mean.value,
3))
self.assertEqual(
imu.noise.accel.stddev.value, 0.01,
'Wrong value for accel.stddev value (SDF 1.4), '
'received={}, expected={}'.format(imu.noise.accel.stddev.value,
0.01))
self.assertEqual(
imu.noise.accel.bias_stddev.value, 3,
'Wrong value for accel.bias_stddev value (SDF 1.4), '
'received={}, expected={}'.format(
imu.noise.accel.bias_stddev.value, 3))
self.assertEqual(
imu.noise.rate.mean.value, 0.1,
'Wrong value for rate.mean value (SDF 1.4), '
'received={}, expected={}'.format(imu.noise.rate.mean.value, 0.1))
self.assertEqual(
imu.noise.rate.bias_mean.value, 3,
'Wrong value for rate.bias_mean value (SDF 1.4), '
'received={}, expected={}'.format(imu.noise.rate.bias_mean.value,
3))
self.assertEqual(
imu.noise.rate.stddev.value, 0.01,
'Wrong value for rate.stddev value (SDF 1.4), '
'received={}, expected={}'.format(imu.noise.rate.stddev.value,
0.01))
self.assertEqual(
imu.noise.rate.bias_stddev.value, 3,
'Wrong value for rate.bias_stddev value (SDF 1.4), '
'received={}, expected={}'.format(imu.noise.rate.bias_stddev.value,
3))
# Checking plugin parameters
self.assertIsNotNone(sensor.plugins, 'No plugin found for sensor')
self.assertEqual(len(sensor.plugins), 1,
'There should be one plugin for sensor')
plugin = sensor.plugins[0]
print(plugin)
self.assertEqual(
plugin.name, 'some_imu_plugin',
'Wrong value for plugin.name value (SDF 1.4), '
'received={}, expected={}'.format(plugin.name, 'some_imu_plugin'))
self.assertEqual(
plugin.filename, 'imu_ros_plugin.so',
'Wrong value for plugin.filename value (SDF 1.4), '
'received={}, expected={}'.format(plugin.filename,
'imu_ros_plugin.so'))
self.assertIn('param', plugin.value,
'Plugin parameter <param> not found')
self.assertEqual(
plugin.value['param'], 10,
'Wrong value for plugin.param value (SDF 1.4), '
'received={}, expected={}'.format(plugin.value['param'], 10))
self.assertIn('block', plugin.value,
'Plugin parameter <block> not found')
self.assertIn('subparam', plugin.value['block'],
'Plugin block parameter <subparam> not found')
self.assertEqual(
plugin.value['block']['subparam'], 1.245,
'Wrong value for plugin.block.subparam value (SDF 1.4), '
'received={}, expected={}'.format(
plugin.value['block']['subparam'], 1.245))
def test_parse_simple_link(self):
filename = get_urdf_file('link_simple')
obj = parse_urdf(filename)
self.assertTrue(obj.is_valid(), 'Link should be valid')
# Check that all elements are valid
self.assertEqual(obj.name, 'simple_link',
'Link should have name simple link')
self.assertIsNotNone(obj.mass, 'Link should have valid mass')
self.assertIsNotNone(obj.center_of_mass,
'Link should have valid center of mass')
self.assertIsNotNone(obj.inertia, 'Link should have valid inertia')
self.assertIsNotNone(obj.inertial, 'Link should have valid inertial')
self.assertIsNotNone(obj.collisions, 'Link should have one collision')
self.assertIsNotNone(obj.visuals, 'Link should have one visual')
# Check values
self.assertEqual(
obj.mass.value, 0.1, 'Wrong value for link mass, received={}, '
'expected={}'.format(obj.mass.value, 0.1))
self.assertEqual(
obj.inertial.mass.value, 0.1,
'Wrong value for link mass in inertial, '
'received={}, expected={}'.format(obj.inertial.mass.value, 0.1))
self.assertEqual(
obj.center_of_mass, [1, 2, 3],
'Wrong value for link.center_of_mass, '
'received={}, expected={}'.format(obj.center_of_mass, [1, 2, 3]))
self.assertEqual(
obj.inertial.origin.xyz, [1, 2, 3],
'Wrong value for link.inertial.origin.xyz, '
'received={}, expected={}'.format(obj.inertial.origin.xyz,
[1, 2, 3]))
self.assertEqual(
obj.inertial.origin.rpy, [0, 0, 0],
'Wrong value for link.inertial.origin.rpy, '
'received={}, expected={}'.format(obj.inertial.origin.rpy,
[0, 0, 0]))
# Check collision geometry
self.assertEqual(len(obj.collisions), 1,
'Link should have one collision geometry')
self.assertEqual(
obj.collisions[0].origin.xyz, [4, 5, 6],
'Wrong collision origin.xyz, received={}, expected={}'.format(
obj.collisions[0].origin.xyz, [4, 5, 6]))
self.assertEqual(
obj.collisions[0].origin.rpy, [1, 2, 3],
'Wrong collision origin.rpy, received={}, expected={}'.format(
obj.collisions[0].origin.rpy, [1, 2, 3]))
self.assertIsNotNone(
obj.collisions[0].geometry.cylinder,
'Collision geometry should have a cylinder description')
self.assertEqual(
obj.collisions[0].geometry.cylinder.radius, 0.5,
'Wrong radius for collision cylinder, '
'received={}, expected={}'.format(
obj.collisions[0].geometry.cylinder.radius, 0.5))
self.assertEqual(
obj.collisions[0].geometry.cylinder.length, 0.5,
'Wrong radius for collision cylinder, '
'received={}, expected={}'.format(
obj.collisions[0].geometry.cylinder.length, 0.5))
# Check visual geometry
self.assertEqual(len(obj.visuals), 1,
'Link should have one visual geometry')
self.assertEqual(
obj.visuals[0].origin.xyz, [4, 5, 6],
'Wrong visual origin.xyz, received={}, expected={}'.format(
obj.visuals[0].origin.xyz, [4, 5, 6]))
self.assertEqual(
obj.visuals[0].origin.rpy, [1, 2, 3], 'Wrong visual origin.rpy, '
'received={}, expected={}'.format(obj.visuals[0].origin.rpy,
[1, 2, 3]))
self.assertIsNotNone(
obj.visuals[0].geometry.mesh,
'Collision geometry should have a mesh description')
self.assertEqual(
obj.visuals[0].geometry.mesh.filename, 'mesh',
'Wrong visual mesh filename, received={}, expected={}'.format(
obj.visuals[0].geometry.mesh.filename, 'mesh'))