def testResolveReferences(self): resolved_model = parser.from_path(_TEST_MODEL_XML) self.assertIs( resolved_model.find('geom', 'b_1_0').material, resolved_model.find('material', 'mat_texture')) unresolved_model = parser.from_path( _TEST_MODEL_XML, resolve_references=False) self.assertEqual( unresolved_model.find('geom', 'b_1_0').material, 'mat_texture') unresolved_model.resolve_references() self.assertIs( unresolved_model.find('geom', 'b_1_0').material, unresolved_model.find('material', 'mat_texture'))
def testFindAll(self): mujoco = parser.from_path(_TEST_MODEL_XML) mujoco.model = 'model' submujoco = copy.copy(mujoco) submujoco.model = 'submodel' subsubmujoco = copy.copy(mujoco) subsubmujoco.model = 'subsubmodel' submujoco.find('site', 'attachment').attach(subsubmujoco) mujoco.attach(submujoco) geoms = mujoco.find_all('geom') self.assertLen(geoms, 6) self.assertEqual(geoms[0].root, mujoco) self.assertEqual(geoms[1].root, mujoco) self.assertEqual(geoms[2].root, submujoco) self.assertEqual(geoms[3].root, subsubmujoco) self.assertEqual(geoms[4].root, subsubmujoco) self.assertEqual(geoms[5].root, submujoco) b_0 = submujoco.find('body', 'b_0') self.assertLen(b_0.find_all('joint'), 6) self.assertLen(b_0.find_all('joint', immediate_children_only=True), 1) self.assertLen(b_0.find_all('joint', exclude_attachments=True), 2)
def testEnterScope(self): mujoco = parser.from_path(_TEST_MODEL_XML, resolve_references=False) mujoco.model = 'model' self.assertIsNone(mujoco.enter_scope('submodel')) submujoco = copy.copy(mujoco) submujoco.model = 'submodel' subsubmujoco = copy.copy(mujoco) subsubmujoco.model = 'subsubmodel' submujoco.find('site', 'attachment').attach(subsubmujoco) mujoco.attach(submujoco) self.assertIsNotNone(mujoco.enter_scope('submodel')) self.assertEqual(mujoco.enter_scope('submodel').find('geom', 'b_0_0'), submujoco.find('geom', 'b_0_0')) self.assertEqual( mujoco.enter_scope('submodel/subsubmodel/').find('geom', 'b_0_0'), subsubmujoco.find('geom', 'b_0_0')) self.assertEqual(mujoco.enter_scope('submodel').enter_scope( 'subsubmodel').find('geom', 'b_0_0'), subsubmujoco.find('geom', 'b_0_0')) self.assertEqual( mujoco.enter_scope('submodel').find('actuator', 'b_0_0').root, submujoco) self.assertEqual( mujoco.enter_scope('submodel').find('actuator', 'b_0_0').tag, 'velocity') self.assertEqual( mujoco.enter_scope('submodel').find('actuator', 'b_0_0').joint, 'b_0_0')
def testGetAssetFromFile(self): with open(_TEXTURE_PATH, 'rb') as f: contents = f.read() _, filename = os.path.split(_TEXTURE_PATH) prefix, extension = os.path.splitext(filename) vfs_filename = prefix + '-' + hashlib.sha1(contents).hexdigest() + extension mujoco = parser.from_path(_TEST_MODEL_XML) self.assertDictEqual({vfs_filename: contents}, mujoco.get_assets())
def testGetAssetFromPlaceholder(self): mujoco = parser.from_path(_TEST_MODEL_XML) # Add an extra texture asset from a placeholder. contents = b'I am a texture bytestring' extension = '.png' vfs_filename = hashlib.sha1(contents).hexdigest() + extension placeholder = mjcf.Asset(contents=contents, extension=extension) mujoco.asset.add('texture', name='fake_texture', file=placeholder) self.assertDictContainsSubset({vfs_filename: contents}, mujoco.get_assets())
def testAssetsCanBeCopied(self): with open(_TEXTURE_PATH, 'rb') as f: contents = f.read() _, filename = os.path.split(_TEXTURE_PATH) prefix, extension = os.path.splitext(filename) vfs_filename = prefix + '-' + hashlib.sha1(contents).hexdigest() + extension mujoco = parser.from_path(_TEST_MODEL_XML) mujoco_copy = copy.copy(mujoco) expected = {vfs_filename: contents} self.assertDictEqual(expected, mujoco.get_assets()) self.assertDictEqual(expected, mujoco_copy.get_assets())
def testParseModelWithNamelessAssets(self): mujoco = parser.from_path(path=_MODEL_WITH_NAMELESS_ASSETS) expected_names_derived_from_filenames = [ ('mesh', 'cube'), # ./test_assets/meshes/cube.stl ('texture', 'deepmind'), # ./test_assets/textures/deepmind.png ('hfield', 'deepmind'), # ./test_assets/textures/deepmind.png ] with self.subTest('Expected asset names are present in the parsed model'): for namespace, name in expected_names_derived_from_filenames: self.assertIsNotNone(mujoco.find(namespace, name)) with self.subTest('Can compile and step the simulation'): physics = mjcf.Physics.from_mjcf_model(mujoco) physics.step()
def testFindAllFrameJoints(self): root_model = parser.from_path(_TEST_MODEL_XML) root_model.model = 'model' submodel = copy.copy(root_model) submodel.model = 'submodel' frame = root_model.attach(submodel) joint_x = frame.add('joint', type='slide', axis=[1, 0, 0]) joint_y = frame.add('joint', type='slide', axis=[0, 1, 0]) joints = frame.find_all('joint', immediate_children_only=True) self.assertListEqual(joints, [joint_x, joint_y])
def testRenameAttachedModel(self): root = parser.from_path(_TEST_MODEL_XML) root.model = 'model' submodel = copy.copy(root) submodel.model = 'submodel' geom = submodel.worldbody.add( 'geom', name='geom', type='sphere', size=[0.1]) frame = root.attach(submodel) submodel.model = 'renamed' self.assertEqual(frame.full_identifier, 'renamed/') self.assertIsSame(root.find('geom', 'renamed/geom'), geom)
def testCopyingWithAssets(self): mjcf_model = parser.from_path(_MODEL_WITH_ASSETS_XML) copied = mjcf.RootElement() copied.include_copy(mjcf_model) original_assets = (mjcf_model.find_all('mesh') + mjcf_model.find_all('texture') + mjcf_model.find_all('hfield')) copied_assets = (copied.find_all('mesh') + copied.find_all('texture') + copied.find_all('hfield')) self.assertLen(copied_assets, len(original_assets)) for original_asset, copied_asset in zip(original_assets, copied_assets): self.assertIs(copied_asset.file, original_asset.file)
def testWorldBodyFullIdentifier(self): mujoco = parser.from_path(_TEST_MODEL_XML) mujoco.model = 'model' self.assertEqual(mujoco.worldbody.full_identifier, 'world') submujoco = copy.copy(mujoco) submujoco.model = 'submodel' self.assertEqual(submujoco.worldbody.full_identifier, 'world') mujoco.attach(submujoco) self.assertEqual(mujoco.worldbody.full_identifier, 'world') self.assertEqual(submujoco.worldbody.full_identifier, 'submodel/') self.assertNotIn('name', mujoco.worldbody.to_xml_string(self_only=True)) self.assertNotIn('name', submujoco.worldbody.to_xml_string(self_only=True))
def testAttachmentFrames(self): mujoco = parser.from_path(_TEST_MODEL_XML) mujoco.model = 'model' submujoco = copy.copy(mujoco) submujoco.model = 'submodel' subsubmujoco = copy.copy(mujoco) subsubmujoco.model = 'subsubmodel' attachment_site = submujoco.find('site', 'attachment') attachment_site.attach(subsubmujoco) mujoco.attach(submujoco) # attachments directly on worldbody can have a <freejoint> submujoco_frame = mujoco.find('attachment_frame', 'submodel') self.assertStartsWith( submujoco_frame.to_xml_string(pretty_print=False), '<body name="submodel/">') self.assertEqual(submujoco_frame.full_identifier, 'submodel/') free_joint = submujoco_frame.add('freejoint') self.assertEqual(free_joint.to_xml_string(pretty_print=False), '<freejoint name="submodel/"/>') self.assertEqual(free_joint.full_identifier, 'submodel/') # attachments elsewhere cannot have a <freejoint> subsubmujoco_frame = submujoco.find('attachment_frame', 'subsubmodel') subsubmujoco_frame_xml = subsubmujoco_frame.to_xml_string( pretty_print=False, prefix_root=mujoco.namescope) self.assertStartsWith( subsubmujoco_frame_xml, '<body ' 'pos="0.10000000000000001 0.10000000000000001 0.10000000000000001" ' 'quat="0 1 0 0" ' 'name="submodel/subsubmodel/">') self.assertEqual(subsubmujoco_frame.full_identifier, 'submodel/subsubmodel/') with six.assertRaisesRegex(self, AttributeError, 'not a valid child'): subsubmujoco_frame.add('freejoint') hinge_joint = subsubmujoco_frame.add('joint', type='hinge', axis=[1, 2, 3]) hinge_joint_xml = hinge_joint.to_xml_string( pretty_print=False, prefix_root=mujoco.namescope) self.assertEqual( hinge_joint_xml, '<joint class="submodel/" type="hinge" axis="1 2 3" ' 'name="submodel/subsubmodel/"/>') self.assertEqual(hinge_joint.full_identifier, 'submodel/subsubmodel/')
def testCopy(self): mujoco = parser.from_path(_TEST_MODEL_XML) self.assertIsSame(mujoco, mujoco) copy_mujoco = copy.copy(mujoco) copy_mujoco.model = 'copied_model' self.assertIsSame(copy_mujoco, mujoco) self.assertNotEqual(copy_mujoco, mujoco) deepcopy_mujoco = copy.deepcopy(mujoco) deepcopy_mujoco.model = 'deepcopied_model' self.assertIsSame(deepcopy_mujoco, mujoco) self.assertNotEqual(deepcopy_mujoco, mujoco) self.assertIsSame(deepcopy_mujoco, copy_mujoco) self.assertNotEqual(deepcopy_mujoco, copy_mujoco)
def testFind(self): mujoco = parser.from_path(_TEST_MODEL_XML, resolve_references=False) mujoco.model = 'model' submujoco = copy.copy(mujoco) submujoco.model = 'submodel' subsubmujoco = copy.copy(mujoco) subsubmujoco.model = 'subsubmodel' submujoco.find('site', 'attachment').attach(subsubmujoco) mujoco.attach(submujoco) self.assertIsNotNone(mujoco.find('geom', 'b_0_0')) self.assertIsNotNone(mujoco.find('body', 'b_0').find('geom', 'b_0_0')) self.assertIsNone(mujoco.find('body', 'b_1').find('geom', 'b_0_0')) self.assertIsNone(mujoco.find('geom', 'nonexistent')) self.assertIsNone(mujoco.find('geom', 'nonexistent/b_0_0')) self.assertEqual(mujoco.find('geom', 'submodel/b_0_0'), submujoco.find('geom', 'b_0_0')) self.assertEqual(mujoco.find('geom', 'submodel/subsubmodel/b_0_0'), submujoco.find('geom', 'subsubmodel/b_0_0')) self.assertEqual(submujoco.find('geom', 'subsubmodel/b_0_0'), subsubmujoco.find('geom', 'b_0_0')) subsubmujoco.find('geom', 'b_0_0').name = 'foo' self.assertIsNone(mujoco.find('geom', 'submodel/subsubmodel/b_0_0')) self.assertIsNone(submujoco.find('geom', 'subsubmodel/b_0_0')) self.assertIsNone(subsubmujoco.find('geom', 'b_0_0')) self.assertEqual(mujoco.find('geom', 'submodel/subsubmodel/foo'), submujoco.find('geom', 'subsubmodel/foo')) self.assertEqual(submujoco.find('geom', 'subsubmodel/foo'), subsubmujoco.find('geom', 'foo')) self.assertEqual(mujoco.find('actuator', 'b_0_0').root, mujoco) self.assertEqual(mujoco.find('actuator', 'b_0_0').tag, 'velocity') self.assertEqual(mujoco.find('actuator', 'b_0_0').joint, 'b_0_0') self.assertEqual( mujoco.find('actuator', 'submodel/b_0_0').root, submujoco) self.assertEqual( mujoco.find('actuator', 'submodel/b_0_0').tag, 'velocity') self.assertEqual( mujoco.find('actuator', 'submodel/b_0_0').joint, 'b_0_0')
def testDuplicateAttachmentFrameJointIdentifiers(self): mujoco = parser.from_path(_TEST_MODEL_XML) mujoco.model = 'model' submujoco_1 = copy.copy(mujoco) submujoco_1.model = 'submodel_1' submujoco_2 = copy.copy(mujoco) submujoco_2.model = 'submodel_2' frame_1 = mujoco.attach(submujoco_1) frame_2 = mujoco.attach(submujoco_2) joint_1 = frame_1.add('joint', type='slide', name='root_x', axis=[1, 0, 0]) joint_2 = frame_2.add('joint', type='slide', name='root_x', axis=[1, 0, 0]) self.assertEqual(joint_1.full_identifier, 'submodel_1/root_x/') self.assertEqual(joint_2.full_identifier, 'submodel_2/root_x/')
def testDetach(self): root = parser.from_path(_TEST_MODEL_XML) root.model = 'model' submodel = copy.copy(root) submodel.model = 'submodel' unattached_xml_1 = root.to_xml_string() root.attach(submodel) attached_xml_1 = root.to_xml_string() submodel.detach() unattached_xml_2 = root.to_xml_string() root.attach(submodel) attached_xml_2 = root.to_xml_string() self.assertEqual(unattached_xml_1, unattached_xml_2) self.assertEqual(attached_xml_1, attached_xml_2)
def testDeletion(self): mujoco = parser.from_path(_TEST_MODEL_XML) mujoco.model = 'model' submujoco = copy.copy(mujoco) submujoco.model = 'submodel' subsubmujoco = copy.copy(mujoco) subsubmujoco.model = 'subsubmodel' submujoco.find('site', 'attachment').attach(subsubmujoco) mujoco.attach(submujoco) with self.assertRaisesRegexp( ValueError, r'use remove\(affect_attachments=True\)'): del mujoco.option mujoco.option.remove(affect_attachments=True) for root in (mujoco, submujoco, subsubmujoco): self.assertIsNotNone(root.option.flag) self.assertEqual( root.option.to_xml_string(pretty_print=False), '<option/>') self.assertIsNotNone(root.option.flag) self.assertEqual( root.option.flag.to_xml_string(pretty_print=False), '<flag/>') with self.assertRaisesRegexp( ValueError, r'use remove\(affect_attachments=True\)'): del mujoco.contact mujoco.contact.remove(affect_attachments=True) for root in (mujoco, submujoco, subsubmujoco): self.assertEqual( root.contact.to_xml_string(pretty_print=False), '<contact/>') b_0 = mujoco.find('body', 'b_0') b_0_inertial = b_0.inertial self.assertEqual(b_0_inertial.mass, 1) self.assertIsNotNone(b_0.inertial) del b_0.inertial self.assertIsNone(b_0.inertial)
def testSimpleCopy(self): mjcf_model = parser.from_path(_TEST_MODEL_XML) mixin = mjcf.RootElement(model='test_mixin') mixin.compiler.boundmass = 1 mjcf_model.include_copy(mixin) self.assertEqual(mjcf_model.model, 'test') # Model name should not change self.assertEqual(mjcf_model.compiler.boundmass, mixin.compiler.boundmass) mixin.compiler.boundinertia = 2 mjcf_model.include_copy(mixin) self.assertEqual(mjcf_model.compiler.boundinertia, mixin.compiler.boundinertia) mixin.compiler.boundinertia = 1 with self.assertRaisesRegexp(ValueError, 'Conflicting values'): mjcf_model.include_copy(mixin) mixin.worldbody.add('body', name='b_0', pos=[0, 1, 2]) mjcf_model.include_copy(mixin, override_attributes=True) self.assertEqual(mjcf_model.compiler.boundmass, mixin.compiler.boundmass) self.assertEqual(mjcf_model.compiler.boundinertia, mixin.compiler.boundinertia) np.testing.assert_array_equal(mjcf_model.worldbody.body['b_0'].pos, [0, 1, 2])
def testParseFromPath(self, model_path): parser.from_path(model_path)
def testAttach(self): mujoco = parser.from_path(_TEST_MODEL_XML) mujoco.model = 'model' submujoco = copy.copy(mujoco) submujoco.model = 'submodel' subsubmujoco = copy.copy(mujoco) subsubmujoco.model = 'subsubmodel' with self.assertRaisesRegexp(ValueError, 'Cannot merge a model to itself'): mujoco.attach(mujoco) attachment_site = submujoco.find('site', 'attachment') attachment_site.attach(subsubmujoco) subsubmodel_frame = submujoco.find('attachment_frame', 'subsubmodel') for attribute_name in ('pos', 'axisangle', 'xyaxes', 'zaxis', 'euler'): np.testing.assert_array_equal( getattr(subsubmodel_frame, attribute_name), getattr(attachment_site, attribute_name)) self._test_properties(subsubmodel_frame, parent=attachment_site.parent, root=submujoco) self.assertEqual( subsubmodel_frame.to_xml_string().split('\n')[0], '<body ' 'pos="0.10000000000000001 0.10000000000000001 0.10000000000000001" ' 'quat="0 1 0 0" ' 'name="subsubmodel/">') self.assertEqual(subsubmodel_frame.all_children(), subsubmujoco.worldbody.all_children()) with self.assertRaisesRegexp(ValueError, 'already attached elsewhere'): mujoco.attach(subsubmujoco) with self.assertRaisesRegexp(ValueError, 'Expected a mjcf.RootElement'): mujoco.attach(submujoco.contact) submujoco.option.flag.gravity = 'enable' with self.assertRaisesRegexp( ValueError, 'Conflicting values for attribute `gravity`'): mujoco.attach(submujoco) submujoco.option.flag.gravity = 'disable' mujoco.attach(submujoco) self.assertEqual(subsubmujoco.parent_model, submujoco) self.assertEqual(submujoco.parent_model, mujoco) self.assertEqual(subsubmujoco.root_model, mujoco) self.assertEqual(submujoco.root_model, mujoco) self.assertEqual(submujoco.full_identifier, 'submodel/') self.assertEqual(subsubmujoco.full_identifier, 'submodel/subsubmodel/') merged_children = ('contact', 'actuator') for child_name in merged_children: for grandchild in getattr(submujoco, child_name).all_children(): self.assertIn(grandchild, getattr(mujoco, child_name).all_children()) for grandchild in getattr(subsubmujoco, child_name).all_children(): self.assertIn(grandchild, getattr(mujoco, child_name).all_children()) self.assertIn(grandchild, getattr(submujoco, child_name).all_children()) base_contact_content = ( '<exclude name="{0}exclude" body1="{0}b_0" body2="{0}b_1"/>') self.assertEqual( mujoco.contact.to_xml_string(pretty_print=False), '<contact>' + base_contact_content.format('') + base_contact_content.format('submodel/') + base_contact_content.format('submodel/subsubmodel/') + '</contact>') actuators_template = ( '<velocity name="{1}b_0_0" class="{0}" joint="{1}b_0_0"/>' '<velocity name="{1}b_1_0" class="{0}" joint="{1}b_1_0"/>') self.assertEqual( mujoco.actuator.to_xml_string(pretty_print=False), '<actuator>' + actuators_template.format('/', '') + actuators_template.format('submodel/', 'submodel/') + actuators_template.format('submodel/subsubmodel/', 'submodel/subsubmodel/') + '</actuator>') self.assertEqual(mujoco.default.full_identifier, '/') self.assertEqual(mujoco.default.default[0].full_identifier, 'big_and_green') self.assertEqual(submujoco.default.full_identifier, 'submodel/') self.assertEqual(submujoco.default.default[0].full_identifier, 'submodel/big_and_green') self.assertEqual(subsubmujoco.default.full_identifier, 'submodel/subsubmodel/') self.assertEqual(subsubmujoco.default.default[0].full_identifier, 'submodel/subsubmodel/big_and_green') default_xml_lines = (mujoco.default.to_xml_string(pretty_print=False) .replace('><', '>><<').split('><')) self.assertEqual(default_xml_lines[0], '<default>') self.assertEqual(default_xml_lines[1], '<default class="/">') self.assertEqual(default_xml_lines[4], '<default class="big_and_green">') self.assertEqual(default_xml_lines[6], '</default>') self.assertEqual(default_xml_lines[7], '</default>') self.assertEqual(default_xml_lines[8], '<default class="submodel/">') self.assertEqual(default_xml_lines[11], '<default class="submodel/big_and_green">') self.assertEqual(default_xml_lines[13], '</default>') self.assertEqual(default_xml_lines[14], '</default>') self.assertEqual(default_xml_lines[15], '<default class="submodel/subsubmodel/">') self.assertEqual(default_xml_lines[18], '<default class="submodel/subsubmodel/big_and_green">') self.assertEqual(default_xml_lines[-3], '</default>') self.assertEqual(default_xml_lines[-2], '</default>') self.assertEqual(default_xml_lines[-1], '</default>')