Add mjSpec bindings.
Co-authored-by: Saran Tunyasuvunakool <stunya@google.com> PiperOrigin-RevId: 648444641 Change-Id: I08ee1d3f4fae1efbc7934ba94702b1c06d8cb92a
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
f2c3be470f
commit
9106f40e2b
@@ -0,0 +1,222 @@
|
||||
# Copyright 2024 DeepMind Technologies Limited
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
# ==============================================================================
|
||||
"""Tests for mjSpec bindings."""
|
||||
|
||||
import inspect
|
||||
import textwrap
|
||||
|
||||
from absl.testing import absltest
|
||||
import mujoco
|
||||
import numpy as np
|
||||
|
||||
|
||||
def get_linenumber():
|
||||
cf = inspect.currentframe()
|
||||
return cf.f_back.f_lineno
|
||||
|
||||
|
||||
class SpecsTest(absltest.TestCase):
|
||||
|
||||
def test_basic(self):
|
||||
# Create a spec.
|
||||
spec = mujoco.MjSpec()
|
||||
|
||||
# Check that euler sequence order is set correctly.
|
||||
self.assertEqual(spec.euler[0], ord('x'))
|
||||
spec.euler = ['z', 'y', 'x']
|
||||
self.assertEqual(spec.euler[0], ord('z'))
|
||||
|
||||
# Add a body, check that it has default orientation.
|
||||
body = spec.worldbody.add_body()
|
||||
self.assertEqual(body.name, '')
|
||||
np.testing.assert_array_equal(body.quat, [1, 0, 0, 0])
|
||||
|
||||
# Change the name of the body and read it back twice.
|
||||
body.name = 'foobar'
|
||||
self.assertEqual(body.name, 'foobar')
|
||||
body.name = 'baz'
|
||||
self.assertEqual(body.name, 'baz')
|
||||
|
||||
# Change the position of the body and read it back.
|
||||
body.pos = [1, 2, 3]
|
||||
np.testing.assert_array_equal(body.pos, [1, 2, 3])
|
||||
self.assertEqual(body.pos.shape, (3,))
|
||||
|
||||
# Change the orientation of the body and read it back.
|
||||
body.quat = [0, 1, 0, 0]
|
||||
np.testing.assert_array_equal(body.quat, [0, 1, 0, 0])
|
||||
self.assertEqual(body.quat.shape, (4,))
|
||||
|
||||
# Add a site to the body with user data and read it back.
|
||||
site = body.add_site()
|
||||
site.name = 'sitename'
|
||||
site.userdata = [1, 2, 3, 4, 5, 6]
|
||||
self.assertEqual(site.name, 'sitename')
|
||||
np.testing.assert_array_equal(site.userdata, [1, 2, 3, 4, 5, 6])
|
||||
|
||||
# Check that the site has no id before compilation.
|
||||
self.assertEqual(body.id(), -1)
|
||||
|
||||
# Compile the spec and check for expected values in the model.
|
||||
model = spec.compile()
|
||||
self.assertEqual(spec.worldbody.id(), 0)
|
||||
self.assertEqual(body.id(), 1)
|
||||
self.assertEqual(model.nbody, 2) # 2 bodies, including the world body
|
||||
np.testing.assert_array_equal(model.body_pos[1], [1, 2, 3])
|
||||
np.testing.assert_array_equal(model.body_quat[1], [0, 1, 0, 0])
|
||||
self.assertEqual(model.nsite, 1)
|
||||
self.assertEqual(model.nuser_site, 6)
|
||||
np.testing.assert_array_equal(model.site_user[0], [1, 2, 3, 4, 5, 6])
|
||||
|
||||
self.assertEqual(spec.to_xml(), textwrap.dedent("""\
|
||||
<mujoco model="MuJoCo Model">
|
||||
<compiler angle="radian"/>
|
||||
|
||||
<size nuser_site="6"/>
|
||||
|
||||
<worldbody>
|
||||
<body name="baz" pos="1 2 3" quat="0 1 0 0">
|
||||
<site name="sitename" pos="0 0 0" user="1 2 3 4 5 6"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
"""),)
|
||||
|
||||
def test_compile_errors_with_line_info(self):
|
||||
spec = mujoco.MjSpec()
|
||||
|
||||
added_on_line = get_linenumber() + 1
|
||||
geom = spec.worldbody.add_geom()
|
||||
geom.name = 'MyGeom'
|
||||
geom.info = f'geom added on line {added_on_line}'
|
||||
|
||||
# Try to compile, get error.
|
||||
expected_error = (
|
||||
'Error: size 0 must be positive in geom\n'
|
||||
+ f'Element name \'MyGeom\', id 0, geom added on line {added_on_line}'
|
||||
)
|
||||
with self.assertRaisesRegex(ValueError, expected_error):
|
||||
spec.compile()
|
||||
|
||||
def test_recompile(self):
|
||||
# Create a spec.
|
||||
spec = mujoco.MjSpec()
|
||||
|
||||
# Add movable body1.
|
||||
body1 = spec.worldbody.add_body()
|
||||
geom = body1.add_geom()
|
||||
geom.size[0] = 1
|
||||
geom.pos = [1, 1, 0]
|
||||
joint = body1.add_joint()
|
||||
joint.type = mujoco.mjtJoint.mjJNT_BALL
|
||||
|
||||
# Compile model, make data.
|
||||
model = spec.compile()
|
||||
data = mujoco.MjData(model)
|
||||
|
||||
# Simulate for 1 second.
|
||||
while data.time < 1:
|
||||
mujoco.mj_step(model, data)
|
||||
|
||||
# Add movable body2.
|
||||
body2 = spec.worldbody.add_body()
|
||||
body2.pos[1] = 3
|
||||
geom = body2.add_geom()
|
||||
geom.size[0] = 1
|
||||
geom.pos = [0, 1, 0]
|
||||
joint = body2.add_joint()
|
||||
joint.type = mujoco.mjtJoint.mjJNT_BALL
|
||||
|
||||
# Recompile model and data while maintaining the state.
|
||||
model_new, data_new = spec.recompile(model, data)
|
||||
|
||||
# Check that the state is preserved.
|
||||
np.testing.assert_array_equal(model_new.body_pos[1], model.body_pos[1])
|
||||
np.testing.assert_array_equal(data_new.qpos[:4], data.qpos)
|
||||
np.testing.assert_array_equal(data_new.qvel[:3], data.qvel)
|
||||
|
||||
def test_uncompiled_spec_cannot_be_written(self):
|
||||
spec = mujoco.MjSpec()
|
||||
|
||||
# Cannot write XML of an uncompiled spec.
|
||||
expected_error = 'XML Write error: Only compiled model can be written'
|
||||
with self.assertRaisesWithLiteralMatch(mujoco.FatalError, expected_error):
|
||||
spec.to_xml()
|
||||
|
||||
def test_modelname_default_class(self):
|
||||
spec = mujoco.MjSpec()
|
||||
spec.modelname = 'test'
|
||||
|
||||
main = spec.default()
|
||||
main.geom.size[0] = 2
|
||||
|
||||
def1 = spec.add_default('def1', main)
|
||||
def1.geom.size[0] = 3
|
||||
|
||||
spec.worldbody.add_geom(def1)
|
||||
spec.worldbody.add_geom(main)
|
||||
|
||||
spec.compile()
|
||||
self.assertEqual(spec.to_xml(), textwrap.dedent("""\
|
||||
<mujoco model="test">
|
||||
<compiler angle="radian"/>
|
||||
|
||||
<default>
|
||||
<geom size="2 0 0"/>
|
||||
<default class="def1">
|
||||
<geom size="3 0 0"/>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
<worldbody>
|
||||
<geom class="def1"/>
|
||||
<geom/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
"""))
|
||||
spec = mujoco.MjSpec()
|
||||
spec.modelname = 'test'
|
||||
|
||||
main = spec.default()
|
||||
main.geom.size[0] = 2
|
||||
|
||||
def1 = spec.add_default('def1', main)
|
||||
def1.geom.size[0] = 3
|
||||
|
||||
spec.worldbody.add_geom(def1)
|
||||
spec.worldbody.add_geom(main)
|
||||
|
||||
spec.compile()
|
||||
self.assertEqual(spec.to_xml(), textwrap.dedent("""\
|
||||
<mujoco model="test">
|
||||
<compiler angle="radian"/>
|
||||
|
||||
<default>
|
||||
<geom size="2 0 0"/>
|
||||
<default class="def1">
|
||||
<geom size="3 0 0"/>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
<worldbody>
|
||||
<geom class="def1"/>
|
||||
<geom/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
"""))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
absltest.main()
|
||||
Reference in New Issue
Block a user