ceca51b0d8
Default humanoid intake to command-following gait and support bounded Cartesian target dragging for arms. Carry model and actuator semantics through calibration, native verification and packaging. Document reusable intake decisions and cover them with regression tests.
111 lines
4.4 KiB
Python
111 lines
4.4 KiB
Python
"""Relocating flattened MJCF must preserve exact compiled dynamics and defaults."""
|
|
import xml.etree.ElementTree as ET
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
mujoco = pytest.importorskip('mujoco')
|
|
|
|
from mjrl.platform.model import copy_model, validate_contained_model
|
|
|
|
|
|
def _model(tmp_path):
|
|
(tmp_path / 'shape.obj').write_text('''v 0 0 0
|
|
v .1 0 0
|
|
v 0 .2 0
|
|
v 0 0 .3
|
|
f 1 3 2
|
|
f 1 2 4
|
|
f 1 4 3
|
|
f 2 3 4
|
|
''')
|
|
source = tmp_path / 'source.xml'
|
|
source.write_text('''<mujoco><compiler angle="radian"/>
|
|
<option timestep=".0025" integrator="implicitfast"/>
|
|
<default><default class="part"><geom friction=".812345678901234 .01 .001"/>
|
|
<joint damping=".123456789012345"/></default></default>
|
|
<asset><mesh name="shape" file="shape.obj"/></asset><worldbody>
|
|
<geom type="plane" size="2 2 .1"/>
|
|
<body name="robot" pos="0 0 .3" quat=".998412345 .00123456789 .0423456789 .03123456789">
|
|
<freejoint/><geom type="sphere" size=".1"/>
|
|
<geom name="visual" type="mesh" mesh="shape" contype="0" conaffinity="0"
|
|
quat=".912345678901 .001234567891 .234567891234 .123456789123"/>
|
|
<body name="arm" pos=".1234567890123 0 .05" childclass="part">
|
|
<joint name="hinge"/><geom name="hand" size=".04"/></body></body>
|
|
</worldbody><actuator><position joint="hinge" kp="10.1234567890123" kv="1"/>
|
|
</actuator></mujoco>''')
|
|
return source
|
|
|
|
|
|
def test_flattened_copy_keeps_exact_compiled_arrays_and_rollout(tmp_path, monkeypatch):
|
|
from mjrl.platform import _serialization
|
|
|
|
def unexpected(spec):
|
|
raise AssertionError('Flattened XML was unnecessarily reserialized')
|
|
|
|
monkeypatch.setattr(_serialization, 'spec_xml', unexpected)
|
|
source = _model(tmp_path)
|
|
output = tmp_path / 'app' / 'robot.xml'
|
|
copy_model(source, output)
|
|
before = mujoco.MjModel.from_xml_path(str(source))
|
|
after = mujoco.MjModel.from_xml_path(str(output))
|
|
for field in dir(before):
|
|
a, b = getattr(before, field), getattr(after, field)
|
|
# _sizes includes storage allocated for relocated path strings.
|
|
if field != "_sizes" and isinstance(a, np.ndarray) and np.issubdtype(a.dtype, np.number):
|
|
np.testing.assert_array_equal(a, b, err_msg=field)
|
|
# Numeric defaults and original anonymous objects survive, not just named geoms.
|
|
assert ET.parse(output).find('.//default[@class="part"]/geom').get('friction') \
|
|
== '.812345678901234 .01 .001'
|
|
assert before.geom(0).name == after.geom(0).name == ''
|
|
(tmp_path / 'shape.obj').unlink()
|
|
validate_contained_model(output.parent, output)
|
|
a, b = mujoco.MjData(before), mujoco.MjData(after)
|
|
for step in range(1000):
|
|
a.ctrl[:] = b.ctrl[:] = .2 * np.sin(step * .01)
|
|
mujoco.mj_step(before, a)
|
|
mujoco.mj_step(after, b)
|
|
np.testing.assert_array_equal(a.qpos, b.qpos)
|
|
np.testing.assert_array_equal(a.qvel, b.qvel)
|
|
|
|
|
|
def test_flattened_copy_still_rejects_sub_tolerance_geometry_change(tmp_path, monkeypatch):
|
|
source = _model(tmp_path)
|
|
output = tmp_path / 'app' / 'robot.xml'
|
|
load = mujoco.MjModel.from_xml_path
|
|
|
|
def changed(path):
|
|
model = load(path)
|
|
if str(path) == str(output):
|
|
model.geom_pos[0, 0] += 1e-12
|
|
return model
|
|
|
|
monkeypatch.setattr(mujoco.MjModel, 'from_xml_path', changed)
|
|
with pytest.raises(ValueError, match='flattened model changed exact geom_pos'):
|
|
copy_model(source, output)
|
|
|
|
|
|
def test_include_and_asset_directory_still_use_normalization(tmp_path):
|
|
source = _model(tmp_path)
|
|
xml = ET.parse(source)
|
|
asset = xml.find('./asset/mesh')
|
|
(tmp_path / 'assets').mkdir()
|
|
(tmp_path / 'shape.obj').rename(tmp_path / 'assets/shape.obj')
|
|
xml.find('compiler').set('meshdir', 'assets')
|
|
body = xml.find('./worldbody/body')
|
|
xml.find('worldbody').remove(body)
|
|
fragment = ET.Element('mujoco')
|
|
fragment.append(body)
|
|
ET.ElementTree(fragment).write(tmp_path / 'body.xml')
|
|
ET.SubElement(xml.find('worldbody'), 'include', file='body.xml')
|
|
xml.write(source)
|
|
assert asset.get('file') == 'shape.obj'
|
|
output = tmp_path / 'app' / 'robot.xml'
|
|
copy_model(source, output)
|
|
result = ET.parse(output)
|
|
assert result.find('.//include') is None
|
|
assert result.find('compiler').get('meshdir') is None
|
|
validate_contained_model(output.parent, output)
|
|
assert mujoco.MjModel.from_xml_path(str(source)).nq \
|
|
== mujoco.MjModel.from_xml_path(str(output)).nq
|