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.
92 lines
3.4 KiB
Python
92 lines
3.4 KiB
Python
"""Native scene export preserves fine geometry rotations without wider tolerances."""
|
|
from types import SimpleNamespace as NS
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
mujoco = pytest.importorskip('mujoco')
|
|
|
|
# Initialize the entity package as a real training scene does; mjlab spec and
|
|
# actuator utilities otherwise have a vendored import-order cycle.
|
|
from mjlab.entity import Entity # noqa: F401
|
|
from mjrl.native_export import write_native_model
|
|
|
|
|
|
def _environment():
|
|
spec = mujoco.MjSpec.from_string('''<mujoco><option timestep=".003"/>
|
|
<asset><mesh name="shape" file="shape.obj"/></asset>
|
|
<worldbody><body name="robot" pos=".012345678901234 .02 .3">
|
|
<freejoint/><geom name="shell" type="mesh" mesh="shape"
|
|
quat=".912345678901 .001234567891 .234567891234 .123456789123"/>
|
|
<body name="limb" pos="0 0 .1"><joint name="hinge"/>
|
|
<geom name="limb_geom" type="capsule" size=".03 .2"
|
|
quat=".998765432109 .0000123456789 .032345678901 .012345678901"/>
|
|
</body></body></worldbody><actuator><motor joint="hinge"/></actuator>
|
|
</mujoco>''', assets={'shape.obj': b'''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
|
|
'''})
|
|
# Mesh-frame composition exposes cancellation that six-digit authored
|
|
# rotations cannot preserve, even with the existing relative tolerance.
|
|
probe = spec.compile()
|
|
inverse = np.empty(4)
|
|
mujoco.mju_negQuat(inverse, probe.mesh_quat[0])
|
|
target = np.array([.9, .3, .2, 1e-6])
|
|
target /= np.linalg.norm(target)
|
|
authored = np.empty(4)
|
|
mujoco.mju_mulQuat(authored, target, inverse)
|
|
spec.geom('shell').quat = authored
|
|
live = spec.compile()
|
|
|
|
class Scene:
|
|
entities = {}
|
|
|
|
def __init__(self):
|
|
self.spec = spec
|
|
|
|
def __getitem__(self, name):
|
|
assert name == 'robot'
|
|
return NS(indexing=NS(root_body_id=live.body('robot').id))
|
|
|
|
return NS(scene=Scene(), sim=NS(mj_model=live), cfg=NS(commands={}))
|
|
|
|
|
|
def test_native_export_retains_sub_six_digit_rotations(tmp_path):
|
|
env = _environment()
|
|
write_native_model(env, tmp_path, {'control': {'sim_dt': .003}})
|
|
copied = mujoco.MjModel.from_xml_path(str(tmp_path / 'robot.xml'))
|
|
np.testing.assert_allclose(copied.geom_quat, env.sim.mj_model.geom_quat, rtol=0, atol=1e-14)
|
|
np.testing.assert_allclose(copied.body_pos, env.sim.mj_model.body_pos, rtol=0, atol=1e-14)
|
|
assert (tmp_path / 'controller.json').is_file()
|
|
|
|
|
|
def test_native_export_rejects_actual_geometry_rotation_change(tmp_path):
|
|
env = _environment()
|
|
# Mutate only the live model: this is a real mismatch with the scene spec.
|
|
env.sim.mj_model.geom_quat[1] = [1., 0., 0., 0.]
|
|
with pytest.raises(ValueError, match='Native model export changed geom_quat'):
|
|
write_native_model(env, tmp_path, {'control': {'sim_dt': .003}})
|
|
assert not (tmp_path / 'controller.json').exists()
|
|
|
|
|
|
def test_six_digit_writer_control_demonstrates_original_failure(tmp_path, monkeypatch):
|
|
from mjrl.platform import _serialization
|
|
|
|
api = _serialization._precision_api()
|
|
if api is None:
|
|
pytest.skip('six digit control requires native precision hooks')
|
|
get, set_ = api
|
|
previous = get()
|
|
try:
|
|
set_(6)
|
|
monkeypatch.setattr('mjrl.native_export.spec_xml', lambda spec: spec.to_xml())
|
|
with pytest.raises(ValueError, match='Native model export changed geom_quat'):
|
|
write_native_model(_environment(), tmp_path, {'control': {'sim_dt': .003}})
|
|
finally:
|
|
set_(previous)
|