MotrixSimWeb/server/headless_motrix_bridge.py
2026-06-26 16:54:17 +08:00

1746 lines
69 KiB
Python

#!/usr/bin/env python3
"""Headless MotrixSim bridge for the universal Three.js preview.
Run this on a Linux server with MotrixSim installed. It exposes:
- /manifest: Three.js scene manifest generated from the XML scene.
- /state: live MotrixSim body pose state.
- /control: pause/resume/reset/step controls for the simulation loop.
- /health: process health.
No display server is required for state streaming. Native simulator rendering is
not used here; the browser does the visual rendering.
"""
from __future__ import annotations
import argparse
import gzip
import hashlib
import json
import mimetypes
import re
import socket
import statistics
import sys
import threading
import time
import xml.etree.ElementTree as ET
import math
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
from pathlib import Path
from typing import Any
from urllib.parse import parse_qs, unquote, urlparse
sys.path.insert(0, str(Path(__file__).resolve().parents[1] / "tools"))
from export_mjcf_manifest import collect_manifest # noqa: E402
DEFAULT_ASSET_ROOT = Path("/opt/Motphys/motrixsim-docs/examples/assets")
DEFAULT_G1_SCENE = DEFAULT_ASSET_ROOT / "g1/g1_mjx_feetonly.xml"
DEFAULT_STORE_SCENE = DEFAULT_ASSET_ROOT / "store/store.xml"
DEFAULT_G1_COMPOSE_POSE = [1.85, -0.95, 0.0]
DEFAULT_COMPOSE_SCENE_ID = "g1-store-blocks-physics"
SUPPORTED_COMPOSE_SCENE_IDS = {
DEFAULT_COMPOSE_SCENE_ID,
"g1-store-floor-staging-physics",
"g1-store-inbound-pallet-physics",
"g1-store-sorting-physics",
}
COMPOSE_PROP_SPECS: dict[str, dict[str, Any]] = {
"red-cube": {
"material": "demo_red",
"geom": {"type": "box", "size": "0.06 0.06 0.06", "density": "350"},
"z": 0.12,
},
"blue-crate": {
"material": "demo_blue",
"geom": {"type": "box", "size": "0.05 0.08 0.045", "density": "350"},
"z": 0.11,
"euler": "0 0 0.4",
},
"green-ball": {
"material": "demo_green",
"geom": {"type": "sphere", "size": "0.075", "density": "260"},
"z": 0.16,
},
"yellow-capsule": {
"material": "demo_yellow",
"geom": {"type": "capsule", "size": "0.035 0.11", "density": "260"},
"z": 0.12,
"euler": "0 1.5708 0.25",
},
"pallet": {
"material": "demo_pallet",
"geom": {"type": "box", "size": "0.21 0.14 0.04", "density": "450"},
"z": 0.04,
},
"store-bottle-350": {
"visual": {"mesh": "V_Bottle_350", "material": "Bottle_A"},
"collisions": [
{"type": "box", "size": "0.0265 0.0265 0.0579022", "pos": "0 0 0.0579022", "density": "280"},
{"type": "cylinder", "size": "0.0197049 0.0285978", "pos": "0 0 0.144402", "density": "280"},
],
"z": 0.0,
},
"store-bottle-360": {
"visual": {"mesh": "V_Bottle_360", "material": "Bottle_B"},
"collisions": [
{"type": "cylinder", "size": "0.029 0.0673171", "pos": "0 0 0.0673171", "density": "280"},
{"type": "cylinder", "size": "0.0145 0.0266829", "pos": "0 0 0.161317", "density": "280"},
],
"z": 0.0,
},
"store-can-250": {
"visual": {"mesh": "V_Cans_N250", "material": "Cans_A"},
"collisions": [
{"type": "cylinder", "size": "0.0331 0.046", "pos": "0 0 0.046", "density": "320"},
],
"z": 0.0,
},
"store-can-330": {
"visual": {"mesh": "V_Cans_N330", "material": "Cans_B"},
"collisions": [
{"type": "cylinder", "size": "0.0331 0.0576", "pos": "0 0 0.0576", "density": "320"},
],
"z": 0.0,
},
"store-tetra-250": {
"visual": {"mesh": "V_Tetra_250_F", "material": "Tetra_A"},
"collisions": [
{"type": "box", "size": "0.0324065 0.020046 0.053404", "pos": "0 0 0.053404", "density": "260"},
],
"z": 0.0,
},
"store-tetra-500": {
"visual": {"mesh": "V_Tetra_500", "material": "Tetra_D"},
"collisions": [
{"type": "box", "size": "0.0363751 0.03602 0.049006", "pos": "0 0 0.049006", "density": "260"},
{
"type": "box",
"size": "0.03602 0.0254984 0.0254984",
"pos": "0 0 0.096384",
"quat": "0.9238795 -0.3826835 0 0",
"density": "260",
},
],
"z": 0.0,
},
}
PRODUCT_BODY_PREFIXES = (
"Bottle_",
"Cans_",
"Can_",
"Tetra_",
"Milk_",
"Carton_",
"Box_",
"Drink_",
)
JSON_COMPRESSION_MIN_BYTES = 1024
ASSET_COMPRESSION_MIN_BYTES = 16 * 1024
COMPRESSIBLE_ASSET_SUFFIXES = {
".dae",
".glb",
".gltf",
".mtl",
".obj",
".stl",
".urdf",
".xml",
}
EXPOSED_HEADERS = (
"X-Motrix-State-Hash, ETag, X-Motrix-Json-Bytes, "
"X-Motrix-Wire-Bytes, X-Motrix-Encoding, X-Motrix-Asset-Bytes, "
"X-Motrix-Asset-Wire-Bytes, X-Motrix-Asset-Encoding, "
"X-Motrix-Asset-Cache"
)
def safe_name(raw: Any, fallback: str) -> str:
text = str(raw or fallback)
cleaned = "".join(ch if ch.isalnum() or ch in {"_", "-"} else "_" for ch in text)
return cleaned.strip("_") or fallback
def accepts_gzip(headers: Any) -> bool:
return "gzip" in (headers.get("Accept-Encoding") or "").lower()
def should_compress_asset(file_path: Path, byte_count: int) -> bool:
return byte_count >= ASSET_COMPRESSION_MIN_BYTES and file_path.suffix.lower() in COMPRESSIBLE_ASSET_SUFFIXES
def parse_float_list(raw: str | None, default: list[float]) -> list[float]:
if not raw:
return list(default)
values = [float(value) for value in raw.split()]
if len(values) != len(default):
return list(default)
return values
def format_float_list(values: list[float]) -> str:
return " ".join(f"{value:.9g}" for value in values)
def quat_conjugate(quat: list[float]) -> list[float]:
return [quat[0], -quat[1], -quat[2], -quat[3]]
def quat_normalize(quat: list[float]) -> list[float]:
length = math.sqrt(sum(value * value for value in quat))
if length <= 1e-12:
return [1.0, 0.0, 0.0, 0.0]
return [value / length for value in quat]
def quat_mul(left: list[float], right: list[float]) -> list[float]:
lw, lx, ly, lz = left
rw, rx, ry, rz = right
return [
lw * rw - lx * rx - ly * ry - lz * rz,
lw * rx + lx * rw + ly * rz - lz * ry,
lw * ry - lx * rz + ly * rw + lz * rx,
lw * rz + lx * ry - ly * rx + lz * rw,
]
def quat_rotate(quat: list[float], vec: list[float]) -> list[float]:
rotated = quat_mul(quat_mul(quat, [0.0, *vec]), quat_conjugate(quat))
return rotated[1:4]
def world_to_local_pose(
body_pos: list[float],
body_quat: list[float],
geom_pos: list[float],
geom_quat: list[float],
) -> tuple[list[float], list[float]]:
inv_quat = quat_conjugate(quat_normalize(body_quat))
delta = [geom_pos[index] - body_pos[index] for index in range(3)]
local_pos = quat_rotate(inv_quat, delta)
local_quat = quat_normalize(quat_mul(inv_quat, quat_normalize(geom_quat)))
return local_pos, local_quat
def is_direct_visual_geom(element: ET.Element) -> bool:
if element.tag != "geom":
return False
class_name = element.get("class") or ""
if class_name.endswith("_visual") or class_name == "store_visual":
return True
mesh_name = element.get("mesh") or ""
return mesh_name.startswith("V_")
def wrap_worldbody_visual_geom_groups(root: ET.Element) -> int:
"""Wrap direct shelf geoms in fixed bodies for state binding without making them dynamic."""
worldbody = root.find("worldbody")
if worldbody is None:
return 0
children = list(worldbody)
rebuilt: list[ET.Element] = []
wrapped_count = 0
index = 0
while index < len(children):
child = children[index]
if not is_direct_visual_geom(child):
rebuilt.append(child)
index += 1
continue
group = [child]
index += 1
while index < len(children):
candidate = children[index]
if candidate.tag != "geom" or is_direct_visual_geom(candidate):
break
group.append(candidate)
index += 1
body_pos = parse_float_list(child.get("pos"), [0.0, 0.0, 0.0])
body_quat = quat_normalize(parse_float_list(child.get("quat"), [1.0, 0.0, 0.0, 0.0]))
mesh_name = child.get("mesh") or child.get("name") or f"world_geom_{wrapped_count}"
body = ET.Element("body", {
"name": safe_name(f"store_movable_{mesh_name}_{wrapped_count}", f"store_movable_{wrapped_count}"),
"pos": format_float_list(body_pos),
"quat": format_float_list(body_quat),
})
for geom in group:
geom_pos = parse_float_list(geom.get("pos"), [0.0, 0.0, 0.0])
geom_quat = quat_normalize(parse_float_list(geom.get("quat"), [1.0, 0.0, 0.0, 0.0]))
local_pos, local_quat = world_to_local_pose(body_pos, body_quat, geom_pos, geom_quat)
geom.set("pos", format_float_list(local_pos))
geom.set("quat", format_float_list(local_quat))
body.append(geom)
rebuilt.append(body)
wrapped_count += 1
if wrapped_count == 0:
return 0
worldbody[:] = rebuilt
return wrapped_count
def freeze_store_product_joints(root: ET.Element) -> int:
"""Turn the pre-stocked store inventory into fixed collision bodies.
The store example contains hundreds of free bodies already resting on shelf
plates. Leaving all of them dynamic makes the scene easy to avalanche when
the robot or a dragged object bumps the display. Productized composition
keeps stock inventory as fixed environment collision and reserves dynamic
motion for user-added props.
"""
frozen = 0
for body in root.iter("body"):
name = body.get("name") or ""
if not is_store_product_body(name):
continue
joints = [child for child in list(body) if child.tag in {"joint", "freejoint"}]
for joint in joints:
body.remove(joint)
if joints:
frozen += 1
return frozen
def write_namespaced_store_scene(store_scene: Path, output_path: Path, *, freeze_inventory: bool = True) -> Path:
tree = ET.parse(store_scene)
root = tree.getroot()
compiler = root.find("compiler")
asset_dir = compiler.get("assetdir") if compiler is not None else None
if compiler is not None:
root.remove(compiler)
asset_base = (store_scene.parent / asset_dir).resolve() if asset_dir else store_scene.parent.resolve()
for element in root.iter():
for attr_name in ("class", "childclass"):
value = element.get(attr_name)
if value and not value.startswith("store_"):
element.set(attr_name, f"store_{value}")
file_name = element.get("file")
if file_name and element.tag in {"mesh", "texture"}:
file_path = Path(file_name)
if not file_path.is_absolute():
element.set("file", (asset_base / file_name).resolve().as_posix())
wrap_worldbody_visual_geom_groups(root)
if freeze_inventory:
freeze_store_product_joints(root)
output_path.parent.mkdir(parents=True, exist_ok=True)
ET.indent(tree, space=" ")
tree.write(output_path, encoding="unicode")
return output_path
def shelf_body_index(body_name: str) -> int | None:
match = re.search(r"store_movable_V_Shelf_(?:Frame|Plat)_(\d+)$", body_name)
return int(match.group(1)) if match else None
def is_shelf_body(body_name: str) -> bool:
return shelf_body_index(body_name) is not None
def is_store_product_body(body_name: str) -> bool:
return body_name.startswith(PRODUCT_BODY_PREFIXES)
def is_robot_state_body(body_name: str) -> bool:
normalized = body_name.lower()
return any(
token in normalized
for token in (
"link",
"pelvis",
"waist",
"hip",
"knee",
"ankle",
"shoulder",
"elbow",
"wrist",
"torso",
"head",
)
)
def body_position(runtime: "MotrixRuntime", body_name: str) -> list[float] | None:
try:
return runtime.pose_for("body", body_name)["pos"]
except Exception:
return None
def nearest_shelf_group(x: float, shelf_centers: list[tuple[float, str]]) -> str | None:
if not shelf_centers:
return None
return min(shelf_centers, key=lambda item: abs(item[0] - x))[1]
def annotate_physics_roles(payload: dict[str, Any], runtime: "MotrixRuntime") -> None:
pose_editable = runtime.pose_editable_bodies
for item in payload.get("objects", []):
if not isinstance(item, dict):
continue
body_name = item.get("body")
if not isinstance(body_name, str):
item["physics"] = "visualOnly"
continue
if is_shelf_body(body_name):
item["physics"] = "fixed"
elif is_robot_state_body(body_name):
item["physics"] = "stateOnly"
elif body_name in pose_editable:
item["physics"] = "dynamic"
elif body_name in runtime.body_names:
item["physics"] = "fixed" if is_store_product_body(body_name) else "stateOnly"
else:
item["physics"] = "stateOnly"
def remove_visual_helper_objects(payload: dict[str, Any]) -> None:
payload["objects"] = [
item
for item in payload.get("objects", [])
if not (
isinstance(item, dict)
and item.get("name") == "combined_floor"
and not item.get("body")
)
]
def annotate_editable_groups(payload: dict[str, Any], runtime: "MotrixRuntime") -> None:
"""Annotate exported objects with product-level edit groups.
Shelf frame/plate bodies are anchors for inspection. Products are members
for grouping metadata; whether they are draggable is decided by their
physics role.
"""
frame_positions: list[tuple[int, float]] = []
for body_name in runtime.body_names:
if not body_name.startswith("store_movable_V_Shelf_Frame_"):
continue
index = shelf_body_index(body_name)
pos = body_position(runtime, body_name)
if index is None or pos is None:
continue
frame_positions.append((index, float(pos[0])))
if not frame_positions:
return
shelf_centers = [
(x, f"store-shelf-{rank + 1}")
for rank, (_index, x) in enumerate(sorted(frame_positions, key=lambda item: item[1]))
]
body_to_group: dict[str, tuple[str, str]] = {}
for body_name in runtime.body_names:
pos = body_position(runtime, body_name)
if pos is None:
continue
group_id = nearest_shelf_group(float(pos[0]), shelf_centers)
if not group_id:
continue
if is_shelf_body(body_name):
body_to_group[body_name] = (group_id, "anchor")
elif is_store_product_body(body_name):
body_to_group[body_name] = (group_id, "member")
for item in payload.get("objects", []):
if not isinstance(item, dict):
continue
body_name = item.get("body")
if not isinstance(body_name, str):
continue
group = body_to_group.get(body_name)
if not group:
continue
item["editableGroup"] = group[0]
item["editableGroupRole"] = group[1]
def prop_position(index: int, payload: dict[str, Any], spec: dict[str, Any]) -> list[float]:
raw = payload.get("position")
if isinstance(raw, list) and len(raw) >= 3:
return [float(raw[0]), float(raw[1]), float(raw[2])]
row = index // 4
col = index % 4
return [0.72 + col * 0.28, -0.48 - row * 0.26, float(spec["z"])]
def vec3_from_payload(value: Any, default: list[float]) -> list[float]:
if isinstance(value, list) and len(value) >= 3:
return [float(value[0]), float(value[1]), float(value[2])]
return [float(default[0]), float(default[1]), float(default[2])]
def parse_float_list(raw: str | None, default: list[float]) -> list[float]:
if not raw:
return [*default]
try:
return [float(item) for item in raw.replace(",", " ").split()]
except ValueError:
return [*default]
def normalize_compose_request(payload: dict[str, Any]) -> dict[str, Any]:
scene_id = str(payload.get("scene") or payload.get("scenePreset") or DEFAULT_COMPOSE_SCENE_ID)
robot_id = str(payload.get("robot") or payload.get("robotPreset") or "none")
if scene_id not in SUPPORTED_COMPOSE_SCENE_IDS:
raise ValueError(f"unsupported physical compose scene: {scene_id}")
if robot_id not in {"none", "g1"}:
raise ValueError("physical compose currently supports the built-in G1 robot only")
raw_objects = payload.get("objects", [])
if not isinstance(raw_objects, list):
raise ValueError("objects must be a list")
if len(raw_objects) > 24:
raise ValueError("at most 24 physical objects are supported per compose request")
stock_physics = str(payload.get("stockPhysics") or "fixed").lower()
objects: list[dict[str, Any]] = []
for index, item in enumerate(raw_objects):
if not isinstance(item, dict):
raise ValueError("each object must be an object")
preset_id = str(item.get("presetId") or item.get("type") or item.get("id") or "")
spec = COMPOSE_PROP_SPECS.get(preset_id)
if spec is None:
raise ValueError(f"unsupported physical object: {preset_id}")
position = prop_position(index, item, spec)
objects.append({
"name": safe_name(item.get("id"), f"user_{preset_id}_{index}"),
"presetId": preset_id,
"position": position,
"spec": spec,
})
return {
"scene": scene_id,
"robot": robot_id,
"robotPose": vec3_from_payload(payload.get("robotPose"), DEFAULT_G1_COMPOSE_POSE),
"freezeInventory": stock_physics not in {"dynamic", "free", "movable"},
"objects": objects,
}
def append_compose_prop_geoms(body: ET.Element, spec: dict[str, Any]) -> None:
visual = spec.get("visual")
if isinstance(visual, dict):
visual_attrs = {
"class": "store_visual",
"mesh": str(visual["mesh"]),
"material": str(visual["material"]),
"contype": "0",
"conaffinity": "0",
"group": "2",
}
if visual.get("pos"):
visual_attrs["pos"] = str(visual["pos"])
if visual.get("quat"):
visual_attrs["quat"] = str(visual["quat"])
ET.SubElement(body, "geom", visual_attrs)
for collision in spec.get("collisions", []):
collision_attrs = {
**collision,
"group": "3",
"contype": "1",
"conaffinity": "1",
"friction": "0.9",
"condim": "3",
"rgba": "1 1 1 0.05",
}
ET.SubElement(body, "geom", collision_attrs)
return
geom_attrs = {
**spec["geom"],
"material": str(spec["material"]),
"friction": "0.8",
"condim": "3",
}
ET.SubElement(body, "geom", geom_attrs)
def compose_request_key(request: dict[str, Any]) -> str:
identity = {
"scene": request["scene"],
"robot": request["robot"],
"robotPose": [round(float(value), 6) for value in request["robotPose"]],
"freezeInventory": request["freezeInventory"],
"objects": [
{
"name": item["name"],
"presetId": item["presetId"],
"position": [round(float(value), 6) for value in item["position"]],
}
for item in request["objects"]
],
}
body = json.dumps(identity, ensure_ascii=False, sort_keys=True, separators=(",", ":")).encode("utf-8")
return hashlib.sha256(body).hexdigest()
def semantic_state_hash(payload: dict[str, Any]) -> str:
"""Hash simulator state semantics while ignoring volatile transport fields."""
identity = {
"scene": payload.get("scene"),
"step": payload.get("step"),
"paused": payload.get("paused"),
"running": payload.get("running"),
"policy": payload.get("policy"),
"command": payload.get("command"),
"bodyPayload": payload.get("bodyPayload"),
"body_names": payload.get("body_names"),
"link_names": payload.get("link_names"),
"joint_names": payload.get("joint_names"),
"bodies": payload.get("bodies"),
"joints": payload.get("joints"),
"actuators": payload.get("actuators"),
"dof_pos": payload.get("dof_pos"),
"dof_vel": payload.get("dof_vel"),
"actuator_ctrls": payload.get("actuator_ctrls"),
}
body = json.dumps(identity, ensure_ascii=False, sort_keys=True, separators=(",", ":")).encode("utf-8")
return hashlib.sha256(body).hexdigest()
def normalize_etag(value: str) -> str:
return value.strip().removeprefix("W/").strip('"')
def if_none_match_contains(header_value: str | None, state_hash: str) -> bool:
if not header_value:
return False
if header_value.strip() == "*":
return True
return any(normalize_etag(part) == state_hash for part in header_value.split(","))
def write_pose_adjusted_mjcf(source_path: Path, target_path: Path, pose: list[float]) -> Path:
tree = ET.parse(source_path)
root = tree.getroot()
source_dir = source_path.parent
for element in root.iter():
file_attr = element.attrib.get("file")
if not file_attr:
continue
file_path = Path(file_attr)
if not file_path.is_absolute():
element.set("file", (source_dir / file_path).as_posix())
worldbody = root.find("worldbody")
if worldbody is None:
raise ValueError(f"robot scene has no worldbody: {source_path}")
robot_body = worldbody.find("body")
if robot_body is None:
raise ValueError(f"robot scene has no root body: {source_path}")
base_pos = parse_float_list(robot_body.attrib.get("pos"), [0.0, 0.0, 0.0])
while len(base_pos) < 3:
base_pos.append(0.0)
robot_body.set("pos", " ".join(f"{base_pos[index] + float(pose[index]):.6g}" for index in range(3)))
ET.indent(tree, space=" ")
tree.write(target_path, encoding="unicode")
return target_path
def compose_physical_scene(payload: dict[str, Any], compose_dir: Path, request: dict[str, Any] | None = None) -> Path:
request = request or normalize_compose_request(payload)
if not DEFAULT_G1_SCENE.is_file():
raise FileNotFoundError(DEFAULT_G1_SCENE.as_posix())
if not DEFAULT_STORE_SCENE.is_file():
raise FileNotFoundError(DEFAULT_STORE_SCENE.as_posix())
compose_dir.mkdir(parents=True, exist_ok=True)
store_scene = write_namespaced_store_scene(
DEFAULT_STORE_SCENE,
compose_dir / "store_namespaced.xml",
freeze_inventory=bool(request["freezeInventory"]),
)
g1_scene = write_pose_adjusted_mjcf(
DEFAULT_G1_SCENE,
compose_dir / "g1_pose_adjusted.xml",
request["robotPose"],
)
scene_path = compose_dir / "active_g1_store_scene.xml"
mujoco = ET.Element("mujoco", {"model": "motrixsim_web_composed"})
ET.SubElement(mujoco, "compiler", {"angle": "radian", "autolimits": "true"})
ET.SubElement(mujoco, "include", {"file": g1_scene.as_posix()})
ET.SubElement(mujoco, "include", {"file": store_scene.as_posix()})
ET.SubElement(mujoco, "option", {"gravity": "0 0 -9.8", "timestep": "0.005"})
ET.SubElement(mujoco, "statistic", {"center": "0.35 0.45 0.85", "extent": "2.6", "meansize": "0.04"})
visual = ET.SubElement(mujoco, "visual")
ET.SubElement(visual, "headlight", {"diffuse": "0.6 0.6 0.6", "ambient": "0.3 0.3 0.3", "specular": "0 0 0"})
ET.SubElement(visual, "rgba", {"haze": "0.15 0.25 0.35 1"})
ET.SubElement(visual, "global", {"azimuth": "130", "elevation": "-18"})
ET.SubElement(visual, "quality", {"shadowsize": "8192"})
asset = ET.SubElement(mujoco, "asset")
ET.SubElement(asset, "texture", {
"type": "2d",
"name": "combined_ground_checker",
"builtin": "checker",
"mark": "edge",
"rgb1": "0.4314 0.5294 0.6431",
"rgb2": "0.8157 0.8549 0.9059",
"markrgb": "0.8 0.8 0.8",
"width": "300",
"height": "300",
})
ET.SubElement(asset, "material", {
"name": "combined_ground",
"texture": "combined_ground_checker",
"texuniform": "true",
"texrepeat": "2 2",
"reflectance": "0.2",
})
for name, rgba in {
"demo_red": "0.9 0.1 0.08 1",
"demo_blue": "0.1 0.35 0.95 1",
"demo_green": "0.1 0.65 0.28 1",
"demo_yellow": "0.95 0.72 0.12 1",
"demo_pallet": "0.63 0.38 0.04 1",
}.items():
ET.SubElement(asset, "material", {"name": name, "rgba": rgba})
worldbody = ET.SubElement(mujoco, "worldbody")
ET.SubElement(worldbody, "light", {"pos": "0 0 2.4", "dir": "0 0 -1", "directional": "true"})
ET.SubElement(worldbody, "geom", {
"name": "combined_floor",
"size": "0 0 0.02",
"type": "plane",
"material": "combined_ground",
"friction": "0.8",
"condim": "3",
})
for item in request["objects"]:
spec = item["spec"]
position = item["position"]
body_attrs = {
"name": item["name"],
"pos": f"{position[0]:.6g} {position[1]:.6g} {position[2]:.6g}",
}
if spec.get("euler"):
body_attrs["euler"] = str(spec["euler"])
body = ET.SubElement(worldbody, "body", body_attrs)
ET.SubElement(body, "freejoint")
append_compose_prop_geoms(body, spec)
tree = ET.ElementTree(mujoco)
ET.indent(tree, space=" ")
tree.write(scene_path, encoding="unicode")
return scene_path
class Go2PolicyAdapter:
"""Headless version of MotrixSim's official Go2 keyboard ONNX controller."""
default_joint_pos = [0.1, 0.9, -1.8, -0.1, 0.9, -1.8, 0.1, 0.9, -1.8, -0.1, 0.9, -1.8]
action_scale = 0.5
lin_vel_scale = 2.0
ang_vel_scale = 3.0
body_name = "base"
link_name = "base"
linear_sensor_name = "local_linvel"
gyro_sensor_name = "gyro"
start_actuator_name = "FL_hip"
def __init__(self, model: Any, policy_path: Path, ctrl_dt: float = 0.02):
import numpy as np
import onnxruntime as ort
self.np = np
self.model = model
self.policy_path = policy_path
self.policy = ort.InferenceSession(policy_path.as_posix(), providers=["CPUExecutionProvider"])
self.input_name = self.policy.get_inputs()[0].name
self.output_name = self.policy.get_outputs()[0].name
self.default_angles = np.array(self.default_joint_pos, dtype=np.float32)
self.last_action = np.zeros_like(self.default_angles, dtype=np.float32)
self.counter = 0
self.n_substeps = max(1, int(round(ctrl_dt / self.model.options.timestep)))
def initialize(self, data: Any) -> None:
self.apply_actions(self.np.zeros_like(self.default_angles, dtype=self.np.float32), data)
def scaled_command(self, command: list[float]) -> Any:
return self.np.array([
command[0] * self.lin_vel_scale,
command[1] * self.lin_vel_scale,
command[2] * self.ang_vel_scale,
], dtype=self.np.float32)
def rotate_vector(self, quat_xyzw: Any, vector: Any, inverse: bool = False) -> Any:
quat = self.np.asarray(quat_xyzw, dtype=self.np.float64)
norm = self.np.linalg.norm(quat)
if norm > 1e-9:
quat = quat / norm
xyz = quat[:3]
if inverse:
xyz = -xyz
w = quat[3]
vec = self.np.asarray(vector, dtype=self.np.float64)
return (
2.0 * self.np.dot(xyz, vec) * xyz
+ (w * w - self.np.dot(xyz, xyz)) * vec
+ 2.0 * w * self.np.cross(xyz, vec)
)
def get_obs(self, data: Any, command: list[float]) -> Any:
body_index = self.model.get_body_index(self.body_name)
body = self.model.get_body(body_index)
linear_vel = self.model.get_sensor_value(self.linear_sensor_name, data)
gyro = self.model.get_sensor_value(self.gyro_sensor_name, data)
pose = body.get_pose(data)
gravity = self.rotate_vector(pose[3:7], self.np.array([0.0, 0.0, -1.0]), inverse=True)
dof_pos = body.get_joint_dof_pos(data)
dof_vel = body.get_joint_dof_vel(data)
obs = self.np.hstack([
linear_vel,
gyro,
gravity,
dof_pos - self.default_angles,
dof_vel,
self.last_action,
self.scaled_command(command),
])
return obs.astype(self.np.float32)
def apply_actions(self, actions: Any, data: Any) -> None:
start_actuator_index = self.model.get_actuator_index(self.start_actuator_name)
for index, action in enumerate(actions):
actuator_index = start_actuator_index + index
ctrl = float(action * self.action_scale + self.default_angles[index])
actuator = self.model.get_actuator(actuator_index)
actuator.set_ctrl(data, ctrl)
def is_fall(self, data: Any) -> bool:
pose = self.model.get_link(self.link_name).get_pose(data)
rotated_z_axis = self.rotate_vector(pose[3:7], self.np.array([0.0, 0.0, 1.0]))
return float(self.np.dot(rotated_z_axis, self.np.array([0.0, 0.0, 1.0]))) < 0.3
def advance(self, data: Any, command: list[float]) -> bool:
self.counter += 1
self.model.step(data)
if self.is_fall(data):
return True
if self.counter % self.n_substeps == 0:
obs = self.get_obs(data, command)
outputs = self.policy.run([self.output_name], {self.input_name: obs.reshape(1, -1)})
actions = outputs[0][0]
self.last_action = actions.copy()
self.apply_actions(actions, data)
return False
class G1PolicyAdapter(Go2PolicyAdapter):
"""Headless version of MotrixSim's official G1 keyboard ONNX controller."""
default_joint_pos = [
-0.312, 0, 0, 0.669, -0.363, 0,
-0.312, 0, 0, 0.669, -0.363, 0,
0, 0, 0.073,
0.2, 0.2, 0, 0.6, 0, 0, 0,
0.2, -0.2, 0, 0.6, 0, 0, 0,
]
action_scale = 0.5
lin_vel_scale = 1.0
ang_vel_scale = 1.0
body_name = "pelvis"
link_name = "pelvis"
linear_sensor_name = "local_linvel_pelvis"
gyro_sensor_name = "gyro_pelvis"
start_actuator_name = "left_hip_pitch_joint"
def __init__(self, model: Any, policy_path: Path, ctrl_dt: float = 0.02):
super().__init__(model, policy_path, ctrl_dt)
self.phase = self.np.array([0.0, self.np.pi])
self.gait_freq = 1.5
self.phase_dt = 2 * self.np.pi * self.gait_freq * ctrl_dt
def get_obs(self, data: Any, command: list[float]) -> Any:
body_index = self.model.get_body_index(self.body_name)
body = self.model.get_body(body_index)
linear_vel = self.model.get_sensor_value(self.linear_sensor_name, data)
gyro = self.model.get_sensor_value(self.gyro_sensor_name, data)
pose = body.get_pose(data)
gravity = self.rotate_vector(pose[3:7], self.np.array([0.0, 0.0, -1.0]), inverse=True)
dof_pos = body.get_joint_dof_pos(data)
dof_vel = body.get_joint_dof_vel(data)
phase = self.np.concatenate([self.np.cos(self.phase), self.np.sin(self.phase)])
obs = self.np.hstack([
linear_vel,
gyro,
gravity,
self.scaled_command(command),
dof_pos - self.default_angles,
dof_vel,
self.last_action,
phase,
])
return obs.astype(self.np.float32)
def advance(self, data: Any, command: list[float]) -> bool:
fell = super().advance(data, command)
if self.counter % self.n_substeps == 0:
phase_next = self.phase + self.phase_dt
self.phase = self.np.fmod(phase_next + self.np.pi, 2 * self.np.pi) - self.np.pi
return fell
def align_manifest_bodies(manifest: dict[str, Any], runtime_body_names: list[str]) -> None:
"""Map generated XML body ids to runtime body names when possible.
MJCF files often omit body names. MotrixSim still exposes generated runtime
names, and state updates only arrive under those names. When the counts line
up, remap manifest bindings so the browser can drive visuals immediately.
"""
existing: list[str] = []
for obj in manifest.get("objects", []):
body = obj.get("body")
if body and body not in existing:
existing.append(body)
if not existing or len(existing) != len(runtime_body_names):
return
if set(existing) == set(runtime_body_names):
return
mapping = dict(zip(existing, runtime_body_names, strict=False))
for obj in manifest.get("objects", []):
body = obj.get("body")
if body in mapping:
obj["body"] = mapping[body]
obj["runtimeBodyRemappedFrom"] = body
def iter_scene_roots(scene: Path, seen: set[Path] | None = None):
seen = seen or set()
scene = scene.resolve()
if scene in seen or not scene.is_file():
return
seen.add(scene)
try:
root = ET.parse(scene).getroot()
except ET.ParseError:
return
yield root
for include in root.findall("include"):
file_name = include.get("file")
if not file_name:
continue
include_path = Path(file_name)
if not include_path.is_absolute():
include_path = scene.parent / include_path
yield from iter_scene_roots(include_path, seen)
def collect_pose_editable_bodies(scene: Path) -> set[str]:
editable: set[str] = set()
for root in iter_scene_roots(scene):
for body in root.iter("body"):
name = body.get("name")
if not name:
continue
if any(child.tag in {"joint", "freejoint"} for child in list(body)):
editable.add(name)
return editable
class MotrixRuntime:
def __init__(self, scene: Path, reset_interval: float | None):
import motrixsim as mx
self.mx = mx
self.scene = scene
self.pose_editable_bodies = collect_pose_editable_bodies(scene)
self.model = mx.load_model(scene.as_posix())
self.data = mx.SceneData(self.model)
self.model.forward_kinematic(self.data)
self.body_names = [str(name) for name in getattr(self.model, "body_names", [])]
self.link_names = [str(name) for name in getattr(self.model, "link_names", [])]
self.joint_names = [str(name) for name in getattr(self.model, "joint_names", [])]
self.actuator_names = [str(name) for name in getattr(self.model, "actuator_names", [])]
self.lock = threading.Lock()
self.step = 0
self.step_ms: list[float] = []
self.running = True
self.paused = False
self.command: list[float] = [0.0, 0.0, 0.0]
self.policy_adapter = self.create_policy_adapter()
self.policy_name = self.policy_adapter.policy_path.name if self.policy_adapter else None
if self.policy_adapter:
self.policy_adapter.initialize(self.data)
self.model.forward_kinematic(self.data)
self.reset_interval = reset_interval
self.last_reset = time.monotonic()
def create_policy_adapter(self) -> Go2PolicyAdapter | None:
if "FL_hip" in self.actuator_names and "base" in {*self.body_names, *self.link_names}:
policy_path = self.scene.parent / "go2_policy.onnx"
if not policy_path.is_file():
policy_path = Path("/opt/Motphys/motrixsim-docs/examples/assets/go2/go2_policy.onnx")
if policy_path.is_file():
return Go2PolicyAdapter(self.model, policy_path)
if "left_hip_pitch_joint" in self.actuator_names and "pelvis" in {*self.body_names, *self.link_names}:
policy_path = self.scene.parent / "g1_policy.onnx"
if not policy_path.is_file():
policy_path = Path("/opt/Motphys/motrixsim-docs/examples/assets/g1/g1_policy.onnx")
if policy_path.is_file():
return G1PolicyAdapter(self.model, policy_path)
return None
def maybe_reset(self) -> None:
if self.reset_interval is None:
return
if time.monotonic() - self.last_reset < self.reset_interval:
return
self.data.reset(self.model)
self.model.forward_kinematic(self.data)
self.last_reset = time.monotonic()
def reset_locked(self) -> None:
self.data.reset(self.model)
if self.policy_adapter:
self.policy_adapter.initialize(self.data)
self.model.forward_kinematic(self.data)
self.step = 0
self.step_ms.clear()
self.command = [0.0, 0.0, 0.0]
self.last_reset = time.monotonic()
def joint_metadata(self) -> list[dict[str, Any]]:
joints: list[dict[str, Any]] = []
for name in self.joint_names:
try:
joint = self.model.get_joint(name)
raw_range = getattr(joint, "range", None)
if raw_range is not None:
values = raw_range.tolist()[0]
limit = [float(values[0]), float(values[1])]
else:
limit = None
joints.append({
"name": name,
"index": int(getattr(joint, "index", len(joints))),
"range": limit,
"value": float(joint.get_dof_pos(self.data)[0]),
})
except Exception:
continue
return joints
def actuator_metadata(self) -> list[dict[str, Any]]:
actuators: list[dict[str, Any]] = []
for name in self.actuator_names:
try:
actuator = self.model.get_actuator(name)
raw_range = getattr(actuator, "ctrl_range", None)
limit = [float(raw_range[0]), float(raw_range[1])] if raw_range else None
actuators.append({
"name": name,
"index": int(getattr(actuator, "index", len(actuators))),
"range": limit,
"targetName": getattr(actuator, "target_name", None),
"value": float(actuator.get_ctrl(self.data)),
})
except Exception:
continue
return actuators
def advance_locked(self, steps: int = 1) -> None:
for _ in range(max(1, steps)):
start = time.perf_counter()
self.maybe_reset()
fell = self.policy_adapter.advance(self.data, self.command) if self.policy_adapter else False
if fell:
self.data.reset(self.model)
self.command = [0.0, 0.0, 0.0]
self.policy_adapter.initialize(self.data)
self.model.forward_kinematic(self.data)
elif not self.policy_adapter:
self.model.step(self.data)
self.step += 1
elapsed_ms = (time.perf_counter() - start) * 1000.0
self.step_ms.append(elapsed_ms)
if len(self.step_ms) > 1000:
del self.step_ms[:500]
def clamp_joint_value(self, joint: Any, value: float) -> float:
raw_range = getattr(joint, "range", None)
if raw_range is None:
return value
values = raw_range.tolist()[0]
return max(float(values[0]), min(float(values[1]), value))
def set_joint_locked(self, name: str, value: float) -> float:
joint = self.model.get_joint(name)
clamped = self.clamp_joint_value(joint, float(value))
joint.set_dof_pos(self.data, clamped)
try:
joint.set_dof_vel(self.data, 0.0)
except Exception:
pass
for actuator_name in self.actuator_names:
try:
actuator = self.model.get_actuator(actuator_name)
if getattr(actuator, "target_name", None) == name:
actuator.set_ctrl(self.data, clamped)
except Exception:
continue
self.model.forward_kinematic(self.data)
return clamped
def set_gripper_locked(self, value: float) -> float:
applied = float(value)
for name in ("finger_joint1", "finger_joint2"):
if name in self.joint_names:
applied = self.set_joint_locked(name, value)
for actuator_name in self.actuator_names:
try:
actuator = self.model.get_actuator(actuator_name)
if getattr(actuator, "target_name", None) in {"finger_joint1", "finger_joint2"}:
actuator.set_ctrl(self.data, applied)
except Exception:
continue
self.model.forward_kinematic(self.data)
return applied
def clamp_actuator_value(self, actuator: Any, value: float) -> float:
raw_range = getattr(actuator, "ctrl_range", None)
if raw_range is None:
return value
return max(float(raw_range[0]), min(float(raw_range[1]), value))
def set_actuator_locked(self, name: str, value: float) -> float:
actuator = self.model.get_actuator(name)
clamped = self.clamp_actuator_value(actuator, float(value))
actuator.set_ctrl(self.data, clamped)
target_name = getattr(actuator, "target_name", None)
if target_name in self.joint_names:
try:
joint = self.model.get_joint(target_name)
joint.set_dof_pos(self.data, self.clamp_joint_value(joint, clamped))
joint.set_dof_vel(self.data, 0.0)
except Exception:
pass
self.model.forward_kinematic(self.data)
return clamped
def set_command_locked(self, payload: dict[str, Any]) -> list[float]:
raw_command = payload.get("command")
if raw_command is None:
raw_command = [
payload.get("x", 0.0),
payload.get("y", 0.0),
payload.get("yaw", 0.0),
]
if not isinstance(raw_command, (list, tuple)) or len(raw_command) != 3:
raise ValueError("command requires [x, y, yaw]")
self.command = [float(raw_command[0]), float(raw_command[1]), float(raw_command[2])]
return list(self.command)
def set_body_pose_locked(self, payload: dict[str, Any]) -> str:
if not self.paused:
raise ValueError("set_body_pose requires paused simulation")
body_name = str(payload.get("body") or "")
if not body_name:
raise ValueError("set_body_pose requires body")
if body_name not in self.pose_editable_bodies:
raise ValueError(f"body is fixed and cannot be moved with set_body_pose: {body_name}")
raw_pos = payload.get("pos")
if raw_pos is None:
raw_pos = payload.get("position")
if not isinstance(raw_pos, (list, tuple)) or len(raw_pos) != 3:
raise ValueError("set_body_pose requires pos [x, y, z]")
body = self.model.get_body(body_name)
pose = [float(value) for value in body.get_pose(self.data)]
raw_quat = payload.get("quat")
if raw_quat is None:
raw_quat = payload.get("quaternion")
quat = pose[3:7]
if raw_quat is not None:
if not isinstance(raw_quat, (list, tuple)) or len(raw_quat) != 4:
raise ValueError("set_body_pose quat must be [x, y, z, w]")
quat = [float(value) for value in raw_quat]
dof_pos = [float(raw_pos[0]), float(raw_pos[1]), float(raw_pos[2]), *quat]
body.set_dof_pos(self.data, dof_pos, True)
try:
body.set_dof_vel(self.data, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], True)
except Exception:
pass
self.model.forward_kinematic(self.data)
return body_name
def set_body_poses_locked(self, payload: dict[str, Any]) -> list[str]:
raw_poses = payload.get("poses")
if not isinstance(raw_poses, list):
raise ValueError("set_body_poses requires poses[]")
updated: list[str] = []
for pose_payload in raw_poses:
if not isinstance(pose_payload, dict):
raise ValueError("each pose must be an object")
updated.append(self.set_body_pose_locked(pose_payload))
return updated
def run(self, hz: float) -> None:
target_dt = 1.0 / hz
while self.running:
start = time.perf_counter()
with self.lock:
if not self.paused:
self.advance_locked(1)
spent = time.perf_counter() - start
if spent < target_dt:
time.sleep(target_dt - spent)
else:
# Keep HTTP control responsive when a large scene takes longer
# than the target frame budget and the sim loop would otherwise
# immediately reacquire the runtime lock.
time.sleep(0.001)
def control_status_snapshot(self) -> dict[str, Any]:
values = list(self.step_ms)
if values:
ordered = sorted(values)
p95 = ordered[max(0, min(len(ordered) - 1, int(len(ordered) * 0.95) - 1))]
mean = statistics.fmean(values)
max_v = max(values)
else:
mean = p95 = max_v = 0.0
return {
"ok": True,
"host": socket.gethostname(),
"scene": self.scene.as_posix(),
"step": self.step,
"paused": self.paused,
"running": self.running,
"policy": self.policy_name,
"server_time": time.time(),
"command": list(self.command),
"step_ms": {"mean": mean, "p95": p95, "max": max_v},
}
def control(self, action: str, payload: dict[str, Any] | None = None) -> dict[str, Any]:
payload = payload or {}
steps = int(payload.get("steps") or 1)
if action in {"pause", "resume", "toggle"}:
if action == "pause":
self.paused = True
elif action == "resume":
self.paused = False
else:
self.paused = not self.paused
snapshot = self.control_status_snapshot()
snapshot["control_action"] = action
return snapshot
with self.lock:
if action == "reset":
self.reset_locked()
elif action == "step":
self.paused = True
self.advance_locked(max(1, min(steps, 240)))
elif action == "home":
self.reset_locked()
self.paused = True
elif action == "set_joint":
name = str(payload.get("joint") or "")
if not name:
raise ValueError("set_joint requires joint")
self.set_joint_locked(name, float(payload.get("value", 0.0)))
self.paused = True
elif action == "set_joints":
values = payload.get("values", {})
if not isinstance(values, dict):
raise ValueError("set_joints values must be an object")
for name, value in values.items():
self.set_joint_locked(str(name), float(value))
self.paused = True
elif action == "set_actuator":
name = str(payload.get("actuator") or "")
if not name:
raise ValueError("set_actuator requires actuator")
self.set_actuator_locked(name, float(payload.get("value", 0.0)))
self.paused = True
elif action == "set_actuators":
values = payload.get("values", {})
if not isinstance(values, dict):
raise ValueError("set_actuators values must be an object")
for name, value in values.items():
self.set_actuator_locked(str(name), float(value))
self.paused = True
elif action == "command":
command = self.set_command_locked(payload)
if self.policy_adapter and any(abs(value) > 1e-6 for value in command):
self.paused = False
elif action == "gripper":
self.set_gripper_locked(float(payload.get("value", 0.0)))
self.paused = True
elif action == "set_body_pose":
payload["updated_bodies"] = [self.set_body_pose_locked(payload)]
self.paused = True
elif action == "set_body_poses":
payload["updated_bodies"] = self.set_body_poses_locked(payload)
self.paused = True
else:
raise ValueError(f"unsupported control action: {action}")
snapshot = self.snapshot(compact=bool(payload.get("compact")))
snapshot["control_action"] = action
if "updated_bodies" in payload:
snapshot["updated_bodies"] = payload["updated_bodies"]
return snapshot
def pose_for(self, kind: str, name: str) -> dict[str, list[float]]:
obj = self.model.get_link(name) if kind == "link" else self.model.get_body(name)
return {
"pos": [float(v) for v in obj.get_position(self.data)],
"quat": [float(v) for v in obj.get_rotation(self.data)],
}
def is_active_state_body(self, name: str) -> bool:
return name in self.pose_editable_bodies or is_robot_state_body(name)
def joint_state_locked(self) -> dict[str, Any]:
joints: dict[str, Any] = {}
for meta in self.joint_metadata():
name = meta["name"]
try:
joint = self.model.get_joint(name)
joints[name] = {
"pos": float(joint.get_dof_pos(self.data)[0]),
"vel": float(joint.get_dof_vel(self.data)[0]),
"range": meta.get("range"),
"index": meta.get("index"),
}
except Exception:
continue
return joints
def actuator_state_locked(self) -> dict[str, Any]:
actuators: dict[str, Any] = {}
for meta in self.actuator_metadata():
name = meta["name"]
try:
actuator = self.model.get_actuator(name)
actuators[name] = {
"ctrl": float(actuator.get_ctrl(self.data)),
"range": meta.get("range"),
"index": meta.get("index"),
"targetName": meta.get("targetName"),
}
except Exception:
continue
return actuators
def snapshot(self, compact: bool = False, body_mode: str = "full") -> dict[str, Any]:
if compact:
return self.control_status_snapshot()
with self.lock:
values = list(self.step_ms)
if values:
ordered = sorted(values)
p95 = ordered[max(0, min(len(ordered) - 1, int(len(ordered) * 0.95) - 1))]
mean = statistics.fmean(values)
max_v = max(values)
else:
mean = p95 = max_v = 0.0
base = {
"ok": True,
"host": socket.gethostname(),
"scene": self.scene.as_posix(),
"step": self.step,
"paused": self.paused,
"running": self.running,
"policy": self.policy_name,
"server_time": time.time(),
"command": list(self.command),
"step_ms": {"mean": mean, "p95": p95, "max": max_v},
}
bodies: dict[str, Any] = {}
seen: set[str] = set()
active_only = body_mode == "active"
total_body_count = len({*self.link_names, *self.body_names})
for name in self.link_names:
if active_only and not self.is_active_state_body(name):
continue
bodies[name] = self.pose_for("link", name)
seen.add(name)
for name in self.body_names:
if name in seen:
continue
if active_only and not self.is_active_state_body(name):
continue
bodies[name] = self.pose_for("body", name)
return {
**base,
"bodyPayload": {
"mode": "active" if active_only else "full",
"count": len(bodies),
"total": total_body_count,
"filtered": max(0, total_body_count - len(bodies)),
},
"body_names": self.body_names,
"link_names": self.link_names,
"joint_names": self.joint_names,
"bodies": bodies,
"joints": self.joint_state_locked(),
"actuators": self.actuator_state_locked(),
"dof_pos": [float(v) for v in getattr(self.data, "dof_pos", [])],
"dof_vel": [float(v) for v in getattr(self.data, "dof_vel", [])],
"actuator_ctrls": [float(v) for v in getattr(self.data, "actuator_ctrls", [])],
}
def main() -> int:
parser = argparse.ArgumentParser()
parser.add_argument("--scene", required=True, type=Path)
parser.add_argument("--host", default="127.0.0.1")
parser.add_argument("--port", type=int, default=18082)
parser.add_argument("--hz", type=float, default=120.0)
parser.add_argument("--asset-base", default="/assets")
parser.add_argument("--compose-dir", type=Path, default=Path("/tmp/motrixsim-web-composed"))
parser.add_argument("--reset-interval", type=float)
parser.add_argument("--paused", action="store_true", help="start the runtime paused for interactive scene layout")
args = parser.parse_args()
state_lock = threading.Lock()
def create_manifest(runtime: MotrixRuntime) -> tuple[dict[str, Any], dict[str, Path]]:
payload = collect_manifest(runtime.scene, args.asset_base)
files = {
key: Path(value)
for key, value in payload.pop("_assetFiles", {}).items()
}
payload["controls"] = {
"joints": runtime.joint_metadata(),
"actuators": runtime.actuator_metadata(),
"keyboard": {
"source": "MotrixSim RenderApp examples",
"command": {
"forward": ["w", "up"],
"backward": ["s", "down"],
"strafeLeft": ["left"],
"strafeRight": ["right"],
"yawLeft": ["a"],
"yawRight": ["d"],
},
"camera": {
"rotate": "left mouse drag",
"pan": "right mouse drag",
"zoom": "mouse wheel",
"focusMarker": True,
},
},
}
align_manifest_bodies(payload, runtime.body_names)
remove_visual_helper_objects(payload)
annotate_editable_groups(payload, runtime)
annotate_physics_roles(payload, runtime)
return payload, files
def create_bundle(scene: Path, *, paused: bool = False) -> tuple[MotrixRuntime, dict[str, Any], dict[str, Path], threading.Thread]:
next_runtime = MotrixRuntime(scene, args.reset_interval)
next_runtime.paused = paused
next_manifest, next_asset_files = create_manifest(next_runtime)
next_thread = threading.Thread(target=next_runtime.run, args=(args.hz,), daemon=True)
return next_runtime, next_manifest, next_asset_files, next_thread
runtime, manifest, asset_files, thread = create_bundle(args.scene, paused=args.paused)
thread.start()
active_compose_key: str | None = None
asset_gzip_cache: dict[tuple[str, int, int], bytes] = {}
asset_gzip_cache_lock = threading.Lock()
def scene_response(
active_runtime: MotrixRuntime,
active_manifest: dict[str, Any],
*,
cached: bool = False,
compose_key: str | None = None,
reset_applied: bool = False,
) -> dict[str, Any]:
return {
"ok": True,
"scene": active_runtime.scene.as_posix(),
"manifestUrl": "/manifest",
"stateUrl": "/state",
"objectCount": len(active_manifest.get("objects", [])),
"bodyCount": len(active_runtime.body_names),
"jointCount": len(active_runtime.joint_names),
"actuatorCount": len(active_runtime.actuator_names),
"policy": active_runtime.policy_name,
"cached": cached,
"composeKey": compose_key,
"resetApplied": reset_applied,
"manifest": active_manifest,
}
def replace_scene(
scene: Path,
*,
paused: bool = True,
compose_key: str | None = None,
reset_applied: bool = False,
) -> dict[str, Any]:
nonlocal runtime, manifest, asset_files, thread, active_compose_key
next_runtime, next_manifest, next_asset_files, next_thread = create_bundle(scene, paused=paused)
with state_lock:
previous_runtime = runtime
runtime = next_runtime
manifest = next_manifest
asset_files = next_asset_files
thread = next_thread
active_compose_key = compose_key
previous_runtime.running = False
next_thread.start()
return scene_response(next_runtime, next_manifest, cached=False, compose_key=compose_key, reset_applied=reset_applied)
class Handler(BaseHTTPRequestHandler):
def log_message(self, fmt: str, *values: Any) -> None:
return
def send_json(self, status: int, payload: dict[str, Any], headers: dict[str, str] | None = None) -> None:
raw_body = json.dumps(payload, ensure_ascii=False).encode("utf-8")
body = raw_body
encoding = "identity"
if accepts_gzip(self.headers) and len(raw_body) >= JSON_COMPRESSION_MIN_BYTES:
compressed = gzip.compress(raw_body, compresslevel=5)
if len(compressed) < len(raw_body):
body = compressed
encoding = "gzip"
self.send_response(status)
self.send_header("Content-Type", "application/json; charset=utf-8")
self.send_header("Access-Control-Allow-Origin", "*")
self.send_header("Access-Control-Expose-Headers", EXPOSED_HEADERS)
self.send_header("Cache-Control", "no-store")
self.send_header("Vary", "Accept-Encoding")
self.send_header("X-Motrix-Json-Bytes", str(len(raw_body)))
self.send_header("X-Motrix-Wire-Bytes", str(len(body)))
self.send_header("X-Motrix-Encoding", encoding)
if encoding == "gzip":
self.send_header("Content-Encoding", "gzip")
for name, value in (headers or {}).items():
self.send_header(name, value)
self.send_header("Content-Length", str(len(body)))
self.end_headers()
self.wfile.write(body)
def send_not_modified(self, state_hash: str) -> None:
self.send_response(304)
self.send_header("Access-Control-Allow-Origin", "*")
self.send_header("Access-Control-Expose-Headers", EXPOSED_HEADERS)
self.send_header("Cache-Control", "no-store")
self.send_header("X-Motrix-State-Hash", state_hash)
self.send_header("ETag", f'"{state_hash}"')
self.send_header("Content-Length", "0")
self.end_headers()
def asset_gzip_cache_key(self, file_path: Path, stat_result: Any) -> tuple[str, int, int]:
return (file_path.as_posix(), int(stat_result.st_mtime_ns), int(stat_result.st_size))
def cached_compressed_asset_body(self, file_path: Path, stat_result: Any) -> bytes | None:
if not accepts_gzip(self.headers) or not should_compress_asset(file_path, int(stat_result.st_size)):
return None
cache_key = self.asset_gzip_cache_key(file_path, stat_result)
with asset_gzip_cache_lock:
return asset_gzip_cache.get(cache_key)
def compressed_asset_body(self, file_path: Path, raw_body: bytes, stat_result: Any) -> tuple[bytes | None, str]:
if not accepts_gzip(self.headers) or not should_compress_asset(file_path, len(raw_body)):
return None, "bypass"
cache_key = self.asset_gzip_cache_key(file_path, stat_result)
with asset_gzip_cache_lock:
cached = asset_gzip_cache.get(cache_key)
if cached is not None:
return cached, "hit"
compressed = gzip.compress(raw_body, compresslevel=5)
if len(compressed) >= len(raw_body):
return None, "bypass"
with asset_gzip_cache_lock:
asset_gzip_cache[cache_key] = compressed
return compressed, "miss"
def send_asset(self, file_path: Path, *, include_body: bool = True) -> None:
if not file_path.is_file():
self.send_json(404, {"ok": False, "error": "asset not found"})
return
stat_result = file_path.stat()
raw_size = int(stat_result.st_size)
raw_body: bytes | None = None
compressed_body = self.cached_compressed_asset_body(file_path, stat_result)
cache_status = "hit" if compressed_body is not None else "bypass"
should_attempt_compression = accepts_gzip(self.headers) and should_compress_asset(file_path, raw_size)
if compressed_body is None and (include_body or should_attempt_compression):
raw_body = file_path.read_bytes()
compressed_body, cache_status = self.compressed_asset_body(file_path, raw_body, stat_result)
if compressed_body is not None:
body = compressed_body
wire_size = len(compressed_body)
else:
if include_body and raw_body is None:
raw_body = file_path.read_bytes()
body = raw_body or b""
wire_size = raw_size
encoding = "gzip" if compressed_body is not None else "identity"
mime_type = mimetypes.guess_type(file_path.as_posix())[0] or "application/octet-stream"
self.send_response(200)
self.send_header("Content-Type", mime_type)
self.send_header("Access-Control-Allow-Origin", "*")
self.send_header("Access-Control-Expose-Headers", EXPOSED_HEADERS)
self.send_header("Cache-Control", "public, max-age=3600")
self.send_header("Vary", "Accept-Encoding")
self.send_header("X-Motrix-Asset-Bytes", str(raw_size))
self.send_header("X-Motrix-Asset-Wire-Bytes", str(wire_size))
self.send_header("X-Motrix-Asset-Encoding", encoding)
self.send_header("X-Motrix-Asset-Cache", cache_status)
if encoding == "gzip":
self.send_header("Content-Encoding", "gzip")
self.send_header("Content-Length", str(wire_size))
self.end_headers()
if include_body:
self.wfile.write(body)
def do_OPTIONS(self) -> None:
self.send_response(204)
self.send_header("Access-Control-Allow-Origin", "*")
self.send_header("Access-Control-Allow-Methods", "GET, HEAD, POST, OPTIONS")
self.send_header("Access-Control-Allow-Headers", "Content-Type, If-None-Match")
self.end_headers()
def do_POST(self) -> None:
request_path = unquote(self.path.split("?", 1)[0])
try:
length = int(self.headers.get("Content-Length", "0"))
except ValueError:
length = 0
try:
payload = json.loads(self.rfile.read(length) or b"{}")
if request_path.startswith("/control"):
action = str(payload.get("action") or "")
with state_lock:
active_runtime = runtime
self.send_json(200, active_runtime.control(action, payload))
return
if request_path.startswith("/compose"):
compose_request = normalize_compose_request(payload)
compose_key = compose_request_key(compose_request)
paused = bool(payload.get("paused", True))
reset_requested = bool(payload.get("reset", False))
cached_response: dict[str, Any] | None = None
with state_lock:
if active_compose_key == compose_key:
with runtime.lock:
if reset_requested:
runtime.reset_locked()
runtime.paused = paused
cached_response = scene_response(
runtime,
manifest,
cached=True,
compose_key=compose_key,
reset_applied=reset_requested,
)
if cached_response is not None:
self.send_json(200, cached_response)
return
scene_path = compose_physical_scene(payload, args.compose_dir, compose_request)
self.send_json(200, replace_scene(
scene_path,
paused=paused,
compose_key=compose_key,
reset_applied=reset_requested,
))
return
self.send_json(404, {"ok": False, "error": "not found"})
except Exception as error: # noqa: BLE001
self.send_json(400, {"ok": False, "error": str(error)})
def do_GET(self) -> None:
parsed_url = urlparse(self.path)
request_path = unquote(parsed_url.path)
query = parse_qs(parsed_url.query)
if request_path.startswith("/manifest"):
with state_lock:
payload = manifest
self.send_json(200, payload)
return
if request_path.startswith("/state"):
with state_lock:
active_runtime = runtime
compact = (query.get("compact") or [""])[0].lower() in {"1", "true", "yes"}
body_mode = (query.get("bodies") or ["full"])[0].lower()
if body_mode not in {"full", "active"}:
body_mode = "full"
payload = active_runtime.snapshot(compact=compact, body_mode=body_mode)
state_hash = semantic_state_hash(payload)
if if_none_match_contains(self.headers.get("If-None-Match"), state_hash):
self.send_not_modified(state_hash)
return
self.send_json(200, payload, {
"X-Motrix-State-Hash": state_hash,
"ETag": f'"{state_hash}"',
})
return
if request_path.startswith("/health"):
with state_lock:
active_runtime = runtime
self.send_json(200, {
"ok": True,
"host": socket.gethostname(),
"scene": active_runtime.scene.as_posix(),
"compose": "/compose",
})
return
if request_path.startswith("/assets/"):
with state_lock:
file_path = asset_files.get(request_path)
if file_path is None:
self.send_json(404, {"ok": False, "error": "asset not mapped"})
return
self.send_asset(file_path)
return
self.send_json(404, {"ok": False, "error": "not found"})
def do_HEAD(self) -> None:
parsed_url = urlparse(self.path)
request_path = unquote(parsed_url.path)
if request_path.startswith("/assets/"):
with state_lock:
file_path = asset_files.get(request_path)
if file_path is None:
self.send_json(404, {"ok": False, "error": "asset not mapped"})
return
self.send_asset(file_path, include_body=False)
return
self.send_json(404, {"ok": False, "error": "not found"})
server = ThreadingHTTPServer((args.host, args.port), Handler)
print(json.dumps({
"ok": True,
"url": f"http://{args.host}:{args.port}",
"manifest": f"http://{args.host}:{args.port}/manifest",
"state": f"http://{args.host}:{args.port}/state",
"scene": args.scene.as_posix(),
"body_names": runtime.body_names,
}, ensure_ascii=False), flush=True)
try:
server.serve_forever()
finally:
with state_lock:
runtime.running = False
return 0
if __name__ == "__main__":
raise SystemExit(main())