Files
UniversalTrain-ai/tests/test_build_fallback_hulls.py
chenlin c32df70326
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
release: v0.1 generic robot training and application platform
2026-11-05 11:05:01 +08:00

107 lines
4.6 KiB
Python

"""Fallback collision hulls reduce vertices without moving visual geometry."""
from pathlib import Path
import numpy as np
import pytest
from scipy.spatial import ConvexHull, cKDTree
mujoco = pytest.importorskip("mujoco")
from mjrl.onboarding.build import build_mjcf
from mjrl.onboarding.hulls import fibonacci_directions
def _source(directory: Path, *, authored_collision=False):
vertices = fibonacci_directions(500) * [.13, .07, .09] + [.04, -.03, .02]
hull = ConvexHull(vertices)
mesh = directory / "shell.obj"
lines = ["v " + " ".join(map(str, row)) for row in vertices]
for face, equation in zip(hull.simplices, hull.equations):
a, b, c = vertices[face]
if np.dot(np.cross(b - a, c - a), equation[:3]) < 0:
face = face[[0, 2, 1]]
lines.append("f " + " ".join(str(i + 1) for i in face))
mesh.write_text("\n".join(lines))
shape = '''<origin xyz=".1 -.2 .3" rpy=".2 -.3 .4"/>
<geometry><mesh filename="shell.obj" scale="2 .5 1.5"/></geometry>'''
collision = f'<collision>{shape}</collision>' if authored_collision else ''
path = directory / "robot.urdf"
path.write_text(f'''<robot name="test"><link name="base">
<inertial><mass value="1"/><inertia ixx=".01" iyy=".01" izz=".01"
ixy="0" ixz="0" iyz="0"/></inertial>
<visual>{shape}</visual>{collision}</link></robot>''')
return path, mesh
def _world_vertices(model, name):
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)
gid = model.geom(name).id
mid = model.geom_dataid[gid]
start, count = model.mesh_vertadr[mid], model.mesh_vertnum[mid]
return (model.mesh_vert[start:start + count] @ data.geom_xmat[gid].reshape(3, 3).T
+ data.geom_xpos[gid])
def test_fallback_simplifies_only_collision_and_preserves_frame(tmp_path):
source, mesh = _source(tmp_path)
original = source.read_bytes(), mesh.read_bytes()
before = tmp_path / "before.xml"
build_mjcf(source, before, floating_base=True)
output = tmp_path / "out" / "robot.xml"
report = build_mjcf(source, output, visual_fallback=True, floating_base=True)
entry, = report["collision_hulls"]
assert entry["status"] == "simplified"
assert entry["source_vertices"] == 500
assert 4 <= entry["hull_vertices"] <= 64
assert entry["max_inward_shrink_m"] > 0
model = mujoco.MjModel.from_xml_path(str(output))
assert model.geom("base_visual").dataid != model.geom("base_meshcol").dataid
baseline = mujoco.MjModel.from_xml_path(str(before))
visual = _world_vertices(model, "base_visual")
np.testing.assert_allclose(visual, _world_vertices(baseline, "base_visual"), atol=1e-7)
collision = _world_vertices(model, "base_meshcol")
# Sampled hull vertices must coincide with source vertices even after
# nonuniform scaling, principal-axis alignment, and rotated geom origins.
distances, _ = cKDTree(visual).query(collision)
assert distances.max() < 1e-6
np.testing.assert_allclose(model.body_mass, baseline.body_mass)
np.testing.assert_allclose(model.body_inertia, baseline.body_inertia)
assert (source.read_bytes(), mesh.read_bytes()) == original
assert not list(output.parent.glob('*.stl')) # Hulls are self-contained inline assets.
@pytest.mark.parametrize("options,status", [
({"collision_hull_exclude": ("shell",)}, "excluded"),
({"collision_hull_dirs": 0}, "simplified"),
])
def test_preserve_support_mesh_or_use_full_hull(tmp_path, options, status):
source, _ = _source(tmp_path)
output = tmp_path / "robot.xml"
report = build_mjcf(source, output, visual_fallback=True, **options)
entry, = report["collision_hulls"]
assert entry["status"] == status
model = mujoco.MjModel.from_xml_path(str(output))
collision = _world_vertices(model, "base_meshcol")
visual = _world_vertices(model, "base_visual")
assert len(collision) == len(visual) == 500
assert cKDTree(visual).query(collision)[0].max() < 1e-6
def test_authored_collision_is_not_replaced(tmp_path):
source, _ = _source(tmp_path, authored_collision=True)
output = tmp_path / "robot.xml"
report = build_mjcf(source, output, visual_fallback=True)
assert report["collision_hulls"] == []
model = mujoco.MjModel.from_xml_path(str(output))
assert model.geom("base_visual").dataid == model.geom("base_meshcol").dataid
def test_invalid_hull_density_does_not_publish(tmp_path):
source, _ = _source(tmp_path)
output = tmp_path / "robot.xml"
with pytest.raises(ValueError, match="collision_hull_dirs"):
build_mjcf(source, output, visual_fallback=True, collision_hull_dirs=3)
assert not output.exists()