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)