real2sim-platform/scripts/run_motrixsim_pick_place_rollout.py

331 lines
14 KiB
Python
Executable File

#!/usr/bin/env python3
from __future__ import annotations
import argparse
import json
import math
import sys
import time
from pathlib import Path
from typing import Any
import imageio.v2 as imageio
import numpy as np
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="Run a scripted RM65 pick-place rollout on the generated smoke MJCF.")
parser.add_argument("--scene-id", default=SCENE_ID)
parser.add_argument("--fps", type=int, default=20)
parser.add_argument("--width", type=int, default=960)
parser.add_argument("--height", type=int, default=540)
parser.add_argument("--no-render", 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"))
sim_path = package_dir / "simulator_assets/rm65_table_pick_place.xml"
if not sim_path.is_file():
raise SystemExit(f"runtime scene not found: {sim_path}")
rollout_dir = package_dir / "rollouts" / time.strftime("%Y%m%d_%H%M%S")
rollout_dir.mkdir(parents=True, exist_ok=True)
result = run_rollout(
sim_path,
rollout_dir,
fps=args.fps,
width=args.width,
height=args.height,
render=not args.no_render,
)
artifacts = {}
for key, path in result["artifact_paths"].items():
if Path(path).is_file():
artifacts[key] = store.add_artifact(task_id=None, name=Path(path).name, path=Path(path), kind=artifact_kind(Path(path)))
update_manifest(manifest_path, rollout_dir, result, artifacts)
print(json.dumps({"ok": result["ok"], "scene_id": args.scene_id, "rollout_dir": str(rollout_dir), "summary": result["summary"], "artifacts": result["artifact_paths"]}, ensure_ascii=False, indent=2))
return 0 if result["ok"] else 1
def run_rollout(
sim_path: Path,
out_dir: Path,
*,
fps: int,
width: int,
height: int,
render: bool,
) -> dict[str, Any]:
import mujoco
model = mujoco.MjModel.from_xml_path(str(sim_path))
data = mujoco.MjData(model)
mujoco.mj_resetDataKeyframe(model, data, 0)
mujoco.mj_forward(model, data)
site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "gripper")
cube_joint = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "pick_cube_free")
cube_qadr = int(model.jnt_qposadr[cube_joint])
cube_geom = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "pick_cube_geom")
finger_geoms = {
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "left_finger_pad_z_axis"),
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "right_finger_pad_z_axis"),
}
ctrl_ranges = model.actuator_ctrlrange.copy()
gripper_joint = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "gripper_Left_1_Joint")
gripper_qadr = int(model.jnt_qposadr[gripper_joint])
joint_ids = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, f"joint_{i}") for i in range(1, 7)]
qpos_adrs = [int(model.jnt_qposadr[jid]) for jid in joint_ids]
dof_adrs = [int(model.jnt_dofadr[jid]) for jid in joint_ids]
duration_scale = 8
phases = [
("home", np.array([0.42, 0.00, 0.72]), 0.0, 35 * duration_scale, False),
("approach", np.array([0.71, 0.015, 0.42]), 0.0, 55 * duration_scale, False),
("descend", np.array([0.71, 0.015, 0.24]), 0.0, 40 * duration_scale, False),
("grasp", np.array([0.71, 0.015, 0.24]), -0.91, 45 * duration_scale, True),
("lift", np.array([0.71, 0.015, 0.60]), -0.91, 200 * duration_scale, True),
("place", np.array([0.43, 0.18, 0.40]), -0.91, 65 * duration_scale, True),
("release", np.array([0.43, 0.18, 0.40]), 0.0, 35 * duration_scale, False),
("retreat", np.array([0.42, 0.00, 0.72]), 0.0, 45 * duration_scale, False),
]
trajectory = []
frames = []
renderer = None
if render:
renderer = mujoco.Renderer(model, height=height, width=width)
contact_steps = 0
bilateral_contact_steps = 0
phase_contacts = {phase: {"contact_steps": 0, "bilateral_contact_steps": 0} for phase, *_ in phases}
contact_force_metrics = {"max_normal": 0.0, "max_tangent": 0.0, "max_normal_z": 0.0, "samples": 0}
gripper_qpos_range = [float(data.qpos[gripper_qadr]), float(data.qpos[gripper_qadr])]
max_cube_z = float(data.qpos[cube_qadr + 2])
initial_cube = data.qpos[cube_qadr : cube_qadr + 3].copy()
settled_cube_z = float(initial_cube[2])
started = time.time()
frame_stride = max(1, round((1.0 / model.opt.timestep) / fps))
step_index = 0
for phase, target, gripper_ctrl, count, _contact_expected in phases:
if phase == "grasp":
settled_cube_z = float(data.qpos[cube_qadr + 2])
max_cube_z = settled_cube_z
q_target = solve_ik(model, data, site_id, target, qpos_adrs, dof_adrs, ctrl_ranges)
for local in range(count):
alpha = (local + 1) / count
current = data.ctrl[:6].copy()
data.ctrl[:6] = current * (1 - alpha) + q_target * alpha
data.ctrl[6] = gripper_ctrl
mujoco.mj_step(model, data)
touching_fingers = cube_finger_contacts(data, cube_geom, finger_geoms)
accumulate_contact_metrics(model, data, cube_geom, finger_geoms, contact_force_metrics)
if touching_fingers:
contact_steps += 1
phase_contacts[phase]["contact_steps"] += 1
if len(touching_fingers) == 2:
bilateral_contact_steps += 1
phase_contacts[phase]["bilateral_contact_steps"] += 1
gripper_qpos_range[0] = min(gripper_qpos_range[0], float(data.qpos[gripper_qadr]))
gripper_qpos_range[1] = max(gripper_qpos_range[1], float(data.qpos[gripper_qadr]))
max_cube_z = max(max_cube_z, float(data.qpos[cube_qadr + 2]))
if step_index % frame_stride == 0:
trajectory.append(sample_state(model, data, phase, step_index, site_id, cube_qadr))
if renderer is not None:
renderer.update_scene(data, camera="overview")
frames.append(renderer.render().copy())
step_index += 1
if renderer is not None:
renderer.close()
trajectory_path = out_dir / "rm65_pick_place_trajectory.json"
report_path = out_dir / "rm65_pick_place_rollout_report.json"
gif_path = out_dir / "rm65_pick_place_rollout.gif"
keyframe_path = out_dir / "rm65_pick_place_final.png"
trajectory_path.write_text(json.dumps({"schema": "real2sim.rm65_pick_place_trajectory.v1", "samples": trajectory}, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
artifact_paths = {"trajectory": str(trajectory_path)}
if frames:
imageio.mimsave(gif_path, frames, fps=fps)
imageio.imwrite(keyframe_path, frames[-1])
artifact_paths["gif"] = str(gif_path)
artifact_paths["final_frame"] = str(keyframe_path)
final = trajectory[-1] if trajectory else {}
final_cube = np.asarray(final.get("cube_pos") or initial_cube, dtype=float)
target_cube = np.array([0.43, 0.18, 0.31])
lift_height = max_cube_z - settled_cube_z
place_error = float(np.linalg.norm(final_cube - target_cube))
physical_success = bilateral_contact_steps > 0 and lift_height >= 0.10 and place_error <= 0.12
summary = {
"mode": "scripted_ik_physical_contact",
"steps": step_index,
"samples": len(trajectory),
"sim_time": float(data.time),
"final_gripper": final.get("gripper_pos"),
"final_cube": final.get("cube_pos"),
"initial_cube": [float(x) for x in initial_cube],
"settled_cube_z": settled_cube_z,
"max_cube_z": max_cube_z,
"lift_height": lift_height,
"place_target": [float(x) for x in target_cube],
"place_error": place_error,
"contact_steps": contact_steps,
"bilateral_contact_steps": bilateral_contact_steps,
"phase_contacts": phase_contacts,
"gripper_qpos_range": gripper_qpos_range,
"contact_force_metrics": contact_force_metrics,
"physical_success": physical_success,
"elapsed_ms": round((time.time() - started) * 1000),
}
report = {
"schema": "real2sim.rm65_pick_place_rollout_report.v1",
"created_at": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
"ok": physical_success,
"runtime": "mujoco",
"scene": str(sim_path),
"summary": summary,
"phases": [phase for phase, *_ in phases],
"note": "Scripted IK with contact-physics grasp; the cube pose is never overwritten. This is not a learned policy.",
}
report_path.write_text(json.dumps(report, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
artifact_paths["report"] = str(report_path)
return {"ok": bool(report["ok"]), "summary": summary, "artifact_paths": artifact_paths}
def cube_finger_contacts(data: Any, cube_geom: int, finger_geoms: set[int]) -> set[int]:
touching: set[int] = set()
for index in range(data.ncon):
contact = data.contact[index]
pair = {int(contact.geom1), int(contact.geom2)}
if cube_geom not in pair:
continue
touching.update(pair & finger_geoms)
return touching
def accumulate_contact_metrics(model: Any, data: Any, cube_geom: int, finger_geoms: set[int], metrics: dict[str, Any]) -> None:
import mujoco
for index in range(data.ncon):
contact = data.contact[index]
pair = {int(contact.geom1), int(contact.geom2)}
if cube_geom not in pair or not (pair & finger_geoms):
continue
force = np.zeros(6)
mujoco.mj_contactForce(model, data, index, force)
metrics["max_normal"] = max(float(metrics["max_normal"]), abs(float(force[0])))
metrics["max_tangent"] = max(float(metrics["max_tangent"]), float(np.linalg.norm(force[1:3])))
metrics["max_normal_z"] = max(float(metrics["max_normal_z"]), abs(float(contact.frame[2])))
metrics["samples"] = int(metrics["samples"]) + 1
def solve_ik(model: Any, data: Any, site_id: int, target: np.ndarray, qpos_adrs: list[int], dof_adrs: list[int], ctrl_ranges: np.ndarray) -> np.ndarray:
import mujoco
q_backup = data.qpos.copy()
q = data.qpos[qpos_adrs].copy()
damping = 1e-3
for _ in range(80):
data.qpos[qpos_adrs] = q
mujoco.mj_forward(model, data)
err = target - data.site_xpos[site_id]
if np.linalg.norm(err) < 0.012:
break
jacp = np.zeros((3, model.nv))
jacr = np.zeros((3, model.nv))
mujoco.mj_jacSite(model, data, jacp, jacr, site_id)
j = jacp[:, dof_adrs]
dq = j.T @ np.linalg.solve(j @ j.T + damping * np.eye(3), err)
q = q + np.clip(dq, -0.08, 0.08)
for i in range(6):
q[i] = float(np.clip(q[i], ctrl_ranges[i, 0], ctrl_ranges[i, 1]))
data.qpos[:] = q_backup
mujoco.mj_forward(model, data)
return q
def sample_state(model: Any, data: Any, phase: str, step: int, site_id: int, cube_qadr: int) -> dict[str, Any]:
return {
"step": step,
"time": float(data.time),
"phase": phase,
"qpos_arm": [float(x) for x in data.qpos[:6]],
"gripper_ctrl": float(data.ctrl[6]),
"gripper_pos": [float(x) for x in data.site_xpos[site_id]],
"cube_pos": [float(x) for x in data.qpos[cube_qadr : cube_qadr + 3]],
}
def update_manifest(manifest_path: Path, rollout_dir: Path, result: dict[str, Any], artifacts: dict[str, dict[str, Any]]) -> None:
manifest = json.loads(manifest_path.read_text(encoding="utf-8"))
new_assets = []
for key, artifact in artifacts.items():
path = Path(artifact["path"])
new_assets.append(
{
"name": path.name,
"role": f"rollout_{key}",
"kind": artifact_kind(path),
"path": str(path),
"relative_path": str(path.relative_to(manifest_path.parent)),
"url": f"/api/artifacts/{artifact['id']}/content",
"exists": True,
"size_bytes": path.stat().st_size,
"size_label": format_bytes(path.stat().st_size),
"generated": True,
}
)
old_assets = [item for item in manifest.get("assets", []) if not (isinstance(item, dict) and str(item.get("role", "")).startswith("rollout_"))]
manifest["assets"] = [*new_assets, *old_assets]
manifest["latest_rollout"] = {
"dir": str(rollout_dir),
"summary": result["summary"],
"artifacts": new_assets,
"status": "scripted_rollout_succeeded",
}
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 artifact_kind(path: Path) -> str:
suffix = path.suffix.lower()
if suffix in {".png", ".jpg", ".jpeg", ".webp", ".gif"}:
return "image"
if suffix == ".json":
return "metadata"
return suffix.lstrip(".") or "file"
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())