fix: isolate MotrixSim task workspace collisions

This commit is contained in:
eust-w 2026-07-12 18:19:55 +08:00
parent 93275ec827
commit 6b15bace18
7 changed files with 102 additions and 4 deletions

View File

@ -917,6 +917,7 @@ def run_scene_composition(config: PlatformConfig, store: PlatformStore, task: di
robot_spec=composition.get("robot") or {},
asset_specs=asset_specs,
source_paths=source_paths,
environment_collision_mode=str(composition.get("environment_collision_mode") or "isolated_task_workspace"),
validation_steps=int(params.get("validation_steps", 120)),
emit=emit,
)

View File

@ -425,6 +425,7 @@ def build_scene_composition(
robot_spec: dict[str, Any],
asset_specs: list[dict[str, Any]],
source_paths: dict[str, Path] | None,
environment_collision_mode: str = "isolated_task_workspace",
validation_steps: int,
emit: Callable[[str], None] | None = None,
) -> dict[str, Any]:
@ -442,6 +443,7 @@ def build_scene_composition(
root = tree.getroot()
if root.tag != "mujoco":
raise ValueError("scene runtime file is not an MJCF <mujoco> document")
environment = _prepare_composition_environment(root, manifest, mode=environment_collision_mode)
compiler = _ensure_radian_compiler(root)
emit("Merging the real RM65 articulated model into the MotrixSim scene")
robot_entry = _merge_rm65_robot(
@ -514,6 +516,8 @@ def build_scene_composition(
composition = {
"schema": SCENE_COMPOSITION_SCHEMA,
"simulator": "motrixsim",
"environment_collision_mode": environment_collision_mode,
"environment": environment,
"robot": robot_entry,
"assets": entries,
"controller": "rm65_pick_place_6d_ik" if collection_ready else None,
@ -533,6 +537,7 @@ def build_scene_composition(
"robot_body_count": robot_preview["body_count"],
"robot_mesh_count": robot_preview["mesh_count"],
"asset_count": len(entries),
"environment": environment,
"collection_ready": collection_ready,
"runtime_validation": validation,
},
@ -565,6 +570,58 @@ def build_scene_composition(
}
def _prepare_composition_environment(root: ET.Element, manifest: dict[str, Any], *, mode: str) -> dict[str, Any]:
if mode not in {"isolated_task_workspace", "reconstructed_collision"}:
raise ValueError("environment collision mode must be isolated_task_workspace or reconstructed_collision")
worldbody = root.find("worldbody")
if worldbody is None:
worldbody = ET.SubElement(root, "worldbody")
legacy_bodies = {
str((item.get("runtime") or {}).get("body") or "")
for item in manifest.get("assets", [])
if isinstance(item, dict) and item.get("role") == "added_asset"
}
legacy_bodies.discard("")
removed_bodies = []
for body in list(worldbody):
if body.tag == "body" and str(body.get("name") or "") in legacy_bodies:
removed_bodies.append(str(body.get("name")))
worldbody.remove(body)
disabled_geoms = 0
if mode == "isolated_task_workspace":
for geom in root.findall(".//geom"):
if str(geom.get("contype") or "1") == "0" and str(geom.get("conaffinity") or "1") == "0":
continue
geom.set("contype", "0")
geom.set("conaffinity", "0")
disabled_geoms += 1
existing_ground = worldbody.find("./geom[@name='composition_ground']")
if existing_ground is not None:
worldbody.remove(existing_ground)
ET.SubElement(
worldbody,
"geom",
{
"name": "composition_ground",
"type": "plane",
"size": "12 12 0.1",
"pos": "0 0 0",
"contype": "1",
"conaffinity": "1",
"group": "0",
"friction": "0.9 0.01 0.001",
"rgba": "0.18 0.2 0.22 1",
},
)
return {
"mode": mode,
"removed_legacy_asset_bodies": removed_bodies,
"disabled_reconstructed_collision_geoms": disabled_geoms,
"task_ground": mode == "isolated_task_workspace",
}
def export_mujoco_body_glb(scene_path: Path, output: Path, *, root_body: str, relative_to_root: bool = False) -> dict[str, Any]:
try:
import mujoco

View File

@ -143,7 +143,13 @@ class RM65PickPlaceRuntime:
self.model.step_n(self.data, 120)
self.simulation_time = 120 * float(self.model.options.timestep)
self.renderer.sync(self.data, wait=True)
return cube, target, float(self.cube_position()[2])
settled_z = float(self.cube_position()[2])
if not np.isfinite(settled_z) or abs(settled_z - float(cube_home[2])) > 0.08:
raise RuntimeError(
f"抓取方块静置高度异常:初始 z={float(cube_home[2]):.4f},静置 z={settled_z:.4f};"
"请改用独立任务工作区,或重新布置资产以避开重建碰撞体"
)
return cube, target, settled_z
def capture_frame(self) -> np.ndarray:
capture = self.render_camera.capture()
@ -169,9 +175,15 @@ class RM65PickPlaceRuntime:
def apply_action(self, action: np.ndarray, *, hold_steps: int = CONTROL_HOLD_STEPS) -> None:
action = self.clip_action(action)
if not np.isfinite(action).all():
raise RuntimeError("MotrixSim RM65 control action contains NaN or infinity")
if not np.isfinite(np.asarray(self.data.dof_pos)).all() or not np.isfinite(np.asarray(self.data.dof_vel)).all():
raise RuntimeError("MotrixSim scene state became non-finite before applying the RM65 action")
for actuator, value in zip(self.actuators, action):
actuator.set_ctrl(self.data, float(value))
self.model.step_n(self.data, hold_steps)
if not np.isfinite(np.asarray(self.data.dof_pos)).all() or not np.isfinite(np.asarray(self.data.dof_vel)).all():
raise RuntimeError("MotrixSim scene state became non-finite after applying the RM65 action")
self.simulation_time += hold_steps * float(self.model.options.timestep)
def cube_position(self) -> np.ndarray:

View File

@ -77,6 +77,7 @@ class SceneCompositionRequest(BaseModel):
name: str | None = None
robot: SceneCompositionRobotRequest
assets: list[SceneCompositionAssetRequest] = Field(default_factory=list)
environment_collision_mode: str = "isolated_task_workspace"
validation_steps: int = 120
@ -684,6 +685,8 @@ def create_app(config: PlatformConfig | None = None) -> FastAPI:
except KeyError as exc:
raise HTTPException(status_code=404, detail="scene not found") from exc
composition = payload.model_dump(exclude_none=True)
if composition.get("environment_collision_mode") not in {"isolated_task_workspace", "reconstructed_collision"}:
raise HTTPException(status_code=400, detail="environment_collision_mode must be isolated_task_workspace or reconstructed_collision")
params = {
"scene_id": scene_id,
"name": composition.pop("name", None),

View File

@ -690,6 +690,7 @@ async function saveSceneComposition() {
transform: cloneValue(asset.transform),
physics: cloneValue(asset.physics || {}),
})),
environment_collision_mode: $("compositionEnvironmentMode")?.value || "isolated_task_workspace",
validation_steps: 120,
}),
});

View File

@ -469,6 +469,7 @@
<div class="composition-action-bar">
<label>组合场景名称<input id="compositionName" value="RM65 茶几 Pick-Place"></label>
<label>环境物理<select id="compositionEnvironmentMode"><option value="isolated_task_workspace">独立任务工作区(推荐)</option><option value="reconstructed_collision">使用重建碰撞体</option></select></label>
<button id="saveSceneComposition" class="primary-button" type="button">保存为组合场景</button>
<button id="testSceneComposition" type="button" disabled>运行 MotrixSim 测试</button>
<button id="collectSceneComposition" type="button" disabled>进入数据采集</button>

View File

@ -4,6 +4,7 @@ import concurrent.futures
import importlib.util
import json
import sys
import xml.etree.ElementTree as ET
from pathlib import Path
import h5py
@ -235,6 +236,11 @@ def test_scene_composition_catalog_and_payload_are_registered(tmp_path: Path) ->
json={"name": "invalid", "robot": {"robot_id": "rm65"}, "assets": [{"catalog_id": "missing", "instance_id": "missing"}]},
)
assert invalid.status_code == 400
invalid_mode = client.post(
f"/api/scenes/{scene['id']}/compose",
json={"name": "invalid mode", "robot": {"robot_id": "rm65"}, "environment_collision_mode": "unknown"},
)
assert invalid_mode.status_code == 400
queued = client.post(
f"/api/scenes/{scene['id']}/compose",
json={
@ -263,10 +269,18 @@ def _build_real_rm65_composition_package(package: Path) -> tuple[Path, dict]:
(package / "assets" / "meshes").mkdir(parents=True)
runtime.parent.mkdir(parents=True)
runtime.write_text(
'<mujoco model="composition"><compiler angle="degree" meshdir="../assets/meshes" autolimits="true"/><option timestep="0.002"/><worldbody><geom type="plane" size="3 3 .1"/></worldbody></mujoco>\n',
'<mujoco model="composition"><compiler angle="degree" meshdir="../assets/meshes" autolimits="true"/><option timestep="0.002"/><worldbody><geom name="environment_collision" type="box" pos=".65 0 .4" size=".5 .5 .4"/><body name="legacy_dynamic" pos="0 0 1"><freejoint name="legacy_free"/><geom type="box" size=".1 .1 .1" mass=".2"/></body></worldbody></mujoco>\n',
encoding="utf-8",
)
(package / "scene_manifest.json").write_text(
json.dumps(
{
"assets": [{"name": "legacy", "role": "added_asset", "runtime": {"body": "legacy_dynamic"}}],
"simulator_artifacts": {"motrixsim": {"runtime_scene": str(runtime)}},
}
),
encoding="utf-8",
)
(package / "scene_manifest.json").write_text(json.dumps({"assets": [], "simulator_artifacts": {"motrixsim": {"runtime_scene": str(runtime)}}}), encoding="utf-8")
(package / "motrix_import.json").write_text(json.dumps({"runtime_scene": str(runtime)}), encoding="utf-8")
asset_specs = [
{
@ -296,6 +310,12 @@ def test_real_rm65_scene_composition_loads_and_steps_in_both_runtimes(tmp_path:
assert result["validation"]["mujoco"]["status"] == "loaded_and_stepped"
assert result["validation"]["motrixsim"]["status"] == "loaded_and_stepped"
assert result["collection_ready"] is True
assert result["composition"]["environment"]["mode"] == "isolated_task_workspace"
assert result["composition"]["environment"]["removed_legacy_asset_bodies"] == ["legacy_dynamic"]
root = ET.parse(runtime).getroot()
assert root.find("./worldbody/body[@name='legacy_dynamic']") is None
assert root.find("./worldbody/geom[@name='environment_collision']").get("contype") == "0"
assert root.find("./worldbody/geom[@name='composition_ground']").get("contype") == "1"
model = mujoco.MjModel.from_xml_path(str(runtime))
assert model.nkey >= 1
for object_type, name in (
@ -602,7 +622,10 @@ def test_worker_image_materializes_motrixsim_lfs_assets() -> None:
assert "REAL2SIM_MOTRIXSIM_PYTHON" in manifest
assert "REAL2SIM_FFMPEG_BIN" in manifest
assert "NVIDIA_DRIVER_CAPABILITIES" in manifest
assert "compute,utility,graphics" in manifest
assert "NVIDIA_DRIVER_CAPABILITIES, value: all" in manifest
assert "WGPU_BACKEND, value: vulkan" in manifest
assert "XDG_RUNTIME_DIR, value: /tmp/xdg-runtime" in manifest
assert "libvulkan1" in dockerfile
assert "GIT_CONFIG_VALUE_3, value: /opt/motrixsim-docs" in manifest