227 lines
9.3 KiB
Python
227 lines
9.3 KiB
Python
#!/usr/bin/env python3
|
|
"""Replace selected MJCF collision meshes with standalone convex hull STLs.
|
|
|
|
The tool reports vertex reduction and maximum outward distance from the new hull's
|
|
face planes to the source points. A sampled hull stays inside the original hull.
|
|
Visual geoms remain unchanged. Exclude the original meshes for intended ground
|
|
contacts if their contact timing and shape must be preserved.
|
|
|
|
Usage:
|
|
python tools/hull_collision.py --model robot.xml --exclude foot_mesh_left,foot_mesh_right
|
|
python tools/hull_collision.py --model robot.xml -k 64 --exclude foot_mesh_left --apply
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import os
|
|
import re
|
|
import sys
|
|
from pathlib import Path
|
|
|
|
from mjrl.onboarding.hulls import (
|
|
fibonacci_directions,
|
|
hull_decimate,
|
|
max_inward_shrink,
|
|
read_stl,
|
|
write_stl,
|
|
)
|
|
|
|
__all__ = [
|
|
"fibonacci_directions", "hull_decimate", "main", "max_inward_shrink",
|
|
"patch_xml", "read_stl", "write_stl",
|
|
]
|
|
|
|
|
|
# ── Finding the targets ───────────────────────────────────────────────────
|
|
|
|
|
|
def collision_mesh_targets(spec, geom_suffix: str) -> list[tuple[str, str]]:
|
|
"""Find the `(geom name, mesh name)` pairs to process.
|
|
|
|
Selected by geom-name suffix rather than by contype: one MJCF often carries
|
|
several collision schemes at once (the hexapod has mesh, capsule and hybrid),
|
|
and the layer above chooses between them by zeroing contype at the spec stage.
|
|
**The compiled contype therefore depends on which scheme was selected**, so
|
|
using it as the criterion would make the result vary with the caller. A name
|
|
suffix is the asset author's explicit marking and is far more stable.
|
|
"""
|
|
out = []
|
|
for body in spec.bodies:
|
|
for g in body.geoms:
|
|
if g.name.endswith(geom_suffix) and g.meshname:
|
|
out.append((g.name, g.meshname))
|
|
return out
|
|
|
|
|
|
def mesh_files(spec) -> dict[str, str]:
|
|
return {me.name: me.file for me in spec.meshes if me.file}
|
|
|
|
|
|
# ── Patching the XML ──────────────────────────────────────────────────────
|
|
|
|
|
|
def patch_xml(path: Path, pairs: list[tuple[str, str, str]], suffix: str,
|
|
file_prefix: str = "") -> int:
|
|
"""Repoint `<geom name=A ... mesh=B>` at `B+suffix` and add the mesh assets.
|
|
|
|
Deliberately a **targeted text substitution** rather than `spec.to_xml()`: the
|
|
latter reflows the entire file, losing hand-maintained comments, indentation
|
|
and element order, and leaving a reviewer unable to see what changed.
|
|
|
|
Idempotent: geoms already pointing at `*suffix` are skipped.
|
|
|
|
Args:
|
|
pairs: `[(geom name, old mesh name, new mesh name), ...]`
|
|
Returns:
|
|
The number of geoms actually changed.
|
|
"""
|
|
text = path.read_text(encoding="utf-8")
|
|
changed = 0
|
|
for geom_name, old_mesh, new_mesh in pairs:
|
|
# Substitute mesh= only inside that one <geom ...> element
|
|
pat = re.compile(
|
|
r'(<geom\b[^>]*\bname\s*=\s*"' + re.escape(geom_name) + r'"[^>]*?)'
|
|
r'\bmesh\s*=\s*"' + re.escape(old_mesh) + r'"',
|
|
re.S,
|
|
)
|
|
text, n = pat.subn(lambda m: m.group(1) + f'mesh="{new_mesh}"', text)
|
|
if n > 1:
|
|
raise SystemExit(f"geom {geom_name} matched {n} times in the XML; "
|
|
f"refusing to edit")
|
|
changed += n
|
|
|
|
# Add <mesh name=... file=.../> before </asset>, copying the indentation of an
|
|
# existing <mesh> line in the same block so generated assets line up with
|
|
# hand-written ones in a diff.
|
|
want = [new for _, _, new in pairs if f'name="{new}"' not in text]
|
|
if want:
|
|
close = re.search(r"^([ \t]*)</asset>", text, re.M)
|
|
if close is None:
|
|
raise SystemExit("the XML has no </asset>; nowhere to put the mesh assets")
|
|
existing = re.search(r"^([ \t]*)<mesh\b", text, re.M)
|
|
pad = existing.group(1) if existing else close.group(1) + " "
|
|
block = "".join(
|
|
f'{pad}<mesh name="{n}" file="{file_prefix}{n}.STL"/>\n'
|
|
for n in dict.fromkeys(want)
|
|
)
|
|
text = text[: close.start()] + block + text[close.start() :]
|
|
path.write_text(text, encoding="utf-8")
|
|
return changed
|
|
|
|
|
|
# ── Main ──────────────────────────────────────────────────────────────────
|
|
|
|
|
|
def main(argv: list[str] | None = None) -> int:
|
|
ap = argparse.ArgumentParser(
|
|
description="replace an MJCF's collision meshes with decimated convex hulls",
|
|
formatter_class=argparse.RawDescriptionHelpFormatter,
|
|
epilog=__doc__.split("Usage:", 1)[-1],
|
|
)
|
|
ap.add_argument("--model", required=True, type=Path, help="path to the MJCF")
|
|
ap.add_argument(
|
|
"-k", "--dirs", type=int, default=64,
|
|
help="number of sampled directions, setting the decimation density "
|
|
"(default 64; 0 = the full hull, no decimation)",
|
|
)
|
|
ap.add_argument(
|
|
"--exclude", default="",
|
|
help="comma-separated mesh names to leave alone -- **put the meshes that "
|
|
"actually touch the ground here**",
|
|
)
|
|
ap.add_argument("--geom-suffix", default="_meshcol",
|
|
help="name suffix of the collision geoms")
|
|
ap.add_argument("--out-suffix", default="_col",
|
|
help="name suffix for the new mesh assets")
|
|
ap.add_argument("--mesh-dir", type=Path, default=None,
|
|
help="STL directory (inferred from the compiler meshdir by default)")
|
|
ap.add_argument("--out-dir", type=Path, default=None,
|
|
help="where to write the hulls, if not beside the source meshes. "
|
|
"The written <mesh file=...> is this path relative to the "
|
|
"compiler's meshdir, so the source tree can stay read-only")
|
|
ap.add_argument("--apply", action="store_true",
|
|
help="actually write the STLs and edit the XML "
|
|
"(report only by default)")
|
|
args = ap.parse_args(argv)
|
|
|
|
import mujoco
|
|
|
|
k = None if args.dirs == 0 else args.dirs
|
|
spec = mujoco.MjSpec.from_file(str(args.model))
|
|
meshdir = args.model.parent / spec.meshdir
|
|
mesh_dir = args.mesh_dir or meshdir
|
|
out_dir = args.out_dir or mesh_dir
|
|
# What the XML must say to find a hull, given where the compiler starts.
|
|
file_prefix = ""
|
|
if out_dir != meshdir:
|
|
file_prefix = Path(os.path.relpath(out_dir, meshdir)).as_posix() + "/"
|
|
excluded = {s for s in args.exclude.split(",") if s}
|
|
|
|
targets = collision_mesh_targets(spec, args.geom_suffix)
|
|
if not targets:
|
|
print(f"no geom found whose name ends in {args.geom_suffix!r} and has a "
|
|
f"mesh.", file=sys.stderr)
|
|
return 1
|
|
files = mesh_files(spec)
|
|
|
|
todo, skipped = [], []
|
|
for geom_name, mesh_name in targets:
|
|
if mesh_name in excluded or mesh_name.endswith(args.out_suffix):
|
|
skipped.append((geom_name, mesh_name))
|
|
continue
|
|
if mesh_name not in files:
|
|
raise SystemExit(f"mesh {mesh_name} has no file (inline vertices?)")
|
|
todo.append((geom_name, mesh_name))
|
|
|
|
print(f"model {args.model} STL directory {mesh_dir}")
|
|
print(f"decimation: {'full hull, none' if k is None else f'k={k} directions'}"
|
|
f" processing {len(todo)} / skipping {len(skipped)}")
|
|
if skipped:
|
|
print(" skipped: " + ", ".join(sorted({m for _, m in skipped})))
|
|
if not todo:
|
|
print("\nNothing to process -- everything is either excluded or already a "
|
|
"decimated hull.")
|
|
return 0
|
|
print()
|
|
print(f" {'mesh':32s} {'verts':>8s} {'hull':>6s} {'saved':>6s} {'max shrink':>12s}")
|
|
print(" " + "-" * 70)
|
|
|
|
pairs, total_before, total_after, worst = [], 0, 0, 0.0
|
|
for geom_name, mesh_name in todo:
|
|
verts = read_stl(mesh_dir / files[mesh_name])
|
|
hull = hull_decimate(verts, k)
|
|
err = max_inward_shrink(verts, hull)
|
|
worst = max(worst, err)
|
|
total_before += len(verts)
|
|
total_after += len(hull)
|
|
print(f" {mesh_name:32s} {len(verts):8d} {len(hull):6d} "
|
|
f"{1 - len(hull) / len(verts):5.1%} {err * 1e3:8.2f} mm")
|
|
pairs.append((geom_name, mesh_name, f"{mesh_name}{args.out_suffix}", hull))
|
|
|
|
print(" " + "-" * 70)
|
|
print(f" {'total':32s} {total_before:8d} {total_after:6d} "
|
|
f"{1 - total_after / max(total_before, 1):5.1%} {worst * 1e3:9.2f} mm max")
|
|
print()
|
|
|
|
if not args.apply:
|
|
print("(report only; pass --apply to write the STLs and edit the XML)")
|
|
return 0
|
|
|
|
for _, mesh_name, new_name, hull in pairs:
|
|
ntri = write_stl(out_dir / f"{new_name}.STL", hull)
|
|
print(f" wrote {new_name}.STL ({ntri} triangles)")
|
|
out_dir.mkdir(parents=True, exist_ok=True)
|
|
n = patch_xml(args.model, [(g, o, nw) for g, o, nw, _ in pairs],
|
|
args.out_suffix, file_prefix)
|
|
print(f" repointed {n} geoms and added the <mesh> assets")
|
|
|
|
# Compile once, to confirm the result still works
|
|
m = mujoco.MjModel.from_xml_path(str(args.model))
|
|
print(f" recompiled successfully: ngeom={m.ngeom} nmesh={m.nmesh}")
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|