commit 0383893682aaa5e56e618ff6be55c67431d00304 Author: Eiichi Shimazu Date: Fri Nov 13 22:00:26 2026 +0900 実機推論の最初の待ちをなくし, チャンクの中の途中の点で止まらずに流せるようにする (依頼 176・Log35). lab_start.sh はトンネル (5) のあと・アームの前に 5b で pi05/policy_warmup.py を流し, お手本 ($RAW_RANGE) の 1 枚目と state[0] と prompt で 1 回だけ推論させて捨てる (つなぐところから --timeout 300 秒. かかった秒を [WARM ] に. 失敗・Ctrl-C でも 1 行出して続ける. --serve-only でも行う). run_policy_client.py に --flow (途中の点は単発の send_angles を 1 回送り, --flow-tol (既定 5°) 以内に近づくか見込みの時間 (0.15 秒 + 角度 ÷ 25°/秒×speed/20) で次を送る. 送る間隔は 0.4 秒以上・1.5 秒まで. 最後の点は今までどおり 3° の到着確認. 送る直前の読み直しと守りは今までどおり) と --speed N (既定 20. チャンクと settle だけ. 補正・巻き戻し・HOME は 20 のまま) を足す. log.jsonl の頭に flow・flow_tol・speed, --flow の中継点ごとに送った時刻・次を送った時刻・進んだわけ. 既定では送り方・log の中身・出力は今までと同じ. lab_start.sh に --flow / --speed N を足しクライアントに渡す. チャンクごとの 推論・確認待ち・動いた・合計 を表で出す pi05/chunk_timing.py (読むだけ) を足す. テストを足す (test_flow_send 16・test_chunk_timing 6・lab_start 210. test_rewind_correct は新しい 3 項目を比べる対象から外す) (依頼 176. codex 4 回) Co-Authored-By: Claude Opus 5.5 --- lab_start.sh | 30 +++- pi05/chunk_timing.py | 229 ++++++++++++++++++++++++++++ pi05/policy_warmup.py | 131 ++++++++++++++++ pi05/run_policy_client.py | 185 +++++++++++++++++++++-- pi05/test_chunk_timing.py | 195 ++++++++++++++++++++++++ pi05/test_flow_send.py | 359 ++++++++++++++++++++++++++++++++++++++++++++ pi05/test_rewind_correct.py | 3 +- test_lab_start.sh | 31 +++- 8 files changed, 1146 insertions(+), 17 deletions(-) diff --git a/lab_start.sh b/lab_start.sh index 606745f..a4dd15f 100755 --- a/lab_start.sh +++ b/lab_start.sh @@ -32,10 +32,12 @@ # --serve-only Pod を作ってサーバーを 8000 番で待たせ, Mac の 8000 番へのトンネルを張るところ (1〜5) までだけ行い, # デーモン・HOME・爪・位置合わせ・推論 (6〜8) には進まない. トンネルを張ったまま待ち, Ctrl-C か TERM で # トンネルを閉じて終わる (Pod は消さない. 片付けは ./lab_stop.sh). 録画の画像で答えを出させる -# pi05/tilt_ab_test.py などのため (依頼 146) +# pi05/tilt_ab_test.py などのため (依頼 146). 5b の空打ちまでは行う (依頼 176) # --no-tilt-fix 傾きをソフトでそろえない (camera_align.py と run_policy_client.py に --no-tilt-fix を渡す. 依頼 144) # --grip-value N 爪の閉じる値 (0〜100. run_policy_client.py に --grip-value を渡す. 付けなければ今までどおり # run_policy_client.py の既定 20. 起動時に [GRIP ] で使う値を出す. 依頼 152) +# --flow / --speed N run_policy_client.py にそのまま渡す (中継点を止まらずに流す / チャンクを送る速さ 1〜100. +# 付けなければ今までどおり. 依頼 176. 起動時に [FLOW ] で出す) # つなぎ直しで, 残っている Pod のサーバーが別のモデルで動いていれば, そのことを出して止まる # # 1. Pod を作る (A40×1・Runpod Pytorch 2.8.0・コンテナのディスク 200GB・22/tcp を外へ出す) @@ -52,6 +54,8 @@ # → addon・設定・assets → serve_policy.py を tmux の中で起動し /workspace/logs/serve.log に残す) # 4. serve.log に「server listening on 0.0.0.0:8000」が出るまで待つ # 5. Mac の 8000 番へのトンネルを裏で張る (このスクリプトが終わるときに閉じる) +# 5b. 空打ち: pi05/policy_warmup.py でお手本と同じ形の観測を 1 回だけ推論させて捨てる (1 回目の推論のコンパイル +# 57.5 秒をアームを動かす前に済ませる. 依頼 176). かかった秒を [WARM ] に出す. 失敗・Ctrl-C でも止めずに続ける # 6. tools/daemon_ctl.sh status で 5555 の LISTEN を見る. なければ start # 7. HOME → 爪を開く → camera_align.py (q で閉じる) # 8. run_policy_client.py (LOG407 と同じ引数). 終わったら, もう1回やるかを聞く (y なら 7 へ) @@ -79,6 +83,7 @@ MODEL=trim TILT_FIX_ARGS=() # 依頼 144: 既定は傾きをソフトでそろえる. --no-tilt-fix で回さない GRIP_ARGS=() # 依頼 152: --grip-value N のときだけ run_policy_client.py に --grip-value N を渡す GRIP_VALUE_SHOWN="20 (既定. run_policy_client.py の GRIP_VALUE)" +FLOW_ARGS=() # 依頼 176: --flow / --speed N のときだけ run_policy_client.py に渡す while [ $# -gt 0 ]; do case "$1" in --gpu) lab_need_val A40 "$@"; GPU="$2"; shift 2 ;; @@ -92,9 +97,14 @@ while [ $# -gt 0 ]; do # 桁数を先に見る (大きな数は算術展開であふれて小さな値になる. codex R1) [ ${#2} -le 3 ] && [ "$((10#$2))" -le 100 ] || { echo "[ERROR] --grip-value は 0〜100 の整数: $2" >&2; exit 2; } GRIP_ARGS=(--grip-value "$((10#$2))"); GRIP_VALUE_SHOWN="$((10#$2)) (--grip-value)"; shift 2 ;; + --flow) FLOW_ARGS+=(--flow); 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,54p' "$0"; exit 0 ;; + -h|--help) sed -n '2,58p' "$0"; exit 0 ;; *) echo "[ERROR] 知らない引数: $1 (./lab_start.sh --help)" >&2; exit 2 ;; esac done @@ -143,6 +153,7 @@ else echo "[TILT ] 上カメラの傾きのお手本 (各エピソードの 1 枚目の中央値) は $RAW_RANGE から (安全範囲と同じ. --no-tilt-fix なので回さない. 依頼 142)" fi echo "[GRIP ] 爪の閉じる値は $GRIP_VALUE_SHOWN. 締め直し r はこの値−3 とこの値を交互に送る (依頼 152)" +[ ${#FLOW_ARGS[@]} = 0 ] || echo "[FLOW ] 推論の送り方: ${FLOW_ARGS[*]} (依頼 176)" # 付けなければ今までと同じ出力 lab_ssh_opts HF_REPO=$MODEL_REPO # --model で選んだモデル (lab_lib.sh の lab_model_def) @@ -453,6 +464,17 @@ stage_done cd "$HERE" || fail "$HERE に移れない" +# ---- 5b. 空打ち (依頼 176): 1 回目の推論のコンパイルをアームを動かす前に済ませる. 失敗しても止めない ---- +# 観測の形は安全範囲と同じお手本 ($RAW_RANGE) の 1 枚目と state[0]. この間の Ctrl-C は空打ちだけをやめる +stage "5b. 空打ち (1 回目の推論の待ちを先に済ませる)" +trap ':' INT +python3 pi05/policy_warmup.py --port "$TUNNEL_PORT" --raw-dir "$RAW_RANGE" +rc=$? +# 1 は policy_warmup.py が失敗の 1 行を出したとき. それ以外 (落ちた・読み込めない) はここで 1 行出す +[ $rc = 0 ] || [ $rc = 1 ] || echo "[WARM ] 空打ちが終了コード $rc で終わりました. 止めずに続けます (1 回目の推論が長くなる)" +trap 'fail "Ctrl-C で止めました"' INT +stage_done + # ---- --serve-only: ここで止まり, トンネルを張ったまま待つ (6〜8 に進まない. 依頼 146) ---- if [ $SERVE_ONLY = 1 ]; then serve_only_end() { @@ -503,13 +525,13 @@ 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[@]}"} + --tilt-raw-dir "$RAW_RANGE" ${TILT_FIX_ARGS[@]+"${TILT_FIX_ARGS[@]}"} ${GRIP_ARGS[@]+"${GRIP_ARGS[@]}"} ${FLOW_ARGS[@]+"${FLOW_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[@]}"} + --tilt-raw-dir "$RAW_RANGE" ${TILT_FIX_ARGS[@]+"${TILT_FIX_ARGS[@]}"} ${GRIP_ARGS[@]+"${GRIP_ARGS[@]}"} ${FLOW_ARGS[@]+"${FLOW_ARGS[@]}"} rc=$? fi echo "[CLIENT] run_policy_client.py が終わった (終了コード $rc. モデル $MODEL_TAG)" diff --git a/pi05/chunk_timing.py b/pi05/chunk_timing.py new file mode 100644 index 0000000..8650f14 --- /dev/null +++ b/pi05/chunk_timing.py @@ -0,0 +1,229 @@ +#!/usr/bin/env python3 +"""chunk_timing.py — 実機推論の試行のチャンクごとの時間の内訳を表で出す (依頼 176. 読むだけ・何も書き出さない). + + python3 pi05/chunk_timing.py <試行のフォルダ> [<試行のフォルダ> ...] + 例: python3 pi05/chunk_timing.py /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261113_193254 + +試行のフォルダ = run_policy_client.py の記録 (log.jsonl があるフォルダ). フォルダ名だけ (pi05_20261113_193254) なら +/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/ の下を探す. + +チャンクごとの列 (秒. log.jsonl の時刻から. 推論を始めた時刻は t − latency_s の近似 (t は計画・画像の保存のあとに +書くので, その分 (ふつう 0.01 秒ほど) 確認待ちが短く, 推論の始まりが遅めに出る)): + 推論 latency_s (π0.5 が答えるまで) + 確認待ち 推論が終わってから送り始めるまで (Enter の確認. --auto なら 0 に近い). 送っていないチャンクは - + 動いた 送り始めてから送り終わるまで (t_exec_start → t_exec_end. 爪の開け閉めを含む) + その他 合計 − 推論 − 確認待ち − 動いた (関節角の読み取り・撮影など. 巻き戻し・補正・送らずに次へ のチャンクは + 確認待ちもここに入る) + 合計 このチャンクの推論を始めてから次のチャンクの推論を始めるまで (最後のチャンクは記録の終わりまで) +試行全体の 合計 と 中央値 (推論は 2 回目からの中央値も) を出す. + +中継点ごと: + --flow の試行 log.jsonl の vias[].flow から, 送ってから次を実際に送るまで (send_gap_s. 最後の点は着くまで) の秒と + 進んだわけ + --flow でない試行 recording/states.csv の send_angles の時刻から, 送ってから次を送るまで (最後の点は送り終わりまで) の秒 + (どちらも 1 チャンク 1 行. 2 つ以上の試行を渡すと, 最後に試行ごとの比べる表を出す) +""" +import argparse +import csv +import json +import statistics +import sys +from pathlib import Path + +RUNS_ROOT = Path("/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy") +ADVANCE_TEXT = {"near": "近", "time": "時", "arrived": "着", "not_arrived": "未", "send_failed": "失"} + + +def resolve_dir(arg: str) -> Path: + p = Path(arg) + if (p / "log.jsonl").exists(): + return p + q = RUNS_ROOT / arg + if (q / "log.jsonl").exists(): + return q + raise FileNotFoundError(f"log.jsonl がない: {arg}") + + +def load_log(run_dir: Path): + """log.jsonl → (頭の model 行 or {}, 推論の行の並び, そのほかの行の数 {type: n}).""" + model, chunks, others = {}, [], {} + for line in (run_dir / "log.jsonl").read_text().splitlines(): + if not line.strip(): + continue + try: + r = json.loads(line) + except json.JSONDecodeError: + others["(読めない行)"] = others.get("(読めない行)", 0) + 1 + continue + if r.get("type") == "model": + model = r + elif r.get("type") is None and "chunk" in r and "latency_s" in r: + chunks.append(r) + else: + k = r.get("type") or "?" + others[k] = others.get(k, 0) + 1 + return model, chunks, others + + +def load_sends(run_dir: Path): + """recording/states.csv の send_angles の時刻 (並べたもの). なければ [].""" + p = run_dir / "recording" / "states.csv" + if not p.exists(): + return [] + out = [] + with p.open(newline="") as f: + for r in csv.DictReader(f): + if r.get("event_type") == "send_angles": + try: + out.append(float(r["ts"])) + except (TypeError, ValueError): + pass + return sorted(out) + + +def last_time(chunks): + """最後のチャンクの記録の終わり (推論の行を書いた時刻か送り終わりの遅い方).""" + r = chunks[-1] + ts = [r.get("t"), r.get("t_exec_end")] + return max(x for x in ts if x is not None) + + +def chunk_rows(chunks): + """チャンクごとの {chunk, infer, confirm, moved, other, total, note}. 時刻のない値は None.""" + rows = [] + for n, r in enumerate(chunks): + start = r["t"] - r["latency_s"] + nxt = chunks[n + 1]["t"] - chunks[n + 1]["latency_s"] if n + 1 < len(chunks) else last_time(chunks) + total = nxt - start + infer = float(r["latency_s"]) + if r.get("t_exec_start") is not None and r.get("t_exec_end") is not None: + confirm = max(0.0, r["t_exec_start"] - r["t"]) + moved = r["t_exec_end"] - r["t_exec_start"] + else: + confirm = moved = None + other = total - infer - (confirm or 0.0) - (moved or 0.0) + note = [] + if r.get("grip_cmd") is not None: + note.append("爪→" + ("閉" if r["grip_cmd"] == 1 else "開")) + if r.get("initial_gripper_open"): + note.append("始めに爪を開いた") + for k, t in (("rewind_requested", "巻き戻し"), ("correction_requested", "補正"), + ("home_requested", "HOME"), ("unsafe", "安全停止")): + if r.get(k): + note.append(t) + if not r.get("executed") and moved is None and not note: + note.append("送っていない") + rows.append({"chunk": r["chunk"], "infer": infer, "confirm": confirm, "moved": moved, + "other": other, "total": total, "note": " ".join(note)}) + return rows + + +def via_waits(r, sends): + """1 チャンクの中継点ごとの (step, 秒, わけ). flow の記録があればそれ, なければ states.csv の送った時刻.""" + vias = r.get("vias") or [] + if any("flow" in v for v in vias): + out = [] + for v in vias: + fl = v.get("flow") + if fl: + # 次を実際に送った時刻があればそれまで (send_gap_s), なければ次へ進むと決めた時刻まで (wait_s) + w = fl.get("send_gap_s", fl.get("wait_s")) + out.append((v["step"], w, ADVANCE_TEXT.get(fl.get("advance"), fl.get("advance")))) + elif v.get("called"): + out.append((v["step"], None, "?")) + else: + out.append((v["step"], None, "送らず")) + return out, "flow" + a, b = r.get("t_exec_start"), r.get("t_exec_end") + if a is None or b is None or not sends: + return [], None + ts = [t for t in sends if a <= t <= b] + if not ts: + return [], None + marks = ts + [b] + return [(None, marks[k + 1] - marks[k], "") for k in range(len(ts))], "states" + + +def fmt(x, w=7): + return f"{'-':>{w}}" if x is None else f"{x:>{w}.2f}" + + +def med(xs): + xs = [x for x in xs if x is not None] + return statistics.median(xs) if xs else None + + +def report(run_dir: Path): + model, chunks, others = load_log(run_dir) + print(f"== {run_dir.name} ({run_dir})") + if model: + flow = model.get("flow") + how = ("記録なし (依頼 176 より前 = --flow なし・速さ 20)" if flow is None else + (f"--flow (近づく {model.get('flow_tol')}°)" if flow else "--flow なし")) + print(f" 送り方: {how} 速さ {model.get('speed', 20)} モデル {model.get('model')} 閉じる値 {model.get('grip_value')}") + if not chunks: + print(" 推論の行がない") + return None + rows = chunk_rows(chunks) + sends = load_sends(run_dir) + print(f" {'チャンク':>6} {'推論':>7} {'確認待ち':>7} {'動いた':>7} {'その他':>7} {'合計':>7} 中継点ごと (秒) メモ") + flow_seen = False + for r, row in zip(chunks, rows): + waits, src = via_waits(r, sends) + if src == "flow": + flow_seen = True + vtxt = " ".join(f"{'-' if s is None else s}:{fmt(w, 0) if w is not None else '-'}{m}" for s, w, m in waits) + elif src == "states": + vtxt = " ".join(f"{w:.2f}" for _, w, _ in waits) + else: + vtxt = "" + print(f" {row['chunk']:>6} {fmt(row['infer'])} {fmt(row['confirm'])} {fmt(row['moved'])} " + f"{fmt(row['other'])} {fmt(row['total'])} {vtxt} {row['note']}") + tot = {k: sum(x[k] for x in rows if x[k] is not None) for k in ("infer", "confirm", "moved", "other", "total")} + mid = {k: med([x[k] for x in rows]) for k in ("infer", "confirm", "moved", "other", "total")} + print(f" {'合計':>6} {fmt(tot['infer'])} {fmt(tot['confirm'])} {fmt(tot['moved'])} {fmt(tot['other'])} {fmt(tot['total'])}") + print(f" {'中央値':>5} {fmt(mid['infer'])} {fmt(mid['confirm'])} {fmt(mid['moved'])} {fmt(mid['other'])} {fmt(mid['total'])}") + infer2 = med([x["infer"] for x in rows[1:]]) + all_waits = [] + for r in chunks: + w, src = via_waits(r, sends) + all_waits += [x[1] for x in w if x[1] is not None] + print(f" 推論: 1 回目 {rows[0]['infer']:.2f} 秒 / 2 回目からの中央値 {fmt(infer2, 0)} 秒 " + f"中継点の間の中央値 {fmt(med(all_waits), 0)} 秒 ({'log.jsonl の flow' if flow_seen else 'states.csv の送った時刻' if all_waits else '記録なし'})") + if flow_seen: + print(" 中継点ごと: step:秒+わけ (近=近づいた 時=見込みの時間 着=最後の点が着いた 未=着かなかった 失=送れなかった)") + if others: + print(" そのほかの行: " + " ".join(f"{k} {v}" for k, v in sorted(others.items()))) + return {"name": run_dir.name, "flow": model.get("flow"), "speed": model.get("speed", 20), "n": len(rows), + "tot": tot, "mid": mid, "infer1": rows[0]["infer"], "infer2": infer2, "via": med(all_waits)} + + +def main(argv=None): + ap = argparse.ArgumentParser(description="実機推論の試行のチャンクごとの時間の内訳 (読むだけ)") + ap.add_argument("runs", nargs="+", help="試行のフォルダ (log.jsonl があるところ) かフォルダ名") + args = ap.parse_args(argv) + res = [] + for a in args.runs: + try: + d = resolve_dir(a) + except FileNotFoundError as e: + print(f"[ERROR] {e}") + return 1 + r = report(d) + print() + if r: + res.append(r) + if len(res) >= 2: + print("== 比べる (秒)") + print(f" {'試行':<22} {'送り方':<10} {'速さ':>4} {'チャンク':>6} {'推論計':>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['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'])}") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/pi05/policy_warmup.py b/pi05/policy_warmup.py new file mode 100644 index 0000000..2be519d --- /dev/null +++ b/pi05/policy_warmup.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 +"""policy_warmup.py — π0.5 のサーバーに空の推論を 1 回だけ投げて捨てる (依頼 176. lab_start.sh の 5b). + +openpi のサーバーは最初の推論のときにコンパイルするので, 1 回目の推論が長い +(2026-11-13 の pi05_20261113_193254 で 57.5 秒. 2 回目からは 0.6〜1.8 秒). アームを動かす前に, +お手本と同じ形の観測 (上カメラの画像の大きさ・state 7 つ・prompt) で 1 回答えさせて, その待ちを先に済ませる. +答え (actions) は使わない・何も書き出さない・アームにも daemon にも触らない. + +観測: + image --raw-dir の最初のエピソードの frames/000000.jpg (お手本の 1 枚目. 学習画像と同じ大きさ) を RGB で + state そのエピソードの episode.npz の state[0] (7 つ. float32) + prompt --prompt (既定 run_policy_client.py の TASK) + +かかった秒を [WARM ] の 1 行で出す. 失敗したら (つながらない・答えがおかしい・つなぐところから --timeout 秒で +答えない・Ctrl-C) [WARM ] の 1 行を出して終了コード 1 (lab_start.sh は止めずに続ける. ほかの終了コードのときだけ +lab_start.sh が 1 行出す). + +Usage: + python3 pi05/policy_warmup.py --raw-dir /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes +""" +import argparse +import sys +import threading +import time +from pathlib import Path + +_HERE = Path(__file__).resolve().parent +sys.path.insert(0, str(_HERE)) + +try: + import cv2 + import numpy as np + + import policy_ws + from policy_ws import PolicyClient +except Exception as _e: # 読み込めなくても [WARM ] の 1 行と終了コード 1 にする (codex R2) + _msg = " / ".join(x.strip() for x in f"{type(_e).__name__}: {_e}".splitlines() if x.strip())[:300] + print(f"[WARM ] 空打ちに失敗 (読み込めない): {_msg}. 止めずに続けます (1 回目の推論が長くなる)", flush=True) + sys.exit(1) + +TASK = "put the red block in the white box" # run_policy_client.py の TASK と同じ +TIMEOUT_SEC = 300.0 # 答えを待つ上限 (1 回目は 57.5 秒だった. コンパイルは数分かかることもある) +CONNECT_WAIT_SEC = 30.0 # サーバーにつながるまで待つ上限 (トンネルは 5 で張れている) + + +def warm_observation(raw_dir: Path, prompt: str) -> dict: + """お手本の最初のエピソードの 1 枚目と state[0] で観測を作る. 作れなければ例外.""" + eps = sorted(p.parent for p in Path(raw_dir).glob("*/episode.npz")) + if not eps: + raise FileNotFoundError(f"お手本のエピソードがない: {raw_dir}") + ep = eps[0] + bgr = cv2.imread(str(ep / "frames" / "000000.jpg")) + if bgr is None: + raise FileNotFoundError(f"お手本の 1 枚目を読めない: {ep / 'frames' / '000000.jpg'}") + state = np.asarray(np.load(ep / "episode.npz")["state"][0], dtype=np.float32) + if state.shape != (7,): + raise ValueError(f"state の形が 7 つでない: {state.shape} ({ep})") + rgb = cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB) + return {"observation/image": rgb, "observation/state": state, "prompt": prompt}, ep + + +def connect_and_infer(host, port, obs, timeout): + """つなぐ (metadata を受け取るまで)・推論させる・閉じる を別のスレッドで行い, 全部で timeout 秒を超えたら + TimeoutError (スレッドは捨てる. daemon スレッドなので終わるときに残らない. codex R1: つないだあと metadata が + 来ないときも止まらない). 戻り値 (答え, 推論だけの秒).""" + box = {} + + def _run(): + policy = None + try: + policy = PolicyClient(host, port, wait_sec=CONNECT_WAIT_SEC) + t1 = time.time() + box["out"] = policy.infer(obs) + box["infer_s"] = time.time() - t1 + except BaseException as e: # noqa: BLE001 落ちたわけを本体へ渡す + box["err"] = e + finally: + if policy is not None: + policy.close() + th = threading.Thread(target=_run, daemon=True, name="warmup_infer") + th.start() + th.join(timeout) + if th.is_alive(): + raise TimeoutError(f"{timeout:.0f} 秒待っても答えない (つなぐところから)") + if "err" in box: + raise box["err"] + return box["out"], box["infer_s"] + + +def one_line(e): + """例外を 1 行に (サーバー側のエラーは改行を含む).""" + return " / ".join(x.strip() for x in f"{type(e).__name__}: {e}".splitlines() if x.strip())[:300] + + +def main(argv=None): + ap = argparse.ArgumentParser() + ap.add_argument("--host", default="localhost") + ap.add_argument("--port", type=int, default=8000) + ap.add_argument("--raw-dir", required=True, help="観測の形を取るお手本 (学習に使った raw_episodes)") + ap.add_argument("--prompt", default=TASK) + ap.add_argument("--timeout", type=float, default=TIMEOUT_SEC, help=f"答えを待つ上限の秒 (既定 {TIMEOUT_SEC:.0f})") + args = ap.parse_args(argv) + t0 = time.time() + # PolicyClient の「サーバー待ち」の行は出さない (失敗しても [WARM ] の 1 行だけにする. codex R1) + policy_ws.print = lambda *a, **k: None + try: + obs, ep = warm_observation(Path(args.raw_dir), args.prompt) + print(f"[WARM ] 空打ち: {args.host}:{args.port} に観測 (画像 {obs['observation/image'].shape[1]}x" + f"{obs['observation/image'].shape[0]}・state 7 つ・prompt) を 1 回投げて捨てます (お手本 {ep.name} の 1 枚目)", + flush=True) + out, infer_s = connect_and_infer(args.host, args.port, obs, args.timeout) + acts = np.asarray(out["actions"], dtype=float) + if acts.ndim != 2 or acts.shape[1] < 7 or not np.all(np.isfinite(acts)): + raise ValueError(f"答えの actions の形がおかしい: {acts.shape}") + print(f"[WARM ] 空打ち {infer_s:.1f} 秒 (つなぐまで含めて {time.time() - t0:.1f} 秒. " + f"答え {acts.shape[0]}x{acts.shape[1]} は捨てた)", flush=True) + return 0 + except KeyboardInterrupt: + print(f"[WARM ] 空打ちを Ctrl-C でやめました ({time.time() - t0:.1f} 秒). 止めずに続けます " + "(1 回目の推論が長くなる)", flush=True) + return 1 + except Exception as e: + print(f"[WARM ] 空打ちに失敗 ({time.time() - t0:.1f} 秒): {one_line(e)}. 止めずに続けます " + "(1 回目の推論が長くなる)", flush=True) + return 1 + finally: + del policy_ws.print + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/pi05/run_policy_client.py b/pi05/run_policy_client.py index e92eaed..0e20e4c 100644 --- a/pi05/run_policy_client.py +++ b/pi05/run_policy_client.py @@ -94,6 +94,19 @@ 巻き戻し・ゲームパッドの補正の閉じる指令はすべてこの値. log.jsonl の頭 (type=model) に grip_value を残す. 推論の終わりに, 閉じて 1 秒後の爪の電流 (recording/currents.csv の gripper_current の中央値) を 1 行で出す (表示だけ) +中継点を止まらずに流す (2026-11-13 依頼 176. --flow. 既定は今までどおり 1 点ごとに 3° の到着を待つ): + --via-stride の途中の点は 3° に入るまで待たず, 単発の send_angles を 1 回送ったあと関節角を読みながら + 「その点まで --flow-tol 度以内に近づいた」か「見込みの時間 (flow_expect_sec) がたった」で次の点を送る. + どちらでも送る間隔は FLOW_MIN_INTERVAL_SEC (0.4 秒) より短くしない (0.1 秒ごとの連打は DAY287 以前に + コントローラーに無視された. 実機で通った単発の送り間隔の最短が 0.40 秒 = 2026-11-13 の 193254). + チャンクの最後の点は今までどおり _send_and_wait_arrival (3° の到着確認) をしてから次の推論へ. + 途中の点は着いたかを見ないので, 途中の点の「未到達」で止まることはない (送信が失敗したときだけ今までどおり). + log.jsonl の中継点ごとに flow (送った時刻 t_send・次へ進むと決めた時刻 t_next (最後の点は着いた時刻)・ + 次を実際に送った時刻 t_next_sent と send_gap_s・進んだわけ advance・待った秒 wait_s) を残す. + 送る直前の関節角は今までどおり毎回読み直す (待つ間に読んだ値で apply_limits をかけない). + --speed N: 推論のチャンクを送る速さ (既定 20 = 今までと同じ. settle の送り直しも同じ速さ). + 補正 (キー・ゲームパッド)・巻き戻し・HOME の送り方と速さは変えない. log.jsonl の頭 (type=model) に flow・flow_tol・speed + 待ち時間 (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 @@ -149,6 +162,7 @@ if "pymycobot" not in sys.modules: sys.modules["pymycobot"] = _pkg; sys.modules["pymycobot.mycobot"] = _mod import camera_resolver # noqa: E402 +import j0_sweep_scan # noqa: E402 --speed のときだけ _send_and_wait_arrival の SPEED をその間だけ替える (依頼 176) 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, @@ -199,6 +213,16 @@ SETTLE_WAIT_SEC = 0.8 # 止まらなくてもここで待つのをや # --state-from-target で「直前に送った目標」を渡すのをやめる差 (LOG359). # 腕がどこかに当たって止まっているときに, 目標だけが先へ進むのを防ぐ. STATE_TARGET_MAX_GAP_DEG = 6.0 +# ---- 中継点を止まらずに流す (--flow. 依頼 176) ---- +CHUNK_SPEED = SPEED # 推論のチャンクを送る速さ (--speed. 既定 20 = 今までと同じ. 補正・巻き戻し・HOME は SPEED のまま) +SPEED_RANGE = (1, 100) # --speed に受ける値 (send_angles の速さの範囲) +FLOW_TOL_DEFAULT = 5.0 # 途中の点にこれだけ近づいたら次を送る (度). 中継点 1 つの動きは 3〜9° (193254) なので半分ほど +FLOW_MIN_INTERVAL_SEC = 0.4 # 途中の点を送ってから次を送るまでの下限. 0.1 秒ごとの連打は DAY287 以前に無視された. + # 193254 の単発の送り間隔 (どれも着いた) の最短が 0.40 秒 +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 秒ほどかかる) DATA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data") LOG_ROOT = DATA_ROOT / "runs" / "pi05_policy" RECORDER = ("127.0.0.1", 5556) @@ -412,7 +436,7 @@ def settle_target(target, label, tol, retries): retries は **settle で送る送信回数の上限** (codex A5). 送り直しは _send_and_wait_arrival ではなく, その中で使っている 1 回だけ送る層 - daemon_send_angles(..., speed=SPEED, wait=SEND_WAIT) を直接呼ぶ. + daemon_send_angles(..., speed=CHUNK_SPEED, wait=SEND_WAIT) を直接呼ぶ (速さはチャンクと同じ --speed. 依頼 176). 中で再送しないので送信回数を正確に数えられる (上限を超えない). 送ったあとは read_until_still() で止まるのを待ってから測る. チャンク本体の送信は今までどおり _send_and_wait_arrival のまま. @@ -456,7 +480,7 @@ def settle_target(target, label, tol, retries): "sent": True, "still": None, "max_err_after": None, "actual": None} sends += 1 try: - daemon_send_angles(tgt.tolist(), speed=SPEED, wait=SEND_WAIT) + daemon_send_angles(tgt.tolist(), speed=CHUNK_SPEED, wait=SEND_WAIT) except Exception as e: entry["sent"] = False entry["error"] = f"{type(e).__name__}: {e}" @@ -499,6 +523,66 @@ def settle_target(target, label, tol, retries): return out, sends +def send_chunk_arrival(target, tag, observer=None): + """推論のチャンクの 1 点を送って 3° の到着を待つ (_send_and_wait_arrival. 依頼 176). + 速さが今までと同じ (CHUNK_SPEED == SPEED) なら今までと同じ呼び方. --speed で違うときだけ, + _send_and_wait_arrival が使う j0_sweep_scan.SPEED をこの 1 回の間だけ替えて戻す (送り方・再送・判定は同じ). + observer は --flow のときだけ渡す (送った時刻と着いた時刻を受け取る).""" + kw = {} if observer is None else {"observer": observer} + if CHUNK_SPEED == SPEED: + return _send_and_wait_arrival(target, tag, 3.0, send_wait_sec=SEND_WAIT, **kw) + old = j0_sweep_scan.SPEED + j0_sweep_scan.SPEED = CHUNK_SPEED + try: + return _send_and_wait_arrival(target, tag, 3.0, send_wait_sec=SEND_WAIT, **kw) + finally: + j0_sweep_scan.SPEED = old + + +def flow_expect_sec(max_delta_deg, speed): + """--flow で途中の点を送ってから次を送るまでの見込みの時間 (依頼 176). + 動き始めるまで FLOW_START_SEC + 一番大きく動く関節の角度 ÷ 速さ (speed 20 で FLOW_DEG_PER_SEC_AT_20, speed に比例). + FLOW_MIN_INTERVAL_SEC より短くせず, FLOW_MAX_WAIT_SEC より長くしない.""" + rate = FLOW_DEG_PER_SEC_AT_20 * float(speed) / float(SPEED) + est = FLOW_START_SEC + abs(float(max_delta_deg)) / rate + return min(FLOW_MAX_WAIT_SEC, max(FLOW_MIN_INTERVAL_SEC, est)) + + +def flow_wait(target, t_send, expect_s, tol, read_fn=None): + """--flow: 途中の点を送ったあと, 次の点を送ってよくなるまで関節角を読みながら待つ (依頼 176. 送らない・止めない). + + 次へ進むのは (送ってから FLOW_MIN_INTERVAL_SEC がたっている) かつ + near: 最後に読めた角度が target から tol 度以内 / time: 送ってから expect_s 秒たった + 読めない回は数えるだけ (古い値で near にしない). 戻り値は log.jsonl の中継点の flow に入れる dict と, + 最後に読めた角度 (ndarray or None) と読んだ時刻.""" + read_fn = read_fn or read_angles_once + tgt = np.asarray(target, dtype=float)[:6] + reads = fails = 0 + err = last = last_t = None + while True: + a = read_fn() + now = time.time() + if a is None or not wr._is_valid_angles(a): + fails += 1 + err = None # 読めなかった回は near にしない (前の値で決めない) + else: + reads += 1 + last, last_t = np.asarray([float(x) for x in a[:6]]), now + err = float(np.max(np.abs(tgt - last))) + el = now - t_send + if el >= FLOW_MIN_INTERVAL_SEC and err is not None and err <= tol: + advance = "near" + break + if el >= expect_s: + advance = "time" + break + time.sleep(FLOW_POLL_SEC) + info = {"t_send": t_send, "t_next": now, "wait_s": round(now - t_send, 3), "advance": advance, + "expect_s": round(float(expect_s), 3), "tol": tol, + "err_deg": None if err is None else round(err, 2), "reads": reads, "read_fails": fails} + return info, last, last_t + + def note_close_value(value): """最後に送った閉じる値を覚える (締め直し r が次に送る値を決める. 依頼 152).""" global LAST_CLOSE_VALUE @@ -1197,6 +1281,25 @@ def apply_grip_args(args): f"締め直し r も {GRIP_VALUE} のまま (0 より下にしないので値を変えられない)")) +def apply_flow_args(args): + """--flow / --flow-tol / --speed を確かめて CHUNK_SPEED へ入れる (依頼 176). 送り方の 1 行を出す.""" + global CHUNK_SPEED + if not (SPEED_RANGE[0] <= args.speed <= SPEED_RANGE[1]): + sys.exit(f"[ERROR] --speed は {SPEED_RANGE[0]}〜{SPEED_RANGE[1]} の整数で指定する (指定: {args.speed})") + if not np.isfinite(args.flow_tol) or args.flow_tol <= 0: + sys.exit(f"[ERROR] --flow-tol は 0 より大きい数で指定する (指定: {args.flow_tol})") + CHUNK_SPEED = int(args.speed) + sp = f"速さ {CHUNK_SPEED}" + (" (既定)" if CHUNK_SPEED == SPEED else f" (--speed. 補正・巻き戻し・HOME は {SPEED} のまま)") + if not args.flow: + if CHUNK_SPEED != SPEED: # 既定 (--flow なし・速さ 20) では何も出さない (今までと同じ出力. codex R1) + print(f"[FLOW ] 中継点ごとに 3° の到着を待つ (今までどおり) {sp}") + elif args.via_stride <= 0: + print(f"[FLOW ] --flow だが --via-stride 0 なので途中の点がない (1 点を送って到着を待つ) {sp}") + else: + print(f"[FLOW ] --flow: 途中の点は {args.flow_tol:.1f}° 以内に近づくか見込みの時間で次を送る " + f"(間隔 {FLOW_MIN_INTERVAL_SEC:.1f}〜{FLOW_MAX_WAIT_SEC:.1f} 秒). 最後の点は 3° の到着を待つ {sp}") + + def decide_tilt_fix(frames, tilt_dir: Path, enabled: bool) -> dict: """始めの数コマ (生の画像) から, AI に渡す画像を回す角度を決める (依頼 144. 落ちない). @@ -1295,11 +1398,20 @@ def main(): ap.add_argument("--grip-value", type=int, default=None, help=f"爪の閉じる値 (既定 {GRIP_VALUE_DEFAULT}. 推論・補正の g / r / R・巻き戻し・ゲームパッドの閉じる指令に使う. " "依頼 152)") + ap.add_argument("--flow", action="store_true", + help="中継点 (--via-stride) の途中の点は 3° に入るまで待たず, 近づいたか見込みの時間がたったら次を送る. " + "チャンクの最後の点は今までどおり到着を確かめる (既定 off = 今までどおり. 依頼 176)") + ap.add_argument("--flow-tol", type=float, default=FLOW_TOL_DEFAULT, + help=f"--flow で途中の点にこれだけ近づいたら次を送る (度. 既定 {FLOW_TOL_DEFAULT:.0f}. " + f"送る間隔は {FLOW_MIN_INTERVAL_SEC:.1f} 秒より短くしない)") + ap.add_argument("--speed", type=int, default=SPEED, + help=f"推論のチャンクを送る速さ (既定 {SPEED} = 今までと同じ. 補正・巻き戻し・HOME は {SPEED} のまま. 依頼 176)") args = ap.parse_args() if args.dry_keys and (args.execute or args.auto): sys.exit("[ERROR] --dry-keys はドライラン用 (--execute / --auto と一緒には使えない)") apply_wait_args(args) apply_grip_args(args) + apply_flow_args(args) if not DATA_ROOT.exists(): sys.exit(f"[ERROR] SSD 未マウント: {DATA_ROOT}") @@ -1321,7 +1433,8 @@ def main(): log_f.write(json.dumps({"type": "model", "model": args.model, "raw_dir": str(args.raw_dir), "framing_raw_dir": str(framing_raw_dir(args)), "tilt_raw_dir": str(tilt_raw_dir(args)), "tilt_fix": not args.no_tilt_fix, - "grip_value": GRIP_VALUE, "host": args.host, "port": args.port, "t": time.time()}, + "grip_value": GRIP_VALUE, "flow": bool(args.flow), "flow_tol": args.flow_tol, + "speed": CHUNK_SPEED, "host": args.host, "port": args.port, "t": time.time()}, ensure_ascii=False) + "\n") log_f.flush() @@ -1724,6 +1837,7 @@ def main(): grip_opened = True rec["initial_gripper_open"] = True broke, final_target, final_sent, sent_any = False, None, False, False + flow_prev = None # --flow: 直前に送った途中の点の flow (次を実際に送った時刻を足す. 依頼 176) for vi, v in enumerate(plan): v_last = vi == len(plan) - 1 tag = (f"pi05_chunk{i:03d}" if args.via_stride <= 0 @@ -1732,6 +1846,7 @@ def main(): if args.via_stride > 0: # 送る直前に実測角を読み, そこから apply_limits と check_send_safe を # かけ直す (codex A2: 1 点だけ送る従来の方式と同じ基準にそろえる). + # --flow でも同じ (待つ間に読んだ値は使わない. 依頼 176 codex R1) qa = read_angles() if qa is None: print(" [WARN] 送る直前の関節角を読めない. " @@ -1761,15 +1876,65 @@ def main(): 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, send_wait_sec=SEND_WAIT) - 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())) + # 前の途中の点の flow に「次を実際に送った時刻」を足す (送った瞬間に. 依頼 176 codex R1・R2) + prev_fl, flow_prev = flow_prev, None + + def mark_next_sent(ts, prev_fl=prev_fl): + if prev_fl is not None and "t_next_sent" not in prev_fl: + prev_fl["t_next_sent"] = ts + prev_fl["send_gap_s"] = round(ts - prev_fl["t_send"], 3) + if args.flow and args.via_stride > 0 and not v_last: + # --flow の途中の点 (依頼 176): 単発の send_angles を 1 回送り, 着いたかは見ずに + # 近づいたか見込みの時間がたったら次の点へ (送る間隔は FLOW_MIN_INTERVAL_SEC 以上) + t_send = time.time() + mark_next_sent(t_send) + try: + daemon_send_angles(tgt.tolist(), speed=CHUNK_SPEED, wait=SEND_WAIT) + ok = True + except Exception as e: + print(f" [WARN] {tag} の送信に失敗: {type(e).__name__}: {e}") + ok = False + sent_any = True + sent_targets.append(np.asarray(tgt, dtype=float).copy()) + if ok: + fl, seen_q, _seen_t = flow_wait(tgt, t_send, flow_expect_sec(np.max(np.abs(vdelta)), + CHUNK_SPEED), args.flow_tol) + flow_prev = fl + else: + fl = {"t_send": t_send, "t_next": time.time(), "advance": "send_failed"} + fl["wait_s"] = round(fl["t_next"] - t_send, 3) + rec["vias"][vi].update(called=True, success=None if ok else False, retried=False, + actual_after=_fmt6(seen_q) if ok else None, flow=fl) + how = {"near": f"近づいた (残り {fl.get('err_deg')}°)", "time": "見込みの時間がたった" + + ("" if fl.get("err_deg") is None else f" (残り {fl['err_deg']}°)"), + "send_failed": "送れなかった"}[fl["advance"]] print(f" [VIA ] step {v['step']:2d} Δ=" + " ".join(f"{d:+5.1f}" for d in vdelta) - + (" 着いた" if ok else " 未到達")) + + f" 流した {fl['wait_s']:.2f} 秒で次へ: {how}") + else: + flow_obs = None + if args.flow and args.via_stride > 0: + # 最後の点 (--flow): 送った時刻と着いた時刻を残す (送り方は今までと同じ) + fl = {"advance": "not_arrived", "t_send": None, "t_next": None, "wait_s": None} + + def flow_obs(ts_send, ts_arrival, attempt, arrived, fl=fl): + mark_next_sent(ts_send) # 1 回目に送った時刻 (送る前の読み取りのあと) + fl.update(t_send=fl["t_send"] or ts_send, attempt=attempt, + advance="arrived" if arrived else "not_arrived", + t_next=ts_arrival if arrived else time.time()) + if fl["t_send"] is not None: + fl["wait_s"] = round(fl["t_next"] - fl["t_send"], 3) + ok, retried = send_chunk_arrival(tgt.tolist(), tag, observer=flow_obs) + 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())) + if flow_obs is not None: + rec["vias"][vi]["flow"] = fl + 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 diff --git a/pi05/test_chunk_timing.py b/pi05/test_chunk_timing.py new file mode 100644 index 0000000..3e046f7 --- /dev/null +++ b/pi05/test_chunk_timing.py @@ -0,0 +1,195 @@ +#!/usr/bin/env python3 +"""test_chunk_timing.py — chunk_timing.py と policy_warmup.py の確かめ (依頼 176. アームなし・サーバーなし). + + python3 pi05/test_chunk_timing.py # 全部 + python3 pi05/test_chunk_timing.py -v + +中身: + - chunk_timing: 作った log.jsonl で 推論・確認待ち・動いた・その他・合計 と合計・中央値を出す. --flow の試行は + 中継点ごとの待ち (vias[].flow), そうでない試行は states.csv の送った時刻から. 何も書き出さない + - policy_warmup: お手本の 1 枚目と state[0] で観測を作る. 答えれば終了コード 0 と [WARM ] の秒, + つながらない・落ちる・答えない (--timeout) は終了コード 1 と [WARM ] の 1 行 +一時フォルダだけを使う. +""" +import io +import json +import sys +import tempfile +import time +import unittest +from pathlib import Path +from unittest import mock + +import cv2 +import numpy as np + +_HERE = Path(__file__).resolve().parent +sys.path.insert(0, str(_HERE)) + +import chunk_timing as ct # noqa: E402 +import policy_warmup as pw # noqa: E402 + +T0 = 1794566000.0 + + +def chunk(i, start, latency, exec_start=None, exec_end=None, vias=None, **kw): + r = {"chunk": i, "t": start + latency, "latency_s": latency, "executed": exec_start is not None} + if exec_start is not None: + r.update(t_exec_start=exec_start, t_exec_end=exec_end) + if vias is not None: + r["vias"] = vias + r.update(kw) + return r + + +def write_run(d: Path, lines, sends=None): + d.mkdir(parents=True) + (d / "log.jsonl").write_text("".join(json.dumps(x) + "\n" for x in lines)) + if sends is not None: + (d / "recording").mkdir() + rows = ["ts,event_type,stage,j0,j1,j2,j3,j4,j5,speed,gripper_state,ts_send,ts_arrival,attempt,arrived,note"] + rows += [f"{t:.6f},send_angles,model,0,0,0,0,0,-30,20,,{t:.6f},,,,rpc_sec=0.1" for t in sends] + rows += [f"{T0:.6f},actual,model,0,0,0,0,0,-30,,,,,,,"] + (d / "recording" / "states.csv").write_text("\n".join(rows) + "\n") + + +def flow_via(step, t_send, wait, adv): + return {"step": step, "called": True, "flow": {"t_send": t_send, "t_next": t_send + wait, "wait_s": wait, + "advance": adv}} + + +class TestChunkTiming(unittest.TestCase): + def setUp(self): + self.td = Path(tempfile.mkdtemp()) + + def run_main(self, *dirs): + before = sorted(p for p in self.td.rglob("*")) + with mock.patch("sys.stdout", new=io.StringIO()) as out: + code = ct.main([str(d) for d in dirs]) + self.assertEqual(sorted(p for p in self.td.rglob("*")), before) # 何も書き出さない + return code, out.getvalue() + + def test_plain_run_uses_states_csv(self): + d = self.td / "pi05_20261113_000001" + write_run(d, [{"type": "model", "model": "m", "grip_value": 25, "t": T0}, + chunk(0, T0, 50.0, T0 + 60.0, T0 + 63.0, initial_gripper_open=True), + chunk(1, T0 + 63.5, 1.0, T0 + 65.0, T0 + 67.0), + chunk(2, T0 + 67.2, 0.5, home_requested=True)], + sends=[T0 + 61.0, T0 + 61.5, T0 + 62.0, T0 + 65.0, T0 + 66.0]) + rows = ct.chunk_rows(ct.load_log(d)[1]) + self.assertAlmostEqual(rows[0]["infer"], 50.0) + self.assertAlmostEqual(rows[0]["confirm"], 10.0) + self.assertAlmostEqual(rows[0]["moved"], 3.0) + self.assertAlmostEqual(rows[0]["total"], 63.5) + self.assertAlmostEqual(rows[0]["other"], 0.5) + self.assertAlmostEqual(rows[1]["total"], 3.7) + self.assertIsNone(rows[2]["moved"]) + waits, src = ct.via_waits(ct.load_log(d)[1][0], ct.load_sends(d)) + self.assertEqual(src, "states") + np.testing.assert_allclose([w for _, w, _ in waits], [0.5, 0.5, 1.0]) + code, out = self.run_main(d) + self.assertEqual(code, 0) + self.assertIn("記録なし (依頼 176 より前", out) + self.assertIn("1 回目 50.00 秒", out) + self.assertIn("始めに爪を開いた", out) + self.assertIn("HOME", out) + + def test_flow_run_and_compare(self): + a = self.td / "pi05_20261113_000002" + write_run(a, [{"type": "model", "flow": True, "flow_tol": 5.0, "speed": 30, "t": T0}, + chunk(0, T0, 1.0, T0 + 1.5, T0 + 3.0, + vias=[flow_via(2, T0 + 1.5, 0.41, "near"), flow_via(5, T0 + 1.91, 0.6, "time"), + {"step": 8, "called": False}, flow_via(9, T0 + 2.6, 0.3, "arrived")]), + chunk(1, T0 + 3.1, 0.6)]) + b = self.td / "pi05_20261113_000003" + write_run(b, [{"type": "model", "flow": False, "flow_tol": 5.0, "speed": 20, "t": T0}, + chunk(0, T0, 1.0, T0 + 1.5, T0 + 4.0), chunk(1, T0 + 4.1, 0.6)]) + code, out = self.run_main(a, b) + self.assertEqual(code, 0) + self.assertIn("--flow (近づく 5.0°)", out) + self.assertIn("2:0.41近 5:0.60時 8:-送らず 9:0.30着", out) + self.assertIn("== 比べる (秒)", out) + self.assertIn("flow", out.split("== 比べる")[1]) + self.assertIn("今まで", out.split("== 比べる")[1]) + + def test_missing_dir(self): + with mock.patch("sys.stdout", new=io.StringIO()) as out: + self.assertEqual(ct.main([str(self.td / "nothing")]), 1) + self.assertIn("log.jsonl がない", out.getvalue()) + + +class FakePolicy: + calls = [] + mode = "ok" + + def __init__(self, host, port, wait_sec=60.0): + if FakePolicy.mode == "refused": + raise ConnectionRefusedError("refused") + if FakePolicy.mode == "hang_connect": # つないだが metadata が来ない (codex R1) + time.sleep(5) + self.closed = False + + def infer(self, obs): + FakePolicy.calls.append(obs) + if FakePolicy.mode == "error": + raise RuntimeError("ポリシーサーバー側のエラー:\nTraceback ...\n ValueError: bad") + if FakePolicy.mode == "hang": + time.sleep(5) + return {"actions": np.zeros((10, 7))} + + def close(self): + self.closed = True + + +class TestWarmup(unittest.TestCase): + def setUp(self): + self.td = Path(tempfile.mkdtemp()) + ep = self.td / "raw" / "ep_b" + (ep / "frames").mkdir(parents=True) + cv2.imwrite(str(ep / "frames" / "000000.jpg"), np.full((216, 384, 3), 80, np.uint8)) + st = np.arange(14, dtype=np.float32).reshape(2, 7) + np.savez(ep / "episode.npz", state=st, actions=st) + FakePolicy.calls = [] + + def run_main(self, mode, extra=()): + FakePolicy.mode = mode + with mock.patch.object(pw, "PolicyClient", FakePolicy), \ + mock.patch("sys.stdout", new=io.StringIO()) as out: + code = pw.main(["--raw-dir", str(self.td / "raw")] + list(extra)) + return code, out.getvalue() + + def test_ok(self): + code, out = self.run_main("ok") + self.assertEqual(code, 0) + self.assertRegex(out, r"\[WARM \] 空打ち [0-9.]+ 秒") + obs = FakePolicy.calls[0] + self.assertEqual(obs["observation/image"].shape, (216, 384, 3)) + self.assertEqual(obs["observation/state"].dtype, np.float32) + np.testing.assert_allclose(obs["observation/state"], np.arange(7)) + self.assertEqual(obs["prompt"], "put the red block in the white box") + + def test_failures_return_1(self): + for mode in ("refused", "error"): + code, out = self.run_main(mode) + self.assertEqual(code, 1, mode) + fail = [x for x in out.splitlines() if "空打ちに失敗" in x] + self.assertEqual(len(fail), 1, out) # 失敗は 1 行 (改行を含むエラーも 1 行に) + self.assertIn("続けます", fail[0]) + self.assertEqual(len(out.splitlines()), 2, out) # 始めの 1 行 + 失敗の 1 行 + for mode in ("hang", "hang_connect"): # つないだあと metadata が来ないときも止まらない + t0 = time.time() + code, out = self.run_main(mode, ["--timeout", "0.5"]) + self.assertEqual(code, 1, mode) + self.assertLess(time.time() - t0, 3.0, mode) + self.assertIn("答えない", out) + + def test_no_episode(self): + with mock.patch.object(pw, "PolicyClient", FakePolicy), \ + mock.patch("sys.stdout", new=io.StringIO()) as out: + code = pw.main(["--raw-dir", str(self.td / "none")]) + self.assertEqual(code, 1) + self.assertIn("お手本のエピソードがない", out.getvalue()) + + +if __name__ == "__main__": + unittest.main() diff --git a/pi05/test_flow_send.py b/pi05/test_flow_send.py new file mode 100644 index 0000000..e66cee3 --- /dev/null +++ b/pi05/test_flow_send.py @@ -0,0 +1,359 @@ +#!/usr/bin/env python3 +"""test_flow_send.py — 中継点を止まらずに流す (--flow) と --speed の確かめ (依頼 176. アームなし). + + python3 pi05/test_flow_send.py # 全部 + python3 pi05/test_flow_send.py -v + +中身: + - flow_expect_sec (見込みの時間) と flow_wait (近づいた / 時間がたった / 読めない回 / 送る間隔の下限) + - run_policy_client を, 時間とともに目標へ動く偽の daemon で通す: + --flow なし: 中継点はどれも _send_and_wait_arrival (3° の到着を待つ). 途中の点を daemon_send_angles で送らない. + 中継点の記録に flow がない + --flow: 途中の点は daemon_send_angles を 1 回ずつ送り, 着くのを待たずに次へ (間隔は 0.4 秒以上). + 最後の点だけ _send_and_wait_arrival. 中継点ごとに送った時刻・次を送った時刻を残す + --speed 30: 途中の点は speed=30. 最後の点の _send_and_wait_arrival の間だけ j0_sweep_scan.SPEED が 30 (終われば 20) + log.jsonl の頭 (type=model) に flow・flow_tol・speed + - --flow なしで今までと同じ送り方・同じ log になることは test_rewind_correct.py の test_unchanged_without_b_c + (git HEAD の版と比べる) で見る +KIOXIA にもアームにも触らない (一時フォルダだけ). +""" +import builtins +import io +import json +import sys +import tempfile +import threading +import time +import unittest +from pathlib import Path +from unittest import mock + +import numpy as np + +_HERE = Path(__file__).resolve().parent +sys.path.insert(0, str(_HERE)) + +import run_policy_client as rpc # noqa: E402 +from test_rewind_correct import FakeCap, FakePolicy # noqa: E402 + + +class MovingArm: + """送られた目標へ rate 度/秒で (一番大きく動く関節に合わせて) 動く偽のアーム. + + daemon_send_angles: その時の位置から新しい目標へ動き始める (待たない) + send_wait (_send_and_wait_arrival の代わり): 送って, 着くまで待ってから (True, False). observer に時刻を渡す + """ + + def __init__(self, rate=25.0): + self.rate = rate + self.lock = threading.Lock() + self.start = np.array([0.0, 0, 0, 0, 0, -30]) + self.goal = self.start.copy() + self.t0 = time.time() + self.fast_sends = [] # (時刻, 目標, speed) daemon_send_angles + self.wait_sends = [] # (時刻, ラベル, 目標, その時の j0_sweep_scan.SPEED) _send_and_wait_arrival + + def pos(self, now=None): + now = time.time() if now is None else now + with self.lock: + d = self.goal - self.start + m = float(np.max(np.abs(d))) + if m <= 1e-9: + return self.goal.copy() + f = min(1.0, (now - self.t0) * self.rate / m) + return self.start + d * f + + def _go(self, target): + now = time.time() + p = self.pos(now) + with self.lock: + self.start, self.goal, self.t0 = p, np.asarray(target, dtype=float)[:6].copy(), now + + fail_reads = False # True の間は読めない (None) + + def get_angles(self, *a, **k): + if self.fail_reads: + return None + return [float(v) for v in self.pos()] + + def send_angles(self, angles, speed=20, wait=0, *a, **k): + self.fast_sends.append((time.time(), [round(float(v), 3) for v in angles], speed)) + self._go(angles) + return {"ok": True, "elapsed_ms": 1.0} + + def send_wait(self, target, label, tol, *a, observer=None, **k): + ts = time.time() + self.wait_sends.append((ts, label, [round(float(v), 3) for v in target], rpc.j0_sweep_scan.SPEED)) + self._go(target) + while float(np.max(np.abs(self.pos() - np.asarray(target, dtype=float)))) > tol: + time.sleep(0.01) + if observer is not None: + observer(ts, time.time(), 1, True) + return True, False + + +def run_flow(argv, td, arm, inputs=()): + raw = td / "raw" + if not raw.exists(): + (raw / "ep0").mkdir(parents=True) + st = np.zeros((20, 7), np.float32) + st[:, 0] = np.linspace(-40, 40, 20); st[:, 1] = np.linspace(0, 60, 20) + st[:, 4] = np.linspace(-80, 0, 20); st[:, 5] = -30 + np.savez(raw / "ep0" / "episode.npz", state=st, actions=st) + log_root = td / "runs" + it = iter(inputs) + patches = [ + mock.patch.object(rpc, "DATA_ROOT", td), mock.patch.object(rpc, "LOG_ROOT", log_root), + mock.patch.object(rpc.camera_resolver, "resolve_cameras", lambda **k: {"top": 0}), + mock.patch.object(rpc.cv2, "VideoCapture", FakeCap), + mock.patch.object(rpc, "load_reference", lambda p: (np.zeros((216, 384, 3), np.uint8), "ref")), + mock.patch.object(rpc, "measure_framing", lambda a, b: (0.0, 0.0, 0.9)), + mock.patch.object(rpc, "PolicyClient", FakePolicy), + mock.patch.object(rpc, "get_daemon_socket", lambda *a, **k: None), + mock.patch.object(rpc, "close_daemon_socket", lambda *a, **k: None), + mock.patch.object(rpc, "daemon_get_angles", arm.get_angles), + mock.patch.object(rpc, "daemon_send_angles", arm.send_angles), + mock.patch.object(rpc, "daemon_set_gripper", lambda *a, **k: None), + mock.patch.object(rpc, "daemon_set_gripper_value", lambda *a, **k: None), + mock.patch.object(rpc, "_send_and_wait_arrival", arm.send_wait), + mock.patch.object(rpc, "GRIPPER_SLEEP", 0.0), + mock.patch.object(builtins, "input", lambda *a: next(it)), + mock.patch.object(sys, "argv", ["run_policy_client.py", "--raw-dir", str(raw)] + list(argv)), + ] + for p in patches: + p.start() + try: + with mock.patch("sys.stdout", new=io.StringIO()) as out: + code = rpc.main() + finally: + for p in reversed(patches): + p.stop() + run = sorted(log_root.glob("pi05_*"))[-1] + lines = [json.loads(x) for x in (run / "log.jsonl").read_text().splitlines()] + return code, lines, out.getvalue() + + +BASE = ["--execute", "--auto", "--steps-per-chunk", "10", "--via-stride", "3", "--settle-retries", "0", + "--ignore-framing", "--max-chunks", "2"] + + +class TestFlowParts(unittest.TestCase): + def test_expect_sec(self): + self.assertAlmostEqual(rpc.flow_expect_sec(0.5, 20), rpc.FLOW_MIN_INTERVAL_SEC) # 下限 + self.assertAlmostEqual(rpc.flow_expect_sec(10.0, 20), 0.15 + 10.0 / 25.0) + self.assertAlmostEqual(rpc.flow_expect_sec(15.0, 30), 0.15 + 15.0 / 37.5) # speed に比例 + self.assertAlmostEqual(rpc.flow_expect_sec(200.0, 20), rpc.FLOW_MAX_WAIT_SEC) # 上限 + + def test_wait_near_respects_floor(self): + """着いていても送ってから 0.4 秒たつまでは次へ進まない.""" + t0 = time.time() + fl, q, tq = rpc.flow_wait([1, 0, 0, 0, 0, -30], t0, 1.0, 5.0, read_fn=lambda: [1, 0, 0, 0, 0, -30]) + self.assertEqual(fl["advance"], "near") + self.assertGreaterEqual(fl["wait_s"], rpc.FLOW_MIN_INTERVAL_SEC) + self.assertLess(fl["wait_s"], rpc.FLOW_MIN_INTERVAL_SEC + 0.3) + self.assertEqual(fl["err_deg"], 0.0) + np.testing.assert_allclose(q, [1, 0, 0, 0, 0, -30]) + + def test_wait_time_when_far(self): + t0 = time.time() + fl, q, _ = rpc.flow_wait([30, 0, 0, 0, 0, -30], t0, 0.6, 5.0, read_fn=lambda: [0, 0, 0, 0, 0, -30]) + self.assertEqual(fl["advance"], "time") + self.assertGreaterEqual(fl["wait_s"], 0.6) + self.assertEqual(fl["err_deg"], 30.0) + + def test_wait_failed_reads_are_not_near(self): + """読めない回は near にしない (時間で進む). 読めた角度もない.""" + t0 = time.time() + fl, q, tq = rpc.flow_wait([0, 0, 0, 0, 0, -30], t0, 0.5, 5.0, read_fn=lambda: None) + self.assertEqual(fl["advance"], "time") + self.assertIsNone(fl["err_deg"]) + self.assertIsNone(q) + self.assertEqual(fl["reads"], 0) + self.assertGreater(fl["read_fails"], 0) + + +class TestFlowClient(unittest.TestCase): + def setUp(self): + self.td = Path(tempfile.mkdtemp()) + time.sleep(1.1) # run のフォルダ名は秒単位なので重ならないように + + def tearDown(self): + rpc.CHUNK_SPEED = rpc.SPEED + self.assertEqual(rpc.j0_sweep_scan.SPEED, 20) # 替えた速さが残っていない + + def test_without_flow_waits_every_via(self): + arm = MovingArm(rate=200.0) + code, lines, out = run_flow(BASE, self.td, arm) + self.assertEqual(code, 0) + self.assertEqual(arm.fast_sends, []) # 途中の点を待たずに送ることはない + self.assertEqual([s[1] for s in arm.wait_sends[:4]], + ["pi05_chunk000_via02", "pi05_chunk000_via05", "pi05_chunk000_via08", "pi05_chunk000_via09"]) + self.assertTrue(all(s[3] == 20 for s in arm.wait_sends)) + model = lines[0] + self.assertEqual((model["type"], model["flow"], model["flow_tol"], model["speed"]), + ("model", False, rpc.FLOW_TOL_DEFAULT, 20)) + ch = [x for x in lines if x.get("type") is None and "chunk" in x] + self.assertTrue(all("flow" not in v for x in ch for v in x["vias"])) + self.assertNotIn("[FLOW ]", out) # 既定では今までと同じ出力 (何も足さない) + + def test_flow_does_not_wait_for_mid_vias(self): + """遅いアーム (5°/秒) で途中の点が 3° に入る前に次を送る. 最後の点だけ到着を待つ.""" + arm = MovingArm(rate=5.0) + code, lines, out = run_flow(BASE + ["--flow", "--flow-tol", "0.5", "--max-chunks", "1"], self.td, arm) + self.assertEqual(code, 0) + self.assertEqual([s[1] for s in arm.fast_sends], [[3.0, 1.5, 0.0, 0.0, 0.0, -30.0], + [6.0, 3.0, 0.0, 0.0, 0.0, -30.0], + [9.0, 4.5, 0.0, 0.0, 0.0, -30.0]]) + self.assertEqual([s[1] for s in arm.wait_sends], ["pi05_chunk000_via09"]) + times = [s[0] for s in arm.fast_sends] + [arm.wait_sends[0][0]] + gaps = np.diff(times) + self.assertTrue(np.all(gaps >= rpc.FLOW_MIN_INTERVAL_SEC - 1e-3), gaps) # 送る間隔の下限 + self.assertTrue(np.all(gaps < rpc.FLOW_MIN_INTERVAL_SEC + 0.25), gaps) # 見込みの時間 (下限 0.4 秒) で次へ + ch = [x for x in lines if x.get("type") is None and "chunk" in x][0] + mids, last = ch["vias"][:3], ch["vias"][3] + for v in mids: + self.assertEqual(v["flow"]["advance"], "time") # 0.5° には近づかないので見込みの時間で進む + self.assertIsNone(v["success"]) # 着いたかは見ていない + self.assertTrue(v["called"]) + self.assertGreater(v["flow"]["t_next"], v["flow"]["t_send"]) + self.assertIsNotNone(v["flow"]["err_deg"]) + for v in mids: + self.assertGreaterEqual(v["flow"]["t_next_sent"], v["flow"]["t_next"]) # 次を実際に送った時刻 + self.assertGreaterEqual(v["flow"]["send_gap_s"], rpc.FLOW_MIN_INTERVAL_SEC - 1e-3) + # 最後の途中の点の「次を送った時刻」は, 最後の点を実際に送った時刻 (observer の ts_send) (codex R2) + self.assertEqual(mids[-1]["flow"]["t_next_sent"], last["flow"]["t_send"]) + self.assertEqual(last["flow"]["advance"], "arrived") + self.assertTrue(last["success"]) + self.assertGreaterEqual(last["flow"]["t_send"], mids[-1]["flow"]["t_next"]) + self.assertTrue(ch["arrived"] and ch["executed"]) + self.assertEqual(lines[0]["flow"], True) + self.assertEqual(lines[0]["flow_tol"], 0.5) + + def test_flow_near_with_fast_arm(self): + arm = MovingArm(rate=200.0) + code, lines, out = run_flow(BASE + ["--flow"], self.td, arm) + self.assertEqual(code, 0) + ch = [x for x in lines if x.get("type") is None and "chunk" in x] + adv = [v["flow"]["advance"] for x in ch for v in x["vias"]] + self.assertEqual(adv, ["near", "near", "near", "arrived"] * 2) + self.assertEqual(len(arm.fast_sends), 6) + self.assertEqual(len(arm.wait_sends), 2) + self.assertIn("流した", out) + + def test_speed(self): + arm = MovingArm(rate=200.0) + code, lines, out = run_flow(BASE + ["--flow", "--speed", "30", "--max-chunks", "1"], self.td, arm) + self.assertEqual(code, 0) + self.assertEqual({s[2] for s in arm.fast_sends}, {30}) + self.assertEqual([s[3] for s in arm.wait_sends], [30]) # 最後の点を送る間だけ 30 + self.assertEqual(rpc.j0_sweep_scan.SPEED, 20) + self.assertEqual(lines[0]["speed"], 30) + + def test_speed_without_flow(self): + arm = MovingArm(rate=200.0) + code, lines, out = run_flow(BASE + ["--speed", "30", "--max-chunks", "1"], self.td, arm) + self.assertEqual(code, 0) + self.assertEqual(arm.fast_sends, []) + self.assertEqual([s[3] for s in arm.wait_sends], [30, 30, 30, 30]) + + def test_speed_does_not_reach_home(self): + """--speed 30 で動かしたあとの h (HOME) は今までどおり速さ 20 (補正・巻き戻しも SPEED のまま).""" + arm = MovingArm(rate=200.0) + code, lines, out = run_flow(["--execute", "--steps-per-chunk", "10", "--via-stride", "3", + "--settle-retries", "0", "--ignore-framing", "--max-chunks", "3", + "--flow", "--speed", "30"], self.td, arm, inputs=["", "h", ""]) + self.assertEqual(code, 0) + self.assertEqual([(s[1], s[3]) for s in arm.wait_sends], [("pi05_chunk000_via09", 30), ("pi05_home", 20)]) + + def test_flow_read_failure_before_send_stops_like_before(self): + """送る直前の読み取りに失敗したら, 待つ間に読めた値を使わず, 今までどおり残りの点を送らない.""" + arm = MovingArm(rate=200.0) + calls = {"n": 0} + orig_wait = rpc.flow_wait + + def wait_then_break(*a, **k): + out = orig_wait(*a, **k) + calls["n"] += 1 + if calls["n"] == 1: + arm.fail_reads = True # 1 つ目の途中の点を待ったあとから読めない + return out + with mock.patch.object(rpc, "flow_wait", wait_then_break): + code, lines, out = run_flow(BASE + ["--flow", "--max-chunks", "1"], self.td, arm) + self.assertEqual(code, 1) # --auto では今までどおり止まる + self.assertEqual(len(arm.fast_sends), 1) + self.assertEqual(arm.wait_sends, []) + self.assertIn("送る直前の関節角を読めない", out) + + def test_next_sent_kept_from_first_attempt(self): + """最後の点が再送になっても, 前の点の「次を送った時刻」は 1 回目に送った時刻のまま (codex R3).""" + arm = MovingArm(rate=200.0) + + def retry_wait(target, label, tol, *a, observer=None, **k): + t1 = time.time() + arm.send_angles(target) + if observer is not None: + observer(t1, None, 1, False) + time.sleep(0.05) + observer(time.time(), time.time(), 2, True) + return True, True + with mock.patch.object(arm, "send_wait", retry_wait): + code, lines, out = run_flow(BASE + ["--flow", "--max-chunks", "1"], self.td, arm) + ch = [x for x in lines if x.get("type") is None and "chunk" in x][0] + mid, last = ch["vias"][2], ch["vias"][3] + self.assertEqual(mid["flow"]["t_next_sent"], last["flow"]["t_send"]) + self.assertEqual(last["flow"]["attempt"], 2) + self.assertEqual(last["flow"]["advance"], "arrived") + + def test_next_sent_absent_when_not_sent(self): + """送る直前に読めず最後の点を送らなかったら, 前の点に「次を送った時刻」は付かない.""" + arm = MovingArm(rate=200.0) + orig_wait = rpc.flow_wait + n = {"k": 0} + + def wait_then_break(*a, **k): + out = orig_wait(*a, **k) + n["k"] += 1 + if n["k"] == 3: + arm.fail_reads = True # 3 つ目の途中の点のあとから読めない + return out + with mock.patch.object(rpc, "flow_wait", wait_then_break): + code, lines, out = run_flow(BASE + ["--flow", "--max-chunks", "1"], self.td, arm) + ch = [x for x in lines if x.get("type") is None and "chunk" in x][0] + self.assertNotIn("t_next_sent", ch["vias"][2]["flow"]) + self.assertIn("t_next_sent", ch["vias"][1]["flow"]) + self.assertEqual(arm.wait_sends, []) + + def test_speed_restored_on_error(self): + def boom(*a, **k): + self.assertEqual(rpc.j0_sweep_scan.SPEED, 30) + raise RuntimeError("x") + rpc.CHUNK_SPEED = 30 + with mock.patch.object(rpc, "_send_and_wait_arrival", boom): + with self.assertRaises(RuntimeError): + rpc.send_chunk_arrival([0, 0, 0, 0, 0, -30], "t") + + def test_bad_args(self): + for argv in (["--speed", "0"], ["--speed", "101"], ["--flow-tol", "0"], ["--flow-tol", "nan"]): + with self.assertRaises(SystemExit, msg=argv): + run_flow(BASE + argv, self.td, MovingArm()) + + +class TestSettleSpeed(unittest.TestCase): + def tearDown(self): + rpc.CHUNK_SPEED = rpc.SPEED + + def test_settle_uses_chunk_speed(self): + sent = [] + reads = iter([[0, 0, 0, 0, 0, -30]] * 2 + [[5, 0, 0, 0, 0, -30]] * 2) + rpc.CHUNK_SPEED = 30 + with mock.patch.object(rpc, "read_angles", lambda: np.array(next(reads), dtype=float)), \ + mock.patch.object(rpc, "SETTLE_POLL_SEC", 0.0), \ + mock.patch.object(rpc, "daemon_send_angles", lambda a, speed, wait: sent.append(speed)), \ + mock.patch("sys.stdout", new=io.StringIO()): + out, n = rpc.settle_target([5, 0, 0, 0, 0, -30], "x", 1.0, 1) + self.assertEqual(sent, [30]) + self.assertEqual(out["reason"], "converged") + + +if __name__ == "__main__": + unittest.main() diff --git a/pi05/test_rewind_correct.py b/pi05/test_rewind_correct.py index fbe945f..ad59e3e 100644 --- a/pi05/test_rewind_correct.py +++ b/pi05/test_rewind_correct.py @@ -604,7 +604,8 @@ class TestClientFlow(unittest.TestCase): self.assertEqual(len(l_old), len(l_new)) new_keys = ("t_exec_start", "t_exec_end", "sent_targets", "framing_raw_dir", # framing_raw_dir は依頼 137 "tilt_raw_dir", "tilt_fix", "measured_on", "rotate_deg", # 依頼 144 - "grip_value") # 依頼 152 + "grip_value", # 依頼 152 + "flow", "flow_tol", "speed") # 依頼 176 (type=model の頭) # HEAD が依頼 25 より前なら new_keys は新しい方にだけある. 依頼 25 以降なら両方にある (両方から外す) drop = ("t", "latency_s", "reference") + new_keys so, sn = strip(l_old, drop=drop), strip(l_new, drop=drop) diff --git a/test_lab_start.sh b/test_lab_start.sh index e15f0bd..d3addf6 100755 --- a/test_lab_start.sh +++ b/test_lab_start.sh @@ -595,11 +595,38 @@ sleep 1 check "待っている間は終わらない" 'kill -0 $SPID 2>/dev/null' kill -TERM $SPID; wait $SPID; RC=$? check "TERM で終了コード 0" '[ $RC = 0 ]' -check "デーモン・HOME・爪・位置合わせ・推論の段に進まない (python3 を呼ばない)" '! has out.txt "=== 6." && ! has out.txt "=== 7." && ! has out.txt "=== 8." && [ ! -s "$T/py.log" ]' +check "デーモン・HOME・爪・位置合わせ・推論の段に進まない (python3 は空打ちだけ)" '! has out.txt "=== 6." && ! has out.txt "=== 7." && ! has out.txt "=== 8." && [ "$(grep -vc "^python3 pi05/policy_warmup.py " "$T/py.log")" = 0 ]' +check "--serve-only でも空打ちまで行う (依頼 176)" 'has out.txt "=== 5b. 空打ち" && [ "$(grep -c "^python3 pi05/policy_warmup.py --port 8000 --raw-dir $T/data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes$" "$T/py.log")" = 1 ] && [ $(grep -n "=== 5b. 空打ち" "$T/out.txt" | cut -d: -f1) -lt $(grep -n "\[SERVE \] --serve-only" "$T/out.txt" | cut -d: -f1) ]' 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 の説明が入る (2〜54 行)" 'sed -n "2,54p" "$HERE/lab_start.sh" | grep -qF -- "--serve-only Pod を作って" && sed -n "2,54p" "$HERE/lab_start.sh" | grep -qF -- "--no-tilt-fix 傾きを" && sed -n "2,54p" "$HERE/lab_start.sh" | grep -qF -- "--grip-value N 爪の閉じる値"' +check "--help に --serve-only・--grip-value・--flow / --speed・5b の空打ちの説明が入る (2〜58 行)" 'sed -n "2,58p" "$HERE/lab_start.sh" | grep -qF -- "--serve-only Pod を作って" && sed -n "2,58p" "$HERE/lab_start.sh" | grep -qF -- "--no-tilt-fix 傾きを" && sed -n "2,58p" "$HERE/lab_start.sh" | grep -qF -- "--grip-value N 爪の閉じる値" && sed -n "2,58p" "$HERE/lab_start.sh" | grep -qF -- "--flow / --speed N run_policy_client.py に" && sed -n "2,58p" "$HERE/lab_start.sh" | grep -qF -- "# 5b. 空打ち" && grep -qF "sed -n '"'"'2,58p'"'"'" "$HERE/lab_start.sh"' + +echo "== 空打ち (依頼 176): トンネルのあと・アームの前に 1 回. 失敗しても止めずに続ける" +setup; state $ID_OLD; pod 200 "$RUNNING"; probe 1 1 1 1 1; run +check "空打ちを安全範囲と同じお手本で 1 回呼び, そのあとで推論のクライアント" '[ "$(grep -c "^python3 pi05/policy_warmup.py --port 8000 --raw-dir $T/data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes$" "$T/py.log")" = 1 ] && [ "$(sed -n 1p "$T/py.log" | cut -d" " -f2)" = pi05/policy_warmup.py ] && [ "$(sed -n 2p "$T/py.log" | cut -d" " -f2)" = pi05/run_policy_client.py ]' +check "空打ちはトンネル (5) のあと" '[ $(grep -n "=== 5. Mac の 8000 番" "$T/out.txt" | cut -d: -f1) -lt $(grep -n "=== 5b. 空打ち" "$T/out.txt" | cut -d: -f1) ] && has out.txt "5b. 空打ち (1 回目の推論の待ちを先に済ませる)"' +check "--flow・--speed を付けなければクライアントに渡さない" '! grep -q -- "--flow\|--speed" "$T/py.log" && ! has out.txt "[FLOW ]"' +rm -rf "$T" +setup; state $ID_OLD; pod 200 "$RUNNING"; probe 1 1 1 1 1 +printf '#!/bin/bash\necho "python3 $*" >> "%s/py.log"\ncase "$1" in pi05/policy_warmup.py) echo "[WARM ] 空打ちに失敗 (偽物)"; exit 1 ;; esac\n' "$T" > "$T/bin/python3" +run +check "空打ちが失敗しても終了コード 0 で推論のクライアントまで進む" '[ $RC = 0 ] && has py.log "pi05/run_policy_client.py" && has out.txt "Pod は残しています"' +check "失敗の行は空打ちが出した 1 行だけ (lab_start.sh は重ねない)" '[ "$(grep -c "^\[WARM \]" "$T/out.txt")" = 1 ] && has out.txt "[WARM ] 空打ちに失敗 (偽物)"' +rm -rf "$T" +setup; state $ID_OLD; pod 200 "$RUNNING"; probe 1 1 1 1 1 +printf '#!/bin/bash\necho "python3 $*" >> "%s/py.log"\ncase "$1" in pi05/policy_warmup.py) exit 3 ;; esac\n' "$T" > "$T/bin/python3" +run +check "空打ちが何も出さずに落ちた (終了コード 3) ら lab_start.sh が 1 行出して続ける" '[ $RC = 0 ] && has py.log "pi05/run_policy_client.py" && has out.txt "[WARM ] 空打ちが終了コード 3 で終わりました. 止めずに続けます"' +rm -rf "$T" +setup; state $ID_OLD; pod 200 "$RUNNING"; probe 1 1 1 1 1; run --flow --speed 030 +check "--flow --speed 30 をクライアントにそのまま渡す" '[ $RC = 0 ] && grep -q -- "pi05/run_policy_client.py .* --flow --speed 30$" "$T/py.log" && has out.txt "[FLOW ] 推論の送り方: --flow --speed 30"' +rm -rf "$T" +for bad in 0 101 abc 1000 ""; do + setup; run --speed "$bad" + check "--speed '$bad' は受けない (終了コード 2・API を呼ばない)" '[ $RC = 2 ] && [ ! -s "$T/calls.log" ] && has out.txt "--speed"' + rm -rf "$T" +done echo echo "OK $OK / NG $NG"