fix: isolate MotrixSim task workspace collisions
This commit is contained in:
parent
93275ec827
commit
6b15bace18
@ -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,
|
||||
)
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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),
|
||||
|
||||
@ -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,
|
||||
}),
|
||||
});
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user