--- title: "フィジカルAI研究室を作る Log#15 | mechArm 270 の失敗を巻き戻し、直して覚えさせる" slug: rewind-and-correct-pi05 date: 2026-10-29 day: 403 meta_description: "Physical Intelligenceのπ0.5で動かすmechArm 270の実機推論に、失敗した区間を巻き戻すキーと関節を1°ずつ直すキーを足し、直した動きだけを学習データにする仕組みを作った。実機ではまだ動かしていない。" tags: [フィジカルAI研究室, π0.5, Physical Intelligence, mechArm 270, 模倣学習, 巻き戻し, 介入, MicroFactory, Claude Code, 再現手順] series: "フィジカルAI研究室" --- Physical Intelligence の π0.5 で動かす小型ロボットアーム mechArm 270 の実機推論に、失敗した区間をアームの動きごと巻き戻すキーと、関節を1°ずつ直すキーを足し、直した動きだけを学習データとして取り出す仕組みを作りました。実機ではまだ動かしておらず、アームなしのテスト37件と、別の AI によるコードレビューを通ったところまでです。 π0.5（パイレイテンゴ）は、米国の [Physical Intelligence](https://www.pi.website/) 社が公開しているロボット用の AI のモデルです。上から見たカメラの画像と6つの関節の角度を受け取り、次の1秒ぶんの関節の角度を返します。人が手で教えた19回の動き（お手本）で学習させてあり、前の回（LOG401）では、3か所に5回ずつ置いた赤いブロックを、15回中9回白い箱に入れました。 残りの6回は失敗です。爪がブロックの奥に下りてつかみ損ねる、つかんで持ち上げた瞬間に落とす、といった失敗でした。今のやり方では、失敗した回は最初からやり直すしかなく、どこをどう直せばよかったかは AI に伝わりません。 そこで参考にしたのが、米国の [MicroFactory](https://microfactory.com/) という会社の、卓上のロボットセル（箱の中でアームが作業する装置）です。紹介動画では、ロボットがミスをしたら時間をさかのぼるように動きを巻き戻し、AI に動かさせたまま人が横から手を入れて直し、その直しを AI が覚えて次から同じミスをしなくなる、という流れを見せています。これを、リーダーアーム（人が手で動かす操作用のアーム）を持っていない mechArm 270 で、キーボードを使ってまねる形にしました。 考えていく途中で、はじめの思い込みが1つ外れました。**記録に残った関節の角度の数字だけを直しても、学習データにはなりません。** 学習データは写真と関節の角度の組で、写真には直す前の動きが写っているからです。直した動きは、カメラに手が写らない所から操作して、実機を動かしたまま撮る必要があります。MicroFactory の動画でも、人が握る操作用のアームは、ロボットが作業する場所の外に置かれていました。 やったことの全体は次のとおりです。 | やったこと | なぜ | 結果 | |---|---|---| | MicroFactory の動画と、前の回の失敗の記録を見比べ、mechArm 270 での直し方を決めた | 失敗した回を最初からやり直すのではなく、失敗したところだけを直して AI に覚えさせるため | 推論1回ぶんの区切りで巻き戻し、カメラの外のキーボードから関節を直す形に決めた | | 実機推論のプログラムに、巻き戻し（b キー）・関節の補正（c キー）・1秒10コマの記録を足した | 失敗した区間に戻り、関節の一部だけを直して、その動きを写真と組で残すため | アームなしのテスト26件が通り、別の AI のレビューを3回通した | | つかんだ区間も巻き戻せるようにし、人の手が写るコマを学習データから外した | 失敗の中には、つかんだあとに落とす・箱にぶつかるものもあるため | テストは合わせて37件が通り、レビューの判定は「実機テストに進んでよい」 | | 直した動きを学習用のエピソードにするスクリプトを作った | 元のお手本19本に足して、π0.5 を学習し直すため | 形はお手本とそろった。実機の記録がまだないので、データセットはまだ作っていない | 【画像】LOG403_01_E5_chunks.jpg LOG401 の位置E の5回目を上のカメラで撮ったコマ。左上が動き始め、右上が爪を閉じる直前（推論4回目）、左下が閉じたあと（推論5回目）、右下が推論6回目。爪はブロックの奥側で閉じ、ブロックは爪の手前に残っている GPU を買うか借りるかの判断は、Runpod で実際に借りて学習させた費用をもとに、別のページにまとめています。 【バナー】gpu-buy-or-rent この回の作業は、Claude（設計と記録を一緒に組み立てた AI）と進めました。実機を動かすプログラムは [Claude Code](https://claude.com/claude-code)（プログラムを書く AI）に依頼箱を通して頼み、[OpenAI の codex](https://github.com/openai/codex)（別の会社のプログラムを書く AI）にコードを読んでもらってから受け取っています。アームはこの回は一度も動かしていません。ここから下は、依頼文・報告・レビュー・コードを全量で載せた作業ログです。効果の数字は、実機で試していないので、どれも見込みです。 ## 作業ログ ― MicroFactory の巻き戻しを、mechArm 270 でどう作るかを決めた ### MicroFactory は、卓上の箱の中でアームに手作業を覚えさせる会社 [MicroFactory](https://microfactory.com/) は、米国の卓上ロボットセルの会社です。箱の中に市販のアームと手先を入れ、人が操作用のアームで数回やって見せた動きを AI に覚えさせて、基板へのコネクタの差し込みや結束バンド締めのような手作業を任せます。この研究室では、LOG276 で仕組みを分解し、LOG329 で紹介動画を読みました。 この回で使う言葉は次のとおりです。 | 言葉 | 意味 | |---|---| | 巻き戻し | ロボットがミスしたとき、今までの動きを逆にたどって、ミスの手前の姿勢へ戻すこと | | クラッチ | AI に動かさせたまま、人が操作用のアームで横から手を入れて動きを直す仕組み。手を離すと、また AI だけで動く | | 介入 | 人が途中で手を入れて、ロボットの動きを直すこと | | リーダーアーム | 人が手で動かす操作用のアーム。本物のロボットがその動きをまねる | 動画は [YouTube](https://www.youtube.com/watch?v=DhcWVqxF_wo) にあり、巻き戻しとクラッチの話は 4:25 ごろからです。共有した2本の切り抜きから読み取れたのは、次の4つです。 - 人は、ロボットの作業場所の外に置いた小さなリーダーアームを握り、クラッチを使ってリアルタイムに動きを直している。手を離すと、ロボットはまた AI だけで続ける（字幕は「あとは私抜きで仕事を進めてください」） - リーダーアームは作業場所の外にあるので、ロボットを撮るカメラに人の手が写らない - 失敗した動きを、時間をさかのぼるように戻してからやり直させている。題材の差し込み作業では、戻す動きがそのまま引き抜く動きになるので、部品の状態もほぼ元に戻る - 直した内容を覚えて、次からは同じ間違いを正す、と説明している。比べる相手として Physical Intelligence の論文の図が出ていて、字幕では、仕上げの学習だけで5〜10時間分のデータが要り、成功率は約85%と紹介されていた クラッチの中身は、映像からの推測です。押している間だけ人の手の動きが AI の動きに上乗せされ、離すと AI の動きだけに戻る仕組みに見えます。これは、関節の一部だけを補正する、という使い方と同じ形です。 ### 実機推論は、推論1回ぶんごとに止まる作りだった mechArm 270 の実機推論は、`pi05/run_policy_client.py` が受け持っています。AI に1回問い合わせると、次の1秒分の関節の角度が10ステップぶん返ってきます。この1回分の答えを、この記事ではチャンクと呼びます。プログラムは、チャンクを1つ受け取るたびに止まって、Enter で実行するかどうかを聞きます。 チャンクごとに、その時の写真1枚・実測の関節の角度・AI が返した10ステップ分の関節の角度・実際に送った中継点が、`log.jsonl` と `frames/` に残っていました。つまり、チャンクの区切りは、そのまま巻き戻せる区切りとして使えます。毎回 Enter で止まる作りなので、そこにキーを足せば、失敗したところで止めて巻き戻せます。 ### はじめの案と、動画を見て直した2点 最初に考えたのは、記録を後から直す方法でした。記録を読むビューアで失敗した区間を選び、その区間の関節の角度に補正を入れ、実機でもう一度再生して撮り直す、という順番です。ここで2つ、前提を置いていました。 1. 記録の関節の角度だけを直しても、写真には直す前の動きが写っているので、学習データにならない。直した動きは、実機で動かして撮り直す必要がある 2. アームは巻き戻せても、ブロックは巻き戻らない。押されたブロックは手で置き直す必要がある 動画を見て、この2つを直しました。 | はじめの前提 | 動画を見て直したこと | |---|---| | 直した動きは、いつも撮り直しが要る | 撮り直しが要るのは、手でアームを動かしたときと、後から数字だけ直したときだけ。カメラの外から操作して直せば、写真と関節の角度が最初から合っているので、直した区間をそのまま学習データに使える | | 巻き戻しても場面は戻らない | mechArm 270 の失敗の多くは、ブロックに触れる前、爪を閉じる所で起きている（LOG401 の位置E の2回・位置D の2回）。ここなら、アームを戻すだけで場面も戻る | ただし、2つ目には続きがありました。LOG401 の位置D では、ブロックをつかんで持ち上げた瞬間に落とす回が2回ありました。つかんだあとの失敗について聞くと、次の答えでした。 > 掴んだ後も問題あるよ。 > つかんだ後の失敗（掴んだブロックを落とす、白箱に入れる際に先にアームが上がり切らないで白箱にぶつかる） この2つに合わせて、つかんだ区間の巻き戻し方を足しています（依頼2の節）。 ### リーダーアームがない mechArm 270 では、キーボードをクラッチにする mechArm 270 には、リーダーアームがありません。そこで、推論のあとの確認（Enter で止まるところ）に、次の2つのキーを足すことにしました。 - b（巻き戻し）：直前に実行したチャンクで送った中継点を逆の順にたどって送り、そのチャンクを始めたときの姿勢へ戻す。続けて押すと、1チャンクずつさかのぼる - c（補正）：キーを押している間だけ、選んだ関節を1°ずつ動かす。Enter で、その姿勢から AI に戻す キーボードはカメラの外にあるので、補正している間の写真に手は写りません。補正している間と再開したあとを1秒10コマで撮り、関節の角度と組にして残せば、直した区間をそのまま学習データに使えます。今は、チャンク1つにつき写真は1枚しか残っていないので、ここは足す必要がありました。 直した区間を、元のお手本19本に足して、π0.5 を学習し直します。お手本はうまくいった動きだけなので、AI は、外れかけたときにどう戻ればよいかを知りません。外れかけた姿勢から正しい動きへ戻るデータが入ることが、この仕組みのねらいです。 本物の形に近づけるなら、SO-101 のリーダーアームで、押している間だけ上乗せする形になります。ただ、それはキーボードの版で、直すのが間に合わないことを実機で測ってからにします。 ## 実機の失敗の例：LOG401 の位置E の5回目は、爪がブロックの奥で閉じた 最初に試す題材にするのは、LOG401 の位置E の5回目（記録 `pi05_20261029_150950`）です。推論4回目で爪を閉じましたが、爪はブロックの奥側で閉じ、ブロックは爪の手前に残りました。推論6回目では、AI が j0（根元の回転）を 13° あまり振る答えを返したので、送らずに h で HOME に戻しています。 `log.jsonl` のチャンクごとの値です（state は推論を始めたときの関節の角度、target はそのチャンクで最後に向かった目標）。 | 推論 | 推論の時間 | state（j0〜j5） | グリッパー | target（j0〜j5） | 爪の指令 | 実行 | |---|---|---|---|---|---|---| | 0 | 560ms | −0.17 +1.49 +1.84 +0.17 −0.26 −30.14 | 開 | +3.75 +18.55 +13.03 −1.87 −23.47 −27.19 | なし | した | | 1 | 688ms | +3.75 +18.55 +13.03 −1.87 −23.47 −27.19 | 開 | +8.04 +36.70 +25.40 −2.69 −51.96 −26.45 | なし | した | | 2 | 1535ms | +8.04 +36.70 +25.40 −2.69 −51.96 −26.45 | 開 | +9.41 +45.70 +30.28 −2.62 −62.92 −26.42 | なし | した | | 3 | 613ms | +9.41 +45.70 +30.28 −2.62 −62.92 −26.42 | 開 | +9.79 +56.04 +21.52 −2.52 −61.91 −26.58 | なし | した | | 4 | 606ms | +9.79 +56.04 +21.52 −2.52 −61.91 −26.58 | 開 | +9.80 +58.98 +19.37 −2.56 −61.62 −26.62 | 閉じる | した | | 5 | 554ms | +9.80 +58.98 +19.37 −2.56 −61.62 −26.62 | 閉 | +9.85 +51.42 +26.97 −2.55 −64.26 −26.78 | なし | した | | 6 | 1542ms | +9.85 +51.42 +26.97 −2.55 −64.26 −26.78 | 閉 | +23.11 +49.40 +18.11 +0.27 −60.64 −29.40 | なし | せずに HOME | 新しいキーがあれば、推論6回目の確認で次のように直せます（実機ではまだ試していません）。 1. b を押す → 推論5回目（爪を閉じたあと持ち上げた区間）を逆にたどり、推論5回目を始めた姿勢へ戻る 2. もう一度 b を押す → 推論4回目はつかんだ区間なので、持っているか（y）落としたか（n）を聞かれる。この回は何もつかめていないので n → その場でグリッパーを開いてから、推論4回目を始めた姿勢へ戻る 3. c を押し、j1・j2（肩とひじ）を少しずつ動かして、爪がブロックの手前に下りる向きへ直す → Enter で AI に戻す 4. 箱に入ったら h で終え、結果を聞かれたら y この流れで記録した回からは、推論4〜5回目の失敗した区間と巻き戻しの動きが除かれ、補正の区間と、そこから AI が続けた動きが学習データに残ります。 ## 依頼1：巻き戻し（b）・関節の補正（c）・1秒10コマの記録を足した 実機を動かすプログラムは、Claude Code（Anthropic のプログラムを書く AI）に頼みました。頼み方は、依頼箱という仕組みです。iCloud の「依頼箱」フォルダの「新規」に依頼文の Markdown を置くと、Mac で動いている見張り係のプログラムが Claude Code に渡し、終わると報告を「結果」に書き戻して、Mac に通知を出します。関節を動かす変更なので、Claude Code が codex にコードを読ませてから報告する決まりにしています。実機には流さない、というのも依頼の決まりです。 ねらい・変わったこと・見込める効果は次のとおりです。 - ねらい：失敗した回を最初からやり直すのではなく、失敗したチャンクまで戻って、関節の一部だけを直してから AI に続けさせる - 変わったこと：推論のあとの確認が「Enter=実行 / s=送らず次へ / h=HOMEに戻して終了 / q=終了」から、b（巻き戻し）と c（補正）を加えた形になった。実行している間は、上のカメラの写真と関節の角度が1秒10コマで残り、1コマごとに、AI の動き・巻き戻し中・補正中・HOME へ戻る間のどれかの印が付く - 見込める効果：失敗1回ごとに、外れかけた姿勢から戻る動きのデータが1本できる見込み。何本で成功率が変わるかは、学習し直したあとに同じ配置で数えて確かめる ### 依頼文 20261029_25_実機推論に巻き戻しと補正を足す.md ````markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 180 --- # 依頼：実機推論（pi05/run_policy_client.py）に「巻き戻し」と「関節の補正」を足し、直した動きを学習データにする（2026-10-29） ## ねらい MicroFactory の「失敗したら巻き戻し、モデルに動かさせながら人が直し、その直しを覚えさせる」を mechArm で試す。 リーダーアームはないので、カメラの外（キーボード）から関節の一部だけを直す。手はカメラに映らないので、直した区間はそのまま学習データに使える。 今の困りごとは「ブロックの位置にアームが行かない」で、ブロックに触れる前の失敗なので、アームを巻き戻せば場面もそのまま戻る。 今日の LOG14 の記録（`/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261029_150950/`）が実例。推論6回目で j0 が +13.5° 跳ぶ予測が出て h で戻した。 ## 今の作り（前提） - 実行コマンド：`python3 pi05/run_policy_client.py --raw-dir $RAW --steps-per-chunk 10 --via-stride 3 --settle-retries 0 --state-from-target --execute --ignore-framing` - 推論1回ごとに `Enter=実行 / s=送らず次へ / h=HOMEに戻して終了 / q=終了` で止まる - `log.jsonl` に推論1回ごとの state・measured_state・actions・vias・arrived が残り、`frames/` に推論1回につき写真1枚が残る ## A. 記録を足す（1秒10コマ） - `--execute` の間、上カメラの写真と実測の関節角・グリッパーを 1秒10コマで記録し続ける。お手本データ（`datasets/mecharm_redblock_box_teach_v1_trim2/` の元になった run）と同じ形で残し、`pi05/build_demo_episodes.py` がそのまま読めるようにする。同じ形にできない場合は、変換を build_demo_episodes.py 側に足す - 1コマごとに「その時の状態」を付ける：`model`（モデルの動き）／`rewind`（巻き戻し中）／`correct`（補正中）／`home` - 記録の置き場所は今と同じ `runs/pi05_policy/{run_stem}/` の中。新しい置き場所は作らない - 終わるとき（h・q のとき）に `結果: y=成功 / n=失敗 / Enter=記録しない >` を聞き、meta に残す ## B. 巻き戻し（b キー） - 推論のあとの確認で `b` を押すと、直前に実行した推論1回分の中継点を逆順にたどって送り、その推論を始めたときの姿勢（measured_state）へ戻す - 続けて `b` を押すと、さらに1回分ずつさかのぼる - 戻ったあとは、その姿勢からもう一度推論する - 巻き戻した推論の区間は log.jsonl に `rewound: true` を付けて残す（消さない） ## C. 関節の補正（c キー） - 推論のあとの確認で `c` を押すと補正モードに入る。補正モードでは Enter を待たずに1文字ずつキーを読む（termios など）。キーを押し続けると、Mac のキーリピートで繰り返し効く - キーの割り当て（案。使いやすい割り当てがあれば変えてよい。画面に一覧を出す） - `1`〜`6`：直す関節を選ぶ（j0〜j5） - `+`／`-`（または `.`／`,`）：選んだ関節を1刻み動かす。刻みの既定は 1° - `[`／`]`：刻みを 0.5° / 1° / 2° で切り替える - `g`：グリッパーの開閉 - `Enter`：補正を終えて、その姿勢からモデルの推論に戻る - `b`：補正を取り消して、補正を始めた姿勢へ戻る - 送り方は今と同じ send_angles（speed 20）と到着確認。今の範囲チェック（apply_limits・check_send_safe）をそのまま通す。**新しい確認や停止条件は足さない** - 補正した内容（関節・量・前後の角度・時刻）を log.jsonl に `type: "correction"` として残す ## D. 直した動きを学習データにする - 新しいスクリプト（例 `pi05/build_correction_episodes.py`）：A の記録から学習用のエピソードを作る - 使うのは、結果が「成功」の run だけ - `rewind` のコマと、巻き戻しで取り消した区間のコマは除く（その手前と、巻き戻したあとの続きだけをつなぐ） - `model` と `correct` のコマを使う。actions は「0.1秒後の state」（trim2 と同じ作り方） - 止まっている区間は `--trim-mid-idle` と同じ決まりで詰める - 出力：`datasets/mecharm_redblock_box_corr_v1/raw_episodes/`（お手本と同じ形・README.md に作り方と数を書く）。元のデータは読むだけ（上書き・移動・削除しない） - 補正の入ったエピソードが、元のお手本19本と一緒に学習に使えるか（形・キー・FPS・画像の大きさ）を確かめる ## E. 確かめ（実機なし） - `--execute` なし（ドライラン）でも b・c が動くようにし、アームなしで流れを試せるようにする - テストを足す：巻き戻しの中継点の並び（逆順・始めの姿勢で終わる）、補正の刻みと範囲チェック、D の区間の除き方（rewind と取り消した区間が入らない） - `b`・`c` を押さなければ、今までと同じ動き・同じログになることを確かめる（ログは項目が増えるだけ） ## 変えないもの - モデルのサーバー・送り方（speed 20・中継点の作り方）・今の範囲チェックの値 - 既存のデータセット・記録 ## 終わったら - 関節を動かす変更なので、codex のレビューを通してから報告する（code_review_request_template.md の形）。指摘は直すか、直さない理由を書く - 実機には流さない。報告に、エイイチが実機で最初に試す手順（LOG14 と同じ配置で、推論1回目のあとに b → c で j0 を数度動かす → Enter で再開 → 成功なら y）を書く - 個別のファイルを指定して add・commit する（`git add .` は使わない） ```` ### 依頼箱の見張り係に、r2r2r-scripts を足した 見張り係が自動で回すのは、設定ファイル `config.json` の `allowed_dirs` に書いた作業フォルダだけです。それまではサービスロボット導入シミュレーションの ServiceSim だけだったので、実機のプログラムがある r2r2r-scripts を足しました。 ```json { "inbox": "~/Library/Mobile Documents/com~apple~CloudDocs/デスクトップ/ADAPT DESIGN/じぶんの/PixelstoMotors/依頼箱", "allowed_dirs": [ "/Users/shimazueiichi/Robotics_UX/ServiceSim", "/Users/shimazueiichi/r2r2r-scripts" ], "claude": "/Users/shimazueiichi/.npm-global/bin/claude", "permission_mode": "auto", "default_limit_minutes": 180, "poll_seconds": 30, "settle_seconds": 15, "extra_args": [], "drop_env": [ "ANTHROPIC_API_KEY", "ANTHROPIC_AUTH_TOKEN" ], "set_env": { "ENABLE_CLAUDEAI_MCP_SERVERS": "false" } } ``` 見張り係は、起動したときに一度だけ設定を読みます。設定を変えても、動いている見張り係には効きません。1回目は、見張り係が動いている窓とは別の窓で入れ替えのスクリプト（`update.sh`）を打ったので、新しい見張り係は「もう動いています」で起動せず、古い設定のままの見張り係が依頼を拾って「失敗」に回しました。何も実行されていないので、コードは変わっていません。見張り係の窓を Ctrl+C で止めてから打ち直して、動き出しました。記録 `記録.log` の該当する行です。 ``` 2026-10-29T19:33:20 「止める」があるので一時停止中 2026-10-29T19:35:20 開始 20261029_25_実機推論に巻き戻しと補正を足す.md 2026-10-29T19:35:20 失敗 20261029_25_実機推論に巻き戻しと補正を足す.md → 結果/20261029_25_実機推論に巻き戻しと補正を足す_20261029_1935.md 2026-10-29T19:36:20 「止める」があるので一時停止中 2026-10-29T19:37:21 見張り係を止めました 2026-10-29T19:37:29 見張り係を起動 2026-10-29T19:37:29 開始 20261029_25_実機推論に巻き戻しと補正を足す.md 2026-10-29T20:27:13 完了 20261029_25_実機推論に巻き戻しと補正を足す.md → 結果/20261029_25_実機推論に巻き戻しと補正を足す_20261029_1937.md ``` 打ち直したときの Mac のターミナルの出力です。 ``` (base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % bash ~/r2r2r-scripts/inbox/update.sh 自動起動：登録済み（/Users/shimazueiichi/Library/LaunchAgents/com.pixelstomotors.inbox.plist） commit: c071dc6 依頼箱の見張り係を更新 依頼箱: /Users/shimazueiichi/Library/Mobile Documents/com~apple~CloudDocs/デスクトップ/ADAPT DESIGN/じぶんの/PixelstoMotors/依頼箱 あり 自動で回してよい場所: /Users/shimazueiichi/Robotics_UX/ServiceSim あり 自動で回してよい場所: /Users/shimazueiichi/r2r2r-scripts あり claude: /Users/shimazueiichi/.npm-global/bin/claude claude の版: 2.1.311 (Claude Code) 承認の扱い: auto ／ 拒否の決まり: /Users/shimazueiichi/r2r2r-scripts/inbox/settings.json 外す環境変数: ANTHROPIC_API_KEY, ANTHROPIC_AUTH_TOKEN ／ うち今この窓で設定されているもの: ANTHROPIC_API_KEY 2026-10-29T19:37:29 見張り係を起動 2026-10-29T19:37:29 開始 20261029_25_実機推論に巻き戻しと補正を足す.md ``` ### Claude Code の報告（50分・テスト26件・codex のレビュー3回） ````markdown # 結果：20261029_25_実機推論に巻き戻しと補正を足す - 状態：完了 - 始めた時刻：2026-10-29 19:37 - 終わった時刻：2026-10-29 20:27（50分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 644ac1ac-9284-49d4-a08b-328fa8add63b` ## Claude Code の報告 やったこと - `pi05/run_policy_client.py` の推論のあとの確認に、b（巻き戻し）と c（関節の補正）を足しました。b は直前の1回分（推論1回、または受け入れた補正1回）で実際に送った目標を逆の順にたどり、始めの姿勢へ戻してからもう一度推論します。続けて b を押すと1回分ずつさかのぼります。巻き戻した区間は log.jsonl に `type: "rewind"`、`rewound: true` の行を書き足して残します。もとの推論の行は消さず、書き換えもしません。 - c では Enter を待たずに1文字ずつ読みます。キーは 1〜6 で関節、+ と - で1刻み、[ と ] で刻みを 0.5° / 1° / 2° に切り替え、g でグリッパー、Enter で推論に戻る、b で取り消しです。送り方は推論と同じ（send_angles 速度20と到着確認）で、apply_limits と check_send_safe をそのまま通します。キーを押し続けても、1回送るたびに溜まったキーを捨てるので、送りすぎることはありません。補正の1刻みごとに `type: "correction"` を記録します。 - `--execute` の間は、上カメラと実測の関節角を1秒10コマで `recording/` に残します。形はお手本と同じで、1コマごとに状態（model / rewind / correct / home）が付きます。h か q で終えるときに結果を聞き、`{run_stem}.json` に残します。 - `--dry-keys` を足しました。アームにも daemon にも触らず、b と c の流れを試せます。 - 新しく作ったファイル：`policy_recorder.py`（記録）、`rewind_correct.py`（計算だけ）、`build_correction_episodes.py`（学習用エピソードを作る）、`test_rewind_correct.py`（テスト）。 - codex のレビューを3回通しました。1回目の必須指摘5件、2回目の必須指摘2件、3回目の軽い指摘1件を、すべて直しました。3回目の判定は「段階的な実機テストに進んでよい」です。記録の頻度と通信の遅れは実機で測るよう求められています。 - 実機には流していません。 結果 - アームなしのテストは26件すべて通りました。b と c を押さなければ、変更前の版と比べて送信・グリッパー・ログが同じになることも、3通りの動かし方で確かめました（ログは項目が増えるだけです）。 - 合成の記録から作った補正エピソードは、お手本 trim2（19本）と形がそろっています（npz のキー・型、FPS 10、画像 384×216）。お手本の作り方（build_demo_episodes）でも、この記録はそのまま読めました。 - `datasets/mecharm_redblock_box_corr_v1` はまだ作っていません。記録付きで成功した run がまだないためです。成功 run ができたら `python3 pi05/build_correction_episodes.py --check-with ` で作成と確認をします。 エイイチが実機で最初に試す手順（LOG14 と同じ配置） 1. `python3 pi05/run_policy_client.py --raw-dir $RAW --steps-per-chunk 10 --via-stride 3 --settle-retries 0 --state-from-target --execute --ignore-framing` 2. 推論1回目は Enter で実行する 3. 次の確認で b を押す。始めの姿勢（HOME 付近）へ戻り、もう一度推論が出ることを確かめる 4. 次の確認で c を押し、1 で j0 を選んで + か - を数回押す（1回で1°）。Enter で推論に戻る 5. そのまま Enter で進め、ブロックが箱に入ったら h を押し、結果を聞かれたら y 6. 終わったら `recording/top_meta.json` の actual_hz と effective_fps（どちらも10前後が目安）を見る。送信と到着確認が前より遅くなっていないかも確かめる 変えたファイル - pi05/run_policy_client.py（変更） - pi05/policy_recorder.py、pi05/rewind_correct.py、pi05/build_correction_episodes.py、pi05/test_rewind_correct.py（新規） - docs/review_request_20261029_rewind_correct.md（r2・r3 も）、docs/review_result_20261029_rewind_correct_codex.md（r2・r3 も）（レビュー記録） - コミット 2c860e5（ファイルを個別に指定して add）。作業記録は logs/20261029_2026.md 決めてほしいこと - グリッパーを動かした推論や補正は、b で巻き戻さず、断って聞き直すようにしました。ブロックを持ったまま開くと落とすためです。この方針でよいか。 - 補正を b で取り消すときは、グリッパーも始めの状態に戻します。ただし姿勢が戻りきれなかったときは、グリッパーは今のままにします。この方針でよいか。 - 依頼どおり、学習データには model と correct のコマだけを使い、HOME へ戻る区間は除いています。お手本には HOME へ戻る動きが入っていますが、この違いのままでよいか。 - 記録スレッドが関節角を1秒10回読むため、本体の送信や到着確認がそのぶん待たされることがあります。実機で測った結果、遅れが大きければ読む回数を減らすかどうか。 ```` ### codex のレビューは、1回目に必須の指摘が5件あった codex のレビューは3回でした。1回目の判定は「現状のまま実機テストへ進むのは見送り」で、必須の指摘が5件ありました。巻き戻しが途中で止まっても終わったことになる、グリッパーを2回押して元に戻すと動かしたことにならない、巻き戻しで除いた区間の写真が境目で混ざる、などです。2回目に必須2件、3回目に軽い指摘1件が出て、3回目の判定が「段階的な実機テストに進んでよい」でした。Claude Code が書いたレビューの依頼文と、codex の答えを全量で載せます。 #### レビューの依頼文 review_request_20261029_rewind_correct.md ````markdown # コードレビュー依頼：実機推論に巻き戻し (b) と関節の補正 (c) と 1 秒 10 コマの記録を足す 対象ファイル： - `pi05/run_policy_client.py`（変更。`git diff HEAD -- pi05/run_policy_client.py` で差分） - `pi05/rewind_correct.py`（新規。計算だけ） - `pi05/policy_recorder.py`（新規。上カメラ + 実測角の記録スレッド） - `pi05/build_correction_episodes.py`（新規。記録 → 学習用エピソード） - `pi05/test_rewind_correct.py`（新規。アームなしのテスト 16 件） --- ## このコードがやろうとしていること mechArm 270 Pi を π0.5 (Runpod) の出力で動かす Mac 側クライアント。推論 1 回ごとに Enter 確認で止まる。 今回、確認のところで b（直前の 1 回分を巻き戻す）と c（キーボードで関節を 1 刻みずつ直す）を足し、 `--execute` の間は上カメラと実測角を 1 秒 10 コマで残して、直した動きを学習データにする。 ## 設計仕様 1. b: 直前に実行した 1 回分（推論 1 回 / 受け入れた補正 1 回）で**実際に送った目標**を逆順にたどり、始めの姿勢（推論なら measured_state）で終わる。続けて b で 1 回分ずつさかのぼる。戻ったらその姿勢から再推論。グリッパーを動かした回は巻き戻さない（断って聞き直す）。各点は送る直前に実測から apply_limits と check_send_safe をかけ直し、通らない点が出たらそこで止める（送らない）。移動量の上限を超える区間は等分する。log.jsonl に `type: rewind, rewound: true` を追記（推論の行は消さない） 2. c: termios cbreak で 1 文字ずつ。1〜6 関節 / +- 1 刻み / [ ] 刻み 0.5・1・2° / g グリッパー / Enter 受け入れ / b 取り消し（始めの姿勢へ戻りグリッパーも戻す）。送り方は推論と同じ `_send_and_wait_arrival`（send_angles speed 20 + 到着確認）。apply_limits と check_send_safe を通し、通らない刻みは送らない。**新しい確認や停止条件は足さない**。1 刻み送ったあとに溜まったキーは tcflush で捨てる（キーリピートで送りすぎない） 3. 記録: 別スレッド 2 本（カメラ / get_angles 10Hz）。daemon のソケットは本体と共用なので `visual_servo_pick._daemon_request` を RLock で包む（install_daemon_lock）。上カメラは記録スレッドだけが読み、本体は「求めた時刻より後に撮れたコマ」を受け取る。states.csv の stage 列 / top_frame_log.csv の mode 列に model / rewind / correct / home 4. h / q で終えるとき結果 y/n/Enter を聞き `{run_stem}.json` に outcome と judgments を残す 5. `--dry-keys`: daemon に触らず仮の姿勢で b / c の流れを試す。b / c を押さなければ従来と同じ送信・同じログ（項目が増えるだけ。テストで git HEAD と比較済み） 6. build_correction_episodes: 成功の run だけ。model / correct のステップのみ。巻き戻した区間（区間開始〜巻き戻し終了）と取り消した補正を除いてつなぐ。actions は次の state。停止は build_demo_episodes と同じ決まりで詰める。戻りきれなかった巻き戻しがある run は使わない ## レビューしてほしいチェック項目 ### A. ロジックの正しさ A1. 巻き戻しの中継点（逆順・始めの姿勢で終わる・等分）と、送る前のかけ直しは正しいか A2. history に積む条件（送った目標が 1 つ以上 / グリッパーを動かした）と、b を断る条件は妥当か A3. 補正の目標の積み上げ（cur）と apply_limits の基準（実測）の組み合わせで、意図しない大きな動きが出ないか A4. 除く時間の窓（segment_t[0]〜t_end）で、巻き戻した区間の画像・姿勢が学習データに残らないか A5. 記録スレッドと本体の競合（ソケット・カメラ・CSV）、stop() の後始末 ### B. 副作用・破壊チェック B1. b / c を押さないとき、従来の送信・グリッパー・ログが変わっていないか B2. 記録スレッドの get_angles（10Hz）が、本体の到着確認や送信の時間を大きく変えないか B3. 既存のデータセット・記録を書き換えないか ### C. 暴走・安全リスク C1. キーリピートで送りすぎないか（tcflush / os.read） C2. 各関節の安全範囲（j4: -90〜+60、j1 上限 88）を補正・巻き戻しでも通っているか C3. 関節角が読めない (None) ときに送らずに済むか C4. Ctrl-C で端末の設定が戻るか・finally でアームを動かさないか ### D. 制約ルールの遵守 D1. send_coords() 不使用 / D2. 定数（SPEED 20・SEND_WAIT 1.0・HOME・J4_SAFE・J1_SAFE_MAX・max_move）不変 / D3. release_all_servos 不使用 / D6. KIOXIA マウントガード ## レビュー結果のフォーマット 各項目 ✅ OK / ⚠️ 要注意 / ❌ NG（行番号と修正案）。最後に実機テストに進んで良いか・直すべき項目。 ## 固定事項 HOME=[0,0,0,0,0,-30] / SPEED=20 / SEND_WAIT=1.0 / J4_SAFE=-90〜+60 / send_coords 禁止 / 失敗時の自動 HOME リトライ禁止 ```` #### codex の答え review_result_20261029_rewind_correct_codex.md（1回目） ````markdown **判定：❌ 現状のまま実機テストへ進むのは見送りです。** 対象5ファイルと `run_policy_client.py` の `git diff HEAD` を確認しました。ファイルは変更していません。 特に修正が必要な点は次のとおりです。行番号は現在のファイル基準です。 1. **❌ 巻き戻し未完了でも `finished=True` になる（A1・A4）** `run_policy_client.py:531–554, 575–579` `traverse()` は送信が未到達でも最後に `True` を返します。また、実測からの `apply_limits` で目標が変更されても、元の中継点や開始姿勢へ到達したか確認しません。結果として、戻りきれない run が `build_correction_episodes.py:163–168` の除外を通過します。 **修正案：** 送信処理を完走したかと、開始姿勢へ戻れたかを分け、最終実測と開始姿勢の差から `finished` を判定する。追加の自動HOMEやリトライは不要です。 2. **❌ グリッパー往復操作が巻き戻し禁止判定から漏れる（A2）** `run_policy_client.py:651–665, 1015–1022` 補正中に `g` を2回押すと、開始時と終了時の状態が一致するため `grip_changed=False` になります。関節も動かしていれば巻き戻し可能になり、グリッパーだけ往復した場合は history にも残りません。「グリッパーを動かした回は巻き戻さない」に違反します。 **修正案：** 最終状態の比較とは別に、一度でもグリッパー指令を出したフラグを保持し、history登録条件と巻き戻し禁止判定に使う。 3. **❌ 途中まで実行した推論が history から消える（A2）** `run_policy_client.py:1050–1079, 1134–1142` 中継点を送った後、次の実測取得に失敗すると `broke=True` になり、history登録に入りません。手動運転では次の確認へ進めるため、そこで `b` を押すと、部分実行した直前の推論を飛ばして古い区間を巻き戻します。 **修正案：** 送信済み目標があれば、途中中断でもその区間をログとhistoryに保存する。 4. **❌ 除外した区間の画像が境界付近で再混入する（A4）** `build_correction_episodes.py:190–202` stateの時刻を除外した後、画像は全フレームから最近傍を選びます。例えば除外終了が10.00秒、残ったstateが10.01秒の場合、9.99秒の除外区間内画像が選ばれ得ます。フレームの `mode` も選択に使っていません。 **修正案：** 画像候補にも除外窓とmodeフィルターを適用し、残った連続区間内で対応付ける。 5. **❌ 記録スレッド停止を保証せず資源を閉じる（A5）** `policy_recorder.py:120–150, 174–199` `join(timeout=5.0)` 後に生存確認せず、CSVとVideoWriterを閉じます。カメラの `read()` が戻らない場合、スレッドが後から `writer.write()` を実行する競合が残ります。本体の `cap.release()` とも競合します。 **修正案：** 停止完了を確認してから資源を閉じる。時間内に終了しなければ正常終了扱いにせず、記録異常を残す。 各チェック項目の結果です。 | 項目 | 判定 | 結果・修正案 | |---|---|---| | A1 | ❌ NG | 逆順・開始姿勢・計画上の等分は妥当。ただし完了判定に指摘1あり。 | | A2 | ❌ NG | グリッパー操作履歴と部分実行の登録に指摘2・3あり。 | | A3 | ⚠️ 要注意 | `run_policy_client.py:508–515, 638–644`。未到達でも `cur` を目標へ更新するため、次の1刻みが実測からは1刻み以上になる。max_moveは通るが、実測遅れを含むテストが必要。 | | A4 | ❌ NG | 時間窓そのものは仕様どおり。完了誤判定と画像選択に指摘1・4あり。 | | A5 | ❌ NG | 通常時のソケット排他・カメラ単独読み取り・CSV排他はある。停止時に指摘5あり。 | | B1 | ⚠️ 要注意 | 差分上、既存の送信・グリッパー処理は維持。記録の並行実行と画像取得待ちによる時間変化はある。HEAD比較テストは未実行。 | | B2 | ⚠️ 要注意 | `policy_recorder.py:52–54, 203–221`。記録側の通信中は本体もロック待ちになる。通信遅延時の待ち時間を計測すべき。 | | B3 | ⚠️ 要注意 | 元記録は読み取りのみ。ただし `build_correction_episodes.py:204–208, 347–367` は指定先の既存データセットを更新し、`--force` でエピソードを削除・再生成する。既存データセット保護が必須なら出力先制限を追加する。 | | C1 | ✅ OK | `run_policy_client.py:480–499, 650, 659`。端末では `os.read` と送信後の `tcflush` を使用。押し続ければ送信後に発生したリピートは次の操作になる仕様。 | | C2 | ✅ OK | 補正・巻き戻しとも `apply_limits` と `check_send_safe` を通る。 | | C3 | ⚠️ 要注意 | 関節角がNoneなら関節送信はしない。ただし補正取消では角度取得失敗後もグリッパー復元を実行する（`run_policy_client.py:675–683`）。「送らない」の対象範囲を明確にする。 | | C4 | ✅ OK | `KeyReader.__exit__` で端末設定を復元。本体のfinallyにアーム動作はない。 | | D1 | ✅ OK | 今回の追加処理に `send_coords()` 使用なし。 | | D2 | ✅ OK | 固定された速度・待機時間・HOME・安全範囲・max_move既定値の変更なし。 | | D3 | ✅ OK | 追加処理に `release_all_servos` 使用なし。 | | D6 | ⚠️ 要注意 | `run_policy_client.py:762–763` の既存ガードは維持。ただし存在確認なので、実マウント確認ではない。builderには明示的なテスト用迂回指定もある。 | `test_rewind_correct.py` は読みましたが、一時ファイルや動画を書き出すため、今回の読み取り専用環境では実行していません。既存テストの偽アームは目標へ即到達するため、未到達・部分実行・グリッパー往復・除外境界・停止タイムアウトの問題を検出できません。 **実機テスト前に、少なくとも指摘1〜5を修正し、それぞれの異常系をアームなしで確認してください。** ```` #### レビューの依頼文 review_request_20261029_rewind_correct_r2.md ````markdown # コードレビュー依頼 R2：実機推論の巻き戻し・補正・記録（R1 指摘の反映確認） 前回: docs/review_request_20261029_rewind_correct.md / 結果 docs/review_result_20261029_rewind_correct_codex.md ## R1 指摘への対応 | # | 対応 | |---|---| | 1 | `reached_pose()` を追加。巻き戻し・補正の取り消しとも、送り切った (traverse_completed) かつ最後の実測が始めの姿勢から 3° 以内 (reached_start) のときだけ finished=True。build_correction_episodes は finished でない巻き戻し/取り消しがある run を使わない | | 2 | 補正中に一度でも g を押したら grip_used=True（往復でも）。history 登録条件と b を断る条件に grip_used を使う | | 3 | 1 点でも送った推論は、途中で止まっても (broke) history に積む（partial=True）。グリッパーは broke では動かしていないので grip_changed=False | | 4 | 画像の候補 (top_frame_log の各コマ) にも、状態 (model/correct) と除く窓を同じ決まりで当ててから最近傍を選ぶ | | 5 | stop(): join のあと生きているスレッドがあれば mp4 を閉じず、カメラも本体で閉じない。top_meta.json の status=thread_not_stopped、{run_stem}.json の recording_enabled=false。build_correction_episodes は status!=ok の run を使わない。カメラスレッドは read 直後に stop_flag を見る | | A3 | 補正の 1 刻みが未到達なら、次の刻みは実測から数える（cur を実測に戻す） | | C3 | 取り消しで始めの姿勢に戻れなかったときはグリッパーを戻さない | | B3 | 出力先に pi05_policy 以外から作ったエピソードがあれば ABORT | | B1 | test_unchanged_without_b_c で git HEAD 版と送信・グリッパー・ログを比較（3 通り。一致） | | D6 | run_policy_client の既存ガードは今回の範囲外のため変更しない。builder は既定で bde.ensure_ssd() (is_mount) を通る。--data-root はテスト用の隠しオプション | | B2 | 記録スレッドの get_angles と本体のロック待ちは実機でしか測れない。top_meta.json に actual_hz を残す | 異常系テストを追加（計 22 件, すべて OK）: 戻りきれない巻き戻し / グリッパー往復 / 未到達の補正 / 戻れない取り消し / 記録スレッドが止まらない / 途中で止まった推論の巻き戻し / 除いた区間の画像が境界で入らない。 ## お願い R1 の 1〜5 が直っているか、直し方で新しい問題が出ていないかを、前回と同じ形式（✅/⚠️/❌・行番号・修正案）で。最後に実機テストに進んで良いか。 ```` #### codex の答え review_result_20261029_rewind_correct_codex_r2.md（2回目） ````markdown **判定：❌ 実機テストは、下記2点の修正後に進めるのが妥当です。** 依頼書、対象5ファイル、`run_policy_client.py` の `git diff HEAD` を確認しました。ファイルは変更していません。行番号は現在のファイル基準です。 1. **❌ 初回推論前の補正で閉じたグリッパーが、推論開始時に勝手に開く（A2・B1）** 対象：[run_policy_client.py:675](/Users/shimazueiichi/r2r2r-scripts/pi05/run_policy_client.py:675)、同1042–1053、1071–1079、1171–1175。 起動直後に `c → g → Enter` で閉じても、`grip_opened` は `False` のままです。その後の初回推論実行で、開始時の開放処理が走ります。補正で物を保持した場合、ここで落とす可能性があります。 また、この開放後も内部の `grip` は閉状態のままで、初期開放はhistoryの `grip_changed` にも含まれません。モデルへ渡す状態と実機が食い違い、グリッパーを動かした推論を巻き戻せる場合があります。 **修正案：** 補正でグリッパーを操作した場合は初期開放を処理済みとして扱う。実際の開閉指令、内部状態、historyの操作履歴を一致させる。初回実行前に `c/g` を使う回帰テストを追加する。 2. **❌ 戻れなかった操作をhistoryから失い、次の `b` が古い区間を巻き戻す（A1・A2）** 対象：[run_policy_client.py:1031](/Users/shimazueiichi/r2r2r-scripts/pi05/run_policy_client.py:1031)、同1044–1054。 巻き戻しは実行前に `history.pop()` し、`finished=False` でも削除されたままです。また、補正取消が失敗しても、その残存移動はhistoryに登録されません。 例えば、推論A→推論B→Bの巻き戻し失敗→再度 `b` と操作すると、Bの途中姿勢からAの経路へ進みます。補正取消失敗でグリッパーを閉じたままの場合も、以前の区間の判定だけで巻き戻せます。builderのrun除外では、実行中の経路の不整合は防げません。 **修正案：** 成功した巻き戻しだけhistoryから確定削除する。未完了の巻き戻し・補正取消は残存区間または巻き戻し不可の境界として保持し、古い区間へ飛ばないようにする。自動HOMEや自動リトライは不要です。 R1指摘の反映状況です。 | R1 | 判定 | 確認結果 | |---|---|---| | 1：巻き戻し完了判定 | ✅ | `run_policy_client.py:588–590, 717–718`。完走と開始姿勢への到達を併用。builderの163–168行で未完了runを除外。操作後のhistoryには上記指摘2が残る。 | | 2：グリッパー往復 | ✅ | 同680、1046–1052行。`grip_used` をhistory登録・巻き戻し禁止に使用。 | | 3：部分実行の保持 | ✅ | 同1168–1176行。中断した推論も送信済み目標があればhistoryに残す。 | | 4：除外画像の再混入 | ✅ | `build_correction_episodes.py:203–211`。画像候補にも時間窓とmode判定を適用してから最近傍を選ぶ。 | | 5：停止時の資源競合 | ✅ | `policy_recorder.py:129–157, 194–195`、本体1229行。生存スレッドがあればVideoWriter・カメラを閉じず、異常記録をbuilderから除外。 | その他のチェック結果です。 | 項目 | 判定 | 結果・修正案 | |---|---|---| | A1・A2 | ❌ | 上記指摘1・2を修正。逆順・等分・送信前の制限適用自体は維持。 | | A3 | ⚠️ | 本体664–669行。未到達後の再読取が成功すれば実測に戻るが、失敗すると古い目標が残る。次のmoveで読めた実測を基準に復帰できるよう、未到達状態を保持する。 | | A4 | ✅ | 除外窓・画像候補のフィルターは改善済み。 | | A5 | ⚠️ | 停止競合は改善。記録の健全性判定には追加余地あり。`policy_recorder.py:209–218` はVideoWriterの `isOpened()` を確認せず、書込例外も最終statusに反映しない。記録失敗フラグを残すとよい。 | | B1 | ✅ | b/cなしのHEAD比較テストを3通り実装済み。今回のレビューでは未実行。 | | B2 | ⚠️ | `policy_recorder.py:53–55`。通信中のロック待ちは残る。`actual_hz` に加え、本体の送信・到着確認の遅延を実機で確認する。 | | B3 | ✅ | builder361–366行で、既存の非pi05_policyエピソードを含む出力先を拒否。元記録は読み取りのみ。 | | C1 | ✅ | `os.read`・`tcflush` によるキー読み取りを維持。 | | C2 | ✅ | 補正・巻き戻しとも `apply_limits` と `check_send_safe` を通る。 | | C3 | ✅ | 実測取得失敗時は関節送信を行わず、取消未到達時はグリッパー復元もしない。 | | C4 | ✅ | 端末設定を復元し、finallyでアームを動かさない。 | | D1・D2・D3 | ✅ | 追加処理に `send_coords()`・サーボ解放なし。指定定数・既存待機時間の変更なし。 | | D6 | ⚠️ | 本体の既存存在確認は据え置き。builderは既定で `ensure_ssd()` を通る。今回の対象範囲内では説明どおり。 | `test_rewind_correct.py` の追加異常系とHEAD比較処理は読みました。一時ファイル・動画を書き出すため、読み取り専用環境ではテストを実行しておらず、「22件成功」は独立に再確認していません。 **実機テスト前に指摘1・2を修正し、「初回推論前のc/g」「巻き戻し失敗後のb」「補正取消失敗後のb」をアームなしで確認してください。** ```` #### レビューの依頼文 review_request_20261029_rewind_correct_r3.md ````markdown # コードレビュー依頼 R3：実機推論の巻き戻し・補正・記録（R2 指摘の反映確認） 前回: docs/review_result_20261029_rewind_correct_codex_r2.md ## R2 指摘への対応 | # | 対応 | |---|---| | 1 | 補正で一度でも g を押したら（受け入れ・取り消しとも）grip_opened=True にし、最初の推論の実行で開始時の開放をしない。内部の grip は補正後の状態のまま（モデルにもそれを渡す）。テスト test_grip_in_correction_before_first_chunk | | 2 | 巻き戻し / 補正の取り消しが finished=False なら、history に {"kind":"blocked","blocked":true} を積む。b は直前が blocked なら断って聞き直す（「これより前へは巻き戻しません」）。テスト test_b_after_failed_rewind_is_refused / test_b_after_failed_cancel_is_refused | | A3 | 未到達のあとは resync フラグを立て、次の move で読めた実測を基準にする（読み直し失敗でも古い目標を使わない） | | A5 | VideoWriter の isOpened() を確認し、開けない / 書けないときは write_errors を数える。top_meta.json の status=write_error、{run_stem}.json の recording_enabled=false → builder は使わない | | B2 / D6 | 前回の説明どおり（実機で actual_hz と送信の遅れを確認する。本体の既存ガードは範囲外） | テスト計 25 件すべて OK（`python3 pi05/test_rewind_correct.py`）。 ## お願い R2 の 1・2 が直っているか、新しい問題がないかを同じ形式で。最後に実機テストに進んで良いか。 ```` #### codex の答え review_result_20261029_rewind_correct_codex_r3.md（3回目） ````markdown **判定：✅ R2の主要2点は修正されています。段階的な実機テストに進んでよいと判断します。** 学習データ生成には、下記の非ブロッキング指摘が1点あります。 依頼書、前回レビュー、対象5ファイル、および `run_policy_client.py` の `git diff HEAD` を確認しました。ファイルは変更していません。行番号は現在のファイル基準です。 1. **⚠️［P2］JPEG保存失敗でも、正常なエピソードとして採用される** 対象：[build_correction_episodes.py:234](/Users/shimazueiichi/r2r2r-scripts/pi05/build_correction_episodes.py:234) `cv2.imwrite()` の戻り値を確認せず、直後に `written` を増やしています。容量不足などで保存が `False` を返した場合も抽出件数の検査を通過し、画像が欠けたエピソードに `episode.npz`・`meta.json` を生成して `used=True` を返します。 **修正案：** 保存成功時だけ件数を増やし、失敗時は生成途中のエピソードを不採用にする。`imwrite=False` の異常系テストを追加する。実機の動作確認は妨げませんが、学習データとして採用する前に対応してください。 R2指摘の反映状況です。 | R2 | 判定 | 確認結果 | |---|---|---| | 1：初回推論前の補正によるグリッパー操作 | ✅ | [本体1058行](/Users/shimazueiichi/r2r2r-scripts/pi05/run_policy_client.py:1058)で、受け入れ・取消とも `grip_used` により `grip_opened=True`。補正後の `grip` を保持し、次のモデル入力にも反映。 | | 2：巻き戻し・補正取消の失敗後に古い区間へ進む | ✅ | [本体1041行](/Users/shimazueiichi/r2r2r-scripts/pi05/run_policy_client.py:1041)・1062行で `blocked` 境界を登録。999行で次の `b` を拒否。後続操作を積んでも、その境界を越える巻き戻しはできません。 | | A3：未到達後の補正基準 | ✅ | `resync` を保持し、次のmoveで実測を取得できたときに補正基準を更新。読み取り失敗時は送信せず、フラグも保持。 | | A5：動画書き込み失敗 | ✅ | VideoWriterの開放状態・書込例外を `write_errors` に反映。終了時の `status` と `recording_enabled` を通じてbuilderが除外。 | その他の確認結果です。 | 項目 | 判定 | 結果 | |---|---|---| | 巻き戻し・補正の送信制限 | ✅ | 実測から `apply_limits`・`check_send_safe` を適用する構造を維持。 | | 未完了操作の学習除外 | ✅ | 未完了の巻き戻し・補正取消を含むrunは除外。画像候補にも除外窓を適用。 | | 追加回帰テスト | ✅ | R2の3ケースを確認。ただし初回c/gテストは受け入れ経路のみで、取消経路の結合テストは未追加。 | | B2：記録による通信待ち | ⚠️ | ロック待ちは残るため、`actual_hz` と送信・到着確認の遅延は実機で確認が必要。 | | 禁止事項 | ✅ | 追加処理に `send_coords()`・サーボ解放なし。指定の校正値や既存待機時間の変更なし。 | 5ファイルの構文解析は成功しました。テスト一式は一時ファイル・動画を生成するため、読み取り専用環境では実行しておらず、依頼書の「25件すべてOK」は独立に再確認していません。 **実機では、初回推論前のc/g、通常の巻き戻し、未到達後のb拒否を順に確認し、記録頻度と通信遅延を測定する段階へ進んでよいと判断します。** ```` #### 作業記録 logs/20261029_2026.md ````markdown # 2026-10-29 実機推論に巻き戻し (b) と関節の補正 (c) を追加 依頼: 20261029_25_実機推論に巻き戻しと補正を足す.md / コミット 2c860e5 ## やったこと - run_policy_client.py に b (巻き戻し) / c (補正) / --dry-keys / 1 秒 10 コマの記録 / 終了時の結果 y/n - 新規: policy_recorder.py / rewind_correct.py / build_correction_episodes.py / test_rewind_correct.py - テスト 26 件 OK (git HEAD 版との比較で, b/c を押さなければ送信・ログが同じことを確認) - codex レビュー 3 回 (R1 必須5 / R2 必須2 / R3 P2 1 をすべて反映) - 実機には流していない. datasets/mecharm_redblock_box_corr_v1 はまだ作っていない (成功 run がないため) ## 未決 (エイイチ) - グリッパーを動かした回は巻き戻さない方針でよいか - 補正の取り消しでグリッパーも始めの状態に戻す方針でよいか - 学習データから home 区間を除く (依頼どおり) でよいか - 記録の get_angles 10Hz が送信・到着確認を遅らせないか, 実機で actual_hz と合わせて確認 ```` ### 報告の「決めてほしいこと」4点への答え | 聞かれたこと | 答え | |---|---| | グリッパーを動かしたチャンクは、b で巻き戻さず聞き直す | つかんだ区間は巻き戻せるようにする（依頼2）。離した区間は今のまま断る | | 補正を取り消すときは、グリッパーも始めの状態に戻す。戻りきれなければ今のまま | このままでよい | | 学習データに、HOME へ戻る区間は入れない | このままでよい。エピソードが箱に入れたところで終わるだけなので、学習の邪魔にはならない | | 記録が関節の角度を1秒10回読むので、送信と到着確認が待たされることがある。遅れが大きければ読む回数を減らすか | 実機で測ってから決める | ## 依頼2：つかんだ区間も巻き戻し、人の手が写るコマを学習データから外した 依頼1の作りでは、グリッパーを動かしたチャンクは巻き戻しを断っていました。つかんだまま戻るとブロックを落とすからです。ただ、それでは、つかみ方が悪かった回（端をつかんだ・奥でつかんだ）をやり直せません。 そこで、つかんだチャンクを b で戻すときは、1文字で聞くようにしました。 | 答え | 動き | 使う場面 | |---|---|---| | y（持っている） | 送った目標を逆にたどって、つかんだ位置まで下ろす → グリッパーを開いてブロックを置く → 推論を始めた姿勢へ戻る | つかみ方が悪かった。ブロックはほぼ元の場所に戻る | | n（落とした） | その場ですぐグリッパーを開く → 推論を始めた姿勢へ戻る → ブロックを手で元の位置へ置き直してから Enter | 運ぶ途中で落とした・何もつかめていなかった。閉じた指が、置き直したブロックに当たらないように先に開く | 箱の上で離したチャンクは、今のまま巻き戻しを断ります。離したブロックをもう一度つかむことはできないからです。白箱にぶつかる失敗は、運ぶ途中のチャンク（グリッパーを動かしていないチャンク）なので、グリッパーを閉じたまま b で戻し、c で j1・j2 を上げる向きへ直せます。 もう1つ、依頼1では抜けていた点がありました。Enter や b を待っている間も記録は続いているので、ブロックや箱を手で置き直すと、手が写ったコマが残ります。入力を待っている間のコマに「待ち」の印を付け、学習データに入らないようにしました。 ねらい・変わったこと・見込める効果は次のとおりです。 - ねらい：つかんだあとの失敗（持ち上げた瞬間に落とす・つかみ方が悪い）も、その手前まで戻って直せるようにする - 変わったこと：巻き戻しを断るのは、箱の上で離したチャンクだけになった。入力を待っている間のコマは学習データから外れる - 見込める効果：LOG401 の失敗6回は、どれもつかむ所か持ち上げた所で起きていて、箱の上で離したあとの失敗はなかった。その6回の形なら、どれも巻き戻して直せる見込み ### 依頼文 20261029_27_つかんだ区間の巻き戻しと落としたときの戻し方.md ````markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 180 --- # 依頼：つかんだ区間も巻き戻せるようにし、落としたとき・人がブロックを置き直す間の扱いを足す（2026-10-29） 依頼 25（コミット 2c860e5・報告は依頼箱の 結果/20261029_25_実機推論に巻き戻しと補正を足す_20261029_1937.md）の続き。 まず logs/20261029_2026.md と docs/review_result_20261029_rewind_correct_codex*.md を読んでから始める。 ## 依頼 25 の「決めてほしいこと」への答え（エイイチと決めた） 1. グリッパーを動かした区間を b で断る → **つかんだ区間は下の A で巻き戻せるようにする。離した区間は今のまま断る** 2. 補正を取り消すときはグリッパーも元に戻す・戻りきれなければ今のまま → このままでよい 3. 学習データに HOME へ戻る区間は入れない → このままでよい 4. 記録のせいで送信が遅れたら読む回数を減らすか → 実機で測ってから決める（今は変えない） ## 実機で起きている、つかんだ後の失敗（エイイチより） - つかんだブロックを運ぶ途中で落とす - 白箱に入れるとき、アームが上がりきらないうちに箱の方へ動き、白箱にぶつかる 2つ目は運ぶ途中の区間（グリッパーを動かしていない区間）なので、今の b と c で戻して直せるはず。B の 3 で確かめる。 ## A. つかんだ区間（グリッパーを閉じた推論1回分）の巻き戻し b でその区間を戻すとき、1文字で聞く：`y=持っている（下ろして置いてから戻る） / n=落とした（先に開いてから戻る） / その他=やめる` - **y（持っている）**：その区間で送った目標を逆の順にたどり、グリッパーを閉じた時点の姿勢（つかんだ位置）まで下ろす → グリッパーを開いてブロックを置く → 残りを逆の順にたどって、その推論を始めた姿勢へ戻る。ブロックはほぼ元の場所に戻る - **n（落とした）**：今の位置ですぐグリッパーを開く → 逆の順にたどって、その推論を始めた姿勢へ戻る（下ろすときに、閉じた指が置き直したブロックに当たらないよう、先に開く）。戻ったあと、ブロックを人が元の位置へ置き直してから Enter で続ける。元の位置の目安として、戻った推論の始めの写真（frames/）のパスを画面に出す - つかんだ区間より前（運ぶ途中の区間がいくつか続いたあと）にさかのぼる場合も、つかんだ区間に来た所で同じように聞く - 送り方・範囲チェックは今の b と同じ。新しい確認や停止条件は、この y/n 以外は足さない - log.jsonl の rewind 行に `grasp_undo: "placed"` か `"dropped"` を残す 離した区間（グリッパーを開いた推論1回分）は今のまま断る。断るときの文言に「ブロックを置き直すなら h で終えてやり直す」と出す。 ## B. 人の手が映るコマを学習データから外す 推論のあとの確認（Enter・b・c・y/n を待っている間）も記録は続いているので、ブロックや箱を手で置き直すと、手が写ったコマが残る。 1. 入力を待っている間のコマには状態 `wait` を付ける（今の作りで既に付いているなら、そのことを報告に書く） 2. build_correction_episodes.py は `wait` のコマを使わない。止まっている区間の詰め方（先頭2コマを残す）で wait のコマが残らないことも確かめる 3. 確かめ（実機なし・--dry-keys と合成の記録）： - 運ぶ途中の区間の b → グリッパーは閉じたままで、送る目標が逆の順になる（白箱にぶつかったときの戻し方） - つかんだ区間の b で y → 「つかんだ位置まで下ろす → 開く → 始めの姿勢へ」の順になる - つかんだ区間の b で n → 「開く → 始めの姿勢へ」の順になる - wait のコマ・rewind のコマ・巻き戻しで取り消した区間が、学習用エピソードに入らない ## 変えないもの - b・c を押さないときの動きとログ（今と同じであることを、依頼 25 と同じ比べ方で確かめる） - 送り方（speed 20・中継点の作り方）・範囲チェックの値・記録の頻度 ## 終わったら - 関節とグリッパーを動かす変更なので、codex のレビューを通してから報告する。指摘は直すか、直さない理由を書く - 実機には流さない。報告に、エイイチが実機で試す手順を書く（つかんで持ち上げたあと b → y／運ぶ途中で b → c で j1・j2 を上げる方へ直す → Enter） - 個別のファイルを指定して add・commit する ```` ### Claude Code の報告（49分・テスト37件・codex のレビュー1回） Claude Code の最後の報告は、レビューの指摘を直したことと、決めてほしいこと2点だけの短いものでした。何を変えたかの詳しい中身は、下の作業記録 `logs/20261029_2142.md` にあります。 ````markdown # 結果：20261029_27_つかんだ区間の巻き戻しと落としたときの戻し方 - 状態：完了 - 始めた時刻：2026-10-29 21:13 - 終わった時刻：2026-10-29 22:02（49分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 4af1baaa-b12e-4a51-a5b4-52e9dd68238c` ## Claude Code の報告 codex のレビュー結果は、もう確認と反映を済ませています。この依頼でほかにやることはありません。 ## やったこと - codex のレビュー結果を読みました。判定は「実機テストに進んでよい」です。 - 指摘は 1 件（P3）でした。安全停止のあとの「h=HOMEに戻して終了」の入力待ちが、`model` のまま記録されていました。ここも `wait` にしました。 - レビューの結果を docs/review_result_20261029_grasp_rewind_codex.md に残しました。 ## 結果 - 直したあとにテストを流し、37 件すべて通りました。 - コミット 4387d69 に入れました。 - 実機には流していません。 ## 変えたファイル 前の報告から増えたものはありません（P3 の修正はコミット 4387d69 に入っています）。 ## 決めてほしいこと 前の報告の 2 点のままです。 1. y で、つかんだ位置まで下ろしきれなかったときは、グリッパーを開かずに止める扱いにしています。これでよいかを決めてください。 2. ブロックを持っていて肩が上がりきらず、始めの姿勢から 3° 以内に戻れないと、「戻りきれなかった」扱いになり、そこから前へは巻き戻しません。実機で起きるかを見てから、どうするかを決めてください。 ```` 決めてほしいことの2点は、まだ決めていません。1つ目（つかんだ位置まで下ろしきれなければ開かない）は、宙でブロックを離さないための扱いで、依頼1の「補正を取り消すときに戻りきれなければ、グリッパーは今のまま」と同じ考えです。2つ目（ブロックを持っていて肩が上がりきらず、始めの姿勢から 3° 以内に戻れなければ、そこから前へは巻き戻さない）は、実機で起きるかを見てから決めます。 codex のレビューは1回で、判定は「実機テストに進んでよい」でした。指摘は軽いもの1件（安全停止のあとの入力待ちが、待ちの印になっていない）で、直してあります。 #### レビューの依頼文 review_request_20261029_grasp_rewind.md ````markdown # コードレビュー依頼：つかんだ区間の巻き戻し (y/n) と 入力待ちのコマ (wait) を学習データから外す 前提: 依頼 25 (コミット 2c860e5. 巻き戻し b / 補正 c / 1 秒 10 コマの記録) の続き。 過去のレビュー: docs/review_result_20261029_rewind_correct_codex*.md 対象 (`git diff HEAD -- pi05/` で差分): - `pi05/run_policy_client.py`（run_rewind に grasp 引数 / 確認の入力ループ / history に grip_cmd） - `pi05/rewind_correct.py`（MODE_WAIT / GRASP_PLACED / GRASP_DROPPED を追加） - `pi05/build_correction_episodes.py`・`pi05/policy_recorder.py`（説明文のみ） - `pi05/test_rewind_correct.py`（テスト 11 件追加。計 37 件） ## 仕様（エイイチと決めたこと） A. つかんだ区間（グリッパーを閉じた推論 1 回分）を b で戻すとき 1 文字で聞く: `y=持っている（下ろして置いてから戻る） / n=落とした（先に開いてから戻る） / その他=やめる` - y: 送った目標を逆にたどり、グリッパーを閉じた時点の姿勢（閉じる前に最後に送った目標）まで下ろす → 開く → 残りを逆にたどって推論を始めた姿勢へ - n: 今の位置ですぐ開く → 逆にたどって推論を始めた姿勢へ → 始めの写真 (frames/NNNN.jpg) のパスを出し、人がブロックを置き直して Enter で続ける - 運ぶ区間をいくつか戻したあと、つかんだ区間に来た所で同じように聞く（b は 1 回分ずつさかのぼる） - 送り方・範囲チェックは今の b と同じ (traverse)。y/n 以外の新しい確認・停止条件は足さない - rewind 行に grasp_undo: "placed" / "dropped" - 離した区間（開いた推論）は今のまま断る。文言に「ブロックを置き直すなら h で終えてやり直す」 - グリッパーを動かした補正は今のまま断る（依頼の範囲外） - 実装上の判断: y で「つかんだ位置まで下ろせなかった」(traverse が止まる / 3° 以内に着かない) ときは開かない（宙で落とさない）。finished=False → 次の b は blocked で断る。依頼 25 で決めた「補正の取り消しで姿勢が戻りきれなければグリッパーは今のまま」と同じ考え B. 入力を待っている間（Enter/b/c/y/n、n のあとの置き直しの Enter）は記録の状態を `wait` にする。build_correction_episodes は TRAIN_MODES=(model, correct) なので wait を使わない（止まっている区間の詰め方より前に外れる） 変えないもの: b/c を押さないときの送信・グリッパー・log.jsonl（git HEAD 版との比較テスト test_unchanged_without_b_c で一致）/ SPEED 20 / 中継点の作り方 / 範囲チェックの値 / 記録の頻度。変わるのは states.csv に wait の stage 行が増えること。 ## チェック項目 A1. y / n の送る順番（下ろす → 開く → 戻る / 開く → 戻る）と、つかんだ位置の決め方は正しいか A2. 失敗時（読めない・範囲外・未到達）にグリッパーと history・grip 変数が食い違わないか。開いたのに grip=1 のまま、などがないか A3. b を断る / 聞く条件（grip_cmd が 1 の推論だけ聞く。0 の推論・グリッパーを動かした補正・blocked は断る）は正しいか A4. wait の付け外しで、学習に使うべき model のコマ（実行中の動き）が wait になったり、wait が model に残ったりしないか B1. b/c を押さないときの送信・ログが変わっていないか C. 安全: 開くタイミングで物を宙で落とす経路がないか（n は仕様で今すぐ開く）。send_coords 不使用・release_all_servos 不使用・定数不変 ## レビュー結果のフォーマット 各項目 ✅ OK / ⚠️ 要注意 / ❌ NG（行番号と修正案）。最後に実機テストに進んで良いか・直すべき項目。 ```` #### codex の答え review_result_20261029_grasp_rewind_codex.md ````markdown **判定：実機テストに進んでよいと判断します。動作を妨げる問題は見つかりませんでした。入力待ちの記録に軽微な漏れが1点あります。** 依頼書と `git diff HEAD -- pi05/`、関連処理を確認しました。ファイルは変更していません。 | 項目 | 判定 | 確認結果 | |---|---|---| | A1：y/n の順序・つかんだ位置 | ✅ OK | y は閉じる前の最後の送信目標へ移動→到達確認→開放→開始姿勢へ。送信なしなら開始姿勢を使用。n は開放→巻き戻し→開始写真の表示→置き直し待ち。 | | A2：失敗時の整合性 | ✅ OK | y の下降失敗・読取失敗・未到達では開かない。開放後は、戻りが失敗しても `grip=0` に更新。未完了なら `blocked` を積み、古い履歴への巻き戻しを拒否。 | | A3：拒否・確認条件 | ✅ OK | `blocked` を先に拒否し、`grip_changed` かつ推論の `grip_cmd==1` の場合だけ質問。開いた推論・グリッパー操作を含む補正は拒否。指定の案内文もあり。 | | A4：wait の付け外し | ⚠️ 要注意 | 通常確認・y/n・置き直し待ちは適切。安全停止後の確認だけ切替漏れあり。下記参照。 | | B1：b/c を押さない場合 | ✅ OK | 差分上、既存の送信・グリッパー・`log.jsonl` の処理を維持。HEADとの比較テストもありますが、今回未実行。 | | C：安全・禁止事項 | ✅ OK | y の開放は `traverse` 完了かつ把持姿勢から3°以内の場合のみ。追加処理に `send_coords()`・`release_all_servos()` なし。速度・範囲チェック値・既存sleepの変更なし。 | **⚠️［P3］安全停止後の入力待ちが `model` のまま記録される** 対象：[run_policy_client.py:1305](/Users/shimazueiichi/r2r2r-scripts/pi05/run_policy_client.py:1305) 安全範囲チェックで停止した後の `h=HOMEに戻して終了 / 他=そのまま終了` は、`MODE_WAIT` に切り替えず入力を待ちます。この間のコマは `model` として残ります。 この終了経路では成功結果を取得しないため、通常のbuilderでは学習対象にならず、直ちに学習データが混入する問題ではありません。ただし「入力待ちはwait」という記録方針には不整合です。 修正案は、この `input()` の直前にも `recorder.set_mode(MODE_WAIT)` を追加することです。h 選択後は既存の `MODE_HOME` 切替を使えます。 5ファイルの構文解析は成功しました。テスト一式は一時ファイル・動画を生成するため、読み取り専用環境では実行しておらず、37件の成功は独立に再確認していません。 **必須修正はありません。上記の記録漏れは修正を推奨します。実機では y/n の順序、y の未到達時に開かないこと、開放後に戻りが失敗しても次のモデル入力が開状態になることを確認してください。** tokens used ```` #### 作業記録 logs/20261029_2142.md ````markdown # 2026-10-29 つかんだ区間の巻き戻し (y/n) と 入力待ちのコマ (wait) を学習データから外す 依頼: 20261029_27_つかんだ区間の巻き戻しと落としたときの戻し方.md (依頼 25 / 2c860e5 の続き) ## やったこと - run_policy_client.py - つかんだ区間 (グリッパーを閉じた推論) の b で y/n を聞く - y: つかんだ位置 (閉じる前に最後に送った目標) まで下ろす → 開く → 逆にたどって始めの姿勢へ. 下ろしきれない (traverse が止まる / 3° 以内に着かない) ときは開かない → finished=False → 次の b は断る - n: すぐ開く → 逆にたどって始めの姿勢へ → frames/NNNN.jpg のパスを出して Enter 待ち - その他: やめる. rewind 行に grasp_undo / gripper_opened / grasp_pose など - 離した区間の b は断る (文言に「ブロックを置き直すなら h で終えてやり直してください」) - 入力待ち (Enter/b/c/y/n・置き直しの Enter・安全停止後の h 確認) の間は記録の状態を wait に (依頼 25 の時点では wait はなく model だった) - rewind_correct.py: MODE_WAIT / GRASP_PLACED / GRASP_DROPPED - build_correction_episodes.py: 処理は変えず (TRAIN_MODES=model/correct なので wait は使わない). 説明文だけ - test_rewind_correct.py: 11 件追加 (計 37 件 OK). HEAD 比較テストは HEAD が依頼 25 版になったので項目の外し方を両側に ## codex レビュー - docs/review_request_20261029_grasp_rewind.md → docs/review_result_20261029_grasp_rewind_codex.md - 判定: 実機テストに進んでよい. P3 1 件 (安全停止後の入力待ちが model のまま) → 反映 ## 未決 (エイイチ) - y で下ろしきれないときは開かない (依頼 25 の「戻りきれなければグリッパーは今のまま」と同じ考え) でよいか - 実機での確認 (手順は依頼箱の報告に記載) ```` 【図解】LOG403_rewind_flow.html ## 実機で最初に試す手順 実機での確かめは次の回にします。LOG401 と同じ配置で、次の順に試します。 1. いつものコマンドで起動し、推論1回目は Enter ``` python3 pi05/run_policy_client.py --raw-dir $RAW --steps-per-chunk 10 --via-stride 3 --settle-retries 0 --state-from-target --execute --ignore-framing ``` 2. 次の確認で b → HOME 付近に戻り、もう一度推論が出るか 3. 次の確認で c → 1 で j0 を選び、+ か - を数回 → Enter で推論に戻る 4. つかんで持ち上げたところで b → y → つかんだ位置に下ろして置き、推論を始めた姿勢へ戻るか 5. 運ぶ途中で b → c で j1・j2 を上げる向きへ直す → Enter 6. ブロックが箱に入ったら h を押し、結果を聞かれたら y 7. 終わったら、記録の `recording/top_meta.json` の actual_hz と effective_fps（どちらも10前後が目安）を見る。送信と到着確認が前より遅くなっていないかも見る ## 変えたコード（全量・r2r2r-scripts のコミット 2c860e5 と 4387d69） | ファイル | 中身 | |---|---| | pi05/run_policy_client.py | 実機推論の本体。b・c・y/n・記録・結果の聞き取りを足した | | pi05/policy_recorder.py | 上のカメラと関節の角度を1秒10コマで残す記録係（新規） | | pi05/rewind_correct.py | 巻き戻しの中継点・補正のキー・学習から外す区間の計算だけ（新規） | | pi05/build_correction_episodes.py | 記録から学習用のエピソードを作る（新規） | | pi05/test_rewind_correct.py | アームなしのテスト37件（新規） | ### pi05/run_policy_client.py ````python #!/usr/bin/env python3 """run_policy_client.py — π0.5 (Runpod の serve_policy) の出力で mechArm 270 を動かす Mac 側クライアント. 流れ (1 チャンクごと): 上カメラ画像 + 実測 6 軸 + グリッパー状態 → π0.5 → 行動チャンク (10 x 7, 0.1 秒刻みの絶対角) → 先頭 N ステップ先 (既定 5 = 0.5 秒先) の姿勢へ単発 send_angles (学習データと同じ速度 20) → --via-stride N を付けると, 途中の姿勢 (N-1, 2N-1, ... ステップ) も中継点として 順に送る (チャンクの中の順番を残すため. 既定 0 = 1 点だけ送る従来どおり) → グリッパーの開閉がチャンク内で切り替わるなら, その直前の姿勢まで動いてから開閉 → 撮り直して次のチャンクへ 安全策: - 既定はドライラン (アームに送らない). 送るには --execute - --execute でも既定は 1 チャンクごとに Enter で確認 (--auto で連続) - 学習データで通った関節範囲 ±ENVELOPE_MARGIN_DEG の外へは出さない (クランプ) - 肩 j1 の上限は J1_SAFE_MAX (88°) で更に切る. これを超える指令はアームが黙って捨てる (LOG352) - 送る直前に最終検査. 6 関節のどれかが安全範囲の外か非有限値なら, 送信もグリッパー操作もせず 停止 (codex R1 / C2). HOME 復帰 (h) はこの検査を通さずに送れる - 1 回の移動量を --max-move-deg で制限 (クランプ), j4 は J4_SAFE 範囲内 - グリッパー操作後は 2 秒待つ (DAY175 対策・本線と同じ) - --raw-dir は必須. 安全範囲は **学習に使ったデータ** から作る (別のデータだと範囲が合わない) - 開始時に上カメラの写り方をお手本と比べ, OK 以外ならアームに何も送らずに終了 (ずれ / 測れない. --ignore-framing で続行). この確認が通るまで daemon にも触らない - 開始時のグリッパー開放も, 最初に送る目標が検査を通ってから (codex R2) - --state-from-target を付けると, モデルに渡す state の 6 関節を「直前に送った目標」に する (既定 off = 実測). 肩の不足 (ブロックを持つと毎チャンク 2.4〜2.6°) が チャンクごとに積み重なるのを防ぐ. 目標と実測の差が 6° を超えたチャンクは実測に戻す. クランプ・検査・中継点のかけ直しは今までどおり実測が基準 (変わるのは state だけ) - 送ったあとに残ったずれ (動きが止まってから測る. > --settle-tol) は, **同じ目標を もう一度送って** 詰める (送信 --settle-retries 回まで. LOG359. 0 で従来どおり 送り直さない). 新しい目標は作らない. 動きが止まらない / 読めない / 送信が失敗した ときは送り直しをやめる. どの終わり方でも止めずに次のチャンクへ進む - 止まったときに finally でアームを動かさない. 戻すときは Enter 確認の h で明示的に送る 巻き戻しと関節の補正 (2026-10-29. 推論のあとの確認で選ぶ. b / c を押さなければ今までと同じ動き): - b = 巻き戻し. 直前に実行した 1 回分 (推論 1 回 か 補正 1 回) で送った目標を逆順にたどり, 始めたときの姿勢 (推論なら measured_state) へ戻す. 戻ったらその姿勢からもう一度推論する. 続けて b を押すと 1 回分ずつさかのぼる. グリッパーを動かした回は巻き戻さない (断って聞き直す). ただし つかんだ区間 (グリッパーを閉じた推論 1 回分) は 1 文字で聞いて巻き戻す (2026-10-29 依頼 27): y = 持っている: 逆にたどって つかんだ位置まで下ろす → 開いて置く → 始めの姿勢へ n = 落とした: 今の位置ですぐ開く → 逆にたどって始めの姿勢へ → 人がブロックを置き直して Enter その他 = やめる. rewind 行に grasp_undo: "placed" / "dropped" 離した区間 (グリッパーを開いた推論) と グリッパーを動かした補正 は今までどおり断る. log.jsonl に {"type": "rewind", "rewound": true, "chunk": 巻き戻した推論の番号, ...} を足す (推論の行は消さない・書き換えない) - c = 関節の補正. Enter を待たずに 1 文字ずつ読む (押し続けるとキーリピートで繰り返す. 1 回送るごとに溜まったキーは捨てる). 1〜6 で関節, + / - (. / ,) で 1 刻み, [ ] で刻み 0.5/1/2°, g でグリッパー, Enter で推論に戻る, b で取り消して始めの姿勢へ戻る. 送り方は推論と同じ (_send_and_wait_arrival = send_angles speed 20 と到着確認) で, apply_limits と check_send_safe をそのまま通す (通らない 1 刻みは送らない). 1 刻みごとに log.jsonl へ {"type": "correction", ...} - --execute の間は上カメラと実測角を 1 秒 10 コマで recording/ に残す (policy_recorder.py. お手本と同じ形). 1 コマごとに状態 (model / rewind / correct / home / wait) を付ける. wait = 推論のあとの確認で入力を待っている間 (人が手でブロックを置き直すので学習データに使わない) h / q で終えるときに 結果 (y / n / Enter=記録しない) を聞き, {run_stem}.json に残す - --dry-keys: ドライランでも 1 回ごとに止まり b / c を試せる (アームにも daemon にも触らない. 仮の姿勢を HOME から動かして流れだけ確かめる. Enter で「仮に実行」= 仮の姿勢が目標へ移る) 終了コード: 0 = 最後まで / q / h / Ctrl-C, 1 = 安全停止・写り方の不一致・途中のエラー 前提: - Pi の daemon が待ち受け中 / 上カメラ (iPhone 連係) が見えている - Runpod で serve_policy が起動し, Mac から SSH トンネル (localhost:8000) が張れている - pip install msgpack "websockets>=11" Usage: RAW=/Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes python3 pi05/camera_align.py --raw-dir $RAW # 先に上カメラの写り方を合わせる python3 pi05/run_policy_client.py --raw-dir $RAW # ドライラン (予測を表示・記録するだけ) python3 pi05/run_policy_client.py --raw-dir $RAW --execute # 1 チャンクごとに Enter で実行 python3 pi05/run_policy_client.py --raw-dir $RAW --execute --auto --max-chunks 40 python3 pi05/run_policy_client.py --raw-dir $RAW --dry-keys # アームなしで b / c の流れを試す 記録: /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/{stamp}/ (log.jsonl + 各チャンクの入力画像. --execute のときは recording/ と {run_stem}.json も) --record を付けると recorder.py --top にも trial_start/trial_end を送り, 上カメラ動画を残す. """ import argparse import json import socket import sys import time import types from datetime import datetime from pathlib import Path import cv2 import numpy as np # ---- パス (run_pick_place.py と同じ構成) ---------------------------------------- _HERE = Path(__file__).resolve().parent _ROOT = _HERE.parent _PLACE = _ROOT / "dynamic_tracking_limits" / "place" / "place_modules" _DRAG = _ROOT / "dynamic_tracking_limits" / "drag_teaching" for p in (_HERE, _ROOT, _PLACE, _DRAG): sys.path.insert(0, str(p)) # pymycobot スタブ (waypoint_replay 経由の import を Mac 上で通すため. アーム制御は daemon 経由のみ) if "pymycobot" not in sys.modules: _pkg = types.ModuleType("pymycobot"); _mod = types.ModuleType("pymycobot.mycobot") class _MyCobotStub: def __init__(self, *a, **k): raise RuntimeError("MyCobot は直接使わない (daemon 経由のみ)") _mod.MyCobot = _MyCobotStub sys.modules["pymycobot"] = _pkg; sys.modules["pymycobot.mycobot"] = _mod import camera_resolver # noqa: E402 import waypoint_replay as wr # noqa: E402 from camera_align import ( # noqa: E402 FRAMING_MAX_DX, FRAMING_MAX_DY, FRAMING_MIN_RESPONSE, framing_verdict, load_reference, measure_framing, ) from fk_lookup import J1_SAFE_MAX # noqa: E402 88.0. 本線と二重定義しない from j0_sweep_scan import SEND_WAIT_SEC, SPEED, _send_and_wait_arrival # noqa: E402 本線と同じ 20 / 1.0 from policy_recorder import PolicyRecorder, install_daemon_lock # noqa: E402 from policy_ws import PolicyClient # noqa: E402 from rewind_correct import ( # noqa: E402 CORRECTION_DEFAULT_STEP_INDEX, CORRECTION_HELP, CORRECTION_STEPS, GRASP_DROPPED, GRASP_PLACED, MODE_CORRECT, MODE_HOME, MODE_MODEL, MODE_REWIND, MODE_WAIT, next_step_index, parse_correction_key, rewind_waypoints, split_leg, ) from visual_servo_pick import ( # noqa: E402 close_daemon_socket, daemon_get_angles, daemon_send_angles, daemon_set_gripper, daemon_set_gripper_value, get_daemon_socket, ) TASK = "put the red block in the white box" IMG_LONG = 384 # build_demo_episodes.py と同じ (学習画像の長辺) HOME = [0.0, 0.0, 0.0, 0.0, 0.0, -30.0] GRIP_VALUE = 20 # run_pick_place.py の close 値 GRIPPER_SLEEP = 2.0 J4_SAFE = (-90.0, 60.0) # run_pick_place.py の J4_SAFE_MIN/MAX ENVELOPE_MARGIN_DEG = 10.0 SEND_EPS_DEG = 1e-6 # 送信前の検査の余裕 (小数の誤差で止まらないように) SETTLE_POLL_SEC = 0.2 # 送り直しの前後, 関節角をこの間隔で読む (codex A2) SETTLE_STILL_DEG = 0.2 # 続けて読んだ 2 回の差が 6 関節すべてこれ未満なら「止まった」 SETTLE_WAIT_SEC = 2.0 # 止まらなくてもここで待つのをやめ, そのときの値で判定する # --state-from-target で「直前に送った目標」を渡すのをやめる差 (LOG359). # 腕がどこかに当たって止まっているときに, 目標だけが先へ進むのを防ぐ. STATE_TARGET_MAX_GAP_DEG = 6.0 DATA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data") LOG_ROOT = DATA_ROOT / "runs" / "pi05_policy" RECORDER = ("127.0.0.1", 5556) def load_envelope(raw_dir: Path): """学習データで通った各関節の範囲 ± マージン. 物理可動域 (wr.JOINT_LIMITS)・J4_SAFE・J1_SAFE_MAX で更に絞る.""" qs = [np.load(p)["state"][:, :6] for p in raw_dir.glob("*/episode.npz")] if not qs: sys.exit(f"[ERROR] 学習エピソードが見つからない (安全範囲を作れない): {raw_dir}") q = np.concatenate(qs) lo = q.min(axis=0) - ENVELOPE_MARGIN_DEG hi = q.max(axis=0) + ENVELOPE_MARGIN_DEG for k, (plo, phi) in enumerate(wr.JOINT_LIMITS): lo[k], hi[k] = max(lo[k], plo), min(hi[k], phi) lo[4], hi[4] = max(lo[4], J4_SAFE[0]), min(hi[4], J4_SAFE[1]) # 肩 j1: wr.JOINT_LIMITS[1] の上限は +123° なので, マージンを足すと +98° まで通ってしまう. # 88° を超える指令はアームが黙って捨てる (LOG352) ので, ここで切る. hi[1] = min(hi[1], J1_SAFE_MAX) # codex R3: lo > hi や非有限値のまま運転しない (np.clip は例外を出さないので黙って通る). # lo を書き換えて辻褄を合わせるのではなく, 起動を中止する. bad = [k for k in range(6) if not (np.isfinite(lo[k]) and np.isfinite(hi[k])) or lo[k] > hi[k]] if bad: sys.exit("[ERROR] 安全範囲が不整合 (lo > hi か非有限値): " + " ".join(f"j{k}[{lo[k]:+.1f},{hi[k]:+.1f}]" for k in bad) + f"\n 学習データが安全制約 (J1_SAFE_MAX={J1_SAFE_MAX:.0f}° / J4_SAFE / " f"JOINT_LIMITS) と合っていない: {raw_dir}") return lo, hi, len(qs) def read_angles(): for _ in range(5): try: a = daemon_get_angles() except Exception: a = None if wr._is_valid_angles(a): return np.array([float(x) for x in a]) time.sleep(0.1) return None def grab_rgb(cap): for _ in range(3): # バッファに溜まった古いフレームを捨てる cap.grab() ok, frame = cap.read() if not ok or frame is None: return None, None return frame_to_model(frame) def frame_to_model(frame): """カメラの 1 コマ (BGR) → (モデルに渡す RGB, 保存用の BGR). 長辺 IMG_LONG に縮める.""" h, w = frame.shape[:2] s = IMG_LONG / max(h, w) small = cv2.resize(frame, (round(w * s), round(h * s)), interpolation=cv2.INTER_AREA) return cv2.cvtColor(small, cv2.COLOR_BGR2RGB), small # モデルへは RGB, 保存は BGR def plan_chunk(actions, q, grip, n_steps): """チャンクから今回動かす目標を決める. 戻り値: (目標6軸, グリッパー指令 None/0/1, 使ったステップ).""" want = (actions[:, 6] > 0.5).astype(int) for f in range(min(n_steps, len(actions))): if want[f] != grip: target = q.copy() if f == 0 else actions[f - 1, :6] return np.asarray(target, dtype=float), int(want[f]), f k = min(n_steps, len(actions)) - 1 return np.asarray(actions[k, :6], dtype=float), None, k def apply_limits(target, q, lo, hi, max_move): notes = [] t = target.copy() clipped = np.clip(t, lo, hi) for k in np.where(np.abs(clipped - t) > 1e-6)[0]: notes.append(f"j{k} 安全範囲でクランプ {t[k]:+.1f}→{clipped[k]:+.1f}") t = clipped d = t - q big = np.abs(d) > max_move for k in np.where(big)[0]: notes.append(f"j{k} 移動量 {d[k]:+.1f}° を ±{max_move:.0f}° に制限") t = q + np.clip(d, -max_move, max_move) return t, notes def check_send_safe(target, lo, hi): """送る直前の最終検査 (codex R1 / C2). 違反の理由を返す (空なら送って良い). apply_limits は「安全範囲でクランプ → 移動量の上限」の順なので, 現在角が安全範囲の 外にあると, クランプで範囲内に入れた目標が移動量の制限でまた範囲の外に戻る. j1 (codex R1): 現在 110° / 予測 100° / 上限 20° → 88° にクランプ後, 110-20 = 90° を送る j4 (codex C2): 現在 100° / 予測 90° / 上限 20° → 60° にクランプ後, 100-20 = 80° を送る j1 だけでなく 6 関節すべてを見る. ここで再クランプすると移動量の上限を破るので, 送らずに止める. HOME 復帰 (Enter 確認の h) はこの検査を通さない: 範囲の外にいるアームを戻す手段として残す. """ if not np.all(np.isfinite(target)): return ["目標に非有限値が入っている: " + " ".join(f"j{k}={v}" for k, v in enumerate(target))] bad = [] for k in range(6): if target[k] < lo[k] - SEND_EPS_DEG or target[k] > hi[k] + SEND_EPS_DEG: msg = f"j{k} {target[k]:+.2f}° が安全範囲 [{lo[k]:+.2f}, {hi[k]:+.2f}] の外" if k == 1 and target[k] > hi[k] and abs(hi[k] - J1_SAFE_MAX) < 1e-9: msg += (f" (上限は J1_SAFE_MAX={J1_SAFE_MAX:.0f}°. " "これを超える指令はアームが黙って捨てる / LOG352)") elif k == 4: msg += f" (j4 の範囲は 学習データ ∩ J4_SAFE [{J4_SAFE[0]:+.0f}, {J4_SAFE[1]:+.0f}])" bad.append(msg) return bad def _fmt6(a): return None if a is None else [round(float(v), 2) for v in a[:6]] def read_until_still(): """関節角が止まるまで待ってから読む (codex A2/A6). 戻り値: (角度 or None, 止まったか, 直前に読めた角度 or None) 1 番目は **読めたときだけ** 値が入る. 読み取りに失敗したら None で, 古い値を代わりに返さない (A6: 失敗を隠さない). 3 番目に最後に読めた値を別に返す. _send_and_wait_arrival も daemon_send_angles も, 戻った直後はまだ動いている ことがある. その値で残りのずれを測ると, 動いている途中の姿勢で判断してしまう. SETTLE_POLL_SEC おきに読み, 続けて読んだ 2 回の差が 6 関節すべてで SETTLE_STILL_DEG 未満になったら「止まった」とみなす. SETTLE_WAIT_SEC まで止まらなければ打ち切り, そのときの値を still=False で返す. """ prev = read_angles() if prev is None: return None, False, None t0 = time.time() while time.time() - t0 < SETTLE_WAIT_SEC: time.sleep(SETTLE_POLL_SEC) cur = read_angles() if cur is None: return None, False, prev if np.max(np.abs(cur[:6] - prev[:6])) < SETTLE_STILL_DEG: return cur, True, cur prev = cur return prev, False, prev def plan_vias(acts, q, final_target, used, stride, lo, hi, max_move, final_raw_index=None): """チャンクの途中の姿勢も中継点として順に通る計画を作る (LOG359). 今までは plan_chunk が選んだ 1 点だけを送っていたので, アームはそこへ一直線に動き, チャンクの中の順番が消えていた. 2026-10-22 の実機 114643 のチャンク 04 では, 予測が「最初の 0.4 秒は持ち上げだけ (j0 はほぼ動かない) → 0.5 秒から土台 j0 が回る」 だったのに, 上げながら回る動きになっていた (お手本は つかんだあと 1.5 秒 j0 を止めて 持ち上げている). stride=0 (既定) なら中継点なし = used の 1 点だけ (従来と同じ計算). stride=N なら 0 始まりで N-1, 2N-1, ... のうち used より小さいステップを中継点にし, 最後に used (plan_chunk が選んだ姿勢) を置く. 例: N=3 used=9 → 2, 5, 8, 9 / N=3 used=6 (7 番目でグリッパーが変わる) → 2, 5, 6. 移動量の制限 (apply_limits) は **直前の中継点の目標から** 測る. 最初の中継点だけ 今の実測角 q から測る. 実測ではなく目標から測るので, 送る前に全部計算できる (= 送り始める前に全部の中継点を check_send_safe にかけられる). final_raw_index は, 最後の点 (used) の raw が actions の何行目か. グリッパーが 切り替わるチャンクでは plan_chunk が used-1 行目を返すので step とずれる (codex A1). 戻り値: [{"step", "raw_index", "raw", "target", "notes", "delta"}] の並び (target は ndarray). target は計画時のもので, 送る直前に実測角からかけ直す. """ steps = [k for k in range(stride - 1, used, stride)] if stride and stride > 0 else [] steps.append(int(used)) plan = [] prev = np.asarray(q, dtype=float).copy() for k in steps: last = k == used raw = (np.asarray(final_target, dtype=float) if last else np.asarray(acts[k, :6], dtype=float)) t, notes = apply_limits(raw, prev, lo, hi, max_move) plan.append({"step": int(k), "raw_index": (final_raw_index if last else int(k)), "raw": raw, "target": t, "notes": notes, "delta": t - prev}) prev = t return plan def settle_target(target, label, tol, retries): """送ったあとに残ったずれを, **同じ目標をもう一度送って** 詰める (LOG359). 2026-10-21/22 の実機 4 回 (183245 / 095251 / 100713 / 102053) で, ブロックを持って 肩を持ち上げる指令 (j1 を小さくする向き) は 28 回すべて指令まで届かず, 0.77〜2.80° (中央 2.00°) 低いところで止まっていた. 逆向き (下げる) は 0.5〜1° 行き過ぎる. 到達判定は _send_and_wait_arrival(..., 3.0) の 3° なので, このずれは「着いた」と 見なされ, モデルは実際の姿勢から次を予測する. その結果 持ち上げが毎回 2° ずつ失われ, ブロックがマットから離れない (102053 は上げる指令の合計 41° に対して実際は 11°). 新しい目標は作らない. 渡された target (apply_limits と check_send_safe を通ったもの) を そのまま送り直すだけ. どの終わり方でも止めない (警告を出して呼び出し側が次へ進む). retries は **settle で送る送信回数の上限** (codex A5). 送り直しは _send_and_wait_arrival ではなく, その中で使っている 1 回だけ送る層 daemon_send_angles(..., speed=SPEED, wait=SEND_WAIT_SEC) を直接呼ぶ. 中で再送しないので送信回数を正確に数えられる (上限を超えない). 送ったあとは read_until_still() で止まるのを待ってから測る. チャンク本体の送信は今までどおり _send_and_wait_arrival のまま. 戻り値: (記録用の dict, 送信回数). dict の reason は within_tol tol 以内 (送っていない) converged 送り直して tol 以内に入った not_converged 送信回数の上限まで送っても tol を超えたまま not_still SETTLE_WAIT_SEC 待っても動きが止まらない (動いている最中には送らない) send_failed daemon_send_angles が例外を投げた read_failed 関節角を読めない (古い値で判定しない) disabled --settle-retries 0 (何も読まない・何も送らない) どの reason でも形は同じ (final_err / joint / final_actual / last_valid_actual / tries). """ out = {"reason": "disabled", "tol": tol, "sends": 0, "final_err": None, "joint": None, "final_actual": None, "last_valid_actual": None, "tries": []} if retries <= 0: # --settle-retries 0 で従来と同じ動き (読みも送りもしない) return out, 0 tgt = np.asarray(target, dtype=float)[:6] tries, sends, n = [], 0, 0 actual, still, last_valid = read_until_still() while True: if actual is None: # 読めない値で判定しない (A6) reason = "read_failed" break err = np.abs(tgt - actual[:6]) k = int(err.argmax()) cur_err = float(err[k]) if cur_err <= tol: reason = "converged" if sends else "within_tol" break if not still: # 動いている最中には送らない (A2) reason = "not_still" break if sends >= retries: reason = "not_converged" break n += 1 entry = {"try": n, "max_err_before": round(cur_err, 2), "joint": k, "sends": 1, "sent": True, "still": None, "max_err_after": None, "actual": None} sends += 1 try: daemon_send_angles(tgt.tolist(), speed=SPEED, wait=SEND_WAIT_SEC) except Exception as e: entry["sent"] = False entry["error"] = f"{type(e).__name__}: {e}" tries.append(entry) print(f" [SETTLE] 送り直し {n} 回目の送信に失敗: {entry['error']}") reason = "send_failed" break after, still, lv = read_until_still() if lv is not None: last_valid = lv if after is None: tries.append(entry) print(f" [SETTLE] 送り直し {n} 回目 最大ずれ {cur_err:.2f}°（関節 j{k}）→ 読めず") actual = None reason = "read_failed" break entry["still"] = bool(still) after_err = float(np.abs(tgt - after[:6]).max()) entry["max_err_after"] = round(after_err, 2) entry["actual"] = _fmt6(after) tries.append(entry) print(f" [SETTLE] 送り直し {n} 回目 最大ずれ {cur_err:.2f}°（関節 j{k}）→ {after_err:.2f}°" + ("" if still else f" (動きが止まらないまま {SETTLE_WAIT_SEC:.1f} 秒で打ち切り)")) actual = after out.update(reason=reason, sends=sends, tries=tries, last_valid_actual=_fmt6(last_valid)) if actual is not None: err = np.abs(tgt - actual[:6]) k = int(err.argmax()) out.update(final_err=round(float(err[k]), 2), joint=k, final_actual=_fmt6(actual)) if sends or reason != "within_tol": gloss = {"within_tol": "もともと範囲内", "converged": "詰まった", "not_converged": "詰まらなかった", "not_still": "動きが止まらない", "send_failed": "送信が失敗", "read_failed": "関節角を読めない"}[reason] tail = ("最後の最大ずれ 不明" if out["final_err"] is None else f"最後の最大ずれ {out['final_err']:.2f}°（関節 j{out['joint']}）") print(f" [SETTLE] {reason} ({gloss}) 送信 {sends} 回 {tail}") if reason != "converged": print(" [WARN] 送り直しで詰めきれませんでした (止めずに次へ進みます)") return out, sends def read_angles_once(): """記録スレッド用: 1 回だけ読む (読めなければ None. 待たない・繰り返さない).""" try: a = daemon_get_angles() except Exception: return None return [float(x) for x in a] if wr._is_valid_angles(a) else None class ArmIO: """巻き戻しと補正で使う送り口. --execute なら daemon, ドライラン (--dry-keys) なら仮の姿勢を動かす. 実機での送り方は推論のチャンクと同じ _send_and_wait_arrival (send_angles speed 20 + 到着確認 3°). グリッパーも推論と同じ (閉 = daemon_set_gripper_value(GRIP_VALUE) / 開 = daemon_set_gripper(0) のあと GRIPPER_SLEEP 待つ). """ def __init__(self, execute, recorder=None, sim_q=None): self.execute = execute self.recorder = recorder self.sim_q = None if sim_q is None else np.asarray(sim_q, dtype=float).copy() def read(self): if self.execute: return read_angles() return None if self.sim_q is None else self.sim_q.copy() def send(self, target, tag): if self.execute: ok, _ = _send_and_wait_arrival(np.asarray(target, dtype=float).tolist(), tag, 3.0) return bool(ok) self.sim_q = np.asarray(target, dtype=float).copy() print(f" [DRY ] {tag}: 送らない (仮の姿勢を " + " ".join(f"{v:+.1f}" for v in self.sim_q) + " に)") return True def gripper(self, state): if self.recorder is not None: self.recorder.gripper(state) if not self.execute: print(f" [DRY ] グリッパー→{'閉' if state == 1 else '開'} (送らない)") return if state == 1: daemon_set_gripper_value(GRIP_VALUE) else: daemon_set_gripper(0) time.sleep(GRIPPER_SLEEP) class KeyReader: """補正モード用: Enter を待たずに 1 文字ずつ読む (termios の cbreak). 端末でないとき (テストでパイプから流すとき) は普通に 1 文字ずつ読む. flush() で溜まったキー (キーリピートで送っている間に溜まった分) を捨てる. """ def __enter__(self): self.fd = None if sys.stdin.isatty(): import termios import tty self.fd = sys.stdin.fileno() self.old = termios.tcgetattr(self.fd) tty.setcbreak(self.fd) return self def get(self): if self.fd is None: return sys.stdin.read(1) # 端末では os.read で 1 バイトずつ読む (Python 側の読み置きに溜めない. 溜まると # flush() で捨てられず, 送ったあとに余分な刻みが出る) import os import select b = os.read(self.fd, 1) if b == b"\x1b": # 矢印キーなど (ESC [ A ...) は丸ごと読み捨てる ('[' を刻みの切り替えと取り違えない) while select.select([self.fd], [], [], 0.02)[0]: os.read(self.fd, 8) return "\x1b" if not b: return "" # 入力の終わり return b.decode("utf-8", errors="ignore") or "\x00" # 日本語入力の途中のバイトなどは無視 def flush(self): if self.fd is not None: import termios termios.tcflush(self.fd, termios.TCIFLUSH) def __exit__(self, *exc): if self.fd is not None: import termios termios.tcsetattr(self.fd, termios.TCSADRAIN, self.old) return False def plan_correction_step(cur, joint, sign, step, q, lo, hi, max_move): """補正の 1 刻み. cur (今の補正目標) の joint を sign*step 動かし, 推論と同じ apply_limits (実測 q から) と check_send_safe を通す. 戻り値: (目標, notes, unsafe).""" want = np.asarray(cur, dtype=float)[:6].copy() want[joint] += sign * step t, notes = apply_limits(want, np.asarray(q, dtype=float)[:6], lo, hi, max_move) return t, notes, check_send_safe(t, lo, hi) REACH_TOL_DEG = 3.0 # 戻れたかの判定 (推論の到着確認 _send_and_wait_arrival(..., 3.0) と同じ) def reached_pose(qa, pose): """実測 qa が pose から REACH_TOL_DEG 以内か. 戻り値: (以内か, 最大ずれ or None).""" if qa is None: return False, None err = float(np.max(np.abs(np.asarray(qa, dtype=float)[:6] - np.asarray(pose, dtype=float)[:6]))) return err <= REACH_TOL_DEG, round(err, 2) def traverse(arm, waypoints, lo, hi, max_move, tag): """中継点を順に送る (巻き戻し・補正の取り消し). 1 点ごとに実測から apply_limits と check_send_safe をかけ直す (推論の中継点と同じ). 通らない点が出たらそこで止める (送らない). 戻り値: (最後まで行けたか, 1 点ごとの記録).""" out = [] for n, wp in enumerate(waypoints): qa = arm.read() if qa is None: print(" [WARN] 関節角を読めない. ここで止めます (送らない)") out.append({"n": n, "waypoint": _fmt6(wp), "sent": False, "why": "read_failed"}) return False, out t, notes = apply_limits(np.asarray(wp, dtype=float), qa[:6], lo, hi, max_move) unsafe = check_send_safe(t, lo, hi) e = {"n": n, "waypoint": _fmt6(wp), "target": _fmt6(t), "notes": notes, "measured_before": _fmt6(qa), "sent": False} for x in notes: print(f" [LIMIT] {tag} {n}: {x}") if unsafe: for u in unsafe: print(f" [ABORT] {tag} {n}: {u}") e["unsafe"] = unsafe out.append(e) return False, out if np.max(np.abs(t - qa[:6])) <= 0.5: # 推論の中継点と同じ「0.5° 以下は送らない」 out.append(e) continue ok = arm.send(t, f"{tag}_{n:02d}") e.update(sent=True, arrived=bool(ok)) print(f" [BACK ] {n + 1}/{len(waypoints)} → " + " ".join(f"{v:+.1f}" for v in t) + (" 着いた" if ok else " 未到達")) if not ok: print(" [WARN] 未到達. j4 の発熱などを確認してください") out.append(e) return True, out def run_rewind(entry, arm, lo, hi, max_move, recorder, grasp=None): """history の 1 件 (推論 1 回分 / 補正 1 回分) を巻き戻す. 戻り値は log.jsonl に足す記録. grasp は つかんだ区間 (グリッパーを閉じた推論) を巻き戻すときだけ渡す (依頼 27): GRASP_PLACED (y = 持っている): 送った目標を逆にたどって つかんだ位置 (最後に送った目標) まで 下ろす → 開いてブロックを置く → 残りを逆にたどって始めの姿勢へ. つかんだ位置まで下ろせなかったら開かない (宙で落とさない. 補正の取り消しと同じ決まり) GRASP_DROPPED (n = 落とした): 今の位置ですぐ開く (閉じた指が置き直したブロックに当たらないよう) → 逆にたどって始めの姿勢へ 送り方・範囲チェックは grasp なしの巻き戻しと同じ traverse.""" t_start = time.time() rec = {"type": "rewind", "rewound": True, "kind": entry["kind"], "chunk": entry.get("chunk"), "correction_id": entry.get("correction_id"), "segment_t": [entry["t_start"], entry["t_end"]], "start_pose": _fmt6(entry["start_pose"]), "sent_targets": [_fmt6(x) for x in entry["sent_targets"]], "t_start": t_start} if grasp is not None: rec.update(grasp_undo=grasp, gripper_opened=False) q0 = arm.read() if q0 is None: print(" [WARN] 関節角を読めないので巻き戻せません") rec.update(finished=False, why="read_failed", t_end=time.time(), waypoints=[]) return rec sent = entry["sent_targets"] name = ('推論 ' + str(entry['chunk']) if entry['kind'] == 'chunk' else '補正 ' + str(entry['correction_id'])) if recorder is not None: recorder.set_mode(MODE_REWIND) opened = False if grasp == GRASP_PLACED: # つかんだ位置 = グリッパーを閉じた時点の姿勢 (閉じる前に最後に送った目標. 送っていなければ始めの姿勢) grasp_pose = np.asarray(sent[-1] if sent else entry["start_pose"], dtype=float)[:6] down = split_leg(q0[:6], grasp_pose, max_move) back = rewind_waypoints(sent, entry["start_pose"], current=grasp_pose, max_move=max_move) print(f"[REWIND] {name} (つかんだ区間) を巻き戻す: つかんだ位置 " + " ".join(f"{v:+.1f}" for v in grasp_pose) + " まで下ろす → 開いて置く → " f"中継点 {len(back)} 点 → 始めの姿勢 " + " ".join(f"{v:+.1f}" for v in np.asarray(entry["start_pose"])[:6])) completed, pts = traverse(arm, down, lo, hi, max_move, "pi05_rewind_down") for e in pts: e["leg"] = "down" qg = arm.read() reached_g, err_g = reached_pose(qg, grasp_pose) rec.update(grasp_pose=_fmt6(grasp_pose), reached_grasp=reached_g, grasp_err_deg=err_g) if completed and reached_g: print("[REWIND] つかんだ位置に着いたのでグリッパーを開いてブロックを置きます") arm.gripper(0) opened = True completed, pts2 = traverse(arm, back, lo, hi, max_move, "pi05_rewind") for e in pts2: e["leg"] = "back" pts += pts2 else: completed = False print("[REWIND] [WARN] つかんだ位置まで下ろせていないので, グリッパーは閉じたままにします (落とさない)") else: if grasp == GRASP_DROPPED: print(f"[REWIND] {name} (つかんだ区間) を巻き戻す: 落としたので先にグリッパーを開きます") arm.gripper(0) opened = True wps = rewind_waypoints(sent, entry["start_pose"], current=q0[:6], max_move=max_move) print(f"[REWIND] {name} を巻き戻す: 中継点 {len(wps)} 点 → 始めの姿勢 " + " ".join(f"{v:+.1f}" for v in np.asarray(entry["start_pose"])[:6])) completed, pts = traverse(arm, wps, lo, hi, max_move, "pi05_rewind") if recorder is not None: recorder.set_mode(MODE_MODEL) qa = arm.read() reached, err = reached_pose(qa, entry["start_pose"]) rec.update(finished=bool(completed and reached and (grasp is None or opened)), traverse_completed=completed, reached_start=reached, start_err_deg=err, waypoints=pts, t_end=time.time(), measured_after=_fmt6(qa)) if grasp is not None: rec["gripper_opened"] = opened if not completed: print("[REWIND] 途中で止めました (送れない中継点). h で HOME に戻せます") elif not reached: print(f"[REWIND] [WARN] 始めの姿勢まで戻りきっていません (最大ずれ " f"{'不明' if err is None else f'{err:.2f}°'} > {REACH_TOL_DEG:.0f}°). " "この run は補正エピソードに使いません") return rec def run_correction(arm, keys, lo, hi, max_move, grip, corr_id, chunk, log_f, recorder): """補正モード. 戻り値: (結果 dict, 補正後のグリッパー状態). 結果 dict の action は accept (この姿勢から推論に戻る) / cancel (取り消して戻った).""" start_q = arm.read() if start_q is None: print("[CORR ] 関節角を読めないので補正に入れません") return None, grip start_q = start_q[:6].copy() start_grip = grip cur = start_q.copy() joint, si = 0, CORRECTION_DEFAULT_STEP_INDEX t_start = time.time() sent, n_steps = [], 0 resync = False # 直前の 1 刻みが未到達 → 次の刻みは実測から数える grip_used = False # 補正中に一度でもグリッパーを動かしたか (往復でも True. codex R1-2) if recorder is not None: recorder.set_mode(MODE_CORRECT) print(f"[CORR ] 補正 {corr_id} に入りました (推論 {chunk} のあと)") print(CORRECTION_HELP) def _status(): print(f" [補正] 関節 j{joint} 刻み {CORRECTION_STEPS[si]:.1f}° 補正目標 " + " ".join(f"j{k}={v:+.1f}" for k, v in enumerate(cur)) + f" グリッパー {'閉' if grip == 1 else '開'}") _status() while True: ch = keys.get() act = parse_correction_key(ch) if act is None: continue kind, val = act if kind == "help": print(CORRECTION_HELP); _status() elif kind == "select": joint = val; _status() elif kind == "step": si = next_step_index(si, val); _status() elif kind == "move": qa = arm.read() if qa is None: print(" [補正] 関節角を読めない. この 1 刻みは送りません") keys.flush(); continue if resync: cur = qa[:6].copy() resync = False step = CORRECTION_STEPS[si] t, notes, unsafe = plan_correction_step(cur, joint, val, step, qa, lo, hi, max_move) e = {"type": "correction", "action": "move", "correction_id": corr_id, "after_chunk": chunk, "joint": joint, "step_deg": step, "sign": val, "t": time.time(), "before": _fmt6(cur), "after": _fmt6(t), "measured_before": _fmt6(qa), "notes": notes, "sent": False} for x in notes: print(f" [LIMIT] {x}") if unsafe: for u in unsafe: print(f" [補正] 送りません: {u}") e["unsafe"] = unsafe elif np.max(np.abs(t - cur)) <= 1e-6: print(f" [補正] j{joint} は範囲の端なのでこれ以上動かせません") else: ok = arm.send(t, f"pi05_corr{corr_id:02d}_{n_steps:03d}") e.update(sent=True, arrived=bool(ok), measured_after=_fmt6(arm.read())) e["delta"] = round(float(t[joint] - cur[joint]), 3) cur = t sent.append(t.copy()) n_steps += 1 if not ok: print(" [WARN] 未到達. j4 の発熱などを確認してください") # codex R1/R2 A3: 次の 1 刻みは届いた所 (次の move で読む実測) から数える resync = True _status() log_f.write(json.dumps(e) + "\n"); log_f.flush() keys.flush() # 送っている間にキーリピートで溜まった分は捨てる (送りすぎない) elif kind == "grip": new = 0 if grip == 1 else 1 arm.gripper(new) log_f.write(json.dumps({"type": "correction", "action": "gripper", "correction_id": corr_id, "after_chunk": chunk, "t": time.time(), "before": grip, "after": new}) + "\n"); log_f.flush() grip = new grip_used = True _status() keys.flush() elif kind == "accept": res = {"type": "correction", "action": "accept", "correction_id": corr_id, "after_chunk": chunk, "t_start": t_start, "t_end": time.time(), "start_pose": _fmt6(start_q), "final_target": _fmt6(cur), "measured_after": _fmt6(arm.read()), "steps_sent": n_steps, "grip_before": start_grip, "grip_after": grip, "grip_used": grip_used, "sent_targets": [_fmt6(x) for x in sent]} if recorder is not None: recorder.set_mode(MODE_MODEL) print(f"[CORR ] 補正 {corr_id} を終えました ({n_steps} 刻み). この姿勢から推論に戻ります") return res, grip elif kind == "cancel": print(f"[CORR ] 補正 {corr_id} を取り消して, 始めの姿勢へ戻ります") if recorder is not None: recorder.set_mode(MODE_REWIND) q0 = arm.read() completed, pts = False, [] if q0 is not None: wps = rewind_waypoints(sent, start_q, current=q0[:6], max_move=max_move) if sent else [] completed, pts = traverse(arm, wps, lo, hi, max_move, "pi05_corr_cancel") else: print(" [WARN] 関節角を読めないので戻せません") qa = arm.read() reached, err = reached_pose(qa, start_q) if grip != start_grip: if completed and reached: arm.gripper(start_grip) grip = start_grip else: # codex R1 C3: 姿勢が戻っていないのにグリッパーだけ戻さない (物を落とさない) print(" [WARN] 始めの姿勢へ戻れていないので, グリッパーは今のままにします") if recorder is not None: recorder.set_mode(MODE_MODEL) res = {"type": "correction", "action": "cancel", "correction_id": corr_id, "after_chunk": chunk, "t_start": t_start, "t_end": time.time(), "start_pose": _fmt6(start_q), "steps_sent": n_steps, "finished": bool(completed and reached and grip == start_grip), "traverse_completed": completed, "reached_start": reached, "start_err_deg": err, "waypoints": pts, "measured_after": _fmt6(qa), "grip_after": grip, "grip_used": grip_used} return res, grip def write_trial_json(run_dir, run_stem, args, recorded, end_reason, outcome, history_log, n_rewinds, n_corrections, exit_code): """{run_stem}.json. お手本の試行 JSON と同じキー (recording_enabled / result / judgments) を持つ. 結果 y なら pick / place とも human_verdict=yes (build_demo_episodes の採否と同じ読み方).""" verdict = {"y": "yes", "n": "no"}.get(outcome) result = ("completed" if end_reason in ("h", "q", "max_chunks") else "interrupted" if end_reason == "ctrl_c" else "aborted") j = {"script": "run_policy_client.py", "source": "pi05_policy", "run_stem": run_stem, "run_dir": str(run_dir), "recording_enabled": bool(recorded), "end_reason": end_reason, "result": result, "exit_code": exit_code, "outcome": {"y": "success", "n": "failure"}.get(outcome), "rewinds": n_rewinds, "corrections": n_corrections, "history_left": history_log, "args": {k: (str(v) if isinstance(v, Path) else v) for k, v in vars(args).items()}, "ended_at": datetime.now().isoformat()} if verdict is not None: j["judgments"] = {"pick": {"human_verdict": verdict, "verdict": verdict, "source": "human"}, "place": {"human_verdict": verdict, "verdict": verdict, "source": "human"}} (Path(run_dir) / f"{run_stem}.json").write_text(json.dumps(j, indent=1, ensure_ascii=False)) def ask_outcome(): while True: a = input(" 結果: y=成功 / n=失敗 / Enter=記録しない > ").strip().lower() if a in ("y", "n", ""): return a or None def notify_recorder(sock, payload): try: payload = dict(payload, ts=time.time()) sock.sendto((json.dumps(payload) + "\n").encode(), RECORDER) except Exception: pass def main(): ap = argparse.ArgumentParser() ap.add_argument("--host", default="localhost") ap.add_argument("--port", type=int, default=8000) ap.add_argument("--execute", action="store_true", help="アームに送る (未指定はドライラン)") ap.add_argument("--auto", action="store_true", help="Enter 確認なしで連続実行") ap.add_argument("--steps-per-chunk", type=int, default=5, help="1 チャンクで何ステップ先まで進むか (0.1 秒単位)") ap.add_argument("--max-chunks", type=int, default=60) ap.add_argument("--max-move-deg", type=float, default=20.0, help="1 回の移動で各関節が動ける上限") ap.add_argument("--raw-dir", required=True, help="安全範囲の算出に使う学習エピソード (必須. 学習に使った raw_episodes を指定する)") ap.add_argument("--prompt", default=TASK) ap.add_argument("--record", action="store_true", help="recorder.py --top に上カメラ動画を記録させる") ap.add_argument("--ignore-framing", action="store_true", help="上カメラの写り方がお手本と違っても続ける (既定は何も送らずに終了)") ap.add_argument("--state-from-target", action="store_true", help="モデルに渡す state の 6 関節を, 実測ではなく直前に送った目標にする " "(既定 off = 実測). 肩の不足がチャンクごとに積み重なるのを防ぐ") ap.add_argument("--via-stride", type=int, default=0, help="チャンクの途中の姿勢も中継点として順に送る (0 始まりで N-1, 2N-1, ... の " "ステップ. 既定 0 = 従来どおり選んだ 1 点だけ送る)") ap.add_argument("--settle-tol", type=float, default=1.0, help="送ったあとの残りのずれ (動きが止まってから測る) がこれを超えていたら " "同じ目標を送り直す (度)") ap.add_argument("--settle-retries", type=int, default=2, help="settle で同じ目標を送り直す回数の上限 (送信の試行回数). " "0 で送り直さない = 従来の動き") ap.add_argument("--dry-keys", action="store_true", help="ドライランでも 1 回ごとに止まり, b (巻き戻し) / c (補正) の流れを試す. " "アームにも daemon にも触らず, HOME から始まる仮の姿勢を動かす") args = ap.parse_args() if args.dry_keys and (args.execute or args.auto): sys.exit("[ERROR] --dry-keys はドライラン用 (--execute / --auto と一緒には使えない)") if not DATA_ROOT.exists(): sys.exit(f"[ERROR] SSD 未マウント: {DATA_ROOT}") lo, hi, n_eps = load_envelope(Path(args.raw_dir)) print(f"[SAFE ] 安全範囲 (学習 {n_eps} 本 ±{ENVELOPE_MARGIN_DEG:.0f}°): " + " ".join(f"j{k}[{lo[k]:+.0f},{hi[k]:+.0f}]" for k in range(6))) print(f"[SAFE ] 肩 j1 の上限は J1_SAFE_MAX={J1_SAFE_MAX:.0f}° で切った " f"(これを超える指令はアームが黙って捨てる / LOG352) 元データ: {args.raw_dir}") stamp = datetime.now().strftime("%Y%m%d_%H%M%S") log_dir = LOG_ROOT / f"pi05_{stamp}" (log_dir / "frames").mkdir(parents=True, exist_ok=True) log_f = (log_dir / "log.jsonl").open("w") run_stem = f"pi05_{stamp}" mode = "EXECUTE" if args.execute else "DRY-RUN (アームには送らない)" print(f"[MODE ] {mode} steps/chunk={args.steps_per_chunk} max_move={args.max_move_deg}° log={log_dir}") # 以降は try/finally の中だけで資源を触る (codex R3: 途中で終了しても cap / log_f を必ず閉じる). # sys.exit は SystemExit を投げるので, この finally を通って後始末される. cap = policy = rec_sock = None exit_code = 0 grip = 0 grip_opened = False # 開始時のグリッパー開放を済ませたか (codex R2) last_sent_target = None # 直前のチャンクで最後に送った目標 (--state-from-target 用) recorder = None # --execute のときの 1 秒 10 コマの記録 (policy_recorder.py) arm = None # 巻き戻し・補正の送り口 (ArmIO) history = [] # 巻き戻せる区間 (推論 1 回分 / 補正 1 回分). 新しいものが後ろ n_rewinds = n_corrections = 0 end_reason, outcome = None, None try: cams = camera_resolver.resolve_cameras(require_top=True, require_side=False, verbose=False) cap = cv2.VideoCapture(cams["top"]) if not cap.isOpened(): sys.exit("[ERROR] 上カメラを開けない") t_cam = time.time() while time.time() - t_cam < 8.0: # 連係カメラは映像が来るまで数秒かかることがある ok, _f = cap.read() if ok and _f is not None: break time.sleep(0.2) else: sys.exit("[ERROR] 上カメラから映像が来ない. iPhone のロック解除・連係カメラの接続を確認 " "(Photo Booth の『カメラ』メニューに iPhone が出るか)") # E (2026-10-21): 上カメラの写り方は 2 通りあり, カメラを開くたびに切り替わることがある. # お手本と違う写り方のまま動かすと, 学習と別の画像を見せることになる # (082645 は dy=+37.5 のまま実機を動かした. その 1 分前のドライラン 082512 は dy=-7.8). # アームは HOME に置いた状態で, 1 枚目を お手本の 1 枚目 (同じく HOME) と比べる. # **この確認が通るまで daemon にも触らない** (アームには何も送らない). ref_bgr, ref_path = load_reference(Path(args.raw_dir)) bgr0 = None for _ in range(10): _rgb0, bgr0 = grab_rgb(cap) if bgr0 is not None: break time.sleep(0.2) if bgr0 is None: sys.exit("[ERROR] 上カメラの 1 枚目を撮れない") dx, dy, resp = measure_framing(ref_bgr, bgr0) verdict, why = framing_verdict(dx, dy, resp) print(f"[CAMERA] 写り方 dy={dy:+.1f} dx={dx:+.1f} response={resp:.3f} → {verdict} ({why})") print(f"[CAMERA] 基準: {ref_path}") log_f.write(json.dumps({"type": "framing", "dy": round(dy, 2), "dx": round(dx, 2), "response": round(resp, 3), "verdict": verdict, "why": why, "max_dy": FRAMING_MAX_DY, "max_dx": FRAMING_MAX_DX, "min_response": FRAMING_MIN_RESPONSE, "ok": verdict == "OK", "ignored": bool(args.ignore_framing), "reference": str(ref_path)}) + "\n") log_f.flush() if verdict != "OK": if not args.ignore_framing: sys.exit(f"[ABORT ] 上カメラの写り方が『{verdict}』. アームには何も送らずに終了しました.\n" f" {why}\n" " pi05/camera_align.py で合わせ直す " "(iPhone を付け直す / 連係カメラを開き直す).\n" " この写り方のまま続けるなら --ignore-framing") print(f"[WARN ] 写り方が『{verdict}』だが --ignore-framing が指定されたので続けます") # ---- ここから先でアームに触る (写り方の確認を通ったあと) ---- if not args.dry_keys: # --dry-keys は daemon に触らない (アームなしで試す) get_daemon_socket() policy = PolicyClient(args.host, args.port) print(f"[POLICY] 接続 {policy.uri} metadata={policy.metadata}") rec_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) if args.record: notify_recorder(rec_sock, {"type": "trial_start", "run_stem": run_stem}) if args.execute: # 1 秒 10 コマの記録 (上カメラ + 実測角). ここから先, 上カメラは記録スレッドだけが読む install_daemon_lock() recorder = PolicyRecorder(log_dir, cap, read_angles_once, frame_to_model) recorder.start() print(f"[RECORD] 上カメラと実測角を 1 秒 10 コマで記録します → {log_dir / 'recording'}") arm = ArmIO(args.execute, recorder, sim_q=(HOME if args.dry_keys else None)) if args.dry_keys: print("[DRY ] --dry-keys: アームには送らず, HOME から始まる仮の姿勢で流れを試します") q = arm.read() if args.dry_keys else read_angles() if q is None: sys.exit("[ERROR] 関節角を読めない (daemon を確認)") if np.max(np.abs(q - np.array(HOME))) > 5.0: print(f"[WARN ] HOME から離れています (max|Δ|={np.max(np.abs(q - np.array(HOME))):.1f}°). " "学習データは HOME から始まっています") print(" 現在角: " + " ".join(f"j{k}={v:+.1f}" for k, v in enumerate(q))) if args.execute: # 開始時の開放は, 最初に送る目標が検査を通ってからにする (codex R2). # 以前はここで開いていたので, 1 チャンク目が安全ガードで止まる場合でも # 爪だけ先に開いてしまっていた (物を持っていれば落とす). print("[GRIP ] 開始時のグリッパー開放は, 最初の目標が検査を通ってから行います " "(爪が閉じていたら手で開けておく)") end_reason = "max_chunks" for i in range(args.max_chunks): q = arm.read() if args.dry_keys else read_angles() if q is None: print("[ERROR] 関節角を読めない (daemon を確認). 停止"); exit_code = 1; end_reason = "error"; break rgb = bgr_small = None for _ in range(10): rgb, bgr_small = grab_rgb(cap) if recorder is None else recorder.grab() if rgb is not None: break time.sleep(0.2) if rgb is None: print("[ERROR] 上カメラの映像が途切れた (iPhone のスリープ等). 停止") exit_code = 1; break # モデルに渡す state の 6 関節 (--state-from-target のときだけ「直前に送った目標」). # apply_limits / check_send_safe / 中継点のかけ直しは今までどおり実測 q を使う. measured_state = np.concatenate([q, [float(grip)]]).astype(np.float32) state_joints, state_source = q, "measured" if args.state_from_target: if last_sent_target is None: print("[STATE ] 実測を渡す (直前に送った目標がない)") else: gap = float(np.max(np.abs(last_sent_target - q[:6]))) if gap > STATE_TARGET_MAX_GAP_DEG: print(f"[WARN ] 目標と実測の差が {gap:.2f}° > {STATE_TARGET_MAX_GAP_DEG:.0f}° " "なので実測を渡します (腕がどこかに当たって止まっている可能性)") else: state_joints, state_source = last_sent_target, "target" print(f"[STATE ] {'目標' if state_source == 'target' else '実測'}を渡す " f"目標と実測の最大差 {gap:.2f}°" f"（関節 j{int(np.argmax(np.abs(last_sent_target - q[:6])))}）") state = np.concatenate([state_joints, [float(grip)]]).astype(np.float32) t0 = time.time() out = policy.infer({"observation/image": rgb, "observation/state": state, "prompt": args.prompt}) latency = time.time() - t0 acts = np.asarray(out["actions"], dtype=float) raw_target, grip_cmd, used = plan_chunk(acts, q, grip, args.steps_per_chunk) # plan_chunk は, グリッパーが切り替わるチャンクでは used-1 行目 (used=0 なら実測角) # を目標に返す. actions の何行目を使ったかを記録に残す (codex A1). final_raw_index = (used if grip_cmd is None else (used - 1 if used > 0 else None)) # 中継点の計画 (--via-stride 0 なら used の 1 点だけ = 従来と同じ). plan = plan_vias(acts, q, raw_target, used, args.via_stride, lo, hi, args.max_move_deg, final_raw_index) target, notes = plan[-1]["target"], plan[-1]["notes"] # チャンクの最終目標 delta = target - q cv2.imwrite(str(log_dir / "frames" / f"{i:04d}.jpg"), bgr_small) rec = {"chunk": i, "t": time.time(), "latency_s": round(latency, 3), "state": state.tolist(), "state_source": state_source, "measured_state": measured_state.tolist(), "actions": acts.tolist(), "used_step": used, "target": target.tolist(), "grip_cmd": grip_cmd, "notes": notes, "executed": False} gtxt = "" if grip_cmd is None else (" グリッパー→閉" if grip_cmd == 1 else " グリッパー→開") print(f"[{i:03d}] 推論 {latency*1000:.0f}ms {used + 1}ステップ先 Δ=" + " ".join(f"{d:+5.1f}" for d in delta) + gtxt) for n in notes: print(f" [LIMIT] {n}") if args.via_stride > 0: rec["vias"] = [{"step": v["step"], "raw_index": v["raw_index"], "raw": _fmt6(v["raw"]), "planned_target": _fmt6(v["target"]), "target": _fmt6(v["target"]), "notes": v["notes"], "called": False, "success": None, "retried": None, "actual_after": None} for v in plan] print(f" [VIA ] 中継点 {len(plan)} 点: step " + ", ".join(str(v["step"]) for v in plan)) for v in plan: for n in v["notes"]: print(f" [LIMIT] step {v['step']}: {n}") if not args.execute: # ドライランは中継点の一覧だけ出して送らない print(f" [VIA ] step {v['step']:2d} Δ=" + " ".join(f"{d:+5.1f}" for d in v["delta"])) # 送る直前の最終検査 (codex R1 / C2). 中継点は 1 点でも引っかかったら # **どれも送らずに** 止める (途中まで動かして止まる方が危ない). # 送信にもグリッパー操作にも入らず止める. # HOME 復帰 (h) だけはこの検査を通さずに送れる (範囲の外にいるアームを戻す手段). unsafe = [] for v in plan: for u in check_send_safe(v["target"], lo, hi): unsafe.append(u if args.via_stride <= 0 else f"step {v['step']}: {u}") stopped = False if not unsafe and (args.execute or args.dry_keys): # ここから先で送れなかった経路 (s / 送信なし / 中断) は, 次のチャンクで # 実測を渡すために None のままにする (--state-from-target 用). last_sent_target = None grasp = None # つかんだ区間を巻き戻すときの答え (GRASP_PLACED / GRASP_DROPPED) if not args.auto: # 入力を待っている間は wait (人が手でブロックを置き直す. 学習データに使わない. 依頼 27) if recorder is not None: recorder.set_mode(MODE_WAIT) while True: grasp = None ans = input(" Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / " "h=HOMEに戻して終了 / q=終了 > ").strip().lower() if ans != "b": break # 巻き戻せない場合は断って聞き直す (アームは動かさない) if not history: print(" [REWIND] 巻き戻せる区間がありません (まだ何も実行していない)") continue if history[-1].get("blocked"): print(" [REWIND] 直前の巻き戻し / 補正の取り消しが始めの姿勢まで戻りきれなかったので, " "これより前へは巻き戻しません (h で HOME に戻せます)") continue if history[-1]["grip_changed"]: h_last = history[-1] if h_last["kind"] == "chunk" and h_last.get("grip_cmd") == 1: # つかんだ区間 (依頼 27): 持っているか落としたかを 1 文字で聞く a = input(f" [REWIND] 推論 {h_last['chunk']} はつかんだ区間です. " "y=持っている（下ろして置いてから戻る） / " "n=落とした（先に開いてから戻る） / その他=やめる > ").strip().lower() if a == "y": grasp = GRASP_PLACED elif a == "n": grasp = GRASP_DROPPED else: print(" [REWIND] やめました (アームは動かしていません)") continue break if h_last["kind"] == "chunk" and h_last.get("grip_cmd") == 0: print(f" [REWIND] 直前の区間 (推論 {h_last['chunk']}) でグリッパーを開いた" "ので巻き戻しません. ブロックを置き直すなら h で終えてやり直してください") continue print(f" [REWIND] 直前の区間 ({'推論 ' + str(h_last['chunk']) if h_last['kind'] == 'chunk' else '補正 ' + str(h_last['correction_id'])})" " でグリッパーを動かしたので巻き戻しません") continue break if recorder is not None and ans not in ("q", "h"): recorder.set_mode(MODE_MODEL) if ans == "q": end_reason = "q" log_f.write(json.dumps(rec) + "\n"); log_f.flush() if recorder is not None: recorder.stop() outcome = ask_outcome() break if ans == "h": end_reason = "h" if recorder is not None: recorder.set_mode(MODE_HOME) if args.dry_keys: print(" [DRY ] HOME へは送らない (ドライラン)") else: ok_home, _ = _send_and_wait_arrival(HOME, "pi05_home", 2.5) rec["home_requested"] = True rec["home_arrived"] = bool(ok_home) log_f.write(json.dumps(rec) + "\n"); log_f.flush() if recorder is not None: recorder.stop() outcome = ask_outcome() break if ans == "s": log_f.write(json.dumps(rec) + "\n"); continue if ans == "b": # 直前の 1 回分を巻き戻して, 戻った姿勢からもう一度推論する rec["rewind_requested"] = True log_f.write(json.dumps(rec) + "\n"); log_f.flush() entry = history.pop() rw = run_rewind(entry, arm, lo, hi, args.max_move_deg, recorder, grasp=grasp) rw["after_chunk"] = i n_rewinds += 1 if rw.get("gripper_opened"): grip = 0 if grasp == GRASP_DROPPED: rw["start_frame"] = str(log_dir / "frames" / f"{entry['chunk']:04d}.jpg") if not rw["finished"]: # codex R2-2: 戻りきれなかったら境界を積む (次の b で古い区間の経路へ飛ばない) history.append({"kind": "blocked", "blocked": True, "chunk": entry.get("chunk"), "correction_id": entry.get("correction_id"), "grip_changed": False, "t_start": rw["t_start"], "t_end": rw["t_end"]}) log_f.write(json.dumps(rw) + "\n"); log_f.flush() if grasp == GRASP_DROPPED and rw.get("gripper_opened"): # 落としたブロックを人が元の位置へ置き直す間は wait (手が写る) print(f" [REWIND] 元の位置の目安 (推論 {entry['chunk']} の始めの写真): " f"{rw['start_frame']}") if recorder is not None: recorder.set_mode(MODE_WAIT) input(" ブロックを元の位置へ置き直したら Enter で続ける > ") if recorder is not None: recorder.set_mode(MODE_MODEL) continue if ans == "c": rec["correction_requested"] = True log_f.write(json.dumps(rec) + "\n"); log_f.flush() n_corrections += 1 with KeyReader() as keys: cr, grip = run_correction(arm, keys, lo, hi, args.max_move_deg, grip, n_corrections, i, log_f, recorder) if cr is not None: log_f.write(json.dumps(cr) + "\n"); log_f.flush() if cr["grip_used"]: # codex R2-1: 補正でグリッパーを動かしたら, 開始時の開放は済んだことにする # (このあと最初の推論の実行で勝手に開いて物を落とさない) grip_opened = True if cr["action"] == "cancel" and not cr["finished"]: history.append({"kind": "blocked", "blocked": True, "chunk": None, "correction_id": n_corrections, "grip_changed": False, "t_start": cr["t_start"], "t_end": cr["t_end"]}) if cr["action"] == "accept" and (cr["steps_sent"] or cr["grip_used"]): history.append({"kind": "correction", "correction_id": n_corrections, "chunk": None, "start_pose": np.asarray(cr["start_pose"], dtype=float), "sent_targets": [np.asarray(x, dtype=float) for x in cr["sent_targets"]], "grip_changed": cr["grip_used"], "t_start": cr["t_start"], "t_end": cr["t_end"]}) continue if not unsafe and args.dry_keys: # --dry-keys の Enter: 送らずに仮の姿勢を目標へ移す (b / c の流れを試すため) t_ex0 = time.time() sent = [np.asarray(v["target"], dtype=float).copy() for v in plan] arm.send(sent[-1], f"pi05_chunk{i:03d}") if grip_cmd is not None: arm.gripper(grip_cmd) grip = grip_cmd history.append({"kind": "chunk", "chunk": i, "correction_id": None, "start_pose": q[:6].copy(), "sent_targets": sent, "grip_changed": grip_cmd is not None, "grip_cmd": grip_cmd, "t_start": t_ex0, "t_end": time.time()}) rec["simulated"] = True if not unsafe and args.execute: t_exec_start = time.time() sent_targets = [] # 実際に送った目標 (送った順. 巻き戻しでこれを逆にたどる) if not grip_opened: # 開始時の開放はここで 1 度だけ (写り方の確認と目標の検査を通り, 送ると決めてから) print("[GRIP ] 開始時にグリッパーを開く") if recorder is not None: recorder.gripper(0) daemon_set_gripper(0) time.sleep(GRIPPER_SLEEP) grip_opened = True rec["initial_gripper_open"] = True broke, final_target, final_sent, sent_any = False, None, False, False for vi, v in enumerate(plan): v_last = vi == len(plan) - 1 tag = (f"pi05_chunk{i:03d}" if args.via_stride <= 0 else f"pi05_chunk{i:03d}_via{v['step']:02d}") tgt, vdelta = v["target"], v["delta"] if args.via_stride > 0: # 送る直前に実測角を読み, そこから apply_limits と check_send_safe を # かけ直す (codex A2: 1 点だけ送る従来の方式と同じ基準にそろえる). qa = read_angles() if qa is None: print(" [WARN] 送る直前の関節角を読めない. " "この中継点も残りの中継点も送りません") print(" [WARN] 未到達. j4 の発熱などを確認してください") rec["arrived"] = False broke = True if args.auto: exit_code = 1 log_f.write(json.dumps(rec) + "\n") stopped = True break tgt, notes_now = apply_limits(v["raw"], qa, lo, hi, args.max_move_deg) if notes_now != v["notes"]: for n in notes_now: print(f" [LIMIT] step {v['step']} (実測から): {n}") rec["vias"][vi].update(target=_fmt6(tgt), notes=notes_now) u2 = check_send_safe(tgt, lo, hi) if u2: unsafe = [f"step {v['step']}: {x}" for x in u2] broke = True break vdelta = tgt - qa # 送らない判定は実測との差で見る (codex ❌1) final_target = tgt if np.max(np.abs(vdelta)) <= 0.5: if args.via_stride > 0: print(f" [VIA ] step {v['step']:2d} Δ=" + " ".join(f"{d:+5.1f}" for d in vdelta) + " 送らず (0.5° 以下)") continue ok, retried = _send_and_wait_arrival(tgt.tolist(), tag, 3.0) sent_any = True sent_targets.append(np.asarray(tgt, dtype=float).copy()) if args.via_stride > 0: rec["vias"][vi].update(called=True, success=bool(ok), retried=bool(retried), actual_after=_fmt6(read_angles())) print(f" [VIA ] step {v['step']:2d} Δ=" + " ".join(f"{d:+5.1f}" for d in vdelta) + (" 着いた" if ok else " 未到達")) if v_last: rec["arrived"] = bool(ok) final_sent = True if not ok: print(" [WARN] 未到達. j4 の発熱などを確認してください") if args.auto: exit_code = 1 log_f.write(json.dumps(rec) + "\n") stopped = True broke = True break if v_last: # 残ったずれを同じ目標の送り直しで詰める (LOG359). 止める条件は増やさない. rec["settle"], rec["settle_retries_used"] = settle_target( tgt, f"pi05_chunk{i:03d}", args.settle_tol, args.settle_retries) if not broke and args.via_stride > 0 and not final_sent and final_target is not None: # 最後の中継点が「送らない」になった場合も, チャンクの最後の目標について # 到達の記録と settle は必ず行う (codex ❌2: 重複送信を省くことと分ける). qa = read_angles() rec["arrived"] = bool(qa is not None and float(np.max(np.abs(final_target - qa[:6]))) <= 3.0) rec["settle"], rec["settle_retries_used"] = settle_target( final_target, f"pi05_chunk{i:03d}", args.settle_tol, args.settle_retries) if not broke: # 次のチャンクで --state-from-target が使う「直前に送った目標」. # 送らなかった (s / 送信なし / 中断) ときは None に戻して実測を渡す. last_sent_target = (np.asarray(final_target, dtype=float).copy() if sent_any and final_target is not None else None) if grip_cmd is not None: if recorder is not None: recorder.gripper(grip_cmd) if grip_cmd == 1: daemon_set_gripper_value(GRIP_VALUE) else: daemon_set_gripper(0) time.sleep(GRIPPER_SLEEP) grip = grip_cmd rec["executed"] = True # 巻き戻し (b) 用に, この推論で実際に送った目標と始めの姿勢 (実測) を残す. # 途中で止まった (broke) 推論も, 1 点でも送っていれば残す (codex R1-3: # 残さないと次の b が 1 つ古い区間を巻き戻してしまう) t_exec_end = time.time() rec["t_exec_start"], rec["t_exec_end"] = t_exec_start, t_exec_end rec["sent_targets"] = [_fmt6(x) for x in sent_targets] grip_moved = (not broke) and grip_cmd is not None if sent_targets or grip_moved: history.append({"kind": "chunk", "chunk": i, "correction_id": None, "start_pose": q[:6].copy(), "sent_targets": sent_targets, "grip_changed": grip_moved, "grip_cmd": grip_cmd if grip_moved else None, "partial": bool(broke), "t_start": t_exec_start, "t_end": t_exec_end}) if stopped: break if unsafe: # 計画の検査 または 送る直前のかけ直しで範囲の外になったとき. # 送信にもグリッパー操作にも入らず止める. # HOME 復帰 (h) だけはこの検査を通さずに送れる (範囲の外にいるアームを戻す手段). rec["unsafe"] = unsafe rec["envelope"] = {"lo": [round(float(v), 2) for v in lo], "hi": [round(float(v), 2) for v in hi]} for u in unsafe: print(f" [ABORT] {u}") print("[STOP ] 安全でない目標のため, 送らずに停止します " "(現在角が安全範囲の外にあると, 移動量の制限で範囲の外に戻る)") # 記録は入力より前に書いて flush する (codex C2-1: 入力待ちや HOME 復帰中の # Ctrl-C でこのチャンクの unsafe / envelope が消えないように). log_f.write(json.dumps(rec) + "\n"); log_f.flush() exit_code = 1 if args.execute and not args.auto: if recorder is not None: recorder.set_mode(MODE_WAIT) # 入力待ちは wait (codex 依頼 27 P3) ans = input(" h=HOMEに戻して終了 / 他=そのまま終了 > ").strip().lower() if ans == "h": if recorder is not None: recorder.set_mode(MODE_HOME) ok_home, _ = _send_and_wait_arrival(HOME, "pi05_home", 2.5) log_f.write(json.dumps({"chunk": i, "type": "home_return", "t": time.time(), "home_requested": True, "home_arrived": bool(ok_home)}) + "\n") log_f.flush() else: print(" アームを HOME に置き直してからやり直してください") break log_f.write(json.dumps(rec) + "\n"); log_f.flush() except KeyboardInterrupt: print("\n[STOP ] Ctrl-C") end_reason = "ctrl_c" finally: # finally ではアームを動かさない (HOME 復帰もサーボ解放もしない). 止まった姿勢は # そのまま保つ: 未知の姿勢から勝手に動かす方が危ない. 戻すときは h で明示的に送る. if recorder is not None: recorder.stop() try: write_trial_json(log_dir, run_stem, args, bool(recorder.stopped_cleanly), end_reason if not (exit_code and end_reason in (None, "max_chunks")) else "error", outcome, [{k: v for k, v in h_.items() if k in ("kind", "chunk", "correction_id", "grip_changed")} for h_ in history], n_rewinds, n_corrections, exit_code) except Exception as e: print(f"[WARN ] {run_stem}.json を書けない: {e}") if args.record and rec_sock is not None: notify_recorder(rec_sock, {"type": "trial_end", "run_stem": run_stem}) if not log_f.closed: log_f.close() if cap is not None and not (recorder is not None and recorder.alive()): cap.release() # 記録スレッドが止まらなかったときは閉じない (読みかけと競合する) if policy is not None: policy.close() try: close_daemon_socket() except Exception: pass print(f"[DONE ] 記録: {log_dir}") # 終了コードは正常に抜けた経路でだけ表示する (sys.exit で抜ける場合は, その [ABORT] / # [ERROR] の文言が出て終了コード 1 になるので, ここで 0 と書くと食い違う). if exit_code: print(f"[DONE ] 終了コード {exit_code} (安全停止 / 途中のエラー)") return exit_code if __name__ == "__main__": sys.exit(main()) ```` ### pi05/policy_recorder.py ````python #!/usr/bin/env python3 """policy_recorder.py — 実機推論 (run_policy_client.py --execute) の間, 上カメラと実測角を 1 秒 10 コマで残す (2026-10-29). お手本 (record_teach_demo.py + recorder.py --top) と同じ形で書くので, build_demo_episodes.build_one() がそのまま読める: {run_dir}/recording/states.csv recorder.py と同じ列. actual / gripper_cmd / stage 行. stage 列 = その時の状態 (model / rewind / correct / home / wait) {run_dir}/recording/top.mp4 上カメラ (元の大きさ. 10 コマ/秒) {run_dir}/recording/top_frame_log.csv frame_idx, capture_ts (+ mode 列. 1 コマごとの状態) {run_dir}/recording/top_meta.json コマ数・実効 fps など アームへは何も送らない (関節角を読むだけ). daemon のソケットは 1 本を本体と共用するので, install_daemon_lock() で 1 回のやりとりごとに鍵をかける (読み書きが混ざらないように). """ import csv import json import threading import time from pathlib import Path import cv2 import visual_servo_pick as vsp RECORD_FPS = 10.0 FRAME_INTERVAL = 1.0 / RECORD_FPS ANGLE_INTERVAL = 1.0 / RECORD_FPS FOURCC = "avc1" # recorder.py と同じ STATES_COLS = ["ts", "event_type", "stage", "j0", "j1", "j2", "j3", "j4", "j5", "speed", "gripper_state", "ts_send", "ts_arrival", "attempt", "arrived", "note"] # recorder.py の STATES_COLS と同じ FRAME_LOG_COLS = ["frame_idx", "capture_ts", "mode"] FLUSH_INTERVAL = 1.0 STOP_JOIN_SEC = 5.0 # stop() でスレッドの終わりを待つ上限 FRAME_WAIT_SEC = 2.0 # 本体が画像を求めたとき, 新しいコマを待つ上限 def install_daemon_lock(): """visual_servo_pick._daemon_request を「1 回のやりとりごとに鍵をかける」版に差し替える. daemon_get_angles / daemon_send_angles / daemon_set_gripper* はどれも モジュールの _daemon_request を呼ぶので, ここを差し替えれば全部に効く. 記録スレッドの get_angles と本体の send_angles が同じソケットで混ざらないようにする. 中身 (送る内容・タイムアウト) は変えない. 2 回呼んでも 1 回だけ差し替える. """ if getattr(vsp._daemon_request, "_policy_locked", False): return orig = vsp._daemon_request lock = threading.RLock() def _locked(*a, **k): with lock: return orig(*a, **k) _locked._policy_locked = True _locked._orig = orig vsp._daemon_request = _locked class PolicyRecorder: """カメラを読むスレッドと関節角を読むスレッドを持つ. 本体は grab() で最新のコマを受け取る.""" def __init__(self, run_dir, cap, read_angles_fn, to_model_fn): self.dir = Path(run_dir) / "recording" self.dir.mkdir(parents=True, exist_ok=True) self.cap = cap self.read_angles_fn = read_angles_fn self.to_model_fn = to_model_fn self.mode = "model" self.stop_flag = False self.stopped_cleanly = None # stop() のあと: スレッドが止まって書き切れたか self.csv_lock = threading.Lock() self.frame_cv = threading.Condition() self.latest = None # (frame, capture_ts) self.writer = None self.size = None self.n_frames = 0 self.n_actual = 0 self.n_actual_fail = 0 self.write_errors = 0 # mp4 を開けない / 書けなかった回数 (codex R2 A5) self.started = time.time() self.last_flush = self.started self.states_f = (self.dir / "states.csv").open("w", newline="") self.states_w = csv.writer(self.states_f) self.states_w.writerow(STATES_COLS) self.flog_f = (self.dir / "top_frame_log.csv").open("w", newline="") self.flog_w = csv.writer(self.flog_f) self.flog_w.writerow(FRAME_LOG_COLS) self._state_row("stage", time.time()) self.t_cam = threading.Thread(target=self._cam_loop, daemon=True, name="record_camera") self.t_ang = threading.Thread(target=self._angle_loop, daemon=True, name="record_angles") def start(self): self.t_cam.start() self.t_ang.start() # ---- 本体から呼ぶ ------------------------------------------------------- def set_mode(self, mode): if mode == self.mode: return self.mode = mode self._state_row("stage", time.time()) def gripper(self, state): """グリッパーに指令した瞬間に呼ぶ (build_demo_episodes は指令の時刻で 0/1 を切り替える).""" self._state_row("gripper_cmd", time.time(), gripper_state=int(state)) def grab(self): """呼んだあとに撮れたコマを返す (古いコマを渡さない). 戻り値は grab_rgb と同じ (rgb, bgr_small).""" t_req = time.time() deadline = t_req + FRAME_WAIT_SEC with self.frame_cv: while not self.stop_flag: if self.latest is not None and self.latest[1] >= t_req: return self.to_model_fn(self.latest[0]) left = deadline - time.time() if left <= 0: break self.frame_cv.wait(left) return None, None def stop(self): """スレッドを止めて書き切る. 何度呼んでもよい.""" if self.stop_flag: return self.stop_flag = True with self.frame_cv: self.frame_cv.notify_all() for t in (self.t_cam, self.t_ang): if t.is_alive(): t.join(timeout=STOP_JOIN_SEC) ended = time.time() alive = [t.name for t in (self.t_cam, self.t_ang) if t.is_alive()] # codex R1-5: スレッドが止まっていないのに mp4 を閉じると, あとから write() が走って競合する. # 止まらなかったときは mp4 を閉じず (カメラも本体で閉じない: alive() を見る), # 記録を「異常」として残す (build_correction_episodes はこの run を使わない). self.stopped_cleanly = not alive and not self.write_errors with self.csv_lock: for f in (self.states_f, self.flog_f): try: f.flush(); f.close() except Exception: pass if self.writer is not None and not alive: try: self.writer.release() except Exception: pass el = ended - self.started meta = {"frames": self.n_frames, "elapsed_sec": round(el, 3), "effective_fps": round(self.n_frames / el, 3) if el > 0 else 0.0, "width": self.size[0] if self.size else None, "height": self.size[1] if self.size else None, "actual_samples": self.n_actual, "actual_read_failures": self.n_actual_fail, "actual_hz": round(self.n_actual / el, 2) if el > 0 else 0.0, "target_fps": RECORD_FPS, "status": ("thread_not_stopped" if alive else "write_error" if self.write_errors else "ok"), "write_errors": self.write_errors, "threads_alive": alive} (self.dir / "top_meta.json").write_text(json.dumps(meta, indent=2, ensure_ascii=False)) if alive: print(f"[RECORD] [WARN] 記録スレッドが止まりません ({', '.join(alive)}). " "mp4 を閉じずに終えます (この記録は学習データに使いません)") print(f"[RECORD] 記録終了 コマ {self.n_frames} ({meta['effective_fps']} fps) " f"実測角 {self.n_actual} 回 ({meta['actual_hz']} Hz) → {self.dir}") # ---- 中身 --------------------------------------------------------------- def alive(self): """記録スレッドがまだ動いているか (stop() のあとに見る. 動いていればカメラを閉じない).""" return self.t_cam.is_alive() or self.t_ang.is_alive() def _state_row(self, ev, ts, angles=None, gripper_state=None): row = [f"{ts:.6f}", ev, self.mode] for k in range(6): row.append("" if angles is None else f"{float(angles[k]):.3f}") row += ["", "" if gripper_state is None else str(gripper_state), "", "", "", "", ""] with self.csv_lock: if self.states_f.closed: return self.states_w.writerow(row) self._maybe_flush(ts) def _maybe_flush(self, now): if now - self.last_flush >= FLUSH_INTERVAL: try: self.states_f.flush(); self.flog_f.flush() except Exception: pass self.last_flush = now def _cam_loop(self): last_written = 0.0 while not self.stop_flag: ok, frame = self.cap.read() ts = time.time() if self.stop_flag: break if not ok or frame is None: time.sleep(0.02) continue with self.frame_cv: self.latest = (frame, ts) self.frame_cv.notify_all() if ts - last_written < FRAME_INTERVAL - 1e-3: continue last_written = ts try: if self.writer is None: h, w = frame.shape[:2] self.size = (w, h) self.writer = cv2.VideoWriter(str(self.dir / "top.mp4"), cv2.VideoWriter_fourcc(*FOURCC), RECORD_FPS, (w, h)) if not self.writer.isOpened(): raise RuntimeError(f"VideoWriter を開けない ({FOURCC})") if not self.writer.isOpened(): raise RuntimeError("VideoWriter が閉じている") self.writer.write(frame) with self.csv_lock: if not self.flog_f.closed: self.flog_w.writerow([self.n_frames, f"{ts:.6f}", self.mode]) self._maybe_flush(ts) self.n_frames += 1 except Exception as e: self.write_errors += 1 if self.write_errors <= 3: print(f"[RECORD] [WARN] コマの書き込みに失敗: {e}") def _angle_loop(self): nxt = time.time() while not self.stop_flag: try: a = self.read_angles_fn() except Exception: a = None ts = time.time() if a is None: self.n_actual_fail += 1 else: self._state_row("actual", ts, angles=a) self.n_actual += 1 nxt += ANGLE_INTERVAL wait = nxt - time.time() if wait > 0: time.sleep(wait) else: nxt = time.time() # 遅れた分は取り戻さない (詰めて読まない) ```` ### pi05/rewind_correct.py ````python #!/usr/bin/env python3 """rewind_correct.py — 実機推論の「巻き戻し (b)」と「関節の補正 (c)」の計算部分 (2026-10-29). アームにも daemon にも触らない (計算だけ). run_policy_client.py と build_correction_episodes.py の両方から使い, test_rewind_correct.py で確かめる. - rewind_waypoints: 実行した区間 (推論1回分 / 補正1回分) で送った目標を逆順にたどり, 始めの姿勢で終わる中継点の並びを作る - parse_correction_key: 補正モードのキー 1 文字を操作に読み替える - excluded_windows / step_keep_mask: 学習データから除く時間の窓 (巻き戻しで取り消した区間) """ import math import numpy as np # 補正の刻み (度). [ / ] で切り替える. 既定は 1° CORRECTION_STEPS = (0.5, 1.0, 2.0) CORRECTION_DEFAULT_STEP_INDEX = 1 # 記録の 1 コマごとの状態 (states.csv の stage 列 / top_frame_log.csv の mode 列) MODE_MODEL = "model" # モデルの動き (推論結果の実行・推論待ち・確認待ち) MODE_REWIND = "rewind" # 巻き戻し中 (補正の取り消しで戻る間も含む) MODE_CORRECT = "correct" # 補正中 MODE_HOME = "home" # HOME へ戻る間 MODE_WAIT = "wait" # 推論のあとの確認で入力 (Enter / b / c / y/n) を待っている間. # 人がブロックや箱を手で置き直すと手が写るので学習データに使わない (2026-10-29 依頼 27) TRAIN_MODES = (MODE_MODEL, MODE_CORRECT) # 学習データに使う状態 (rewind / home / wait は使わない) # つかんだ区間 (グリッパーを閉じた推論 1 回分) を巻き戻すときの答え (log.jsonl の rewind 行の grasp_undo) GRASP_PLACED = "placed" # y = 持っている: つかんだ位置まで下ろして開き (置いて), 始めの姿勢へ戻る GRASP_DROPPED = "dropped" # n = 落とした: 今の位置ですぐ開いてから, 始めの姿勢へ戻る CORRECTION_HELP = ( " [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み\n" " [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉\n" " Enter = 補正を終えてモデルの推論に戻る / b = 補正を取り消して始めの姿勢へ戻る\n" " ? = この一覧" ) def split_leg(a, b, max_move): """a から b へ, どの関節も 1 回 max_move 以内になるよう等分した点を返す (a は含めず b は含む).""" a = np.asarray(a, dtype=float) b = np.asarray(b, dtype=float) span = float(np.max(np.abs(b - a))) if len(a) else 0.0 n = max(1, int(math.ceil(span / max_move - 1e-9))) if max_move and max_move > 0 else 1 return [a + (b - a) * (k / n) for k in range(1, n + 1)] def rewind_waypoints(sent_targets, start_pose, current=None, max_move=None): """実行した区間を逆にたどる中継点の並び. sent_targets: その区間で送った目標 (送った順). start_pose: 区間を始めたときの姿勢 (推論なら measured_state の 6 関節, 補正なら補正を始めたときの実測). 戻り値は「最後に送った目標の 1 つ手前 → … → 最初に送った目標 → start_pose」. (最後に送った目標は今いる場所なので送らない.) max_move を渡すと, current (既定は最後に送った目標) から順に, 1 回の移動が max_move を超える区間を等分する (apply_limits の移動量制限で途中止まりにしないため). """ start = np.asarray(start_pose, dtype=float)[:6] sent = [np.asarray(x, dtype=float)[:6] for x in sent_targets] pts = sent[:-1][::-1] + [start] if not max_move: return pts prev = (np.asarray(current, dtype=float)[:6] if current is not None else (sent[-1] if sent else start)) out = [] for p in pts: out.extend(split_leg(prev, p, max_move)) prev = p return out def parse_correction_key(ch): """補正モードの 1 文字を操作に読み替える. 戻り値は (操作, 値) か None (関係ないキー). ("select", j) / ("move", +1|-1) / ("step", +1|-1) / ("grip", None) / ("accept", None) / ("cancel", None) / ("help", None) """ if ch in ("1", "2", "3", "4", "5", "6"): return ("select", int(ch) - 1) if ch in ("+", "=", "."): return ("move", +1) if ch in ("-", "_", ","): return ("move", -1) if ch == "]": return ("step", +1) if ch == "[": return ("step", -1) if ch in ("g", "G"): return ("grip", None) if ch in ("\n", "\r", ""): # "" = 入力の終わり (EOF). 姿勢はそのまま補正を終える return ("accept", None) if ch in ("b", "B"): return ("cancel", None) if ch == "?": return ("help", None) return None def next_step_index(i, d): return min(max(i + d, 0), len(CORRECTION_STEPS) - 1) def excluded_windows(records): """log.jsonl の記録から, 学習データに入れない時間の窓 [(t0, t1), ...] を作る. - 巻き戻した区間: 区間を始めた時刻 (segment_t[0]) から 巻き戻しが終わった時刻 (t_end) まで. 区間の実行・その後の推論待ち (間違った姿勢で止まっている間)・巻き戻しの動きをまとめて除く - 取り消した補正: 補正を始めた時刻から 始めの姿勢へ戻り終えた時刻まで """ out = [] for r in records: if r.get("type") == "rewind" and r.get("rewound"): seg = r.get("segment_t") or [None, None] t0 = seg[0] if seg[0] is not None else r.get("t_start") t1 = r.get("t_end") if t0 is not None and t1 is not None: out.append((float(t0), float(t1))) elif r.get("type") == "correction" and r.get("action") == "cancel": t0, t1 = r.get("t_start"), r.get("t_end") if t0 is not None and t1 is not None: out.append((float(t0), float(t1))) return out def mode_at(t, mode_ts, modes, default=MODE_MODEL): """各時刻の状態 (その時刻までで最後に切り替わった状態).""" t = np.asarray(t, dtype=float) if len(mode_ts) == 0: return np.array([default] * len(t), dtype=object) order = np.argsort(mode_ts) mts = np.asarray(mode_ts, dtype=float)[order] ms = np.asarray(modes, dtype=object)[order] k = np.searchsorted(mts, t, side="right") - 1 return np.where(k >= 0, ms[np.clip(k, 0, None)], default) def step_keep_mask(t, mode_ts, modes, windows): """学習に使うステップ: 状態が model / correct で, 除く窓のどれにも入らないもの.""" m = mode_at(t, mode_ts, modes) keep = np.isin(m, TRAIN_MODES) t = np.asarray(t, dtype=float) for t0, t1 in windows: keep &= ~((t >= t0) & (t <= t1)) return keep, m ```` ### pi05/build_correction_episodes.py ````python #!/usr/bin/env python3 """build_correction_episodes.py — 実機推論 (巻き戻し・補正つき) の記録から π0.5 学習用エピソードを作る (2026-10-29). 入力 (1 試行 = 1 run_stem. run_policy_client.py --execute が残したもの): runs/pi05_policy/{run_stem}/{run_stem}.json 結果 (outcome / judgments) runs/pi05_policy/{run_stem}/log.jsonl 推論・巻き戻し・補正の記録 runs/pi05_policy/{run_stem}/recording/states.csv actual (実測6軸) / gripper_cmd / stage (= 状態) runs/pi05_policy/{run_stem}/recording/top.mp4 + top_frame_log.csv 出力 (お手本 mecharm_redblock_box_teach_v1_trim2 と同じ形): datasets/{DATASET}/raw_episodes/{run_stem}/frames/000000.jpg ... / episode.npz / meta.json datasets/{DATASET}/index.json / README.md 作り方: - 使うのは 結果が「成功」(y) の run だけ - 時間軸は 10Hz. state は実測角の線形補間 + グリッパーは指令の時刻で 0/1 (build_demo_episodes と同じ) - 学習に使うのは状態が model / correct のステップだけ. rewind / home / wait (確認の入力待ち. 人がブロックを置き直すと手が写る) のステップと, 巻き戻しで取り消した区間 (区間を始めた時刻 〜 巻き戻しが終わった時刻) と, 取り消した補正 (補正を始めた時刻 〜 戻り終えた時刻) は除く. 除いたあとは「その手前」と「巻き戻したあとの続き」をつなぐ (戻った先は区間の始めの姿勢なので続く) - actions は「0.1 秒後の state」. 除いたあとに残ったステップで作り直す (trim2 と同じ作り方) - 止まっている区間は build_demo_episodes と同じ決まりで詰める (IDLE_* と --trim-mid-idle の mid_idle_keep_mask. 既定で詰める. --no-trim-mid-idle で前者だけ) - 元の記録は読むだけ (上書き・移動・削除しない) Usage: python3 pi05/build_correction_episodes.py # runs/pi05_policy の全 run python3 pi05/build_correction_episodes.py --run pi05_20261030_101500 --force python3 pi05/build_correction_episodes.py --check-with \\ /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes """ import argparse import csv import json import shutil import sys from pathlib import Path import cv2 import numpy as np _HERE = Path(__file__).resolve().parent sys.path.insert(0, str(_HERE)) import build_demo_episodes as bde # noqa: E402 from rewind_correct import MODE_CORRECT, MODE_MODEL, excluded_windows, step_keep_mask # noqa: E402 DATA_ROOT = bde.DATA_ROOT RUNS_ROOT = DATA_ROOT / "runs" / "pi05_policy" DEFAULT_DATASET = "mecharm_redblock_box_corr_v1" FPS = bde.FPS def load_stage_times(path): """states.csv の stage 行 (状態の切り替わり) と, 各行の stage 列.""" ts, modes = [], [] with path.open(newline="") as f: for row in csv.DictReader(f): if row.get("event_type") != "stage": continue try: ts.append(float(row["ts"])); modes.append(row.get("stage") or MODE_MODEL) except Exception: continue return np.array(ts), modes def load_log(path): out = [] if not path.exists(): return out for line in path.read_text().splitlines(): line = line.strip() if line: try: out.append(json.loads(line)) except Exception: continue return out def idle_keep_mask(state, actions): """build_demo_episodes.build_one の「停止区間を詰める」(IDLE_*) と同じ決まり. 戻り値: (keep, 詰めた数).""" n = len(state) motion = np.max(np.abs(actions - state), axis=1) keep = np.ones(n, dtype=bool) i, trimmed = 0, 0 while i < n: if motion[i] < bde.IDLE_EPS_DEG: j = i while j < n and motion[j] < bde.IDLE_EPS_DEG: j += 1 if (j - i) / FPS > bde.IDLE_MAX_SEC: cut_from = i + int(bde.IDLE_KEEP_SEC * FPS) keep[cut_from:j] = False trimmed += j - cut_from i = j else: i += 1 guard = bde.gripper_guard_mask(state) trimmed -= int((~keep & guard).sum()) keep |= guard return keep, trimmed def next_state_actions(state): """actions = 次のステップの state (最後は自分自身). 7 列とも.""" return np.concatenate([state[1:], state[-1:]], axis=0) def select_steps(t, q, g, mode_ts, modes, windows, trim_mid_idle=True): """学習に使うステップを選び, state / actions を作る (ファイルを読まない部分. テストで使う). 戻り値: dict(idx=元の t の添字, state, actions, modes, counts) """ state_all = np.concatenate([q, g[:, None]], axis=1).astype(np.float32) keep, m_all = step_keep_mask(t, mode_ts, modes, windows) idx = np.flatnonzero(keep) counts = {"grid_steps": int(len(t)), "excluded_mode": int((~np.isin(m_all, (MODE_MODEL, MODE_CORRECT))).sum()), "excluded_window": int((np.isin(m_all, (MODE_MODEL, MODE_CORRECT)) & ~keep).sum())} if len(idx) == 0: return {"idx": idx, "state": state_all[:0], "actions": state_all[:0], "modes": [], "counts": counts} state = state_all[idx] # 除いたあとに残ったステップでつなぐ. actions は「つないだ並びでの次の state」 actions = next_state_actions(state) k1, idle_trimmed = idle_keep_mask(state, actions) idx, state, actions = idx[k1], state[k1], actions[k1] counts["idle_trimmed"] = int(idle_trimmed) counts["steps_before_mid_trim"] = int(len(idx)) mid = 0 if trim_mid_idle and len(idx): mk = bde.mid_idle_keep_mask(state, actions) mid = int((~mk).sum()) idx, state, actions = idx[mk], state[mk], actions[mk] # trim2 と同じ: 関節 6 列は残ったステップで作り直す. グリッパー列は元の (次の) 指令のまま actions = actions.copy() actions[:-1, :6] = state[1:, :6] actions[-1, :6] = state[-1, :6] counts["mid_idle_trimmed"] = mid sel_modes = [str(m_all[k]) for k in idx] counts["model_steps"] = sel_modes.count(MODE_MODEL) counts["correct_steps"] = sel_modes.count(MODE_CORRECT) return {"idx": idx, "state": state, "actions": actions, "modes": sel_modes, "counts": counts} def build_one(run_dir, out_dir, args): stem = run_dir.name trial_p = run_dir / f"{stem}.json" rec = run_dir / "recording" if not trial_p.exists(): return {"run": stem, "used": False, "reason": f"missing:{trial_p.name}"} trial = json.loads(trial_p.read_text()) if trial.get("recording_enabled") is not True: return {"run": stem, "used": False, "reason": "no_recording"} if trial.get("outcome") != "success": return {"run": stem, "used": False, "reason": f"outcome={trial.get('outcome')}"} need = [rec / "states.csv", rec / "top.mp4", rec / "top_frame_log.csv"] missing = [p.name for p in need if not p.exists()] if missing: return {"run": stem, "used": False, "reason": "missing:" + ",".join(missing)} log = load_log(run_dir / "log.jsonl") unfinished = [r for r in log if r.get("type") == "rewind" and not r.get("finished", True)] unfinished += [r for r in log if r.get("type") == "correction" and r.get("action") == "cancel" and not r.get("finished", True)] if unfinished: # 戻りきれなかった巻き戻しがあると, つないだ前後で姿勢が続かない return {"run": stem, "used": False, "reason": f"rewind_unfinished x{len(unfinished)}"} at, aq, gt, gs = bde.load_states(rec / "states.csv") if len(at) < bde.MIN_ACTUAL_SAMPLES: return {"run": stem, "used": False, "reason": f"actual 実測が少ない ({len(at)})"} order = np.argsort(at); at, aq = at[order], aq[order] at, aq = bde.drop_spikes(at, aq) try: tm = json.loads((rec / "top_meta.json").read_text()) except Exception: tm = {} if tm.get("status") != "ok": return {"run": stem, "used": False, "reason": f"recording_status={tm.get('status')}"} fidx, fts = bde.load_frame_log(rec / "top_frame_log.csv") if len(fts) < FPS: return {"run": stem, "used": False, "reason": f"top フレームが少ない ({len(fts)})"} t0, t1 = max(fts[0], at[0]), min(fts[-1], at[-1]) if t1 - t0 < 3.0: return {"run": stem, "used": False, "reason": "記録の重なりが短い"} t = np.arange(t0, t1, 1.0 / FPS) q = np.stack([np.interp(t, at, aq[:, k]) for k in range(6)], axis=1) g = np.zeros(len(t)) if len(gt): go = np.argsort(gt); gt, gs = gt[go], gs[go] for i, ti in enumerate(t): k = np.searchsorted(gt, ti, side="right") - 1 g[i] = gs[k] if k >= 0 else 0.0 mode_ts, modes = load_stage_times(rec / "states.csv") windows = excluded_windows(log) sel = select_steps(t, q, g, mode_ts, modes, windows, trim_mid_idle=not args.no_trim_mid_idle) if len(sel["idx"]) < FPS: return {"run": stem, "used": False, "reason": f"使えるステップが少ない ({len(sel['idx'])})"} tt = t[sel["idx"]] # codex R1-4: 画像の候補も同じ決まりで絞る (除いた区間・rewind / home の画像を, 境界の # 最近傍として拾わない). 候補から選ぶので, 除いた時刻の画像は入らない fkeep, _ = step_keep_mask(fts, mode_ts, modes, windows) fidx, fts = fidx[fkeep], fts[fkeep] if len(fts) < 2: return {"run": stem, "used": False, "reason": "使える画像が少ない"} nearest = np.clip(np.searchsorted(fts, tt), 1, len(fts) - 1) left = fts[nearest - 1]; right = fts[nearest] nearest = np.where(np.abs(tt - left) <= np.abs(right - tt), nearest - 1, nearest) want = {} for step, n in enumerate(nearest): want.setdefault(int(fidx[n]), []).append(step) if out_dir.exists(): if not args.force: return {"run": stem, "used": True, "reason": "既存 (スキップ・--force で再生成)", "steps": int(len(tt))} shutil.rmtree(out_dir) (out_dir / "frames").mkdir(parents=True) cap = cv2.VideoCapture(str(rec / "top.mp4")) fi, written, max_need, size = 0, 0, max(want), None while fi <= max_need: ok, frame = cap.read() if not ok: break if fi in want: h, w = frame.shape[:2] s = bde.IMG_LONG / max(h, w) small = cv2.resize(frame, (round(w * s), round(h * s)), interpolation=cv2.INTER_AREA) size = small.shape[1], small.shape[0] for step in want[fi]: # codex R3 P2: 保存に失敗したコマは数えない (欠けたエピソードを採用しない) if cv2.imwrite(str(out_dir / "frames" / f"{step:06d}.jpg"), small, [cv2.IMWRITE_JPEG_QUALITY, bde.JPEG_QUALITY]): written += 1 fi += 1 cap.release() if written != len(tt): shutil.rmtree(out_dir) return {"run": stem, "used": False, "reason": f"フレーム抽出不足 written={written} steps={len(tt)} (mp4 と frame_log の不一致)"} np.savez_compressed(out_dir / "episode.npz", t=(np.arange(len(tt)) / FPS).astype(np.float64), t_orig=tt - tt[0], state=sel["state"], actions=sel["actions"]) c = sel["counts"] meta = { "run_stem": stem, "task": bde.TASK, "fps": FPS, "steps": int(len(tt)), "duration_sec": round(len(tt) / FPS, 1), "idle_trimmed_sec": round(c.get("idle_trimmed", 0) / FPS, 1), "trim_mid_idle": not args.no_trim_mid_idle, "steps_before_mid_trim": c.get("steps_before_mid_trim"), "mid_idle_trimmed_steps": c.get("mid_idle_trimmed", 0), "image_size": size, "state_dim": 7, "state_names": ["j0", "j1", "j2", "j3", "j4", "j5", "gripper"], "action": "absolute target = state 0.1s later", "actual_samples": int(len(at)), "gripper_cmds": int(len(gt)), "human_verdict": {"pick": "yes", "place": "yes"}, "source": "pi05_policy (model + correct)", "model_steps": c.get("model_steps", 0), "correct_steps": c.get("correct_steps", 0), "excluded_mode_steps": c["excluded_mode"], "excluded_window_steps": c["excluded_window"], "excluded_windows": [[round(a, 3), round(b, 3)] for a, b in windows], "rewinds": sum(1 for r in log if r.get("type") == "rewind"), "corrections_accepted": sum(1 for r in log if r.get("type") == "correction" and r.get("action") == "accept"), "step_modes": sel["modes"], } (out_dir / "meta.json").write_text(json.dumps(meta, indent=2, ensure_ascii=False)) return {"run": stem, "used": True, "reason": "ok", "steps": int(len(tt)), "model_steps": meta["model_steps"], "correct_steps": meta["correct_steps"], "rewinds": meta["rewinds"], "corrections_accepted": meta["corrections_accepted"]} def check_compat(ours, ref): """補正エピソードがお手本と一緒に学習に使えるか (形・キー・FPS・画像の大きさ). 問題の一覧を返す.""" probs = [] def _eps(d): return sorted(p.parent for p in Path(d).glob("*/episode.npz")) a, b = _eps(ours), _eps(ref) if not a: return ["補正エピソードがない"] if not b: return ["お手本エピソードがない"] rm = json.loads((b[0] / "meta.json").read_text()) rz = np.load(b[0] / "episode.npz") for ep in a: m = json.loads((ep / "meta.json").read_text()) z = np.load(ep / "episode.npz") if sorted(z.files) != sorted(rz.files): probs.append(f"{ep.name}: npz のキー {sorted(z.files)} != お手本 {sorted(rz.files)}") for k in ("state", "actions"): if z[k].dtype != rz[k].dtype or z[k].shape[1:] != rz[k].shape[1:]: probs.append(f"{ep.name}: {k} {z[k].dtype}{z[k].shape} != お手本 {rz[k].dtype}{rz[k].shape}") for k in ("fps", "image_size", "state_dim", "state_names", "task"): if m.get(k) != rm.get(k): probs.append(f"{ep.name}: meta {k}={m.get(k)} != お手本 {rm.get(k)}") n = len(z["state"]) nf = len(list((ep / "frames").glob("*.jpg"))) if not (n == len(z["actions"]) == nf == len(z["t"])): probs.append(f"{ep.name}: 長さが合わない state={n} actions={len(z['actions'])} frames={nf}") img = cv2.imread(str(ep / "frames" / "000000.jpg")) if img is None or (img.shape[1], img.shape[0]) != tuple(rm["image_size"]): probs.append(f"{ep.name}: 画像の大きさ {None if img is None else img.shape[:2][::-1]} " f"!= {rm['image_size']}") return probs def write_readme(ds, results, args): used = [r for r in results.values() if r.get("used")] lines = [ f"# {ds.name}", "", "実機推論 (`pi05/run_policy_client.py --execute`) で, 失敗しかけたら巻き戻し (b) /", "キーボードで関節を直し (c) ながら成功させた run から作ったエピソード。", "お手本 `mecharm_redblock_box_teach_v1_trim2` と同じ形 (frames/ + episode.npz + meta.json, 10Hz)。", "", "- 作成: `python3 pi05/build_correction_episodes.py`" + ("" if not args.no_trim_mid_idle else " --no-trim-mid-idle"), f"- 元データ: `runs/pi05_policy//` (読むだけ。上書き・移動・削除しない)", "", "## 作り方", "", "- 結果が「成功」(y) の run だけ使う (`{run_stem}.json` の outcome)", "- 状態が model / correct のステップだけ使う。rewind / home / wait (確認の入力待ち。手が写り得る) は除く", "- 巻き戻しで取り消した区間 (区間を始めた時刻 〜 巻き戻しが終わった時刻) と, 取り消した補正は除き,", " その手前と巻き戻したあとの続きをつなぐ", "- actions は「0.1 秒後の state」(つないだ並びで作り直す。グリッパー列は次の指令)", "- 止まっている区間は build_demo_episodes と同じ決まり (IDLE_* と mid_idle_keep_mask) で詰める", "- 戻りきれなかった巻き戻しがある run は使わない (前後の姿勢が続かない)", "", "## 数", "", f"採用 {len(used)} 本 / 総ステップ {sum(r.get('steps', 0) for r in used)} " f"(model {sum(r.get('model_steps', 0) for r in used)} / correct {sum(r.get('correct_steps', 0) for r in used)})", "", "| run_stem | 採用 | steps | model | correct | 巻き戻し | 補正 | 理由 |", "| --- | --- | --- | --- | --- | --- | --- | --- |", ] for k in sorted(results): r = results[k] lines.append(f"| {k} | {'○' if r.get('used') else '-'} | {r.get('steps', '')} | " f"{r.get('model_steps', '')} | {r.get('correct_steps', '')} | {r.get('rewinds', '')} | " f"{r.get('corrections_accepted', '')} | {r.get('reason', '')} |") (ds / "README.md").write_text("\n".join(lines) + "\n") def main(): ap = argparse.ArgumentParser() ap.add_argument("--dataset", default=DEFAULT_DATASET) ap.add_argument("--runs-root", default=str(RUNS_ROOT), help="run_policy_client の記録の置き場所") ap.add_argument("--run", action="append", help="特定の run_stem だけ (複数可)") ap.add_argument("--force", action="store_true", help="既存エピソードを作り直す") ap.add_argument("--no-trim-mid-idle", action="store_true", help="途中の停止を mid_idle_keep_mask で詰めない (既定は詰める = trim2 と同じ)") ap.add_argument("--check-with", help="お手本の raw_episodes. 一緒に学習に使えるかを確かめる") ap.add_argument("--data-root", help=argparse.SUPPRESS) # テスト用 (KIOXIA の代わり) args = ap.parse_args() if args.data_root: root = Path(args.data_root) else: bde.ensure_ssd() root = DATA_ROOT runs_root = Path(args.runs_root) ds = root / "datasets" / args.dataset out_root = ds / "raw_episodes" # 既存のお手本データセット (pi05_policy 以外から作ったエピソード) には書かない (codex R1 B3) others = [p.parent.name for p in out_root.glob("*/meta.json") if not str(json.loads(p.read_text()).get("source", "")).startswith("pi05_policy")] if others: sys.exit(f"[ABORT] 出力先に補正エピソード以外が入っています: {ds} (例: {', '.join(others[:3])})\n" " 元データを上書きしないよう, --dataset に別の名前を指定してください") runs = sorted(p for p in runs_root.glob("pi05_*") if p.is_dir() and (p / "recording").is_dir()) if args.run: runs = [p for p in runs if p.name in set(args.run)] if not runs: print(f"[INFO] 記録つきの run がない: {runs_root}") out_root.mkdir(parents=True, exist_ok=True) results = {} for r in runs: try: res = build_one(r, out_root / r.name, args) except Exception as e: res = {"run": r.name, "used": False, "reason": f"exception:{type(e).__name__}:{e}"} results[r.name] = res print(f"[{'OK ' if res['used'] else '-- '}] {res['run']} {res.get('steps', '')} {res['reason']}") index_path = ds / "index.json" old = json.loads(index_path.read_text()) if index_path.exists() else {} old.update(results) index_path.write_text(json.dumps(old, indent=2, ensure_ascii=False)) write_readme(ds, old, args) used = [v for v in old.values() if v.get("used")] print(f"\n採用エピソード合計: {len(used)} 総ステップ: {sum(v.get('steps', 0) for v in used)} → {out_root}") if args.check_with: probs = check_compat(out_root, args.check_with) print("[CHECK] お手本と一緒に使えるか: " + ("OK" if not probs else f"NG {len(probs)} 件")) for p in probs: print(" " + p) return 1 if probs else 0 return 0 if __name__ == "__main__": sys.exit(main()) ```` ### pi05/test_rewind_correct.py ````python #!/usr/bin/env python3 """test_rewind_correct.py — 巻き戻し・補正・補正エピソード作りの確かめ (アームなし. 2026-10-29). python3 pi05/test_rewind_correct.py # 全部 python3 pi05/test_rewind_correct.py -v 中身: - 巻き戻しの中継点の並び (逆順・始めの姿勢で終わる・移動量の上限で等分) - 補正の刻みと範囲チェック (apply_limits / check_send_safe を通る) - 補正エピソードの区間の除き方 (rewind と取り消した区間が入らない) - 合成の記録から build_correction_episodes がお手本と同じ形を作り, build_demo_episodes もそのまま読める - run_policy_client を偽の daemon / カメラ / モデルで通す: b / c の流れ (--dry-keys と --execute), b / c を押さないときは 変更前 (git HEAD) と同じログになる (項目が増えるだけ) KIOXIA にもアームにも触らない (一時フォルダだけ). """ import argparse import builtins import importlib.util import io import json import subprocess import sys import tempfile import time import types 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 run_policy_client as rpc # noqa: E402 import build_correction_episodes as bce # noqa: E402 import build_demo_episodes as bde # noqa: E402 from policy_recorder import STATES_COLS # noqa: E402 from rewind_correct import ( # noqa: E402 CORRECTION_STEPS, excluded_windows, next_step_index, parse_correction_key, rewind_waypoints, split_leg, step_keep_mask, ) LO = np.array([-90.0, -10.0, -10.0, -30.0, -90.0, -60.0]) HI = np.array([90.0, 88.0, 60.0, 30.0, 60.0, 0.0]) class TestRewindWaypoints(unittest.TestCase): def test_reverse_and_end_at_start(self): start = [0, 0, 0, 0, 0, -30] sent = [[1, 0, 0, 0, 0, -30], [2, 1, 0, 0, 0, -30], [3, 2, 1, 0, 0, -30]] w = rewind_waypoints(sent, start) self.assertEqual(len(w), 3) np.testing.assert_allclose(w[0], sent[1]) # 最後に送った目標 (今いる所) の 1 つ手前から np.testing.assert_allclose(w[1], sent[0]) np.testing.assert_allclose(w[-1], start) # 始めの姿勢で終わる def test_single_target(self): w = rewind_waypoints([[5, 5, 5, 5, 5, -25]], [0, 0, 0, 0, 0, -30]) self.assertEqual(len(w), 1) np.testing.assert_allclose(w[0], [0, 0, 0, 0, 0, -30]) def test_split_by_max_move(self): start = np.zeros(6) sent = [np.array([30.0, 0, 0, 0, 0, 0]), np.array([45.0, 0, 0, 0, 0, 0])] w = rewind_waypoints(sent, start, current=sent[-1], max_move=20.0) # 45 → 30 (15°: 1 回) → 0 (30°: 2 回に等分) np.testing.assert_allclose([p[0] for p in w], [30.0, 15.0, 0.0]) prev = sent[-1] for p in w: self.assertLessEqual(np.max(np.abs(p - prev)), 20.0 + 1e-9) prev = p np.testing.assert_allclose(w[-1], start) def test_split_leg_counts(self): self.assertEqual(len(split_leg(np.zeros(6), np.full(6, 20.0), 20.0)), 1) self.assertEqual(len(split_leg(np.zeros(6), np.full(6, 20.1), 20.0)), 2) class TestCorrection(unittest.TestCase): def test_keys(self): self.assertEqual(parse_correction_key("1"), ("select", 0)) self.assertEqual(parse_correction_key("6"), ("select", 5)) self.assertEqual(parse_correction_key("+"), ("move", 1)) self.assertEqual(parse_correction_key("."), ("move", 1)) self.assertEqual(parse_correction_key("-"), ("move", -1)) self.assertEqual(parse_correction_key(","), ("move", -1)) self.assertEqual(parse_correction_key("\n"), ("accept", None)) self.assertEqual(parse_correction_key("b"), ("cancel", None)) self.assertEqual(parse_correction_key("g"), ("grip", None)) self.assertIsNone(parse_correction_key("x")) self.assertEqual(CORRECTION_STEPS, (0.5, 1.0, 2.0)) self.assertEqual(next_step_index(1, +1), 2) self.assertEqual(next_step_index(2, +1), 2) self.assertEqual(next_step_index(0, -1), 0) def test_step_size(self): cur = np.array([10.0, 20, 30, 0, -40, -30]) for step in CORRECTION_STEPS: t, notes, unsafe = rpc.plan_correction_step(cur, 0, +1, step, cur, LO, HI, 20.0) self.assertEqual(unsafe, []) self.assertEqual(notes, []) np.testing.assert_allclose(t - cur, [step, 0, 0, 0, 0, 0]) t, _, _ = rpc.plan_correction_step(cur, 3, -1, step, cur, LO, HI, 20.0) np.testing.assert_allclose(t - cur, [0, 0, 0, -step, 0, 0]) def test_envelope_clamp(self): cur = np.array([89.5, 20, 30, 0, -40, -30]) t, notes, unsafe = rpc.plan_correction_step(cur, 0, +1, 2.0, cur, LO, HI, 20.0) self.assertAlmostEqual(t[0], 90.0) # 安全範囲で止まる (apply_limits) self.assertTrue(any("クランプ" in n for n in notes)) self.assertEqual(unsafe, []) def test_unsafe_measured_outside(self): # 実測が範囲の外 (j1=110) だと, 移動量の制限で範囲の外へ戻る → check_send_safe が止める cur = np.array([0.0, 110, 30, 0, -40, -30]) t, notes, unsafe = rpc.plan_correction_step(cur, 1, -1, 1.0, cur, LO, HI, 20.0) self.assertTrue(unsafe) def test_correction_session_dry(self): """補正モードを仮の腕で通す: j1 を +2 刻み, 刻み 2°, j0 -1, Enter.""" arm = rpc.ArmIO(False, None, sim_q=[0, 0, 0, 0, 0, -30]) log = io.StringIO() class Keys: def __init__(self, s): self.s = list(s) def get(self): return self.s.pop(0) if self.s else "" def flush(self): pass res, grip = rpc.run_correction(arm, Keys("2++]1-\n"), LO, HI, 20.0, 0, 1, 0, log, None) self.assertEqual(res["action"], "accept") np.testing.assert_allclose(res["final_target"], [-2.0, 2.0, 0, 0, 0, -30]) lines = [json.loads(x) for x in log.getvalue().splitlines()] moves = [x for x in lines if x["action"] == "move"] self.assertEqual([m["joint"] for m in moves], [1, 1, 0]) self.assertEqual([m["step_deg"] for m in moves], [1.0, 1.0, 2.0]) self.assertTrue(all(m["type"] == "correction" for m in moves)) self.assertEqual(moves[0]["before"], [0, 0, 0, 0, 0, -30]) self.assertEqual(moves[0]["after"], [0, 1.0, 0, 0, 0, -30]) def test_correction_cancel_returns(self): arm = rpc.ArmIO(False, None, sim_q=[0, 0, 0, 0, 0, -30]) class Keys: def __init__(self, s): self.s = list(s) def get(self): return self.s.pop(0) if self.s else "" def flush(self): pass res, grip = rpc.run_correction(arm, Keys("1+++gb"), LO, HI, 20.0, 0, 1, 0, io.StringIO(), None) self.assertEqual(res["action"], "cancel") self.assertTrue(res["finished"]) np.testing.assert_allclose(arm.read(), [0, 0, 0, 0, 0, -30]) self.assertEqual(grip, 0) # g で閉じたグリッパーも始めに戻す class LaggyArm(rpc.ArmIO): """送っても半分しか動かない仮の腕 (未到達).""" def send(self, target, tag): t = np.asarray(target, dtype=float) self.sim_q = self.sim_q + (t - self.sim_q) * 0.5 return False class Keys: def __init__(self, s): self.s = list(s) def get(self): return self.s.pop(0) if self.s else "" def flush(self): pass class TestAbnormal(unittest.TestCase): """codex R1 の指摘 1〜5 と A3 / C3 の異常系.""" def test_rewind_not_reached_is_not_finished(self): arm = LaggyArm(False, None, sim_q=[10, 0, 0, 0, 0, -30]) entry = {"kind": "chunk", "chunk": 0, "start_pose": np.array([0.0, 0, 0, 0, 0, -30]), "sent_targets": [np.array([5.0, 0, 0, 0, 0, -30]), np.array([10.0, 0, 0, 0, 0, -30])], "grip_changed": False, "t_start": 0.0, "t_end": 1.0} rw = rpc.run_rewind(entry, arm, LO, HI, 20.0, None) self.assertTrue(rw["traverse_completed"]) self.assertFalse(rw["reached_start"]) self.assertFalse(rw["finished"]) # 送り切っても戻れていなければ finished ではない def test_gripper_round_trip_counts(self): arm = rpc.ArmIO(False, None, sim_q=[0, 0, 0, 0, 0, -30]) res, grip = rpc.run_correction(arm, Keys("1+gg\n"), LO, HI, 20.0, 0, 1, 0, io.StringIO(), None) self.assertEqual(res["grip_before"], res["grip_after"]) self.assertTrue(res["grip_used"]) # 往復でも「動かした」 → 巻き戻さない res, grip = rpc.run_correction(arm, Keys("gg\n"), LO, HI, 20.0, 0, 2, 0, io.StringIO(), None) self.assertEqual(res["steps_sent"], 0) self.assertTrue(res["grip_used"]) def test_correction_unreached_counts_from_measured(self): arm = LaggyArm(False, None, sim_q=[0, 0, 0, 0, 0, -30]) res, _ = rpc.run_correction(arm, Keys("1]++\n"), LO, HI, 20.0, 0, 1, 0, io.StringIO(), None) # 1 刻み目 2° → 1° しか動かない → 2 刻み目は実測 1° から 2° = 3° へ (目標だけ 4° へ進まない) np.testing.assert_allclose(res["sent_targets"][1][0], 3.0) def test_cancel_unreached_keeps_gripper(self): class StuckOnReturn(rpc.ArmIO): """補正の刻みは届くが, 取り消しで戻るときは動かない腕.""" def send(self, target, tag): if tag.startswith("pi05_corr_cancel"): return False return super().send(target, tag) arm = StuckOnReturn(False, None, sim_q=[0, 0, 0, 0, 0, -30]) with mock.patch("sys.stdout", new=io.StringIO()): res, grip = rpc.run_correction(arm, Keys("1]+++gb"), LO, HI, 20.0, 0, 1, 0, io.StringIO(), None) self.assertEqual(res["action"], "cancel") self.assertFalse(res["finished"]) self.assertFalse(res["reached_start"]) self.assertEqual(grip, 1) # 戻れていないのでグリッパーは閉じたまま def test_recorder_stop_timeout(self): import policy_recorder as pr class StuckCap: def __init__(self): self.ev = __import__("threading").Event() def read(self): self.ev.wait(); return False, None with tempfile.TemporaryDirectory() as td, mock.patch.object(pr, "STOP_JOIN_SEC", 0.3), \ mock.patch("sys.stdout", new=io.StringIO()): cap = StuckCap() r = pr.PolicyRecorder(Path(td), cap, lambda: [0, 0, 0, 0, 0, -30], rpc.frame_to_model) r.start() time.sleep(0.2) r.stop() self.assertFalse(r.stopped_cleanly) self.assertTrue(r.alive()) meta = json.loads((Path(td) / "recording" / "top_meta.json").read_text()) self.assertEqual(meta["status"], "thread_not_stopped") cap.ev.set() class TestImwriteFailure(unittest.TestCase): def test_imwrite_false_not_used(self): """JPEG の保存が失敗したら, そのエピソードは使わない (codex R3 P2).""" with tempfile.TemporaryDirectory() as td: td = Path(td) run = td / "runs" / "pi05_20990101_000200" write_fake_recording(run, [(0, 6, "model", 0, 30)]) (run / "log.jsonl").write_text("") out = td / "ds" / "raw_episodes" / run.name with mock.patch.object(bce.cv2, "imwrite", lambda *a, **k: False): r = bce.build_one(run, out, argparse.Namespace(force=True, no_trim_mid_idle=False)) self.assertFalse(r["used"]) self.assertFalse(out.exists()) class TestExcludeSegments(unittest.TestCase): def test_rewind_and_cancel_excluded(self): # 0〜10 秒: model / 3〜5: 推論 3 を実行 (あとで巻き戻す) / 6〜7: rewind / 7〜8: model # 8〜9: correct (取り消し) 9〜9.5: rewind / 9.5〜: model t = np.arange(0, 12, 0.1) mode_ts = [0.0, 6.0, 7.0, 8.0, 9.0, 9.5] modes = ["model", "rewind", "model", "correct", "rewind", "model"] log = [{"chunk": 3, "executed": True, "t_exec_start": 3.0, "t_exec_end": 5.0}, {"type": "rewind", "rewound": True, "chunk": 3, "segment_t": [3.0, 5.0], "t_start": 6.0, "t_end": 7.0, "finished": True}, {"type": "correction", "action": "cancel", "t_start": 8.0, "t_end": 9.5}] w = excluded_windows(log) self.assertEqual(w, [(3.0, 7.0), (8.0, 9.5)]) keep, m = step_keep_mask(t, mode_ts, modes, w) kt = t[keep] self.assertFalse(np.any((kt >= 3.0 - 1e-9) & (kt <= 7.0 + 1e-9))) self.assertFalse(np.any((kt >= 8.0 - 1e-9) & (kt <= 9.5 + 1e-9))) self.assertTrue(np.all(np.isin(m[keep], ["model", "correct"]))) self.assertTrue(np.any(kt < 3.0) and np.any(kt > 9.5)) def test_select_steps_joins(self): """巻き戻した区間の動きが入らず, 手前と続きがつながる (actions = つないだ並びの次の state).""" t = np.arange(0, 10, 0.1) q = np.zeros((len(t), 6)) q[:, 5] = -30 # 0〜2: j0 0→10, 2〜4: j0 10→40 (巻き戻す区間), 4〜6: rewind 40→10, 6〜10: j0 10→20 (j1 も) q[:, 0] = np.interp(t, [0, 2, 4, 6, 10], [0, 10, 40, 10, 20]) q[:, 1] = np.interp(t, [0, 6, 10], [0, 0, 8]) g = np.zeros(len(t)) log = [{"type": "rewind", "rewound": True, "segment_t": [2.0, 4.0], "t_start": 4.0, "t_end": 6.0, "finished": True}] sel = bce.select_steps(t, q, g, np.array([0.0, 4.0, 6.0]), ["model", "rewind", "model"], excluded_windows(log), trim_mid_idle=True) st = sel["state"] self.assertLessEqual(st[:, 0].max(), 20.0 + 1e-6) # 40° まで行った区間が入っていない self.assertTrue(np.all(np.diff(t[sel["idx"]]) > 0)) np.testing.assert_allclose(sel["actions"][:-1, :6], st[1:, :6]) np.testing.assert_allclose(sel["actions"][-1, :6], st[-1, :6]) # つなぎ目の跳びは通常の 1 歩の範囲 (10° 付近で手前と続きがつながる) self.assertLess(np.max(np.abs(np.diff(st[:, :6], axis=0))), 1.5) def write_fake_recording(run_dir, segments, fps_cam=24.0, size=(640, 360), outcome="y", white=()): """合成の記録. segments = [(t0, t1, mode, j0_from, j0_to)]. 時刻は 1e9 秒台 (実時刻らしく).""" rec = run_dir / "recording" rec.mkdir(parents=True) base = 1.8e9 t_end = segments[-1][1] with (rec / "states.csv").open("w") as f: f.write(",".join(STATES_COLS) + "\n") rows = [] for (a, b, mode, j0a, j0b) in segments: rows.append((base + a, "stage", mode, None, None)) for tt in np.arange(a, b, 0.1): j0 = j0a + (j0b - j0a) * (tt - a) / (b - a) rows.append((base + tt + 0.01, "actual", mode, [j0, 10, 5, 0, -40, -30], None)) rows.append((base + 1.0, "gripper_cmd", "model", None, 0)) rows.sort(key=lambda r: r[0]) for ts, ev, mode, ang, gs in rows: angs = ["" if ang is None else f"{v:.3f}" for v in (ang or [None] * 6)] f.write(",".join([f"{ts:.6f}", ev, mode] + angs + ["", "" if gs is None else str(gs)] + [""] * 5) + "\n") w = cv2.VideoWriter(str(rec / "top.mp4"), cv2.VideoWriter_fourcc(*"mp4v"), fps_cam, size) with (rec / "top_frame_log.csv").open("w") as f: f.write("frame_idx,capture_ts,mode\n") for k, tt in enumerate(np.arange(0, t_end, 1.0 / fps_cam)): # white の窓 (除く区間) の中の画像だけ白 (250), ほかは暗い (k で少しずつ変える) v = 250 if any(a <= tt <= b for a, b in white) else 20 + (k % 40) img = np.full((size[1], size[0], 3), v, np.uint8) w.write(img) f.write(f"{k},{base + tt:.6f},model\n") w.release() (rec / "top_meta.json").write_text(json.dumps({"status": "ok"})) verdict = {"y": "yes", "n": "no"}[outcome] (run_dir / f"{run_dir.name}.json").write_text(json.dumps({ "recording_enabled": True, "result": "completed", "outcome": {"y": "success", "n": "failure"}[outcome], "judgments": {"pick": {"human_verdict": verdict}, "place": {"human_verdict": verdict}}})) return base class TestBuildCorrectionEpisodes(unittest.TestCase): def test_end_to_end(self): with tempfile.TemporaryDirectory() as td: td = Path(td) runs = td / "runs" good = runs / "pi05_20990101_000000" base = write_fake_recording(good, [ (0, 2, "model", 0, 10), (2, 4, "model", 10, 10), (4, 6, "model", 10, 60), # 4〜6 を巻き戻す (6, 8, "rewind", 60, 10), (8, 10, "correct", 10, 20), (10, 14, "model", 20, 35)], white=[(4.0, 8.0)]) (good / "log.jsonl").write_text(json.dumps( {"type": "rewind", "rewound": True, "segment_t": [base + 4, base + 6], "t_start": base + 6, "t_end": base + 8, "finished": True}) + "\n") bad = runs / "pi05_20990101_000100" write_fake_recording(bad, [(0, 6, "model", 0, 10)], outcome="n") (bad / "log.jsonl").write_text("") args = argparse.Namespace(force=True, no_trim_mid_idle=False) out = td / "ds" / "raw_episodes" r_bad = bce.build_one(bad, out / bad.name, args) self.assertFalse(r_bad["used"]) # 失敗の run は使わない r = bce.build_one(good, out / good.name, args) self.assertTrue(r["used"], r) z = np.load(out / good.name / "episode.npz") meta = json.loads((out / good.name / "meta.json").read_text()) self.assertLessEqual(z["state"][:, 0].max(), 35.0 + 1e-3) # 60° まで行った区間がない self.assertLess(np.max(np.abs(np.diff(z["state"][:, :6], axis=0))), 3.0) # つなぎ目が続く self.assertNotIn("rewind", meta["step_modes"]) self.assertGreater(meta["correct_steps"], 0) self.assertEqual(meta["image_size"], [384, 216]) frames = sorted((out / good.name / "frames").glob("*.jpg")) self.assertEqual(len(frames), len(z["state"])) # 除いた区間 (4〜8 秒) の画像は, 境界の最近傍としても入らない (codex R1-4) self.assertLess(max(float(cv2.imread(str(f)).mean()) for f in frames), 150) # お手本の作り方 (build_demo_episodes.build_one) もこの記録をそのまま読める ref = td / "ref" / "raw_episodes" rd = bde.build_one(good, ref / good.name, argparse.Namespace( include_failed=False, force=True, trim_mid_idle=True)) self.assertTrue(rd["used"], rd) # 一緒に学習に使えるか (キー・形・FPS・画像の大きさ) self.assertEqual(bce.check_compat(out, ref), []) # ---- run_policy_client を偽の daemon / カメラ / モデルで通す --------------------------- class FakeWorld: def __init__(self): self.q = np.array([0.0, 0, 0, 0, 0, -30]) self.sends = [] self.grips = [] self.events = [] # 送信とグリッパーを起きた順に (依頼 27: 下ろす → 開く → 戻る の順) self.fail_main_reads = 0 # 本体 (主スレッド) の読み取りをこの回数だけ失敗させる self.fail_after_label = None # この送信のあとから失敗させる self.stuck_prefix = None # この名前で始まる送信では動かない def get_angles(self, *a, **k): import threading if self.fail_main_reads and threading.current_thread() is threading.main_thread(): self.fail_main_reads -= 1 return None return [float(v) for v in self.q] def send_wait(self, target, label, tol, *a, **k): self.sends.append((label, [round(float(v), 3) for v in target])) self.events.append(("send", label, [round(float(v), 3) for v in target])) if self.stuck_prefix and label.startswith(self.stuck_prefix): return False, False # この送信では動かない (未到達) self.q = np.array(target, dtype=float) if label == self.fail_after_label: self.fail_main_reads = 5 # read_angles の 5 回とも失敗 → 次の中継点を送らずに止まる return True, False class FakeCap: def __init__(self, *a, **k): pass def isOpened(self): return True def grab(self): return True def read(self): time.sleep(0.01) return True, np.full((360, 640, 3), 100, np.uint8) def release(self): pass class FakePolicy: """state から決まる行動 (画像は見ない): 毎ステップ j0 +1°, j1 +0.5°.""" def __init__(self, *a, **k): self.uri, self.metadata = "fake", {} def infer(self, obs): s = np.asarray(obs["observation/state"], dtype=float) acts = np.zeros((10, 7)) for k in range(10): acts[k, :6] = s[:6] + np.array([1.0, 0.5, 0, 0, 0, 0]) * (k + 1) return {"actions": acts} def close(self): pass def run_client(mod, argv, inputs, td, keys="", fail_after_label=None, stuck_prefix=None, policy_cls=None): world = FakeWorld() world.fail_after_label = fail_after_label world.stuck_prefix = stuck_prefix 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(mod, "DATA_ROOT", td), mock.patch.object(mod, "LOG_ROOT", log_root), mock.patch.object(mod.camera_resolver, "resolve_cameras", lambda **k: {"top": 0}), mock.patch.object(mod.cv2, "VideoCapture", FakeCap), mock.patch.object(mod, "load_reference", lambda p: (np.zeros((216, 384, 3), np.uint8), "ref")), mock.patch.object(mod, "measure_framing", lambda a, b: (0.0, 0.0, 0.9)), mock.patch.object(mod, "PolicyClient", policy_cls or FakePolicy), mock.patch.object(mod, "get_daemon_socket", lambda *a, **k: None), mock.patch.object(mod, "close_daemon_socket", lambda *a, **k: None), mock.patch.object(mod, "daemon_get_angles", world.get_angles), mock.patch.object(mod, "daemon_set_gripper", lambda s, *a, **k: (world.grips.append(s), world.events.append(("grip", s)))), mock.patch.object(mod, "daemon_set_gripper_value", lambda v, *a, **k: (world.grips.append(("v", v)), world.events.append(("grip", ("v", v))))), mock.patch.object(mod, "_send_and_wait_arrival", world.send_wait), mock.patch.object(mod, "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)] + argv), mock.patch.object(sys, "stdin", io.StringIO(keys)), ] for p in patches: p.start() try: with mock.patch("sys.stdout", new=io.StringIO()) as out: code = mod.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, world, run, lines, out.getvalue() def strip(lines, drop=("t", "latency_s", "reference")): return [{k: v for k, v in d.items() if k not in drop} for d in lines] class TestClientFlow(unittest.TestCase): def setUp(self): self.td = Path(tempfile.mkdtemp()) time.sleep(1.1) # run のフォルダ名は秒単位なので重ならないように def test_dry_keys_flow(self): code, world, run, lines, out = run_client( rpc, ["--dry-keys", "--steps-per-chunk", "10", "--via-stride", "3", "--max-chunks", "8"], ["", "", "b", "b", "c", "q"], self.td, keys="1+++\n") self.assertEqual(code, 0) self.assertEqual(world.sends, []) # アームには何も送っていない rw = [x for x in lines if x.get("type") == "rewind"] self.assertEqual([x["chunk"] for x in rw], [1, 0]) # 新しい方から 1 回分ずつさかのぼる self.assertTrue(all(x["rewound"] and x["finished"] for x in rw)) np.testing.assert_allclose(rw[-1]["waypoints"][-1]["target"], [0, 0, 0, 0, 0, -30]) ch = [x for x in lines if "chunk" in x and x.get("type") is None] self.assertEqual(len(ch), 6) # 推論の行はどれも消えていない corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["move", "move", "move", "accept"]) self.assertEqual(corr[-1]["final_target"][0], 3.0) # 補正のあとの推論 (最後の行) は補正した姿勢から self.assertAlmostEqual(ch[-1]["state"][0], 3.0, places=4) self.assertFalse((run / "recording").exists()) # ドライランは記録しない def test_execute_flow_with_recording(self): code, world, run, lines, out = run_client( rpc, ["--execute", "--steps-per-chunk", "10", "--via-stride", "3", "--settle-retries", "0", "--state-from-target", "--ignore-framing", "--max-chunks", "8"], ["", "b", "c", "", "h", "y"], self.td, keys="1++\n") self.assertEqual(code, 0) labels = [s[0] for s in world.sends] self.assertEqual(labels[:4], ["pi05_chunk000_via02", "pi05_chunk000_via05", "pi05_chunk000_via08", "pi05_chunk000_via09"]) rw_sends = [s for s in world.sends if s[0].startswith("pi05_rewind")] self.assertEqual([s[1][0] for s in rw_sends], [9.0, 6.0, 3.0, 0.0]) # 逆順で始めの姿勢へ self.assertEqual(rw_sends[-1][1], [0.0, 0.0, 0.0, 0.0, 0.0, -30.0]) corr_sends = [s for s in world.sends if s[0].startswith("pi05_corr")] self.assertEqual([s[1][0] for s in corr_sends], [1.0, 2.0]) self.assertEqual(labels[-1], "pi05_home") trial = json.loads((run / f"{run.name}.json").read_text()) self.assertEqual(trial["outcome"], "success") self.assertEqual(trial["judgments"]["pick"]["human_verdict"], "yes") self.assertEqual(trial["end_reason"], "h") rec = run / "recording" for n in ("states.csv", "top.mp4", "top_frame_log.csv", "top_meta.json"): self.assertTrue((rec / n).exists(), n) stages = [l.split(",")[2] for l in (rec / "states.csv").read_text().splitlines()[1:] if l.split(",")[1] == "stage"] # 入力を待っている間は wait (依頼 27). Enter / b / c のあとは model に戻ってから各状態へ self.assertEqual(stages, ["model", "wait", "model", "wait", "model", "rewind", "model", "wait", "model", "correct", "model", "wait", "model", "wait", "home"]) ex = [x for x in lines if x.get("executed")] self.assertTrue(all("t_exec_start" in x and x["sent_targets"] for x in ex)) def test_partial_chunk_can_be_rewound(self): """中継点の途中で止まった推論も history に残り, 次の b でそれが巻き戻される (codex R1-3).""" code, world, run, lines, out = run_client( rpc, ["--execute", "--steps-per-chunk", "10", "--via-stride", "3", "--settle-retries", "0", "--ignore-framing", "--max-chunks", "6"], ["", "", "b", "q", ""], self.td, fail_after_label="pi05_chunk001_via02") rw = [x for x in lines if x.get("type") == "rewind"] self.assertEqual([x["chunk"] for x in rw], [1]) # 途中で止まった推論 1 を巻き戻す self.assertTrue(rw[0]["finished"]) back = [s for s in world.sends if s[0].startswith("pi05_rewind")] self.assertEqual(back[-1][1], [10.0, 5.0, 0.0, 0.0, 0.0, -30.0]) # 推論 1 を始めた姿勢へ EXEC = ["--execute", "--steps-per-chunk", "10", "--via-stride", "3", "--settle-retries", "0", "--ignore-framing", "--max-chunks", "6"] def test_grip_in_correction_before_first_chunk(self): """初回推論の前に c → g で閉じたら, 開始時の開放をしない (codex R2-1).""" code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys="g\n") self.assertEqual(world.grips[0], ("v", rpc.GRIP_VALUE)) self.assertFalse(any(x.get("initial_gripper_open") for x in lines)) ch = [x for x in lines if x.get("type") is None and "chunk" in x] self.assertEqual(ch[1]["state"][6], 1.0) # モデルには「閉」を渡す def test_b_after_failed_rewind_is_refused(self): """巻き戻しが戻りきれなかったら, 次の b で古い区間へ飛ばない (codex R2-2).""" code, world, run, lines, out = run_client(rpc, self.EXEC, ["", "", "b", "b", "q", ""], self.td, stuck_prefix="pi05_rewind") rw = [x for x in lines if x.get("type") == "rewind"] self.assertEqual([x["chunk"] for x in rw], [1]) self.assertFalse(rw[0]["finished"]) self.assertIn("これより前へは巻き戻しません", out) def test_b_after_failed_cancel_is_refused(self): code, world, run, lines, out = run_client(rpc, self.EXEC, ["", "c", "b", "q", ""], self.td, keys="1]+++b", stuck_prefix="pi05_corr_cancel") self.assertEqual([x for x in lines if x.get("type") == "rewind"], []) cancel = [x for x in lines if x.get("action") == "cancel"] self.assertFalse(cancel[0]["finished"]) self.assertFalse(any(s[0].startswith("pi05_rewind") for s in world.sends)) self.assertIn("これより前へは巻き戻しません", out) def test_unchanged_without_b_c(self): """b / c を押さなければ 変更前 (git HEAD) と同じ送信・同じログ (項目が増えるだけ).""" 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 の版が読めない") p = self.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) for argv, inputs in ( (["--execute", "--steps-per-chunk", "10", "--via-stride", "3", "--settle-retries", "0", "--state-from-target", "--ignore-framing", "--max-chunks", "6"], ["", "", "s", "", "h", "y"]), (["--execute", "--max-chunks", "4"], ["", "", "", "q", ""]), (["--steps-per-chunk", "10", "--via-stride", "3", "--max-chunks", "3"], [])): c_old, w_old, _, l_old, _ = run_client(old, argv, inputs, self.td) time.sleep(1.1) c_new, w_new, _, l_new, _ = run_client(rpc, argv, inputs, self.td) time.sleep(1.1) self.assertEqual(c_old, c_new) self.assertEqual(w_old.sends, w_new.sends, argv) self.assertEqual(w_old.grips, w_new.grips, argv) self.assertEqual(len(l_old), len(l_new)) new_keys = ("t_exec_start", "t_exec_end", "sent_targets") # HEAD が依頼 25 より前なら new_keys は新しい方にだけある. 依頼 25 以降なら両方にある (両方から外す) drop = ("t", "latency_s", "reference") + new_keys so, sn = strip(l_old, drop=drop), strip(l_new, drop=drop) self.assertEqual(so, sn, argv) # ---- 依頼 27: つかんだ区間の巻き戻し (y / n) と wait のコマ ------------------------------ class GraspPolicy(FakePolicy): """状態で決まる仮のモデル. 開いていて j0 が 10〜30 → 3 ステップ目から閉じる (つかむ). 閉じている間は j0 +1° / j1 -0.5° (持ち上げながら運ぶ). j0 が 30 以上 → 3 ステップ目から開く (離す).""" def infer(self, obs): s = np.asarray(obs["observation/state"], dtype=float) holding = s[6] > 0.5 acts = np.zeros((10, 7)) for k in range(10): acts[k, :6] = s[:6] + np.array([1.0, -0.5 if holding else 0.5, 0, 0, 0, 0]) * (k + 1) if holding: acts[k, 6] = 0.0 if (s[0] >= 30 and k >= 3) else 1.0 else: acts[k, 6] = 1.0 if (10 <= s[0] < 30 and k >= 3) else 0.0 return {"actions": acts} class EventArm(rpc.ArmIO): """仮の腕. 送信とグリッパーを起きた順に残す.""" def __init__(self, *a, **k): super().__init__(*a, **k) self.events = [] def send(self, target, tag): self.events.append(("send", tag, [round(float(v), 3) for v in target])) return super().send(target, tag) def gripper(self, state): self.events.append(("grip", int(state))) return super().gripper(state) class TestGraspRewind(unittest.TestCase): """つかんだ区間の巻き戻し (計算と送る順番). 腕は持ち上げた位置にいる.""" S = np.array([10.0, 5, 0, 0, 0, -30]) # 推論を始めた姿勢 A = np.array([12.0, 6, 0, 0, 0, -30]) # 途中の中継点 G = np.array([13.0, 6.5, 0, 0, 0, -30]) # つかんだ位置 (閉じる前に最後に送った目標) UP = [13.0, 0.0, 0, 0, 0, -30] # 持ち上げた所 (今いる所) def entry(self): return {"kind": "chunk", "chunk": 1, "start_pose": self.S, "sent_targets": [self.A, self.G], "grip_changed": True, "grip_cmd": 1, "t_start": 0.0, "t_end": 1.0} def test_placed_order(self): arm = EventArm(False, None, sim_q=self.UP) with mock.patch("sys.stdout", new=io.StringIO()): rw = rpc.run_rewind(self.entry(), arm, LO, HI, 20.0, None, grasp="placed") # つかんだ位置まで下ろす → 開く → 逆にたどって始めの姿勢へ self.assertEqual(arm.events, [("send", "pi05_rewind_down_00", list(self.G)), ("grip", 0), ("send", "pi05_rewind_00", list(self.A)), ("send", "pi05_rewind_01", list(self.S))]) self.assertEqual(rw["grasp_undo"], "placed") self.assertTrue(rw["gripper_opened"] and rw["reached_grasp"] and rw["finished"]) def test_dropped_order(self): arm = EventArm(False, None, sim_q=self.UP) with mock.patch("sys.stdout", new=io.StringIO()): rw = rpc.run_rewind(self.entry(), arm, LO, HI, 20.0, None, grasp="dropped") # 今の位置ですぐ開く → 逆にたどって始めの姿勢へ (つかんだ位置へは下ろさない) self.assertEqual(arm.events, [("grip", 0), ("send", "pi05_rewind_00", list(self.A)), ("send", "pi05_rewind_01", list(self.S))]) self.assertEqual(rw["grasp_undo"], "dropped") self.assertTrue(rw["gripper_opened"] and rw["finished"]) def test_placed_not_reached_keeps_closed(self): """つかんだ位置まで下ろせなければ開かない (宙で落とさない) → 戻りきれない扱い.""" class Stuck(EventArm): def send(self, target, tag): self.events.append(("send", tag, [round(float(v), 3) for v in target])) return False # 動かない arm = Stuck(False, None, sim_q=self.UP) with mock.patch("sys.stdout", new=io.StringIO()): rw = rpc.run_rewind(self.entry(), arm, LO, HI, 20.0, None, grasp="placed") self.assertNotIn(("grip", 0), arm.events) self.assertFalse(rw["gripper_opened"]) self.assertFalse(rw["finished"]) class TestGraspClientFlow(unittest.TestCase): """run_policy_client を偽の daemon で通す. 推論 0 = 近づく / 1 = つかむ / 2 = 運ぶ.""" EXEC = ["--execute", "--steps-per-chunk", "10", "--via-stride", "3", "--settle-retries", "0", "--ignore-framing", "--max-chunks", "8"] def setUp(self): self.td = Path(tempfile.mkdtemp()) time.sleep(1.1) def run_(self, inputs, argv=None): return run_client(rpc, argv or self.EXEC, inputs, self.td, policy_cls=GraspPolicy) @staticmethod def after_carry_rewind(world): """推論 2 (運ぶ) を巻き戻し終えたあとの出来事.""" ev = world.events last = max(k for k, e in enumerate(ev) if e[0] == "send" and e[1].startswith("pi05_rewind") and e[2][0] == 13.0) return ev[:last + 1], ev[last + 1:] def test_carry_rewind_keeps_gripper_closed(self): """運ぶ途中の区間の b: グリッパーは閉じたまま, 送った目標を逆の順にたどる (白箱にぶつかったとき).""" code, world, run, lines, out = self.run_(["", "", "", "b", "q", ""]) self.assertEqual(code, 0) carry = [s[1][0] for s in world.sends if s[0].startswith("pi05_chunk002")] self.assertEqual(carry, [16.0, 19.0, 22.0, 23.0]) back = [s[1][0] for s in world.sends if s[0].startswith("pi05_rewind")] self.assertEqual(back, [22.0, 19.0, 16.0, 13.0]) # 逆の順で推論 2 の始め (つかんだ位置) へ self.assertEqual(world.grips, [0, ("v", rpc.GRIP_VALUE)]) # 開始時の開放と つかむ だけ. 開いていない rw = [x for x in lines if x.get("type") == "rewind"] self.assertEqual([x["chunk"] for x in rw], [2]) self.assertNotIn("grasp_undo", rw[0]) self.assertTrue(rw[0]["finished"]) ch = [x for x in lines if x.get("type") is None and "chunk" in x] self.assertEqual(ch[-1]["state"][6], 1.0) # 戻ったあともモデルには「閉」 def test_grasp_rewind_y(self): """運ぶ区間を戻したあと, つかんだ区間の b で y: 下ろす → 開く → 始めの姿勢へ.""" code, world, run, lines, out = self.run_(["", "", "", "b", "b", "y", "q", ""]) self.assertEqual(code, 0) _, tail = self.after_carry_rewind(world) # つかんだ位置 (13, 6.5) には運ぶ区間の巻き戻しで既に着いているので, 下ろす送信は 0.5° 以下で省く self.assertEqual(tail, [("grip", 0), ("send", "pi05_rewind_00", [10.0, 5.0, 0, 0, 0, -30.0])]) rw = [x for x in lines if x.get("type") == "rewind"] self.assertEqual([x["chunk"] for x in rw], [2, 1]) self.assertEqual(rw[1]["grasp_undo"], "placed") self.assertTrue(rw[1]["reached_grasp"] and rw[1]["gripper_opened"] and rw[1]["finished"]) self.assertEqual(rw[1]["grasp_pose"], [13.0, 6.5, 0, 0, 0, -30.0]) self.assertEqual([w["leg"] for w in rw[1]["waypoints"]], ["down", "back"]) ch = [x for x in lines if x.get("type") is None and "chunk" in x] self.assertEqual(ch[-1]["state"][:2], [10.0, 5.0]) # 推論 1 の始めの姿勢から self.assertEqual(ch[-1]["state"][6], 0.0) # 開いた状態で再推論 self.assertNotIn("置き直したら Enter", out) def test_grasp_rewind_n(self): """つかんだ区間の b で n: 開く → 始めの姿勢へ → 写真のパスを出して Enter を待つ.""" code, world, run, lines, out = self.run_(["", "", "", "b", "b", "n", "", "q", ""]) self.assertEqual(code, 0) _, tail = self.after_carry_rewind(world) self.assertEqual(tail, [("grip", 0), ("send", "pi05_rewind_00", [10.0, 5.0, 0, 0, 0, -30.0])]) rw = [x for x in lines if x.get("type") == "rewind"] self.assertEqual(rw[1]["grasp_undo"], "dropped") self.assertTrue(rw[1]["gripper_opened"] and rw[1]["finished"]) self.assertEqual([w.get("leg") for w in rw[1]["waypoints"]], [None]) frame = str(run / "frames" / "0001.jpg") self.assertEqual(rw[1]["start_frame"], frame) self.assertIn(frame, out) self.assertTrue(Path(frame).exists()) # 置き直す間も wait stages = [l.split(",")[2] for l in (run / "recording" / "states.csv").read_text().splitlines()[1:] if l.split(",")[1] == "stage"] k = len(stages) - 1 - stages[::-1].index("rewind") self.assertEqual(stages[k:k + 4], ["rewind", "model", "wait", "model"]) def test_grasp_rewind_other_cancels(self): code, world, run, lines, out = self.run_(["", "", "", "b", "b", "x", "q", ""]) rw = [x for x in lines if x.get("type") == "rewind"] self.assertEqual([x["chunk"] for x in rw], [2]) # つかんだ区間は戻していない self.assertEqual(world.grips, [0, ("v", rpc.GRIP_VALUE)]) self.assertIn("やめました", out) def test_release_refused(self): """離した区間は今のまま断る. 置き直すなら h で終えてやり直す, と出す.""" code, world, run, lines, out = self.run_(["", "", "", "", "", "b", "q", ""]) self.assertEqual(world.grips, [0, ("v", rpc.GRIP_VALUE), 0]) # 推論 4 で離した self.assertEqual([x for x in lines if x.get("type") == "rewind"], []) self.assertIn("ブロックを置き直すなら h で終えてやり直してください", out) def test_dry_keys_grasp_y(self): code, world, run, lines, out = run_client( rpc, ["--dry-keys", "--steps-per-chunk", "10", "--via-stride", "3", "--max-chunks", "8"], ["", "", "", "b", "b", "y", "q"], self.td, policy_cls=GraspPolicy) self.assertEqual(world.sends, []) rw = [x for x in lines if x.get("type") == "rewind"] self.assertEqual([x["chunk"] for x in rw], [2, 1]) self.assertEqual(rw[1]["grasp_undo"], "placed") self.assertTrue(rw[1]["finished"]) class TestWaitExcluded(unittest.TestCase): def test_wait_rewind_and_undone_not_in_episode(self): """wait のコマ・rewind のコマ・巻き戻しで取り消した区間は, 停止の詰め方 (先頭を残す) を 通っても学習用エピソードに入らない.""" with tempfile.TemporaryDirectory() as td: td = Path(td) run = td / "runs" / "pi05_20990101_000300" # 0〜3 近づく / 3〜5 つかんで運ぶ (あとで n で巻き戻す) / 5〜7 rewind / 7〜10 wait (置き直す) # / 10〜14 model. 3〜10 秒の画像は白 (手が写る想定) base = write_fake_recording(run, [ (0, 3, "model", 0, 15), (3, 5, "model", 15, 40), (5, 7, "rewind", 40, 15), (7, 10, "wait", 15, 15), (10, 14, "model", 15, 30)], white=[(3.0, 9.99)]) (run / "log.jsonl").write_text(json.dumps( {"type": "rewind", "rewound": True, "grasp_undo": "dropped", "gripper_opened": True, "segment_t": [base + 3, base + 5], "t_start": base + 5, "t_end": base + 7, "finished": True}) + "\n") out = td / "ds" / "raw_episodes" / run.name r = bce.build_one(run, out, argparse.Namespace(force=True, no_trim_mid_idle=False)) self.assertTrue(r["used"], r) meta = json.loads((out / "meta.json").read_text()) z = np.load(out / "episode.npz") self.assertEqual(set(meta["step_modes"]), {"model"}) self.assertGreater(meta["excluded_mode_steps"], 0) # rewind と wait self.assertGreater(meta["excluded_window_steps"], 0) # 取り消した区間 self.assertLessEqual(z["state"][:, 0].max(), 30.0 + 1e-3) self.assertLess(np.max(np.abs(np.diff(z["state"][:, :6], axis=0))), 3.0) frames = sorted((out / "frames").glob("*.jpg")) self.assertEqual(len(frames), len(z["state"])) self.assertLess(max(float(cv2.imread(str(f)).mean()) for f in frames), 150) def test_select_steps_drops_long_wait(self): """止まっている区間の詰め方は先頭 2 コマを残すが, wait は詰める前に外すので 1 コマも残らない.""" t = np.arange(0, 10, 0.1) q = np.zeros((len(t), 6)); q[:, 5] = -30 q[:, 0] = np.interp(t, [0, 3, 7, 10], [0, 10, 10, 20]) # 3〜7 秒は止まっている g = np.zeros(len(t)) sel = bce.select_steps(t, q, g, np.array([0.0, 3.0, 7.0]), ["model", "wait", "model"], [], trim_mid_idle=True) kt = t[sel["idx"]] self.assertFalse(np.any((kt >= 3.0 - 1e-9) & (kt < 7.0 - 1e-9))) self.assertNotIn("wait", sel["modes"]) self.assertEqual(sel["counts"]["excluded_mode"], 40) if __name__ == "__main__": unittest.main() ```` ## 次は実機で巻き戻しと補正を試し、直したお手本で π0.5 を学習し直す 次の回でやることは3つです。 - 上の手順で、巻き戻し・補正・つかんだ区間の y と n を実機で試し、記録の頻度と送信の遅れを測る - 位置D と位置E で失敗した所を巻き戻して直し、成功した回を集める。集まったら `build_correction_episodes.py` で `datasets/mecharm_redblock_box_corr_v1` を作る - 元のお手本19本と直した区間を合わせて、Runpod で π0.5 を学習し直し、LOG401 と同じ3か所に5回ずつ置いて、入る割合が変わるかを数える