#!/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 で明示的に送る 巻き戻しと関節の補正 (2026-10-29. 推論のあとの確認で選ぶ. b / c を押さなければ今までと同じ動き): - b = 巻き戻し. 直前に実行した 1 回分 (推論 1 回 か 補正 1 回) で送った目標を逆順にたどり, 始めたときの姿勢 (推論なら measured_state) へ戻す. 戻ったらその姿勢からもう一度推論する. 続けて b を押すと 1 回分ずつさかのぼる. グリッパーを動かした回は巻き戻さない (断って聞き直す). ただし つかんだ区間 (グリッパーを閉じた推論 1 回分) は 1 文字で聞いて巻き戻す (2026-10-29 依頼 27): y = 持っている: 逆にたどって つかんだ位置まで下ろす → 開いて置く → 始めの姿勢へ n = 落とした: 今の位置ですぐ開く → 逆にたどって始めの姿勢へ → 人がブロックを置き直して Enter その他 = やめる. rewind 行に grasp_undo: "placed" / "dropped" 離した区間 (グリッパーを開いた推論) と グリッパーを動かした補正 は今までどおり断る. log.jsonl に {"type": "rewind", "rewound": true, "chunk": 巻き戻した推論の番号, ...} を足す (推論の行は消さない・書き換えない) - c = 関節の補正. Enter を待たずに 1 文字ずつ読む (押し続けるとキーリピートで繰り返す. 1 回送るごとに溜まったキーは捨てる). 1〜6 で関節, + / - (. / ,) で 1 刻み, [ ] で刻み 0.5/1/2°, g でグリッパー, Enter で推論に戻る, b で取り消して始めの姿勢へ戻る. 送り方は推論と同じ (_send_and_wait_arrival = send_angles speed 20 と到着確認) で, apply_limits と check_send_safe をそのまま通す (通らない 1 刻みは送らない). 1 刻みごとに log.jsonl へ {"type": "correction", ...} - --execute の間は上カメラと実測角を 1 秒 10 コマで recording/ に残す (policy_recorder.py. お手本と同じ形). 1 コマごとに状態 (model / rewind / correct / home / wait) を付ける. wait = 推論のあとの確認で入力を待っている間 (人が手でブロックを置き直すので学習データに使わない) h / q で終えるときに 結果 (y / n / Enter=記録しない) を聞き, {run_stem}.json に残す - --dry-keys: ドライランでも 1 回ごとに止まり b / c を試せる (アームにも daemon にも触らない. 仮の姿勢を HOME から動かして流れだけ確かめる. Enter で「仮に実行」= 仮の姿勢が目標へ移る) 終了コード: 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 python3 pi05/run_policy_client.py --raw-dir $RAW --dry-keys # アームなしで b / c の流れを試す 記録: /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/{stamp}/ (log.jsonl + 各チャンクの入力画像. --execute のときは recording/ と {run_stem}.json も) --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_recorder import PolicyRecorder, install_daemon_lock # noqa: E402 from policy_ws import PolicyClient # noqa: E402 from rewind_correct import ( # noqa: E402 CORRECTION_DEFAULT_STEP_INDEX, CORRECTION_HELP, CORRECTION_STEPS, GRASP_DROPPED, GRASP_PLACED, MODE_CORRECT, MODE_HOME, MODE_MODEL, MODE_REWIND, MODE_WAIT, next_step_index, parse_correction_key, rewind_waypoints, split_leg, ) 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 return frame_to_model(frame) def frame_to_model(frame): """カメラの 1 コマ (BGR) → (モデルに渡す RGB, 保存用の BGR). 長辺 IMG_LONG に縮める.""" 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 read_angles_once(): """記録スレッド用: 1 回だけ読む (読めなければ None. 待たない・繰り返さない).""" try: a = daemon_get_angles() except Exception: return None return [float(x) for x in a] if wr._is_valid_angles(a) else None class ArmIO: """巻き戻しと補正で使う送り口. --execute なら daemon, ドライラン (--dry-keys) なら仮の姿勢を動かす. 実機での送り方は推論のチャンクと同じ _send_and_wait_arrival (send_angles speed 20 + 到着確認 3°). グリッパーも推論と同じ (閉 = daemon_set_gripper_value(GRIP_VALUE) / 開 = daemon_set_gripper(0) のあと GRIPPER_SLEEP 待つ). """ def __init__(self, execute, recorder=None, sim_q=None): self.execute = execute self.recorder = recorder self.sim_q = None if sim_q is None else np.asarray(sim_q, dtype=float).copy() def read(self): if self.execute: return read_angles() return None if self.sim_q is None else self.sim_q.copy() def send(self, target, tag): if self.execute: ok, _ = _send_and_wait_arrival(np.asarray(target, dtype=float).tolist(), tag, 3.0) return bool(ok) self.sim_q = np.asarray(target, dtype=float).copy() print(f" [DRY ] {tag}: 送らない (仮の姿勢を " + " ".join(f"{v:+.1f}" for v in self.sim_q) + " に)") return True def gripper(self, state): if self.recorder is not None: self.recorder.gripper(state) if not self.execute: print(f" [DRY ] グリッパー→{'閉' if state == 1 else '開'} (送らない)") return if state == 1: daemon_set_gripper_value(GRIP_VALUE) else: daemon_set_gripper(0) time.sleep(GRIPPER_SLEEP) class KeyReader: """補正モード用: Enter を待たずに 1 文字ずつ読む (termios の cbreak). 端末でないとき (テストでパイプから流すとき) は普通に 1 文字ずつ読む. flush() で溜まったキー (キーリピートで送っている間に溜まった分) を捨てる. """ def __enter__(self): self.fd = None if sys.stdin.isatty(): import termios import tty self.fd = sys.stdin.fileno() self.old = termios.tcgetattr(self.fd) tty.setcbreak(self.fd) return self def get(self): if self.fd is None: return sys.stdin.read(1) # 端末では os.read で 1 バイトずつ読む (Python 側の読み置きに溜めない. 溜まると # flush() で捨てられず, 送ったあとに余分な刻みが出る) import os import select b = os.read(self.fd, 1) if b == b"\x1b": # 矢印キーなど (ESC [ A ...) は丸ごと読み捨てる ('[' を刻みの切り替えと取り違えない) while select.select([self.fd], [], [], 0.02)[0]: os.read(self.fd, 8) return "\x1b" if not b: return "" # 入力の終わり return b.decode("utf-8", errors="ignore") or "\x00" # 日本語入力の途中のバイトなどは無視 def flush(self): if self.fd is not None: import termios termios.tcflush(self.fd, termios.TCIFLUSH) def __exit__(self, *exc): if self.fd is not None: import termios termios.tcsetattr(self.fd, termios.TCSADRAIN, self.old) return False def plan_correction_step(cur, joint, sign, step, q, lo, hi, max_move): """補正の 1 刻み. cur (今の補正目標) の joint を sign*step 動かし, 推論と同じ apply_limits (実測 q から) と check_send_safe を通す. 戻り値: (目標, notes, unsafe).""" want = np.asarray(cur, dtype=float)[:6].copy() want[joint] += sign * step t, notes = apply_limits(want, np.asarray(q, dtype=float)[:6], lo, hi, max_move) return t, notes, check_send_safe(t, lo, hi) REACH_TOL_DEG = 3.0 # 戻れたかの判定 (推論の到着確認 _send_and_wait_arrival(..., 3.0) と同じ) def reached_pose(qa, pose): """実測 qa が pose から REACH_TOL_DEG 以内か. 戻り値: (以内か, 最大ずれ or None).""" if qa is None: return False, None err = float(np.max(np.abs(np.asarray(qa, dtype=float)[:6] - np.asarray(pose, dtype=float)[:6]))) return err <= REACH_TOL_DEG, round(err, 2) def traverse(arm, waypoints, lo, hi, max_move, tag): """中継点を順に送る (巻き戻し・補正の取り消し). 1 点ごとに実測から apply_limits と check_send_safe をかけ直す (推論の中継点と同じ). 通らない点が出たらそこで止める (送らない). 戻り値: (最後まで行けたか, 1 点ごとの記録).""" out = [] for n, wp in enumerate(waypoints): qa = arm.read() if qa is None: print(" [WARN] 関節角を読めない. ここで止めます (送らない)") out.append({"n": n, "waypoint": _fmt6(wp), "sent": False, "why": "read_failed"}) return False, out t, notes = apply_limits(np.asarray(wp, dtype=float), qa[:6], lo, hi, max_move) unsafe = check_send_safe(t, lo, hi) e = {"n": n, "waypoint": _fmt6(wp), "target": _fmt6(t), "notes": notes, "measured_before": _fmt6(qa), "sent": False} for x in notes: print(f" [LIMIT] {tag} {n}: {x}") if unsafe: for u in unsafe: print(f" [ABORT] {tag} {n}: {u}") e["unsafe"] = unsafe out.append(e) return False, out if np.max(np.abs(t - qa[:6])) <= 0.5: # 推論の中継点と同じ「0.5° 以下は送らない」 out.append(e) continue ok = arm.send(t, f"{tag}_{n:02d}") e.update(sent=True, arrived=bool(ok)) print(f" [BACK ] {n + 1}/{len(waypoints)} → " + " ".join(f"{v:+.1f}" for v in t) + (" 着いた" if ok else " 未到達")) if not ok: print(" [WARN] 未到達. j4 の発熱などを確認してください") out.append(e) return True, out def run_rewind(entry, arm, lo, hi, max_move, recorder, grasp=None): """history の 1 件 (推論 1 回分 / 補正 1 回分) を巻き戻す. 戻り値は log.jsonl に足す記録. grasp は つかんだ区間 (グリッパーを閉じた推論) を巻き戻すときだけ渡す (依頼 27): GRASP_PLACED (y = 持っている): 送った目標を逆にたどって つかんだ位置 (最後に送った目標) まで 下ろす → 開いてブロックを置く → 残りを逆にたどって始めの姿勢へ. つかんだ位置まで下ろせなかったら開かない (宙で落とさない. 補正の取り消しと同じ決まり) GRASP_DROPPED (n = 落とした): 今の位置ですぐ開く (閉じた指が置き直したブロックに当たらないよう) → 逆にたどって始めの姿勢へ 送り方・範囲チェックは grasp なしの巻き戻しと同じ traverse.""" t_start = time.time() rec = {"type": "rewind", "rewound": True, "kind": entry["kind"], "chunk": entry.get("chunk"), "correction_id": entry.get("correction_id"), "segment_t": [entry["t_start"], entry["t_end"]], "start_pose": _fmt6(entry["start_pose"]), "sent_targets": [_fmt6(x) for x in entry["sent_targets"]], "t_start": t_start} if grasp is not None: rec.update(grasp_undo=grasp, gripper_opened=False) q0 = arm.read() if q0 is None: print(" [WARN] 関節角を読めないので巻き戻せません") rec.update(finished=False, why="read_failed", t_end=time.time(), waypoints=[]) return rec sent = entry["sent_targets"] name = ('推論 ' + str(entry['chunk']) if entry['kind'] == 'chunk' else '補正 ' + str(entry['correction_id'])) if recorder is not None: recorder.set_mode(MODE_REWIND) opened = False if grasp == GRASP_PLACED: # つかんだ位置 = グリッパーを閉じた時点の姿勢 (閉じる前に最後に送った目標. 送っていなければ始めの姿勢) grasp_pose = np.asarray(sent[-1] if sent else entry["start_pose"], dtype=float)[:6] down = split_leg(q0[:6], grasp_pose, max_move) back = rewind_waypoints(sent, entry["start_pose"], current=grasp_pose, max_move=max_move) print(f"[REWIND] {name} (つかんだ区間) を巻き戻す: つかんだ位置 " + " ".join(f"{v:+.1f}" for v in grasp_pose) + " まで下ろす → 開いて置く → " f"中継点 {len(back)} 点 → 始めの姿勢 " + " ".join(f"{v:+.1f}" for v in np.asarray(entry["start_pose"])[:6])) completed, pts = traverse(arm, down, lo, hi, max_move, "pi05_rewind_down") for e in pts: e["leg"] = "down" qg = arm.read() reached_g, err_g = reached_pose(qg, grasp_pose) rec.update(grasp_pose=_fmt6(grasp_pose), reached_grasp=reached_g, grasp_err_deg=err_g) if completed and reached_g: print("[REWIND] つかんだ位置に着いたのでグリッパーを開いてブロックを置きます") arm.gripper(0) opened = True completed, pts2 = traverse(arm, back, lo, hi, max_move, "pi05_rewind") for e in pts2: e["leg"] = "back" pts += pts2 else: completed = False print("[REWIND] [WARN] つかんだ位置まで下ろせていないので, グリッパーは閉じたままにします (落とさない)") else: if grasp == GRASP_DROPPED: print(f"[REWIND] {name} (つかんだ区間) を巻き戻す: 落としたので先にグリッパーを開きます") arm.gripper(0) opened = True wps = rewind_waypoints(sent, entry["start_pose"], current=q0[:6], max_move=max_move) print(f"[REWIND] {name} を巻き戻す: 中継点 {len(wps)} 点 → 始めの姿勢 " + " ".join(f"{v:+.1f}" for v in np.asarray(entry["start_pose"])[:6])) completed, pts = traverse(arm, wps, lo, hi, max_move, "pi05_rewind") if recorder is not None: recorder.set_mode(MODE_MODEL) qa = arm.read() reached, err = reached_pose(qa, entry["start_pose"]) rec.update(finished=bool(completed and reached and (grasp is None or opened)), traverse_completed=completed, reached_start=reached, start_err_deg=err, waypoints=pts, t_end=time.time(), measured_after=_fmt6(qa)) if grasp is not None: rec["gripper_opened"] = opened if not completed: print("[REWIND] 途中で止めました (送れない中継点). h で HOME に戻せます") elif not reached: print(f"[REWIND] [WARN] 始めの姿勢まで戻りきっていません (最大ずれ " f"{'不明' if err is None else f'{err:.2f}°'} > {REACH_TOL_DEG:.0f}°). " "この run は補正エピソードに使いません") return rec def run_correction(arm, keys, lo, hi, max_move, grip, corr_id, chunk, log_f, recorder): """補正モード. 戻り値: (結果 dict, 補正後のグリッパー状態). 結果 dict の action は accept (この姿勢から推論に戻る) / cancel (取り消して戻った).""" start_q = arm.read() if start_q is None: print("[CORR ] 関節角を読めないので補正に入れません") return None, grip start_q = start_q[:6].copy() start_grip = grip cur = start_q.copy() joint, si = 0, CORRECTION_DEFAULT_STEP_INDEX t_start = time.time() sent, n_steps = [], 0 resync = False # 直前の 1 刻みが未到達 → 次の刻みは実測から数える grip_used = False # 補正中に一度でもグリッパーを動かしたか (往復でも True. codex R1-2) if recorder is not None: recorder.set_mode(MODE_CORRECT) print(f"[CORR ] 補正 {corr_id} に入りました (推論 {chunk} のあと)") print(CORRECTION_HELP) def _status(): print(f" [補正] 関節 j{joint} 刻み {CORRECTION_STEPS[si]:.1f}° 補正目標 " + " ".join(f"j{k}={v:+.1f}" for k, v in enumerate(cur)) + f" グリッパー {'閉' if grip == 1 else '開'}") _status() while True: ch = keys.get() act = parse_correction_key(ch) if act is None: continue kind, val = act if kind == "help": print(CORRECTION_HELP); _status() elif kind == "select": joint = val; _status() elif kind == "step": si = next_step_index(si, val); _status() elif kind == "move": qa = arm.read() if qa is None: print(" [補正] 関節角を読めない. この 1 刻みは送りません") keys.flush(); continue if resync: cur = qa[:6].copy() resync = False step = CORRECTION_STEPS[si] t, notes, unsafe = plan_correction_step(cur, joint, val, step, qa, lo, hi, max_move) e = {"type": "correction", "action": "move", "correction_id": corr_id, "after_chunk": chunk, "joint": joint, "step_deg": step, "sign": val, "t": time.time(), "before": _fmt6(cur), "after": _fmt6(t), "measured_before": _fmt6(qa), "notes": notes, "sent": False} for x in notes: print(f" [LIMIT] {x}") if unsafe: for u in unsafe: print(f" [補正] 送りません: {u}") e["unsafe"] = unsafe elif np.max(np.abs(t - cur)) <= 1e-6: print(f" [補正] j{joint} は範囲の端なのでこれ以上動かせません") else: ok = arm.send(t, f"pi05_corr{corr_id:02d}_{n_steps:03d}") e.update(sent=True, arrived=bool(ok), measured_after=_fmt6(arm.read())) e["delta"] = round(float(t[joint] - cur[joint]), 3) cur = t sent.append(t.copy()) n_steps += 1 if not ok: print(" [WARN] 未到達. j4 の発熱などを確認してください") # codex R1/R2 A3: 次の 1 刻みは届いた所 (次の move で読む実測) から数える resync = True _status() log_f.write(json.dumps(e) + "\n"); log_f.flush() keys.flush() # 送っている間にキーリピートで溜まった分は捨てる (送りすぎない) elif kind == "grip": new = 0 if grip == 1 else 1 arm.gripper(new) log_f.write(json.dumps({"type": "correction", "action": "gripper", "correction_id": corr_id, "after_chunk": chunk, "t": time.time(), "before": grip, "after": new}) + "\n"); log_f.flush() grip = new grip_used = True _status() keys.flush() elif kind == "accept": res = {"type": "correction", "action": "accept", "correction_id": corr_id, "after_chunk": chunk, "t_start": t_start, "t_end": time.time(), "start_pose": _fmt6(start_q), "final_target": _fmt6(cur), "measured_after": _fmt6(arm.read()), "steps_sent": n_steps, "grip_before": start_grip, "grip_after": grip, "grip_used": grip_used, "sent_targets": [_fmt6(x) for x in sent]} if recorder is not None: recorder.set_mode(MODE_MODEL) print(f"[CORR ] 補正 {corr_id} を終えました ({n_steps} 刻み). この姿勢から推論に戻ります") return res, grip elif kind == "cancel": print(f"[CORR ] 補正 {corr_id} を取り消して, 始めの姿勢へ戻ります") if recorder is not None: recorder.set_mode(MODE_REWIND) q0 = arm.read() completed, pts = False, [] if q0 is not None: wps = rewind_waypoints(sent, start_q, current=q0[:6], max_move=max_move) if sent else [] completed, pts = traverse(arm, wps, lo, hi, max_move, "pi05_corr_cancel") else: print(" [WARN] 関節角を読めないので戻せません") qa = arm.read() reached, err = reached_pose(qa, start_q) if grip != start_grip: if completed and reached: arm.gripper(start_grip) grip = start_grip else: # codex R1 C3: 姿勢が戻っていないのにグリッパーだけ戻さない (物を落とさない) print(" [WARN] 始めの姿勢へ戻れていないので, グリッパーは今のままにします") if recorder is not None: recorder.set_mode(MODE_MODEL) res = {"type": "correction", "action": "cancel", "correction_id": corr_id, "after_chunk": chunk, "t_start": t_start, "t_end": time.time(), "start_pose": _fmt6(start_q), "steps_sent": n_steps, "finished": bool(completed and reached and grip == start_grip), "traverse_completed": completed, "reached_start": reached, "start_err_deg": err, "waypoints": pts, "measured_after": _fmt6(qa), "grip_after": grip, "grip_used": grip_used} return res, grip def write_trial_json(run_dir, run_stem, args, recorded, end_reason, outcome, history_log, n_rewinds, n_corrections, exit_code): """{run_stem}.json. お手本の試行 JSON と同じキー (recording_enabled / result / judgments) を持つ. 結果 y なら pick / place とも human_verdict=yes (build_demo_episodes の採否と同じ読み方).""" verdict = {"y": "yes", "n": "no"}.get(outcome) result = ("completed" if end_reason in ("h", "q", "max_chunks") else "interrupted" if end_reason == "ctrl_c" else "aborted") j = {"script": "run_policy_client.py", "source": "pi05_policy", "run_stem": run_stem, "run_dir": str(run_dir), "recording_enabled": bool(recorded), "end_reason": end_reason, "result": result, "exit_code": exit_code, "outcome": {"y": "success", "n": "failure"}.get(outcome), "rewinds": n_rewinds, "corrections": n_corrections, "history_left": history_log, "args": {k: (str(v) if isinstance(v, Path) else v) for k, v in vars(args).items()}, "ended_at": datetime.now().isoformat()} if verdict is not None: j["judgments"] = {"pick": {"human_verdict": verdict, "verdict": verdict, "source": "human"}, "place": {"human_verdict": verdict, "verdict": verdict, "source": "human"}} (Path(run_dir) / f"{run_stem}.json").write_text(json.dumps(j, indent=1, ensure_ascii=False)) def ask_outcome(): while True: a = input(" 結果: y=成功 / n=失敗 / Enter=記録しない > ").strip().lower() if a in ("y", "n", ""): return a or None 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 で送り直さない = 従来の動き") ap.add_argument("--dry-keys", action="store_true", help="ドライランでも 1 回ごとに止まり, b (巻き戻し) / c (補正) の流れを試す. " "アームにも daemon にも触らず, HOME から始まる仮の姿勢を動かす") args = ap.parse_args() if args.dry_keys and (args.execute or args.auto): sys.exit("[ERROR] --dry-keys はドライラン用 (--execute / --auto と一緒には使えない)") 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 用) recorder = None # --execute のときの 1 秒 10 コマの記録 (policy_recorder.py) arm = None # 巻き戻し・補正の送り口 (ArmIO) history = [] # 巻き戻せる区間 (推論 1 回分 / 補正 1 回分). 新しいものが後ろ n_rewinds = n_corrections = 0 end_reason, outcome = None, None 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 が指定されたので続けます") # ---- ここから先でアームに触る (写り方の確認を通ったあと) ---- if not args.dry_keys: # --dry-keys は daemon に触らない (アームなしで試す) 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}) if args.execute: # 1 秒 10 コマの記録 (上カメラ + 実測角). ここから先, 上カメラは記録スレッドだけが読む install_daemon_lock() recorder = PolicyRecorder(log_dir, cap, read_angles_once, frame_to_model) recorder.start() print(f"[RECORD] 上カメラと実測角を 1 秒 10 コマで記録します → {log_dir / 'recording'}") arm = ArmIO(args.execute, recorder, sim_q=(HOME if args.dry_keys else None)) if args.dry_keys: print("[DRY ] --dry-keys: アームには送らず, HOME から始まる仮の姿勢で流れを試します") q = arm.read() if args.dry_keys else 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 ] 開始時のグリッパー開放は, 最初の目標が検査を通ってから行います " "(爪が閉じていたら手で開けておく)") end_reason = "max_chunks" for i in range(args.max_chunks): q = arm.read() if args.dry_keys else read_angles() if q is None: print("[ERROR] 関節角を読めない (daemon を確認). 停止"); exit_code = 1; end_reason = "error"; break rgb = bgr_small = None for _ in range(10): rgb, bgr_small = grab_rgb(cap) if recorder is None else recorder.grab() 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 or args.dry_keys): # ここから先で送れなかった経路 (s / 送信なし / 中断) は, 次のチャンクで # 実測を渡すために None のままにする (--state-from-target 用). last_sent_target = None grasp = None # つかんだ区間を巻き戻すときの答え (GRASP_PLACED / GRASP_DROPPED) if not args.auto: # 入力を待っている間は wait (人が手でブロックを置き直す. 学習データに使わない. 依頼 27) if recorder is not None: recorder.set_mode(MODE_WAIT) while True: grasp = None ans = input(" Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / " "h=HOMEに戻して終了 / q=終了 > ").strip().lower() if ans != "b": break # 巻き戻せない場合は断って聞き直す (アームは動かさない) if not history: print(" [REWIND] 巻き戻せる区間がありません (まだ何も実行していない)") continue if history[-1].get("blocked"): print(" [REWIND] 直前の巻き戻し / 補正の取り消しが始めの姿勢まで戻りきれなかったので, " "これより前へは巻き戻しません (h で HOME に戻せます)") continue if history[-1]["grip_changed"]: h_last = history[-1] if h_last["kind"] == "chunk" and h_last.get("grip_cmd") == 1: # つかんだ区間 (依頼 27): 持っているか落としたかを 1 文字で聞く a = input(f" [REWIND] 推論 {h_last['chunk']} はつかんだ区間です. " "y=持っている（下ろして置いてから戻る） / " "n=落とした（先に開いてから戻る） / その他=やめる > ").strip().lower() if a == "y": grasp = GRASP_PLACED elif a == "n": grasp = GRASP_DROPPED else: print(" [REWIND] やめました (アームは動かしていません)") continue break if h_last["kind"] == "chunk" and h_last.get("grip_cmd") == 0: print(f" [REWIND] 直前の区間 (推論 {h_last['chunk']}) でグリッパーを開いた" "ので巻き戻しません. ブロックを置き直すなら h で終えてやり直してください") continue print(f" [REWIND] 直前の区間 ({'推論 ' + str(h_last['chunk']) if h_last['kind'] == 'chunk' else '補正 ' + str(h_last['correction_id'])})" " でグリッパーを動かしたので巻き戻しません") continue break if recorder is not None and ans not in ("q", "h"): recorder.set_mode(MODE_MODEL) if ans == "q": end_reason = "q" log_f.write(json.dumps(rec) + "\n"); log_f.flush() if recorder is not None: recorder.stop() outcome = ask_outcome() break if ans == "h": end_reason = "h" if recorder is not None: recorder.set_mode(MODE_HOME) if args.dry_keys: print(" [DRY ] HOME へは送らない (ドライラン)") else: 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"); log_f.flush() if recorder is not None: recorder.stop() outcome = ask_outcome() break if ans == "s": log_f.write(json.dumps(rec) + "\n"); continue if ans == "b": # 直前の 1 回分を巻き戻して, 戻った姿勢からもう一度推論する rec["rewind_requested"] = True log_f.write(json.dumps(rec) + "\n"); log_f.flush() entry = history.pop() rw = run_rewind(entry, arm, lo, hi, args.max_move_deg, recorder, grasp=grasp) rw["after_chunk"] = i n_rewinds += 1 if rw.get("gripper_opened"): grip = 0 if grasp == GRASP_DROPPED: rw["start_frame"] = str(log_dir / "frames" / f"{entry['chunk']:04d}.jpg") if not rw["finished"]: # codex R2-2: 戻りきれなかったら境界を積む (次の b で古い区間の経路へ飛ばない) history.append({"kind": "blocked", "blocked": True, "chunk": entry.get("chunk"), "correction_id": entry.get("correction_id"), "grip_changed": False, "t_start": rw["t_start"], "t_end": rw["t_end"]}) log_f.write(json.dumps(rw) + "\n"); log_f.flush() if grasp == GRASP_DROPPED and rw.get("gripper_opened"): # 落としたブロックを人が元の位置へ置き直す間は wait (手が写る) print(f" [REWIND] 元の位置の目安 (推論 {entry['chunk']} の始めの写真): " f"{rw['start_frame']}") if recorder is not None: recorder.set_mode(MODE_WAIT) input(" ブロックを元の位置へ置き直したら Enter で続ける > ") if recorder is not None: recorder.set_mode(MODE_MODEL) continue if ans == "c": rec["correction_requested"] = True log_f.write(json.dumps(rec) + "\n"); log_f.flush() n_corrections += 1 with KeyReader() as keys: cr, grip = run_correction(arm, keys, lo, hi, args.max_move_deg, grip, n_corrections, i, log_f, recorder) if cr is not None: log_f.write(json.dumps(cr) + "\n"); log_f.flush() if cr["grip_used"]: # codex R2-1: 補正でグリッパーを動かしたら, 開始時の開放は済んだことにする # (このあと最初の推論の実行で勝手に開いて物を落とさない) grip_opened = True if cr["action"] == "cancel" and not cr["finished"]: history.append({"kind": "blocked", "blocked": True, "chunk": None, "correction_id": n_corrections, "grip_changed": False, "t_start": cr["t_start"], "t_end": cr["t_end"]}) if cr["action"] == "accept" and (cr["steps_sent"] or cr["grip_used"]): history.append({"kind": "correction", "correction_id": n_corrections, "chunk": None, "start_pose": np.asarray(cr["start_pose"], dtype=float), "sent_targets": [np.asarray(x, dtype=float) for x in cr["sent_targets"]], "grip_changed": cr["grip_used"], "t_start": cr["t_start"], "t_end": cr["t_end"]}) continue if not unsafe and args.dry_keys: # --dry-keys の Enter: 送らずに仮の姿勢を目標へ移す (b / c の流れを試すため) t_ex0 = time.time() sent = [np.asarray(v["target"], dtype=float).copy() for v in plan] arm.send(sent[-1], f"pi05_chunk{i:03d}") if grip_cmd is not None: arm.gripper(grip_cmd) grip = grip_cmd history.append({"kind": "chunk", "chunk": i, "correction_id": None, "start_pose": q[:6].copy(), "sent_targets": sent, "grip_changed": grip_cmd is not None, "grip_cmd": grip_cmd, "t_start": t_ex0, "t_end": time.time()}) rec["simulated"] = True if not unsafe and args.execute: t_exec_start = time.time() sent_targets = [] # 実際に送った目標 (送った順. 巻き戻しでこれを逆にたどる) if not grip_opened: # 開始時の開放はここで 1 度だけ (写り方の確認と目標の検査を通り, 送ると決めてから) print("[GRIP ] 開始時にグリッパーを開く") if recorder is not None: recorder.gripper(0) 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 sent_targets.append(np.asarray(tgt, dtype=float).copy()) 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 recorder is not None: recorder.gripper(grip_cmd) 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 # 巻き戻し (b) 用に, この推論で実際に送った目標と始めの姿勢 (実測) を残す. # 途中で止まった (broke) 推論も, 1 点でも送っていれば残す (codex R1-3: # 残さないと次の b が 1 つ古い区間を巻き戻してしまう) t_exec_end = time.time() rec["t_exec_start"], rec["t_exec_end"] = t_exec_start, t_exec_end rec["sent_targets"] = [_fmt6(x) for x in sent_targets] grip_moved = (not broke) and grip_cmd is not None if sent_targets or grip_moved: history.append({"kind": "chunk", "chunk": i, "correction_id": None, "start_pose": q[:6].copy(), "sent_targets": sent_targets, "grip_changed": grip_moved, "grip_cmd": grip_cmd if grip_moved else None, "partial": bool(broke), "t_start": t_exec_start, "t_end": t_exec_end}) 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: if recorder is not None: recorder.set_mode(MODE_WAIT) # 入力待ちは wait (codex 依頼 27 P3) ans = input(" h=HOMEに戻して終了 / 他=そのまま終了 > ").strip().lower() if ans == "h": if recorder is not None: recorder.set_mode(MODE_HOME) 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") end_reason = "ctrl_c" finally: # finally ではアームを動かさない (HOME 復帰もサーボ解放もしない). 止まった姿勢は # そのまま保つ: 未知の姿勢から勝手に動かす方が危ない. 戻すときは h で明示的に送る. if recorder is not None: recorder.stop() try: write_trial_json(log_dir, run_stem, args, bool(recorder.stopped_cleanly), end_reason if not (exit_code and end_reason in (None, "max_chunks")) else "error", outcome, [{k: v for k, v in h_.items() if k in ("kind", "chunk", "correction_id", "grip_changed")} for h_ in history], n_rewinds, n_corrections, exit_code) except Exception as e: print(f"[WARN ] {run_stem}.json を書けない: {e}") 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 and not (recorder is not None and recorder.alive()): 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())