commit 68f514a41c129b6fbd2663435a366734e3d374e7 Author: Eiichi Shimazu Date: Sat Nov 14 19:15:55 2026 +0900 実機推論で, 箱に置き終えたら自動で終わる (依頼 181・Log35). run_policy_client.py に --auto-finish を足す (つかんで放した (補正の中の g も数える. codex R2) あと, 爪が開いていて, 推論の前の実測角と HOME の差が各関節 --finish-home-tol (既定 8°) 以内, 答えの先頭 10 ステップ (codex R1) と実測角の差が各関節 --finish-still-deg (既定 3°) 以内のチャンクが --finish-chunks (既定 2) 回続いたら, そのチャンクは送らずに h と同じ送り方で HOME へ戻し結果の入力へ. 画面に [FINISH], log.jsonl に type=finish (event=auto_finish), チャンクの行に auto_finish・finish_check, {run_stem}.json の end_reason=auto_finish. 放したあとにまた閉じる答え (2 回目のつかみ) は送らずに入力欄 (type=finish / event=regrasp. --auto なら送らずに終了コード 1). 止める予約・s・b・c・p のあとは入力欄を優先 (確かめの形によらない. codex 2 回目 R1). 付けなければ log の頭も含めて今までと同じ (codex R3)). 既定は Log35 の 7 本で決めた (HOME に戻って止まったチャンクは差 4.1〜5.5°・Δ 1.1〜2.7°. 180457→17・181300→20・182039→15・182327→17 (2 回目のつかみ 51 より前)・180704/181057/182211 は記録の終わりまでに終わらない). lab_start.sh は既定で --auto-finish を渡し, --no-auto-finish で今までどおり (起動時に [FINISH] の行. --help は 2〜64 行). chunk_timing.py に終わり方 (自動・人・上限・2 回目のつかみ) とメモ. テストを足す (test_auto_finish 17・lab_start 222) (依頼 181. codex 3 回) Co-Authored-By: Claude Opus 5.5 --- lab_start.sh | 16 +- pi05/chunk_timing.py | 56 +++++- pi05/run_policy_client.py | 186 ++++++++++++++++++- pi05/test_auto_finish.py | 464 ++++++++++++++++++++++++++++++++++++++++++++++ test_lab_start.sh | 14 +- 5 files changed, 723 insertions(+), 13 deletions(-) diff --git a/lab_start.sh b/lab_start.sh index 9dfafaf..18b1cb0 100755 --- a/lab_start.sh +++ b/lab_start.sh @@ -41,6 +41,9 @@ # --step-confirm 実機推論でチャンクごとに確かめ (Enter) を待つ今までの形に戻す. 付けなければ (既定) run_policy_client.py に # --no-step-confirm を渡し, 始めの確かめのあとは止まらずに進み, スペース (Enter も可) を押したときだけ次の推論の # あとで入力欄を出す (依頼 178). 起動時に [CONFIRM] の行でどちらかを出す +# --no-auto-finish 箱に置き終えたら自動で終わる判定 (run_policy_client.py --auto-finish) を渡さない (今までどおり). 付けなければ +# (既定) --auto-finish を渡し, つかんで放したあと HOME 近くで止まったら送らずに HOME へ戻して結果の入力へ進む. +# 2 回目のつかみは送らずに入力欄を出す (依頼 181). 起動時に [FINISH] の行でどちらかを出す # つなぎ直しで, 残っている Pod のサーバーが別のモデルで動いていれば, そのことを出して止まる # # 1. Pod を作る (A40×1・Runpod Pytorch 2.8.0・コンテナのディスク 200GB・22/tcp を外へ出す) @@ -92,6 +95,7 @@ GRIP_ARGS=() # 依頼 152: --grip-value N のときだけ run_policy_cli GRIP_VALUE_SHOWN="20 (既定. run_policy_client.py の GRIP_VALUE)" FLOW_ARGS=() # 依頼 176: --flow / --speed N のときだけ run_policy_client.py に渡す STEP_ARGS=(--no-step-confirm) # 依頼 178: 既定はチャンクごとに確かめない. --step-confirm で今までどおり +FINISH_ARGS=(--auto-finish) # 依頼 181: 既定は置き終えたら自動で終わる. --no-auto-finish で今までどおり while [ $# -gt 0 ]; do case "$1" in --gpu) lab_need_val A40 "$@"; GPU="$2"; shift 2 ;; @@ -107,13 +111,14 @@ while [ $# -gt 0 ]; do GRIP_ARGS=(--grip-value "$((10#$2))"); GRIP_VALUE_SHOWN="$((10#$2)) (--grip-value)"; shift 2 ;; --flow) FLOW_ARGS+=(--flow); shift ;; --step-confirm) STEP_ARGS=(); shift ;; + --no-auto-finish) FINISH_ARGS=(); shift ;; --speed) [ $# -ge 2 ] || { echo "[ERROR] --speed には 1〜100 の整数を付ける" >&2; exit 2; } case "$2" in ''|*[!0-9]*) echo "[ERROR] --speed は 1〜100 の整数: $2" >&2; exit 2 ;; esac [ ${#2} -le 3 ] && [ "$((10#$2))" -ge 1 ] && [ "$((10#$2))" -le 100 ] || { echo "[ERROR] --speed は 1〜100 の整数: $2" >&2; exit 2; } FLOW_ARGS+=(--speed "$((10#$2))"); shift 2 ;; --model) [ $# -ge 2 ] || { echo "[ERROR] --model には ${LAB_MODELS// / か } を付ける" >&2; exit 2; } # 値がないと shift 2 が失敗して回り続ける (codex R1) MODEL="$2"; shift 2 ;; - -h|--help) sed -n '2,61p' "$0"; exit 0 ;; + -h|--help) sed -n '2,64p' "$0"; exit 0 ;; *) echo "[ERROR] 知らない引数: $1 (./lab_start.sh --help)" >&2; exit 2 ;; esac done @@ -168,6 +173,11 @@ if [ ${#STEP_ARGS[@]} = 0 ]; then else echo "[CONFIRM] チャンクごとには確かめない (--no-step-confirm. 始めの確かめのあとは止まらずに進む. 止めるときはスペース. 依頼 178)" fi +if [ ${#FINISH_ARGS[@]} = 0 ]; then + echo "[FINISH] 置き終えても自動では終わらない (--no-auto-finish. 今までどおり)" +else + echo "[FINISH] 箱に置き終えたら自動で終わる (--auto-finish. つかんで放したあと HOME 近くで止まったら HOME へ戻して結果の入力へ. 2 回目のつかみは送らずに止める. 依頼 181)" +fi lab_ssh_opts HF_REPO=$MODEL_REPO # --model で選んだモデル (lab_lib.sh の lab_model_def) @@ -541,14 +551,14 @@ while :; do stage "8. 実機推論 ($round 回目)" python3 pi05/run_policy_client.py --raw-dir "$RAW_RANGE" --steps-per-chunk 10 --via-stride 3 \ --settle-retries 0 --state-from-target --execute --ignore-framing --model "$MODEL_TAG" --framing-raw-dir "$RAW" \ - --tilt-raw-dir "$RAW_RANGE" ${TILT_FIX_ARGS[@]+"${TILT_FIX_ARGS[@]}"} ${GRIP_ARGS[@]+"${GRIP_ARGS[@]}"} ${STEP_ARGS[@]+"${STEP_ARGS[@]}"} ${FLOW_ARGS[@]+"${FLOW_ARGS[@]}"} \ + --tilt-raw-dir "$RAW_RANGE" ${TILT_FIX_ARGS[@]+"${TILT_FIX_ARGS[@]}"} ${GRIP_ARGS[@]+"${GRIP_ARGS[@]}"} ${FINISH_ARGS[@]+"${FINISH_ARGS[@]}"} ${STEP_ARGS[@]+"${STEP_ARGS[@]}"} ${FLOW_ARGS[@]+"${FLOW_ARGS[@]}"} \ ${CONFIRM_ARGS[@]+"${CONFIRM_ARGS[@]}"} rc=$? else stage "8. 推論のクライアント (ドライラン・--dry-keys. アームには触らない)" python3 pi05/run_policy_client.py --raw-dir "$RAW_RANGE" --steps-per-chunk 10 --via-stride 3 \ --settle-retries 0 --state-from-target --dry-keys --ignore-framing --model "$MODEL_TAG" --framing-raw-dir "$RAW" \ - --tilt-raw-dir "$RAW_RANGE" ${TILT_FIX_ARGS[@]+"${TILT_FIX_ARGS[@]}"} ${GRIP_ARGS[@]+"${GRIP_ARGS[@]}"} ${STEP_ARGS[@]+"${STEP_ARGS[@]}"} ${FLOW_ARGS[@]+"${FLOW_ARGS[@]}"} \ + --tilt-raw-dir "$RAW_RANGE" ${TILT_FIX_ARGS[@]+"${TILT_FIX_ARGS[@]}"} ${GRIP_ARGS[@]+"${GRIP_ARGS[@]}"} ${FINISH_ARGS[@]+"${FINISH_ARGS[@]}"} ${STEP_ARGS[@]+"${STEP_ARGS[@]}"} ${FLOW_ARGS[@]+"${FLOW_ARGS[@]}"} \ ${CONFIRM_ARGS[@]+"${CONFIRM_ARGS[@]}"} rc=$? fi diff --git a/pi05/chunk_timing.py b/pi05/chunk_timing.py index 0f98f90..d381e14 100644 --- a/pi05/chunk_timing.py +++ b/pi05/chunk_timing.py @@ -19,6 +19,9 @@ 確認待ちもここに入る) 合計 このチャンクの推論を始めてから次のチャンクの推論を始めるまで (最後のチャンクは記録の終わりまで) 試行全体の 合計 と 中央値 (推論は 2 回目からの中央値も) を出す. +終わり方 (依頼 181): 自動で終わった (--auto-finish で置き終えた) / 人が止めた (h・q・Ctrl-C) / 上限 (--max-chunks) / + 2 回目のつかみで止めた (--auto) / エラー を 1 行で出す. {試行名}.json の end_reason を読み, なければ log.jsonl の + type=finish の行と最後のチャンクの HOME から見る. チャンクのメモに「自動で終えた」「2回目のつかみ」 中継点ごと: --flow の試行 log.jsonl の vias[].flow から, 送ってから次を実際に送るまで (send_gap_s. 最後の点は着くまで) の秒と @@ -120,6 +123,10 @@ def chunk_rows(chunks): note.append("止めた(" + ({"space": "スペース", "enter": "Enter"}.get(p.get("key")) or {"not_arrived": "未到達", "read_failed": "読めない"}.get(p.get("reason"), str(p.get("reason")))) + ")") + if r.get("auto_finish"): + note.append("自動で終えた") # 依頼 181: --auto-finish で置き終えたと判定し, 送らずに HOME へ + if r.get("regrasp_stop"): + note.append("2回目のつかみ") # 依頼 181: 放したあとにまた閉じる答え (送らずに入力欄) for k, t in (("rewind_requested", "巻き戻し"), ("correction_requested", "補正"), ("home_requested", "HOME"), ("unsafe", "安全停止")): if r.get(k): @@ -163,6 +170,46 @@ def step_text(model): return "毎チャンク(記録なし)" if sc is None else ("毎チャンク" if sc else "止めたときだけ") +END_TEXT = {"auto_finish": "自動で終わった (置き終えた)", "h": "人が止めた (h で HOME)", "q": "人が止めた (q)", + "ctrl_c": "人が止めた (Ctrl-C)", "max_chunks": "上限 (--max-chunks)", + "regrasp": "2 回目のつかみで止めた (--auto)", "error": "エラーで止まった"} + +END_SHORT = {"auto_finish": "自動", "h": "人(h)", "q": "人(q)", "ctrl_c": "人(Ctrl-C)", "max_chunks": "上限", + "regrasp": "2回目のつかみ", "error": "エラー"} + + +def end_how(run_dir: Path, chunks): + """終わり方 (依頼 181) → (end_reason か None, 1 行の文). {試行名}.json の end_reason を先に見る.""" + reason = None + try: + reason = json.loads((run_dir / f"{run_dir.name}.json").read_text()).get("end_reason") + except (OSError, ValueError, AttributeError): + pass + fins = [] + for line in (run_dir / "log.jsonl").read_text().splitlines(): + try: + r = json.loads(line) + except json.JSONDecodeError: + continue + if isinstance(r, dict) and r.get("type") == "finish": + fins.append(r) + auto = [f for f in fins if f.get("event") == "auto_finish"] + if reason is None: + if auto: + reason = "auto_finish" + elif chunks and chunks[-1].get("home_requested"): + reason = "h" + text = END_TEXT.get(reason, "記録なし" if reason is None else str(reason)) + if reason == "auto_finish" and auto: + f = auto[-1] + text += (f" 推論 {f.get('chunk')} で HOME へ (HOME からの差 最大 {max(f.get('home_err') or [0]):.1f}°・" + f"Δ 最大 {max(f.get('delta_max') or [0]):.1f}°・{f.get('still_run')} 回続いた)") + n_re = sum(1 for f in fins if f.get("event") == "regrasp") + if n_re: + text += f" (2 回目のつかみで止めた {n_re} 回)" + return reason, text + + def fmt(x, w=7): return f"{'-':>{w}}" if x is None else f"{x:>{w}.2f}" @@ -212,10 +259,13 @@ def report(run_dir: Path): f"中継点の間の中央値 {fmt(med(all_waits), 0)} 秒 ({'log.jsonl の flow' if flow_seen else 'states.csv の送った時刻' if all_waits else '記録なし'})") if flow_seen: print(" 中継点ごと: step:秒+わけ (近=近づいた 時=見込みの時間 着=最後の点が着いた 未=着かなかった 失=送れなかった)") + end_reason, end_text = end_how(run_dir, chunks) + print(f" 終わり方: {end_text}") if others: print(" そのほかの行: " + " ".join(f"{k} {v}" for k, v in sorted(others.items()))) return {"name": run_dir.name, "flow": model.get("flow"), "step": step_text(model) if model else "-", "speed": model.get("speed", 20), "n": len(rows), - "tot": tot, "mid": mid, "infer1": rows[0]["infer"], "infer2": infer2, "via": med(all_waits)} + "tot": tot, "mid": mid, "infer1": rows[0]["infer"], "infer2": infer2, "via": med(all_waits), + "end": end_reason} def main(argv=None): @@ -236,12 +286,12 @@ def main(argv=None): if len(res) >= 2: print("== 比べる (秒)") print(f" {'試行':<22} {'送り方':<10} {'確かめ':<12} {'速さ':>4} {'チャンク':>6} {'推論計':>7} {'確認待ち計':>7} {'動いた計':>7} " - f"{'動いた中央':>7} {'中継点中央':>7} {'合計':>7}") + f"{'動いた中央':>7} {'中継点中央':>7} {'合計':>7} 終わり方") for r in res: how = "記録なし" if r["flow"] is None else ("flow" if r["flow"] else "今まで") print(f" {r['name']:<22} {how:<10} {r['step']:<12} {r['speed']:>4} {r['n']:>6} {fmt(r['tot']['infer'])} " f"{fmt(r['tot']['confirm'])} {fmt(r['tot']['moved'])} {fmt(r['mid']['moved'])} {fmt(r['via'])} " - f"{fmt(r['tot']['total'])}") + f"{fmt(r['tot']['total'])} {END_SHORT.get(r['end'], '記録なし' if r['end'] is None else r['end'])}") return 0 diff --git a/pi05/run_policy_client.py b/pi05/run_policy_client.py index 41eff0c..f2db2c8 100644 --- a/pi05/run_policy_client.py +++ b/pi05/run_policy_client.py @@ -119,6 +119,20 @@ --speed N: 推論のチャンクを送る速さ (既定 20 = 今までと同じ. settle の送り直しも同じ速さ). 補正 (キー・ゲームパッド)・巻き戻し・HOME の送り方と速さは変えない. log.jsonl の頭 (type=model) に flow・flow_tol・speed +置き終えたら自動で終わる (2026-11-14 依頼 181. --auto-finish. 既定は off = 今までどおり. lab_start.sh が既定で付ける): + 推論の答えを送る前に, 次の 3 つをすべて満たしたら, そのチャンクは送らずに HOME へ戻し (h と同じ送り方) 結果の入力へ進む: + a. この試行の中で爪を閉じ, そのあと開いた (推論・補正のどちらでも. つかんだ区間を巻き戻して開いたら数え直す) + b. 推論の前の実測角と HOME の差が 6 関節すべて --finish-home-tol (既定 8°) 以内 + c. b の状態で, 答えの 10 ステップ先までと実測角の差が 6 関節すべて --finish-still-deg (既定 3°) 以内のチャンクが + --finish-chunks (既定 2) 回続いた (s・b・c・p を選んだら数え直す) + 既定は Log35 の 7 本の放したあとのチャンクで決めた (HOME に戻って止まっているチャンクは HOME からの差 4.1〜5.5°・Δ 1.1〜2.7°. + 運ぶ途中は HOME からの差 50° 以上. test_auto_finish.py --table). 画面に [FINISH] の 1 行, log.jsonl に type=finish + (event=auto_finish. 条件の値・何チャンク目・HOME からの差・Δ), そのチャンクの行に auto_finish, {run_stem}.json の + end_reason=auto_finish. 止める予約 (スペース) や s・b・c・p のあとで止めているときは, 終わらずに入力欄を出す. + 放したあとにまた閉じる答えが出たら (2 回目のつかみ. 182327 の空振り), そのチャンクは送らずに入力欄を出す + (画面に [FINISH]. log.jsonl に type=finish / event=regrasp. Enter で送るのは今までどおり人の判断. --auto なら送らずに + 終える (終了コード 1・end_reason=regrasp・アームは動かさない)). 各チャンクの行に finish_check (判定の値) + 待ち時間 (2026-10-30 依頼 35. エイイチの判断で縮めた. 本線と共有の定数は変えず, ここで上書きする): --send-wait 0.0 (前 1.0) / --grip-wait 0.8 (前 2.0) / --grip-open-wait 0.0 (前 2.0) / --settle-wait 0.8 (前 2.0) 前の値に戻すには --send-wait 1.0 --grip-wait 2.0 --grip-open-wait 2.0 --settle-wait 2.0 @@ -205,6 +219,7 @@ GRIP_VALUE_DEFAULT = 20 # run_pick_place.py の close 値 (--grip-value GRIP_VALUE = GRIP_VALUE_DEFAULT # 閉じる値 (--grip-value で変える. 依頼 152) GRIP_VALUE_RANGE = (0, 100) # --grip-value に受ける値 (set_gripper_value の範囲) LAST_CLOSE_VALUE = None # 最後に送った閉じる値 (締め直し r で違う値を選ぶため. 依頼 152) +GRIP_OBSERVER = None # 補正・巻き戻しの爪の指令を 1 つずつ知らせる先 (--auto-finish の AutoFinish.on_grip. 依頼 181 codex R2) # ---- 待ち時間 (2026-10-30 依頼 35. エイイチの判断で縮めた. 「time.sleep() を短縮しない」より優先) ---- # 本線 (j0_sweep_scan.py / visual_servo_pick.py) と共有の定数は変えず, この中でだけ上書きする. # コマンドの引数 (--send-wait / --grip-wait / --grip-open-wait / --settle-wait) で前の値に戻せる. @@ -235,6 +250,11 @@ FLOW_MAX_WAIT_SEC = 1.5 # 見込みの時間の上限 (近づかなく FLOW_START_SEC = 0.15 # 送ってから動き始めるまで (193254 で 0.13〜0.14 秒) FLOW_DEG_PER_SEC_AT_20 = 25.0 # speed 20 で動いている間の速さ (お手本 deep_even10_v1 の中央 25.6°/秒). speed に比例すると見込む FLOW_POLL_SEC = 0.1 # 待つ間に関節角を読む間隔 (読む 1 回に 0.1 秒ほどかかる) +# ---- 置き終えたら自動で終わる (--auto-finish. 依頼 181. Log35 の 7 本の放したあとのチャンクで決めた) ---- +FINISH_HOME_TOL_DEFAULT = 8.0 # HOME からの差 (各関節). 戻って止まっているチャンクは 4.1〜5.5° (j1). 運ぶ途中は 50° より大きい +FINISH_STILL_DEG_DEFAULT = 3.0 # 10 ステップ先までの Δ (各関節). HOME 近くの小さな動きは 1.1〜2.7°. 戻る途中は 3.1° 以上 (182327 の 15) +FINISH_CHUNKS_DEFAULT = 2 # 上の 2 つを満たすチャンクがこの回数続いたら終わる +FINISH_LOOK_STEPS = 10 # 答えの先頭この数のステップ (0.1 秒刻みで 1 秒先まで) だけを見る (codex R1) DATA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data") LOG_ROOT = DATA_ROOT / "runs" / "pi05_policy" RECORDER = ("127.0.0.1", 5556) @@ -668,6 +688,7 @@ class ArmIO: def gripper(self, state, value=None): """value は閉じるときの値 (省くと GRIP_VALUE. 締め直し r だけが渡す. 依頼 152).""" + notify_grip(state) if self.recorder is not None: self.recorder.gripper(state) if not self.execute: @@ -678,6 +699,12 @@ class ArmIO: gripper_cmd(state, value) +def notify_grip(state): + """爪の指令を GRIP_OBSERVER へ知らせる (--auto-finish のときだけ入っている. 依頼 181 codex R2).""" + if GRIP_OBSERVER is not None: + GRIP_OBSERVER(int(state)) + + def regrip_tighten(arm): """締め直し (r. 補正の中と外で共通): 開かずに, 前に送った閉じる値と違う値を送る (依頼 152). 戻り値: log.jsonl の regrip_tighten に足す項目 (送った値・閉じる値・前に送った値).""" @@ -797,6 +824,67 @@ class PauseKeys: return "other" if s else None +class AutoFinish: + """--auto-finish (依頼 181): 箱に置き終えたら推論を終える判定. アームには触らない (判定と数えるだけ). + + 終わる条件 (すべて): a. この試行の中で爪を閉じ, そのあと開いた (つかんで放した) / b. 推論の前の実測角と + HOME の差が 6 関節すべて home_tol 以内 / c. b の状態で, 推論の答え (10 ステップ先まで) と実測角の差が + 6 関節すべて still_deg 以内のチャンクが need 回続いた. + 放したあとにまた閉じる答え (2 回目のつかみ) が出たら regrasp (送らずに入力欄). + on_grip で爪の状態 (推論・補正で変わった後の値) を受け, on_rewind_open (つかんだ区間を巻き戻して開いた) で + つかんだこと自体を忘れる (ブロックは元の位置へ戻っている).""" + + def __init__(self, enabled, home_tol=FINISH_HOME_TOL_DEFAULT, still_deg=FINISH_STILL_DEG_DEFAULT, + need=FINISH_CHUNKS_DEFAULT): + self.enabled = bool(enabled) + self.home_tol, self.still_deg, self.need = float(home_tol), float(still_deg), int(need) + self.closed = self.released = False + self.grip = 0 # 今の爪 (知らされた最後の指令) + self.still_run = 0 + + def on_grip(self, grip): + self.grip = grip + if grip == 1: + self.closed = True + elif grip == 0 and self.closed: + self.released = True + + def on_rewind_open(self): + self.closed = self.released = False + self.grip = 0 + self.still_run = 0 + + def reset_still(self): + self.still_run = 0 + + def judge(self, q, acts, grip_cmd): + """推論 1 回分を見る. 戻り値 {action: None / "finish" / "regrasp", home_err, still, ...}.""" + q6 = np.asarray(q, dtype=float)[:6] + home_err = np.abs(q6 - np.asarray(HOME, dtype=float)) + still = np.max(np.abs(np.asarray(acts, dtype=float)[:FINISH_LOOK_STEPS, :6] - q6), axis=0) + near = bool(np.all(home_err <= self.home_tol)) + quiet = bool(np.all(still <= self.still_deg)) + info = {"released": self.released, "grip_open": self.grip == 0, "near_home": near, "still": quiet, + "home_err": [round(float(v), 2) for v in home_err], + "delta_max": [round(float(v), 2) for v in still]} + if self.released and grip_cmd == 1: + self.still_run = 0 + info.update(action="regrasp", still_run=0) + return info + # 爪が開いているときだけ (補正の R で開いて閉じ直したあと, 持ったまま HOME で止まっても終えない) + self.still_run = self.still_run + 1 if (self.released and self.grip == 0 and near and quiet) else 0 + info.update(still_run=self.still_run, + action="finish" if self.still_run >= self.need else None) + return info + + +def finish_text(info): + """[FINISH] の 1 行の中身 (HOME からの差と Δ の最大).""" + he, dm = info["home_err"], info["delta_max"] + return (f"HOME からの差 最大 {max(he):.1f}° (j{int(np.argmax(he))}) " + f"Δ (10 ステップ先まで) 最大 {max(dm):.1f}° (j{int(np.argmax(dm))})") + + 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).""" @@ -1179,6 +1267,7 @@ def run_pad_correction(arm, grip, corr_id, chunk, log_f, recorder, lo, hi): def gripper(self, state, gate): def _send(): + notify_grip(state) # 依頼 181 codex R2: 補正の中の閉→開も --auto-finish に数える if recorder is not None: recorder.gripper(state) if state == 1: @@ -1267,7 +1356,7 @@ def write_trial_json(run_dir, run_stem, args, recorded, end_reason, outcome, his """{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 + result = ("completed" if end_reason in ("h", "q", "max_chunks", "auto_finish") 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, @@ -1489,7 +1578,21 @@ def main(): "始めの確かめは今までどおり (既定 off = 1 チャンクごとに確かめる. 依頼 178)") ap.add_argument("--speed", type=int, default=SPEED, help=f"推論のチャンクを送る速さ (既定 {SPEED} = 今までと同じ. 補正・巻き戻し・HOME は {SPEED} のまま. 依頼 176)") + ap.add_argument("--auto-finish", action="store_true", + help="つかんで放したあと, HOME 近く (--finish-home-tol) で答えの Δ が小さい (--finish-still-deg) " + "チャンクが --finish-chunks 回続いたら, そのチャンクは送らずに HOME へ戻して終える. 放したあとに " + "また閉じる答えが出たら送らずに入力欄を出す (既定 off = 今までどおり. lab_start.sh が既定で付ける. 依頼 181)") + ap.add_argument("--finish-home-tol", type=float, default=FINISH_HOME_TOL_DEFAULT, + help=f"--auto-finish: HOME からの差 (各関節. 度. 既定 {FINISH_HOME_TOL_DEFAULT:.0f})") + ap.add_argument("--finish-still-deg", type=float, default=FINISH_STILL_DEG_DEFAULT, + help=f"--auto-finish: 10 ステップ先までの Δ (各関節. 度. 既定 {FINISH_STILL_DEG_DEFAULT:.0f})") + ap.add_argument("--finish-chunks", type=int, default=FINISH_CHUNKS_DEFAULT, + help=f"--auto-finish: 続ける回数 (既定 {FINISH_CHUNKS_DEFAULT})") args = ap.parse_args() + if args.auto_finish and not (np.isfinite(args.finish_home_tol) and args.finish_home_tol > 0 + and np.isfinite(args.finish_still_deg) and args.finish_still_deg > 0 + and args.finish_chunks >= 1): + sys.exit("[ERROR] --finish-home-tol と --finish-still-deg は 0 より大きい数, --finish-chunks は 1 以上で指定する") if args.dry_keys and (args.execute or args.auto): sys.exit("[ERROR] --dry-keys はドライラン用 (--execute / --auto と一緒には使えない)") if args.no_step_confirm and args.auto: @@ -1521,8 +1624,10 @@ def main(): "grip_value": GRIP_VALUE, "flow": bool(args.flow), "flow_tol": args.flow_tol, "speed": CHUNK_SPEED, "skip_first_confirm": bool(args.skip_first_confirm), "step_confirm": not args.no_step_confirm, - "host": args.host, "port": args.port, "t": time.time()}, - ensure_ascii=False) + "\n") + "host": args.host, "port": args.port, "t": time.time()} + | ({"auto_finish": {"home_tol": args.finish_home_tol, "still_deg": args.finish_still_deg, + "chunks": args.finish_chunks}} if args.auto_finish else {}), + ensure_ascii=False) + "\n") # auto_finish は付けたときだけ (付けなければ今までと同じ log. codex R3) log_f.flush() # 以降は try/finally の中だけで資源を触る (codex R3: 途中で終了しても cap / log_f を必ず閉じる). @@ -1544,6 +1649,16 @@ def main(): pause_req = None # 止める予約 (log.jsonl の type=pause / event=requested の行). Enter で進めば None hold_confirm = False # 入力欄で s・b・c・p を選んだ. 次の推論でも入力欄を出す (Enter で実行すれば False. codex R1) last_exec_chunk = None # 最後に送り始めた推論の番号 (止める予約の「どのチャンクのあとか」) + # 依頼 181: --auto-finish. つかんで放したあと HOME 近くで止まったら終える / 2 回目のつかみは送らずに入力欄 + finisher = AutoFinish(args.auto_finish, args.finish_home_tol, args.finish_still_deg, args.finish_chunks) + global GRIP_OBSERVER + GRIP_OBSERVER = finisher.on_grip if finisher.enabled else None # 補正・巻き戻しの爪の指令も 1 つずつ数える (codex R2) + regrasp_hold = False # 2 回目のつかみの答えが出た. このチャンクは入力欄を出す + finish_hold = False # 入力欄で s・b・c・p を選んだ (確かめの形によらず). Enter で実行するまで自動で終えない (codex 2 回目 R1) + if args.auto_finish: + print(f"[FINISH] --auto-finish: つかんで放したあと, HOME からの差が各関節 {args.finish_home_tol:g}° 以内で " + f"答えの Δ (10 ステップ先まで) が各関節 {args.finish_still_deg:g}° 以内のチャンクが " + f"{args.finish_chunks} 回続いたら, 送らずに HOME へ戻して終えます. 2 回目のつかみは送らずに止めます") if args.no_step_confirm: print("[PAUSE ] --no-step-confirm: 始めの確かめのあとは入力欄を出さずに進みます. 止めるときはスペース " "(Enter も可). 今のチャンクを送り終えてから, 次の推論のあと (送る前) で入力欄を出します") @@ -1760,10 +1875,61 @@ def main(): skip_now, skip_confirm = skip_confirm and not args.auto, False # 依頼 178: --no-step-confirm は始めの確かめと, 止める予約が入ったときだけ入力欄を出す check_pause(i, "infer") + # 依頼 181: --auto-finish. 送る前に, 置き終えたか・2 回目のつかみかを見る + regrasp_hold = False + fin = finisher.judge(q, acts, grip_cmd) if finisher.enabled else None + if fin is not None: + rec["finish_check"] = {"still_run": fin["still_run"], "near_home": fin["near_home"], + "still": fin["still"], "released": fin["released"], + "home_err_max": max(fin["home_err"]), "delta_max": max(fin["delta_max"])} + fe = {"type": "finish", "chunk": i, "released": fin["released"], + "home_tol": finisher.home_tol, "still_deg": finisher.still_deg, "chunks": finisher.need, + "still_run": fin["still_run"], "home_err": fin["home_err"], "delta_max": fin["delta_max"]} + if fin is not None and fin["action"] == "regrasp": + # 放したあとにまた閉じる答え (182327 の空振り). このチャンクは送らずに入力欄を出す + rec["regrasp_stop"] = True + log_f.write(json.dumps(dict(fe, event="regrasp", reason="close_after_release", + t=time.time())) + "\n") + log_f.flush() + print(f" [FINISH] 放したあとにまた閉じる答え (2 回目のつかみ). 推論 {i} は送らずに止めます " + + finish_text(fin)) + if args.auto: + # --auto は入力欄を出せないので, 送らずにそのまま終える (アームは動かさない) + end_reason, exit_code = "regrasp", 1 + log_f.write(json.dumps(rec) + "\n"); log_f.flush() + break + regrasp_hold = True + elif fin is not None and fin["action"] == "finish": + if pause_req is not None or hold_confirm or finish_hold: + # 人が止めた (スペース・s・b・c・p のあと) ときは入力欄を優先する (勝手に HOME へ動かさない) + print(f" [FINISH] 終わる条件を満たしたが, 止めているので入力欄を出します {finish_text(fin)}") + else: + end_reason = "auto_finish" + step_pause.disarm() # 結果の入力の前に端末の設定を元に戻す + log_f.write(json.dumps(dict(fe, event="auto_finish", + reason="released_near_home_still", t=time.time())) + "\n") + log_f.flush() + print(f" [FINISH] 置き終えたので終えます (放したあと HOME 近くで止まったチャンクが " + f"{fin['still_run']} 回続いた). 推論 {i} は送らずに HOME へ戻します " + finish_text(fin)) + rec["auto_finish"] = True + if recorder is not None: + recorder.set_mode(MODE_HOME) + if args.dry_keys: + print(" [DRY ] HOME へは送らない (ドライラン)") + else: + # h と同じ送り方 + ok_home, _ = _send_and_wait_arrival(HOME, "pi05_home", 2.5, send_wait_sec=SEND_WAIT) + 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 need_confirm = ((not args.no_step_confirm) or first_confirm or pause_req is not None - or hold_confirm) + or hold_confirm or regrasp_hold) first_confirm = False - if skip_now and pause_req is None: + if skip_now and pause_req is None and not regrasp_hold: # 依頼 177: lab_start.sh の 2 回目からの試行は, 始めの確かめ (Enter) を待たずに実行する. # 2 回目からの確かめ・補正・巻き戻し・止める操作は今までどおり print(" [CONFIRM] 2 回目からの試行なので始めの確かめ (Enter) を待たずに実行します " @@ -1860,6 +2026,11 @@ def main(): break if recorder is not None and ans not in ("q", "h"): recorder.set_mode(MODE_MODEL) + if ans in ("s", "b", "c", "p"): + finisher.reset_still() # 依頼 181: 人が手を入れたら「続いた」を数え直す + finish_hold = True # Enter で実行するまでは自動で終えずに入力欄を出す (codex 2 回目 R1) + elif ans == "": + finish_hold = False if args.no_step_confirm: # 依頼 178: 入力欄を出していた時刻 (送らなかったチャンクも chunk_timing の確認待ちに入れる. codex R5) rec["prompt_t"] = [t_prompt0, time.time()] @@ -1904,6 +2075,7 @@ def main(): n_rewinds += 1 if rw.get("gripper_opened"): grip = 0 + finisher.on_rewind_open() # 依頼 181: つかんだ区間を戻した (ブロックは元の位置) if grasp == GRASP_DROPPED and entry["kind"] == "chunk": rw["start_frame"] = str(log_dir / "frames" / f"{entry['chunk']:04d}.jpg") if not rw["finished"]: @@ -1935,6 +2107,7 @@ def main(): with KeyReader() as keys: cr, grip = run_correction(arm, keys, lo, hi, args.max_move_deg, grip, n_corrections, i, log_f, recorder) + finisher.on_grip(grip) # 依頼 181: 補正で爪を動かした (g で放したのも数える) if cr is not None: log_f.write(json.dumps(cr) + "\n"); log_f.flush() if cr["grip_used"] or cr.get("regrip_used"): @@ -1977,6 +2150,7 @@ def main(): if grip_cmd is not None: arm.gripper(grip_cmd) grip = grip_cmd + finisher.on_grip(grip) 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, @@ -2134,6 +2308,7 @@ def main(): recorder.gripper(grip_cmd) gripper_cmd(grip_cmd) grip = grip_cmd + finisher.on_grip(grip) # 依頼 181: つかんだ・放したを数える rec["executed"] = True # 巻き戻し (b) 用に, この推論で実際に送った目標と始めの姿勢 (実測) を残す. # 途中で止まった (broke) 推論も, 1 点でも送っていれば残す (codex R1-3: @@ -2187,6 +2362,7 @@ def main(): print("\n[STOP ] Ctrl-C") end_reason = "ctrl_c" finally: + GRIP_OBSERVER = None # 依頼 181: この試行の判定器を外す try: step_pause.disarm() # 依頼 178: Ctrl-C・例外でも端末の設定を必ず元に戻す except Exception as e: diff --git a/pi05/test_auto_finish.py b/pi05/test_auto_finish.py new file mode 100644 index 0000000..7c9da3d --- /dev/null +++ b/pi05/test_auto_finish.py @@ -0,0 +1,464 @@ +#!/usr/bin/env python3 +"""test_auto_finish.py — 箱に置き終えたら自動で終わる --auto-finish の確かめ (依頼 181. アームなし). + + python3 pi05/test_auto_finish.py # 全部 + python3 pi05/test_auto_finish.py -v + python3 pi05/test_auto_finish.py --table # Log35 の 7 本を流して, チャンクごとの値と何チャンク目で終わるかを表で出す + + - Log35 (2026-11-14) の 7 本の log.jsonl を AutoFinish に流し, 何チャンク目で終わる判定になるかを確かめる + (182327 は 2 回目のつかみ (チャンク 51) の前に終わる. 置く前・運ぶ途中では終わらない). KIOXIA がなければ飛ばす + - run_policy_client を test_rewind_correct.run_client (偽の daemon / カメラ / モデル) で通し, + つかんで放して HOME で止まったら, そのチャンクは送らずに HOME へ (h と同じ送り方)・結果を聞く・type=finish の行 + - 放したあとにまた閉じる答えが出たら送らずに入力欄 (画面に [FINISH]). --auto なら送らずに終える + - 止める予約 (スペース) があるときは入力欄を優先する + - 付けなければ git HEAD の版と同じ送り方・同じ log (項目も増えない) +KIOXIA は読むだけ (書かない). アームには触らない (一時フォルダだけ). +""" +import importlib.util +import json +import subprocess +import sys +import tempfile +import time +import unittest +from pathlib import Path + +import numpy as np + +_HERE = Path(__file__).resolve().parent +sys.path.insert(0, str(_HERE)) + +import chunk_timing as ct # noqa: E402 +import run_policy_client as rpc # noqa: E402 +import test_no_step_confirm as tnsc # noqa: E402 +from test_rewind_correct import FakePolicy, run_client, strip # noqa: E402 + +RUNS = Path("/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy") +# Log35 の 7 本と, 既定 (8°・3°・2 回) で終わる判定になるチャンク (None = 記録の終わりまでに終わらない) +# 181057: 放したあと HOME に戻らないうちに人が止めた (HOME からの差 27.6° 以上) +# 182211: HOME 近くで止まったチャンクが 1 回だけ (チャンク 12) のところで人が止めた +LOG35 = {"pi05_20261114_180457": 17, "pi05_20261114_180704": None, "pi05_20261114_181057": None, + "pi05_20261114_181300": 20, "pi05_20261114_182039": 15, "pi05_20261114_182211": None, + "pi05_20261114_182327": 17} +EXEC = ["--execute", "--steps-per-chunk", "10", "--via-stride", "3", "--settle-retries", "0", + "--state-from-target", "--ignore-framing"] + + +def replay(run_dir, fin=None, stop=True): + """log.jsonl の推論の行を順に AutoFinish に流す. 戻り値 (終わる判定のチャンク or None, 2 回目のつかみのチャンク, 行). + 推論の前の実測角 (measured_state) と答え (actions) で判定し, 実行したチャンクの爪の指令で つかんだ・放した を数える + (client と同じ順). 終わる判定が出たらそこで止める (そのチャンクは送らない).""" + fin = fin or rpc.AutoFinish(True) + rows, regrasp = [], None + for line in (Path(run_dir) / "log.jsonl").read_text().splitlines(): + r = json.loads(line) + if r.get("type") is not None or "actions" not in r: + continue + info = fin.judge(r["measured_state"][:6], np.asarray(r["actions"]), r.get("grip_cmd")) + rows.append(dict(info, chunk=r["chunk"], grip_cmd=r.get("grip_cmd"), executed=r.get("executed"))) + if info["action"] == "finish" and stop: + return r["chunk"], regrasp, rows + if info["action"] == "regrasp" and regrasp is None: + regrasp = r["chunk"] + if r.get("executed") and r.get("grip_cmd") is not None: + fin.on_grip(r["grip_cmd"]) + return None, regrasp, rows + + +def first_release(run_dir): + """最初に閉じたあとで開いたチャンク (実行したもの).""" + closed = False + for line in (Path(run_dir) / "log.jsonl").read_text().splitlines(): + r = json.loads(line) + if r.get("type") is not None or not r.get("executed"): + continue + if r.get("grip_cmd") == 1: + closed = True + elif r.get("grip_cmd") == 0 and closed: + return r["chunk"] + return None + + +def print_table(): + """Log35 の 7 本: 放したあとのチャンクごとの値 (既定の判定. 終わる判定のあとも記録の終わりまで出す).""" + for name in LOG35: + d = RUNS / name + if not (d / "log.jsonl").exists(): + print(f"{name}: log.jsonl がない") + continue + rel = first_release(d) + end, regrasp, _ = replay(d) + _, _, rows = replay(d, stop=False) + print(f"== {name} 放したチャンク {rel} 終わる判定 {end} 2 回目のつかみ {regrasp}") + print(" chunk HOMEからの差(最大) Δ10ステップ(最大) 続いた") + for r in rows: + if rel is None or r["chunk"] <= rel: + continue + print(f" {r['chunk']:>5} {max(r['home_err']):>8.1f} (j{int(np.argmax(r['home_err']))})" + f" {max(r['delta_max']):>8.1f} (j{int(np.argmax(r['delta_max']))}) {r['still_run']:>4}" + + (" ← 終わる" if r["chunk"] == end else "") + + (" ← 2回目のつかみ" if r["action"] == "regrasp" else "")) + + +class TestLog35(unittest.TestCase): + def setUp(self): + if not RUNS.exists(): + self.skipTest("KIOXIA の Log35 の記録がない") + + def test_finish_chunks(self): + for name, want in LOG35.items(): + d = RUNS / name + if not (d / "log.jsonl").exists(): + self.skipTest(f"{name} がない") + end, regrasp, rows = replay(d) + self.assertEqual(end, want, name) + rel = first_release(d) + self.assertIsNotNone(rel, name) + # 置く前・運ぶ途中 (放したチャンクまで) では終わらない + self.assertTrue(end is None or end > rel, name) + # 運ぶ途中は HOME からの差が既定より大きい (つかむ前に HOME 近くにいるのは始めだけ) + for r in rows: + if r["chunk"] <= rel: + self.assertFalse(r["released"], (name, r["chunk"])) + + def test_182327_finishes_before_second_grasp(self): + d = RUNS / "pi05_20261114_182327" + if not (d / "log.jsonl").exists(): + self.skipTest("182327 がない") + end, regrasp, _ = replay(d) + self.assertEqual(end, 17) + self.assertIsNone(regrasp) # 終わったので 2 回目のつかみまで行かない + # 終わる判定を外して流すと, チャンク 51 の閉じる答えを 2 回目のつかみとして止める + _, regrasp2, _ = replay(d, rpc.AutoFinish(True, 1e-9, 1e-9, 10 ** 9)) + self.assertEqual(regrasp2, 51) + + +class TestAutoFinishUnit(unittest.TestCase): + def acts(self, q, d=0.5): + a = np.zeros((10, 7)) + a[:, :6] = np.asarray(q) + d + return a + + def test_needs_grasp_and_release(self): + f = rpc.AutoFinish(True) + home = np.array(rpc.HOME) + for _ in range(5): # まだつかんでいない: HOME で止まっていても終わらない + self.assertIsNone(f.judge(home, self.acts(home), None)["action"]) + f.on_grip(0) + self.assertIsNone(f.judge(home, self.acts(home), None)["action"]) # 開いただけ (閉じていない) + f.on_grip(1) + self.assertIsNone(f.judge(home, self.acts(home), None)["action"]) # 閉じたまま + f.on_grip(0) + self.assertEqual(f.judge(home, self.acts(home), None)["still_run"], 1) + self.assertEqual(f.judge(home, self.acts(home), None)["action"], "finish") + + def test_thresholds_and_reset(self): + f = rpc.AutoFinish(True) + f.on_grip(1); f.on_grip(0) + home = np.array(rpc.HOME) + far = home + np.array([0, 8.5, 0, 0, 0, 0]) + self.assertIsNone(f.judge(far, self.acts(far), None)["action"]) # 8° より遠い + self.assertEqual(f.still_run, 0) + self.assertEqual(f.judge(home + 8.0, self.acts(home + 8.0), None)["still_run"], 1) # ちょうど 8° は入る + self.assertEqual(f.judge(home, self.acts(home, 3.5), None)["still_run"], 0) # Δ 3.5° は動いている + self.assertEqual(f.judge(home, self.acts(home, 3.0), None)["still_run"], 1) + f.reset_still() + self.assertEqual(f.judge(home, self.acts(home), None)["still_run"], 1) # 数え直し + self.assertEqual(f.judge(home, self.acts(home), None)["action"], "finish") + + def test_looks_only_first_10_steps(self): + """答えが 10 ステップより長くても, 先頭 10 ステップ (1 秒先まで) だけを見る (codex R1).""" + f = rpc.AutoFinish(True) + f.on_grip(1); f.on_grip(0) + home = np.array(rpc.HOME) + a = np.zeros((50, 7)) + a[:, :6] = home + 0.5 + a[10:, 0] = 20.0 + self.assertEqual([f.judge(home, a, None)["action"] for _ in range(2)], [None, "finish"]) + a[9, 0] = 20.0 # 10 ステップ目は見る + self.assertEqual(f.judge(home, a, None)["still_run"], 0) + + def test_not_while_holding(self): + """開いてから閉じ直した (補正の R) あとは, 持ったまま HOME で止まっても終えない.""" + f = rpc.AutoFinish(True) + home = np.array(rpc.HOME) + f.on_grip(1); f.on_grip(0); f.on_grip(1) + self.assertEqual([f.judge(home, self.acts(home), None)["action"] for _ in range(3)], [None] * 3) + f.on_grip(0) + self.assertEqual([f.judge(home, self.acts(home), None)["action"] for _ in range(2)], [None, "finish"]) + + def test_regrasp_and_rewind(self): + f = rpc.AutoFinish(True) + home = np.array(rpc.HOME) + self.assertIsNone(f.judge(home, self.acts(home), 1)["action"]) # 1 回目のつかみはそのまま + f.on_grip(1); f.on_grip(0) + self.assertEqual(f.judge(home, self.acts(home), 1)["action"], "regrasp") + f.on_rewind_open() # つかんだ区間を巻き戻した: つかんだこと自体を忘れる + self.assertIsNone(f.judge(home, self.acts(home), 1)["action"]) + self.assertIsNone(f.judge(home, self.acts(home), None)["action"]) + + +class PlacePolicy(FakePolicy): + """つかむ → 運ぶ → 放す → HOME へ戻る → HOME で小さく動き続ける (Log35 の形). regrasp=True なら HOME に + 戻ったあとでまた閉じる答えを出す (182327 の空振り). 状態 (state) と自分の段で答えを決める (画像は見ない).""" + regrasp = False + + def __init__(self, *a, **k): + super().__init__(*a, **k) + self.held = self.released = False + self.home_chunks = 0 + + def infer(self, obs): + s = np.asarray(obs["observation/state"], dtype=float) + holding = s[6] > 0.5 + if holding: + self.held = True + elif self.held: + self.released = True + acts = np.zeros((10, 7)) + home = np.array(rpc.HOME) + for k in range(10): + if self.released: + a = s[:6] + (home - s[:6]) * min(1.0, (k + 1) / 5) + if np.max(np.abs(s[:6] - home)) < 3: + a = s[:6] + 0.3 * ((k % 2) * 2 - 1) # HOME 近くで小さく動く (±0.3°) + acts[k, :6] = a + acts[k, 6] = 1.0 if (self.regrasp and np.max(np.abs(s[:6] - home)) < 3 and k >= 3) else 0.0 + elif holding: + acts[k, :6] = s[:6] + np.array([1.0, 0, 0, 0, 0, 0]) * (k + 1) + acts[k, 6] = 0.0 if (s[0] >= 30 and k >= 3) else 1.0 + else: + acts[k, :6] = s[:6] + (np.array([2.0, 0, 0, 0, 0, 0]) * (k + 1) if s[0] < 10 else 0) + acts[k, 6] = 1.0 if (s[0] >= 10 and k >= 3) else 0.0 + return {"actions": acts} + + +class RegraspPolicy(PlacePolicy): + regrasp = True + + +def chunks(lines): + return [x for x in lines if x.get("type") is None and "chunk" in x] + + +class TestClientAutoFinish(unittest.TestCase): + def setUp(self): + self.td = Path(tempfile.mkdtemp()) + time.sleep(1.1) + + def test_finish_goes_home_and_asks(self): + code, world, run, lines, out = run_client( + rpc, EXEC + ["--max-chunks", "20", "--no-step-confirm", "--auto-finish"], ["", "y"], self.td, + policy_cls=PlacePolicy) + self.assertEqual(code, 0, out) + fins = [x for x in lines if x.get("type") == "finish"] + self.assertEqual(len(fins), 1, out) + f = fins[0] + self.assertEqual(f["event"], "auto_finish") + self.assertEqual(f["still_run"], 2) + self.assertTrue(f["released"]) + self.assertEqual((f["home_tol"], f["still_deg"], f["chunks"]), (8.0, 3.0, 2)) + self.assertEqual(len(f["home_err"]), 6) + self.assertEqual(len(f["delta_max"]), 6) + ch = chunks(lines) + last = ch[-1] + self.assertEqual(last["chunk"], f["chunk"]) + self.assertTrue(last["auto_finish"]) + self.assertFalse(last["executed"]) # 終わったチャンクは送らない + self.assertTrue(last["home_requested"] and last["home_arrived"]) + self.assertFalse(any(s[0].startswith(f"pi05_chunk{last['chunk']:03d}") for s in world.sends)) + self.assertEqual(world.sends[-1][0], "pi05_home") + self.assertEqual(world.sends[-1][1], [0.0, 0.0, 0.0, 0.0, 0.0, -30.0]) + self.assertLess(last["chunk"], 19) + # 開いたチャンクのあと, HOME に戻ってから 2 チャンクで終わる + rel = [x["chunk"] for x in ch if x.get("grip_cmd") == 0 and x["executed"]][0] + self.assertGreater(last["chunk"], rel) + self.assertIn("[FINISH] 置き終えたので終えます", out) + self.assertIn("[FINISH] --auto-finish:", out) + trial = json.loads((run / f"{run.name}.json").read_text()) + self.assertEqual(trial["end_reason"], "auto_finish") + self.assertEqual(trial["result"], "completed") + self.assertEqual(trial["outcome"], "success") + self.assertEqual(lines[0]["auto_finish"], {"home_tol": 8.0, "still_deg": 3.0, "chunks": 2}) + self.assertIsNone(rpc.GRIP_OBSERVER) # 終わったら判定器を外す + # chunk_timing の終わり方 + reason, text = ct.end_how(run, ch) + self.assertEqual(reason, "auto_finish") + self.assertIn("自動で終わった", text) + self.assertIn("自動で終えた", ct.chunk_rows(ch)[-1]["note"]) + + def test_finish_with_step_confirm(self): + """チャンクごとに確かめる形でも, 終わる判定のチャンクは入力欄を出さずに HOME へ (近いので小さく動くだけ).""" + code, world, run, lines, out = run_client( + rpc, EXEC + ["--max-chunks", "20", "--auto-finish"], [""] * 7 + ["n"], self.td, policy_cls=PlacePolicy) + self.assertEqual(code, 0, out) + self.assertEqual(world.sends[-1][0], "pi05_home") + self.assertEqual(json.loads((run / f"{run.name}.json").read_text())["end_reason"], "auto_finish") + + def test_custom_chunks(self): + code, world, run, lines, out = run_client( + rpc, EXEC + ["--max-chunks", "20", "--no-step-confirm", "--auto-finish", "--finish-chunks", "3"], + ["", "y"], self.td, policy_cls=PlacePolicy) + self.assertEqual(code, 0, out) + f = [x for x in lines if x.get("type") == "finish"][0] + self.assertEqual(f["still_run"], 3) + self.assertEqual(lines[0]["auto_finish"]["chunks"], 3) + + def test_regrasp_stops_with_prompt(self): + code, world, run, lines, out = run_client( + rpc, EXEC + ["--max-chunks", "20", "--no-step-confirm", "--auto-finish", "--finish-chunks", "5"], + ["", "h", "y"], self.td, policy_cls=RegraspPolicy) + self.assertEqual(code, 0, out) + re_ = [x for x in lines if x.get("type") == "finish"] + self.assertEqual([x["event"] for x in re_], ["regrasp"], out) + ch = chunks(lines) + last = ch[-1] + self.assertEqual(last["chunk"], re_[0]["chunk"]) + self.assertTrue(last["regrasp_stop"]) + self.assertFalse(last["executed"]) + self.assertEqual(last["grip_cmd"], 1) + self.assertIn("[FINISH] 放したあとにまた閉じる答え (2 回目のつかみ)", out) + # 2 回目の閉じる指令は送っていない (始めの開く・1 回目の閉じる・放す だけ) + closes = [g for g in world.grips if g == 1 or (isinstance(g, tuple) and g[0] == "v")] + self.assertEqual(len(closes), 1, world.grips) + self.assertEqual(world.sends[-1][0], "pi05_home") # h で戻した + self.assertIn("2回目のつかみ", ct.chunk_rows(ch)[-1]["note"]) + self.assertIn("2 回目のつかみで止めた 1 回", ct.end_how(run, ch)[1]) + + def test_regrasp_enter_sends(self): + """2 回目のつかみで Enter を押せば今までどおり送る (人の判断).""" + code, world, run, lines, out = run_client( + rpc, EXEC + ["--max-chunks", "20", "--no-step-confirm", "--auto-finish", "--finish-chunks", "5"], + ["", "", "q", ""], self.td, policy_cls=RegraspPolicy) + self.assertEqual(code, 0, out) + ch = chunks(lines) + sent_regrasp = [x for x in ch if x.get("regrasp_stop") and x["executed"]] + self.assertEqual(len(sent_regrasp), 1, out) + closes = [g for g in world.grips if g == 1 or (isinstance(g, tuple) and g[0] == "v")] + self.assertEqual(len(closes), 2, world.grips) + + def test_close_open_in_correction_counts(self): + """補正 (c) の中で g で閉じて g で開いた (つかんで放した) のも数える (codex R2). そのあとの閉じる答えは 2 回目の + つかみとして止め, HOME で止まったら終える.""" + class StayHomePolicy(FakePolicy): + calls = 0 + + def infer(self, obs): + StayHomePolicy.calls += 1 + s = np.asarray(obs["observation/state"], dtype=float) + acts = np.zeros((10, 7)) + acts[:, :6] = s[:6] + 0.3 + acts[:, 6] = 1.0 if StayHomePolicy.calls == 2 else 0.0 # 補正のあとの 1 回目だけ閉じる答え + return {"actions": acts} + + code, world, run, lines, out = run_client( + rpc, EXEC + ["--max-chunks", "10", "--no-step-confirm", "--auto-finish"], + ["c", "s", "", "y"], self.td, keys="gg\n", policy_cls=StayHomePolicy) + self.assertEqual(code, 0, out) + ev = [x["event"] for x in lines if x.get("type") == "finish"] + self.assertEqual(ev, ["regrasp", "auto_finish"], out) + self.assertEqual(world.sends[-1][0], "pi05_home") + closes = [g for g in world.grips if g == 1 or (isinstance(g, tuple) and g[0] == "v")] + self.assertEqual(len(closes), 1, world.grips) # 閉じたのは補正の g だけ + + def test_hold_after_s_with_step_confirm(self): + """チャンクごとに確かめる形で --finish-chunks 1 でも, c・s のあとは自動で終えずに入力欄 (codex 2 回目 R1). + HOME で止まっている腕を, 始めの確かめで c → g (閉) → g (開) にしてから流す.""" + class StillPolicy(FakePolicy): + def infer(self, obs): + s = np.asarray(obs["observation/state"], dtype=float) + acts = np.zeros((10, 7)) + acts[:, :6] = s[:6] + 0.3 + return {"actions": acts} + + code, world, run, lines, out = run_client( + rpc, EXEC + ["--max-chunks", "10", "--auto-finish", "--finish-chunks", "1"], + ["c", "s", "", "y"], self.td, keys="gg\n", policy_cls=StillPolicy) + self.assertEqual(code, 0, out) + ch = chunks(lines) + self.assertEqual([x["event"] for x in lines if x.get("type") == "finish"], ["auto_finish"], out) + # c・s を選んだあとの推論 (終わる条件は満たしている) では終えず, Enter で実行したあとの推論で終える + self.assertEqual(out.count("[FINISH] 終わる条件を満たしたが, 止めているので入力欄を出します"), 2, out) + self.assertEqual([x["chunk"] for x in ch], [0, 1, 2, 3]) + self.assertTrue(ch[2]["executed"]) + self.assertTrue(ch[3]["auto_finish"]) + self.assertEqual(world.sends[-1][0], "pi05_home") + + def test_regrasp_auto_ends(self): + code, world, run, lines, out = run_client( + rpc, EXEC + ["--max-chunks", "20", "--auto", "--auto-finish", "--finish-chunks", "5"], ["y"], self.td, + policy_cls=RegraspPolicy) + self.assertEqual(code, 1, out) + last = chunks(lines)[-1] + self.assertTrue(last["regrasp_stop"]) + self.assertFalse(last["executed"]) + self.assertFalse(any(s[0] == "pi05_home" for s in world.sends)) # 勝手に動かさない + self.assertEqual(json.loads((run / f"{run.name}.json").read_text())["end_reason"], "regrasp") + + def test_pause_wins_over_finish(self): + """止める予約 (スペース) が入っていれば, 終わる条件を満たしても入力欄を出す (勝手に HOME へ動かさない).""" + class HomeCountPolicy(PlacePolicy): + home_infers = 0 + + def infer(self, obs): + s = np.asarray(obs["observation/state"], dtype=float) + if self.released and np.max(np.abs(s[:6] - np.array(rpc.HOME))) < 3: + HomeCountPolicy.home_infers += 1 + return super().infer(obs) + + class SpaceAtSecondHome(tnsc.FakeKeys): + def _read_pending(self): + self.n_poll += 1 + return " " if HomeCountPolicy.home_infers >= 2 else "" + + from unittest import mock + with mock.patch.object(rpc, "PauseKeys", SpaceAtSecondHome): + code, world, run, lines, out = run_client( + rpc, EXEC + ["--max-chunks", "20", "--no-step-confirm", "--auto-finish"], + ["", "h", "y"], self.td, policy_cls=HomeCountPolicy) + self.assertEqual(code, 0, out) + self.assertEqual([x for x in lines if x.get("type") == "finish"], []) + last = chunks(lines)[-1] + self.assertTrue(last.get("paused")) + self.assertEqual(last["finish_check"]["still_run"], 2) # 終わる条件は満たしていた + self.assertIn("[FINISH] 終わる条件を満たしたが, 止めているので入力欄を出します", out) + self.assertEqual(json.loads((run / f"{run.name}.json").read_text())["end_reason"], "h") + + +class TestUnchangedWithoutFlag(unittest.TestCase): + """付けなければ git HEAD の版と同じ送り方・同じ log (項目も増えない. codex R3).""" + def test_same_as_head(self): + src = subprocess.run(["git", "show", "HEAD:pi05/run_policy_client.py"], cwd=_HERE, + capture_output=True, text=True) + if src.returncode != 0: + self.skipTest("git HEAD の版が読めない") + td = Path(tempfile.mkdtemp()) + p = td / "run_policy_client_head.py" + p.write_text(src.stdout) + spec = importlib.util.spec_from_file_location("run_policy_client_head", p) + old = importlib.util.module_from_spec(spec) + spec.loader.exec_module(old) + drop = ("t", "latency_s", "reference", "t_start", "t_end", "t_exec_start", "t_exec_end", + "ended_at", "segment_t", "prompt_t", "requested_t") + for argv, inputs, pol in ( + (EXEC + ["--max-chunks", "14", "--no-step-confirm"], ["", "h", "y"], PlacePolicy), + (EXEC + ["--max-chunks", "10"], [""] * 9 + ["q", ""], RegraspPolicy), + (EXEC + ["--max-chunks", "6", "--no-step-confirm"], ["", "c", "b", "s", "", "h", "y"], None)): + time.sleep(1.1) + c_old, w_old, _, l_old, o_old = run_client(old, argv, inputs, td, policy_cls=pol, keys="1++\n") + time.sleep(1.1) + c_new, w_new, _, l_new, o_new = run_client(rpc, argv, inputs, td, policy_cls=pol, keys="1++\n") + self.assertEqual(c_old, c_new, argv) + self.assertEqual(w_old.sends, w_new.sends, argv) + self.assertEqual(w_old.grips, w_new.grips, argv) + self.assertEqual(strip(l_old, drop=drop), strip(l_new, drop=drop), argv) + self.assertNotIn("[FINISH]", o_new) + self.assertTrue(all("auto_finish" not in x and "finish_check" not in x for x in l_new)) # 頭にも足さない (codex R3) + self.assertIsNone(rpc.GRIP_OBSERVER) + + +if __name__ == "__main__": + if "--table" in sys.argv: + print_table() + sys.exit(0) + unittest.main() diff --git a/test_lab_start.sh b/test_lab_start.sh index df1279f..c7cd4e6 100755 --- a/test_lab_start.sh +++ b/test_lab_start.sh @@ -600,7 +600,7 @@ check "--serve-only でも空打ちまで行う (依頼 176)" 'has out.txt "=== check "Pod は消さない (DELETE を呼ばない) で, かかった時間と片付け方を出す" '[ $(n_call DELETE) = 0 ] && has out.txt "=== かかった時間" && has out.txt "Pod は残しています (id $ID_NEW). 片付けは ./lab_stop.sh --log LogT"' check "記録 lab_start_*.txt に残る・始めの行に --serve-only と出る" 'grep -qF "サーバーとトンネルだけ (--serve-only" "$T/data/lab/LogT"/lab_start_*.txt' rm -rf "$T" -check "--help に --serve-only・--grip-value・--flow / --speed・5b の空打ちの説明が入る (2〜61 行)" 'sed -n "2,61p" "$HERE/lab_start.sh" | grep -qF -- "--serve-only Pod を作って" && sed -n "2,61p" "$HERE/lab_start.sh" | grep -qF -- "--no-tilt-fix 傾きを" && sed -n "2,61p" "$HERE/lab_start.sh" | grep -qF -- "--grip-value N 爪の閉じる値" && sed -n "2,61p" "$HERE/lab_start.sh" | grep -qF -- "--flow / --speed N run_policy_client.py に" && sed -n "2,61p" "$HERE/lab_start.sh" | grep -qF -- "# 5b. 空打ち" && grep -qF "sed -n '"'"'2,61p'"'"'" "$HERE/lab_start.sh"' +check "--help に --serve-only・--grip-value・--flow / --speed・5b の空打ちの説明が入る (2〜64 行)" 'sed -n "2,64p" "$HERE/lab_start.sh" | grep -qF -- "--serve-only Pod を作って" && sed -n "2,64p" "$HERE/lab_start.sh" | grep -qF -- "--no-tilt-fix 傾きを" && sed -n "2,64p" "$HERE/lab_start.sh" | grep -qF -- "--grip-value N 爪の閉じる値" && sed -n "2,64p" "$HERE/lab_start.sh" | grep -qF -- "--flow / --speed N run_policy_client.py に" && sed -n "2,64p" "$HERE/lab_start.sh" | grep -qF -- "# 5b. 空打ち" && grep -qF "sed -n '"'"'2,64p'"'"'" "$HERE/lab_start.sh"' echo "== 空打ち (依頼 176): トンネルのあと・アームの前に 1 回. 失敗しても止めずに続ける" setup; state $ID_OLD; pod 200 "$RUNNING"; probe 1 1 1 1 1; run @@ -660,7 +660,17 @@ setup; state $ID_OLD; pod 200 "$RUNNING"; probe 1 1 1 1 1; run --step-confirm -- check "--step-confirm なら --no-step-confirm を渡さず (ほかの引数は同じ), [CONFIRM] で出す" '[ $RC = 0 ] && has py.log "pi05/run_policy_client.py" && ! has py.log "--no-step-confirm" && grep -q -- "pi05/run_policy_client.py .* --flow$" "$T/py.log" && has out.txt "[CONFIRM] チャンクごとに確かめる (--step-confirm"' rm -rf "$T" check "実機の 8 にも既定で --no-step-confirm を渡す (8 の 2 か所)" '[ "$(grep -c '"'"'${STEP_ARGS\[@\]+"${STEP_ARGS\[@\]}"}'"'"' "$HERE/lab_start.sh")" = 2 ] && grep -qF "STEP_ARGS=(--no-step-confirm)" "$HERE/lab_start.sh" && grep -qF -- "--step-confirm) STEP_ARGS=(); shift ;;" "$HERE/lab_start.sh"' -check "--help に --step-confirm の説明が入る" 'sed -n "2,61p" "$HERE/lab_start.sh" | grep -qF -- "--step-confirm 実機推論で"' +check "--help に --step-confirm の説明が入る" 'sed -n "2,64p" "$HERE/lab_start.sh" | grep -qF -- "--step-confirm 実機推論で"' + +echo "== 置き終えたら自動で終わる (依頼 181): 既定は --auto-finish を渡す. --no-auto-finish で今までどおり" +setup; state $ID_OLD; pod 200 "$RUNNING"; probe 1 1 1 1 1; run +check "既定ではクライアントに --auto-finish を渡し, [FINISH] で出す" '[ $RC = 0 ] && [ "$(grep -c -- "pi05/run_policy_client.py .* --auto-finish" "$T/py.log")" = 1 ] && has out.txt "[FINISH] 箱に置き終えたら自動で終わる (--auto-finish"' +rm -rf "$T" +setup; state $ID_OLD; pod 200 "$RUNNING"; probe 1 1 1 1 1; run --no-auto-finish --flow +check "--no-auto-finish なら --auto-finish を渡さず (ほかの引数は同じ), [FINISH] で出す" '[ $RC = 0 ] && has py.log "pi05/run_policy_client.py" && ! has py.log "--auto-finish" && grep -q -- "pi05/run_policy_client.py .* --no-step-confirm --flow$" "$T/py.log" && has out.txt "[FINISH] 置き終えても自動では終わらない (--no-auto-finish"' +rm -rf "$T" +check "実機の 8 にも既定で --auto-finish を渡す (8 の 2 か所)" '[ "$(grep -c '"'"'${FINISH_ARGS\[@\]+"${FINISH_ARGS\[@\]}"}'"'"' "$HERE/lab_start.sh")" = 2 ] && grep -qF "FINISH_ARGS=(--auto-finish)" "$HERE/lab_start.sh" && grep -qF -- "--no-auto-finish) FINISH_ARGS=(); shift ;;" "$HERE/lab_start.sh"' +check "--help に --no-auto-finish の説明が入る" 'sed -n "2,64p" "$HERE/lab_start.sh" | grep -qF -- "--no-auto-finish 箱に置き終えたら" && ./lab_start.sh --help | grep -qF -- "--no-auto-finish"' echo echo "OK $OK / NG $NG"