def test_load_from_sdf(self):
lights = [Light(name=generate_random_string(5)) for _ in range(3)]
models = [
SimulationModel(name=generate_random_string(5)) for _ in range(2)
]
# Test import from a list of SDF elements
sdf_elements = [light.to_sdf() for light in lights] + \
[model.to_sdf() for model in models]
group = ModelGroup.from_sdf(sdf_elements)
self.assertIsNotNone(group)
self.assertEqual(group.n_models, len(models))
self.assertEqual(group.n_lights, len(lights))
# Test import from one single SDF element
sdf = create_sdf_element('sdf')
for model in models:
sdf.add_model(model.name, model.to_sdf())
for light in lights:
sdf.add_light(light.name, light.to_sdf())
group = ModelGroup.from_sdf(sdf)
self.assertIsNotNone(group)
self.assertEqual(group.n_models, len(models))
self.assertEqual(group.n_lights, len(lights))
def test_init_from_world_sdf(self):
generator = WorldGenerator(
world_file=os.path.join(os.path.dirname(os.path.abspath(__file__)),
'worlds', 'test_pcg_example.world'))
self.assertIsNotNone(generator.world)
self.assertIsNotNone(generator.world.n_models, 2)
self.assertIsNotNone(generator.world.n_lights, 1)
self.assertIn('box', generator.world.models)
self.assertIn('ground_plane', generator.world.models)
self.assertIn('sun', generator.world.lights)
box_name = 'box_' + generate_random_string(5)
random_box = dict(type='box',
args=dict(
size="__import__('pcg_gazebo').random.rand(3)",
mass="__import__('pcg_gazebo').random.rand()",
name=box_name))
self.assertTrue(
generator.add_asset(random_box, tag=box_name, type='factory'))
workspace_name = generate_random_string(5)
self.assertTrue(
generator.add_constraint(name=workspace_name,
type='workspace',
geometry_type='circle',
radius=100,
center=[0, 0]))
num_models = dict()
num_models[box_name] = random.randint(2, 5)
engine_name = generate_random_string(5)
self.assertTrue(
generator.add_engine(tag=engine_name,
engine_name='random_pose',
models=list(num_models.keys()),
model_picker='random',
no_collision=True,
max_num=num_models,
policies=[
dict(models=list(num_models.keys()),
config=[
dict(dofs=['x', 'y'],
tag='workspace',
workspace=workspace_name)
])
]))
generator.run_engines(attach_models=True)
self.assertEqual(generator.world.n_models, num_models[box_name] + 2)
self.assertIn('box', generator.world.models)
self.assertIn('ground_plane', generator.world.models)
self.assertIn('sun', generator.world.lights)
def test_convert_wrong_model_sdf(self):
lights = [Light(name=generate_random_string(5)) for _ in range(3)]
models = [
SimulationModel(name=generate_random_string(5)) for _ in range(2)
]
sdf_elements = [light.to_sdf() for light in lights] + \
[model.to_sdf() for model in models]
group = ModelGroup.from_sdf(sdf_elements)
self.assertIsNotNone(group)
self.assertEqual(group.n_models, len(models))
self.assertEqual(group.n_lights, len(lights))
self.assertIsNone(group.to_sdf(type='model'))
def test_roulette_picker_no_max_num(self):
if sys.version_info.major == 2:
return
max_num = dict()
fitness = dict()
counter = dict()
for i in range(5):
name = generate_random_string(5)
max_num[name] = None
fitness[name] = random.randint(1, 10)
counter[name] = 0
picker = create_picker(tag='roulette',
items=list(max_num.keys()),
max_num=max_num,
fitness=fitness)
self.assertIsNotNone(picker)
total_items = 100
for _ in range(total_items):
output = picker.get_selection()
self.assertIsNotNone(output)
self.assertIn(output, counter.keys())
counter[output] += 1
def test_heightmap_generator_to_gazebo_model(self):
hg = HeightmapGenerator()
hg.biome = WhittakerBiome()
model_name = generate_random_string(10)
output_dir = os.path.join(os.path.expanduser('~'), '.gazebo', 'models')
if not os.path.isdir(output_dir):
os.makedirs(output_dir)
model = hg.as_model()
model.name = model_name
model.to_gazebo_model(output_dir=output_dir, copy_resources=True)
self.assertTrue(os.path.isdir(os.path.join(output_dir, model_name)))
sdf = parse_sdf(os.path.join(output_dir, model_name, 'model.sdf'))
self.assertIsNotNone(sdf)
self.assertIsNotNone(sdf.models[0])
heightmap = \
sdf.models[0].links[0].visuals[0].geometry.heightmap
self.assertIsNotNone(heightmap.uri)
self.assertIn('model://{}'.format(model_name), heightmap.uri.value)
self.assertIn('/materials/textures/', heightmap.uri.value)
self.assertTrue(heightmap.uri.value.endswith('.png'))
heightmap = \
sdf.models[0].links[0].collisions[0].geometry.heightmap
self.assertIsNotNone(heightmap.uri)
self.assertIn('model://{}'.format(model_name), heightmap.uri.value)
self.assertIn('/materials/textures/', heightmap.uri.value)
self.assertTrue(heightmap.uri.value.endswith('.png'))
shutil.rmtree(os.path.join(output_dir, model_name))
def test_load_gazebo_models(self):
load_gazebo_models()
self.assertGreater(len(get_gazebo_model_names()), 1,
'There should be at least one model in the list')
root_test_model_path = os.path.join(CUR_DIR, 'gazebo_models')
LIST_TEST_MODELS = dict(test_static_model=dict(path=os.path.join(
root_test_model_path, 'test_static_model'),
sdf=['model.sdf']))
for test_model in LIST_TEST_MODELS:
self.assertIn(test_model, get_gazebo_model_names(),
'Model {} not found in list of Gazebo models')
self.assertEqual(
get_gazebo_model_path(test_model),
LIST_TEST_MODELS[test_model]['path'],
'Invalid path returned for model {}'.format(test_model))
self.assertEqual(
get_gazebo_model_sdf_filenames(test_model),
LIST_TEST_MODELS[test_model]['sdf'],
'Invalid list of SDF files returned '
'for model {}'.format(test_model))
INVALID_MODELS = [generate_random_string(4) for _ in range(10)]
for model_name in INVALID_MODELS:
self.assertIsNone(
get_gazebo_model_path(model_name),
'Function should return None for non-existent model')
def test_add_model_groups(self):
box_factory_config = dict(
type='box_factory',
args=dict(
size="__import__('numpy').random.random((2, 3))",
use_permutation=True,
name='box',
color='xkcd'
)
)
model_sdfs = config2models(box_factory_config)
group_name = generate_random_string(5)
group = ModelGroup(name=group_name)
for name, sdf in zip(['box_0', 'box_1'], model_sdfs):
group.add_model(name, SimulationModel.from_sdf(sdf))
manager = AssetsManager.get_instance()
manager.add(group, group_name)
self.assertIn(group_name, manager.tags)
self.assertIsInstance(manager.get(group_name), ModelGroup)
manager.reset()
self.assertGreaterEqual(len(manager.tags), 0)
def test_random_picker(self):
if sys.version_info.major == 2:
return
max_num = dict()
counter = dict()
total_items = 0
for i in range(5):
name = generate_random_string(5)
max_num[name] = random.randint(1, 10)
counter[name] = 0
total_items += max_num[name]
picker = create_picker(tag='random',
items=list(max_num.keys()),
max_num=max_num)
self.assertIsNotNone(picker)
output = picker.get_selection()
while output is not None:
self.assertIsNotNone(output)
self.assertIn(output, counter.keys())
counter[output] += 1
temp_counter = 0
for tag in counter:
temp_counter += counter[tag]
self.assertLessEqual(temp_counter, total_items)
output = picker.get_selection()
temp_counter = 0
for tag in counter:
temp_counter += counter[tag]
self.assertEqual(temp_counter, total_items)
def generate_string_test_obj(name, value=None):
if value is None:
value = generate_random_string(5)
sdf_str = '<{}>{}</{}>'.format(name, value, name)
expected_sdf = create_sdf_element(name)
self.assertIsNotNone(expected_sdf, '{} returned None'.format(name))
expected_sdf.value = value
return sdf_str, expected_sdf
def create_random_box(self):
box = Box()
size = [random() for _ in range(3)]
name = generate_random_string(5)
# Setting random size
box.size = size
box.name = name
box.add_inertial(random())
return box
def test_add_mesh_file(self):
mesh_manager = MeshManager.get_instance()
mesh_manager.reset()
self.assertEqual(len(mesh_manager.tags), 0)
tag_1 = generate_random_string(5)
tag = mesh_manager.add(
tag=tag_1, filename=os.path.join(CWD, 'meshes', 'monkey.stl'))
self.assertEqual(tag, tag_1)
tag_2 = generate_random_string(5)
tag = mesh_manager.add(
tag=tag_2, filename=os.path.join(CWD, 'meshes', 'cube.stl'))
self.assertEqual(tag, tag_2)
self.assertEqual(
mesh_manager.add(filename=os.path.join(CWD, 'meshes', 'cube.stl')),
tag_2)
def test_group_name(self):
valid_name = generate_random_string(size=3)
group = ModelGroup(name=valid_name)
self.assertEqual(valid_name, group.name)
invalid_names = [10, None, dict(), list(), '']
for name in invalid_names:
with self.assertRaises(AssertionError):
group = ModelGroup(name=name)
def test_convert_lights_models_group_to_sdfs(self):
lights = [Light(name=generate_random_string(5)) for _ in range(3)]
light_names = [obj.name for obj in lights]
models = [
SimulationModel(name=generate_random_string(5)) for _ in range(2)
]
model_names = [obj.name for obj in models]
sdf_elements = [light.to_sdf() for light in lights] + \
[model.to_sdf() for model in models]
group = ModelGroup.from_sdf(sdf_elements)
group.name = generate_random_string(5)
self.assertIsNotNone(group)
self.assertEqual(group.n_models, len(models))
self.assertEqual(group.n_lights, len(lights))
# 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), len(lights))
for tag in sdf_lights:
self.assertIn(
sdf_lights[tag].name.replace('{}/'.format(group.name), ''),
light_names)
self.assertEqual(len(sdf_includes), 0)
# Convert to world
sdf_world = group.to_sdf(type='world')
self.assertIsNotNone(sdf_world)
self.assertEqual(sdf_world.xml_element_name, 'world')
self.assertIsNotNone(sdf_world.models)
self.assertEqual(len(sdf_world.models), 1)
self.assertEqual(len(sdf_world.models[0].models), len(models))
for i in range(len(sdf_world.models[0].models)):
self.assertIn(sdf_world.models[0].models[i].name, model_names)
self.assertEqual(len(sdf_world.lights), len(lights))
for i in range(len(sdf_world.lights)):
self.assertIn(sdf_world.lights[i].name, light_names)
def test_add_rule_to_manager(self):
rm = RulesManager.get_instance()
rules = [
UNIFORM_RULE_SAMPLE, FIXED_VALUE_RULE_SAMPLE, FROM_SET_RULE_SAMPLE,
RANDOM_RULE_SAMPLE, WORKSPACE_RULE_SAMPLE
]
for rule in rules:
name = generate_random_string(5)
rm.add(name=name, **rule)
self.assertIn(name, rm.tags)
self.assertEqual(len(rm.tags), len(rules))
def test_convert_models_group_to_sdf(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)
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 world
sdf_world = group.to_sdf(type='world')
self.assertIsNotNone(sdf_world)
self.assertEqual(sdf_world.xml_element_name, 'world')
self.assertIsNotNone(sdf_world.models)
self.assertEqual(len(sdf_world.models), 1)
self.assertEqual(len(sdf_world.models[0].models), len(models))
for i in range(len(sdf_world.models[0].models)):
self.assertIn(sdf_world.models[0].models[i].name, model_names)
# Convert to SDF with model group
sdf = group.to_sdf(type='sdf')
self.assertIsNotNone(sdf)
self.assertEqual(sdf.xml_element_name, 'sdf')
self.assertIsNotNone(sdf_world.models)
self.assertEqual(len(sdf_world.models), 1)
self.assertEqual(len(sdf_world.models[0].models), len(models))
for i in range(len(sdf_world.models[0].models)):
self.assertIn(sdf_world.models[0].models[i].name, model_names)
# Convert to model with nested models
sdf_model = group.to_sdf(type='model')
self.assertIsNotNone(sdf_model)
self.assertIsNotNone(sdf_model.models)
self.assertEqual(len(sdf_model.models), len(models))
for i in range(len(sdf_model.models)):
self.assertIn(sdf_model.models[i].name, model_names)
def test_add_model(self):
generator = WorldGenerator()
name = generate_random_string(5)
obj = SimulationModel(name)
obj.add_cuboid_link(link_name='link',
size=[random.rand() for _ in range(3)],
mass=random.rand())
pose = [random.rand() for _ in range(6)]
generator.add_model(obj, [pose])
self.assertTrue(generator.assets.is_model(name))
self.assertTrue(generator.run_engines())
self.assertIn(name, generator.world.models)
def test_add_simulation_models(self):
box_config = dict(
type='box',
args=dict(
size=[2, 2, 2],
mass=10,
name='box',
pose=[0, 0, 1, 0, 0, 0],
color='random'
)
)
model = SimulationModel.from_sdf(config2models(box_config)[0])
name = generate_random_string(5)
manager = AssetsManager.get_instance()
manager.add(model, name)
self.assertIn(name, manager.tags)
self.assertIsInstance(manager.get(name), SimulationModel)
manager.reset()
self.assertGreaterEqual(len(manager.tags), 0)
def test_add_factory_config(self):
manager = AssetsManager.get_instance()
added_models = list()
# Add box factory
box_config = dict(
type='box',
args=dict(
size=[2, 2, 2],
mass=10,
name='box',
pose=[0, 0, 1, 0, 0, 0],
color='random'
)
)
added_models.append(generate_random_string(5))
manager.add(box_config, tag=added_models[-1])
self.assertIn(added_models[-1], manager.tags)
model = manager.get(added_models[-1])
self.assertIsNotNone(model)
self.assertIn('<box>', model.to_sdf().to_xml_as_str())
self.assertEqual(len(model.links), 1)
# Add cylinder factory
cylinder_config = dict(
type='cylinder',
args=dict(
radius=3,
length=2,
mass=10,
name='cylinder',
pose=[0, 0, 1, 0, 0, 0]
)
)
added_models.append(generate_random_string(5))
manager.add(cylinder_config, tag=added_models[-1])
self.assertIn(added_models[-1], manager.tags)
model = manager.get(added_models[-1])
self.assertIsNotNone(model)
self.assertIn('<cylinder>', model.to_sdf().to_xml_as_str())
self.assertEqual(len(model.links), 1)
# Add sphere factory
sphere_config = dict(
type='sphere',
args=dict(
radius=3,
name='sphere',
mass=10,
pose=[0, 0, 1.5, 0, 0, 0]
)
)
added_models.append(generate_random_string(5))
manager.add(sphere_config, tag=added_models[-1])
self.assertIn(added_models[-1], manager.tags)
model = manager.get(added_models[-1])
self.assertIsNotNone(model)
self.assertIn('<sphere>', model.to_sdf().to_xml_as_str())
self.assertEqual(len(model.links), 1)
# Add mesh factory
mesh_config = dict(
type='mesh',
args=dict(
visual_mesh=os.path.join(CUR_DIR, 'meshes', 'monkey.stl'),
visual_mesh_scale=[1, 1, 1],
use_approximated_collision=True,
name='mesh',
color='xkcd'
)
)
added_models.append(generate_random_string(5))
manager.add(mesh_config, tag=added_models[-1])
self.assertIn(added_models[-1], manager.tags)
model = manager.get(added_models[-1])
self.assertIsNotNone(model)
self.assertIn('<mesh>', model.to_sdf().to_xml_as_str())
self.assertEqual(len(model.links), 1)
manager.reset()
self.assertGreaterEqual(len(manager.tags), 0)
def test_multiple_engines(self):
generator = WorldGenerator()
box_name = 'box_' + generate_random_string(5)
random_box = dict(type='box',
args=dict(
size="__import__('pcg_gazebo').random.rand(3)",
mass="__import__('pcg_gazebo').random.rand()",
name=box_name))
self.assertTrue(
generator.add_asset(random_box, tag=box_name, type='factory'))
self.assertIn(box_name, generator.assets.tags)
cyl_name = 'cyl_' + generate_random_string(5)
random_cyl = dict(type='cylinder',
args=dict(
radius="__import__('pcg_gazebo').random.rand()",
length="__import__('pcg_gazebo').random.rand()",
name=cyl_name))
self.assertTrue(
generator.add_asset(random_cyl, tag=cyl_name, type='factory'))
self.assertIn(cyl_name, generator.assets.tags)
sphere_name = 'sphere_' + generate_random_string(5)
random_sphere = dict(
type='sphere',
args=dict(radius="__import__('pcg_gazebo').random.rand()",
name=sphere_name))
self.assertTrue(
generator.add_asset(random_sphere, tag=sphere_name,
type='factory'))
self.assertIn(sphere_name, generator.assets.tags)
workspace_name = generate_random_string(5)
self.assertTrue(
generator.add_constraint(name=workspace_name,
type='workspace',
geometry_type='circle',
radius=100,
center=[0, 0]))
models = [box_name, cyl_name, sphere_name]
total_models = 0
for item in models:
max_num = dict()
max_num[item] = random.randint(1, 3)
total_models += max_num[item]
self.assertTrue(
generator.add_engine(tag=item + '_engine',
engine_name='random_pose',
models=[item],
model_picker='random',
no_collision=True,
max_num=max_num,
policies=[
dict(models=[item],
config=[
dict(
dofs=['x', 'y'],
tag='workspace',
workspace=workspace_name)
])
]))
self.assertTrue(generator.run_engines())
self.assertEqual(len(generator.world.models), total_models)
# See the License for the specific language governing permissions and
# limitations under the License.
import os
import sys
import unittest
import numpy as np
from pcg_gazebo.utils import generate_random_string
from pcg_gazebo.generators import ModelGroupGenerator
from pcg_gazebo.generators.creators import box_factory
from pcg_gazebo import random
BOX_MODEL = box_factory(size=[[1, 1, 1]],
mass=1,
use_permutation=True,
name='box')[0]
BOX_MODEL.name = 'box_' + generate_random_string(5)
BOX_FLOOR_MODEL = box_factory(size=[[1, 1, 0.01]],
mass=1,
use_permutation=True,
name='box_floor')[0]
BOX_FLOOR_MODEL.name = 'box_floor_' + generate_random_string(5)
FIXED_ENGINE = dict(tag='add_fixed_models',
engine_name='fixed_pose',
models=[BOX_FLOOR_MODEL.name],
poses=[[0, 0, 0, 0, 0, 0]])
NUM_BOXES = dict()
NUM_BOXES[BOX_MODEL.name] = 2
def test_export_model_to_gazebo_model(self):
box = self.create_random_box()
model = SimulationModel.from_sdf(box.to_sdf())
self.assertIsNotNone(model)
# Export Gazebo model with default parameters
model.to_gazebo_model()
default_dir = os.path.join(os.path.expanduser('~'), '.gazebo',
'models')
model_dir = os.path.join(default_dir, box.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, box.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')
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)
# Export Gazebo model with default parameters
author = generate_random_string(5)
email = generate_random_string(5)
description = generate_random_string(10)
model_metaname = generate_random_string(10)
model.to_gazebo_model(author=author,
email=email,
description=description,
model_metaname=model_metaname,
overwrite=True)
# 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)
self.assertEqual(sdf_config.name.value, model_metaname)
self.assertEqual(len(sdf_config.authors), 1)
self.assertEqual(sdf_config.authors[0].name.value, author)
self.assertEqual(sdf_config.authors[0].email.value, email)
self.assertEqual(sdf_config.description.value, description)
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)
# Delete generated Gazebo model directory
shutil.rmtree(model_dir)
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_sphere_object(self):
sphere = Sphere()
self.assertEqual(sphere.radius, 1, 'Default radius must be 1')
N_TESTS = 10
valid_radius = [random() for _ in range(10)]
names = [generate_random_string(5) for _ in range(N_TESTS)]
for s_radius, s_name in zip(valid_radius, names):
sphere = Sphere(name=s_name, radius=s_radius)
self.assertEqual(sphere.radius, s_radius,
'Radius should be {}'.format(s_radius))
self.assertEqual(sphere.name, s_name,
'Name should be {}'.format(s_name))
# Test setting attributes
sphere = Sphere()
sphere.name = s_name
sphere.radius = s_radius
self.assertEqual(sphere.radius, s_radius,
'Radius should be {}'.format(s_radius))
self.assertEqual(sphere.name, s_name,
'Name should be {}'.format(s_name))
sdf = sphere.to_sdf('sphere')
self.assertEqual(sdf.radius.value, s_radius,
'SDF converted radius is invalid')
# Test setting different pose vectors (position + quaternion)
valid_positions = [[random() for _ in range(3)]
for _ in range(N_TESTS)]
for pos in valid_positions:
sphere = Sphere()
sphere.pose = pos + [0.0 for _ in range(3)]
self.assertEqual(
sphere.pose.position.tolist(), pos,
'Sphere position should be {}, retrieved={}'.format(
pos, sphere.pose.position))
sdf_types = [
'sphere', 'geometry', 'collision', 'visual', 'link', 'model'
]
for t in sdf_types:
sphere = Sphere()
radius = random()
name = generate_random_string(5)
# Setting random size
sphere.radius = radius
sphere.name = name
sdf = sphere.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
if sdf._NAME == 'sphere':
sdf_radius = sdf.radius.value
elif sdf._NAME == 'geometry':
sdf_radius = sdf.sphere.radius.value
elif sdf._NAME in ['collision', 'visual']:
sdf_radius = sdf.geometry.sphere.radius.value
if sdf_radius is not None:
self.assertEqual(
sdf_radius, radius, 'SDF element has the wrong radius,'
' expected={}, received={}'.format(radius, sdf_radius))
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_generator_seed(self):
if sys.version_info[0] < 3:
return
generator = ModelGroupGenerator()
# Set random generator's seed
generator.seed = np.random.randint(low=0, high=10000)
box_name = 'box_' + generate_random_string(5)
random_box = dict(type='box',
args=dict(
size="__import__('pcg_gazebo').random.rand(3)",
mass="__import__('pcg_gazebo').random.rand()",
name=box_name))
self.assertTrue(
generator.add_asset(random_box, tag=box_name, type='factory'))
cylinder_name = 'cyl_' + generate_random_string(5)
random_cyl = dict(type='cylinder',
args=dict(
radius="__import__('pcg_gazebo').random.rand()",
length="__import__('pcg_gazebo').random.rand()",
mass="__import__('pcg_gazebo').random.rand()",
name=cylinder_name))
self.assertTrue(generator.add_asset(random_cyl, tag=cylinder_name))
sphere_name = 'sphere_' + generate_random_string(5)
random_sphere = dict(
type='sphere',
args=dict(radius="__import__('pcg_gazebo').random.rand()",
mass="__import__('pcg_gazebo').random.rand()",
name=sphere_name))
self.assertTrue(generator.add_asset(random_sphere, tag=sphere_name))
workspace_name = generate_random_string(5)
self.assertTrue(
generator.add_constraint(name=workspace_name,
type='workspace',
geometry_type='circle',
radius=100,
center=[0, 0]))
num_models = dict()
num_models[box_name] = random.randint(1, 3)
num_models[cylinder_name] = random.randint(1, 3)
num_models[sphere_name] = random.randint(1, 3)
total_num_models = 0
for tag in num_models:
total_num_models += num_models[tag]
engine_name = generate_random_string(5)
self.assertTrue(
generator.add_engine(
tag=engine_name,
engine_name='random_pose',
models=list(num_models.keys()),
model_picker='random',
no_collision=True,
max_num=num_models,
policies=[
dict(models=list(num_models.keys()),
config=[
dict(dofs=['x', 'y'],
tag='workspace',
workspace=workspace_name),
dict(dofs=['roll', 'pitch', 'yaw'],
tag='uniform',
min=-2 * np.pi,
max=2 * np.pi)
])
]))
ref = generator.run('test')
self.assertIsNotNone(ref)
self.assertEqual(ref.n_models, total_num_models)
for _ in range(3):
model = generator.run('test')
self.assertIsNotNone(model)
self.assertEqual(ref.to_sdf('model'), model.to_sdf('model'))
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))
def test_joint(self):
# Test assertion error at joint constructor with missing arguments
joint_args = [
dict(name=None,
parent=generate_random_string(3),
child=generate_random_string(3)),
dict(name=generate_random_string(3),
parent=None,
child=generate_random_string(3)),
dict(name=generate_random_string(3),
parent=generate_random_string(3),
child=None),
dict(name=generate_random_string(3),
parent=generate_random_string(3),
child=generate_random_string(3),
joint_type='abc')
]
for item in joint_args:
with self.assertRaises(AssertionError):
joint = Joint(**item)
joint_types = [
'revolute', 'revolute2', 'gearbox', 'prismatic', 'ball', 'screw',
'universal', 'fixed'
]
N_TESTS = 10
valid_inputs = [
dict(name=generate_random_string(3),
parent=generate_random_string(3),
child=generate_random_string(3),
joint_type=choice(joint_types)) for _ in range(N_TESTS)
]
for joint_input in valid_inputs:
joint = Joint(**joint_input)
self.assertEqual(
joint.name, joint_input['name'],
'Joint name was not set, expected={}, '
'retrieved={}'.format(joint_input['name'], joint.name))
self.assertEqual(
joint.parent, joint_input['parent'],
'Joint parent name was not set, expected={}, '
'retrieved={}'.format(joint_input['parent'], joint.parent))
self.assertEqual(
joint.child, joint_input['child'],
'Joint child name was not set, expected={}, '
'retrieved={}'.format(joint_input['child'], joint.child))
self.assertEqual(
joint.type, joint_input['joint_type'],
'Joint type was not set, expected={}, '
'retrieved={}'.format(joint_input['joint_type'], joint.type))
# Testing parameter setting for fixed joints
joint = Joint(name=generate_random_string(3),
parent=generate_random_string(3),
child=generate_random_string(3),
joint_type='fixed')
self.assertFalse(joint.set_axis_xyz([0, 0, 1]),
'Axis vector should be ignored for fixed joints')
self.assertFalse(
joint.set_axis_limits(lower=-1 * random(), upper=random()),
'Fixed joints have no limits')
self.assertFalse(
joint.set_axis_dynamics(damping=random(), friction=random()),
'Fixed joints have no dynamic parameters')
def test_full_config_from_dict(self):
wg_config = dict(
assets=dict(assets=[
dict(description=BOX_MODEL),
dict(description=BOX_FLOOR_MODEL), BOX_FACTORY_INPUT,
TEST_MODEL_GROUP_GENERATOR
],
ground_plane=['box_floor']),
engines=[
dict(tag='add_box_factory',
engine_name='fixed_pose',
models=['box_factory'],
poses=[[0, 0, 0, 0, 0, 0]]),
dict(tag='add_box_floor',
engine_name='fixed_pose',
models=['box_floor'],
poses=[[1, 1, 1, 0, 0, 0]]),
dict(tag='add_model_group_generator',
engine_name='fixed_pose',
models=['box_generated'],
poses=[[10, 10, 10, 0, 0, 0]]),
dict(tag='add_boxes',
engine_name='random_pose',
models=['box'],
model_picker='random',
max_area=0.9,
no_collision=True,
max_num=dict(box=5),
policies=[
dict(models=['box'],
config=[
dict(dofs=['x', 'y'],
policy=dict(name='workspace',
args='boxes_workspace'))
])
]),
dict(tag='add_generatored_model_groups_random',
engine_name='random_pose',
models=['box_generated'],
model_picker='random',
max_area=0.9,
no_collision=True,
max_num=dict(box_generated=1),
policies=[
dict(models=['box_generated'],
config=[
dict(dofs=['x', 'y'],
policy=dict(
name='workspace',
args='generated_boxes_workspace')),
dict(dofs=['z', 'roll', 'pitch', 'yaw'],
policy=dict(name='value', args=0))
])
])
],
constraints=[
dict(name='boxes_workspace',
type='workspace',
frame='world',
geometry_type='area',
points=[[-6, -4, 0], [-3, -4, 0], [-3, 0, 0], [-6, 0,
0]]),
dict(name='generated_boxes_workspace',
type='workspace',
frame='world',
geometry_type='area',
points=[[-100, -100, 0], [100, -100, 0], [100, 100, 0],
[-100, 100, 0]])
])
generator = WorldGenerator()
generator.from_dict(wg_config)
self.assertTrue(generator.is_asset('box'))
self.assertTrue(generator.assets.is_model('box'))
model = generator.get_asset('box')
self.assertIsNotNone(model)
self.assertIsInstance(model, SimulationModel)
self.assertEqual(model.name, 'box')
self.assertTrue(generator.is_asset('box_floor'))
self.assertTrue(generator.assets.is_model('box_floor'))
model = generator.get_asset('box_floor')
self.assertIsNotNone(model)
self.assertIsInstance(model, SimulationModel)
self.assertEqual(model.name, 'box_floor')
self.assertTrue(generator.is_asset('box_factory'))
self.assertTrue(generator.assets.is_factory_input('box_factory'))
model = generator.get_asset('box_factory')
self.assertIsNotNone(model)
self.assertIsInstance(model, SimulationModel)
self.assertEqual(model.name, 'box_factory')
self.assertTrue(generator.is_asset('box_generated'))
self.assertTrue(
generator.assets.is_model_group_generator('box_generated'))
model = generator.get_asset('box_generated')
self.assertIsNotNone(model)
self.assertIsInstance(model, ModelGroup)
self.assertEqual(model.name, 'box_generated')
self.assertEqual(model.n_models, 5)
for engine in wg_config['engines']:
self.assertTrue(generator.engines.has_element(engine['tag']))
for constraint in wg_config['constraints']:
self.assertTrue(
generator.engines.has_constraint(constraint['name']))
self.assertTrue(generator.run_engines())
# Test if all models where generated
self.assertEqual(generator.world.n_models, 17)
self.assertEqual(len(generator.world.model_groups), 3)
self.assertIn('default', generator.world.model_groups)
self.assertIn('box_generated', generator.world.model_groups)
self.assertIn('box_generated_1', generator.world.model_groups)
# Test export to world SDF file
self.assertEqual(generator.export_world('/tmp', 'test.world'),
'/tmp/test.world')
self.assertTrue(os.path.isfile('/tmp/test.world'))
# Test export to mesh
export_formats = ['obj']
for f in export_formats:
for mesh_type in ['visual', 'collision']:
filename = mesh_type + generate_random_string(5)
generator.world.export_as_mesh(format=f,
mesh_type=mesh_type,
filename=filename,
folder='/tmp')
self.assertTrue(
os.path.isfile(os.path.join('/tmp', filename + '.' + f)))
self.assertGreater(
os.path.getsize(os.path.join('/tmp', filename + '.' + f)),
0)
