#!/usr/bin/env python3 """run_policy_client.py — π0.5 (Runpod の serve_policy) の出力で mechArm 270 を動かす Mac 側クライアント. 流れ (1 チャンクごと): 上カメラ画像 + 実測 6 軸 + グリッパー状態 → π0.5 → 行動チャンク (10 x 7, 0.1 秒刻みの絶対角) → 先頭 N ステップ先 (既定 5 = 0.5 秒先) の姿勢へ単発 send_angles (学習データと同じ速度 20) → グリッパーの開閉がチャンク内で切り替わるなら, その直前の姿勢まで動いてから開閉 → 撮り直して次のチャンクへ 安全策: - 既定はドライラン (アームに送らない). 送るには --execute - --execute でも既定は 1 チャンクごとに Enter で確認 (--auto で連続) - 学習データで通った関節範囲 ±ENVELOPE_MARGIN_DEG の外へは出さない (クランプ) - 肩 j1 の上限は J1_SAFE_MAX (88°) で更に切る. これを超える指令はアームが黙って捨てる (LOG352) - 送る直前に最終検査. 6 関節のどれかが安全範囲の外か非有限値なら, 送信もグリッパー操作もせず 停止 (codex R1 / C2). HOME 復帰 (h) はこの検査を通さずに送れる - 1 回の移動量を --max-move-deg で制限 (クランプ), j4 は J4_SAFE 範囲内 - グリッパー操作後は 2 秒待つ (DAY175 対策・本線と同じ) - --raw-dir は必須. 安全範囲は **学習に使ったデータ** から作る (別のデータだと範囲が合わない) - 開始時に上カメラの写り方をお手本と比べ, OK 以外ならアームに何も送らずに終了 (ずれ / 測れない. --ignore-framing で続行). この確認が通るまで daemon にも触らない - 開始時のグリッパー開放も, 最初に送る目標が検査を通ってから (codex R2) - 止まったときに finally でアームを動かさない. 戻すときは Enter 確認の h で明示的に送る 終了コード: 0 = 最後まで / q / h / Ctrl-C, 1 = 安全停止・写り方の不一致・途中のエラー 前提: - Pi の daemon が待ち受け中 / 上カメラ (iPhone 連係) が見えている - Runpod で serve_policy が起動し, Mac から SSH トンネル (localhost:8000) が張れている - pip install msgpack "websockets>=11" Usage: RAW=/Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes python3 pi05/camera_align.py --raw-dir $RAW # 先に上カメラの写り方を合わせる python3 pi05/run_policy_client.py --raw-dir $RAW # ドライラン (予測を表示・記録するだけ) python3 pi05/run_policy_client.py --raw-dir $RAW --execute # 1 チャンクごとに Enter で実行 python3 pi05/run_policy_client.py --raw-dir $RAW --execute --auto --max-chunks 40 記録: /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/{stamp}/ (log.jsonl + 各チャンクの入力画像) --record を付けると recorder.py --top にも trial_start/trial_end を送り, 上カメラ動画を残す. """ import argparse import json import socket import sys import time import types from datetime import datetime from pathlib import Path import cv2 import numpy as np # ---- パス (run_pick_place.py と同じ構成) ---------------------------------------- _HERE = Path(__file__).resolve().parent _ROOT = _HERE.parent _PLACE = _ROOT / "dynamic_tracking_limits" / "place" / "place_modules" _DRAG = _ROOT / "dynamic_tracking_limits" / "drag_teaching" for p in (_HERE, _ROOT, _PLACE, _DRAG): sys.path.insert(0, str(p)) # pymycobot スタブ (waypoint_replay 経由の import を Mac 上で通すため. アーム制御は daemon 経由のみ) if "pymycobot" not in sys.modules: _pkg = types.ModuleType("pymycobot"); _mod = types.ModuleType("pymycobot.mycobot") class _MyCobotStub: def __init__(self, *a, **k): raise RuntimeError("MyCobot は直接使わない (daemon 経由のみ)") _mod.MyCobot = _MyCobotStub sys.modules["pymycobot"] = _pkg; sys.modules["pymycobot.mycobot"] = _mod import camera_resolver # noqa: E402 import waypoint_replay as wr # noqa: E402 from camera_align import ( # noqa: E402 FRAMING_MAX_DX, FRAMING_MAX_DY, FRAMING_MIN_RESPONSE, framing_verdict, load_reference, measure_framing, ) from fk_lookup import J1_SAFE_MAX # noqa: E402 88.0. 本線と二重定義しない from j0_sweep_scan import _send_and_wait_arrival # noqa: E402 from policy_ws import PolicyClient # noqa: E402 from visual_servo_pick import ( # noqa: E402 close_daemon_socket, daemon_get_angles, daemon_set_gripper, daemon_set_gripper_value, get_daemon_socket, ) TASK = "put the red block in the white box" IMG_LONG = 384 # build_demo_episodes.py と同じ (学習画像の長辺) HOME = [0.0, 0.0, 0.0, 0.0, 0.0, -30.0] GRIP_VALUE = 20 # run_pick_place.py の close 値 GRIPPER_SLEEP = 2.0 J4_SAFE = (-90.0, 60.0) # run_pick_place.py の J4_SAFE_MIN/MAX ENVELOPE_MARGIN_DEG = 10.0 SEND_EPS_DEG = 1e-6 # 送信前の検査の余裕 (小数の誤差で止まらないように) DATA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data") LOG_ROOT = DATA_ROOT / "runs" / "pi05_policy" RECORDER = ("127.0.0.1", 5556) def load_envelope(raw_dir: Path): """学習データで通った各関節の範囲 ± マージン. 物理可動域 (wr.JOINT_LIMITS)・J4_SAFE・J1_SAFE_MAX で更に絞る.""" qs = [np.load(p)["state"][:, :6] for p in raw_dir.glob("*/episode.npz")] if not qs: sys.exit(f"[ERROR] 学習エピソードが見つからない (安全範囲を作れない): {raw_dir}") q = np.concatenate(qs) lo = q.min(axis=0) - ENVELOPE_MARGIN_DEG hi = q.max(axis=0) + ENVELOPE_MARGIN_DEG for k, (plo, phi) in enumerate(wr.JOINT_LIMITS): lo[k], hi[k] = max(lo[k], plo), min(hi[k], phi) lo[4], hi[4] = max(lo[4], J4_SAFE[0]), min(hi[4], J4_SAFE[1]) # 肩 j1: wr.JOINT_LIMITS[1] の上限は +123° なので, マージンを足すと +98° まで通ってしまう. # 88° を超える指令はアームが黙って捨てる (LOG352) ので, ここで切る. hi[1] = min(hi[1], J1_SAFE_MAX) # codex R3: lo > hi や非有限値のまま運転しない (np.clip は例外を出さないので黙って通る). # lo を書き換えて辻褄を合わせるのではなく, 起動を中止する. bad = [k for k in range(6) if not (np.isfinite(lo[k]) and np.isfinite(hi[k])) or lo[k] > hi[k]] if bad: sys.exit("[ERROR] 安全範囲が不整合 (lo > hi か非有限値): " + " ".join(f"j{k}[{lo[k]:+.1f},{hi[k]:+.1f}]" for k in bad) + f"\n 学習データが安全制約 (J1_SAFE_MAX={J1_SAFE_MAX:.0f}° / J4_SAFE / " f"JOINT_LIMITS) と合っていない: {raw_dir}") return lo, hi, len(qs) def read_angles(): for _ in range(5): try: a = daemon_get_angles() except Exception: a = None if wr._is_valid_angles(a): return np.array([float(x) for x in a]) time.sleep(0.1) return None def grab_rgb(cap): for _ in range(3): # バッファに溜まった古いフレームを捨てる cap.grab() ok, frame = cap.read() if not ok or frame is None: return None, None h, w = frame.shape[:2] s = IMG_LONG / max(h, w) small = cv2.resize(frame, (round(w * s), round(h * s)), interpolation=cv2.INTER_AREA) return cv2.cvtColor(small, cv2.COLOR_BGR2RGB), small # モデルへは RGB, 保存は BGR def plan_chunk(actions, q, grip, n_steps): """チャンクから今回動かす目標を決める. 戻り値: (目標6軸, グリッパー指令 None/0/1, 使ったステップ).""" want = (actions[:, 6] > 0.5).astype(int) for f in range(min(n_steps, len(actions))): if want[f] != grip: target = q.copy() if f == 0 else actions[f - 1, :6] return np.asarray(target, dtype=float), int(want[f]), f k = min(n_steps, len(actions)) - 1 return np.asarray(actions[k, :6], dtype=float), None, k def apply_limits(target, q, lo, hi, max_move): notes = [] t = target.copy() clipped = np.clip(t, lo, hi) for k in np.where(np.abs(clipped - t) > 1e-6)[0]: notes.append(f"j{k} 安全範囲でクランプ {t[k]:+.1f}→{clipped[k]:+.1f}") t = clipped d = t - q big = np.abs(d) > max_move for k in np.where(big)[0]: notes.append(f"j{k} 移動量 {d[k]:+.1f}° を ±{max_move:.0f}° に制限") t = q + np.clip(d, -max_move, max_move) return t, notes def check_send_safe(target, lo, hi): """送る直前の最終検査 (codex R1 / C2). 違反の理由を返す (空なら送って良い). apply_limits は「安全範囲でクランプ → 移動量の上限」の順なので, 現在角が安全範囲の 外にあると, クランプで範囲内に入れた目標が移動量の制限でまた範囲の外に戻る. j1 (codex R1): 現在 110° / 予測 100° / 上限 20° → 88° にクランプ後, 110-20 = 90° を送る j4 (codex C2): 現在 100° / 予測 90° / 上限 20° → 60° にクランプ後, 100-20 = 80° を送る j1 だけでなく 6 関節すべてを見る. ここで再クランプすると移動量の上限を破るので, 送らずに止める. HOME 復帰 (Enter 確認の h) はこの検査を通さない: 範囲の外にいるアームを戻す手段として残す. """ if not np.all(np.isfinite(target)): return ["目標に非有限値が入っている: " + " ".join(f"j{k}={v}" for k, v in enumerate(target))] bad = [] for k in range(6): if target[k] < lo[k] - SEND_EPS_DEG or target[k] > hi[k] + SEND_EPS_DEG: msg = f"j{k} {target[k]:+.2f}° が安全範囲 [{lo[k]:+.2f}, {hi[k]:+.2f}] の外" if k == 1 and target[k] > hi[k] and abs(hi[k] - J1_SAFE_MAX) < 1e-9: msg += (f" (上限は J1_SAFE_MAX={J1_SAFE_MAX:.0f}°. " "これを超える指令はアームが黙って捨てる / LOG352)") elif k == 4: msg += f" (j4 の範囲は 学習データ ∩ J4_SAFE [{J4_SAFE[0]:+.0f}, {J4_SAFE[1]:+.0f}])" bad.append(msg) return bad def notify_recorder(sock, payload): try: payload = dict(payload, ts=time.time()) sock.sendto((json.dumps(payload) + "\n").encode(), RECORDER) except Exception: pass def main(): ap = argparse.ArgumentParser() ap.add_argument("--host", default="localhost") ap.add_argument("--port", type=int, default=8000) ap.add_argument("--execute", action="store_true", help="アームに送る (未指定はドライラン)") ap.add_argument("--auto", action="store_true", help="Enter 確認なしで連続実行") ap.add_argument("--steps-per-chunk", type=int, default=5, help="1 チャンクで何ステップ先まで進むか (0.1 秒単位)") ap.add_argument("--max-chunks", type=int, default=60) ap.add_argument("--max-move-deg", type=float, default=20.0, help="1 回の移動で各関節が動ける上限") ap.add_argument("--raw-dir", required=True, help="安全範囲の算出に使う学習エピソード (必須. 学習に使った raw_episodes を指定する)") ap.add_argument("--prompt", default=TASK) ap.add_argument("--record", action="store_true", help="recorder.py --top に上カメラ動画を記録させる") ap.add_argument("--ignore-framing", action="store_true", help="上カメラの写り方がお手本と違っても続ける (既定は何も送らずに終了)") args = ap.parse_args() if not DATA_ROOT.exists(): sys.exit(f"[ERROR] SSD 未マウント: {DATA_ROOT}") lo, hi, n_eps = load_envelope(Path(args.raw_dir)) print(f"[SAFE ] 安全範囲 (学習 {n_eps} 本 ±{ENVELOPE_MARGIN_DEG:.0f}°): " + " ".join(f"j{k}[{lo[k]:+.0f},{hi[k]:+.0f}]" for k in range(6))) print(f"[SAFE ] 肩 j1 の上限は J1_SAFE_MAX={J1_SAFE_MAX:.0f}° で切った " f"(これを超える指令はアームが黙って捨てる / LOG352) 元データ: {args.raw_dir}") stamp = datetime.now().strftime("%Y%m%d_%H%M%S") log_dir = LOG_ROOT / f"pi05_{stamp}" (log_dir / "frames").mkdir(parents=True, exist_ok=True) log_f = (log_dir / "log.jsonl").open("w") run_stem = f"pi05_{stamp}" mode = "EXECUTE" if args.execute else "DRY-RUN (アームには送らない)" print(f"[MODE ] {mode} steps/chunk={args.steps_per_chunk} max_move={args.max_move_deg}° log={log_dir}") # 以降は try/finally の中だけで資源を触る (codex R3: 途中で終了しても cap / log_f を必ず閉じる). # sys.exit は SystemExit を投げるので, この finally を通って後始末される. cap = policy = rec_sock = None exit_code = 0 grip = 0 grip_opened = False # 開始時のグリッパー開放を済ませたか (codex R2) try: cams = camera_resolver.resolve_cameras(require_top=True, require_side=False, verbose=False) cap = cv2.VideoCapture(cams["top"]) if not cap.isOpened(): sys.exit("[ERROR] 上カメラを開けない") t_cam = time.time() while time.time() - t_cam < 8.0: # 連係カメラは映像が来るまで数秒かかることがある ok, _f = cap.read() if ok and _f is not None: break time.sleep(0.2) else: sys.exit("[ERROR] 上カメラから映像が来ない. iPhone のロック解除・連係カメラの接続を確認 " "(Photo Booth の『カメラ』メニューに iPhone が出るか)") # E (2026-10-21): 上カメラの写り方は 2 通りあり, カメラを開くたびに切り替わることがある. # お手本と違う写り方のまま動かすと, 学習と別の画像を見せることになる # (082645 は dy=+37.5 のまま実機を動かした. その 1 分前のドライラン 082512 は dy=-7.8). # アームは HOME に置いた状態で, 1 枚目を お手本の 1 枚目 (同じく HOME) と比べる. # **この確認が通るまで daemon にも触らない** (アームには何も送らない). ref_bgr, ref_path = load_reference(Path(args.raw_dir)) bgr0 = None for _ in range(10): _rgb0, bgr0 = grab_rgb(cap) if bgr0 is not None: break time.sleep(0.2) if bgr0 is None: sys.exit("[ERROR] 上カメラの 1 枚目を撮れない") dx, dy, resp = measure_framing(ref_bgr, bgr0) verdict, why = framing_verdict(dx, dy, resp) print(f"[CAMERA] 写り方 dy={dy:+.1f} dx={dx:+.1f} response={resp:.3f} → {verdict} ({why})") print(f"[CAMERA] 基準: {ref_path}") log_f.write(json.dumps({"type": "framing", "dy": round(dy, 2), "dx": round(dx, 2), "response": round(resp, 3), "verdict": verdict, "why": why, "max_dy": FRAMING_MAX_DY, "max_dx": FRAMING_MAX_DX, "min_response": FRAMING_MIN_RESPONSE, "ok": verdict == "OK", "ignored": bool(args.ignore_framing), "reference": str(ref_path)}) + "\n") log_f.flush() if verdict != "OK": if not args.ignore_framing: sys.exit(f"[ABORT ] 上カメラの写り方が『{verdict}』. アームには何も送らずに終了しました.\n" f" {why}\n" " pi05/camera_align.py で合わせ直す " "(iPhone を付け直す / 連係カメラを開き直す).\n" " この写り方のまま続けるなら --ignore-framing") print(f"[WARN ] 写り方が『{verdict}』だが --ignore-framing が指定されたので続けます") # ---- ここから先でアームに触る (写り方の確認を通ったあと) ---- get_daemon_socket() policy = PolicyClient(args.host, args.port) print(f"[POLICY] 接続 {policy.uri} metadata={policy.metadata}") rec_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) if args.record: notify_recorder(rec_sock, {"type": "trial_start", "run_stem": run_stem}) q = read_angles() if q is None: sys.exit("[ERROR] 関節角を読めない (daemon を確認)") if np.max(np.abs(q - np.array(HOME))) > 5.0: print(f"[WARN ] HOME から離れています (max|Δ|={np.max(np.abs(q - np.array(HOME))):.1f}°). " "学習データは HOME から始まっています") print(" 現在角: " + " ".join(f"j{k}={v:+.1f}" for k, v in enumerate(q))) if args.execute: # 開始時の開放は, 最初に送る目標が検査を通ってからにする (codex R2). # 以前はここで開いていたので, 1 チャンク目が安全ガードで止まる場合でも # 爪だけ先に開いてしまっていた (物を持っていれば落とす). print("[GRIP ] 開始時のグリッパー開放は, 最初の目標が検査を通ってから行います " "(爪が閉じていたら手で開けておく)") for i in range(args.max_chunks): q = read_angles() if q is None: print("[ERROR] 関節角を読めない (daemon を確認). 停止"); exit_code = 1; break rgb = bgr_small = None for _ in range(10): rgb, bgr_small = grab_rgb(cap) if rgb is not None: break time.sleep(0.2) if rgb is None: print("[ERROR] 上カメラの映像が途切れた (iPhone のスリープ等). 停止") exit_code = 1; break state = np.concatenate([q, [float(grip)]]).astype(np.float32) t0 = time.time() out = policy.infer({"observation/image": rgb, "observation/state": state, "prompt": args.prompt}) latency = time.time() - t0 acts = np.asarray(out["actions"], dtype=float) target, grip_cmd, used = plan_chunk(acts, q, grip, args.steps_per_chunk) target, notes = apply_limits(target, q, lo, hi, args.max_move_deg) delta = target - q cv2.imwrite(str(log_dir / "frames" / f"{i:04d}.jpg"), bgr_small) rec = {"chunk": i, "t": time.time(), "latency_s": round(latency, 3), "state": state.tolist(), "actions": acts.tolist(), "used_step": used, "target": target.tolist(), "grip_cmd": grip_cmd, "notes": notes, "executed": False} gtxt = "" if grip_cmd is None else (" グリッパー→閉" if grip_cmd == 1 else " グリッパー→開") print(f"[{i:03d}] 推論 {latency*1000:.0f}ms {used + 1}ステップ先 Δ=" + " ".join(f"{d:+5.1f}" for d in delta) + gtxt) for n in notes: print(f" [LIMIT] {n}") # 送る直前の最終検査 (codex R1 / C2). 送信にもグリッパー操作にも入らず止める. # HOME 復帰 (h) だけはこの検査を通さずに送れる (範囲の外にいるアームを戻す手段). unsafe = check_send_safe(target, lo, hi) if unsafe: rec["unsafe"] = unsafe rec["envelope"] = {"lo": [round(float(v), 2) for v in lo], "hi": [round(float(v), 2) for v in hi]} for u in unsafe: print(f" [ABORT] {u}") print("[STOP ] 安全でない目標のため, 送らずに停止します " "(現在角が安全範囲の外にあると, 移動量の制限で範囲の外に戻る)") # 記録は入力より前に書いて flush する (codex C2-1: 入力待ちや HOME 復帰中の # Ctrl-C でこのチャンクの unsafe / envelope が消えないように). log_f.write(json.dumps(rec) + "\n"); log_f.flush() exit_code = 1 if args.execute and not args.auto: ans = input(" h=HOMEに戻して終了 / 他=そのまま終了 > ").strip().lower() if ans == "h": ok_home, _ = _send_and_wait_arrival(HOME, "pi05_home", 2.5) log_f.write(json.dumps({"chunk": i, "type": "home_return", "t": time.time(), "home_requested": True, "home_arrived": bool(ok_home)}) + "\n") log_f.flush() else: print(" アームを HOME に置き直してからやり直してください") break if args.execute: if not args.auto: ans = input(" Enter=実行 / s=送らず次へ / h=HOMEに戻して終了 / q=終了 > ").strip().lower() if ans == "q": log_f.write(json.dumps(rec) + "\n"); break if ans == "h": ok_home, _ = _send_and_wait_arrival(HOME, "pi05_home", 2.5) rec["home_requested"] = True rec["home_arrived"] = bool(ok_home) log_f.write(json.dumps(rec) + "\n"); break if ans == "s": log_f.write(json.dumps(rec) + "\n"); continue if not grip_opened: # 開始時の開放はここで 1 度だけ (写り方の確認と目標の検査を通り, 送ると決めてから) print("[GRIP ] 開始時にグリッパーを開く") daemon_set_gripper(0) time.sleep(GRIPPER_SLEEP) grip_opened = True rec["initial_gripper_open"] = True if np.max(np.abs(delta)) > 0.5: ok, _ = _send_and_wait_arrival(target.tolist(), f"pi05_chunk{i:03d}", 3.0) rec["arrived"] = bool(ok) if not ok: print(" [WARN] 未到達. j4 の発熱などを確認してください") if args.auto: exit_code = 1 log_f.write(json.dumps(rec) + "\n"); break if grip_cmd is not None: if grip_cmd == 1: daemon_set_gripper_value(GRIP_VALUE) else: daemon_set_gripper(0) time.sleep(GRIPPER_SLEEP) grip = grip_cmd rec["executed"] = True log_f.write(json.dumps(rec) + "\n"); log_f.flush() except KeyboardInterrupt: print("\n[STOP ] Ctrl-C") finally: # finally ではアームを動かさない (HOME 復帰もサーボ解放もしない). 止まった姿勢は # そのまま保つ: 未知の姿勢から勝手に動かす方が危ない. 戻すときは h で明示的に送る. if args.record and rec_sock is not None: notify_recorder(rec_sock, {"type": "trial_end", "run_stem": run_stem}) if not log_f.closed: log_f.close() if cap is not None: cap.release() if policy is not None: policy.close() try: close_daemon_socket() except Exception: pass print(f"[DONE ] 記録: {log_dir}") # 終了コードは正常に抜けた経路でだけ表示する (sys.exit で抜ける場合は, その [ABORT] / # [ERROR] の文言が出て終了コード 1 になるので, ここで 0 と書くと食い違う). if exit_code: print(f"[DONE ] 終了コード {exit_code} (安全停止 / 途中のエラー)") return exit_code if __name__ == "__main__": sys.exit(main())