107 lines
4.6 KiB
Python
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()
|