255 lines
10 KiB
Python
Executable File
255 lines
10 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import json
|
|
import math
|
|
import sys
|
|
import time
|
|
import xml.etree.ElementTree as ET
|
|
from pathlib import Path
|
|
from typing import Any
|
|
|
|
import numpy as np
|
|
import trimesh
|
|
|
|
|
|
SCRIPT_DIR = Path(__file__).resolve().parent
|
|
APP_ROOT = SCRIPT_DIR.parent
|
|
SRC_ROOT = APP_ROOT / "src"
|
|
if str(SRC_ROOT) not in sys.path:
|
|
sys.path.insert(0, str(SRC_ROOT))
|
|
|
|
from real2sim_platform.config import ensure_workspace, load_config # noqa: E402
|
|
from real2sim_platform.store import PlatformStore # noqa: E402
|
|
|
|
|
|
SCENE_ID = "1f2618bdfcac4e45456569980716405a"
|
|
PACKAGE_SLUG = "parlour_rm65_pick_place"
|
|
|
|
|
|
def main() -> int:
|
|
parser = argparse.ArgumentParser(description="Generate a browser GLB preview from the MotrixSim parlour + RM65 scaffold package.")
|
|
parser.add_argument("--scene-id", default=SCENE_ID)
|
|
parser.add_argument("--max-parlour-meshes", type=int, default=240, help="Limit parlour meshes to keep the browser preview responsive.")
|
|
parser.add_argument("--skip-rm65", action="store_true")
|
|
args = parser.parse_args()
|
|
|
|
config = load_config()
|
|
ensure_workspace(config)
|
|
store = PlatformStore(config.db_path)
|
|
scene = store.get_scene(args.scene_id)
|
|
package_dir = Path(str((scene.get("metadata") or {}).get("package") or config.workspace_dir / "motrixsim_assets" / PACKAGE_SLUG))
|
|
manifest_path = Path(str(scene.get("manifest_path") or package_dir / "motrixsim_scene_manifest.json"))
|
|
if not manifest_path.is_file():
|
|
raise SystemExit(f"manifest not found: {manifest_path}")
|
|
|
|
output_dir = package_dir / "viewer_assets"
|
|
output_dir.mkdir(parents=True, exist_ok=True)
|
|
output_path = output_dir / "motrixsim_parlour_rm65_preview.glb"
|
|
report_path = output_dir / "motrixsim_preview_report.json"
|
|
|
|
scene_glb = trimesh.Scene()
|
|
report: dict[str, Any] = {
|
|
"schema": "real2sim.motrixsim_preview_report.v1",
|
|
"created_at": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
|
"inputs": [],
|
|
"warnings": [],
|
|
}
|
|
|
|
parlour_xml = package_dir / "assets/parlour/parlour.xml"
|
|
parlour_stats = add_mjcf_visuals(scene_glb, parlour_xml, max_meshes=args.max_parlour_meshes, name_prefix="parlour")
|
|
report["inputs"].append({"role": "parlour", "xml": str(parlour_xml), **parlour_stats})
|
|
|
|
if not args.skip_rm65:
|
|
rm65_xml = package_dir / "assets/rm65_open_cabinet/mjx_rm65.xml"
|
|
rm65_base = translation_matrix(np.array([0.8, 0.0, 0.55]))
|
|
rm65_stats = add_mjcf_visuals(scene_glb, rm65_xml, root_transform=rm65_base, max_meshes=80, name_prefix="rm65")
|
|
report["inputs"].append({"role": "rm65", "xml": str(rm65_xml), **rm65_stats})
|
|
|
|
if not scene_glb.geometry:
|
|
raise SystemExit("no mesh geometry was loaded from MotrixSim package")
|
|
|
|
data = scene_glb.export(file_type="glb")
|
|
output_path.write_bytes(data)
|
|
report["output"] = {"path": str(output_path), "size_bytes": output_path.stat().st_size, "geometry_count": len(scene_glb.geometry)}
|
|
report_path.write_text(json.dumps(report, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
|
|
|
|
artifact = store.add_artifact(task_id=None, name=output_path.name, path=output_path, kind="mesh")
|
|
report_artifact = store.add_artifact(task_id=None, name=report_path.name, path=report_path, kind="json")
|
|
update_manifest(manifest_path, output_path, artifact["id"], report_path, report_artifact["id"], report)
|
|
print(json.dumps({"ok": True, "scene_id": args.scene_id, "glb": str(output_path), "report": str(report_path), "geometry_count": len(scene_glb.geometry), "size_bytes": output_path.stat().st_size}, ensure_ascii=False, indent=2))
|
|
return 0
|
|
|
|
|
|
def add_mjcf_visuals(
|
|
scene: trimesh.Scene,
|
|
xml_path: Path,
|
|
*,
|
|
root_transform: np.ndarray | None = None,
|
|
max_meshes: int,
|
|
name_prefix: str,
|
|
) -> dict[str, Any]:
|
|
root = ET.parse(xml_path).getroot()
|
|
asset_dir = xml_path.parent / (root.find("compiler").attrib.get("assetdir", "") if root.find("compiler") is not None else "")
|
|
mesh_defs = {
|
|
item.attrib["name"]: asset_dir / item.attrib["file"]
|
|
for item in root.findall("./asset/mesh")
|
|
if item.attrib.get("name") and item.attrib.get("file")
|
|
}
|
|
stats = {"mesh_defs": len(mesh_defs), "loaded": 0, "missing": 0, "skipped": 0}
|
|
world = root.find("worldbody")
|
|
if world is None:
|
|
return stats
|
|
root_matrix = root_transform if root_transform is not None else np.eye(4)
|
|
for node in list(world):
|
|
traverse_mjcf_node(scene, node, mesh_defs, root_matrix, stats, max_meshes=max_meshes, name_prefix=name_prefix)
|
|
return stats
|
|
|
|
|
|
def traverse_mjcf_node(
|
|
scene: trimesh.Scene,
|
|
node: ET.Element,
|
|
mesh_defs: dict[str, Path],
|
|
parent_transform: np.ndarray,
|
|
stats: dict[str, int],
|
|
*,
|
|
max_meshes: int,
|
|
name_prefix: str,
|
|
) -> None:
|
|
current = parent_transform
|
|
if node.tag == "body":
|
|
current = parent_transform @ node_transform(node.attrib)
|
|
elif node.tag == "geom":
|
|
mesh_name = node.attrib.get("mesh")
|
|
geom_class = node.attrib.get("class", "")
|
|
if mesh_name and mesh_name in mesh_defs and (geom_class != "collision" or node.attrib.get("group") == "2"):
|
|
if stats["loaded"] >= max_meshes:
|
|
stats["skipped"] += 1
|
|
else:
|
|
mesh_path = mesh_defs[mesh_name]
|
|
if mesh_path.is_file():
|
|
try:
|
|
mesh = load_mesh(mesh_path)
|
|
mesh.apply_transform(parent_transform @ node_transform(node.attrib))
|
|
scene.add_geometry(mesh, geom_name=f"{name_prefix}_{mesh_name}_{stats['loaded']}")
|
|
stats["loaded"] += 1
|
|
except Exception:
|
|
stats["skipped"] += 1
|
|
else:
|
|
stats["missing"] += 1
|
|
for child in list(node):
|
|
traverse_mjcf_node(scene, child, mesh_defs, current, stats, max_meshes=max_meshes, name_prefix=name_prefix)
|
|
|
|
|
|
def load_mesh(path: Path) -> trimesh.Trimesh:
|
|
loaded = trimesh.load(path, force="mesh", process=False)
|
|
if isinstance(loaded, trimesh.Scene):
|
|
return trimesh.util.concatenate(tuple(loaded.geometry.values()))
|
|
return loaded
|
|
|
|
|
|
def node_transform(attrs: dict[str, str]) -> np.ndarray:
|
|
transform = np.eye(4)
|
|
if "pos" in attrs:
|
|
transform = transform @ translation_matrix(parse_vec(attrs["pos"], 3))
|
|
if "quat" in attrs:
|
|
transform = transform @ quat_matrix(parse_vec(attrs["quat"], 4))
|
|
elif "euler" in attrs:
|
|
transform = transform @ euler_matrix(parse_vec(attrs["euler"], 3))
|
|
if "scale" in attrs:
|
|
scale = parse_vec(attrs["scale"], 3)
|
|
transform = transform @ np.diag([scale[0], scale[1], scale[2], 1.0])
|
|
return transform
|
|
|
|
|
|
def parse_vec(value: str, count: int) -> np.ndarray:
|
|
parts = [float(item) for item in value.split()]
|
|
if len(parts) == 1 and count == 3:
|
|
parts = parts * 3
|
|
while len(parts) < count:
|
|
parts.append(0.0)
|
|
return np.array(parts[:count], dtype=float)
|
|
|
|
|
|
def translation_matrix(vec: np.ndarray) -> np.ndarray:
|
|
matrix = np.eye(4)
|
|
matrix[:3, 3] = vec[:3]
|
|
return matrix
|
|
|
|
|
|
def quat_matrix(q: np.ndarray) -> np.ndarray:
|
|
w, x, y, z = q
|
|
norm = math.sqrt(w * w + x * x + y * y + z * z)
|
|
if norm <= 0:
|
|
return np.eye(4)
|
|
w, x, y, z = w / norm, x / norm, y / norm, z / norm
|
|
matrix = np.eye(4)
|
|
matrix[:3, :3] = np.array(
|
|
[
|
|
[1 - 2 * y * y - 2 * z * z, 2 * x * y - 2 * z * w, 2 * x * z + 2 * y * w],
|
|
[2 * x * y + 2 * z * w, 1 - 2 * x * x - 2 * z * z, 2 * y * z - 2 * x * w],
|
|
[2 * x * z - 2 * y * w, 2 * y * z + 2 * x * w, 1 - 2 * x * x - 2 * y * y],
|
|
]
|
|
)
|
|
return matrix
|
|
|
|
|
|
def euler_matrix(euler: np.ndarray) -> np.ndarray:
|
|
x, y, z = euler
|
|
cx, cy, cz = math.cos(x), math.cos(y), math.cos(z)
|
|
sx, sy, sz = math.sin(x), math.sin(y), math.sin(z)
|
|
rx = np.array([[1, 0, 0, 0], [0, cx, -sx, 0], [0, sx, cx, 0], [0, 0, 0, 1]])
|
|
ry = np.array([[cy, 0, sy, 0], [0, 1, 0, 0], [-sy, 0, cy, 0], [0, 0, 0, 1]])
|
|
rz = np.array([[cz, -sz, 0, 0], [sz, cz, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
|
|
return rz @ ry @ rx
|
|
|
|
|
|
def update_manifest(manifest_path: Path, glb_path: Path, artifact_id: str, report_path: Path, report_artifact_id: str, report: dict[str, Any]) -> None:
|
|
manifest = json.loads(manifest_path.read_text(encoding="utf-8"))
|
|
assets = [item for item in manifest.get("assets", []) if not (isinstance(item, dict) and item.get("name") == glb_path.name)]
|
|
glb_asset = {
|
|
"name": glb_path.name,
|
|
"role": "viewer_preview",
|
|
"kind": "mesh",
|
|
"path": str(glb_path),
|
|
"relative_path": str(glb_path.relative_to(manifest_path.parent)),
|
|
"url": f"/api/artifacts/{artifact_id}/content",
|
|
"exists": True,
|
|
"size_bytes": glb_path.stat().st_size,
|
|
"size_label": format_bytes(glb_path.stat().st_size),
|
|
"autoload": True,
|
|
"generated": True,
|
|
}
|
|
report_asset = {
|
|
"name": report_path.name,
|
|
"role": "preview_report",
|
|
"kind": "metadata",
|
|
"path": str(report_path),
|
|
"relative_path": str(report_path.relative_to(manifest_path.parent)),
|
|
"url": f"/api/artifacts/{report_artifact_id}/content",
|
|
"exists": True,
|
|
"size_bytes": report_path.stat().st_size,
|
|
"size_label": format_bytes(report_path.stat().st_size),
|
|
"generated": True,
|
|
}
|
|
manifest["assets"] = [glb_asset, report_asset, *assets]
|
|
manifest["viewer_assets"] = {"primary": glb_asset, "report": report_asset, "generator": "generate_motrixsim_preview.py"}
|
|
manifest["preview_generation"] = report
|
|
manifest["updated_at"] = time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime())
|
|
manifest_path.write_text(json.dumps(manifest, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
|
|
|
|
|
|
def format_bytes(value: int) -> str:
|
|
units = ["B", "KB", "MB", "GB"]
|
|
size = float(value)
|
|
for unit in units:
|
|
if size < 1024 or unit == units[-1]:
|
|
return f"{size:.1f} {unit}" if unit != "B" else f"{int(size)} B"
|
|
size /= 1024
|
|
return f"{value} B"
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|