#!/usr/bin/env python3 """eval_policy_offline.py — π0.5 サーバーの予測を、お手本と答え合わせする (アームは動かさない). 学習が終わったら、実機で動かす前にこれを実行する (lab Log11 / LOG358). お手本の各ステップの「上カメラ画像 + 関節角 + グリッパー」をそのままサーバーへ送り, 返ってきた 1 秒分の予測 (10 x 7) と, お手本のその後 1 秒の動きを比べる. 見るもの: - 止まり (STUCK): お手本は 1 秒で 2° 以上動くのに, 予測は 0.5° 未満しか動かない (LOG358: 学習データの停止を覚えて, ブロックの真上で止まった現象) - ずれ: 予測とお手本の関節角の差 (1 秒分の平均・最大) - グリッパー: 0.5 秒先までの開閉の予測が, お手本と食い違う回数 注意: 使うのは学習に使ったお手本なので, 「覚えたか」の確認であって, 初めて見る位置での確認ではない. 実機は, 着いた姿勢で止まったまま次の画像を撮る. その「止まった姿勢から動き出せるか」を見るのが一番の目的. 前提: - Runpod で serve_policy が起動し, Mac から SSH トンネル (localhost:8000) が張れている - pip install msgpack "websockets>=11" (run_policy_client.py と同じ) Usage: python3 pi05/eval_policy_offline.py # 位置ごとに 1 本ずつ, 3 ステップおき python3 pi05/eval_policy_offline.py --episodes all --stride 1 python3 pi05/eval_policy_offline.py --raw-dir /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes 記録: /Volumes/KIOXIA/r2r2r-data/lab/Log11/offline_eval/{stamp}/ (rows.csv / summary.json) """ import argparse import csv import json import sys import time from datetime import datetime from pathlib import Path import cv2 import numpy as np sys.path.insert(0, str(Path(__file__).resolve().parent)) from policy_ws import PolicyClient # noqa: E402 TASK = "put the red block in the white box" KIOXIA = Path("/Volumes/KIOXIA") DATA_ROOT = KIOXIA / "r2r2r-data" DEFAULT_RAW = DATA_ROOT / "datasets" / "mecharm_redblock_box_teach_v1_trim2" / "raw_episodes" DEFAULT_OUT = DATA_ROOT / "lab" / "Log11" / "offline_eval" HORIZON = 10 # 学習設定の action_horizon (0.1 秒 x 10 = 1 秒) CLIENT_STEPS = 5 # run_policy_client.py が 1 チャンクで進む既定のステップ数 (0.5 秒先) STUCK_DEMO_DEG = 2.0 # お手本が 1 秒でこれ以上動く場面で STUCK_PRED_DEG = 0.5 # 予測がこれ未満しか動かなければ「止まり」 def pick_episodes(raw_dir: Path, mode: str): eps = sorted(p.parent for p in raw_dir.glob("*/episode.npz")) if not eps: sys.exit(f"[ERROR] お手本が見つからない: {raw_dir}") if mode == "all": return eps # ブロックの位置ごとに 1 本: つかんだ瞬間 (グリッパーの状態が閉に変わった所) の j0 を 5° 刻みで分ける groups = {} for ep in eps: s = np.load(ep / "episode.npz")["state"] ch = np.where(np.diff(s[:, 6]) != 0)[0] key = round(float(s[ch[0] + 1, 0]) / 5.0) if len(ch) else None groups.setdefault(key, ep) return [groups[k] for k in sorted(groups, key=lambda k: (k is None, k))] def load_rgb(path: Path): bgr = cv2.imread(str(path)) if bgr is None: raise RuntimeError(f"画像を読めない: {path}") return cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB) # run_policy_client.py と同じく RGB で送る def future(arr, t, n): """t から n ステップ分. 終わりを越える分は最後の値で埋める.""" out = arr[t:t + n] if len(out) < n: out = np.concatenate([out, np.repeat(arr[-1:], n - len(out), axis=0)]) return out def main(): ap = argparse.ArgumentParser() ap.add_argument("--host", default="localhost") ap.add_argument("--port", type=int, default=8000) ap.add_argument("--raw-dir", default=str(DEFAULT_RAW)) ap.add_argument("--episodes", default="per-position", choices=["per-position", "all"]) ap.add_argument("--stride", type=int, default=3, help="何ステップおきに問い合わせるか") ap.add_argument("--out", default=str(DEFAULT_OUT)) args = ap.parse_args() out_root = Path(args.out) if str(out_root).startswith(str(KIOXIA)) and not KIOXIA.is_mount(): sys.exit(f"[ERROR] KIOXIA がマウントされていない: {KIOXIA}") eps = pick_episodes(Path(args.raw_dir), args.episodes) stamp = datetime.now().strftime("%Y%m%d_%H%M%S") out_dir = out_root / stamp out_dir.mkdir(parents=True, exist_ok=True) print(f"[EVAL ] お手本 {len(eps)} 本 / {args.stride} ステップおき / 記録 {out_dir}") for ep in eps: print(f" {ep.name}") policy = PolicyClient(args.host, args.port) print(f"[POLICY] 接続 {policy.uri}") fields = ["episode", "t", "j0", "j1", "j2", "j3", "j4", "j5", "grip", "latency_s", "demo_move_1s", "pred_move_1s", "demo_move_05s", "pred_move_05s", "err_mean", "err_max", "err_max_joint", "grip_demo_05s", "grip_pred_05s", "grip_mismatch", "stuck"] rows = [] per_ep = {} t_start = time.time() with (out_dir / "rows.csv").open("w", newline="") as f: w = csv.DictWriter(f, fieldnames=fields) w.writeheader() for ep in eps: z = np.load(ep / "episode.npz") state, actions = z["state"].astype(np.float32), z["actions"].astype(np.float32) frames = sorted((ep / "frames").glob("*.jpg")) if len(frames) != len(state): sys.exit(f"[ERROR] 画像とステップの数が違う: {ep.name} ({len(frames)} / {len(state)})") stats = {"queries": 0, "stuck": 0, "grip_mismatch": 0, "err_mean": [], "stuck_t": []} for t in range(0, len(state), args.stride): q = state[t] t0 = time.time() out = policy.infer({"observation/image": load_rgb(frames[t]), "observation/state": q, "prompt": TASK}) lat = time.time() - t0 pred = np.asarray(out["actions"], dtype=np.float32)[:HORIZON] demo = future(actions, t, HORIZON) d1 = float(np.abs(demo[-1, :6] - q[:6]).max()) p1 = float(np.abs(pred[-1, :6] - q[:6]).max()) d05 = float(np.abs(demo[CLIENT_STEPS - 1, :6] - q[:6]).max()) p05 = float(np.abs(pred[CLIENT_STEPS - 1, :6] - q[:6]).max()) diff = np.abs(pred[:, :6] - demo[:, :6]) g_demo = "".join(str(int(v > 0.5)) for v in demo[:CLIENT_STEPS, 6]) g_pred = "".join(str(int(v > 0.5)) for v in pred[:CLIENT_STEPS, 6]) stuck = d1 >= STUCK_DEMO_DEG and p1 < STUCK_PRED_DEG row = {"episode": ep.name, "t": t, **{f"j{k}": round(float(q[k]), 2) for k in range(6)}, "grip": int(q[6] > 0.5), "latency_s": round(lat, 3), "demo_move_1s": round(d1, 2), "pred_move_1s": round(p1, 2), "demo_move_05s": round(d05, 2), "pred_move_05s": round(p05, 2), "err_mean": round(float(diff.mean()), 2), "err_max": round(float(diff.max()), 2), "err_max_joint": int(diff.max(axis=0).argmax()), "grip_demo_05s": g_demo, "grip_pred_05s": g_pred, "grip_mismatch": int(g_demo != g_pred), "stuck": int(stuck)} w.writerow(row) rows.append(row) stats["queries"] += 1 stats["stuck"] += int(stuck) stats["grip_mismatch"] += int(g_demo != g_pred) stats["err_mean"].append(float(diff.mean())) if stuck: stats["stuck_t"].append(t) mark = " [STUCK]" if stuck else "" gm = f" グリッパー お手本{g_demo} 予測{g_pred}" if g_demo != g_pred else "" print(f"[{ep.name[-15:]} t={t:3d}] 1秒の動き お手本{d1:5.1f}° 予測{p1:5.1f}° " f"ずれ 平均{diff.mean():4.1f}° 最大{diff.max():4.1f}°(j{row['err_max_joint']}){mark}{gm}") per_ep[ep.name] = {"queries": stats["queries"], "stuck": stats["stuck"], "stuck_t": stats["stuck_t"], "grip_mismatch": stats["grip_mismatch"], "err_mean_median": round(float(np.median(stats["err_mean"])), 2), "err_mean_max": round(float(np.max(stats["err_mean"])), 2)} policy.close() n = len(rows) summary = { "stamp": stamp, "raw_dir": args.raw_dir, "episodes_mode": args.episodes, "stride": args.stride, "queries": n, "elapsed_sec": round(time.time() - t_start, 1), "stuck": sum(r["stuck"] for r in rows), "grip_mismatch": sum(r["grip_mismatch"] for r in rows), "err_mean_median": round(float(np.median([r["err_mean"] for r in rows])), 2), "thresholds": {"stuck_demo_deg": STUCK_DEMO_DEG, "stuck_pred_deg": STUCK_PRED_DEG, "horizon": HORIZON, "client_steps": CLIENT_STEPS}, "per_episode": per_ep, } (out_dir / "summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2)) print() print(f"[SUMMARY] 問い合わせ {n} 回 / 止まり {summary['stuck']} 回 / グリッパーの食い違い {summary['grip_mismatch']} 回 / " f"ずれの中央値 {summary['err_mean_median']}°") for name, s in per_ep.items(): print(f" {name}: 止まり {s['stuck']}/{s['queries']} {s['stuck_t']} グリッパー {s['grip_mismatch']} " f"ずれ中央値 {s['err_mean_median']}°") print(f"[DONE ] 記録: {out_dir}") if __name__ == "__main__": main()