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.
91 lines
4.3 KiB
Python
91 lines
4.3 KiB
Python
"""EPA scratch grows before graph capture; collision failures remain fatal."""
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
wp = pytest.importorskip("warp")
|
|
mujoco = pytest.importorskip("mujoco")
|
|
pytest.importorskip("mujoco_warp")
|
|
|
|
from mjlab.sim import SimulationCfg
|
|
from mjrl.backend.warp_sim import (
|
|
EPA_HORIZON_CAPACITY,
|
|
CheckedWarpSimulation,
|
|
_ensure_epa_horizon_capacity,
|
|
)
|
|
from mujoco_warp._src import collision_convex, collision_flex
|
|
from mujoco_warp._src.types import OverflowType
|
|
|
|
|
|
def test_epa_shim_preserves_larger_upstream_capacity_and_rejects_unknown_allocator(monkeypatch):
|
|
monkeypatch.setattr(collision_convex, "MJ_MAX_EPAHORIZON", 256)
|
|
monkeypatch.setattr(collision_flex, "MJ_MAX_EPAHORIZON", 24)
|
|
assert _ensure_epa_horizon_capacity() == 256
|
|
assert collision_flex.MJ_MAX_EPAHORIZON == 256
|
|
monkeypatch.delattr(collision_flex, "MJ_MAX_EPAHORIZON")
|
|
with pytest.raises(RuntimeError, match="Unsupported MuJoCo Warp EPA scratch allocator"):
|
|
_ensure_epa_horizon_capacity()
|
|
|
|
|
|
def test_epa_host_allocation_uses_capacity_and_overflow_stays_fatal(monkeypatch):
|
|
monkeypatch.setattr(collision_convex, "MJ_MAX_EPAHORIZON", 24)
|
|
monkeypatch.setattr(collision_flex, "MJ_MAX_EPAHORIZON", 24)
|
|
allocations = []
|
|
original_empty = wp.empty
|
|
|
|
def record_empty(*args, **kwargs):
|
|
allocations.append(kwargs.get("shape"))
|
|
return original_empty(*args, **kwargs)
|
|
|
|
monkeypatch.setattr(wp, "empty", record_empty)
|
|
model = mujoco.MjModel.from_xml_string("""<mujoco><worldbody>
|
|
<geom type="box" size=".1 .1 .1"/>
|
|
<body pos="0 0 .15"><freejoint/><geom type="box" size=".1 .1 .1"/></body>
|
|
</worldbody></mujoco>""")
|
|
sim = CheckedWarpSimulation(1, SimulationCfg(), model, "cpu")
|
|
assert sim.epa_horizon_capacity == EPA_HORIZON_CAPACITY == 128
|
|
assert collision_flex.MJ_MAX_EPAHORIZON == 128
|
|
sim.forward()
|
|
sim.sense()
|
|
assert (sim.wp_data.naccdmax, 128) in allocations
|
|
assert sim.wp_data.nacon.numpy()[0] > 0 # Contacts are still computed.
|
|
sim.wp_data.overflow.assign(np.array([int(OverflowType.EPA_HORIZON)], dtype=np.int32))
|
|
sim.reset() # Preserve an overflow even if environment reset clears its flag.
|
|
with pytest.raises(RuntimeError, match="EPA horizon scratch capacity: 128") as failure:
|
|
sim.sense()
|
|
assert "--warp-max-contacts" not in str(failure.value)
|
|
assert "--warp-max-constraints" not in str(failure.value)
|
|
|
|
|
|
def test_actual_epa_horizon_negative_control_and_enlarged_scratch_agree():
|
|
# Upstream 3.11.0's test_box_box_horizon regression exercises a 13-edge
|
|
# horizon. Reuse its public-distribution test harness, not a mock kernel.
|
|
from mujoco_warp import test_data
|
|
from mujoco_warp._src.collision_gjk_test import _geom_dist
|
|
|
|
with wp.ScopedDevice("cpu"):
|
|
_, _, model, data = test_data.fixture(xml="""<mujoco><worldbody>
|
|
<geom size=".025 .025 .025" type="box"/>
|
|
<geom size=".025 .025 .025" type="box"/>
|
|
</worldbody></mujoco>""")
|
|
geometry = dict(
|
|
pos1=wp.vec3(.065118454396725, -.125125020742416, .124963559210300),
|
|
mat1=wp.mat33(.996357858181000, .085266821086407, -.000942531623878,
|
|
-.085266284644604, .996358215808868, .000591202871874,
|
|
.000989508931525, -.000508683384396, .999999582767487),
|
|
pos2=wp.vec3(.065104484558105, -.124979749321938, .174992129206657),
|
|
mat2=wp.mat33(.996556758880615, -.082913912832737, -.000453041866422,
|
|
.082915119826794, .996536433696747, .006357696373016,
|
|
-.000075668765930, -.006373368669301, .999979794025421),
|
|
)
|
|
results = {}
|
|
for capacity in (5, 24, EPA_HORIZON_CAPACITY):
|
|
overflow = wp.zeros(1, dtype=int)
|
|
results[capacity] = _geom_dist(model, data, 0, 1, horizon_size=capacity,
|
|
warn_overflow=False, overflow_out=overflow, **geometry)
|
|
assert bool(int(overflow.numpy()[0]) & OverflowType.EPA_HORIZON) == (capacity == 5)
|
|
assert results[128][0] == pytest.approx(-.00011578822, abs=1e-6)
|
|
assert results[128][1] > 0
|
|
for expected, actual in zip(results[24], results[128], strict=True):
|
|
np.testing.assert_allclose(actual, expected, atol=1e-8, rtol=0)
|