#!/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) → --via-stride N を付けると, 途中の姿勢 (N-1, 2N-1, ... ステップ) も中継点として 順に送る (チャンクの中の順番を残すため. 既定 0 = 1 点だけ送る従来どおり) → グリッパーの開閉がチャンク内で切り替わるなら, その直前の姿勢まで動いてから開閉 → 撮り直して次のチャンクへ 安全策: - 既定はドライラン (アームに送らない). 送るには --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) - --state-from-target を付けると, モデルに渡す state の 6 関節を「直前に送った目標」に する (既定 off = 実測). 肩の不足 (ブロックを持つと毎チャンク 2.4〜2.6°) が チャンクごとに積み重なるのを防ぐ. 目標と実測の差が 6° を超えたチャンクは実測に戻す. クランプ・検査・中継点のかけ直しは今までどおり実測が基準 (変わるのは state だけ) - 送ったあとに残ったずれ (動きが止まってから測る. > --settle-tol) は, **同じ目標を もう一度送って** 詰める (送信 --settle-retries 回まで. LOG359. 0 で従来どおり 送り直さない). 新しい目標は作らない. 動きが止まらない / 読めない / 送信が失敗した ときは送り直しをやめる. どの終わり方でも止めずに次のチャンクへ進む - 止まったときに 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_WAIT_SEC, SPEED, _send_and_wait_arrival # noqa: E402 本線と同じ 20 / 1.0 from policy_ws import PolicyClient # noqa: E402 from visual_servo_pick import ( # noqa: E402 close_daemon_socket, daemon_get_angles, daemon_send_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 # 送信前の検査の余裕 (小数の誤差で止まらないように) SETTLE_POLL_SEC = 0.2 # 送り直しの前後, 関節角をこの間隔で読む (codex A2) SETTLE_STILL_DEG = 0.2 # 続けて読んだ 2 回の差が 6 関節すべてこれ未満なら「止まった」 SETTLE_WAIT_SEC = 2.0 # 止まらなくてもここで待つのをやめ, そのときの値で判定する # --state-from-target で「直前に送った目標」を渡すのをやめる差 (LOG359). # 腕がどこかに当たって止まっているときに, 目標だけが先へ進むのを防ぐ. STATE_TARGET_MAX_GAP_DEG = 6.0 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 _fmt6(a): return None if a is None else [round(float(v), 2) for v in a[:6]] def read_until_still(): """関節角が止まるまで待ってから読む (codex A2/A6). 戻り値: (角度 or None, 止まったか, 直前に読めた角度 or None) 1 番目は **読めたときだけ** 値が入る. 読み取りに失敗したら None で, 古い値を代わりに返さない (A6: 失敗を隠さない). 3 番目に最後に読めた値を別に返す. _send_and_wait_arrival も daemon_send_angles も, 戻った直後はまだ動いている ことがある. その値で残りのずれを測ると, 動いている途中の姿勢で判断してしまう. SETTLE_POLL_SEC おきに読み, 続けて読んだ 2 回の差が 6 関節すべてで SETTLE_STILL_DEG 未満になったら「止まった」とみなす. SETTLE_WAIT_SEC まで止まらなければ打ち切り, そのときの値を still=False で返す. """ prev = read_angles() if prev is None: return None, False, None t0 = time.time() while time.time() - t0 < SETTLE_WAIT_SEC: time.sleep(SETTLE_POLL_SEC) cur = read_angles() if cur is None: return None, False, prev if np.max(np.abs(cur[:6] - prev[:6])) < SETTLE_STILL_DEG: return cur, True, cur prev = cur return prev, False, prev def plan_vias(acts, q, final_target, used, stride, lo, hi, max_move, final_raw_index=None): """チャンクの途中の姿勢も中継点として順に通る計画を作る (LOG359). 今までは plan_chunk が選んだ 1 点だけを送っていたので, アームはそこへ一直線に動き, チャンクの中の順番が消えていた. 2026-10-22 の実機 114643 のチャンク 04 では, 予測が「最初の 0.4 秒は持ち上げだけ (j0 はほぼ動かない) → 0.5 秒から土台 j0 が回る」 だったのに, 上げながら回る動きになっていた (お手本は つかんだあと 1.5 秒 j0 を止めて 持ち上げている). stride=0 (既定) なら中継点なし = used の 1 点だけ (従来と同じ計算). stride=N なら 0 始まりで N-1, 2N-1, ... のうち used より小さいステップを中継点にし, 最後に used (plan_chunk が選んだ姿勢) を置く. 例: N=3 used=9 → 2, 5, 8, 9 / N=3 used=6 (7 番目でグリッパーが変わる) → 2, 5, 6. 移動量の制限 (apply_limits) は **直前の中継点の目標から** 測る. 最初の中継点だけ 今の実測角 q から測る. 実測ではなく目標から測るので, 送る前に全部計算できる (= 送り始める前に全部の中継点を check_send_safe にかけられる). final_raw_index は, 最後の点 (used) の raw が actions の何行目か. グリッパーが 切り替わるチャンクでは plan_chunk が used-1 行目を返すので step とずれる (codex A1). 戻り値: [{"step", "raw_index", "raw", "target", "notes", "delta"}] の並び (target は ndarray). target は計画時のもので, 送る直前に実測角からかけ直す. """ steps = [k for k in range(stride - 1, used, stride)] if stride and stride > 0 else [] steps.append(int(used)) plan = [] prev = np.asarray(q, dtype=float).copy() for k in steps: last = k == used raw = (np.asarray(final_target, dtype=float) if last else np.asarray(acts[k, :6], dtype=float)) t, notes = apply_limits(raw, prev, lo, hi, max_move) plan.append({"step": int(k), "raw_index": (final_raw_index if last else int(k)), "raw": raw, "target": t, "notes": notes, "delta": t - prev}) prev = t return plan def settle_target(target, label, tol, retries): """送ったあとに残ったずれを, **同じ目標をもう一度送って** 詰める (LOG359). 2026-10-21/22 の実機 4 回 (183245 / 095251 / 100713 / 102053) で, ブロックを持って 肩を持ち上げる指令 (j1 を小さくする向き) は 28 回すべて指令まで届かず, 0.77〜2.80° (中央 2.00°) 低いところで止まっていた. 逆向き (下げる) は 0.5〜1° 行き過ぎる. 到達判定は _send_and_wait_arrival(..., 3.0) の 3° なので, このずれは「着いた」と 見なされ, モデルは実際の姿勢から次を予測する. その結果 持ち上げが毎回 2° ずつ失われ, ブロックがマットから離れない (102053 は上げる指令の合計 41° に対して実際は 11°). 新しい目標は作らない. 渡された target (apply_limits と check_send_safe を通ったもの) を そのまま送り直すだけ. どの終わり方でも止めない (警告を出して呼び出し側が次へ進む). retries は **settle で送る送信回数の上限** (codex A5). 送り直しは _send_and_wait_arrival ではなく, その中で使っている 1 回だけ送る層 daemon_send_angles(..., speed=SPEED, wait=SEND_WAIT_SEC) を直接呼ぶ. 中で再送しないので送信回数を正確に数えられる (上限を超えない). 送ったあとは read_until_still() で止まるのを待ってから測る. チャンク本体の送信は今までどおり _send_and_wait_arrival のまま. 戻り値: (記録用の dict, 送信回数). dict の reason は within_tol tol 以内 (送っていない) converged 送り直して tol 以内に入った not_converged 送信回数の上限まで送っても tol を超えたまま not_still SETTLE_WAIT_SEC 待っても動きが止まらない (動いている最中には送らない) send_failed daemon_send_angles が例外を投げた read_failed 関節角を読めない (古い値で判定しない) disabled --settle-retries 0 (何も読まない・何も送らない) どの reason でも形は同じ (final_err / joint / final_actual / last_valid_actual / tries). """ out = {"reason": "disabled", "tol": tol, "sends": 0, "final_err": None, "joint": None, "final_actual": None, "last_valid_actual": None, "tries": []} if retries <= 0: # --settle-retries 0 で従来と同じ動き (読みも送りもしない) return out, 0 tgt = np.asarray(target, dtype=float)[:6] tries, sends, n = [], 0, 0 actual, still, last_valid = read_until_still() while True: if actual is None: # 読めない値で判定しない (A6) reason = "read_failed" break err = np.abs(tgt - actual[:6]) k = int(err.argmax()) cur_err = float(err[k]) if cur_err <= tol: reason = "converged" if sends else "within_tol" break if not still: # 動いている最中には送らない (A2) reason = "not_still" break if sends >= retries: reason = "not_converged" break n += 1 entry = {"try": n, "max_err_before": round(cur_err, 2), "joint": k, "sends": 1, "sent": True, "still": None, "max_err_after": None, "actual": None} sends += 1 try: daemon_send_angles(tgt.tolist(), speed=SPEED, wait=SEND_WAIT_SEC) except Exception as e: entry["sent"] = False entry["error"] = f"{type(e).__name__}: {e}" tries.append(entry) print(f" [SETTLE] 送り直し {n} 回目の送信に失敗: {entry['error']}") reason = "send_failed" break after, still, lv = read_until_still() if lv is not None: last_valid = lv if after is None: tries.append(entry) print(f" [SETTLE] 送り直し {n} 回目 最大ずれ {cur_err:.2f}°（関節 j{k}）→ 読めず") actual = None reason = "read_failed" break entry["still"] = bool(still) after_err = float(np.abs(tgt - after[:6]).max()) entry["max_err_after"] = round(after_err, 2) entry["actual"] = _fmt6(after) tries.append(entry) print(f" [SETTLE] 送り直し {n} 回目 最大ずれ {cur_err:.2f}°（関節 j{k}）→ {after_err:.2f}°" + ("" if still else f" (動きが止まらないまま {SETTLE_WAIT_SEC:.1f} 秒で打ち切り)")) actual = after out.update(reason=reason, sends=sends, tries=tries, last_valid_actual=_fmt6(last_valid)) if actual is not None: err = np.abs(tgt - actual[:6]) k = int(err.argmax()) out.update(final_err=round(float(err[k]), 2), joint=k, final_actual=_fmt6(actual)) if sends or reason != "within_tol": gloss = {"within_tol": "もともと範囲内", "converged": "詰まった", "not_converged": "詰まらなかった", "not_still": "動きが止まらない", "send_failed": "送信が失敗", "read_failed": "関節角を読めない"}[reason] tail = ("最後の最大ずれ 不明" if out["final_err"] is None else f"最後の最大ずれ {out['final_err']:.2f}°（関節 j{out['joint']}）") print(f" [SETTLE] {reason} ({gloss}) 送信 {sends} 回 {tail}") if reason != "converged": print(" [WARN] 送り直しで詰めきれませんでした (止めずに次へ進みます)") return out, sends 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="上カメラの写り方がお手本と違っても続ける (既定は何も送らずに終了)") ap.add_argument("--state-from-target", action="store_true", help="モデルに渡す state の 6 関節を, 実測ではなく直前に送った目標にする " "(既定 off = 実測). 肩の不足がチャンクごとに積み重なるのを防ぐ") ap.add_argument("--via-stride", type=int, default=0, help="チャンクの途中の姿勢も中継点として順に送る (0 始まりで N-1, 2N-1, ... の " "ステップ. 既定 0 = 従来どおり選んだ 1 点だけ送る)") ap.add_argument("--settle-tol", type=float, default=1.0, help="送ったあとの残りのずれ (動きが止まってから測る) がこれを超えていたら " "同じ目標を送り直す (度)") ap.add_argument("--settle-retries", type=int, default=2, help="settle で同じ目標を送り直す回数の上限 (送信の試行回数). " "0 で送り直さない = 従来の動き") 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) last_sent_target = None # 直前のチャンクで最後に送った目標 (--state-from-target 用) 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 の 6 関節 (--state-from-target のときだけ「直前に送った目標」). # apply_limits / check_send_safe / 中継点のかけ直しは今までどおり実測 q を使う. measured_state = np.concatenate([q, [float(grip)]]).astype(np.float32) state_joints, state_source = q, "measured" if args.state_from_target: if last_sent_target is None: print("[STATE ] 実測を渡す (直前に送った目標がない)") else: gap = float(np.max(np.abs(last_sent_target - q[:6]))) if gap > STATE_TARGET_MAX_GAP_DEG: print(f"[WARN ] 目標と実測の差が {gap:.2f}° > {STATE_TARGET_MAX_GAP_DEG:.0f}° " "なので実測を渡します (腕がどこかに当たって止まっている可能性)") else: state_joints, state_source = last_sent_target, "target" print(f"[STATE ] {'目標' if state_source == 'target' else '実測'}を渡す " f"目標と実測の最大差 {gap:.2f}°" f"（関節 j{int(np.argmax(np.abs(last_sent_target - q[:6])))}）") state = np.concatenate([state_joints, [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) raw_target, grip_cmd, used = plan_chunk(acts, q, grip, args.steps_per_chunk) # plan_chunk は, グリッパーが切り替わるチャンクでは used-1 行目 (used=0 なら実測角) # を目標に返す. actions の何行目を使ったかを記録に残す (codex A1). final_raw_index = (used if grip_cmd is None else (used - 1 if used > 0 else None)) # 中継点の計画 (--via-stride 0 なら used の 1 点だけ = 従来と同じ). plan = plan_vias(acts, q, raw_target, used, args.via_stride, lo, hi, args.max_move_deg, final_raw_index) target, notes = plan[-1]["target"], plan[-1]["notes"] # チャンクの最終目標 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(), "state_source": state_source, "measured_state": measured_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}") if args.via_stride > 0: rec["vias"] = [{"step": v["step"], "raw_index": v["raw_index"], "raw": _fmt6(v["raw"]), "planned_target": _fmt6(v["target"]), "target": _fmt6(v["target"]), "notes": v["notes"], "called": False, "success": None, "retried": None, "actual_after": None} for v in plan] print(f" [VIA ] 中継点 {len(plan)} 点: step " + ", ".join(str(v["step"]) for v in plan)) for v in plan: for n in v["notes"]: print(f" [LIMIT] step {v['step']}: {n}") if not args.execute: # ドライランは中継点の一覧だけ出して送らない print(f" [VIA ] step {v['step']:2d} Δ=" + " ".join(f"{d:+5.1f}" for d in v["delta"])) # 送る直前の最終検査 (codex R1 / C2). 中継点は 1 点でも引っかかったら # **どれも送らずに** 止める (途中まで動かして止まる方が危ない). # 送信にもグリッパー操作にも入らず止める. # HOME 復帰 (h) だけはこの検査を通さずに送れる (範囲の外にいるアームを戻す手段). unsafe = [] for v in plan: for u in check_send_safe(v["target"], lo, hi): unsafe.append(u if args.via_stride <= 0 else f"step {v['step']}: {u}") stopped = False if not unsafe and args.execute: # ここから先で送れなかった経路 (s / 送信なし / 中断) は, 次のチャンクで # 実測を渡すために None のままにする (--state-from-target 用). last_sent_target = None 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 broke, final_target, final_sent, sent_any = False, None, False, False for vi, v in enumerate(plan): v_last = vi == len(plan) - 1 tag = (f"pi05_chunk{i:03d}" if args.via_stride <= 0 else f"pi05_chunk{i:03d}_via{v['step']:02d}") tgt, vdelta = v["target"], v["delta"] if args.via_stride > 0: # 送る直前に実測角を読み, そこから apply_limits と check_send_safe を # かけ直す (codex A2: 1 点だけ送る従来の方式と同じ基準にそろえる). qa = read_angles() if qa is None: print(" [WARN] 送る直前の関節角を読めない. " "この中継点も残りの中継点も送りません") print(" [WARN] 未到達. j4 の発熱などを確認してください") rec["arrived"] = False broke = True if args.auto: exit_code = 1 log_f.write(json.dumps(rec) + "\n") stopped = True break tgt, notes_now = apply_limits(v["raw"], qa, lo, hi, args.max_move_deg) if notes_now != v["notes"]: for n in notes_now: print(f" [LIMIT] step {v['step']} (実測から): {n}") rec["vias"][vi].update(target=_fmt6(tgt), notes=notes_now) u2 = check_send_safe(tgt, lo, hi) if u2: unsafe = [f"step {v['step']}: {x}" for x in u2] broke = True break vdelta = tgt - qa # 送らない判定は実測との差で見る (codex ❌1) final_target = tgt if np.max(np.abs(vdelta)) <= 0.5: if args.via_stride > 0: print(f" [VIA ] step {v['step']:2d} Δ=" + " ".join(f"{d:+5.1f}" for d in vdelta) + " 送らず (0.5° 以下)") continue ok, retried = _send_and_wait_arrival(tgt.tolist(), tag, 3.0) sent_any = True if args.via_stride > 0: rec["vias"][vi].update(called=True, success=bool(ok), retried=bool(retried), actual_after=_fmt6(read_angles())) print(f" [VIA ] step {v['step']:2d} Δ=" + " ".join(f"{d:+5.1f}" for d in vdelta) + (" 着いた" if ok else " 未到達")) if v_last: rec["arrived"] = bool(ok) final_sent = True if not ok: print(" [WARN] 未到達. j4 の発熱などを確認してください") if args.auto: exit_code = 1 log_f.write(json.dumps(rec) + "\n") stopped = True broke = True break if v_last: # 残ったずれを同じ目標の送り直しで詰める (LOG359). 止める条件は増やさない. rec["settle"], rec["settle_retries_used"] = settle_target( tgt, f"pi05_chunk{i:03d}", args.settle_tol, args.settle_retries) if not broke and args.via_stride > 0 and not final_sent and final_target is not None: # 最後の中継点が「送らない」になった場合も, チャンクの最後の目標について # 到達の記録と settle は必ず行う (codex ❌2: 重複送信を省くことと分ける). qa = read_angles() rec["arrived"] = bool(qa is not None and float(np.max(np.abs(final_target - qa[:6]))) <= 3.0) rec["settle"], rec["settle_retries_used"] = settle_target( final_target, f"pi05_chunk{i:03d}", args.settle_tol, args.settle_retries) if not broke: # 次のチャンクで --state-from-target が使う「直前に送った目標」. # 送らなかった (s / 送信なし / 中断) ときは None に戻して実測を渡す. last_sent_target = (np.asarray(final_target, dtype=float).copy() if sent_any and final_target is not None else None) 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 if stopped: break if unsafe: # 計画の検査 または 送る直前のかけ直しで範囲の外になったとき. # 送信にもグリッパー操作にも入らず止める. # HOME 復帰 (h) だけはこの検査を通さずに送れる (範囲の外にいるアームを戻す手段). 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 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())