def test_add_quaternion(self):
p_ref = Pose()
for _ in range(10):
p_target = Pose.random_orientation()
p_result = p_ref + p_target
self.assertEqual(np.sum(p_result.quat - p_target.quat), 0)
def check_pose(self, obj, pos, rpy):
self.assertAlmostEqual(
np.linalg.norm(obj.pose.position - pos), 0, 7,
'Position was not parsed correctly, retrieved={}, '
'expected={}'.format(obj.pose.position.tolist(), pos))
diff = Pose.get_transform(obj.pose.quat, Pose.rpy2quat(*rpy))
self.assertAlmostEqual(
np.sum(diff[0:3]), 0, 7,
'Orientation should be {}, retrieved={}'.format(
Pose.rpy2quat(*rpy), obj.pose.quat))
def test_euler_transformation(self):
for i in range(3):
vec = np.zeros(3)
vec[i] = 2 * np.pi * np.random.random()
p = Pose(rot=Pose.rpy2quat(*vec))
diff = Pose.get_transform(p.quat, Pose.rpy2quat(*vec))
self.assertTrue(np.isclose(diff[0], 0))
self.assertTrue(np.isclose(diff[1], 0))
self.assertTrue(np.isclose(diff[2], 0))
self.assertTrue(np.isclose(diff[3], 1))
def test_add_positions(self):
for _ in range(10):
p1 = Pose.random_position()
p2 = Pose.random_position()
p_result = p1 + p2
self.assertEqual(
np.sum(p1.position + p2.position - p_result.position),
0)
self.assertEqual(p_result.x, p1.x + p2.x)
self.assertEqual(p_result.y, p1.y + p2.y)
self.assertEqual(p_result.z, p1.z + p2.z)
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_to_urdf(self):
for _ in range(10):
p = Pose.random()
urdf = p.to_urdf()
self.assertTrue(
np.isclose(np.sum(np.array(urdf.xyz) - p.position), 0))
self.assertTrue(
np.isclose(np.sum(np.array(urdf.rpy) - np.array(p.rpy)), 0))
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_bounds(self):
box_1 = box_factory(size=[[1, 1, 1]],
mass=1,
use_permutation=True,
name='box_1')[0]
self.assertTrue(box_1.name, 'box_1')
box_2 = box_factory(size=[[1, 1, 1]],
mass=1,
use_permutation=True,
name='box_2')[0]
self.assertTrue(box_2.name, 'box_2')
# Test bounds for different positions of the boxes
box_1.pose = Pose.random_position()
box_2.pose = Pose.random_position()
group = ModelGroup()
group.add_model(box_1.name, box_1)
group.add_model(box_2.name, box_2)
self.assertEqual(group.n_models, 2)
bounds = group.get_bounds()
ref_bounds = numpy.array(
[[
numpy.min([box_1.pose.x, box_2.pose.x]) - 1,
numpy.min([box_1.pose.y, box_2.pose.y]) - 1,
numpy.min([box_1.pose.z, box_2.pose.z]) - 1
],
[
numpy.max([box_1.pose.x, box_2.pose.x]) + 1,
numpy.max([box_1.pose.y, box_2.pose.y]) + 1,
numpy.max([box_1.pose.z, box_2.pose.z]) + 1
]])
self.assertAlmostEqual(numpy.sum(ref_bounds - bounds), 0)
def test_retrieve_model(self):
box = box_factory(size=[[1, 1, 1]],
mass=1,
use_permutation=True,
name='box')[0]
group = ModelGroup(name='group')
group.pose = Pose.random()
box.pose = Pose.random()
group.add_model('box', box)
model = group.get_model('box')
self.assertTrue(box.pose + group.pose, model.pose)
models = group.get_models()
self.assertEqual(len(models), 1)
group_model = group.get_model('box')
self.assertIsNotNone(group_model)
def test_group_init_pose(self):
# Set initial pose with Pose object
pose = Pose.random()
group = ModelGroup(pose=pose)
self.assertEqual(numpy.sum(group.pose.position - pose.position), 0)
self.assertEqual(numpy.sum(group.pose.quat - pose.quat), 0)
# Test invalid pose objects
invalid_poses = [
dict(),
list(), 'asd', '', [0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0], None
]
for pose in invalid_poses:
with self.assertRaises(AssertionError):
group.pose = pose
def test_box_object(self):
box = Box()
self.assertEqual(box.size, [1, 1, 1], 'Default size should be 1, 1, 1')
# Test setting different box sizes
N_TESTS = 10
valid_sizes = [[random() for _ in range(3)] for _ in range(N_TESTS)]
names = [generate_random_string(5) for _ in range(N_TESTS)]
for box_size, box_name in zip(valid_sizes, names):
# Test setting box size on constructor
box = Box(name=box_name, size=box_size)
self.assertEqual(box.size, box_size,
'Size should be {}'.format(box_size))
self.assertEqual(box.name, box_name,
'Name should be {}'.format(box_name))
# Test setting the attribute
box = Box()
box.name = box_name
box.size = box_size
self.assertEqual(box.size, box_size,
'Size should be {}'.format(box_size))
self.assertEqual(box.name, box_name,
'Name should be {}'.format(box_name))
sdf = box.to_sdf('box')
self.assertEqual(sdf.size.value, box_size,
'SDF converted size is invalid')
# Test setting different pose vectors (position + Euler angles)
valid_poses = \
[[random() for _ in range(3)] +
[random() * 2 * np.pi for _ in range(3)] for _ in range(N_TESTS)]
for pose in valid_poses:
box = Box()
box.pose = pose
self.assertEqual(
box.pose.position.tolist(), pose[0:3],
'Box position should be {}, retrieved={}'.format(
pose[0:3], box.pose.position))
ref_q = Pose.rpy2quat(*pose[3::])
q = box.pose.quat
diff = Pose.get_transform(ref_q, q)
self.assertAlmostEqual(
np.sum(diff[0:3]), 0, 7, 'Box orientation element rpy '
'should be {}, retrieved={}'.format(pose[3::], q))
# Test setting different pose vectors (position + quaternion)
valid_positions = [[random() for _ in range(3)]
for _ in range(N_TESTS)]
valid_quat = [Pose.random_orientation().quat for _ in range(N_TESTS)]
for pos, quat in zip(valid_positions, valid_quat):
box = Box()
box.pose = pos + list(quat)
self.assertEqual(
box.pose.position.tolist(), pos,
'Box position should be {}, retrieved={}'.format(
pos, box.pose.position))
ref_q = list(quat)
q = list(box.pose.quat)
self.assertEqual(
ref_q, q, 'Quaternion was assigned incorrectly, '
'expected={}, retrieved={}'.format(ref_q, q))
for value in INVALID_SIZES:
with self.assertRaises(AssertionError):
box = Box(size=value)
invalid_sdf_types = ['a', dict(), None, True, 1]
for t in invalid_sdf_types:
with self.assertRaises(AssertionError):
box.to_sdf(t)
sdf_types = ['box', 'geometry', 'collision', 'visual', 'link', 'model']
for t in sdf_types:
box = Box()
size = [random() for _ in range(3)]
name = generate_random_string(5)
# Setting random size
box.size = size
box.name = name
sdf = box.to_sdf(t)
self.assertEqual(sdf._NAME, t,
'Wrong SDF type in request for {}'.format(t))
self.assertTrue(sdf.is_valid(),
'Invalid SDF structure was returned')
sdf_size = None
if sdf._NAME == 'box':
sdf_size = sdf.size.value
elif sdf._NAME == 'geometry':
sdf_size = sdf.box.size.value
elif sdf._NAME in ['collision', 'visual']:
sdf_size = sdf.geometry.box.size.value
if sdf_size is not None:
self.assertEqual(
sdf_size, size, 'SDF element has the wrong size vector,'
' expected={}, received={}'.format(size, sdf_size))
if sdf._NAME in ['link', 'model']:
self.assertEqual(
sdf.name, name,
'Name property for {} was not set, expected={}, '
'retrieved={}'.format(sdf._NAME, name, sdf.name))
def test_cylinder_object(self):
cylinder = Cylinder()
self.assertEqual(cylinder.radius, 1, 'Default radius must be 1')
self.assertEqual(cylinder.length, 1, 'Default length must be 1')
N_TESTS = 10
valid_sizes = [[random(), random()] for _ in range(N_TESTS)]
names = [generate_random_string(5) for _ in range(N_TESTS)]
for cyl_size, cyl_name in zip(valid_sizes, names):
# Testing setting cylinder size on constructor
cylinder = Cylinder(name=cyl_name,
radius=cyl_size[0],
length=cyl_size[1])
self.assertEqual(cylinder.radius, cyl_size[0],
'Radius should be {}'.format(cyl_size[0]))
self.assertEqual(cylinder.length, cyl_size[1],
'Length should be {}'.format(cyl_size[1]))
self.assertEqual(cylinder.name, cyl_name,
'Name should be {}'.format(cyl_name))
# Test setting attribute
cylinder = Cylinder()
cylinder.name = cyl_name
cylinder.radius = cyl_size[0]
cylinder.length = cyl_size[1]
self.assertEqual(cylinder.radius, cyl_size[0],
'Radius should be {}'.format(cyl_size[0]))
self.assertEqual(cylinder.length, cyl_size[1],
'Length should be {}'.format(cyl_size[1]))
self.assertEqual(cylinder.name, cyl_name,
'Name should be {}'.format(cyl_name))
sdf = cylinder.to_sdf('cylinder')
self.assertEqual(sdf.radius.value, cyl_size[0],
'SDF converted radius is invalid')
self.assertEqual(sdf.length.value, cyl_size[1],
'SDF converted length is invalid')
# Test setting different pose vectors (position + Euler angles)
valid_poses = \
[[random() for _ in range(3)] +
[random() * 2 * np.pi for _ in range(3)] for _ in range(N_TESTS)]
for pose in valid_poses:
cylinder = Cylinder()
cylinder.pose = pose
self.assertEqual(
cylinder.pose.position.tolist(), pose[0:3],
'Box position should be {}, retrieved={}'.format(
pose[0:3], cylinder.pose.position))
ref_q = Pose.rpy2quat(*pose[3::])
q = cylinder.pose.quat
diff = Pose.get_transform(ref_q, q)
self.assertAlmostEqual(
np.sum(diff[0:3]), 0, 7,
'Box orientation element rpy should be {}, '
'retrieved={}'.format(pose[3::], q))
# Test setting different pose vectors (position + quaternion)
valid_positions = [[random() for _ in range(3)]
for _ in range(N_TESTS)]
valid_quat = [Pose.random_orientation().quat for _ in range(N_TESTS)]
for pos, quat in zip(valid_positions, valid_quat):
cylinder = Cylinder()
cylinder.pose = pos + list(quat)
self.assertEqual(
cylinder.pose.position.tolist(), pos,
'Box position should be {}, retrieved={}'.format(
pos, cylinder.pose.position))
ref_q = list(quat)
q = list(cylinder.pose.quat)
self.assertEqual(
ref_q, q, 'Quaternion was assigned incorrectly, '
'expected={}, retrieved={}'.format(ref_q, q))
sdf_types = [
'cylinder', 'geometry', 'collision', 'visual', 'link', 'model'
]
for t in sdf_types:
cylinder = Cylinder()
size = [random(), random()]
name = generate_random_string(5)
# Setting random size
cylinder.radius = size[0]
cylinder.length = size[1]
cylinder.name = name
sdf = cylinder.to_sdf(t)
self.assertEqual(sdf._NAME, t,
'Wrong SDF type in request for {}'.format(t))
self.assertTrue(sdf.is_valid(),
'Invalid SDF structure was returned')
sdf_radius = None
sdf_length = None
if sdf._NAME == 'cylinder':
sdf_radius = sdf.radius.value
sdf_length = sdf.length.value
elif sdf._NAME == 'geometry':
sdf_radius = sdf.cylinder.radius.value
sdf_length = sdf.cylinder.length.value
elif sdf._NAME in ['collision', 'visual']:
sdf_radius = sdf.geometry.cylinder.radius.value
sdf_length = sdf.geometry.cylinder.length.value
if sdf_radius is not None and sdf_length is not None:
self.assertEqual(
sdf_radius, size[0], 'SDF element has the wrong radius,'
' expected={}, received={}'.format(size[0], sdf_radius))
self.assertEqual(
sdf_length, size[1], 'SDF element has the wrong length,'
' expected={}, received={}'.format(size[1], sdf_length))
if sdf._NAME in ['link', 'model']:
self.assertEqual(
sdf.name, name,
'Name property for {} was not set, expected={}, '
'retrieved={}'.format(sdf._NAME, name, sdf.name))
def test_to_sdf(self):
for _ in range(10):
p = Pose.random()
sdf = p.to_sdf()
self.assertEqual(sdf.value, p.position.tolist() + p.rpy)
def test_export_models_to_gazebo_model(self):
models = [
SimulationModel(name=generate_random_string(5)) for _ in range(2)
]
model_names = [obj.name for obj in models]
sdf_elements = [model.to_sdf() for model in models]
group = ModelGroup.from_sdf(sdf_elements)
group.name = generate_random_string(5)
group.pose = Pose.random()
self.assertIsNotNone(group)
self.assertEqual(group.n_models, len(models))
self.assertEqual(group.n_lights, 0)
# Convert to separate SDF elements
sdf_models, sdf_lights, sdf_includes = group.to_sdf()
self.assertEqual(len(sdf_models), len(models))
for tag in sdf_models:
self.assertIn(
sdf_models[tag].name.replace('{}/'.format(group.name), ''),
model_names)
self.assertEqual(len(sdf_lights), 0)
self.assertEqual(len(sdf_includes), 0)
# Convert to default Gazebo model
self.assertIsNotNone(group.to_gazebo_model())
default_dir = os.path.join(os.path.expanduser('~'), '.gazebo',
'models')
model_dir = os.path.join(default_dir, group.name)
# Check if all model files were created
self.assertTrue(os.path.isdir(model_dir))
self.assertTrue(os.path.isfile(os.path.join(model_dir,
'model.config')))
self.assertTrue(os.path.isfile(os.path.join(model_dir, 'model.sdf')))
# Parse model config file
sdf_config = parse_sdf_config(os.path.join(model_dir, 'model.config'))
self.assertIsNotNone(sdf_config)
# Get name of current user, used in default model metadata input
username = getpass.getuser()
self.assertEqual(sdf_config.name.value, group.name)
self.assertEqual(len(sdf_config.authors), 1)
self.assertEqual(sdf_config.authors[0].name.value, username)
self.assertEqual(sdf_config.authors[0].email.value,
'{}@email.com'.format(username))
self.assertEqual(sdf_config.description.value, '')
self.assertEqual(sdf_config.version.value, '1.6')
self.assertEqual(len(sdf_config.sdfs), 1)
self.assertEqual(sdf_config.sdfs[0].value, 'model.sdf')
self.assertEqual(sdf_config.sdfs[0].version, '1.6')
# Parse the SDF file
sdf = parse_sdf(os.path.join(model_dir, 'model.sdf'))
self.assertIsNotNone(sdf)
self.assertEqual(sdf.xml_element_name, 'sdf')
self.assertIsNotNone(sdf.models)
self.assertEqual(len(sdf.models), 1)
for i, j in zip(sdf.models[0].pose.value, group.pose.to_sdf().value):
self.assertTrue(numpy.isclose(i, j))
self.assertEqual(len(sdf.models[0].models), len(models))
for i in range(len(sdf.models[0].models)):
self.assertEqual(sdf.models[0].models[i].pose.value,
[0 for _ in range(6)])
self.assertIn(sdf.models[0].models[i].name, model_names)
# Delete generated Gazebo model directory
shutil.rmtree(model_dir)
# Rename group
group.name = generate_random_string(5)
# Export group with individually exported models
self.assertIsNotNone(group.to_gazebo_model(nested=False))
# Check if model folders exist
self.assertTrue(os.path.isdir(os.path.join(default_dir, group.name)))
for name in model_names:
self.assertTrue(os.path.isdir(os.path.join(default_dir, name)))
# Parse the SDF file for the main model
sdf = parse_sdf(os.path.join(default_dir, group.name, 'model.sdf'))
self.assertIsNotNone(sdf)
self.assertEqual(sdf.xml_element_name, 'sdf')
self.assertIsNotNone(sdf.models)
self.assertEqual(len(sdf.models), 1)
self.assertIsNone(sdf.models[0].models)
self.assertIsNotNone(sdf.models[0].includes)
self.assertEqual(len(sdf.models[0].includes), len(models))
for i, j in zip(sdf.models[0].pose.value, group.pose.to_sdf().value):
self.assertTrue(numpy.isclose(i, j))
for i in range(len(sdf.models[0].includes)):
self.assertEqual(sdf.models[0].includes[i].pose.value,
[0 for _ in range(6)])
self.assertIn(sdf.models[0].includes[i].name.value, model_names)
self.assertEqual(
sdf.models[0].includes[i].uri.value,
'model://{}'.format(sdf.models[0].includes[i].name.value))
shutil.rmtree(os.path.join(default_dir, group.name))
for name in model_names:
shutil.rmtree(os.path.join(default_dir, name))