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.
190 lines
7.2 KiB
Python
190 lines
7.2 KiB
Python
"""Fusion can reorder geoms; serialization must preserve poses at full precision."""
|
|
import xml.etree.ElementTree as ET
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
mujoco = pytest.importorskip('mujoco')
|
|
|
|
from mjrl.onboarding.build import write_mjcf
|
|
|
|
|
|
def _source(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
|
|
''')
|
|
path = tmp_path / 'robot.xml'
|
|
path.write_text('''<mujoco><compiler fusestatic="true"/>
|
|
<asset><mesh name="shape" file="shape.obj"/></asset><worldbody>
|
|
<body name="base"><joint name="axis"/><geom size=".1"/>
|
|
<body name="fixed_a" pos=".1234567890123 .032123456789 .2"
|
|
quat=".998412345 .00123456789 .0423456789 .03123456789">
|
|
<geom name="first" type="mesh" mesh="shape"/>
|
|
<geom name="second" size=".03" pos=".01 .02 .03"/>
|
|
<body name="nested" pos=".0123456789 .02345678901 .03456789012">
|
|
<geom name="nested_geom" type="mesh" mesh="shape"/>
|
|
</body>
|
|
</body>
|
|
<body name="fixed_b" pos=".2 0 0"><geom name="last" size=".02"/></body>
|
|
</body></worldbody></mujoco>''')
|
|
return path
|
|
|
|
|
|
def test_fusion_order_and_mesh_poses_preserved(tmp_path, monkeypatch):
|
|
from mjrl.platform import _serialization
|
|
|
|
serialize = _serialization.spec_xml
|
|
|
|
def reordered(spec):
|
|
root = ET.fromstring(serialize(spec))
|
|
parent = root.find(".//body[@name='fixed_a']")
|
|
geoms = parent.findall('geom')
|
|
for geom in geoms:
|
|
parent.remove(geom)
|
|
for index, geom in enumerate(reversed(geoms)):
|
|
parent.insert(index, geom)
|
|
return ET.tostring(root, encoding='unicode')
|
|
|
|
monkeypatch.setattr(_serialization, 'spec_xml', reordered)
|
|
source = _source(tmp_path)
|
|
spec = mujoco.MjSpec.from_file(str(source))
|
|
original = mujoco.MjModel.from_xml_path(str(source))
|
|
copied = write_mjcf(spec, source, tmp_path / 'output' / 'robot.xml')
|
|
assert original.nbody == copied.nbody == 2
|
|
assert copied.geom(0).name == ''
|
|
assert '__mjrl_roundtrip_' not in (tmp_path / 'output' / 'robot.xml').read_text()
|
|
# Native fusion changes IDs after canonicalization; prove the test has a
|
|
# reordered geom and then verify each object by its actual identity.
|
|
assert original.geom('first').id != copied.geom('first').id
|
|
for name in ['first', 'nested_geom', 'last']:
|
|
a, b = original.geom(name).id, copied.geom(name).id
|
|
np.testing.assert_allclose(original.geom_pos[a], copied.geom_pos[b], atol=1e-12, rtol=0)
|
|
np.testing.assert_allclose(original.geom_quat[a], copied.geom_quat[b], atol=1e-12, rtol=0)
|
|
np.testing.assert_allclose(original.body_inertia, copied.body_inertia, atol=1e-9)
|
|
|
|
|
|
def test_roundtrip_still_rejects_transform_corruption(tmp_path, monkeypatch):
|
|
source = _source(tmp_path)
|
|
load = mujoco.MjModel.from_xml_path
|
|
|
|
def corrupted(path):
|
|
model = load(path)
|
|
model.geom_pos[model.geom('first').id, 0] += .01
|
|
return model
|
|
|
|
monkeypatch.setattr(mujoco.MjModel, 'from_xml_path', corrupted)
|
|
output = tmp_path / 'output' / 'robot.xml'
|
|
with pytest.raises(ValueError, match='round-trip changed geom_pos'):
|
|
write_mjcf(mujoco.MjSpec.from_file(str(source)), source, output)
|
|
assert not output.exists()
|
|
|
|
|
|
@pytest.mark.parametrize('raises', [False, True])
|
|
def test_precision_restored_on_success_and_error(raises):
|
|
from mjrl.platform._serialization import _precision_api, spec_xml
|
|
|
|
api = _precision_api()
|
|
if api is None:
|
|
pytest.skip('native writer precision hooks unavailable')
|
|
get, set_ = api
|
|
previous = get()
|
|
|
|
class Writer:
|
|
def to_xml(self):
|
|
assert get() == 17
|
|
if raises:
|
|
raise ValueError('writer error')
|
|
return '<mujoco/>'
|
|
|
|
try:
|
|
set_(9)
|
|
if raises:
|
|
with pytest.raises(ValueError, match='writer error'):
|
|
spec_xml(Writer())
|
|
else:
|
|
assert spec_xml(Writer()) == '<mujoco/>'
|
|
assert get() == 9
|
|
finally:
|
|
set_(previous)
|
|
|
|
|
|
def test_precision_fallback_without_native_hooks(monkeypatch):
|
|
from mjrl.platform import _serialization
|
|
|
|
monkeypatch.setattr(_serialization, '_precision_api', lambda: None)
|
|
|
|
class Writer:
|
|
def to_xml(self):
|
|
return '<mujoco/>'
|
|
|
|
assert _serialization.spec_xml(Writer()) == '<mujoco/>'
|
|
|
|
|
|
def test_packaged_copy_preserves_precision(tmp_path):
|
|
from mjrl.platform.model import copy_model
|
|
|
|
source = _source(tmp_path)
|
|
before = mujoco.MjModel.from_xml_path(str(source))
|
|
output = tmp_path / 'app' / 'robot.xml'
|
|
copy_model(source, output)
|
|
after = mujoco.MjModel.from_xml_path(str(output))
|
|
assert '__mjrl_roundtrip_' not in output.read_text()
|
|
for name in ['first', 'nested_geom', 'last']:
|
|
a, b = before.geom(name).id, after.geom(name).id
|
|
np.testing.assert_allclose(before.geom_pos[a], after.geom_pos[b], atol=1e-12, rtol=0)
|
|
np.testing.assert_allclose(before.geom_quat[a], after.geom_quat[b], atol=1e-12, rtol=0)
|
|
|
|
|
|
@pytest.mark.parametrize('packaged', [False, True])
|
|
def test_temporary_names_do_not_strip_other_object_names(tmp_path, packaged):
|
|
from mjrl.platform.model import copy_model
|
|
|
|
source = tmp_path / 'source.xml'
|
|
source.write_text('''<mujoco><worldbody><body><joint name="j"/>
|
|
<geom size=".1"/><site name="__mjrl_roundtrip_1" size=".01"/>
|
|
</body></worldbody><sensor><framepos name="p" objtype="site"
|
|
objname="__mjrl_roundtrip_1"/></sensor></mujoco>''')
|
|
output = tmp_path / 'result.xml'
|
|
if packaged:
|
|
copy_model(source, output)
|
|
else:
|
|
write_mjcf(mujoco.MjSpec.from_file(str(source)), source, output)
|
|
model = mujoco.MjModel.from_xml_path(str(output))
|
|
assert model.site('__mjrl_roundtrip_1').id == 0
|
|
assert model.body(1).name == ''
|
|
assert model.geom(0).name == ''
|
|
|
|
|
|
@pytest.mark.parametrize('packaged', [False, True])
|
|
def test_massless_collision_defaults_preserve_inferred_inertia(tmp_path, packaged):
|
|
from mjrl.platform.model import copy_model
|
|
|
|
# Include the body so packaging must flatten the spec. Both zero settings
|
|
# together trigger the native writer's elision of the collision default.
|
|
(tmp_path / 'body.xml').write_text('''<mujoco><body name="link">
|
|
<joint name="hinge"/><geom size=".1" mass="1.25"/>
|
|
<geom class="pad" size=".2" pos="0 .25 0"/>
|
|
<geom class="pad" size=".15" pos=".25 0 0" mass=".5"/>
|
|
</body></mujoco>''')
|
|
source = tmp_path / 'scene.xml'
|
|
source.write_text('''<mujoco><default><default class="collision">
|
|
<geom density="0" mass="0"/><default class="pad"/>
|
|
</default></default><worldbody><include file="body.xml"/></worldbody></mujoco>''')
|
|
before = mujoco.MjModel.from_xml_path(str(source))
|
|
assert before.body('link').mass.item() == 1.75
|
|
output = tmp_path / 'output' / 'robot.xml'
|
|
if packaged:
|
|
copy_model(source, output)
|
|
else:
|
|
write_mjcf(mujoco.MjSpec.from_file(str(source)), source, output)
|
|
after = mujoco.MjModel.from_xml_path(str(output))
|
|
for field in ('body_mass', 'body_inertia', 'body_ipos', 'body_iquat'):
|
|
np.testing.assert_array_equal(getattr(after, field), getattr(before, field))
|
|
assert all(after.geom(gid).name == '' for gid in range(after.ngeom))
|