Files
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

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())