Files
chenlin ceca51b0d8
Checks / Layout, translation and Colab checks (push) Has been cancelled
Checks / Ruff (push) Has been cancelled
Checks / Framework tests (native backend, CPU) (push) Has been cancelled
Checks / Portable Rust controller (push) Has been cancelled
Improve generic robot onboarding, walking controls and end-effector interaction
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.
2026-11-06 10:54:58 +08:00

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)