def test_star_urdf_structure(self):
urdf = create_urdf_element('robot')
# Add origin link
i = 0
urdf.add_link('link_{}'.format(i))
i += 1
for _ in range(3):
for j in range(randint(3, 5)):
urdf.add_link('link_{}'.format(i))
joint_urdf = create_urdf_element('joint')
if j == 0:
joint_urdf.parent.link = 'link_{}'.format(0)
else:
joint_urdf.parent.link = 'link_{}'.format(i - 1)
joint_urdf.child.link = 'link_{}'.format(i)
joint_urdf.origin = Pose.random().to_urdf()
urdf.add_joint('joint_{}'.format(i), joint_urdf)
i += 1
model = SimulationModel.from_urdf(urdf)
self.assertIsNotNone(model)
def test_invalid_disconnected_robot_graph(self):
urdf = create_urdf_element('robot')
for i in range(3):
urdf.add_link('link_{}'.format(i))
with self.assertRaises(ValueError):
SimulationModel.from_urdf(urdf)
def test_convert_from_urdf(self):
# Test correct conversion of joint relative
# poses into absolute link poses
# Test random sets of poses
pose_sets = [[Pose.random() for _ in range(5)],
[Pose.random_position() for _ in range(5)],
[Pose.random_orientation() for _ in range(5)]]
for poses in pose_sets:
urdf = create_urdf_element('robot')
# Create links
for i in range(len(poses) + 1):
urdf.add_link('link_{}'.format(i))
for i in range(len(poses)):
joint_urdf = create_urdf_element('joint')
joint_urdf.parent.link = 'link_{}'.format(i)
joint_urdf.child.link = 'link_{}'.format(i + 1)
joint_urdf.origin = poses[i].to_urdf()
urdf.add_joint('joint_{}'.format(i), joint_urdf)
model = SimulationModel.from_urdf(urdf)
self.assertIsNotNone(model)
cur_pose = None
for i in range(len(poses) + 1):
link = model.get_link_by_name('link_{}'.format(i))
self.assertIsNotNone(link)
if i == 0:
self.assertEqual(link.pose,
Pose(pos=[0, 0, 0], rot=[0, 0, 0]))
else:
if cur_pose is None:
cur_pose = poses[i - 1]
else:
cur_pose = cur_pose + poses[i - 1]
self.check_pose(link, cur_pose.position, cur_pose.rpy)
def test_reset_urdf(self):
for c in get_all_urdf_element_classes():
obj = create_urdf_element(c._NAME)
self.assertIsNotNone(obj)
if len(obj.modes):
for tag in obj.modes:
obj.reset(mode=tag, with_optional_elements=True)
self.assertTrue(obj.is_valid())
else:
print(obj.xml_element_name)
obj.reset(with_optional_elements=True)
self.assertTrue(obj.is_valid())
def test_xml_input_visual_collision(self):
for urdf_tag in ['visual', 'collision']:
obj = create_urdf_element(urdf_tag)
assert obj is not None
obj.origin = create_urdf_element('origin')
obj.origin.random()
obj.geometry = create_urdf_element('geometry')
geometries = ['box', 'cylinder', 'sphere', 'mesh']
for geo_name in geometries:
obj.geometry.reset(mode=geo_name)
obj.geometry.random()
output = subprocess.check_output(
['pcg-urdf2sdf', '--xml',
obj.to_xml_as_str(), '--print'])
urdf = parse_sdf(output.decode('utf-8'))
assert urdf is not None
response_urdf = sdf2urdf(urdf)
assert obj == response_urdf
def test_xml_input_random(self):
urdf_elements = [
'mass', 'child', 'parent', 'origin', 'box', 'cylinder', 'sphere',
'mesh', 'limit', 'inertial', 'inertia', 'dynamics'
]
for urdf_name in urdf_elements:
obj = create_urdf_element(urdf_name)
assert obj is not None
obj.random()
output = subprocess.check_output(
['pcg-urdf2sdf', '--xml',
obj.to_xml_as_str(), '--print'])
urdf = parse_sdf(output.decode('utf-8'))
assert urdf is not None
response_urdf = sdf2urdf(urdf)
assert obj == response_urdf