--- title: "フィジカルAI研究室を作る Log#16 | 直した mechArm 270 の動きを π0.5 に覚えさせる手前まで" slug: rewind-correct-real-arm date: 2026-10-30 day: 407 meta_description: "Physical Intelligenceのπ0.5で動かす実物のmechArm 270で、失敗しかけた動きをキーで巻き戻し根元の向きを直すと、AIは続けてブロックをつかめた。持ち上げる間に爪がゆるみ、直した回はまだ箱まで運べていない。" tags: [フィジカルAI研究室, π0.5, Physical Intelligence, mechArm 270, 模倣学習, 巻き戻し, 介入, Runpod, Claude Code, 再現手順] series: "フィジカルAI研究室" --- Physical Intelligence の π0.5 で動かす実物の小型ロボットアーム mechArm 270 で、失敗しかけた動きをキーボードで巻き戻し、根元の向きを人が直すと、AI はその姿勢から続けてブロックをつかめました。ただ、つかんだあとに人が腕を持ち上げている間に爪がゆるんでブロックを落とし、人が直した回はまだ箱まで運べていません。 π0.5（パイレイテンゴ）は、米国の [Physical Intelligence](https://www.pi.website/) 社が公開しているロボット用の AI のモデルです。上から見たカメラの画像と6つの関節の角度を受け取り、次の1秒ぶんの関節の角度を返します。人が手で教えた19回の動き（お手本）で学習させ、赤いブロックをつかんで白い箱に入れる作業をさせています。前の回（LOG403）では、その実機推論に、失敗した動きを逆にたどって戻すキー（b）と、関節を1°ずつ動かして直すキー（c）を足しました。人が直した動きを写真と組で記録し、次の学習で AI に覚えさせるためです。ただ、そのときはアームを一度も動かしていませんでした。 この回は、そのキーを初めて実機で使いました。π0.5 は、借りた GPU（[Runpod](https://www.runpod.io/) の A40）の上で動かし、Mac から手元の mechArm 270 に関節の角度を送ります。3回動かした結果は次のとおりです。 | 回 | やったこと | 結果 | |---|---|---| | 1回目 | キーを使わず、AI だけで動かした | ブロックを箱に入れた。足した記録（1秒に約9コマ）は動き、関節を送る間隔も前と変わらなかった | | 2回目 | b で巻き戻し、c で根元の向き（j0）を直した | 巻き戻しは4点を逆にたどって始めの姿勢へ戻った。ブロックから離れていく向きを21回直すと、AI はブロックの方へ回り続けてつかんだ。運ぶ途中で手首（j4）を上げようとしたが上がらず、箱に当たってブロックを落とした | | 3回目 | つかんだあと、肩（j1）とひじ（j2）を直して箱の縁より上へ持ち上げた | 肩を − に動かすと、ブロックを持ったまま腕が上がると分かった。つかんでから約3分、爪を締め直さないまま持ち上げているうちに、ブロックが爪からすべり落ちた | **人が根元の向きを直すと、AI はその姿勢から続きを組み立て直してブロックをつかみに行きました。** 人が横から手を入れて、AI がその続きを引き取るという形が、mechArm 270 の実機で成り立ったことになります。足りなかったのは、つかんだあとです。爪は一度閉じたらそのままで、人が腕を動かしている間に締め直す手段がありませんでした。そこで、補正の途中で爪を締め直すキー（r）とつかみ直すキー（R）を足す依頼を出し、この回のうちに出来上がっています。 【画像】LOG407_02_run2_top_j0.jpg 2回目を上のカメラで撮ったコマ。左上が根元の向きを直す前（爪がブロックの左にある）、右上が直している途中、左下が直し終えたところ（爪がブロックの真上）、右下が AI がブロックをつかんだあと 人が直した区間を学習データにするには、直したうえで箱に入れ、「成功」で終わる必要があります。この回の3回のうち成功は直さなかった1回だけなので、直した動きの学習データはまだできていません。 追記：同じ日の21時に動かした4回目では、つかんだあと箱の縁の手前で止まりかけた腕を、人が肩の j1 で持ち上げてから AI に戻し、箱に入れて「成功」で終えられました。起動の手順も `lab_start.sh` の1行にまとめています。詳しくは、下の「追記」の節に載せています。翌朝には、Pod の片付けと、推論の確認のキーの押し間違いも直しました（「追記2」の節）。 GPU を買うか借りるかの判断は、Runpod で実際に借りて学習させた費用をもとに、別のページにまとめています。 【バナー】gpu-buy-or-rent この回の作業は、Claude（手順と分析を一緒に組み立てた AI）と進めました。アームの操作と、目で見た判断はこちらで行い、記録の集計は Claude が行っています。爪の締め直しのキーは [Claude Code](https://claude.com/claude-code)（プログラムを書く AI）に依頼箱を通して頼み、[OpenAI の codex](https://github.com/openai/codex)（別の会社のプログラムを書く AI）にコードを読んでもらってから受け取りました。ここから下は、使ったコマンド・出力・依頼文・報告・コードを全量で載せた作業ログです。ログの中の Pod の IP アドレスとポート番号は、`` と `` に置き換えています。Hugging Face の鍵（アクセストークン）は、どこにも載せていません。費用の円は 1ドル＝158円 で計算しています。結果の数字は記録から数えた値で、見込みのものは見込みと書いています。 ## 作業ログ ― Runpod の A40 に π0.5 を立て、mechArm 270 で巻き戻しと補正を試した ### この回で使う言葉 前の回で足した仕組みの言葉を、先にまとめます。 | 言葉 | 意味 | |---|---| | チャンク | AI に1回問い合わせて返ってくる、次の1秒ぶん（10ステップ）の関節の角度。プログラムはチャンクを1つ受け取るたびに止まり、Enter で実行するかを聞く | | 中継点 | チャンクの10ステップのうち、実際にアームへ送る点。この回は3ステップおきに4点（2・5・8・9番目）を送った | | 巻き戻し（b） | 直前に実行したチャンクで送った中継点を逆の順にたどって送り、そのチャンクを始めたときの姿勢へ戻すキー | | 補正（c） | キーで関節を選び、1回押すごとに決まった角度（刻み）だけ動かすモード。Enter で、その姿勢から AI に戻す。c を押したときに AI が返していた答えは送らない | | j0〜j5 | mechArm 270 の6つの関節。j0 は根元の回転、j1 は肩、j2 はひじ、j3〜j5 は手首。補正では 1〜6 のキーが j0〜j5 にあたる | | 1秒10コマの記録 | 推論の間、上カメラの写真と関節の角度を1秒に10回ずつ残す仕組み。コマごとに「AI の動き（model）・入力待ち（wait）・巻き戻し（rewind）・補正（correct）・HOME へ戻る（home）」の印が付く | ### Runpod は、GPU を時間で借りられるサービス [Runpod](https://www.runpod.io/) は、GPU を積んだ計算機（Pod）を、1時間いくらで借りられるサービスです。この研究室では、π0.5 を動かすサーバーを Pod の上に立て、Mac から SSH のトンネル（Pod の 8000 番を Mac の 8000 番につなぐ通り道）で問い合わせています。 | 言葉 | 意味 | |---|---| | Pod | Runpod で借りた1台の計算機。消すと中身も消える | | Connect | 借りた Pod の画面のボタン。SSH でつなぐための IP アドレスとポート番号が出る | | Terminate | Pod を消すボタン。Stop だけではディスクの料金がかかり続けるので、使い終わったら Terminate まで押す | この回は、前の回と同じ A40（GPU のメモリ 48GB）を1枚、テンプレートは Runpod Pytorch 2.8.0 で借りました。 ### Hugging Face は、AI のモデルを置いて配るサービス [Hugging Face](https://huggingface.co/) は、AI のモデルとデータを置いて、ほかの計算機から取ってこられるようにするサービスです。学習させた π0.5 のモデル（約6.3GB）は、Hugging Face の非公開の置き場所（リポジトリ）に置いてあり、Pod を借りるたびに `hf download` というコマンドで取ってきます。取ってくるには、自分のアカウントの鍵（アクセストークン。`hf_` で始まる37文字）が要ります。 ### 起動の手順：Pod・トンネル・アームの順に3つのターミナルを開く 使ったコマンドを、打った順にまとめます。Mac のターミナルを3つ使います。ターミナル1は Pod の中、ターミナル2は Pod とのトンネル、ターミナル3はアームです。 ターミナル1（Mac から Pod へスクリプトを送り、Pod に入る）: ``` cd ~/r2r2r-scripts P= IP= K=~/.ssh/id_ed25519 ssh -p $P -i $K root@$IP "mkdir -p /workspace/r2r2r /workspace/logs /workspace/ckpt" scp -P $P -i $K -r ~/r2r2r-scripts/pi05 root@$IP:/workspace/r2r2r/ scp -P $P -i $K /Volumes/KIOXIA/r2r2r-data/lab/Log11/pod/add_trim_config.py root@$IP:/workspace/r2r2r/ ssh -p $P -i $K root@$IP ``` ターミナル1（Pod の中。鍵を入れて、モデルを取ってきて、サーバーを起動する）: ``` tmux new -s serve pip install -U --break-system-packages huggingface_hub hash -r read -s HF_TOKEN && export HF_TOKEN hf auth whoami time hf download PixelstoMotors/pi05-mecharm-trim-lora-v1 --local-dir /workspace/ckpt/3999 cd /workspace git clone --recurse-submodules https://github.com/Physical-Intelligence/openpi.git cd openpi && git checkout 215abfb curl -LsSf https://astral.sh/uv/install.sh | sh source $HOME/.local/bin/env GIT_LFS_SKIP_SMUDGE=1 uv sync python3 /workspace/r2r2r/pi05/openpi_addon/install_addon.py /workspace/openpi python3 /workspace/r2r2r/add_trim_config.py /workspace/openpi mkdir -p assets/pi05_mecharm_trim_lora/local && cp -r /workspace/ckpt/3999/assets/local/mecharm_redblock_box_teach_v1_trim2 assets/pi05_mecharm_trim_lora/local/ uv run scripts/serve_policy.py policy:checkpoint --policy.config=pi05_mecharm_trim_lora --policy.dir=/workspace/ckpt/3999 2>&1 | tee /workspace/logs/serve.log ``` `tmux` は、接続が切れても Pod の中の作業が続く画面です。[openpi](https://github.com/Physical-Intelligence/openpi) は、Physical Intelligence が公開している π0.5 を動かすためのコードで、前の回と同じコミット 215abfb の版を使っています。 ターミナル2（Mac。Pod の 8000 番を手元の 8000 番へつなぐ）: ``` ssh -N -L 8000:localhost:8000 -p -i ~/.ssh/id_ed25519 root@ ``` ターミナル3（Mac。アームの常駐プログラムを確かめ、HOME に戻し、爪を開き、ブロックの位置を合わせてから推論する）: ``` cd ~/r2r2r-scripts ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 'pgrep -f mecharm_daemon.py >/dev/null && echo デーモン動いてる || (setsid nohup python3 /home/ubuntu/mecharm_daemon.py >/home/ubuntu/daemon.log 2>&1 &); sleep 8; tail -3 ~/daemon.log; ss -ltn | grep 5555' python3 dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py RAW=/Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes python3 -c "from visual_servo_pick import get_daemon_socket, daemon_set_gripper, close_daemon_socket; get_daemon_socket(); daemon_set_gripper(0); close_daemon_socket()" python3 pi05/camera_align.py --raw-dir $RAW --episode run_0001_20261019_194818 --scale 5 --alpha 0.7 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 ``` 位置合わせ（`camera_align.py`）は、お手本の最初の写真と今の上カメラの写りを重ねて、ブロックをお手本と同じ場所に置くための画面です。この回は、前の回（LOG401）の位置E と同じ、お手本 `run_0001_20261019_194818`（つかんだ瞬間の j0 が +13.2°）の場所に置きました。 ### Pod の準備で、IP の書き方と鍵の貼り付けを間違えた 最初は、手順書の `` と `` をそのまま打ち、Mac のターミナルが止まりました。`<` と `>` は、ターミナルでは特別な記号として読まれます。Runpod の Connect に出る `ssh root@（IP） -p （ポート）` の1行から値を取り、`P=`・`IP=` に入れ直して通りました。 Pod の中では、Hugging Face の鍵を1回貼り間違えました。鍵は `read -s`（打った文字を画面に出さず、履歴にも残さない）で入れているので、貼った中身は見えません。文字数だけを出して確かめると51文字で、37文字の鍵とは別の文字列でした。前の回（LOG401）でも50文字を貼っていて、同じ間違いを2回しています。貼り直して `✓ Logged in` を確かめてから、モデルを取ってきました。 ### アームの常駐プログラムが、5555番で受け付けていなかった アームは、Raspberry Pi の上で動く常駐プログラム（`mecharm_daemon.py`）が、5555番で Mac からの指令を受け付けて動かしています。ターミナル3の最初の確認で「デーモン動いてる」とは出ましたが、いつもその下に出る `LISTEN ... 0.0.0.0:5555` の行がなく、HOME へ戻すのも推論も `ConnectionRefusedError`（つなぐのを断られた）で止まりました。プログラムは残っているのに、受け付けていない状態です。 止めて起動し直すコマンドを、2回間違えました。`pkill -f mecharm_daemon.py` は、名前に `mecharm_daemon.py` を含む処理を全部止めます。同じ1行の中に起動する側の `mecharm_daemon.py` も書いていたので、Pi の上でこのコマンドを動かしている処理まで止まり、起動する前に終わっていました。止めるのと起動するのを2回に分け、止める側は `"[m]echarm_daemon.py"` と書いて（自分の行には `[m]` という文字がそのまま入るので、自分には当たらない）通りました。 ``` ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 'pkill -f "[m]echarm_daemon.py"; echo 止めた' ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 '(setsid nohup python3 /home/ubuntu/mecharm_daemon.py >/home/ubuntu/daemon.log 2>&1 &); sleep 8; tail -5 ~/daemon.log; ss -ltn | grep 5555' ``` 起動し直したときの記録では、アームの姿勢は `[0.17, 93.42, -11.07, 2.02, -84.19, -30.05]` で、肩（j1）が 93° まで倒れていました。その前の位置合わせで、写りの合い具合（response）が 0.01 前後しか出なかったのは、アームが HOME にいなかったためと見ています（見込み）。HOME に戻してから合わせ直すと、response は 0.967 になりました。 ## 1回目：キーを使わず、AI だけでブロックを箱に入れた（pi05_20261030_184245） 最初の1回は、b と c を使わず、確認はすべて Enter で通しました。足した記録が実機で動くかと、記録のせいで関節を送るのが遅れないかを見るためです。推論14回で、爪を閉じたのが推論4回目、箱の上で開いたのが推論9回目です。推論13回目の確認で h を押して HOME に戻し、結果を「成功」にしました。 【画像】LOG407_01_run1_side.jpg 1回目を横から撮った動画のコマ（12秒おき）。上の段の左から、動き始め・ブロックへ下りる・爪を閉じる前・つかむ所、下の段は、持ち上げて箱の方へ運び、右下で箱の上に来ている ### 1秒10コマの記録は、1秒9.03コマで残った 記録の数字は次のとおりです（`recording/top_meta.json` と `top_frame_log.csv` から数えた値）。 | 項目 | 値 | |---|---| | 上カメラのコマ | 2,174コマ（240.6秒）。1秒に9.034コマ | | 関節の角度を読めた回数 | 1,110回。1秒に4.61回。読めなかった回数は0 | | 書き込みの失敗 | 0回 | | コマの印 | AI の動き（model）1,899・入力待ち（wait）252・HOME へ戻る（home）23 | 関節の角度を読めたのは1秒4.61回で、写真の約半分でした。お手本は1秒10コマなので、学習データを作るときに、写真1枚ごとに関節の角度をどう合わせるかを確かめる必要があります。 ### 関節を送る間隔は、記録を足す前と同じだった 記録を足したことで、関節を送るのが遅れていないかを、推論の答えが返ってから次の推論が返るまでの間隔で比べました（次の推論にかかった時間を引いた値。Enter を押すまでの時間も含みます）。 | 記録 | 記録あり・なし | 中継点4点のチャンクの間隔（秒） | |---|---|---| | pi05_20261029_150950（LOG401） | なし | 20.1, 11.6, 11.2, 11.5, 12.8 | | pi05_20261029_150534（LOG401） | なし | 21.0, 11.7, 41.1, 11.5, 12.2, 11.9, 11.8, 11.2, 11.2, 11.6, 11.3, 11.2 | | pi05_20261030_184245（この回） | あり | 22.8, 12.1, 11.5, 11.7, 12.9, 12.2, 12.5, 11.6, 12.3, 12.0, 14.4 | チャンク1つを送り終えるまでの時間（`t_exec_start` から `t_exec_end`）は、この回の記録で中継点4点のチャンクが 10.5〜10.7秒でした（最初のチャンクは爪を開く時間を含めて18.1秒）。 - ねらい：補正した動きを写真と組で残すために、推論の間ずっと上カメラと関節の角度を記録する - 変わったこと：推論の間、1秒9.03コマの写真と、1秒4.61回の関節の角度が残るようになった。入力待ちの252コマには印が付き、学習データから外せる - 見込める効果：チャンクの間隔は記録なしの回と同じ11〜13秒台で、記録を足したことによる遅れは見えなかった（記録の値）。補正した動きを、そのまま学習データの材料として残せる 最初の推論だけ 48,251ms かかり、2回目からは 0.80〜2.62秒でした。サーバーを起動して最初の問い合わせのときに、モデルの準備が走ったためと見ています（見込み）。2回目と3回目の推論の最初は 1.78秒と 1.51秒でした。 ## 2回目：b で巻き戻し、c で根元の向きを直すと、AI はブロックをつかみに戻った（pi05_20261030_185133） ### b は、中継点4点を逆にたどって始めの姿勢へ戻った 推論0回目を実行したあと、推論1回目の確認で b を押しました。推論0回目で送った中継点4点を、逆の順にたどって送り、推論0回目を始めた姿勢（HOME 付近）まで戻っています。 ``` [REWIND] 推論 0 を巻き戻す: 中継点 4 点 → 始めの姿勢 -0.4 +1.1 +1.2 +0.1 +0.2 -30.1 [BACK ] 1/4 → +3.5 +15.8 +12.0 -2.6 -20.9 -27.5 着いた [BACK ] 2/4 → +1.9 +10.6 +7.9 -1.6 -13.5 -28.5 着いた [BACK ] 3/4 → +0.4 +5.3 +3.9 -0.5 -6.0 -29.5 着いた [BACK ] 4/4 → -0.4 +1.0 +1.2 +0.1 +0.2 -30.1 着いた ``` 戻ったあと、その姿勢からもう一度推論が出て（推論2回目）、続けられました。巻き戻しの区間は、記録に `rewind` の印で95コマ残っています。 ### 根元の j0 を21回直すと、AI はブロックの方へ回り続けた このあと、補正を5回使いました。 | 補正 | 入った所 | 動かした関節 | 刻みの数 | 始めの姿勢（j0, j1, j2） | 終わりの実測（j0, j1, j2） | 爪 | |---|---|---|---|---|---|---| | 1 | 推論2回目のあと | j0 | 7 | +0.08, +2.19, +2.63 | −3.16, +3.07, +3.33 | 開 | | 2 | 推論3回目のあと | j0 | 5 | −3.16, +3.07, +3.33 | −4.39, +3.86, +4.04 | 開 | | 3 | 推論5回目のあと | j0 | 21 | −10.28, +25.75, +13.00 | +10.54, +26.71, +13.53 | 開 | | 4 | 推論7回目のあと | j0 | 6 | +16.17, +46.49, +25.22 | +12.12, +47.28, +25.22 | 開 | | 5 | 推論11回目のあと | j3・j4 | 9 | +13.27, +50.97, +27.86 | +13.79, +51.50, +27.50 | 閉 | 補正1と2では、j0 を − の向き（ブロックから離れる向き）へ動かしていました。そのあと AI の答えも j0 を − へ回し続け、推論4回目で −6.1°、推論5回目で −11.4° 動かす答えになりました。ブロックをつかむ位置は j0 が +13° あたりです。 補正3で、j0 を −10.3° から +10.7° まで、1°ずつ21回動かしました。その次の推論6回目から、AI は j0 を +5.7° とブロックの方へ回し、推論9回目で爪を閉じてブロックをつかんでいます。補正4は、行きすぎた j0（+16.2°）を +12.2° に戻したものです。 | 推論 | AI の答えの j0 の動き | 実行 | |---|---|---| | 4回目 | −6.1° | した | | 5回目 | −11.4° | せずに補正3 | | 6回目 | +5.7° | した | | 7回目 | +0.3° | せずに補正4 | | 8回目 | +0.2° | した | | 9回目 | +0.9°（爪を閉じる） | した | - ねらい：AI がブロックから離れていくとき、最初からやり直すのではなく、人が向きだけ直して AI に続けさせる - 変わったこと：j0 を21回直した姿勢から、AI はブロックの方へ回り直し、爪を閉じてブロックをつかんだ - 見込める効果：外れかけた姿勢から正しい向きへ戻る動きが、写真と組で記録に残った（補正の区間は `correct` の印で3,084コマ）。この回は失敗で終えたので学習データにはならないが、成功で終えれば、お手本19本にはない「外れかけたときの戻り方」を足せる見込み ### 手首の j4 は、ブロックを持ったまま上がらなかった つかんで持ち上げたあと、推論11回目で AI は j0 を +18.6° 回すのと同時に、ひじの j2 を −20.3° 動かして腕を前へ伸ばす答えを返しました。持ち上がりきらないうちに箱の方へ動くと、箱の縁に当たります。そこで補正5で、手首の j4 を上げて爪を高くしようとしました。 | 送った j4 の目標 | そのときの実際の j4 | 表示 | |---|---|---| | −53.7° | 差 0.52° | 着いた | | −52.7° | 差 1.51° | 着いた | | −51.7° | 送る前で約 −56.9°、8秒待っても −56.42° | 届かない。再送しても届かず | 目標は +1° ずつ上げているのに、実際の j4 は −54.7° 付近から −56.4° まで下がりました。ブロックの重さが手首にかかり、たれ下がっていたと見ています（見込み）。手首は爪の向きを変える関節なので、上がったとしても高さはあまり変わりません。 ここで1つ分かったことがあります。補正の刻みは1°なのに、「着いた」の判定は 3°以内でした。**1°刻みの補正だと、関節がまったく動かなくても「着いた」と出ます。** 2つ目の −52.7° が「着いた」になったのはそのためです。 そのあと推論12回目・13回目を実行し、腕は箱の方へ動きましたが、爪が箱の縁に当たって箱が押され、ブロックは箱の外に落ちました。推論14回目の確認で h を押して HOME に戻し、結果を「失敗」にしました。 【画像】LOG407_03_run2_top_box.jpg 2回目の終わりを上のカメラで撮ったコマ。左上はブロックを持った所、右上は箱へ寄った所、左下は箱が押されて斜めになり、ブロックが爪から外れた所、右下はブロックが箱の外に残った所 ### 2回目の記録の数字 | 項目 | 値 | |---|---| | 上カメラのコマ | 6,419コマ（717.7秒）。1秒に8.944コマ | | 関節の角度を読めた回数 | 5,040回。1秒に7.02回 | | コマの印 | 補正（correct）3,084・入力待ち（wait）2,093・AI の動き（model）1,124・巻き戻し（rewind）95・HOME へ戻る（home）23 | | 巻き戻し | 1回 | | 補正 | 5回（刻みは合わせて48回） | ## 3回目：肩の j1 を − に動かすと、ブロックを持ったまま腕が上がった（pi05_20261030_190659） 2回目で箱に当たったので、3回目は、つかんだあと箱の縁より上まで腕を持ち上げてから、AI に戻すことにしました。補正は4回使っています。 | 補正 | 入った所 | 動かした関節 | 刻み | 刻みの数 | 始めの姿勢（j1, j2, j4） | 終わりの実測（j1, j2, j4） | 爪 | |---|---|---|---|---|---|---|---| | 1 | 推論2回目のあと | j0（+8.7 → +11.7） | 1° | 3 | +38.84, +24.60, −53.96 | +39.81, +24.69, −53.78 | 開 | | 2 | 推論5回目のあと | j1 | 1° | 1 | +58.44, +21.97, −62.66 | +58.88, +21.79, −62.66 | 閉 | | 3 | 推論6回目のあと | j1 | 2° | 2 | +58.88, +21.79, −62.66 | +63.36, +21.35, −68.90 | 閉 | | 4 | 推論8回目のあと | j2 → j1 | 1°・2° | 24 | +54.14, +28.47, −67.85 | +29.53, +34.71, −67.67 | 閉 | 補正2と3では、j1 を + に動かしました。j1 を + にすると肩が前へ倒れ、爪は机の方へ下がります。補正3の2つ目（j1 の目標 +62.9°）は8秒で届かず、「未到達」と出ています。爪が机に押しつけられていたと見ています（見込み）。 補正4では、まず j2 を +6°（+28.5° → +34.5°）動かし、そのあと j1 を 2°ずつ12回、+54.1° から +28.1° まで − に動かしました。j1 を − にすると肩が後ろへ起き、ブロックを持ったまま腕が上がりました。2回目で上がらなかった手首の j4 とは違い、j1 は送った目標に毎回 0.9〜1.7° 足りない所まで動いています。 | j1 の目標 | 送ったあとの実測の j1 | |---|---| | 52.14 | 52.99 | | 50.14 | 51.41 | | 48.14 | 49.83 | | 46.14 | 47.72 | | 44.14 | 45.70 | | 42.14 | 43.68 | | 40.14 | 41.74 | | 38.14 | 39.63 | | 36.14 | 37.70 | | 34.14 | 35.59 | | 32.14 | 33.57 | | 30.14 | 31.46 | | 28.14 | 29.53 | ### 持ち上げている間に爪がゆるみ、ブロックがすべり落ちた 推論4回目で爪を閉じてから、補正4を終えるまで約3分40秒ありました。補正4だけで116秒です。その間、爪を閉じる指令は推論4回目の1回だけで、締め直していません。横から撮った動画では、j1 を − に動かして腕が上がっていく途中で、ブロックが爪の間からすべり落ちています。 【画像】LOG407_04_run3_side_drop.jpg 3回目を横から撮った動画のコマ。左上は爪がブロックをつかんでいる所、右上は少し持ち上がった所、左下はブロックが爪の間から下がっている所、右下はブロックが机に残り、爪だけが上がった所 推論9回目の確認で h を押して HOME に戻し、結果を「失敗」にしました。 確認の画面で `c]` と打った所（推論7回目）は、補正に入らず、そのチャンクを実行しています。確認の画面は、知らない文字が来ると Enter と同じに扱う作りです。補正の刻みを `]` で 2° に切り替えるつもりで、補正に入る前に打っていました。 ### 3回目の記録の数字 | 項目 | 値 | |---|---| | 上カメラのコマ | 2,839コマ（323.2秒）。1秒に8.785コマ | | 関節の角度を読めた回数 | 1,922回。1秒に5.95回 | | コマの印 | 補正（correct）1,727・AI の動き（model）673・入力待ち（wait）416・HOME へ戻る（home）23 | | 補正 | 4回（刻みは合わせて30回） | ## 3回の結果のまとめ | 回 | 記録 | 巻き戻し | 補正 | 結果 | 学習データに使えるか | |---|---|---|---|---|---| | 1 | pi05_20261030_184245 | 0回 | 0回 | 成功 | 使える（AI だけの動き） | | 2 | pi05_20261030_185133 | 1回 | 5回 | 失敗（箱に当たってブロックが外へ） | 使わない | | 3 | pi05_20261030_190659 | 0回 | 4回 | 失敗（持ち上げる途中で落とした） | 使わない | 【図解】LOG407_rewind_correct_runs.html 前の回で決めたとおり、直した動きを学習データにするスクリプト（`build_correction_episodes.py`）は、結果が「成功」の回だけを使います。実機で分かったことは次の4つです。 - 巻き戻し（b）は、送った中継点を逆にたどって、推論を始めた姿勢へ戻れた - 根元の j0 の補正は、送った目標どおりに動き、AI はその姿勢から続けてブロックをつかめた - ブロックを持ったまま高くするには、手首の j4 ではなく肩の j1 を − に動かす - 爪は一度閉じたらそのままで、補正で数分持ち上げている間にゆるむ ## 依頼34：補正の途中で爪を締め直すキー（r・R）を足し、「着いた」の判定を刻みに合わせた 3回目のあと、補正のモードに、爪を締め直すキーと、つかみ直すキーを足すことにしました。キーの割り当ては次の案で進めています。 | キー | 動き | |---|---| | r | 爪を開かずに、閉じる指令をもう一度送って締め直す。持ったままゆるんだときに使う | | R | 爪を開いて、閉じ直す。ブロックが爪の中でずれたときに、その場でつかみ直す | あわせて、補正のときだけ、「着いた」の判定を刻みの大きさに合わせることにしました。止める条件は増やさず、ターミナルの表示が「着いた」か「届いていない」かを正しく出すだけです。依頼は依頼箱に置き、Claude Code が10分で仕上げました。 ### 依頼文 20261030_34_補正に爪の締め直しキーを足し到着の判定を刻みに合わせる.md ```markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 120 --- # 依頼：補正モードに爪の締め直し・つかみ直しのキーを足し、補正の「着いた」の判定を刻みに合わせる（2026-10-30） 依頼 25・27（コミット 4387d69）の続き。対象は pi05/run_policy_client.py の補正モード（c キー）。 まず logs/ の直近の作業記録と docs/review_result_20261029_grasp_rewind_codex.md を読んでから始める。 ## 実機で分かったこと（2026-10-30、エイイチが実機で試した） 記録は KIOXIA の runs/pi05_policy/ の pi05_20261030_184245（b・c なしで成功）、pi05_20261030_185133（失敗）、pi05_20261030_190659（失敗）。ターミナルの出力は lab/Log16/terminal_1836-1915.txt。 - b（巻き戻し）と c（補正）は思ったとおりに動いた。j0 の補正で向きを直すと、AI はその姿勢から続けてブロックをつかめた - 肩 j1 を − にすると腕が上がり、+ にすると机の方へ下がる（190659 の補正4で、j1 を 54°→28° に下げてブロックを持ち上げた） - **爪がゆるむ**：190659 では、爪を閉じたのは推論4回目の1回だけ（GRIPPER VALUE=20）。そのあと補正で約3分持ち上げている間に、ブロックが爪からすべり落ちた（動画で確認） - **補正の「着いた」の判定がゆるい**：補正の刻みは 1〜2° なのに、到着の判定は 3° 以内。185133 の補正5では、j4 を +1° ずつ送っても実際の j4 は −54.7°→−56.4° と逆に下がったのに、途中まで「着いた」と出ていた ## A. 締め直し・つかみ直しのキー（補正モードの中） - `r`：爪を開かずに、閉じる指令（今と同じ値・速さ）をもう一度送って締め直す - `R`：爪を開き、少し待ってから（0.5〜1 秒。爪が開ききる時間に合わせて決めてよい）閉じ直す。爪の中でブロックがずれたとき、その場でつかみ直すため - 爪が開いている状態で `r` を押したときは、何もせずに「爪が開いているので締め直しはしない（閉じるのは g）」と表示する - 押した時刻・どちらのキーか・前後のグリッパーの状態を log.jsonl の `type: "correction"` に残す（action は例 `regrip_tighten` / `regrip_reopen`） - 補正の一覧（? で出る表示と、補正に入ったときの表示）に r・R を足す - 今の `g`（開け閉めの切り替え）はそのまま - 補正を b で取り消すときの扱い（グリッパーも始めの状態へ戻す）はそのまま。r・R を使ったあとでも、始めの状態へ戻せばよい ## B. 補正のときの「着いた」の判定 - 補正モードで関節を動かしたときだけ、到着の判定を「その刻みの大きさの半分」程度にする（例：刻み 1° なら 0.5°、2° なら 1°。決め方は実機の読み値のぶれ（0.5〜1°程度）を見て決めてよい。決めた値と理由を報告に書く） - 判定は、動かした関節について見る（ほかの関節の小さな揺れで「届いていない」にしない） - 届いていないときは、今と同じく「届いていない」と表示して、そのまま補正を続けられるようにする。**新しい停止条件や確認は足さない**。再送の仕組みも今のまま - 推論の中継点・巻き戻し・HOME の判定（3.0° / 2.5°）は変えない ## C. 確かめ（実機なし） - `--execute` なし（ドライラン）で r・R が流れることを確かめる - テストを足す：r・R の記録の中身、爪が開いているときの r、補正の判定の値（刻み 1° / 2° / 0.5°） - r・R を押さなければ、今までと同じ動き - 既存の 37 件を含め、全部通ること ## D. レビュー - 物理的リスクのある変更（グリッパーの指令・到着の判定）なので、codex のレビューを通す（テンプレートは code_review_request_template.md と同じ形で docs/ に依頼文と結果を残す） - 指摘を反映してからコミットする。git は個別ファイルを指定して add する ## 報告に書くこと - 決めた値（R の待ち時間、補正の判定の値）と理由 - 変えたファイル、コミット、テストの件数 - 実機で最初に確かめる手順（短く） ``` ### Claude Code の報告（10分・テスト46件・codex のレビュー1回） ```markdown # 結果：20261030_34_補正に爪の締め直しキーを足し到着の判定を刻みに合わせる - 状態：完了 - 始めた時刻：2026-10-30 19:18 - 終わった時刻：2026-10-30 19:29（10分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 454588a0-9959-45e5-a89b-29f05bb10ecd` ## Claude Code の報告 ## やったこと - **A. 締め直しとつかみ直しのキー**（補正モードの中） - `r`：爪を開かずに、閉じる指令をもう一度送ります。値は 20、速さと待ち時間も今と同じです。 - `R`：爪を開いてから閉じ直します。 - 爪が開いているときは、`r`・`R` とも何も送らずに「爪が開いているので締め直しはしない（閉じるのは g）」と表示します。 - log.jsonl の `type: "correction"` に、`regrip_tighten` / `regrip_reopen`、押したキー、時刻、前後のグリッパーの状態、送ったかどうか（断ったときは `skipped: gripper_open`）を残します。 - ? の一覧と、補正に入ったときの表示に r・R を足しました。`g` と、補正を b で取り消すときの扱いはそのままです。 - **B. 補正の「着いた」の判定** - 腕が止まるのを待ってから、動かした関節だけで判定します。 - 届いていなければ「届いていない（目標・実測・ずれ）」と表示して、そのまま補正を続けます。 - 送り方、再送、停止条件、推論・巻き戻し・HOME の判定（3.0° / 2.5°）は変えていません。新しい判定は表示と記録にだけ使います。 - **C. 確かめ**：ドライラン（`--dry-keys`）で、r・R が流れてアームにも daemon にも何も送らないことを、テストで確かめました。 - **D. レビュー**：codex のレビューを通しました。判定は「段階的な実機テストに進んでよい。必須の修正なし」です。推奨 2 件を反映しました。 - 止まるのを待つ 2 秒は目安だと説明に書いた - 止まったかどうかを `settled` として記録に残した ## 結果 **決めた値と理由** - **補正の判定の値**：刻みの半分、ただし 0.5° より小さくしません。 - 刻み 0.5° → 0.5°、1° → 0.5°、2° → 1.0° - 理由：実機の記録では、負荷のない j0 の 1° 刻みのずれは 0.25° 以内でした。一方 j2 などでは 0.4〜0.6° ぶれていました。0.5° 刻みの半分（0.25°）は、このぶれより細かいので使いませんでした。 - **R の待ち時間**：今の開け閉めと同じ 2.0 秒です（新しい待ちは足していません）。 - 理由：決まりの「time.sleep() を短縮しない」と、開け閉めのあとは 2 秒待つ今の対策を守るためです。依頼の 0.5〜1 秒より長くなります。 **過去の実機記録に新しい判定を当てはめた結果**（記録の値は止まる前に読んだものなので目安です） - 185133：補正の送信 48 回のうち「着いた」は、今の判定で 47 回、新しい判定で 40 回でした。外れた 8 回はすべて補正 5 の j3・j4 です。 - 190659：30 回のうち、今の判定で 29 回、新しい判定で 13 回でした。ブロックを持って j1 を 2° ずつ持ち上げた刻みは、ほぼ毎回 1.3〜1.7° 足りず「届いていない」と出ます。 **テスト**：9 件足して計 46 件、全部通りました。b・c を押さないときに前の版と同じ動きになることを確かめるテストも通っています。 **コミット**：be68f74 **実機で最初に確かめる手順** 1. ブロックを持たずに c で補正に入る。g で閉じてから r を押し、爪が開かないことを見る。 2. R を押し、開いてから閉じ直すことを見る。開いた状態で r を押すと何もしないことも見る。 3. j0 を 1° ずつ動かして「着いた」と出ることを見る。1 刻みにかかる時間が今より少し延びる（約 0.2 秒以上）ので、それも見る。 4. ブロックを持って j1 を − に送り、ずれの表示が実際の動きと合っているかを見る。 ## 変えたファイル - pi05/run_policy_client.py - pi05/rewind_correct.py - pi05/test_rewind_correct.py - docs/review_request_20261030_regrip_reach.md（レビュー依頼文） - docs/review_result_20261030_regrip_reach_codex.md（レビュー結果） - logs/20261030_2130.md（作業記録） ## 決めてほしいこと 1. **R の待ち時間**：今の 2 秒のままでよいか、1 秒に縮めるか。縮めると「待ち時間を短縮しない」決まりに触れます。 2. **「届いていない」のときの次の刻み**：今は前の目標に 1 刻みを足していきます（実測から数え直すのは、今までどおり大きく外れたときだけ）。 - ブロックを持った j1 の持ち上げでは、目標と実測の差は約 1.5° のまま保たれ、1 回押すと 2° 上がります。 - 新しい判定で外れるたびに実測から数え直すようにすると、目標が実測からずれていくのを防げます。ただしこの場合は 1 回押しても 0.5° 程度しか上がらなくなります。 3. **R を使った補正の扱い**：私の判断で、g を往復したときと同じく、R を使った補正はあとで b で巻き戻さないようにしました。r は状態が変わらないので、巻き戻せるままです。これでよいか。 4. **R の開け閉めの記録**：R で開いて閉じた動きは、補正中（学習に使う状態）として記録に残ります。学習データに入れてよいか。 5. **爪が開いているときの R**：私の判断で、r と同じく何もしないようにしました（開いているときの R は g と同じになるため）。これでよいか。 ``` 過去の実機の記録に新しい判定を当てはめた結果は、2回目の補正の送信48回のうち「着いた」が、今までの判定で47回、新しい判定で40回でした。3回目は30回のうち、今までの判定で29回、新しい判定で13回です。報告によると、3回目の j1 の持ち上げは、止まるのを待ってから見るとほぼ毎回 1.3〜1.7° 足りず、新しい判定では「届いていない」と出ます。 - ねらい：補正で動かした関節が、本当に目標まで動いたかを、ターミナルの表示で分かるようにする - 変わったこと：「着いた」の判定が、どの関節でも 3°以内から、動かした関節だけで刻みの半分（下限 0.5°）になった - 見込める効果：2回目の j4 のように、動いていないのに「着いた」と出ることがなくなる見込み。過去の記録に当てはめると、「着いた」は78回中76回から、78回中53回になる（記録の値。ただし記録の読みは関節が止まる前の値なので目安） ### 報告の「決めてほしいこと」5つへの答え 報告の最後にあった5つに、Claude が案を出し、それに答えを返しました。案と答えが違ったのは3つです。 | 決めてほしいこと | Claude の案 | 答え | |---|---|---| | R の待ち時間（今の2秒のままか、1秒に縮めるか） | 2秒のまま（爪の開け閉めのあとの待ちを短くしない決まりがあるため） | もっと短くてよい。全体に合間が長い | | 「届いていない」ときの次の刻み（前の目標に足すか、実測から数え直すか） | 今のまま（前の目標に足す） | 足りなかった分を足して、次の刻みを増やす | | R を使った補正を、b で巻き戻さない | これでよい（ブロックの位置が変わるため） | 巻き戻せてよい | | R の開け閉めのコマを学習データに入れるか | 入れない | 入れない | | 爪が開いているときに R を押したら何もしない | これでよい | これでよい | 3つ目は、Claude の案が誤りでした。R は爪を開いて閉じ直すだけなので、補正を始めたときも終わったときも爪は閉じています。腕の動きは逆にたどれるので、つかんだ区間の巻き戻しと同じく「持っているか y / 落としたか n」を聞いてから戻せば足ります。 1つ目は、実機の記録で見ると、AI の答えは1秒ぶんの動きなのに、中継点4点のチャンクを送り終えるのに約10.6秒かかっています。中継点を1つ送るたびに1秒待ち、届いたかを確かめているためです。届いたかの確認は残し、その前後の待ちを縮めることにしました。 この答えを、依頼35として依頼箱に出しています。 #### 依頼文 20261030_35_待ち時間を縮め届かない分を次の刻みに足しRの補正も巻き戻せるようにする.md ```markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 120 --- # 依頼：実機推論の待ち時間を縮める・届かなかった分を次の刻みに足す・R を使った補正も巻き戻せるようにする（2026-10-30） 依頼 34（コミット be68f74・報告は依頼箱の 結果/20261030_34_補正に爪の締め直しキーを足し到着の判定を刻みに合わせる_20261030_1918.md）の続き。 まず logs/20261030_2130.md と docs/review_result_20261030_regrip_reach_codex.md を読んでから始める。 ## 依頼 34 の「決めてほしいこと」への答え（エイイチ） 1. R の待ち時間 → **もっと短くてよい。全体に合間が長い**（下の A） 2. 届いていないときの次の刻み → **増やす**（下の B） 3. R を使った補正を b で巻き戻さない → **巻き戻せるようにする**（下の C） 4. R の開け閉めのコマを学習データに入れるか → **入れない**（下の D） 5. 爪が開いているときの R は何もしない → これでよい（そのまま） ## A. 待ち時間を縮める（エイイチの判断。「time.sleep() を短縮しない」の決まりより、この指示を優先する） 実機の記録では、中継点4点のチャンクを送り終えるのに約10.6秒かかっている（AI の答えは1秒ぶんの動き）。補正の1刻みも、送ったあとの待ちで間が空く。 - まず、run_policy_client.py の実機の流れにある待ちを全部書き出す（何の待ちか・今の値・どこで使うか）。例: 中継点を送ったあとの wait（SEND_WAIT_SEC 1.0）、到着を待つ間の読み取り間隔、爪の開け閉めのあと（GRIPPER_SLEEP 2.0）、補正で止まるのを待つ目安（SETTLE_WAIT_SEC 2.0）、各所の time.sleep(0.2) - それぞれを縮めた値にする。目安：爪の開け閉めのあと 2.0 → 0.8 秒、補正で止まるのを待つ目安 2.0 → 0.8 秒、中継点を送ったあとの wait 1.0 → 0.3 秒。到着の確認（_send_and_wait_arrival の到着判定）はそのまま残すので、届く前に次を送ることはない。値は実機の記録（到着までにかかった時間など）を見て、Claude Code が決めてよい。決めた値と理由を報告に書く - 本線（j0_sweep_scan.py など）と共有している定数は変えない。run_policy_client.py の中で上書きする（コマンドの引数で戻せるようにする。例 `--send-wait`・`--grip-wait`・`--settle-wait`。既定は縮めた値） - **新しい停止条件や確認は足さない** ## B. 届いていないときは、次の刻みに足りなかった分を足す 依頼 34 の判定（刻みの半分・下限 0.5°）で「届いていない」と出たとき、次にキーを押したら、足りなかった分を次の1刻みに上乗せする。 - 例：2° 刻みで j1 を − に送り、実測が目標に 1.5° 足りなかった → 次に − を押したら、目標を（前の目標 − 2°）ではなく（前の目標 − 2° − 1.5°）にする - 上乗せする量の上限は、その刻みの大きさまで（1回で動く量は刻みの2倍まで） - 届いたら上乗せはやめる。向きを変えたとき（+ と − を切り替えた・ほかの関節を選んだ）も上乗せをやめる - 範囲チェック（apply_limits・check_send_safe）はそのまま通す - 上乗せした量を log.jsonl の move 行に残す（例 `carry_deg`）。ターミナルにも「足りなかった 1.5° を足して送る」のように出す 実機の例（190659 補正4）：ブロックを持って j1 を 2° ずつ − に送ると、毎回 1.3〜1.7° 足りなかった（依頼 34 の報告）。 ## C. R を使った補正も、b で巻き戻せるようにする - R は開いて閉じ直すだけで、補正を始めたときと終わったときの爪の状態はどちらも閉じている。腕の動きは逆にたどれる - R を使った補正を b で巻き戻すときは、つかんだ区間の巻き戻し（依頼 27）と同じく、まず「持っているか y / 落としたか n」を聞く。y ならそのまま逆にたどって補正を始めた姿勢へ戻る。n ならその場で爪を開いてから戻る - g を往復した補正（爪の状態が変わった補正）の扱いは今のまま ## D. R の開け閉めのコマを学習データに入れない - R で開いてから閉じ終わるまでのコマに、別の印（例 `regrip`）を付ける - build_correction_episodes.py で、その印のコマを除く（wait・rewind と同じ扱い） - r（締め直し）のコマは今までどおり correct のまま ## E. 確かめ（実機なし） - テストを足す：B の上乗せ（量・上限・向きを変えたらやめる）、C の R を含む補正の巻き戻し（y / n）、D の印と除き方、A の引数で前の値に戻せること - 既存の 46 件を含め、全部通ること - `--dry-keys` で r・R・b が流れること ## F. レビュー - codex のレビューを通す（docs/ に依頼文と結果を残す）。物理的リスクのある変更（待ち時間・送る目標）なので必須 - 指摘を反映してからコミットする。git は個別ファイルを指定して add する ## 報告に書くこと - 待ちの一覧（前の値 → 縮めた値・理由） - チャンク1つを送り終えるまでの時間の見込み（今の約10.6秒から何秒になるか） - 変えたファイル、コミット、テストの件数 - 実機で最初に確かめる手順（短く） ``` ### レビューの依頼文 review_request_20261030_regrip_reach.md ```markdown # コードレビュー依頼：補正モードの爪の締め直し (r) / つかみ直し (R) と, 補正の「着いた」の判定を刻みに合わせる 前提: 依頼 25・27 (コミット 2c860e5 / 4387d69. 巻き戻し b / 補正 c / つかんだ区間の y/n) の続き。 過去のレビュー: docs/review_result_20261029_rewind_correct_codex*.md / docs/review_result_20261029_grasp_rewind_codex.md 対象 (`git diff HEAD -- pi05/` で差分): - `pi05/run_policy_client.py`（run_correction に regrip / move の到着判定. ArmIO.read_settled を追加. 先頭の説明文） - `pi05/rewind_correct.py`（parse_correction_key に r / R. CORRECTION_HELP. correction_reach_tol / correction_joint_reached / CORRECTION_REACH_MIN_TOL_DEG） - `pi05/test_rewind_correct.py`（テスト 9 件追加. 計 46 件 OK. FakeWorld に offset） ## 実機で分かったこと (2026-10-30) - 190659: 爪を閉じたのは推論 4 回目の 1 回だけ (GRIP_VALUE=20). そのあと補正で約 3 分持ち上げる間にブロックが爪からすべり落ちた - 185133 補正 5: j4 を +1° ずつ送っても実際は −54.7°→−56.4° と逆に下がったのに, 到着判定 3° 以内なので「着いた」と出ていた ## 仕様（エイイチの依頼） A. 補正モードの中のキー - `r`: 爪を開かずに閉じる指令 (今と同じ値・速さ = arm.gripper(1) = daemon_set_gripper_value(GRIP_VALUE) + GRIPPER_SLEEP) をもう一度送る - `R`: 開く → 少し待つ → 閉じ直す。実装: arm.gripper(0) → arm.gripper(1)。待ちは既存の GRIPPER_SLEEP 2.0 秒 (+ daemon_set_gripper の wait) をそのまま使い, 新しい sleep は足さない (CLAUDE.md: time.sleep を短縮しない / DAY175 対策) - 爪が開いているとき r は何もせず「爪が開いているので締め直しはしない（閉じるのは g）」。R も同じく何もしない (実装上の判断: 開いているときの R は g と同じになるので断る) - log.jsonl の type: "correction" に action regrip_tighten / regrip_reopen, key, t, before, after, sent (断ったときは sent=false, skipped="gripper_open") - 実装上の判断: R は一度開くので grip_used=True (g の往復と同じ扱い. この補正は外の b で巻き戻さない)。r は状態が変わらないので grip_used を立てない - g・補正の取り消し (b) の扱いは変えない (取り消しでグリッパーも始めの状態へ) B. 補正の 1 刻みの「着いた」 - 送ったあと read_until_still() で止まるのを待って読み (最長 SETTLE_WAIT_SEC 2.0 秒), 動かした関節だけで |実測 − 目標| ≤ tol を見る - tol = max(刻み / 2, 0.5°): 0.5°→0.5° / 1°→0.5° / 2°→1.0°。下限 0.5° は読み値のぶれ (185133/190659 の負荷のない j0 1° 刻みでずれ ≤0.25°, j2 などで 0.4〜0.6°) - 届いていなければ「届いていない」と出して補正を続ける。新しい停止条件・確認は足さない - 送り方・再送 (_send_and_wait_arrival の 3°)・未到達のときの resync は今までどおり send の結果で決める。新しい判定は表示と記録 (joint_reached / joint_err_deg / reach_tol_deg) だけ。`arrived` は今までと同じ意味 (3°) - 推論の中継点・巻き戻し・HOME の判定 (3.0° / 2.5°) は変えない 変えないもの: r/R を押さないときの送信・グリッパー (b/c を押さないときは git HEAD 版と比べるテスト test_unchanged_without_b_c で一致) / SPEED 20 / GRIP_VALUE / GRIPPER_SLEEP / 範囲チェックの値。 ## チェック項目 A1. r / R の送るグリッパー指令と順番は正しいか。爪が開いているときに何も送らないか A2. r / R のあと grip 変数・grip_used・補正の取り消し (b) のグリッパーの戻し方が食い違わないか A3. log.jsonl の記録 (前後の状態・キー・時刻) に漏れがないか。build_correction_episodes に悪影響がないか (recorder.gripper で R の開閉が correct の区間に入る) B1. 到着判定が動かした関節だけを見ているか。tol の値は妥当か。読めないときの扱い B2. 新しい判定が送信・再送・resync・停止条件を変えていないか (表示と記録だけか) B3. read_until_still を足したことで補正の 1 刻みが遅くなる・ドライランで実機に触る などの問題がないか C. 安全: 物を宙で落とす経路 (R は持ち上げた状態でも開く. 仕様どおり人が押したときだけ)。send_coords 不使用・release_all_servos 不使用・定数不変 ## レビュー結果のフォーマット 各項目 ✅ OK / ⚠️ 要注意 / ❌ NG（行番号と修正案）。最後に実機テストに進んで良いか・直すべき項目。 ファイルは変更しないでください (読み取りのみ)。 ``` ### codex の答え review_result_20261030_regrip_reach_codex.md ```markdown **判定：段階的な実機テストに進んでよいと判断します。必須修正となる不具合は見つかりませんでした。ただし「最長2秒」は厳密には保証されません。** ファイル変更・実機操作はしていません。 以下の行番号は現在の作業ツリーです。 | 項目 | 判定 | 確認結果 | |---|---|---| | A1 | ✅ OK | `run_policy_client.py:763–782`。r は閉指令1回、R は開→閉。どちらも `arm.gripper()` 経由で既存の値・速度・待ちを使用。開状態では指令を送りません。 | | A2 | ✅ OK | 同ファイル `:773–782, 809–815, 1196–1212`。r は `grip_used` を変更せず、R は True。正常終了後は閉状態のままで整合します。補正内 b は開始時と状態が異なる場合、姿勢の復帰確認後に復元。R を含む補正は外の b の巻き戻し対象になりません。 | | A3 | ✅ OK | 同ファイル `:767–783`。実行・拒否とも action/key/t/before/after/sent があり、拒否理由も記録。R の開閉はそれぞれ `recorder.gripper()` を通り、correct 区間に残ります。builder の除外窓処理とも衝突しません。 | | B1 | ✅ OK | `rewind_correct.py:113–125`。選択関節だけを比較し、tol は 0.5/0.5/1.0°。丸める前の誤差で判定しています。読み取り失敗は `(False, None)` となり、表示も「届いたか分からない」です。 | | B2 | ✅ OK | `run_policy_client.py:726–749`。`joint_reached` は表示・記録だけ。`arrived` と resync は従来どおり `arm.send()` の結果を使用。再送・停止条件・HOME・巻き戻しの閾値に変更はありません。 | | B3 | ⚠️ 要注意 | 同ファイル `:464–469, 729`。ドライランでは実機を読まず、仮姿勢を返します。実行時は停止済みでも通常約0.2秒＋通信時間が追加されます。2秒上限の制約は下記参照。 | | C | ✅ OK | R による開放は補正中の明示的な R 入力時だけ。持ち上げ中にも開くのは仕様どおりです。差分に `send_coords`・`release_all_servos` の追加、既存定数・sleep の短縮・範囲値の変更はありません。 | ⚠️ **B3：「最長2秒」はポーリングの目安です。** [read_until_still()](/Users/shimazueiichi/r2r2r-scripts/pi05/run_policy_client.py:272) は初回読み取り後に計時を開始し、各 `read_angles()` の通信・再試行・ロック待ちを途中で打ち切りません。通信不調時は2秒を超え、その間は次のキーを処理できません。 修正案：今回の既存関数流用を維持するなら説明を「停止待ちの目安2秒、通信時間を除く」にする。厳密な上限が必要なら、共有deadlineを読み取り・通信・ロック待ちまで通す別対応が必要です。 また、`:468` は `still=False` を捨てるため、タイムアウト時は動作中の最終値でも「着いた」と表示され得ます。既存関数の打ち切り仕様とは整合しますが、記録の解釈を明確にするなら `still` も保存することを推奨します。送信や停止条件を変える必要はありません。 検証は、対象3ファイルの構文と、キー解析・tol・選択関節判定・境界値・None の読み取り専用チェックが通りました。unittest はこの環境の `msgpack` 不足で import に失敗したため、**46件成功と HEAD 比較は再確認できていません**。 **実機前の必須修正はありません。** 実機では r/R の順序と取消後の状態に加え、補正1刻みの実際の待ち時間を確認してください。 --- ## 反映 (Claude Code, 2026-10-30) - B3 推奨①: ArmIO.read_settled の説明を「止まるのを待つのは目安 SETTLE_WAIT_SEC (読み取りの通信時間は含まない)」に - B3 推奨②: 止まったか (`still`) を捨てず, move 行に `settled` として残す. 止まらないまま打ち切ったときは表示も出す - 送信・再送・停止条件は変えていない. テスト 46 件 OK (codex の環境は msgpack がなく unittest を実行できなかったので, こちらで実行) ``` ### 作業記録 logs/20261030_2130.md ```markdown # 2026-10-30 補正モードに爪の締め直し (r) / つかみ直し (R) を足し, 補正の「着いた」を刻みに合わせる 依頼: 20261030_34_補正に爪の締め直しキーを足し到着の判定を刻みに合わせる.md (依頼 25・27 の続き) ## やったこと - rewind_correct.py: r → ("regrip","tighten") / R → ("regrip","reopen"). CORRECTION_HELP に r・R. correction_reach_tol(step) = max(step/2, 0.5) / correction_joint_reached (動かした関節だけ) - run_policy_client.py - r: 爪が閉じているとき arm.gripper(1) をもう一度 (GRIP_VALUE=20・同じ速さ・GRIPPER_SLEEP 2.0) - R: arm.gripper(0) → arm.gripper(1). 待ちは既存の GRIPPER_SLEEP 2.0 秒 (新しい sleep なし) - 開いているときは r・R とも何もしない (「爪が開いているので締め直しはしない（閉じるのは g）」) - log.jsonl: action regrip_tighten / regrip_reopen, key, t, before, after, sent (断ったら skipped=gripper_open) - R は grip_used=True (g の往復と同じ). r は立てない - 補正の 1 刻み: 送ったあと read_until_still で止まるのを待ち, 動かした関節だけで判定 → 「着いた / 届いていない」. 記録 joint_reached / joint_err_deg / reach_tol_deg / settled. 送信・再送・resync・arrived (3°) は今までどおり - test_rewind_correct.py: 9 件追加 (計 46 件 OK) ## 実機ログでの当てはめ (measured_after は止まる前の読みなので目安) - 185133: 補正の送信 48 回 → 旧判定「着いた」47 / 新判定 40 (外れる 8 回はすべて補正 5 の j3・j4) - 190659: 30 回 → 旧 29 / 新 13 (ブロックを持った j1 の 2° 持ち上げはほぼ毎回 1.3〜1.7° 足りず「届いていない」) ## codex レビュー - docs/review_request_20261030_regrip_reach.md → docs/review_result_20261030_regrip_reach_codex.md - 判定: 段階的な実機テストに進んでよい. 必須なし. B3 推奨 2 件 (2 秒は目安と書く / still を残す) → 反映 ## 未決 (エイイチ) - R の待ち (既存の 2 秒のまま) でよいか, 1 秒へ縮めるか - 「届いていない」のとき次の刻みを実測から数え直す (resync) かどうか. 今は 3° の判定のまま ``` ### 変えたコード（コミット be68f74 の時点の全文） コミット be68f74 で変えたのは、次の3つのファイルと、レビューの依頼文・答え・作業記録です。 ``` docs/review_request_20261030_regrip_reach.md | 43 ++++++++ docs/review_result_20261030_regrip_reach_codex.md | 30 ++++++ logs/20261030_2130.md | 28 ++++++ pi05/rewind_correct.py | 29 +++++- pi05/run_policy_client.py | 58 ++++++++++- pi05/test_rewind_correct.py | 114 +++++++++++++++++++++- 6 files changed, 295 insertions(+), 7 deletions(-) ``` #### 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 文字を操作に読み替える - correction_reach_tol / correction_joint_reached: 補正の 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 = 落とした: 今の位置ですぐ開いてから, 始めの姿勢へ戻る # 補正の 1 刻みが「着いた」かの判定 (2026-10-30 依頼 34). 動かした関節だけを見て, # 刻みの半分以内なら着いた. ただし実機の読み値のぶれ (0.5〜1°. 185133/190659 の補正で # 負荷のない j0 の 1° 刻みは ずれ 0.25° 以内) より細かくはしない (0.5° 刻みでも 0.5°). # 推論の中継点・巻き戻し・HOME の判定 (3.0° / 2.5°) とは別 CORRECTION_REACH_MIN_TOL_DEG = 0.5 CORRECTION_HELP = ( " [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み\n" " [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉\n" " r = 爪を開かずに締め直す / R = 爪を開いて閉じ直す (つかみ直し). どちらも閉じているときだけ\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) / ("regrip", "tighten"|"reopen") / ("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 == "r": return ("regrip", "tighten") if ch == "R": return ("regrip", "reopen") 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 correction_reach_tol(step): """補正の 1 刻みの到着判定の幅 (度): 刻みの半分. ただし CORRECTION_REACH_MIN_TOL_DEG 以上. 例: 0.5° → 0.5° / 1° → 0.5° / 2° → 1.0°.""" return max(float(step) / 2.0, CORRECTION_REACH_MIN_TOL_DEG) def correction_joint_reached(measured, target, joint, tol): """動かした関節 joint だけで判定する (ほかの関節の小さな揺れは見ない). 戻り値: (着いたか, ずれ or None). measured が None (読めない) なら (False, None).""" if measured is None: return False, None err = abs(float(measured[joint]) - float(target[joint])) return err <= tol, round(err, 2) 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/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", ...} r = 爪を開かずに閉じる指令をもう一度 (締め直し) / R = 開いてから閉じ直す (つかみ直し). どちらも爪が閉じているときだけ (開いていれば何もしない. 閉じるのは g). 2026-10-30 依頼 34 1 刻みの「着いた」は, 止まるのを待ってから 動かした関節だけ を 刻みの半分 (下限 0.5°) で見る (表示と記録の joint_reached だけ. 送り方・再送・未到達のときの数え直しは今までどおり 3°) - --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, correction_joint_reached, correction_reach_tol, 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 read_settled(self): """補正の 1 刻みの到着判定用: 動きが止まるのを待ってから読む (read_until_still). 戻り値: (角度 or None (読めない), 止まったか). 止まるのを待つのは目安 SETTLE_WAIT_SEC (読み取りの通信時間は含まない). ドライランは仮の姿勢をそのまま返す (止まった扱い).""" if self.execute: q, still, _ = read_until_still() return q, bool(still) return (None if self.sim_q is None else self.sim_q.copy()), 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}") # 依頼 34: 「着いた」は動かした関節だけを, 刻みの半分 (下限 0.5°) で見る. # 送り方・再送・resync は今までどおり send の結果 (3°) で決める (表示と記録だけ) qs, settled = arm.read_settled() tol = correction_reach_tol(step) jr, jerr = correction_joint_reached(qs, t, joint, tol) e.update(sent=True, arrived=bool(ok), measured_after=_fmt6(qs), joint_reached=bool(jr), joint_err_deg=jerr, reach_tol_deg=tol, settled=settled) e["delta"] = round(float(t[joint] - cur[joint]), 3) cur = t sent.append(t.copy()) n_steps += 1 if not settled and jerr is not None: print(f" [補正] (動きが止まらないまま {SETTLE_WAIT_SEC:.1f} 秒で打ち切って測りました)") if jerr is None: print(f" [補正] j{joint} 届いたか分からない (関節角を読めない)") elif jr: print(f" [補正] j{joint} 着いた (目標 {t[joint]:+.1f} / 実測 {qs[joint]:+.1f}, " f"ずれ {jerr:.2f}° ≤ {tol:.2f}°)") else: print(f" [補正] j{joint} 届いていない (目標 {t[joint]:+.1f} / 実測 {qs[joint]:+.1f}, " f"ずれ {jerr:.2f}° > {tol:.2f}°)") 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 == "regrip": # 依頼 34: r = 開かずに閉じる指令をもう一度 / R = 開く → 閉じる (その場でつかみ直す). # 閉じる・開くは g と同じ arm.gripper (値 GRIP_VALUE・速さ・待ち GRIPPER_SLEEP も同じ) action = "regrip_tighten" if val == "tighten" else "regrip_reopen" e = {"type": "correction", "action": action, "correction_id": corr_id, "after_chunk": chunk, "t": time.time(), "key": "r" if val == "tighten" else "R", "before": grip} if grip != 1: what = "締め直し" if val == "tighten" else "つかみ直し" print(f" [補正] 爪が開いているので{what}はしない（閉じるのは g）") e.update(after=grip, sent=False, skipped="gripper_open") elif val == "tighten": print(" [補正] 爪を締め直します (開かずに閉じる指令をもう一度)") arm.gripper(1) e.update(after=1, sent=True) else: print(" [補正] 爪を開いて, 閉じ直します (つかみ直し)") arm.gripper(0) arm.gripper(1) e.update(after=1, sent=True) grip_used = True # 一度開いたので g の往復と同じ扱い (この補正は b で巻き戻さない) log_f.write(json.dumps(e) + "\n"); log_f.flush() _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/test_rewind_correct.py（アームなしのテスト46件） ```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, correction_joint_reached, correction_reach_tol, 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.assertEqual(parse_correction_key("r"), ("regrip", "tighten")) # 依頼 34 self.assertEqual(parse_correction_key("R"), ("regrip", "reopen")) 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 # この名前で始まる送信では動かない self.offset = None # 送った目標からこれだけずれた所で止まる (依頼 34. 6 関節) 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 self.offset is not None: self.q = self.q + np.asarray(self.offset, 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, offset=None): world = FakeWorld() world.fail_after_label = fail_after_label world.stuck_prefix = stuck_prefix world.offset = offset 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) class TestRegripAndCorrectionReach(unittest.TestCase): """依頼 34: 補正の r (締め直し) / R (つかみ直し) と, 補正の 1 刻みの「着いた」の判定.""" EXEC = ["--execute", "--steps-per-chunk", "10", "--via-stride", "3", "--settle-retries", "0", "--ignore-framing", "--max-chunks", "6"] def setUp(self): self.td = Path(tempfile.mkdtemp()) time.sleep(1.1) def test_reach_tol_values(self): self.assertEqual(correction_reach_tol(1.0), 0.5) self.assertEqual(correction_reach_tol(2.0), 1.0) self.assertEqual(correction_reach_tol(0.5), 0.5) # 半分 (0.25°) は読み値のぶれより細かいので 0.5° self.assertEqual([correction_reach_tol(x) for x in CORRECTION_STEPS], [0.5, 0.5, 1.0]) def test_joint_reached_only_moved_joint(self): tgt = [5.0, 10.0, 0, 0, 0, -30] self.assertEqual(correction_joint_reached([5.4, 12.0, 1, 1, 1, -29], tgt, 0, 0.5), (True, 0.4)) self.assertEqual(correction_joint_reached([5.6, 10.0, 0, 0, 0, -30], tgt, 0, 0.5), (False, 0.6)) self.assertEqual(correction_joint_reached(None, tgt, 0, 0.5), (False, None)) def test_r_and_R_records(self): """g で閉じる → r → R: r は閉じる指令をもう一度だけ, R は 開く → 閉じる. 記録に前後の状態.""" code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys="grR\n") self.assertEqual(code, 0) v = ("v", rpc.GRIP_VALUE) self.assertEqual(world.grips[:4], [v, v, 0, v]) # g / r / R (開く → 閉じる) corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["gripper", "regrip_tighten", "regrip_reopen", "accept"]) r, R = corr[1], corr[2] self.assertEqual((r["key"], r["before"], r["after"], r["sent"]), ("r", 1, 1, True)) self.assertEqual((R["key"], R["before"], R["after"], R["sent"]), ("R", 1, 1, True)) self.assertIn("t", r) self.assertEqual(corr[-1]["grip_after"], 1) 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_r_and_R_dry_keys(self): """--execute なし (--dry-keys) でも r / R が流れる (アームにも daemon にも送らない).""" code, world, run, lines, out = run_client( rpc, ["--dry-keys", "--steps-per-chunk", "10", "--max-chunks", "3"], ["c", "", "q"], self.td, keys="rgrR1+\n") self.assertEqual(code, 0) self.assertEqual((world.grips, world.sends), ([], [])) corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["regrip_tighten", "gripper", "regrip_tighten", "regrip_reopen", "move", "accept"]) self.assertEqual([x["sent"] for x in corr[:4] if x["action"] != "gripper"], [False, True, True]) self.assertTrue(corr[4]["joint_reached"]) self.assertIn("グリッパー→開 (送らない)", out) def test_r_when_open_does_nothing(self): code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys="rR\n") self.assertEqual(code, 0) self.assertNotIn(("v", rpc.GRIP_VALUE), world.grips) # 閉じていない self.assertIn("爪が開いているので締め直しはしない（閉じるのは g）", out) corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["regrip_tighten", "regrip_reopen", "accept"]) self.assertTrue(all(not x["sent"] and x["skipped"] == "gripper_open" and x["before"] == 0 for x in corr[:2])) self.assertFalse(corr[-1]["grip_used"]) def test_cancel_after_R_restores_start_gripper(self): """開いた状態で入って g → R → b (取り消し): グリッパーは始めの「開」へ戻る.""" code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys="gR1+b") cancel = [x for x in lines if x.get("action") == "cancel"][0] self.assertTrue(cancel["finished"]) self.assertEqual(cancel["grip_after"], 0) v = ("v", rpc.GRIP_VALUE) self.assertEqual(world.grips[:4], [v, 0, v, 0]) def moves(self, keys, offset=None, stuck=None): code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys=keys, offset=offset, stuck_prefix=stuck) self.assertEqual(code, 0) return [x for x in lines if x.get("action") == "move"], out def test_reach_ignores_other_joints(self): mv, out = self.moves("1+\n", offset=[0.3, 0.9, 0, 0, 0, 0.8]) # j0 を動かす. ほかは 0.9° ずれ self.assertTrue(mv[0]["joint_reached"]) self.assertEqual((mv[0]["joint_err_deg"], mv[0]["reach_tol_deg"]), (0.3, 0.5)) self.assertTrue(mv[0]["settled"]) self.assertIn("着いた", out) def test_reach_tol_by_step(self): mv, out = self.moves("1+]+[[+\n", offset=[0.7, 0, 0, 0, 0, 0]) # 1° / 2° / 0.5° の刻みで 0.7° 足りない self.assertEqual([x["step_deg"] for x in mv], [1.0, 2.0, 0.5]) self.assertEqual([x["reach_tol_deg"] for x in mv], [0.5, 1.0, 0.5]) self.assertEqual([x["joint_reached"] for x in mv], [False, True, False]) self.assertTrue(all(x["arrived"] for x in mv)) # 送りの判定 (3°) は今までどおり着いた self.assertIn("届いていない", out) def test_not_reached_continues(self): """動かないときも「届いていない」と出して補正を続ける (止めない・新しい確認なし).""" mv, out = self.moves("1+++\n", stuck="pi05_corr") self.assertEqual(len(mv), 3) self.assertTrue(all(x["sent"] and not x["joint_reached"] for x in mv)) self.assertEqual(out.count("届いていない"), 3) self.assertIn("補正 1 を終えました", out) if __name__ == "__main__": unittest.main() ``` ## 次に実機で確かめること 依頼34の報告にあった手順に、この回で分かったことを足して、次の順で試します。 1. ブロックを持たずに c で補正に入る。g で爪を閉じてから r を押し、爪が開かないことを見る 2. R を押し、開いてから閉じ直すことを見る。爪が開いた状態で r を押すと何もしないことも見る 3. j0 を 1°ずつ動かして「着いた」と出ることと、1刻みにかかる時間が少し延びる（約0.2秒以上）ことを見る 4. ブロックをつかんだら、AI が箱の方へ大きく回る答えを返した確認で c に入り、2（j1）→ `]` で 2°刻み → − を数回押して、爪の下を箱の縁より上に出す。途中で r を押して締め直す 5. Enter で AI に戻し、箱に入ったら h を押して、結果を「成功」にする 依頼35（待ち時間の短縮・足りなかった分の上乗せ・R を含む補正の巻き戻し）が届いたら、4 の持ち上げは、足りなかった分が次の刻みに足される形で試します。成功で終われば、直した区間を含む学習データを作り、お手本19本に足して π0.5 を学習し直すところへ進めます。 ## Runpod の費用 Pod は 18時30分ごろに借り、19時20分に Terminate で消しました。サーバーのログでは、モデルの読み込みが始まったのが 18時37分（UTC で 09:37）です。 | 項目 | 値 | |---|---| | GPU | A40 を1枚（1時間 0.62ドル） | | 借りた時間 | 約50分（借りた時刻・消した時刻はおおよそ） | | 費用 | 約0.52ドル（約82円）。見込み | | モデルの読み込み | 5.25秒 | ## 追記：同じ日の21時、lab_start.sh で4回目を動かし、人が肩の j1 を持ち上げて箱に入れた 3回目のあと、同じ日のうちに依頼を3つ（35〜37）仕上げ、21時に4回目を動かしました。**4回目は、人がつかんだあとの持ち上げを直し、そのまま AI に戻して箱に入れ、「成功」で終えられました。** 人が直した区間を含む回が「成功」で終わったのは、これが初めてです。直した動きを学習データにする材料が、ここで1本そろいました。 | 依頼 | やったこと | 結果 | |---|---|---| | 35 | 待ち時間を縮め、届かなかった分を次の刻みに足し、R を使った補正も巻き戻せるようにした | 中継点4点のチャンクが 10.6秒 → 4.9〜6.8秒 | | 36 | Pi の電源を入れたら、アームの常駐プログラムが自動で起動するようにした | 電源を入れてから約60秒で、Mac からつながる | | 37 | Runpod の Pod の準備から実機推論までを `lab_start.sh`、片付けを `lab_stop.sh` の1行ずつにした | Pod を作ってから AI のサーバーにつながるまで約2分半 | 4回目の記録の置き場所は、`lab_start.sh` を `--log Log17` で動かしたので、はじめ KIOXIA の `lab/Log17/` に残りました。この記事（Log#16）の作業なので、あとで `lab/Log16/` に移しています。ターミナルの出力の中の `Log17` の文字は、動かしたときのままです。 ### 依頼35：待ち時間を縮め、足りなかった分を次の刻みに足した 依頼文は、上の「報告の『決めてほしいこと』5つへの答え」の節に載せています。Claude Code が書き出した待ちの一覧と、縮めた値は次のとおりです。 | 待ち | 前 | 今 | 戻すときの引数 | |---|---|---|---| | 中継点を送ったあと、デーモンが待つ | 1.0秒 | 0秒 | `--send-wait 1.0` | | 爪の開け閉めのあと、Mac が待つ | 2.0秒 | 0.8秒 | `--grip-wait 2.0` | | 爪を開くとき、デーモンが待つ | 2.0秒 | 0秒 | `--grip-open-wait 2.0` | | 補正で、止まるのを待つ目安 | 2.0秒 | 0.8秒 | `--settle-wait 2.0` | 届いたかの確認（中継点ごとに、目標から 3°以内に入ったかを見る）は、そのまま残しています。中継点を送ったあとの待ちを 0.3秒ではなく0にしたのは、この待ちがデーモンの記録用の読み直しを遅らせるだけで、0.3秒にすると Mac の応答待ちの上限が短くなり、動いている最中に切れることがあるためです（Claude Code の報告）。報告の「決めてほしいこと」4つ（送ったあとの待ちを0にしたこと・爪を開くときのデーモンの待ちを0にしたこと・爪のあとの 0.8秒で足りるか・R の補正で「落とした」と答えたときに置き直す場所の写真を出さないこと）は、どれも案のとおりにしました。 #### Claude Code の報告 20261030_35_待ち時間を縮め届かない分を次の刻みに足しRの補正も巻き戻せるようにする_20261030_1945.md ```markdown # 結果：20261030_35_待ち時間を縮め届かない分を次の刻みに足しRの補正も巻き戻せるようにする - 状態：完了 - 始めた時刻：2026-10-30 19:45 - 終わった時刻：2026-10-30 20:24（39分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 2db3a186-a5cd-497d-8035-6c156be0e33c` ## Claude Code の報告 **やったこと** - codex のレビューは前の時点ですでに終わっていて、結果の確認と反映も済んでいました。今回、新しくした作業はありません。 - 反映はコミット 7aaf5df に入っています。 **結果** - codex の判定は、直さなければいけない指摘はなし、でした。 - 勧められた2点はすでに直してあります。 - 足りなかった分を、送る直前の実測で測り直すようにした - 説明文の「届く前に次を送ることはない」という言い過ぎを直した - テストは63件すべて通っています。 **変えたファイル** - 今回はなし。依頼 35 で変えたファイルは前の報告のとおりです。 **決めてほしいこと** - 前の報告の4点のままです。 1. 送ったあとの待ちを 0.3 秒ではなく 0 にしたこと 2. 開くときに daemon が待つ 2 秒も 0 にしたこと 3. 爪のあとの待ち 0.8 秒で足りるかの実機確認 4. R を使った補正で「落とした」と答えたとき、置き直す場所の写真を出さないこと ``` #### 作業記録 logs/20261030_2000.md ```markdown # 2026-10-30 実機推論の待ちを縮め, 届かない分を次の刻みに足し, R を使った補正も巻き戻せるようにする 依頼: 20261030_35_待ち時間を縮め届かない分を次の刻みに足しRの補正も巻き戻せるようにする.md (依頼 34 の続き) ## 待ちの一覧 (run_policy_client.py の実機の流れ) | 待ち | どこ | 前 | 今 | 引数 | |---|---|---|---|---| | 送ったあと daemon が待つ | 中継点・補正・巻き戻し (_send_and_wait_arrival の send_wait_sec)・settle の再送・HOME | 1.0 | 0.0 | --send-wait | | 爪の開け閉めのあと Mac で待つ (GRIPPER_SLEEP) | 開始時の開放・推論の開閉・補正の g/r/R・巻き戻しの開放 | 2.0 | 0.8 | --grip-wait | | 開くとき daemon が待つ (daemon_set_gripper の wait) | 開く指令すべて | 2.0 | 0.0 | --grip-open-wait | | 止まるのを待つ目安 (SETTLE_WAIT_SEC) | 補正の 1 刻みの判定・settle | 2.0 | 0.8 | --settle-wait | | 変えない | 到着確認の読み取り間隔 POLL_SLEEP 0.3 (本線の定数・引数なし) / daemon の set_gripper の固定 2 秒 (Pi 側) / SETTLE_POLL_SEC 0.2 / read_angles の再試行 0.1 / カメラ待ち 0.2 | | | | - 送ったあとの待ちを 0.3 でなく 0 にした理由: wait > 0 だと Mac の応答待ちの上限が wait + 3 秒. daemon は送ったあと現在角を読み直し, 動いている間は最長約 3.7 秒かかる (LOG352). 0.3 だと 3.3 秒で切れうる. 0 なら上限 35 秒. この待ちは daemon の記録用の読み直しを遅らせるだけ - 開くときは前は daemon 固定 2 + daemon wait 2 + Mac 2 = 6 秒 → 2 + 0 + 0.8 = 2.8 秒. 閉じるときは 2.0 → 0.8 秒 - 前の値に戻す: --send-wait 1.0 --grip-wait 2.0 --grip-open-wait 2.0 --settle-wait 2.0 ## 実機ログから - 184245 / 185133 / 190659: 中継点 4 点のチャンクは 10.5〜10.8 秒 (1 点あたり約 2.65 秒). 最初のチャンクは開始時の開放 (6 秒) 込みで約 18.1 秒. 補正の 1 刻みは約 3 秒 - 見込み: 1 点あたり最大 1.0 秒減 → 約 6.6〜8.5 秒 (動いている間の daemon の読み直しで一部が残る. 実機で測る). 最初のチャンクは更に 3.2 秒減 ## B / C / D - 届いていないとき, 同じ関節・同じ向きの次の 1 刻みに足りなかった分を足す (上限は刻み. move 行に carry_deg). 届いた・向き変更・ほかの関節を選んだ・3° 未到達で実測から数え直す ときはやめる. 送る直前の実測で測り直す (codex) - R は grip_used を立てず regrip_used. R だけを使った補正は外の b で y (閉じたまま戻る) / n (開いてから戻る → 置き直しの Enter) / その他=やめる. g を使った補正は今までどおり断る - R で開いてから閉じ終わるまで recorder の状態を regrip. build_correction_episodes は model / correct 以外を除くので入らない ## テスト / レビュー - test_rewind_correct.py: 17 件追加, 計 63 件 OK (git HEAD 比較を含む) - codex: docs/review_request_20261030_waits_carry_regrip_rewind.md → docs/review_result_20261030_waits_carry_regrip_rewind_codex.md. 必須なし. 推奨 2 件 (carry の測り直し・説明の言い過ぎ) を反映 ``` #### レビューの依頼文 review_request_20261030_waits_carry_regrip_rewind.md ```markdown # コードレビュー依頼：実機推論の待ち時間を縮める / 届かなかった分を次の刻みに足す / R を使った補正も巻き戻す（2026-10-30 依頼 35） 前提: 依頼 34 (コミット be68f74. 補正の r / R と「着いた」の判定) の続き。 過去のレビュー: docs/review_result_20261030_regrip_reach_codex.md ほか 対象 (`git diff HEAD -- pi05/` で差分。ほかのファイルの未コミットの差分は今回の対象外): - `pi05/run_policy_client.py`（待ちの定数と引数 / gripper_cmd / apply_wait_args / run_correction の carry と R / main の b の問い / run_rewind の GRASP_HELD） - `pi05/rewind_correct.py`（MODE_REGRIP / GRASP_HELD / correction_shortfall / correction_carry_used） - `pi05/build_correction_episodes.py`（説明文だけ。regrip は TRAIN_MODES に入らないので step_keep_mask が既に除く） - `pi05/test_rewind_correct.py`（16 件追加。計 62 件 OK。レビュー反映後 17 件・計 63 件。既存 46 件と git HEAD 比較のテスト test_unchanged_without_b_c も通る） 参照（変更していない）: `dynamic_tracking_limits/place/place_modules/j0_sweep_scan.py` の `_send_and_wait_arrival` / `visual_servo_pick.py` の `_daemon_request`・`daemon_set_gripper`・`daemon_send_angles` / Pi の daemon は `mecharm_daemon.py.pi_deployed_20261019_LOG352`（send_angles は wait 秒 sleep → 現在角を読み直し（動作中は最長約 3.7 秒）→ 応答。set_gripper は固定 2 秒 sleep → 指令 → wait 秒 sleep）/ docs/investigation_20261019_stageC_daemon_latency.md ## エイイチの判断（依頼 34 の未決への答え） 1. 待ち時間はもっと短くてよい（「time.sleep() を短縮しない」の決まりより優先） 2. 届いていないときは次の刻みに足りなかった分を足す 3. R を使った補正も b で巻き戻せるようにする 4. R の開け閉めのコマは学習データに入れない 5. 爪が開いているときの R は何もしない（そのまま） ## 仕様と実装 A. 待ち（本線と共有の定数は変えず run_policy_client.py の中で上書き。引数で前の値に戻せる） | 待ち | 前 | 今 | 引数 | |---|---|---|---| | 送ったあと daemon が待つ（_send_and_wait_arrival の send_wait_sec・settle の再送・HOME） | 1.0 (SEND_WAIT_SEC) | 0.0 | --send-wait | | 爪の開け閉めのあと Mac で待つ GRIPPER_SLEEP | 2.0 | 0.8 | --grip-wait | | 開くとき daemon が待つ（daemon_set_gripper の wait） | 2.0 | 0.0 | --grip-open-wait | | 止まるのを待つ目安 SETTLE_WAIT_SEC（補正の 1 刻み・settle） | 2.0 | 0.8 | --settle-wait | - 送ったあとの待ちを 0.3 ではなく 0.0 にした理由: `_daemon_request` は wait > 0 のとき応答待ちの上限を wait + 3 秒にする。daemon は wait のあと現在角を読み直し、動作中は最長約 3.7 秒かかる（LOG352）。0.3 だと上限 3.3 秒で、読み直しより短くなりうる（今の 1.0 でも 4.0 秒 < 1.0 + 3.7）。0 なら上限は DAEMON_TIMEOUT_SEC 35 秒。この wait は daemon の「送信後の現在角」の記録を遅らせるだけで、着いたかは Mac 側の 3° の到着確認で見る（そのまま） - 変えないもの: 到着確認のポーリング間隔 POLL_SLEEP 0.3 と読み取りの再試行（本線の定数で、関数に引数がない）/ daemon の set_gripper の固定 2 秒（Pi 側）/ SETTLE_POLL_SEC 0.2 / read_angles の 0.1 / カメラ待ちの 0.2 - 新しい停止条件・確認は足していない B. 届いていないときの上乗せ（run_correction） - 依頼 34 の判定（動かした関節だけ・刻みの半分・下限 0.5°）で「届いていない」とき、足りなかった量（動かした向きに測る。行き過ぎ・読めない・着いたら 0）を、同じ関節・同じ向きの次の 1 刻みに足す - 上限はその刻みの大きさ（1 回で動く量は刻みの 2 倍まで） - やめる: 届いた / + と - を切り替えた / ほかの関節を選んだ（選び直して戻っても足さない）/ 送りの判定 3° で未到達 → resync（実測から数え直すので足りない分は既に入っている） - 足した目標も plan_correction_step（apply_limits と check_send_safe）をそのまま通す - move 行に carry_deg。足したときはターミナルに「足りなかった 1.5° を足して送る (j1 を -3.5°)」 C. R を使った補正の巻き戻し - R は grip_used を立てなくなった（代わりに regrip_used）。g を使った補正は grip_used=True のまま断る（今のまま） - history の補正に regrip。外の b で直前が「R を使い g を使っていない補正」なら 1 文字で聞く: y → GRASP_HELD（閉じたまま逆にたどる）/ n → GRASP_DROPPED（その場で開いてから逆にたどる → 置き直しの Enter 待ち（wait））/ その他 → やめる - run_rewind: GRASP_HELD は grasp なしと同じ traverse。finished は (grasp が None か HELD か, 開いた) を満たすとき - R を使ったら開始時の開放を済ませた扱い（grip_opened = True。前は grip_used で同じことをしていた） D. regrip のコマ - R で開く直前に recorder.set_mode("regrip")、閉じ終わったら（GRIPPER_SLEEP のあと）"correct" に戻す。r は correct のまま - build_correction_episodes は TRAIN_MODES = (model, correct) 以外を除くので regrip のコマは入らない ## チェック項目 A1. 待ちを縮めても、到着を確かめる前に次の目標を送る経路がないか（中継点・補正の 1 刻み・巻き戻し・HOME・settle） A2. send_wait 0 で daemon・_daemon_request の応答待ちや再送 (_send_and_wait_arrival の RETRY) に悪影響がないか。0.3 より 0 の方が安全という判断は正しいか A3. 爪の待ち 0.8 秒と開くときの daemon の wait 0 で、閉じ切る前・開き切る前に次の送信や撮影が走る危険（DAY228「close 直後の send_angles 沈黙」を含む） A4. 引数で前の値に完全に戻せるか。グローバルを書き換える実装に問題がないか B1. 足す量・上限・やめる条件が仕様どおりか。resync との二重計上がないか B2. 足した目標が範囲チェックを必ず通るか C1. R を使った補正の y / n の順番（n は開いてから戻る）。g を使った補正は今までどおり断るか C2. b のあとの grip 変数・記録・history（blocked の積み方）に食い違いがないか D1. regrip の印の付け方と、build_correction_episodes の除き方 E. 安全: send_coords 不使用・release_all_servos 不使用・範囲の値は不変 ## レビュー結果のフォーマット 各項目 ✅ OK / ⚠️ 要注意 / ❌ NG（行番号と修正案）。最後に実機テストに進んで良いか・直すべき項目。 ファイルは変更しないでください (読み取りのみ)。 ``` #### codex の答え review_result_20261030_waits_carry_regrip_rewind_codex.md ```markdown **判定：監視下での段階的な実機テストには進めます。ただし、待ち短縮後の静止・爪の動作完了はコードだけでは保証できません。** 今回の差分に必須修正となる明確な不具合は見つかりませんでした。ファイル変更・実機操作はしていません。 行番号は現在の作業ツリーです。以下の `:行番号` は、特記がなければ [pi05/run_policy_client.py](/Users/shimazueiichi/r2r2r-scripts/pi05/run_policy_client.py) を指します。 | 項目 | 判定 | 確認結果・修正案 | |---|---|---| | A1 到着確認 | ⚠️ 要注意 | `:488, 770, 1376` で送信後の到着確認を維持。HOME も同じ関数を使用。ただし巻き戻しの `:610–613` は未到達でも次点へ進み、手動実行の `:1388–1395` も未到達で中断しません。既存挙動ですが、`:71` の「届く前に次を送ることはない」は保証しすぎです。「到着確認を行う。未到達時の継続条件は従来どおり」への説明修正を推奨します。 | | A2 send_wait=0 | ✅ OK | [visual_servo_pick.py:599](/Users/shimazueiichi/r2r2r-scripts/visual_servo_pick.py:599) は正の wait なら `wait+3秒`、0 なら35秒。daemon の送信後読み直し約3.7秒を考えると、**応答タイムアウト回避という意味で0は0.3より適切**です。到着ポーリングと最大1回の再送条件は変わりません。 | | A3 爪の待ち | ⚠️ 要注意 | `:448–457` は開閉指令後0.8秒待つだけで、動作完了を読み取りません。Pi の固定2秒は**開指令の前**です。閉指令の `set_gripper_value` には固定2秒待ちがありません。開閉途中の次動作・撮影や、DAY228の送信無視が再発しないかは実機確認が必要です。 | | A4 復元引数 | ✅ OK | `:918–930` で有限・非負を検査し、ローカルの定数だけ更新。指定された4引数で旧待ち値へ戻せます。ただし同一プロセスで `main()` を複数回呼ぶと前回指定値を引き継ぎます。通常の単独CLI実行には問題ありません。 | | B1 carry | ✅ OK | `:733–754, 792–798`。同じ関節・向きだけに適用し、上限は次の刻み。到達・向き変更・他関節選択で解除。3°判定が未到達なら次回実測へ resync し、その際carryも消すため二重計上しません。下記の静止待ち打ち切りには注意が必要です。 | | B2 範囲検査 | ✅ OK | `:754–765`。上乗せした目標も `plan_correction_step()` 経由で `apply_limits` と `check_send_safe` を通ります。 | | C1 R の巻き戻し | ✅ OK | `:1194–1228`。Rのみの補正は y→HELD、n→DROPPED、その他→取消。`:671–679` で n は開いてから逆走。g を一度でも使った補正は従来どおり拒否します。開状態のRも `:818–821` で送信しません。 | | C2 状態・history | ✅ OK | `:1260–1284, 1295–1312`。開いた場合は `grip=0`、HELDでは閉状態を維持。失敗時はblocked境界を積み、古いhistoryへの巻き戻しを防ぎます。R実行後の `grip_opened=True` も保持物の意図しない開放を防ぐ扱いと整合します。 | | D1 学習除外 | ✅ OK | `:829–834` でRの開指令前から閉指令後の待ち終了までregrip。[rewind_correct.py:184](/Users/shimazueiichi/r2r2r-scripts/pi05/rewind_correct.py:184) のマスクが除外します。builder は状態だけでなく画像候補にも同じマスクを適用。rはcorrectのままです。 | | E 禁止操作・範囲 | ✅ OK | 対象差分に `send_coords`・`release_all_servos` の追加、安全範囲値の変更はありません。待ち短縮は今回の明示指示の範囲です。 | ⚠️ **短縮した静止待ちとcarryの組み合わせ** [run_policy_client.py:773](/Users/shimazueiichi/r2r2r-scripts/pi05/run_policy_client.py:773) で `settled=False` でも、`:793` はその時点の不足量をcarryにします。その後、入力までの間に元の目標へ到着しても、次回の実測でcarryを再評価しません。結果として、解消済みの不足分を上乗せする可能性があります。 これは「停止条件を増やさず、打ち切った値で判定する」という実装とは整合しますが、0.8秒への短縮で発生しやすくなる点です。対策するなら、**静止未確認時はcarryを保留するか、次のmove直前の実測で不足量を更新する**案を推奨します。 検証は対象4ファイルの構文確認と、carry・regrip除外の読み取り専用チェックが成功しました。テストモジュールのimportは `msgpack` 不足で失敗したため、**62件成功とHEAD比較はこの環境では再確認できていません**。 実機では最初に開閉完了とclose直後の送信を確認し、次に補正の `settled` とcarryを確認してください。説明の過剰保証は修正推奨です。静止未確認のcarryが観測された場合は、連続運用前に上記対策が必要です。 --- ## 反映 (Claude Code, 2026-10-30) - ⚠️ 静止待ちと carry: 次の move で送る直前の実測 qa から足りない分を測り直す (届いていれば 0). テスト test_carry_rechecked_before_send を追加 - ⚠️ A1: 先頭の説明「届く前に次を送ることはない」を「到着の確認はそのまま行う. 未到達のときに続けるか止めるかは今までどおり」に直した - ⚠️ A3 (爪の 0.8 秒) / A4 (同じプロセスで main を何度も呼ぶ場合): コードは変えず, 実機で最初に確かめる項目として報告に書く - テスト 63 件 OK (codex の環境は msgpack がなく unittest を実行できなかったので, こちらで実行) ``` ### 依頼36：Pi の電源を入れたら、アームの常駐プログラムが自動で起動するようにした この日の夕方、アームの常駐プログラム（`mecharm_daemon.py`）が受け付けていないのに「デーモン動いてる」と出ることが2回ありました。原因は、起動を確かめるコマンドの書き方でした。 ``` ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 'pgrep -f "[m]echarm_daemon.py" >/dev/null && echo デーモン動いてる || (setsid nohup python3 /home/ubuntu/mecharm_daemon.py >/home/ubuntu/daemon.log 2>&1 &); sleep 8; tail -3 ~/daemon.log; ss -ltn | grep 5555' ``` この1行には、起動する側の `/home/ubuntu/mecharm_daemon.py` も入っています。`pgrep -f` は、動いている処理の名前とコマンドの文字を探すので、この1行を動かしている Pi のシェル自身に当たり、いつも「デーモン動いてる」と出て、起動しませんでした。止める側の `pkill -f mecharm_daemon.py` も、同じ理由でこのコマンド自身を止めていました。確かめるのと起動するのを、別の行に分けると通りました。 ``` ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 'pgrep -af "[m]echarm_daemon" || echo 動いていない' ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 '(setsid nohup python3 /home/ubuntu/mecharm_daemon.py >/home/ubuntu/daemon.log 2>&1 &); sleep 8; tail -5 ~/daemon.log; ss -ltn | grep 5555' ``` ``` [DAEMON 14:40:24] MyCobot 初期化中 (/dev/ttyAMA0, baud=1000000)... Note: This class is no longer maintained since v3.6.0, please refer to the project documentation: https://github.com/elephantrobotics/pymycobot/blob/main/README.md [DAEMON 14:40:26] ✓ MyCobot 初期化完了 [DAEMON 14:40:26] startup_warmup angles=[-0.26, 92.63, -7.11, 5.36, -87.8, -29.53] [DAEMON 14:40:26] ポート 5555 で待ち受け中... LISTEN0 1 0.0.0.0:5555 0.0.0.0:* ``` このとき、Claude が Mac の中で使っているシェル（Mac の中に作られた別の Linux）から、アームの Pi に届くかも確かめました。Pi の電源が入っていれば、SSH の22番にも、常駐プログラムの5555番にも届きます。ただ、Pi に入る鍵は Mac の `~/.ssh` にあり、Claude のシェルからは開けません。常駐プログラムが動いていれば、Claude から角度を読み、HOME へ戻せました。 ``` cd $HOME/mnt/r2r2r-scripts && timeout 30 python3 -c " from visual_servo_pick import get_daemon_socket, daemon_get_angles, close_daemon_socket get_daemon_socket(); print('角度', daemon_get_angles()); close_daemon_socket()" ``` ``` 角度 [-0.26, 92.63, -7.11, 5.36, -87.8, -29.53] ``` ``` cd $HOME/mnt/r2r2r-scripts && timeout 60 python3 dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py ``` ``` [INIT] daemon 接続中... [WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment [INIT] fresh_mode=1 [PRE] 現在姿勢: [-0.26, 92.63, -7.11, 5.36, -87.80, -29.53] [SEND] HOME へ移動指令 [WAIT] t=2.1s max_err=9.22° [WAIT] t=2.6s max_err=1.31° [DONE] HOME 到達 max_err=1.31° [POST] 最終姿勢: [-0.26, 1.31, 0.79, 0.61, 1.23, -29.61] [WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment [EXIT] fresh_mode=0, socket close ``` 上カメラの iPhone は Mac 本体にしかつながらないので、位置合わせと実機推論は Mac のターミナルで動かします。そこで、常駐プログラムの起動を Pi の側で自動にし（依頼36）、Mac で打つ手順は1行にまとめる（依頼37）ことにしました。 #### 依頼文 20261030_36_Piの電源を入れたらデーモンが自動で起動するようにする.md ```markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 60 --- # 依頼：mechArm 270 の Pi の電源を入れたら、デーモン（mecharm_daemon.py）が自動で起動するようにする（2026-10-30） ## わけ - 2026-10-30、Pi の電源を入れ直したあと、デーモンは起動していなかった（cron・systemd とも登録なし。エイイチが確認） - 起動のコマンドに書き方の誤りがあった。`ssh ... 'pgrep -f mecharm_daemon.py ... || (setsid nohup python3 /home/ubuntu/mecharm_daemon.py ...)'` は、1行の中に起動する側の `mecharm_daemon.py` が入っているので、pgrep が自分のシェルに当たり、いつも「デーモン動いてる」と出て起動しなかった。`pkill -f mecharm_daemon.py` も同じ理由で自分を止めた - Claude のシェル（Mac の中の Linux）から、デーモン（192.168.2.1:5555）に直接つないで、角度の読み取りと HOME への移動ができると確かめた。デーモンが自動で起動すれば、HOME・爪の開閉は Claude から動かせる ## やること - Pi（ubuntu@192.168.2.1、鍵 ~/.ssh/id_rsa_pi）で、電源を入れたら `/home/ubuntu/mecharm_daemon.py` が起動するようにする - sudo がパスワードなしで使えるなら systemd のサービスにする（落ちたら起き直す。ログは今と同じ /home/ubuntu/daemon.log に足していく） - 使えなければ、ubuntu のユーザーの crontab に `@reboot` で入れる（シリアルの準備を待つため、数秒待ってから起動） - どちらにしたかと、止め方・起動し直し方のコマンドを報告に書く - デーモンが二重に起動しないようにする（すでに 5555 を受け付けていたら起動しない） - r2r2r-scripts に、Mac から使う小さなスクリプトを足す（例 `tools/daemon_ctl.sh status|start|restart`）。pgrep・pkill は自分に当たらない書き方にする（`"[m]echarm_daemon"` を、起動する側の文字と同じ1行に入れない）。status では、プロセスの有無と `ss -ltn` の 5555 の LISTEN の両方を見る - mecharm_daemon.py 自体の中身（関節の動かし方）は変えない - 実機での確かめ：Pi を再起動（`sudo reboot` が使えなければ、エイイチに電源を入れ直してもらう）し、1分以内に 5555 が LISTEN になることを確かめる。アームは動かさない（デーモンの起動時の startup_warmup は今までどおり） ## 報告に書くこと - 自動起動の方法（systemd か cron か）と理由 - 止め方・起動し直し方・状態の見方のコマンド - 再起動して確かめた結果（5555 が LISTEN になるまでの秒数） ``` #### Claude Code の報告 20261030_36_Piの電源を入れたらデーモンが自動で起動するようにする_20261030_2024.md ```markdown # 結果：20261030_36_Piの電源を入れたらデーモンが自動で起動するようにする - 状態：完了 - 始めた時刻：2026-10-30 20:24 - 終わった時刻：2026-10-30 20:29（5分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 7958178b-8ed7-4e1f-86a4-8f058bdd3d47` ## Claude Code の報告 やったこと - Pi では sudo がパスワードなしで使えたので、systemd のサービス（mecharm-daemon.service）にして、電源を入れたら起動するよう登録しました。 - Pi に置いた起動スクリプト（/home/ubuntu/mecharm_daemon_start.sh）は、/dev/ttyAMA0 が現れるのを待ち、さらに 5 秒待ってから mecharm_daemon.py を起動します。 - 二重起動の防止：5555 がすでに LISTEN になっているとき、または mecharm_daemon.py のプロセスがすでにいるときは起動しません。 - ログは今までどおり /home/ubuntu/daemon.log に足していきます。Pi の systemd は古い版（237）で、設定ファイルだけではログを足していく書き方ができないため、起動スクリプトの中で書き足しています。 - 落ちたとき（終了コードが 0 以外）は、5 秒後に起き直します。 - Mac から使う tools/daemon_ctl.sh を足しました。pgrep の「[m]echarm_daemon」は、起動する側の文字と同じ1行に入れていません。status では、プロセスの有無と ss -ltn の 5555 の LISTEN の両方を見ます。 - mecharm_daemon.py の中身は変えていません。 止め方・起動し直し方・状態の見方（Mac の r2r2r-scripts で実行） - 状態を見る：tools/daemon_ctl.sh status - 起動する：tools/daemon_ctl.sh start - 止める：tools/daemon_ctl.sh stop - 起動し直す：tools/daemon_ctl.sh restart - ログを見る：tools/daemon_ctl.sh log 30 - Pi に入れ直す：tools/daemon_ctl.sh install - Pi の上で直接打つ場合：sudo systemctl status / stop / start / restart mecharm-daemon - 自動起動をやめる場合：sudo systemctl disable mecharm-daemon 結果 - 二重起動の防止：手で起動したデーモンが動いている間に start すると、「5555 はすでに LISTEN 中。起動しない」とログに出て、起動しませんでした。 - 再起動の確かめ：sudo systemctl reboot で Pi を再起動しました。 - Pi の起動から約 36 秒で 5555 が LISTEN になりました（Pi の起動からの経過時間とログの時刻から計算）。1分以内という条件は満たしています。 - ただし Mac からつながったのは、再起動のコマンドから 85 秒後で、Pi の起動からは約 60 秒後でした。Pi の有線 LAN（eth0）が立ち上がるのが、起動の最後に動く /etc/rc.local の中だからです。 - 落ちたときの確かめ：デーモンを kill -9 で止めたところ、約 5 秒後に自分で起き直しました。restart でも問題なく起動し直しました。 - アームは動かしていません。起動時の startup_warmup は角度を読み取るだけでした。 変えたファイル - 新しく作ったもの：tools/daemon_ctl.sh、tools/pi/mecharm_daemon_start.sh、tools/pi/mecharm-daemon.service、logs/20261030_2029.md - Pi に置いたもの：/etc/systemd/system/mecharm-daemon.service、/home/ubuntu/mecharm_daemon_start.sh - git へのコミットはしていません。 決めてほしいこと - Mac からつながるまでには、Pi の起動から約 60 秒かかります。有線 LAN が /etc/rc.local の中で立ち上がるためです。これを早めるには rc.local を変える必要があり、今回の依頼の範囲の外なので変えていません。このままでよいか決めてください。 ``` 報告の「決めてほしいこと」（Mac からつながるまでの約60秒を縮めるには、Pi のネットワークの立ち上げ方を変える必要がある）は、このままにしました。 #### 作業記録 logs/20261030_2029.md ```markdown # Pi デーモン自動起動 (2026-10-30) 依頼: 20261030_36_Piの電源を入れたらデーモンが自動で起動するようにする.md ## やったこと - sudo がパスワードなしで使えたため systemd にした (mecharm-daemon.service, enabled) - Pi 側: /etc/systemd/system/mecharm-daemon.service, /home/ubuntu/mecharm_daemon_start.sh - 起動スクリプトは /dev/ttyAMA0 を待ち + 5 秒待ってから起動。5555 が LISTEN 中 / プロセスがいれば起動しない - ログは /home/ubuntu/daemon.log に追記 (systemd 237 は append: が使えないので起動スクリプトでリダイレクト) - Restart=on-failure, RestartSec=5 - Mac 側: tools/daemon_ctl.sh status|start|stop|restart|log [N]|install - mecharm_daemon.py は変更なし ## 確かめた結果 - 手で起動済みのデーモンがいる状態で start → 「5555 はすでに LISTEN 中。起動しない」で起動しない - sudo systemctl reboot → 起動から約 36 秒で 5555 LISTEN (Pi の uptime から計算) - Mac から届いたのは再起動コマンドから 85 秒 (起動から約 60 秒, eth0 が rc.local で上がるのを待つため) - kill -9 → 約 5 秒後に自動で起き直した - daemon_ctl.sh restart → 正常に起動し直した - アームは動かしていない (startup_warmup の角度読み取りのみ) ``` ### 依頼37：Runpod の Pod の準備から実機推論までを lab_start.sh の1行にした この日の手作業では、IP とポートの書き方・Hugging Face の鍵の貼り間違い・常駐プログラムの起動のコマンド・serve.log の持ち帰り先の間違いで、何度も止まりました。全部を1つのスクリプトにまとめ、Pod の IP とポートは手で打たずに Runpod の API から受け取る形にしました。 Runpod の API キーと Hugging Face の鍵は、KIOXIA の `keys/` フォルダに1行ずつのファイルで置いています。スクリプトは鍵を読むだけで、画面・ログ・コミットのどこにも出しません（Claude Code が文字列で検索して、0件を確かめています）。API キーは Runpod の画面の Credentials → API Keys で作ります（[Runpod のドキュメント](https://docs.runpod.io/get-started/api-keys)）。 #### 依頼文 20261030_37_実機推論の起動と片付けを1行ずつにまとめる.md ```markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 180 --- # 依頼：Runpod の Pod の準備から mechArm 270 の実機推論までを `lab_start.sh` に、片付けを `lab_stop.sh` にまとめる（2026-10-30） 依頼 36（デーモンの自動起動）のあとに行う。今の手順は、LOG407（pixelstomotors.com/record/rewind-correct-real-arm）の「起動の手順」の節に全部ある。2026-10-30 は、IP とポートの書き方・Hugging Face の鍵の貼り間違い・デーモンの起動のコマンドの誤り・serve.log の持ち帰り先の間違いで、何度も止まった。 ## 鍵（エイイチが置く。中身を画面・ログ・記録に出さない） - Runpod の API キー：`/Volumes/KIOXIA/keys/runpod_api_key.txt` - Hugging Face の鍵：`/Volumes/KIOXIA/keys/hf_token.txt` - Pod に入る SSH の鍵は今までどおり `~/.ssh/id_ed25519`（Runpod のアカウントに公開鍵が入っている） - 鍵のファイルがない・空のときは、どのファイルに書けばよいかを出して止まる - Cloudflare の鍵（/Volumes/KIOXIA/p2m_cloudflare_token.txt）は upload_media.py が読んでいるので、動かさない ## lab_start.sh（Mac のターミナルで1行） 例：`./lab_start.sh --gpu A40 --episode run_0001_20261019_194818` 1. Runpod の API で Pod を作る。前回と同じ設定を既定にする（GPU A40 を1枚・テンプレートは Runpod Pytorch 2.8.0・コンテナのディスク 200GB・SSH の 22 番を外へ出す）。`--gpu` で GPU の種類を変えられる。「This machine does not have the resources」で借りられなかったら、少し待って数回やり直す 2. Pod が立ち上がり、SSH の IP とポートが出るまで待つ。IP とポートは、手で打たずに API から受け取る。画面には出さず、ログでは ``・`` に置き換える 3. 今の手順どおり Pod を準備する（pi05/ と add_trim_config.py を送る → huggingface_hub を入れる → 鍵をファイルから渡して `hf download` → openpi を 215abfb で取る → uv sync → addon・設定・assets → serve_policy.py を tmux の中で起動し、/workspace/logs/serve.log に残す）。鍵は、コマンドの引数や履歴に残らない渡し方にする 4. サーバーが `server listening on 0.0.0.0:8000` を出すまで待つ 5. Mac の 8000 番へのトンネルを、裏で張る（終わるときに閉じる） 6. デーモンの状態を見る（依頼 36 の daemon_ctl.sh）。止まっていれば起動する 7. HOME に戻す → 爪を開く → 位置合わせ（camera_align.py。`--episode` のお手本で。q で閉じる） 8. 実機推論（run_policy_client.py。今と同じ引数。`--raw-dir` は trim2 の raw_episodes）。終わったら（h・q）、もう1回やるかを聞く。y なら 7 に戻る 9. 推論を何回やっても、Pod は消さずに残す（片付けは lab_stop.sh） - どこで何秒かかったかを、最後に一覧で出す（Pod が立ち上がるまで・モデルを取るまで・uv sync・サーバーの起動） - ターミナルの出力は、今までどおり画面に出しつつ、`/Volumes/KIOXIA/r2r2r-data/lab/` の下に残す。Log 番号の決め方は `--log Log17` のように引数で受け取る（なければ聞く） - 途中で失敗したら、どこで止まったかと、Pod が残っていること（消すなら lab_stop.sh）を出して止まる ## lab_stop.sh 1. serve.log を `--log` のフォルダへ持ち帰る 2. Pod を Terminate まで消す（Stop だけにしない）。消えたことを API で確かめる 3. 借りていた時間と、その GPU の1時間の料金からの費用の見込みを出す（ドルと円。1ドル＝158円） 4. 裏のトンネルが残っていれば閉じる - Pod が残っていないかを確かめるだけのモード（`--check`）も付ける。消し忘れの確かめに使う ## 変えないもの - run_policy_client.py・camera_align.py の中身と引数（呼び出すだけ） - 新しい停止条件や確認は足さない ## 確かめ - アームなしで、Pod の作成 → 準備 → サーバーの起動 → トンネル → 推論のクライアントが `接続 ws://localhost:8000` を出すところまで（`--execute` なしのドライランで）通す。そのあと lab_stop.sh で消す。費用は数十円の見込みなので、通しで1回試してよい - 鍵の中身がログ・画面・git に出ていないことを確かめる（`hf_`・API キーの文字列を検索） - 鍵のファイルと KIOXIA のログは git に入れない ## 報告に書くこと - 使い方（コマンドの例） - 試した結果（各段階の秒数・費用） - 変えたファイル、コミット ``` #### Claude Code の報告 20261030_37_実機推論の起動と片付けを1行ずつにまとめる_20261030_2029.md ```markdown # 結果：20261030_37_実機推論の起動と片付けを1行ずつにまとめる - 状態：完了 - 始めた時刻：2026-10-30 20:29 - 終わった時刻：2026-10-30 20:46（17分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 704b7db6-1b2c-44d8-be99-fb09f9dcd2a6` ## Claude Code の報告 前の報告のあとに変わったことはありません。今届いた通知は、通し試験を見張っていた処理が10分で終わったというものです。通し試験は前に報告したとおり、もう終わっています。 **やったこと** ドライランの通し試験を見張っていた処理は、時間切れで止まりました。張り直してはいません。 **結果** - 試験はもう終わっています。Pod は消してあり、`./lab_stop.sh --check` で残っている Pod がないことを確かめました。 - 試験の各段階の秒数と費用（約 0.04 ドル、約6円）は、前の報告のとおりです。 **変えたファイル** なし（前の報告のあと、変えたファイルはありません） **決めてほしいこと** なし（前の報告に書いた3つは、まだ決まっていません。Pod の保存領域を 0GB にしたこと、実機で通すかどうかとそのときの Log 番号、serve.log を持ち帰れないときに Pod を消さずに止まる動きの3つです） ``` #### 作業記録 logs/20261030_2045.md ```markdown # 2026-10-30 20:45 依頼 37: 実機推論の起動と片付けを1行ずつにまとめる ## やったこと - lab_start.sh / lab_stop.sh / lab_lib.sh を追加（コミット a043b60） - アームなしの通し試験（--dry-run）を1回: Pod 作成 → 準備 → サーバー起動 → トンネル → run_policy_client.py が「接続 ws://localhost:8000」を出し、推論の答えも1回返った → lab_stop.sh で Terminate ## 秒数（test_lab_20261030） Pod を作る 2 / 立ち上がり 17 / 送る 33 / huggingface_hub と鍵 10 / hf download 12（6.0GB） / openpi 14 / uv sync 21 / addon 2 / サーバーの起動 40 / トンネル 3 / クライアント 62 ## 費用 A40 0.59 ドル/時 × 3分51秒 = 約 0.04 ドル（約 6 円） ## 鍵 hf_token・Runpod API キーの文字列は、ログ・画面の出力・スクリプト・コミットのどれにも出ていない（grep -F で 0 件） ## 残り - 実機（--dry-run なし）での通しは未実施（エイイチが動かす） - Pod のボリュームは 0GB（/workspace もコンテナのディスク 200GB の上）にした ``` アームなしの通し試験（`--dry-run`）では、Pod を作ってから推論のクライアントが AI のサーバーにつながるまで、次の時間でした。 | 段階 | 通し試験（20:36） | 4回目（21:04） | |---|---|---| | Pod を作る | 2秒 | 3秒 | | Pod が立ち上がるまで待つ | 17秒 | 38秒 | | スクリプトを送る | 33秒 | 32秒 | | huggingface_hub を入れ、鍵を渡す | 10秒 | 10秒 | | モデルを取る（hf download） | 12秒 | 8秒 | | openpi を取る（215abfb） | 14秒 | 24秒 | | uv sync | 21秒 | 20秒 | | addon・設定・assets | 2秒 | 2秒 | | サーバーの起動 | 40秒 | 40秒 | | Mac の 8000 番へのトンネル | 3秒 | 3秒 | | 借りていた時間・費用 | 3分51秒・約0.04ドル（約6円） | 9分56秒・約0.10ドル（約15円） | 報告の「決めてほしいこと」3つは、Pod に別の保存領域を付けずコンテナのディスク200GBだけにしたことはこのまま、実機の Log 番号は Log17（あとで Log16 に移した）、serve.log を持ち帰れないときは3回やり直してから Pod を消す形に変える（依頼39）、にしました。 - ねらい：Pod の準備で何度も止まり、手作業で約30分かかっていた起動を、打ち間違いの起きない1行にする - 変わったこと：`./lab_start.sh --log Log17` の1行で、Pod を作る → 準備 → サーバー → トンネル → 常駐プログラムの確認 → HOME → 爪を開く → 位置合わせ → 実機推論 まで進むようになった。片付けは `./lab_stop.sh` で、Pod を Terminate まで消して費用を出す - 見込める効果：Pod を作ってから AI のサーバーにつながるまでが、手作業の約30分から約2分半〜3分になった（記録の値）。Pod の消し忘れも `./lab_stop.sh --check` で確かめられる ### 4回目：lab_start.sh から動かし、肩の j1 を持ち上げて箱に入れた（pi05_20261030_210954） `./lab_start.sh --log Log17` を動かすと、位置合わせの画面まで自動で進みました。手で打ったのは、位置合わせを閉じる q と、推論の確認のキーだけです。 最初の推論（pi05_20261030_210821）は、最初の確認ですぐに終わり、結果も聞かれずに進みました。位置合わせの画面を閉じるときの q が、推論の確認の入力に残っていたと見ています（見込み）。`lab_start.sh` に「もう1回やるか」を聞かれて y を押し、2回目（pi05_20261030_210954）を動かしました。 推論7回目で、AI は j0 を +17.1° 回し、j2 を −20.6° 動かして、腕を箱の方へ伸ばす答えを返しました。3回目と同じく、持ち上がりきらないまま箱の縁に当たる形です。ここで c を押して補正に入りました。 | 刻み | 送った j1 の目標 | 実測 | 表示 | |---|---|---|---| | +2° | +51.9 | +53.1 | 届いていない（1.16°） | | +2° | +53.9 | +55.1 | 届いていない（1.18°） | | +2° | +55.9 | +56.8 | 着いた（0.85°） | | −2° | +53.9 | +56.3 | 届いていない（2.41°） | | −2° に 2.0° を足す | +49.9 | +51.8 | 届いていない（1.84°） | | −2° に 1.8° を足す | +46.1 | +47.8 | 届いていない（1.73°） | | −2° に 1.7° を足す | +42.4 | +44.0 | 届いていない（1.68°） | | −2° に 1.7° を足す | +38.7 | +40.3 | 届いていない（1.67°） | | −2° に 1.7° を足す | +35.0 | +36.7 | 届いていない（1.73°） | | −2° に 1.7° を足す | +31.3 | +33.0 | 届いていない（1.68°） | はじめに j2 を1°ずつ3回動かし、j1 を + に3回（爪が下がる向き）押してから、− の向きに切り替えています。ブロックを持った j1 は、どの刻みでも目標に 1.7°前後足りませんでしたが、足りなかった分が次の刻みに足されるので、− を1回押すと実測で約3.7°ずつ上がりました。j1 は +56.8° から +33.0° まで、− を7回押して上がっています。 【画像】LOG407_05_run4_top_lift.jpg 4回目を上のカメラで撮ったコマ。左上は補正に入った所（ブロックを持った爪が箱の縁の手前にある）、右上は補正の途中、左下は j1 を上げ終えた所、右下は AI に戻したあと、箱の上で爪を開いてブロックが箱に入る所 補正を終えて Enter で AI に戻すと、推論8回目で腕は箱の上へ動き、推論9回目で箱の上で爪を開いて、ブロックは箱に入りました。推論13回目の確認で h を押して HOME に戻し、結果を「成功」にしました。 推論8回目の確認では、爪を締め直すつもりで r を押しました。ただ、ここは補正のモードの外で、補正の外の確認は知らない文字を Enter と同じに扱うので、推論8回目のチャンクがそのまま実行されています。r と R は、c で補正に入ってから押す作りでした。この日の3回目の `c]` と同じ動きなので、補正の外でも r で締め直せるようにし、知らない文字ではチャンクを実行しないように直す依頼（依頼40）を出しています。 関節を送るのにかかった時間は、次のとおりです。 | チャンク | 中継点 | 送り終えるまで（秒） | |---|---|---| | 0（爪を開く時間を含む） | 4 | 11.0 | | 1 | 4 | 6.5 | | 2 | 4 | 4.9 | | 3 | 4 | 6.6 | | 4（爪を閉じる） | 2 | 5.6 | | 5 | 4 | 6.7 | | 6 | 4 | 6.6 | | 8 | 4 | 6.7 | | 9（爪を開く） | 4 | 10.9 | | 10 | 4 | 6.6 | | 11 | 4 | 6.8 | | 12 | 4 | 6.7 | - ねらい：AI の答えは1秒ぶんの動きなのに、中継点4点を送り終えるのに10秒以上かかっていた間を詰める - 変わったこと：中継点を送ったあとの待ちと、爪の開け閉めのあとの待ちを縮めた。届いたかの確認は残した - 見込める効果：中継点4点のチャンクが、1〜3回目の 10.5〜10.8秒から、4回目は 4.9〜6.8秒になった（記録の値）。最初のチャンクは 18.1秒から 11.0秒になった。推論1回分の記録（193.5秒）は、1回目（240.6秒）より短い 4回目の記録の数字です。 | 項目 | 値 | |---|---| | 上カメラのコマ | 1,708コマ（193.6秒）。1秒に8.824コマ | | 関節の角度を読めた回数 | 996回。1秒に5.15回 | | 補正 | 1回（13刻み） | | 結果 | 成功 | ### 次は、成功した4回目から学習データを作る 依頼39（serve.log を持ち帰れないときは3回やり直してから Pod を消す・依頼36の分のコミット）と、依頼40（補正の外の r・知らない文字で実行しない・位置合わせのあとに残ったキーを捨てる）は、この追記を書いた時点で依頼箱の順番を待っています。 4回目の記録から、直した区間を含む学習データを `build_correction_episodes.py` で作り、元のお手本19本と一緒に使えるかを確かめるのが次です。 ### 変えたコード（コミット a043b60 の時点の全文と、依頼36で作ったファイル） #### lab_start.sh ```bash #!/bin/bash # lab_start.sh — Runpod の Pod の準備から mechArm 270 の実機推論までを1行で行う (依頼 37 / 手順は LOG407「起動の手順」) # # ./lab_start.sh --gpu A40 --episode run_0001_20261019_194818 --log Log17 # ./lab_start.sh --log Log17 --dry-run アームなしで試す (デーモン・HOME・爪・位置合わせを飛ばし, # run_policy_client.py を --execute なし・--dry-keys で呼ぶ) # # --gpu GPU の種類 (既定 A40). 「A40」は「NVIDIA A40」として API に渡す. 名前に空白を含む種類は # 「--gpu "NVIDIA RTX A6000"」のように Runpod の名前のまま渡す # --episode 位置合わせ (camera_align.py) で重ねるお手本 (既定 run_0001_20261019_194818) # --log ターミナルの出力を残す /Volumes/KIOXIA/r2r2r-data/lab// (なければ聞く) # # 1. Pod を作る (A40×1・Runpod Pytorch 2.8.0・コンテナのディスク 200GB・22/tcp を外へ出す) # 「resources」で借りられなければ 30 秒待って, 合わせて 5 回まで作り直す # 2. Pod が立ち上がり, SSH の IP とポートが API から出るまで待つ (画面・ログには出さない) # 3. Pod を準備する (pi05/ と add_trim_config.py → huggingface_hub → hf download → openpi 215abfb → uv sync # → addon・設定・assets → serve_policy.py を tmux の中で起動し /workspace/logs/serve.log に残す) # 4. serve.log に「server listening on 0.0.0.0:8000」が出るまで待つ # 5. Mac の 8000 番へのトンネルを裏で張る (このスクリプトが終わるときに閉じる) # 6. tools/daemon_ctl.sh status で 5555 の LISTEN を見る. なければ start # 7. HOME → 爪を開く → camera_align.py (q で閉じる) # 8. run_policy_client.py (LOG407 と同じ引数). 終わったら, もう1回やるかを聞く (y なら 7 へ) # 9. Pod は消さない. 片付けは ./lab_stop.sh # # Pod の情報 (id・GPU・1時間の料金・作った時刻) は /pod/lab_pod.json に残す (IP とポートは残さない). set -u set -o pipefail HERE="$(cd "$(dirname "$0")" && pwd)" # shellcheck source=lab_lib.sh . "$HERE/lab_lib.sh" ORIG_ARGS=("$@") GPU=A40 EPISODE=run_0001_20261019_194818 LOG="" DRY_RUN=0 while [ $# -gt 0 ]; do case "$1" in --gpu) GPU="$2"; shift 2 ;; --episode) EPISODE="$2"; shift 2 ;; --log) LOG="$2"; shift 2 ;; --dry-run) DRY_RUN=1; shift ;; -h|--help) sed -n '2,27p' "$0"; exit 0 ;; *) echo "[ERROR] 知らない引数: $1 (./lab_start.sh --help)" >&2; exit 2 ;; esac done case "$GPU" in *" "*) GPU_ID="$GPU" ;; *) GPU_ID="NVIDIA $GPU" ;; esac if [ ! -d /Volumes/KIOXIA/r2r2r-data ]; then echo "[ERROR] SSD 未マウント: /Volumes/KIOXIA/r2r2r-data" >&2; exit 1 fi if [ -z "$LOG" ]; then if [ -t 0 ]; then read -r -p "Log 番号 (例 Log17) > " LOG; fi [ -n "$LOG" ] || { echo "[ERROR] --log Log17 のように Log 番号を付けてください" >&2; exit 2; } fi LOGDIR="$LAB_ROOT/$LOG" STATE="$LOGDIR/pod/lab_pod.json" # ---- ターミナルの出力を画面に出しつつ / に残す ---- if [ -z "${LAB_LOGGED:-}" ]; then mkdir -p "$LOGDIR/pod" LOGFILE="$LOGDIR/lab_start_$(date +%Y%m%d_%H%M%S).txt" echo "[LOG ] ターミナルの出力を残します → $LOGFILE" if [ -t 0 ] && [ -t 1 ]; then # script は端末のまま記録する (camera_align.py・run_policy_client.py の入力待ちもそのまま使える) exec env LAB_LOGGED=1 script -q -a "$LOGFILE" "$0" "${ORIG_ARGS[@]}" --log "$LOG" fi export LAB_LOGGED=1 exec > >(tee -a "$LOGFILE") 2>&1 fi export PYTHONUNBUFFERED=1 lab_need_key "$RUNPOD_KEY_FILE" "Runpod の API キー" lab_need_key "$HF_TOKEN_FILE" "Hugging Face の鍵" [ -s "$POD_SSH_KEY" ] || { echo "[ERROR] Pod に入る SSH の鍵がありません: $POD_SSH_KEY" >&2; exit 1; } [ -s "$POD_SSH_KEY.pub" ] || { echo "[ERROR] SSH の公開鍵がありません: $POD_SSH_KEY.pub" >&2; exit 1; } ADD_TRIM=/Volumes/KIOXIA/r2r2r-data/lab/Log11/pod/add_trim_config.py [ -f "$ADD_TRIM" ] || { echo "[ERROR] add_trim_config.py がありません: $ADD_TRIM" >&2; exit 1; } RAW=/Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes [ -d "$RAW/$EPISODE" ] || { echo "[ERROR] お手本がありません: $RAW/$EPISODE" >&2; exit 1; } lab_ssh_opts HF_REPO=PixelstoMotors/pi05-mecharm-trim-lora-v1 OPENPI_COMMIT=215abfb TEMPLATE_ID=runpod-torch-v280 IMAGE=runpod/pytorch:1.0.2-cu1281-torch280-ubuntu2404 # ---- 段階ごとの秒数 ---- T_NAMES=(); T_SECS=() CUR_STAGE="(始める前)" stage() { CUR_STAGE="$1"; STAGE_T0=$SECONDS; echo; echo "=== $1"; } stage_done() { T_NAMES+=("$CUR_STAGE"); T_SECS+=($((SECONDS - STAGE_T0))); echo " ($((SECONDS - STAGE_T0)) 秒)"; } summary() { echo echo "=== かかった時間" local i for i in "${!T_NAMES[@]}"; do printf ' %-32s %6d 秒\n' "${T_NAMES[$i]}" "${T_SECS[$i]}"; done } POD_ID="" fail() { echo echo "[STOP ] 「$CUR_STAGE」で止まりました: $*" if [ -n "$POD_ID" ]; then echo "[STOP ] Pod は残っています (id $POD_ID). 消すなら ./lab_stop.sh --log $LOG" fi summary exit 1 } TUNNEL_PID="" cleanup() { if [ -n "$TUNNEL_PID" ] && kill -0 "$TUNNEL_PID" 2>/dev/null; then kill "$TUNNEL_PID" 2>/dev/null echo "[TUNNEL] 8000 番のトンネルを閉じました" fi } trap cleanup EXIT trap 'fail "Ctrl-C で止めました"' INT echo "$LOG" > "$LAB_CURRENT" echo "[LAB ] $(date '+%Y-%m-%d %H:%M:%S') GPU=$GPU_ID episode=$EPISODE log=$LOGDIR $([ $DRY_RUN = 1 ] && echo 'ドライラン (アームなし)')" # ---- 1. Pod を作る ---- stage "1. Pod を作る" if [ -f "$STATE" ]; then OLD_ID=$(jq -r '.id' "$STATE") if runpod_api GET "/pods/$OLD_ID" >/dev/null 2>&1; then echo "[WARN ] $LOG の Pod (id $OLD_ID) がまだ残っています. 新しく作らずに止まります." echo " 消すなら ./lab_stop.sh --log $LOG" exit 1 fi fi others=$(runpod_api GET /pods) || fail "Pod の一覧を API から取れない (HTTP $RUNPOD_HTTP)" n_others=$(jq 'length' <<<"$others") if [ "$n_others" -gt 0 ]; then echo "[WARN ] ほかに Pod が $n_others 個あります (消し忘れなら ./lab_stop.sh --check で確かめる):" jq -r '.[] | " \(.id) \(.name) \(.desiredStatus) \(.costPerHr) ドル/時"' <<<"$others" fi body=$(jq -n --arg name "p2m-$LOG" --arg gpu "$GPU_ID" --arg tpl "$TEMPLATE_ID" --arg img "$IMAGE" \ --arg pk "$(cat "$POD_SSH_KEY.pub")" \ '{name:$name, templateId:$tpl, imageName:$img, gpuTypeIds:[$gpu], gpuCount:1, cloudType:"SECURE", containerDiskInGb:200, volumeInGb:0, ports:["22/tcp"], env:{PUBLIC_KEY:$pk}}') for attempt in 1 2 3 4 5; do resp=$(runpod_api POST /pods "$body") && break msg=$(jq -r '.error // .message // .' <<<"$resp" 2>/dev/null || echo "$resp") echo "[POD ] 作れなかった ($attempt/5, HTTP $RUNPOD_HTTP): $msg" case "$msg" in *resources*|*"no longer any instances"*|*"not available"*|*"no instances"*) [ "$attempt" -lt 5 ] && { echo "[POD ] 30 秒待って作り直します"; sleep 30; continue; } ;; esac fail "Pod を作れない: $msg" done POD_ID=$(jq -r '.id // ""' <<<"$resp") [ -n "$POD_ID" ] || fail "API の答えに Pod の id がない" COST_PER_HR=$(jq -r '.costPerHr // 0' <<<"$resp") POD_T0=$(date +%s) jq -n --arg id "$POD_ID" --arg gpu "$GPU_ID" --arg log "$LOG" --argjson cost "$COST_PER_HR" --argjson t0 "$POD_T0" \ '{id:$id, gpu:$gpu, log:$log, costPerHr:$cost, created_epoch:$t0, created_at:($t0|strflocaltime("%Y-%m-%d %H:%M:%S"))}' > "$STATE" echo "[POD ] 作った: id $POD_ID GPU=$GPU_ID 1時間 $COST_PER_HR ドル (記録 $STATE)" stage_done # ---- 2. Pod が立ち上がり, SSH の IP とポートが出るまで待つ ---- stage "2. Pod が立ち上がるまで待つ" for _ in $(seq 1 90); do lab_pod_addr && break sleep 10 done [ -n "${POD_IP:-}" ] && [ -n "${SSH_PORT:-}" ] || fail "15 分待っても SSH の IP とポートが出ない" echo "[POD ] SSH の IP とポートを API から受け取った (:)" for _ in $(seq 1 60); do pod_ssh true >/dev/null && break sleep 5 done pod_ssh 'echo "[POD ] 入れた: $(hostname)"; nvidia-smi --query-gpu=name,memory.total --format=csv,noheader' \ || fail "Pod に SSH で入れない" stage_done # ---- 3. Pod を準備する ---- stage "3a. スクリプトを送る" pod_ssh "mkdir -p /workspace/r2r2r /workspace/logs /workspace/ckpt" || fail "Pod にフォルダを作れない" pod_scp -r "$HERE/pi05" "root@$POD_IP:/workspace/r2r2r/" || fail "pi05/ を送れない" pod_scp "$ADD_TRIM" "root@$POD_IP:/workspace/r2r2r/" || fail "add_trim_config.py を送れない" stage_done stage "3b. huggingface_hub を入れ, 鍵を渡す" pod_ssh "pip install -q -U --break-system-packages huggingface_hub" || fail "huggingface_hub を入れられない" # 鍵は ssh の標準入力で渡す (コマンドの引数・履歴に残らない). hf はこのファイルを読む tr -d '\r\n' < "$HF_TOKEN_FILE" | pod_ssh 'umask 077; mkdir -p ~/.cache/huggingface; cat > ~/.cache/huggingface/token' \ || fail "Hugging Face の鍵を渡せない" pod_ssh 'hf auth whoami' || fail "Hugging Face に入れない (鍵が違う: $HF_TOKEN_FILE を確かめる)" stage_done stage "3c. モデルを取る (hf download)" pod_ssh "hf download $HF_REPO --local-dir /workspace/ckpt/3999 && du -sh /workspace/ckpt/3999" \ || fail "hf download に失敗" stage_done stage "3d. openpi を取る ($OPENPI_COMMIT)" pod_ssh "cd /workspace && { [ -d openpi/.git ] || git clone -q --recurse-submodules https://github.com/Physical-Intelligence/openpi.git; } \ && cd openpi && git checkout -q $OPENPI_COMMIT && git log --oneline -1" \ || fail "openpi を取れない" stage_done stage "3e. uv sync" pod_ssh "curl -LsSf https://astral.sh/uv/install.sh | sh >/dev/null && source \$HOME/.local/bin/env \ && cd /workspace/openpi && GIT_LFS_SKIP_SMUDGE=1 uv sync" || fail "uv sync に失敗" stage_done stage "3f. addon・設定・assets" pod_ssh "cd /workspace/openpi && python3 /workspace/r2r2r/pi05/openpi_addon/install_addon.py /workspace/openpi \ && python3 /workspace/r2r2r/add_trim_config.py /workspace/openpi \ && mkdir -p assets/pi05_mecharm_trim_lora/local \ && cp -r /workspace/ckpt/3999/assets/local/mecharm_redblock_box_teach_v1_trim2 assets/pi05_mecharm_trim_lora/local/" \ || fail "addon・設定・assets に失敗" stage_done # ---- 3g・4. サーバーを tmux の中で起動し, 受け付けを始めるまで待つ ---- stage "4. サーバーの起動" pod_ssh 'command -v tmux >/dev/null || { apt-get update -qq && apt-get install -y -qq tmux >/dev/null; }' \ || fail "tmux を入れられない" pod_ssh "tmux kill-session -t serve 2>/dev/null; : > /workspace/logs/serve.log; \ tmux new -d -s serve 'cd /workspace/openpi && source \$HOME/.local/bin/env && uv run scripts/serve_policy.py policy:checkpoint --policy.config=pi05_mecharm_trim_lora --policy.dir=/workspace/ckpt/3999 2>&1 | tee /workspace/logs/serve.log'" \ || fail "serve_policy.py を起動できない" listening=0 for _ in $(seq 1 120); do if pod_ssh "grep -q 'server listening on 0.0.0.0:8000' /workspace/logs/serve.log"; then listening=1; break; fi if ! pod_ssh "tmux has-session -t serve 2>/dev/null"; then pod_ssh "tail -20 /workspace/logs/serve.log" fail "serve_policy.py が終わってしまった (上は serve.log の末尾)" fi sleep 10 done [ $listening = 1 ] || { pod_ssh "tail -20 /workspace/logs/serve.log"; fail "20 分待っても server listening が出ない"; } pod_ssh "grep 'server listening' /workspace/logs/serve.log | tail -1" stage_done # ---- 5. トンネル ---- stage "5. Mac の 8000 番へのトンネル" if lsof -nP -iTCP:$TUNNEL_PORT -sTCP:LISTEN >/dev/null 2>&1; then fail "Mac の $TUNNEL_PORT 番がすでに使われている (前のトンネルなら ./lab_stop.sh で閉じる)" fi ssh -N -L $TUNNEL_PORT:localhost:$TUNNEL_PORT "${SSH_OPTS[@]}" -o ExitOnForwardFailure=yes \ -p "$SSH_PORT" "root@$POD_IP" 2>&1 | lab_mask & sleep 1 TUNNEL_PID=$(lab_tunnel_pids | head -1) for _ in $(seq 1 15); do lsof -nP -iTCP:$TUNNEL_PORT -sTCP:LISTEN >/dev/null 2>&1 && break sleep 1 done lsof -nP -iTCP:$TUNNEL_PORT -sTCP:LISTEN >/dev/null 2>&1 || fail "トンネルを張れない" echo "[TUNNEL] localhost:$TUNNEL_PORT → Pod の 8000 番 (pid $TUNNEL_PID. このスクリプトが終わると閉じる)" stage_done cd "$HERE" || fail "$HERE に移れない" # ---- 6. デーモン ---- if [ $DRY_RUN = 0 ]; then stage "6. デーモンの状態" st=$("$HERE/tools/daemon_ctl.sh" status 2>&1); echo "$st" if ! grep -q ':5555 ' <<<"$st"; then echo "[DAEMON] 5555 番で受け付けていないので起動します" st=$("$HERE/tools/daemon_ctl.sh" start 2>&1); echo "$st" grep -q ':5555 ' <<<"$st" || fail "デーモンが 5555 番で受け付けない (tools/daemon_ctl.sh log で確かめる)" fi stage_done fi # ---- 7・8. 位置合わせと実機推論 (くり返せる) ---- # ここから先の Ctrl-C は, 動いている Python (位置合わせ・推論) だけを止める (このスクリプトは続ける) trap ':' INT round=0 while :; do round=$((round + 1)) if [ $DRY_RUN = 0 ]; then stage "7. HOME → 爪を開く → 位置合わせ ($round 回目)" python3 dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py || fail "HOME に戻せない" python3 -c "from visual_servo_pick import get_daemon_socket, daemon_set_gripper, close_daemon_socket; get_daemon_socket(); daemon_set_gripper(0); close_daemon_socket()" \ || fail "爪を開けない" python3 pi05/camera_align.py --raw-dir "$RAW" --episode "$EPISODE" --scale 5 --alpha 0.7 rc=$? [ $rc = 0 ] || [ $rc = 130 ] || fail "camera_align.py が失敗 (終了コード $rc)" stage_done stage "8. 実機推論 ($round 回目)" 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 rc=$? else stage "8. 推論のクライアント (ドライラン・--dry-keys. アームには触らない)" python3 pi05/run_policy_client.py --raw-dir "$RAW" --steps-per-chunk 10 --via-stride 3 \ --settle-retries 0 --state-from-target --dry-keys --ignore-framing rc=$? fi echo "[CLIENT] run_policy_client.py が終わった (終了コード $rc)" stage_done if [ -t 0 ]; then read -r -p "もう1回やりますか? y=やる / 他=終わる > " again else again=n fi [ "$again" = y ] || break done # ---- 9. Pod は残す ---- summary echo echo "[LAB ] Pod は残しています (id $POD_ID). 片付けは ./lab_stop.sh --log $LOG" ``` #### lab_stop.sh ```bash #!/bin/bash # lab_stop.sh — lab_start.sh で作った Pod を片付ける (依頼 37) # # ./lab_stop.sh --log Log17 serve.log を持ち帰り, Pod を Terminate まで消し, 費用の見込みを出す # ./lab_stop.sh --log なしは, 最後に lab_start.sh を動かした Log を使う # ./lab_stop.sh --check Pod が残っていないかを確かめるだけ (消さない. 残っていれば終了コード 1) # ./lab_stop.sh --log Log17 --skip-log serve.log を持ち帰れなくても消す # # 1. Pod の /workspace/logs/serve.log を /Volumes/KIOXIA/r2r2r-data/lab// へ持ち帰る # (serve.log がすでにあれば serve_<時刻>.log にする). 持ち帰れなければ消さずに止まる # 2. Pod を Terminate (DELETE) する. 一覧からも消えたことを API で確かめる # 3. 借りていた時間 × 1時間の料金 で費用の見込みを出す (ドルと円. 1ドル=158円) # 4. 裏の 8000 番のトンネルが残っていれば閉じる set -u set -o pipefail HERE="$(cd "$(dirname "$0")" && pwd)" # shellcheck source=lab_lib.sh . "$HERE/lab_lib.sh" LOG=""; CHECK=0; SKIP_LOG=0 while [ $# -gt 0 ]; do case "$1" in --log) LOG="$2"; shift 2 ;; --check) CHECK=1; shift ;; --skip-log) SKIP_LOG=1; shift ;; -h|--help) sed -n '2,15p' "$0"; exit 0 ;; *) echo "[ERROR] 知らない引数: $1 (./lab_stop.sh --help)" >&2; exit 2 ;; esac done lab_need_key "$RUNPOD_KEY_FILE" "Runpod の API キー" close_tunnel() { local pids pids=$(lab_tunnel_pids) if [ -n "$pids" ]; then kill $pids 2>/dev/null echo "[TUNNEL] 残っていた 8000 番のトンネルを閉じました (pid $(echo $pids))" else echo "[TUNNEL] 8000 番のトンネルは残っていません" fi } list_pods() { # 残っている Pod を出す. 戻り値は数 local j n j=$(runpod_api GET /pods) || { echo "[ERROR] Pod の一覧を API から取れない (HTTP $RUNPOD_HTTP)" >&2; return 255; } n=$(jq 'length' <<<"$j") if [ "$n" -eq 0 ]; then echo "[CHECK ] 残っている Pod はありません" else echo "[CHECK ] 残っている Pod が $n 個あります:" jq -r '.[] | " \(.id) \(.name) \(.desiredStatus) \(.costPerHr) ドル/時"' <<<"$j" fi return "$n" } if [ $CHECK = 1 ]; then list_pods; n=$? if [ -n "$(lab_tunnel_pids)" ]; then echo "[CHECK ] 8000 番のトンネルが残っています (pid $(echo $(lab_tunnel_pids)))" fi [ $n = 0 ] && exit 0 || exit 1 fi if [ -z "$LOG" ] && [ -s "$LAB_CURRENT" ]; then LOG=$(cat "$LAB_CURRENT"); fi [ -n "$LOG" ] || { echo "[ERROR] --log Log17 のように Log 番号を付けてください" >&2; exit 2; } LOGDIR="$LAB_ROOT/$LOG" STATE="$LOGDIR/pod/lab_pod.json" if [ -d "$LOGDIR" ]; then # 画面に出しつつ / に残す exec > >(tee -a "$LOGDIR/lab_stop_$(date +%Y%m%d_%H%M%S).txt") 2>&1 fi [ -f "$STATE" ] || { echo "[ERROR] $STATE がありません (lab_start.sh で作った Pod の記録). ./lab_stop.sh --check で確かめる" >&2; exit 1; } POD_ID=$(jq -r '.id' "$STATE") COST_PER_HR=$(jq -r '.costPerHr' "$STATE") GPU_ID=$(jq -r '.gpu' "$STATE") POD_T0=$(jq -r '.created_epoch' "$STATE") echo "[STOP ] $LOG の Pod: id $POD_ID GPU=$GPU_ID 作った時刻 $(jq -r '.created_at' "$STATE")" lab_ssh_opts # ---- 1. serve.log を持ち帰る ---- if runpod_api GET "/pods/$POD_ID" >/dev/null; then if lab_pod_addr; then dest="$LOGDIR/serve.log" [ -e "$dest" ] && dest="$LOGDIR/serve_$(date +%Y%m%d_%H%M%S).log" if pod_scp "root@$POD_IP:/workspace/logs/serve.log" "$dest"; then echo "[LOG ] serve.log を持ち帰った → $dest ($(wc -l < "$dest" | tr -d ' ') 行)" elif [ $SKIP_LOG = 0 ]; then echo "[STOP ] serve.log を持ち帰れないので, Pod は消さずに止まります." echo " 持ち帰らずに消すなら ./lab_stop.sh --log $LOG --skip-log" exit 1 fi elif [ $SKIP_LOG = 0 ]; then echo "[STOP ] Pod の SSH の IP とポートが API から出ない (状態 ${POD_STATUS:-?}). serve.log を持ち帰れません." echo " 持ち帰らずに消すなら ./lab_stop.sh --log $LOG --skip-log" exit 1 fi else echo "[STOP ] Pod (id $POD_ID) は API から見つかりません (HTTP $RUNPOD_HTTP). すでに消えている見込み" fi # ---- 2. Terminate ---- if runpod_api GET "/pods/$POD_ID" >/dev/null; then runpod_api DELETE "/pods/$POD_ID" >/dev/null || { echo "[ERROR] Pod を消せない (HTTP $RUNPOD_HTTP)"; exit 1; } T_END=$(date +%s) echo "[STOP ] Terminate を送った ($(date -r "$T_END" '+%H:%M:%S'))" else T_END=$(jq -r '.terminated_epoch // empty' "$STATE") # 前に消したときの時刻 (なければ今) [ -n "$T_END" ] || T_END=$(date +%s) fi gone=0 for _ in $(seq 1 30); do if pods=$(runpod_api GET /pods) \ && ! jq -e --arg id "$POD_ID" 'any(.[]; .id == $id and .desiredStatus != "TERMINATED")' <<<"$pods" >/dev/null; then gone=1; break fi sleep 5 done if [ $gone = 1 ]; then echo "[STOP ] Pod (id $POD_ID) が消えたことを API で確かめた" else echo "[ERROR] 2 分半待っても Pod (id $POD_ID) が API に残っています. Runpod の画面で確かめてください" fi # ---- 3. 費用の見込み ---- secs=$((T_END - POD_T0)) awk -v s="$secs" -v c="$COST_PER_HR" -v y="$YEN_PER_USD" 'BEGIN { h = s / 3600; usd = h * c printf "[COST ] 借りていた時間 %d 分 %d 秒 (%.2f 時間) × 1時間 %.2f ドル = 約 %.2f ドル (約 %d 円. 1ドル=%d円)\n", int(s / 60), s % 60, h, c, usd, int(usd * y + 0.5), y }' jq --argjson t "$T_END" --argjson s "$secs" \ '. + {terminated_epoch:$t, terminated_at:($t|strflocaltime("%Y-%m-%d %H:%M:%S")), rented_sec:$s}' \ "$STATE" > "$STATE.tmp" && mv "$STATE.tmp" "$STATE" # ---- 4. トンネル ---- close_tunnel list_pods >/dev/null; n=$? [ $gone = 1 ] || exit 1 [ $n = 0 ] || { echo "[WARN ] ほかの Pod が $n 個残っています (./lab_stop.sh --check)"; } ``` #### lab_lib.sh ```bash # lab_lib.sh — lab_start.sh / lab_stop.sh が読む共通の関数 (単体では動かさない) # # 鍵の扱い: # - Runpod の API キーは curl のヘッダーを <(printf ...) で渡す (printf は bash の組み込みなので # ps にもシェルの履歴にも出ない) # - Hugging Face の鍵は ssh の標準入力で Pod の ~/.cache/huggingface/token に書く # - Pod の IP とポートは画面・ログに出さない. Pod に入る ssh の出力は lab_mask で ・ に置き換える LAB_ROOT=/Volumes/KIOXIA/r2r2r-data/lab LAB_CURRENT="$LAB_ROOT/.lab_current" # 今使っている Log 名 (lab_stop.sh が --log なしで読む) RUNPOD_KEY_FILE=/Volumes/KIOXIA/keys/runpod_api_key.txt HF_TOKEN_FILE=/Volumes/KIOXIA/keys/hf_token.txt POD_SSH_KEY="$HOME/.ssh/id_ed25519" RUNPOD_API=https://rest.runpod.io/v1 TUNNEL_PORT=8000 YEN_PER_USD=158 lab_need_key() { # 鍵のファイルがない・空なら, どこに書けばよいかを出して止まる local f="$1" what="$2" if [ ! -s "$f" ]; then echo "[ERROR] $what がありません (または空です)." >&2 echo " 次のファイルに鍵だけを1行で書いてください: $f" >&2 exit 1 fi } runpod_api() { # runpod_api METHOD PATH [JSON] → 本文を出す. 失敗の HTTP は戻り値 1 local method="$1" path="$2" data="${3:-}" out code out=$(mktemp) if [ -n "$data" ]; then code=$(curl -sS -o "$out" -w '%{http_code}' -X "$method" \ -H @<(printf 'Authorization: Bearer %s\n' "$(tr -d '\r\n' < "$RUNPOD_KEY_FILE")") \ -H 'Content-Type: application/json' --data "$data" "$RUNPOD_API$path") || code=000 else code=$(curl -sS -o "$out" -w '%{http_code}' -X "$method" \ -H @<(printf 'Authorization: Bearer %s\n' "$(tr -d '\r\n' < "$RUNPOD_KEY_FILE")") \ "$RUNPOD_API$path") || code=000 fi cat "$out" rm -f "$out" RUNPOD_HTTP=$code case "$code" in 2??) return 0 ;; *) return 1 ;; esac } lab_pod_addr() { # POD_ID から POD_IP / SSH_PORT を API で受け取る (画面には出さない) local j j=$(runpod_api GET "/pods/$POD_ID") || return 1 POD_IP=$(jq -r '.publicIp // ""' <<<"$j") SSH_PORT=$(jq -r '.portMappings["22"] // ""' <<<"$j") POD_STATUS=$(jq -r '.desiredStatus // ""' <<<"$j") [ -n "$POD_IP" ] && [ -n "$SSH_PORT" ] } lab_mask() { # Pod の IP とポートを置き換えて流す if [ -n "${POD_IP:-}" ] && [ -n "${SSH_PORT:-}" ]; then sed -l -e "s/${POD_IP//./\\.}//g" \ -e "s/\([^0-9]\)${SSH_PORT}\([^0-9]\)/\1\2/g" \ -e "s/\([^0-9]\)${SSH_PORT}\$/\1/" else cat fi } # known_hosts は使わない (Pod ごとに IP が変わる). LogLevel=ERROR で「Permanently added 」も出さない lab_ssh_opts() { SSH_OPTS=(-i "$POD_SSH_KEY" -o StrictHostKeyChecking=no -o UserKnownHostsFile=/dev/null -o LogLevel=ERROR -o ConnectTimeout=10 -o ServerAliveInterval=30) } pod_ssh() { # pod_ssh 'コマンド' (標準入力はそのまま Pod へ) { ssh "${SSH_OPTS[@]}" -p "$SSH_PORT" "root@$POD_IP" "$@" 2>&1; } | lab_mask } pod_scp() { # pod_scp 送る元... 送り先 (送り先の「:」の前に root@IP を付ける) { scp "${SSH_OPTS[@]}" -P "$SSH_PORT" "$@" 2>&1; } | lab_mask } lab_tunnel_pids() { # このスクリプトが張った 8000 番のトンネル (ssh -N -L 8000:localhost:8000) pgrep -f "ssh -N -L ${TUNNEL_PORT}:localhost:${TUNNEL_PORT}" || true } ``` #### tools/daemon_ctl.sh ```bash #!/bin/bash # daemon_ctl.sh — Mac から Pi の mecharm_daemon.py (TCP 5555) を見る・起動する・止める # # tools/daemon_ctl.sh status プロセスの有無と 5555 の LISTEN を見る # tools/daemon_ctl.sh start systemd で起動 (すでに 5555 が LISTEN なら起動しない) # tools/daemon_ctl.sh stop systemd で止める # tools/daemon_ctl.sh restart systemd で起動し直す # tools/daemon_ctl.sh log [N] daemon.log の末尾 N 行 (既定 30) # tools/daemon_ctl.sh install tools/pi/ の 2 ファイルを Pi に置き, 自動起動を有効にする # # 注意: pgrep / pkill の「[m]echarm_daemon」は、起動する側の文字 # (mecharm_daemon.py の実名) と同じ 1 行に入れない。入れると自分のシェルに当たる。 set -u PI=ubuntu@192.168.2.1 KEY="$HOME/.ssh/id_rsa_pi" SSH=(ssh -i "$KEY" -o ConnectTimeout=5 "$PI") UNIT=mecharm-daemon.service HERE="$(cd "$(dirname "$0")" && pwd)" remote_status() { "${SSH[@]}" 'bash -s' <<'REMOTE' echo "--- process" pgrep -af '[m]echarm_daemon\.py' || echo "(なし)" echo "--- 5555" ss -ltn | grep ':5555 ' || echo "(LISTEN なし)" echo "--- systemd" systemctl is-enabled mecharm-daemon.service 2>&1 | sed 's/^/enabled: /' systemctl is-active mecharm-daemon.service 2>&1 | sed 's/^/active: /' REMOTE } case "${1:-status}" in status) remote_status ;; start) "${SSH[@]}" "sudo systemctl start $UNIT" sleep 8 remote_status ;; stop) "${SSH[@]}" "sudo systemctl stop $UNIT" remote_status ;; restart) "${SSH[@]}" "sudo systemctl restart $UNIT" sleep 8 remote_status ;; log) "${SSH[@]}" "tail -n ${2:-30} /home/ubuntu/daemon.log" ;; install) scp -i "$KEY" "$HERE/pi/mecharm_daemon_start.sh" "$HERE/pi/mecharm-daemon.service" "$PI:/tmp/" && "${SSH[@]}" "install -m 755 /tmp/mecharm_daemon_start.sh /home/ubuntu/mecharm_daemon_start.sh && sudo install -m 644 /tmp/mecharm-daemon.service /etc/systemd/system/$UNIT && sudo systemctl daemon-reload && sudo systemctl enable $UNIT" remote_status ;; *) echo "使い方: $0 status|start|stop|restart|log [N]|install" >&2 exit 2 ;; esac ``` #### tools/pi/mecharm_daemon_start.sh ```bash #!/bin/bash # mecharm_daemon_start.sh (Pi 側, systemd mecharm-daemon.service から呼ぶ) # - シリアルの準備を待ってから mecharm_daemon.py を起動する # - すでに 5555 を受け付けている / デーモンのプロセスがいるときは起動しない (二重起動防止) # - ログは今までどおり /home/ubuntu/daemon.log に足していく # このファイルのコマンド行は「mecharm_daemon_start.sh」なので # 下の pgrep -f '[m]echarm_daemon\.py' は自分に当たらない。 LOG=/home/ubuntu/daemon.log DAEMON=/home/ubuntu/mecharm_daemon.py SERIAL=/dev/ttyAMA0 BOOT_WAIT_SEC=5 stamp() { echo "[START $(date '+%H:%M:%S')] $*" >> "$LOG"; } # シリアルの準備を待つ (最大 30 秒, その後さらに BOOT_WAIT_SEC 秒) for i in $(seq 1 30); do [ -e "$SERIAL" ] && break sleep 1 done sleep "$BOOT_WAIT_SEC" if ss -ltn | grep -q ':5555 '; then stamp "5555 はすでに LISTEN 中。起動しない" exit 0 fi if pgrep -f '[m]echarm_daemon\.py' > /dev/null; then stamp "mecharm_daemon.py のプロセスがすでにいる。起動しない" exit 0 fi stamp "mecharm_daemon.py を起動する" cd /home/ubuntu exec /usr/bin/python3 -u "$DAEMON" >> "$LOG" 2>&1 ``` #### tools/pi/mecharm-daemon.service ```ini # /etc/systemd/system/mecharm-daemon.service # Pi の電源を入れたら mecharm_daemon.py (TCP 5555) を起動する。 # 落ちたら (終了コードが 0 以外) 5 秒後に起き直す。 # 二重起動の防止とログの追記は mecharm_daemon_start.sh で行う。 [Unit] Description=mechArm 270 daemon (mecharm_daemon.py, TCP 5555) After=network.target [Service] Type=simple User=ubuntu Group=ubuntu SupplementaryGroups=dialout WorkingDirectory=/home/ubuntu ExecStart=/bin/bash /home/ubuntu/mecharm_daemon_start.sh Restart=on-failure RestartSec=5 KillSignal=SIGTERM TimeoutStopSec=10 [Install] WantedBy=multi-user.target ``` #### pi05/run_policy_client.py（コミット 7aaf5df の時点） ```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 範囲内 - グリッパー操作後は GRIPPER_SLEEP 待つ (2026-10-30 依頼 35 で 2.0 → 0.8 秒. --grip-wait で戻せる) - --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", ...} r = 爪を開かずに閉じる指令をもう一度 (締め直し) / R = 開いてから閉じ直す (つかみ直し). どちらも爪が閉じているときだけ (開いていれば何もしない. 閉じるのは g). 2026-10-30 依頼 34 1 刻みの「着いた」は, 止まるのを待ってから 動かした関節だけ を 刻みの半分 (下限 0.5°) で見る (表示と記録の joint_reached だけ. 送り方・再送・未到達のときの数え直しは今までどおり 3°) 届いていないときは, 同じ関節・同じ向きの次の 1 刻みに 足りなかった分 を足す (上限は刻みの大きさ. move 行の carry_deg). 届いた・向きを変えた・ほかの関節を選んだら足すのをやめる. 2026-10-30 依頼 35 R で開いてから閉じ終わるまでのコマは regrip (学習データに使わない). R だけを使った補正は b で巻き戻せる (つかんだ区間と同じく y=持っている / n=落とした を聞く). 依頼 35 - --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 で「仮に実行」= 仮の姿勢が目標へ移る) 待ち時間 (2026-10-30 依頼 35. エイイチの判断で縮めた. 本線と共有の定数は変えず, ここで上書きする): --send-wait 0.0 (前 1.0) / --grip-wait 0.8 (前 2.0) / --grip-open-wait 0.0 (前 2.0) / --settle-wait 0.8 (前 2.0) 前の値に戻すには --send-wait 1.0 --grip-wait 2.0 --grip-open-wait 2.0 --settle-wait 2.0 送ったあとの到着の確認 (_send_and_wait_arrival の 3°) はそのまま行う. 未到達のときに続けるか止めるかは 今までどおり (--auto は止める. 手動の推論・巻き戻しは警告を出して続ける) 終了コード: 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 (待ちは下の SEND_WAIT で上書き) 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, GRASP_HELD, MODE_HOME, MODE_MODEL, MODE_REGRIP, MODE_REWIND, MODE_WAIT, correction_carry_used, correction_joint_reached, correction_reach_tol, correction_shortfall, 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 値 # ---- 待ち時間 (2026-10-30 依頼 35. エイイチの判断で縮めた. 「time.sleep() を短縮しない」より優先) ---- # 本線 (j0_sweep_scan.py / visual_servo_pick.py) と共有の定数は変えず, この中でだけ上書きする. # コマンドの引数 (--send-wait / --grip-wait / --grip-open-wait / --settle-wait) で前の値に戻せる. # 2026-10-30 の実機 (184245 / 185133 / 190659) では中継点 4 点のチャンクを送り終えるのに 10.5〜10.8 秒. GRIPPER_SLEEP = 0.8 # 爪の開け閉めのあと Mac で待つ秒 (前 2.0. DAY175 対策の daemon 側の 2 秒は Pi のまま) GRIP_OPEN_DAEMON_WAIT = 0.0 # 開くとき daemon が指令のあとで待つ秒 (前 2.0 = daemon_set_gripper の既定. # 前は開くたびに daemon 2 + Mac 2.0 = 4 秒待っていた. 閉じる方は daemon で待たない) SEND_WAIT = 0.0 # 送ったあと daemon が待つ秒 (前 SEND_WAIT_SEC 1.0). この待ちは daemon の # 「送信後の現在角度」の記録を遅らせるだけで, 着いたかは Mac が 3° で読んで確かめる. # 0 にするのは, 0 < wait のとき Mac の待ち上限が wait + 3 秒になり, 動いている間の # daemon の読み直し (最長 3.7 秒. LOG352) より短くなるため (0 なら上限 35 秒) 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 = 0.8 # 止まらなくてもここで待つのをやめ, そのときの値で判定する (前 2.0. 依頼 35) # --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) を直接呼ぶ. 中で再送しないので送信回数を正確に数えられる (上限を超えない). 送ったあとは 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) 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 gripper_cmd(state): """爪の指令と, そのあとの待ち (推論・補正・巻き戻しで共通). 閉 = daemon_set_gripper_value(GRIP_VALUE) / 開 = daemon_set_gripper(0, wait=GRIP_OPEN_DAEMON_WAIT). そのあと GRIPPER_SLEEP 待つ (依頼 35 で縮めた. 引数で前の値に戻せる).""" if state == 1: daemon_set_gripper_value(GRIP_VALUE) else: daemon_set_gripper(0, wait=GRIP_OPEN_DAEMON_WAIT) time.sleep(GRIPPER_SLEEP) 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°). グリッパーも推論と同じ gripper_cmd (閉 = 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, send_wait_sec=SEND_WAIT) 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 read_settled(self): """補正の 1 刻みの到着判定用: 動きが止まるのを待ってから読む (read_until_still). 戻り値: (角度 or None (読めない), 止まったか). 止まるのを待つのは目安 SETTLE_WAIT_SEC (読み取りの通信時間は含まない). ドライランは仮の姿勢をそのまま返す (止まった扱い).""" if self.execute: q, still, _ = read_until_still() return q, bool(still) return (None if self.sim_q is None else self.sim_q.copy()), 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 gripper_cmd(state) 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 は つかんだ区間 (グリッパーを閉じた推論) と R を使った補正 を巻き戻すときだけ渡す (依頼 27 / 35): GRASP_PLACED (y = 持っている): 送った目標を逆にたどって つかんだ位置 (最後に送った目標) まで 下ろす → 開いてブロックを置く → 残りを逆にたどって始めの姿勢へ. つかんだ位置まで下ろせなかったら開かない (宙で落とさない. 補正の取り消しと同じ決まり) GRASP_DROPPED (n = 落とした): 今の位置ですぐ開く (閉じた指が置き直したブロックに当たらないよう) → 逆にたどって始めの姿勢へ GRASP_HELD (y = 持っている. R を使った補正だけ): 閉じたまま逆にたどって始めの姿勢へ (開かない) 送り方・範囲チェックは 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: what = "つかんだ区間" if entry["kind"] == "chunk" else "R でつかみ直した補正" print(f"[REWIND] {name} ({what}) を巻き戻す: 落としたので先にグリッパーを開きます") 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 in (None, GRASP_HELD) 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 # 補正中に一度でも g でグリッパーを動かしたか (往復でも True. codex R1-2) regrip_used = False # R (つかみ直し) を使ったか. 始めと終わりはどちらも閉 → b で巻き戻せる (依頼 35) carry, carry_key = 0.0, None # 届かなかった分 と それを足す (関節, 向き) (依頼 35) 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": if val != joint: carry, carry_key = 0.0, None # ほかの関節を選んだら足すのをやめる (依頼 35) 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 carry, carry_key = 0.0, None # 実測から数え直すので足りない分はもう入っている if carry_key != (joint, val): carry, carry_key = 0.0, None # 向きを変えた (+ と - / ほかの関節) ら足すのをやめる step = CORRECTION_STEPS[si] if carry > 0: # codex 依頼 35 ⚠️: 止まる前に打ち切って測った不足は, 入力を待つ間に解消していることがある. # 送る直前の実測 qa で足りない分を測り直す (届いていれば 0) jr_now, _ = correction_joint_reached(qa, cur, joint, correction_reach_tol(step)) carry = correction_shortfall(qa, cur, joint, val, jr_now) # 依頼 35: 前の 1 刻みが届いていなければ, 足りなかった分 (上限は刻みの大きさ) を足して送る. # 範囲チェック (apply_limits / check_send_safe) はそのまま通す add = correction_carry_used(carry, step) t, notes, unsafe = plan_correction_step(cur, joint, val, step + add, qa, lo, hi, max_move) e = {"type": "correction", "action": "move", "correction_id": corr_id, "after_chunk": chunk, "joint": joint, "step_deg": step, "sign": val, "carry_deg": round(add, 3), "t": time.time(), "before": _fmt6(cur), "after": _fmt6(t), "measured_before": _fmt6(qa), "notes": notes, "sent": False} if add > 0: print(f" [補正] 足りなかった {add:.1f}° を足して送る (j{joint} を {val * (step + add):+.1f}°)") 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}") # 依頼 34: 「着いた」は動かした関節だけを, 刻みの半分 (下限 0.5°) で見る. # 送り方・再送・resync は今までどおり send の結果 (3°) で決める (表示と記録だけ) qs, settled = arm.read_settled() tol = correction_reach_tol(step) jr, jerr = correction_joint_reached(qs, t, joint, tol) e.update(sent=True, arrived=bool(ok), measured_after=_fmt6(qs), joint_reached=bool(jr), joint_err_deg=jerr, reach_tol_deg=tol, settled=settled) e["delta"] = round(float(t[joint] - cur[joint]), 3) cur = t sent.append(t.copy()) n_steps += 1 if not settled and jerr is not None: print(f" [補正] (動きが止まらないまま {SETTLE_WAIT_SEC:.1f} 秒で打ち切って測りました)") if jerr is None: print(f" [補正] j{joint} 届いたか分からない (関節角を読めない)") elif jr: print(f" [補正] j{joint} 着いた (目標 {t[joint]:+.1f} / 実測 {qs[joint]:+.1f}, " f"ずれ {jerr:.2f}° ≤ {tol:.2f}°)") else: print(f" [補正] j{joint} 届いていない (目標 {t[joint]:+.1f} / 実測 {qs[joint]:+.1f}, " f"ずれ {jerr:.2f}° > {tol:.2f}°)") # 依頼 35: 届いていなければ, 足りなかった分を次の同じ向きの 1 刻みに足す. 届いたらやめる carry = correction_shortfall(qs, t, joint, val, jr) carry_key = (joint, val) if carry > 0 else None 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 == "regrip": # 依頼 34: r = 開かずに閉じる指令をもう一度 / R = 開く → 閉じる (その場でつかみ直す). # 閉じる・開くは g と同じ arm.gripper (値 GRIP_VALUE・速さ・待ち GRIPPER_SLEEP も同じ) action = "regrip_tighten" if val == "tighten" else "regrip_reopen" e = {"type": "correction", "action": action, "correction_id": corr_id, "after_chunk": chunk, "t": time.time(), "key": "r" if val == "tighten" else "R", "before": grip} if grip != 1: what = "締め直し" if val == "tighten" else "つかみ直し" print(f" [補正] 爪が開いているので{what}はしない（閉じるのは g）") e.update(after=grip, sent=False, skipped="gripper_open") elif val == "tighten": print(" [補正] 爪を締め直します (開かずに閉じる指令をもう一度)") arm.gripper(1) e.update(after=1, sent=True) else: print(" [補正] 爪を開いて, 閉じ直します (つかみ直し)") # 依頼 35: 開いてから閉じ終わるまでのコマは regrip (学習データに使わない) if recorder is not None: recorder.set_mode(MODE_REGRIP) arm.gripper(0) arm.gripper(1) if recorder is not None: recorder.set_mode(MODE_CORRECT) e.update(after=1, sent=True) # 始めも終わりも閉. g の往復とは分け, b で巻き戻せるようにする (y/n を聞く. 依頼 35) regrip_used = True log_f.write(json.dumps(e) + "\n"); log_f.flush() _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, "regrip_used": regrip_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, "regrip_used": regrip_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 apply_wait_args(args): """--send-wait などを この中の待ち (SEND_WAIT / GRIPPER_SLEEP / GRIP_OPEN_DAEMON_WAIT / SETTLE_WAIT_SEC) へ入れる. 指定がなければ今の値のまま. 決まった値は args にも書き戻す ({run_stem}.json の args に残る).""" global SEND_WAIT, GRIPPER_SLEEP, GRIP_OPEN_DAEMON_WAIT, SETTLE_WAIT_SEC for name, attr in (("send_wait", "SEND_WAIT"), ("grip_wait", "GRIPPER_SLEEP"), ("grip_open_wait", "GRIP_OPEN_DAEMON_WAIT"), ("settle_wait", "SETTLE_WAIT_SEC")): v = getattr(args, name, None) if v is None: setattr(args, name, globals()[attr]) continue if not np.isfinite(v) or v < 0: sys.exit(f"[ERROR] --{name.replace('_', '-')} は 0 以上の数で指定する (指定: {v})") globals()[attr] = float(v) print(f"[WAIT ] 送ったあと {SEND_WAIT:.1f}s / 爪のあと {GRIPPER_SLEEP:.1f}s (開くときの daemon " f"{GRIP_OPEN_DAEMON_WAIT:.1f}s) / 止まるのを待つ目安 {SETTLE_WAIT_SEC:.1f}s") 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 から始まる仮の姿勢を動かす") # 待ち時間 (依頼 35). 既定 None = この中の値 (縮めた値). 前の値に戻すときに指定する ap.add_argument("--send-wait", type=float, default=None, help=f"送ったあと daemon が待つ秒 (既定 {SEND_WAIT:.1f}. 前は {SEND_WAIT_SEC:.1f}). " "0 より大きいと Mac の応答待ちの上限が この値 + 3 秒になる") ap.add_argument("--grip-wait", type=float, default=None, help=f"爪の開け閉めのあと Mac で待つ秒 (既定 {GRIPPER_SLEEP:.1f}. 前は 2.0)") ap.add_argument("--grip-open-wait", type=float, default=None, help=f"爪を開くとき daemon が待つ秒 (既定 {GRIP_OPEN_DAEMON_WAIT:.1f}. 前は 2.0)") ap.add_argument("--settle-wait", type=float, default=None, help=f"動きが止まるのを待つ目安の秒 (補正の 1 刻み・settle. 既定 {SETTLE_WAIT_SEC:.1f}. 前は 2.0)") args = ap.parse_args() if args.dry_keys and (args.execute or args.auto): sys.exit("[ERROR] --dry-keys はドライラン用 (--execute / --auto と一緒には使えない)") apply_wait_args(args) 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 h_last = history[-1] if (h_last["kind"] == "correction" and h_last.get("regrip") and not h_last["grip_changed"]): # R を使った補正 (依頼 35): 始めも終わりも閉. つかんだ区間と同じく 1 文字で聞く a = input(f" [REWIND] 補正 {h_last['correction_id']} では R でつかみ直しました. " "y=持っている（閉じたまま戻る） / " "n=落とした（先に開いてから戻る） / その他=やめる > ").strip().lower() if a == "y": grasp = GRASP_HELD elif a == "n": grasp = GRASP_DROPPED else: print(" [REWIND] やめました (アームは動かしていません)") continue break if h_last["grip_changed"]: 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, send_wait_sec=SEND_WAIT) rec["home_requested"] = True rec["home_arrived"] = bool(ok_home) log_f.write(json.dumps(rec) + "\n"); log_f.flush() if recorder is not None: recorder.stop() outcome = ask_outcome() break 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 and entry["kind"] == "chunk": 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 (手が写る) if "start_frame" in rw: 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"] or cr.get("regrip_used"): # codex R2-1: 補正でグリッパーを動かしたら, 開始時の開放は済んだことにする # (このあと最初の推論の実行で勝手に開いて物を落とさない. R も同じ) 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"] or cr["regrip_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"], "regrip": cr["regrip_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) gripper_cmd(0) 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, send_wait_sec=SEND_WAIT) sent_any = True sent_targets.append(np.asarray(tgt, dtype=float).copy()) if args.via_stride > 0: rec["vias"][vi].update(called=True, success=bool(ok), retried=bool(retried), actual_after=_fmt6(read_angles())) 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) gripper_cmd(grip_cmd) 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, send_wait_sec=SEND_WAIT) 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/rewind_correct.py（コミット 7aaf5df の時点） ```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 文字を操作に読み替える - correction_reach_tol / correction_joint_reached: 補正の 1 刻みが着いたかの判定 (動かした関節だけ) - correction_shortfall / correction_carry_used: 届かなかった分を次の 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) MODE_REGRIP = "regrip" # 補正の R (つかみ直し) で開いてから閉じ終わるまで. 学習データに使わない (2026-10-30 依頼 35). # r (締め直し) は correct のまま TRAIN_MODES = (MODE_MODEL, MODE_CORRECT) # 学習データに使う状態 (rewind / home / wait / regrip は使わない) # つかんだ区間 (グリッパーを閉じた推論 1 回分) を巻き戻すときの答え (log.jsonl の rewind 行の grasp_undo) GRASP_PLACED = "placed" # y = 持っている: つかんだ位置まで下ろして開き (置いて), 始めの姿勢へ戻る GRASP_DROPPED = "dropped" # n = 落とした: 今の位置ですぐ開いてから, 始めの姿勢へ戻る GRASP_HELD = "held" # R を使った補正の巻き戻しで y = 持っている: 閉じたまま逆にたどって始めの姿勢へ (依頼 35) # 補正の 1 刻みが「着いた」かの判定 (2026-10-30 依頼 34). 動かした関節だけを見て, # 刻みの半分以内なら着いた. ただし実機の読み値のぶれ (0.5〜1°. 185133/190659 の補正で # 負荷のない j0 の 1° 刻みは ずれ 0.25° 以内) より細かくはしない (0.5° 刻みでも 0.5°). # 推論の中継点・巻き戻し・HOME の判定 (3.0° / 2.5°) とは別 CORRECTION_REACH_MIN_TOL_DEG = 0.5 CORRECTION_HELP = ( " [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み\n" " [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉\n" " r = 爪を開かずに締め直す / R = 爪を開いて閉じ直す (つかみ直し). どちらも閉じているときだけ\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) / ("regrip", "tighten"|"reopen") / ("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 == "r": return ("regrip", "tighten") if ch == "R": return ("regrip", "reopen") 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 correction_reach_tol(step): """補正の 1 刻みの到着判定の幅 (度): 刻みの半分. ただし CORRECTION_REACH_MIN_TOL_DEG 以上. 例: 0.5° → 0.5° / 1° → 0.5° / 2° → 1.0°.""" return max(float(step) / 2.0, CORRECTION_REACH_MIN_TOL_DEG) def correction_joint_reached(measured, target, joint, tol): """動かした関節 joint だけで判定する (ほかの関節の小さな揺れは見ない). 戻り値: (着いたか, ずれ or None). measured が None (読めない) なら (False, None).""" if measured is None: return False, None err = abs(float(measured[joint]) - float(target[joint])) return err <= tol, round(err, 2) def correction_shortfall(measured, target, joint, sign, reached): """補正の 1 刻みで, 動かした向きに 足りなかった量 (度. 2026-10-30 依頼 35). 着いた (reached) / 読めない / 行き過ぎた ときは 0. 例: j1 を - に送って 目標 50.0 / 実測 51.5 → 1.5.""" if reached or measured is None: return 0.0 short = (float(target[joint]) - float(measured[joint])) * (1 if sign > 0 else -1) return round(short, 3) if short > 0 else 0.0 def correction_carry_used(carry, step): """次の 1 刻みに足す量. 上限は その刻みの大きさ (1 回で動く量は刻みの 2 倍まで).""" return min(max(float(carry), 0.0), float(step)) 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（コミット 7aaf5df の時点） ```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 (確認の入力待ち. 人がブロックを置き直すと手が写る) / regrip (補正の R で開いてから閉じ終わるまで. 2026-10-30 依頼 35) のステップと, 巻き戻しで取り消した区間 (区間を始めた時刻 〜 巻き戻しが終わった時刻) と, 取り消した補正 (補正を始めた時刻 〜 戻り終えた時刻) は除く. 除いたあとは「その手前」と「巻き戻したあとの続き」をつなぐ (戻った先は区間の始めの姿勢なので続く) - 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 (確認の入力待ち。手が写り得る) / " "regrip (補正の R で開いてから閉じ終わるまで) は除く", "- 巻き戻しで取り消した区間 (区間を始めた時刻 〜 巻き戻しが終わった時刻) と, 取り消した補正は除き,", " その手前と巻き戻したあとの続きをつなぐ", "- 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（コミット 7aaf5df の時点・テスト63件） ```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) と同じログになる (項目が増えるだけ) - 依頼 35: 待ちの引数 (前の値に戻せる) / 届かなかった分を次の刻みに足す / R を使った補正の巻き戻し (y / n) / regrip のコマを学習データから除く 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, correction_carry_used, correction_joint_reached, correction_reach_tol, correction_shortfall, 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.assertEqual(parse_correction_key("r"), ("regrip", "tighten")) # 依頼 34 self.assertEqual(parse_correction_key("R"), ("regrip", "reopen")) 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 # この名前で始まる送信では動かない self.offset = None # 送った目標からこれだけずれた所で止まる (依頼 34. 6 関節) 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 self.offset is not None: self.q = self.q + np.asarray(self.offset, 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, offset=None): world = FakeWorld() world.fail_after_label = fail_after_label world.stuck_prefix = stuck_prefix world.offset = offset 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) class TestRegripAndCorrectionReach(unittest.TestCase): """依頼 34: 補正の r (締め直し) / R (つかみ直し) と, 補正の 1 刻みの「着いた」の判定.""" EXEC = ["--execute", "--steps-per-chunk", "10", "--via-stride", "3", "--settle-retries", "0", "--ignore-framing", "--max-chunks", "6"] def setUp(self): self.td = Path(tempfile.mkdtemp()) time.sleep(1.1) def test_reach_tol_values(self): self.assertEqual(correction_reach_tol(1.0), 0.5) self.assertEqual(correction_reach_tol(2.0), 1.0) self.assertEqual(correction_reach_tol(0.5), 0.5) # 半分 (0.25°) は読み値のぶれより細かいので 0.5° self.assertEqual([correction_reach_tol(x) for x in CORRECTION_STEPS], [0.5, 0.5, 1.0]) def test_joint_reached_only_moved_joint(self): tgt = [5.0, 10.0, 0, 0, 0, -30] self.assertEqual(correction_joint_reached([5.4, 12.0, 1, 1, 1, -29], tgt, 0, 0.5), (True, 0.4)) self.assertEqual(correction_joint_reached([5.6, 10.0, 0, 0, 0, -30], tgt, 0, 0.5), (False, 0.6)) self.assertEqual(correction_joint_reached(None, tgt, 0, 0.5), (False, None)) def test_r_and_R_records(self): """g で閉じる → r → R: r は閉じる指令をもう一度だけ, R は 開く → 閉じる. 記録に前後の状態.""" code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys="grR\n") self.assertEqual(code, 0) v = ("v", rpc.GRIP_VALUE) self.assertEqual(world.grips[:4], [v, v, 0, v]) # g / r / R (開く → 閉じる) corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["gripper", "regrip_tighten", "regrip_reopen", "accept"]) r, R = corr[1], corr[2] self.assertEqual((r["key"], r["before"], r["after"], r["sent"]), ("r", 1, 1, True)) self.assertEqual((R["key"], R["before"], R["after"], R["sent"]), ("R", 1, 1, True)) self.assertIn("t", r) self.assertEqual(corr[-1]["grip_after"], 1) 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_r_and_R_dry_keys(self): """--execute なし (--dry-keys) でも r / R が流れる (アームにも daemon にも送らない).""" code, world, run, lines, out = run_client( rpc, ["--dry-keys", "--steps-per-chunk", "10", "--max-chunks", "3"], ["c", "", "q"], self.td, keys="rgrR1+\n") self.assertEqual(code, 0) self.assertEqual((world.grips, world.sends), ([], [])) corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["regrip_tighten", "gripper", "regrip_tighten", "regrip_reopen", "move", "accept"]) self.assertEqual([x["sent"] for x in corr[:4] if x["action"] != "gripper"], [False, True, True]) self.assertTrue(corr[4]["joint_reached"]) self.assertIn("グリッパー→開 (送らない)", out) def test_r_when_open_does_nothing(self): code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys="rR\n") self.assertEqual(code, 0) self.assertNotIn(("v", rpc.GRIP_VALUE), world.grips) # 閉じていない self.assertIn("爪が開いているので締め直しはしない（閉じるのは g）", out) corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["regrip_tighten", "regrip_reopen", "accept"]) self.assertTrue(all(not x["sent"] and x["skipped"] == "gripper_open" and x["before"] == 0 for x in corr[:2])) self.assertFalse(corr[-1]["grip_used"]) def test_cancel_after_R_restores_start_gripper(self): """開いた状態で入って g → R → b (取り消し): グリッパーは始めの「開」へ戻る.""" code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys="gR1+b") cancel = [x for x in lines if x.get("action") == "cancel"][0] self.assertTrue(cancel["finished"]) self.assertEqual(cancel["grip_after"], 0) v = ("v", rpc.GRIP_VALUE) self.assertEqual(world.grips[:4], [v, 0, v, 0]) def moves(self, keys, offset=None, stuck=None): code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys=keys, offset=offset, stuck_prefix=stuck) self.assertEqual(code, 0) return [x for x in lines if x.get("action") == "move"], out def test_reach_ignores_other_joints(self): mv, out = self.moves("1+\n", offset=[0.3, 0.9, 0, 0, 0, 0.8]) # j0 を動かす. ほかは 0.9° ずれ self.assertTrue(mv[0]["joint_reached"]) self.assertEqual((mv[0]["joint_err_deg"], mv[0]["reach_tol_deg"]), (0.3, 0.5)) self.assertTrue(mv[0]["settled"]) self.assertIn("着いた", out) def test_reach_tol_by_step(self): mv, out = self.moves("1+]+[[+\n", offset=[0.7, 0, 0, 0, 0, 0]) # 1° / 2° / 0.5° の刻みで 0.7° 足りない self.assertEqual([x["step_deg"] for x in mv], [1.0, 2.0, 0.5]) self.assertEqual([x["reach_tol_deg"] for x in mv], [0.5, 1.0, 0.5]) self.assertEqual([x["joint_reached"] for x in mv], [False, True, False]) self.assertTrue(all(x["arrived"] for x in mv)) # 送りの判定 (3°) は今までどおり着いた self.assertIn("届いていない", out) def test_not_reached_continues(self): """動かないときも「届いていない」と出して補正を続ける (止めない・新しい確認なし).""" mv, out = self.moves("1+++\n", stuck="pi05_corr") self.assertEqual(len(mv), 3) self.assertTrue(all(x["sent"] and not x["joint_reached"] for x in mv)) self.assertEqual(out.count("届いていない"), 3) self.assertIn("補正 1 を終えました", out) # ---- 依頼 35: 待ちを縮める / 届かなかった分を次の刻みに足す / R の補正も巻き戻す / regrip のコマ ---------- class ShortArm(rpc.ArmIO): """送った向きに frac だけしか動かない仮の腕. 送りの判定 (3°) は「着いた」を返す (実機の j1 持ち上げと同じ).""" def __init__(self, *a, frac=0.25, ok=True, **k): super().__init__(*a, **k) self.frac, self.ok = frac, ok def send(self, target, tag): t = np.asarray(target, dtype=float) self.sim_q = self.sim_q + (t - self.sim_q) * (1.0 - self.frac) return self.ok class TestCarry(unittest.TestCase): START = [0.0, 50.0, 0, 0, 0, -30] def run_keys(self, keys, frac=0.25, ok=True): arm = ShortArm(False, None, sim_q=self.START, frac=frac, ok=ok) log = io.StringIO() with mock.patch("sys.stdout", new=io.StringIO()) as out: res, _ = rpc.run_correction(arm, Keys(keys), LO, HI, 20.0, 0, 1, 0, log, None) mv = [json.loads(x) for x in log.getvalue().splitlines()] return [m for m in mv if m["action"] == "move"], out.getvalue() def test_shortfall_and_cap(self): self.assertEqual(correction_shortfall([0, 51.5, 0, 0, 0, 0], [0, 50.0, 0, 0, 0, 0], 1, -1, False), 1.5) self.assertEqual(correction_shortfall([0, 51.5, 0, 0, 0, 0], [0, 50.0, 0, 0, 0, 0], 1, -1, True), 0.0) self.assertEqual(correction_shortfall([0, 49.0, 0, 0, 0, 0], [0, 50.0, 0, 0, 0, 0], 1, -1, False), 0.0) self.assertEqual(correction_shortfall(None, [0, 50.0, 0, 0, 0, 0], 1, -1, False), 0.0) self.assertEqual(correction_shortfall([0, 48.5, 0, 0, 0, 0], [0, 50.0, 0, 0, 0, 0], 1, +1, False), 1.5) self.assertEqual(correction_carry_used(1.5, 2.0), 1.5) self.assertEqual(correction_carry_used(3.0, 2.0), 2.0) # 上限は刻みの大きさ self.assertEqual(correction_carry_used(0.0, 1.0), 0.0) def test_no_carry_when_reached(self): """2° 刻みで毎回 0.5° 足りない → 判定 (刻みの半分 1.0°) では「着いた」なので足さない.""" mv, out = self.run_keys("2]---\n") self.assertEqual([m["joint_reached"] for m in mv], [True, True, True]) self.assertEqual([m["carry_deg"] for m in mv], [0.0, 0.0, 0.0]) self.assertEqual([m["after"][1] for m in mv], [48.0, 46.0, 44.0]) self.assertNotIn("足りなかった", out) def test_carry_example_and_cap(self): """届かない量が刻みより大きくても, 足すのは刻みの大きさまで (1 回で動く量は刻みの 2 倍まで).""" mv, out = self.run_keys("2]---\n", frac=0.75) # 1 回目 50→48 の指令で 1.5° しか動かない → 足りない 1.5 → 2 回目は 48 - 2 - 1.5 = 44.5 self.assertEqual(mv[0]["after"][1], 48.0) self.assertEqual(mv[1]["carry_deg"], 1.5) self.assertEqual(mv[1]["after"][1], 44.5) self.assertIn("足りなかった 1.5° を足して送る", out) mv, _ = self.run_keys("--\n", frac=1.0) # 動かない. 刻み 1° self.assertEqual([m["carry_deg"] for m in mv], [0.0, 1.0]) mv, _ = self.run_keys("---\n", frac=1.0) self.assertEqual([m["carry_deg"] for m in mv], [0.0, 1.0, 1.0]) # 足りない 2° でも足すのは 1° self.assertTrue(all(abs(m["delta"]) <= 2.0 + 1e-9 for m in mv)) def test_carry_stops(self): mv, _ = self.run_keys("2]--+\n", frac=0.75) # 向きを変えた self.assertEqual([m["carry_deg"] for m in mv], [0.0, 1.5, 0.0]) mv, _ = self.run_keys("2]-1-2-\n", frac=0.75) # ほかの関節を選んで戻った self.assertEqual([m["joint"] for m in mv], [1, 0, 1]) self.assertEqual([m["carry_deg"] for m in mv], [0.0, 0.0, 0.0]) mv, _ = self.run_keys("2]--\n", frac=0.0) # 届いた self.assertEqual([m["carry_deg"] for m in mv], [0.0, 0.0]) mv, _ = self.run_keys("2]--\n", frac=0.75, ok=False) # 未到達 (3°) → 実測から数え直す. 足さない self.assertEqual(mv[1]["carry_deg"], 0.0) self.assertEqual(mv[1]["after"][1], 49.5 - 2.0) # 1 回目は 1.5° しか動かず実測 49.5 def test_carry_rechecked_before_send(self): """codex 依頼 35: 打ち切って測った不足が, 次に押すまでに解消していたら足さない.""" class LateArm(ShortArm): def read(self): q = super().read() if getattr(self, "arrive_late", False) and self.last is not None: self.sim_q = self.last.copy(); q = self.sim_q.copy() # 入力を待つ間に目標へ着いた return q def send(self, target, tag): self.last = np.asarray(target, dtype=float).copy() return super().send(target, tag) def read_settled(self): q, st = super().read_settled() self.arrive_late = True return q, False arm = LateArm(False, None, sim_q=self.START, frac=0.75) arm.last = None log = io.StringIO() with mock.patch("sys.stdout", new=io.StringIO()): rpc.run_correction(arm, Keys("2]--\n"), LO, HI, 20.0, 0, 1, 0, log, None) mv = [json.loads(x) for x in log.getvalue().splitlines() if '"move"' in x] self.assertFalse(mv[0]["joint_reached"]) # 打ち切った時点では届いていない self.assertEqual(mv[1]["carry_deg"], 0.0) # 送る直前には着いていたので足さない self.assertEqual(mv[1]["after"][1], 46.0) def test_carry_still_limited(self): """足した目標も apply_limits / check_send_safe を通る (安全範囲の端で止まる).""" arm = ShortArm(False, None, sim_q=[0.0, 87.0, 0, 0, 0, -30], frac=0.75) log = io.StringIO() with mock.patch("sys.stdout", new=io.StringIO()): rpc.run_correction(arm, Keys("2]++\n"), LO, HI, 20.0, 0, 1, 0, log, None) mv = [json.loads(x) for x in log.getvalue().splitlines() if '"move"' in x] self.assertEqual(mv[0]["after"][1], 88.0) # 87 + 2 → 88 (範囲の端) にクランプ self.assertTrue(all(m["after"][1] <= 88.0 for m in mv)) class TestWaitArgs(unittest.TestCase): NAMES = ("SEND_WAIT", "GRIPPER_SLEEP", "GRIP_OPEN_DAEMON_WAIT", "SETTLE_WAIT_SEC") def apply(self, argv): ap = argparse.Namespace(send_wait=None, grip_wait=None, grip_open_wait=None, settle_wait=None) for k, v in argv.items(): setattr(ap, k, v) with mock.patch("sys.stdout", new=io.StringIO()): rpc.apply_wait_args(ap) return ap, tuple(getattr(rpc, n) for n in self.NAMES) def setUp(self): self.patches = [mock.patch.object(rpc, n, getattr(rpc, n)) for n in self.NAMES] for p in self.patches: p.start() def tearDown(self): for p in self.patches: p.stop() def test_defaults_shortened(self): ap, vals = self.apply({}) self.assertEqual(vals, (0.0, 0.8, 0.0, 0.8)) self.assertEqual((ap.send_wait, ap.grip_wait, ap.grip_open_wait, ap.settle_wait), (0.0, 0.8, 0.0, 0.8)) def test_restore_previous_values(self): _, vals = self.apply({"send_wait": 1.0, "grip_wait": 2.0, "grip_open_wait": 2.0, "settle_wait": 2.0}) self.assertEqual(vals, (1.0, 2.0, 2.0, 2.0)) self.assertEqual(rpc.SEND_WAIT_SEC, 1.0) # 本線の定数は変えていない # 前の値が送り口とグリッパーまで届く sw, gr, sl = [], [], [] with mock.patch.object(rpc, "_send_and_wait_arrival", lambda *a, **k: (sw.append(k), (True, False))[1]), \ mock.patch.object(rpc, "daemon_set_gripper", lambda s, **k: gr.append((s, k))), \ mock.patch.object(rpc, "daemon_set_gripper_value", lambda v, **k: gr.append(("v", v))), \ mock.patch.object(rpc.time, "sleep", lambda x: sl.append(x)), \ mock.patch("sys.stdout", new=io.StringIO()): arm = rpc.ArmIO(True, None) arm.send([0, 0, 0, 0, 0, -30], "x") arm.gripper(0) arm.gripper(1) self.assertEqual(sw, [{"send_wait_sec": 1.0}]) self.assertEqual(gr, [(0, {"wait": 2.0}), ("v", rpc.GRIP_VALUE)]) self.assertEqual(sl, [2.0, 2.0]) def test_new_values_reach_senders(self): self.apply({}) sw, gr, sl, ds = [], [], [], [] with mock.patch.object(rpc, "_send_and_wait_arrival", lambda *a, **k: (sw.append(k), (True, False))[1]), \ mock.patch.object(rpc, "daemon_set_gripper", lambda s, **k: gr.append((s, k))), \ mock.patch.object(rpc, "daemon_send_angles", lambda *a, **k: ds.append(k)), \ mock.patch.object(rpc, "read_until_still", side_effect=[(np.array([0, 2.0, 0, 0, 0, -30]), True, None), (np.array([0, 0.0, 0, 0, 0, -30]), True, None)]), \ mock.patch.object(rpc.time, "sleep", lambda x: sl.append(x)), \ mock.patch("sys.stdout", new=io.StringIO()): arm = rpc.ArmIO(True, None) arm.send([0, 0, 0, 0, 0, -30], "x") arm.gripper(0) rpc.settle_target([0, 0, 0, 0, 0, -30], "x", 1.0, 2) self.assertEqual(sw, [{"send_wait_sec": 0.0}]) self.assertEqual(gr, [(0, {"wait": 0.0})]) self.assertEqual(sl, [0.8]) self.assertEqual(ds, [{"speed": rpc.SPEED, "wait": 0.0}]) def test_settle_wait_used(self): self.apply({"settle_wait": 0.3}) reads = iter([np.zeros(6) + k for k in range(100)]) # 動き続ける (止まらない) with mock.patch.object(rpc, "read_angles", lambda: next(reads)), \ mock.patch.object(rpc, "SETTLE_POLL_SEC", 0.05): t0 = time.time() q, still, _ = rpc.read_until_still() dt = time.time() - t0 self.assertFalse(still) self.assertLess(dt, 0.6) def test_negative_rejected(self): with self.assertRaises(SystemExit): self.apply({"grip_wait": -1.0}) class TestRegripRewind(unittest.TestCase): """R だけを使った補正を b で巻き戻す (y / n). g を往復した補正は今までどおり断る.""" EXEC = ["--execute", "--steps-per-chunk", "10", "--max-chunks", "6", "--settle-retries", "0", "--ignore-framing"] def setUp(self): self.td = Path(tempfile.mkdtemp()) time.sleep(1.1) def run_(self, inputs, keys): return run_client(rpc, self.EXEC, inputs, self.td, keys=keys) def test_rewind_R_correction_y(self): # 補正 1 で g (閉じる) / 補正 2 で R と j1 +2 刻み → b → y code, world, run, lines, out = self.run_(["c", "c", "b", "y", "q", ""], "g\nR2++\n") self.assertEqual(code, 0) v = ("v", rpc.GRIP_VALUE) self.assertEqual(world.grips, [v, 0, v]) # g / R (開く → 閉じる). 巻き戻しで開かない corr = [x for x in lines if x.get("action") == "accept"] self.assertEqual((corr[1]["grip_used"], corr[1]["regrip_used"]), (False, True)) rw = [x for x in lines if x.get("type") == "rewind"] self.assertEqual(len(rw), 1) self.assertEqual((rw[0]["kind"], rw[0]["correction_id"], rw[0]["grasp_undo"]), ("correction", 2, "held")) self.assertTrue(rw[0]["finished"]) self.assertFalse(rw[0]["gripper_opened"]) back = [s[1] for s in world.sends if s[0].startswith("pi05_rewind")] self.assertEqual(back[-1], corr[1]["start_pose"]) # 補正 2 を始めた姿勢へ戻る ch = [x for x in lines if x.get("type") is None and "chunk" in x] self.assertEqual(ch[-1]["state"][6], 1.0) # 閉じたまま self.assertNotIn("落としたので", out) def test_rewind_R_correction_n(self): code, world, run, lines, out = self.run_(["c", "c", "b", "n", "", "q", ""], "g\nR2++\n") self.assertEqual(code, 0) v = ("v", rpc.GRIP_VALUE) self.assertEqual(world.grips, [v, 0, v, 0]) # 巻き戻しの前にその場で開く k_open = max(k for k, e in enumerate(world.events) if e == ("grip", 0)) self.assertTrue(all(e[0] == "send" and e[1].startswith("pi05_rewind") for e in world.events[k_open + 1:])) # 開いてから戻る rw = [x for x in lines if x.get("type") == "rewind"][0] self.assertEqual(rw["grasp_undo"], "dropped") self.assertTrue(rw["gripper_opened"] and rw["finished"]) self.assertNotIn("start_frame", rw) # 置き直す間は wait (つかんだ区間の n と同じ) 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"]) ch = [x for x in lines if x.get("type") is None and "chunk" in x] self.assertEqual(ch[-1]["state"][6], 0.0) def test_rewind_R_correction_other_cancels(self): code, world, run, lines, out = self.run_(["c", "c", "b", "x", "q", ""], "g\nR2++\n") self.assertEqual([x for x in lines if x.get("type") == "rewind"], []) self.assertIn("やめました", out) def test_g_round_trip_still_refused(self): """g を使った補正 (R も使っていても) は今までどおり巻き戻さない.""" code, world, run, lines, out = self.run_(["c", "b", "q", ""], "gR2+\n") self.assertEqual([x for x in lines if x.get("type") == "rewind"], []) self.assertIn("でグリッパーを動かしたので巻き戻しません", out) def test_dry_keys_r_R_b(self): """--dry-keys でも r・R・b (y) が流れる (アームにも daemon にも送らない).""" code, world, run, lines, out = run_client( rpc, ["--dry-keys", "--steps-per-chunk", "10", "--max-chunks", "6"], ["c", "c", "b", "y", "q"], self.td, keys="g\nrR2-]-\n") self.assertEqual(code, 0) self.assertEqual((world.grips, world.sends), ([], [])) corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["gripper", "accept", "regrip_tighten", "regrip_reopen", "move", "move", "accept"]) self.assertEqual([x["carry_deg"] for x in corr if x["action"] == "move"], [0.0, 0.0]) rw = [x for x in lines if x.get("type") == "rewind"] self.assertEqual([(x["correction_id"], x["grasp_undo"], x["finished"]) for x in rw], [(2, "held", True)]) def test_regrip_frames_marked_and_excluded(self): code, world, run, lines, out = self.run_(["c", "q", ""], "grR\n") stages = [l.split(",")[2] for l in (run / "recording" / "states.csv").read_text().splitlines()[1:] if l.split(",")[1] == "stage"] self.assertEqual(stages.count("regrip"), 1) # R だけ. r は correct のまま k = stages.index("regrip") self.assertEqual(stages[k - 1:k + 2], ["correct", "regrip", "correct"]) # R の開く指令と閉じる指令は regrip の間に入る rows = [l.split(",") for l in (run / "recording" / "states.csv").read_text().splitlines()[1:]] ts_stage = [(float(r[0]), r[2]) for r in rows if r[1] == "stage"] t_in = [t for t, m in ts_stage if m == "regrip"][0] t_out = min(t for t, m in ts_stage if t > t_in) g_in = [(float(r[0]), r[10]) for r in rows if r[1] == "gripper_cmd" and t_in <= float(r[0]) <= t_out] self.assertEqual([g for _, g in g_in], ["0", "1"]) # build_correction_episodes の除き方: regrip は学習に使わない (wait / rewind と同じ) keep, m = step_keep_mask([0.5, 1.5, 2.5], [0.0, 1.0, 2.0], ["correct", "regrip", "correct"], []) self.assertEqual(list(keep), [True, False, True]) if __name__ == "__main__": unittest.main() ``` ### 4回目のターミナルの出力 #### lab_start.sh の通し試験（アームなし）lab/test_lab_20261030/lab_start_20261030_203624.txt ``` [LAB ] 2026-10-30 20:36:24 GPU=NVIDIA A40 episode=run_0001_20261019_194818 log=/Volumes/KIOXIA/r2r2r-data/lab/test_lab_20261030 ドライラン (アームなし) === 1. Pod を作る [POD ] 作った: id q38suh09bciokm GPU=NVIDIA A40 1時間 0.59 ドル (記録 /Volumes/KIOXIA/r2r2r-data/lab/test_lab_20261030/pod/lab_pod.json) (2 秒) === 2. Pod が立ち上がるまで待つ [POD ] SSH の IP とポートを API から受け取った (:) [POD ] 入れた: eec2ad0d6aef NVIDIA A40, 46068 MiB (17 秒) === 3a. スクリプトを送る (33 秒) === 3b. huggingface_hub を入れ, 鍵を渡す WARNING: Running pip as the 'root' user can result in broken permissions and conflicting behaviour with the system package manager, possibly rendering your system unusable. It is recommended to use a virtual environment instead: https://pip.pypa.io/warnings/venv. Use the --root-user-action option if you know what you are doing and want to suppress this warning. Hint: The `hf-cli` skill is not installed. Run `hf skills add -g` to teach your AI agents how to use the `hf` CLI. ✓ Logged in user: PixelstoMotors (10 秒) === 3c. モデルを取る (hf download) Fetching 33 files: 0%| | 0/33 [00:00 [DONE ] 記録: /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_203859 Traceback (most recent call last): File "/Users/shimazueiichi/r2r2r-scripts/pi05/run_policy_client.py", line 1509, in sys.exit(main()) ^^^^^^ File "/Users/shimazueiichi/r2r2r-scripts/pi05/run_policy_client.py", line 1187, in main ans = input(" Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / " ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ EOFError: EOF when reading a line [CLIENT] run_policy_client.py が終わった (終了コード 1) (62 秒) === かかった時間 1. Pod を作る 2 秒 2. Pod が立ち上がるまで待つ 17 秒 3a. スクリプトを送る 33 秒 3b. huggingface_hub を入れ, 鍵を渡す 10 秒 3c. モデルを取る (hf download) 12 秒 3d. openpi を取る (215abfb) 14 秒 3e. uv sync 21 秒 3f. addon・設定・assets 2 秒 4. サーバーの起動 40 秒 5. Mac の 8000 番へのトンネル 3 秒 8. 推論のクライアント (ドライラン・--dry-keys. アームには触らない) 62 秒 [LAB ] Pod は残しています (id q38suh09bciokm). 片付けは ./lab_stop.sh --log test_lab_20261030 [TUNNEL] 8000 番のトンネルを閉じました ``` #### lab_stop.sh（通し試験）lab/test_lab_20261030/lab_stop_20261030_204009.txt ``` [STOP ] test_lab_20261030 の Pod: id q38suh09bciokm GPU=NVIDIA A40 作った時刻 2026-10-30 20:36:26 [LOG ] serve.log を持ち帰った → /Volumes/KIOXIA/r2r2r-data/lab/test_lab_20261030/serve.log (21 行) [STOP ] Terminate を送った (20:40:17) [STOP ] Pod (id q38suh09bciokm) が消えたことを API で確かめた [COST ] 借りていた時間 3 分 51 秒 (0.06 時間) × 1時間 0.59 ドル = 約 0.04 ドル (約 6 円. 1ドル=158円) [TUNNEL] 8000 番のトンネルは残っていません ``` #### lab_start.sh（4回目）lab/Log16/lab_start_20261030_210435.txt ``` [LAB ] 2026-10-30 21:04:35 GPU=NVIDIA A40 episode=run_0001_20261019_194818 log=/Volumes/KIOXIA/r2r2r-data/lab/Log17 === 1. Pod を作る [POD ] 作った: id m8is5nqx61gofi GPU=NVIDIA A40 1時間 0.59 ドル (記録 /Volumes/KIOXIA/r2r2r-data/lab/Log17/pod/lab_pod.json) (3 秒) === 2. Pod が立ち上がるまで待つ [POD ] SSH の IP とポートを API から受け取った (:) [POD ] 入れた: 15f5e69fe17f NVIDIA A40, 46068 MiB (38 秒) === 3a. スクリプトを送る (32 秒) === 3b. huggingface_hub を入れ, 鍵を渡す WARNING: Running pip as the 'root' user can result in broken permissions and conflicting behaviour with the system package manager, possibly rendering your system unusable. It is recommended to use a virtual environment instead: https://pip.pypa.io/warnings/venv. Use the --root-user-action option if you know what you are doing and want to suppress this warning. Hint: The `hf-cli` skill is not installed. Run `hf skills add -g` to teach your AI agents how to use the `hf` CLI. ✓ Logged in user: PixelstoMotors (10 秒) === 3c. モデルを取る (hf download) Fetching 33 files: 0%| | 0/33 [00:00=0.20) q [DONE ] 終了 (何も書いていません) (43 秒) === 8. 実機推論 (1 回目) [WAIT ] 送ったあと 0.0s / 爪のあと 0.8s (開くときの daemon 0.0s) / 止まるのを待つ目安 0.8s [SAFE ] 安全範囲 (学習 19 本 ±10°): j0[-72,+55] j1[-9,+88] j2[-46,+41] j3[-18,+27] j4[-90,+10] j5[-51,-14] [SAFE ] 肩 j1 の上限は J1_SAFE_MAX=88° で切った (これを超える指令はアームが黙って捨てる / LOG352) 元データ: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes [MODE ] EXECUTE steps/chunk=10 max_move=20.0° log=/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_210821 [CAMERA] 写り方 dy=-4.7 dx=+10.2 response=0.561 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [CAMERA] 基準: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes/run_0001_20261019_183454/frames/000000.jpg [POLICY] 接続 ws://localhost:8000 metadata={} [RECORD] 上カメラと実測角を 1 秒 10 コマで記録します → /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_210821/recording [GRIP ] 開始時のグリッパー開放は, 最初の目標が検査を通ってから行います (爪が閉じていたら手で開けておく) [STATE ] 実測を渡す (直前に送った目標がない) [000] 推論 52330ms 10ステップ先 Δ= +4.0 +17.2 +11.1 -2.4 -25.9 +2.6 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [RECORD] 記録終了 コマ 456 (8.681 fps) 実測角 525 回 (9.99 Hz) → /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_210821/recording 結果: y=成功 / n=失敗 / Enter=記録しない > [DONE ] 記録: /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_210821 [CLIENT] run_policy_client.py が終わった (終了コード 0) (57 秒) もう1回やりますか? y=やる / 他=終わる > y === 7. HOME → 爪を開く → 位置合わせ (2 回目) [INIT] daemon 接続中... [WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment [INIT] fresh_mode=1 [PRE] 現在姿勢: [-0.35, 1.14, 0.79, -0.17, -0.26, -29.70] [SEND] HOME へ移動指令 [WAIT] t=2.0s max_err=1.14° [DONE] HOME 到達 max_err=1.14° [POST] 最終姿勢: [-0.35, 1.14, 0.79, -0.17, -0.26, -29.70] [WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment [EXIT] fresh_mode=0, socket close [Pi] GRIPPER: OPEN result= -1 [REF ] 基準画像: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes/run_0001_20261019_194818/frames/000000.jpg [REF ] お手本 run_0001_20261019_194818 (つかんだ瞬間の j0 = +13.2°) [REF ] 重なった画像のブロックに実物のブロックを合わせると, お手本と同じ位置に置ける [ALIGN ] |dy| <= 20 なら OK. アームは HOME に置いた状態で合わせる. q または Esc で終了 (ファイルには何も書かない) [ALIGN ] dy= +1.7 dx= +3.3 response=0.890 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [DONE ] 終了 (何も書いていません) (23 秒) === 8. 実機推論 (2 回目) [WAIT ] 送ったあと 0.0s / 爪のあと 0.8s (開くときの daemon 0.0s) / 止まるのを待つ目安 0.8s [SAFE ] 安全範囲 (学習 19 本 ±10°): j0[-72,+55] j1[-9,+88] j2[-46,+41] j3[-18,+27] j4[-90,+10] j5[-51,-14] [SAFE ] 肩 j1 の上限は J1_SAFE_MAX=88° で切った (これを超える指令はアームが黙って捨てる / LOG352) 元データ: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes [MODE ] EXECUTE steps/chunk=10 max_move=20.0° log=/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_210954 [CAMERA] 写り方 dy=-4.7 dx=+10.2 response=0.562 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [CAMERA] 基準: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes/run_0001_20261019_183454/frames/000000.jpg [POLICY] 接続 ws://localhost:8000 metadata={} [RECORD] 上カメラと実測角を 1 秒 10 コマで記録します → /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_210954/recording [GRIP ] 開始時のグリッパー開放は, 最初の目標が検査を通ってから行います (爪が閉じていたら手で開けておく) [STATE ] 実測を渡す (直前に送った目標がない) [000] 推論 620ms 10ステップ先 Δ= +5.1 +16.6 +10.9 -1.8 -23.5 +2.2 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [GRIP ] 開始時にグリッパーを開く [Pi] GRIPPER: OPEN result= -1 [SEND] pi05_chunk000_via02: send_angles (single) [ +0.70, +5.01, +3.03, -0.23, -5.81, -29.41] speed=20 wait=0.0s (pre max|Δ|=5.55°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.04° (tol 3.0°) [VIA ] step 2 Δ= +1.1 +3.9 +2.2 -0.1 -5.5 +0.3 着いた [SEND] pi05_chunk000_via05: send_angles (single) [ +2.16, +10.53, +6.59, -0.95, -13.96, -28.65] speed=20 wait=0.0s (pre max|Δ|=10.19°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.22° (tol 3.0°) [VIA ] step 5 Δ= +1.7 +4.6 +2.6 -0.7 -10.2 +1.0 着いた [SEND] pi05_chunk000_via08: send_angles (single) [ +3.83, +15.79, +10.54, -1.84, -21.40, -27.79] speed=20 wait=0.0s (pre max|Δ|=8.66°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.01° (tol 3.0°) [VIA ] step 8 Δ= +2.2 +4.5 +3.1 -1.1 -8.7 +1.6 着いた [SEND] pi05_chunk000_via09: send_angles (single) [ +4.71, +17.72, +11.69, -1.96, -23.74, -27.50] speed=20 wait=0.0s (pre max|Δ|=3.35°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.91° (tol 3.0°) [VIA ] step 9 Δ= +1.5 +1.0 +0.4 -0.2 -3.3 +1.1 着いた [STATE ] 目標を渡す 目標と実測の最大差 0.91°（関節 j1） [001] 推論 594ms 10ステップ先 Δ= +5.0 +17.5 +11.7 -0.3 -26.5 +1.1 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk001_via02: send_angles (single) [ +6.16, +23.25, +15.33, -2.54, -31.39, -27.00] speed=20 wait=0.0s (pre max|Δ|=7.66°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.00° (tol 3.0°) [VIA ] step 2 Δ= +1.6 +4.6 +3.0 +0.0 -7.7 +0.9 着いた [SEND] pi05_chunk001_via05: send_angles (single) [ +7.75, +29.04, +19.24, -2.71, -39.92, -26.88] speed=20 wait=0.0s (pre max|Δ|=9.43°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.01° (tol 3.0°) [VIA ] step 5 Δ= +1.9 +4.8 +3.2 -0.1 -9.4 +0.4 着いた [SEND] pi05_chunk001_via08: send_angles (single) [ +9.02, +34.94, +23.32, -2.84, -48.15, -26.85] speed=20 wait=0.0s (pre max|Δ|=8.96°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.91° (tol 3.0°) [VIA ] step 8 Δ= +1.7 +4.9 +3.5 -0.3 -9.0 +0.3 着いた [SEND] pi05_chunk001_via09: send_angles (single) [ +9.53, +36.11, +23.95, -2.85, -50.23, -26.81] speed=20 wait=0.0s (pre max|Δ|=2.69°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.97° (tol 3.0°) [VIA ] step 9 Δ= +0.8 +0.3 +0.2 -0.2 -2.7 +0.3 着いた [STATE ] 目標を渡す 目標と実測の最大差 0.97°（関節 j1） [002] 推論 377ms 10ステップ先 Δ= +2.3 +9.5 +6.2 -0.5 -13.0 +0.3 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk002_via02: send_angles (single) [ +11.02, +42.36, +28.10, -3.31, -59.36, -26.78] speed=20 wait=0.0s (pre max|Δ|=8.92°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.88° (tol 3.0°) [VIA ] step 2 Δ= +1.4 +5.3 +4.4 -0.7 -8.9 +0.2 着いた [SEND] pi05_chunk002_via05: send_angles (single) [ +11.76, +46.06, +30.41, -3.14, -63.38, -26.77] speed=20 wait=0.0s (pre max|Δ|=4.32°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [VIA ] step 5 Δ= +1.0 +2.8 +2.3 -0.2 -4.3 +0.2 着いた [VIA ] step 8 Δ= +0.0 -0.5 -0.4 -0.3 +0.5 +0.2 送らず (0.5° 以下) [SEND] pi05_chunk002_via09: send_angles (single) [ +11.93, +46.57, +29.90, -3.14, -63.46, -26.73] speed=20 wait=0.0s (pre max|Δ|=0.77°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.80° (tol 3.0°) [VIA ] step 9 Δ= +0.1 -0.4 -0.8 -0.3 +0.5 +0.2 着いた [STATE ] 目標を渡す 目標と実測の最大差 0.80°（関節 j1） [003] 推論 374ms 10ステップ先 Δ= +0.3 +10.7 -10.0 -0.3 +1.5 +0.0 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk003_via02: send_angles (single) [ +12.03, +51.35, +26.68, -3.07, -63.83, -26.75] speed=20 wait=0.0s (pre max|Δ|=3.99°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.56° (tol 3.0°) [VIA ] step 2 Δ= +0.2 +4.0 -4.0 -0.3 +0.1 +0.1 着いた [SEND] pi05_chunk003_via05: send_angles (single) [ +12.21, +55.01, +23.60, -3.10, -63.02, -26.78] speed=20 wait=0.0s (pre max|Δ|=3.82°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.22° (tol 3.0°) [VIA ] step 5 Δ= +0.3 +3.0 -3.8 -0.1 +2.4 -0.9 着いた [SEND] pi05_chunk003_via08: send_angles (single) [ +12.14, +57.66, +21.24, -3.16, -62.51, -26.84] speed=20 wait=0.0s (pre max|Δ|=3.45°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.94° (tol 3.0°) [VIA ] step 8 Δ= +0.2 +1.6 -3.4 -0.2 +1.7 -0.8 着いた [SEND] pi05_chunk003_via09: send_angles (single) [ +12.13, +58.10, +20.69, -3.15, -62.44, -26.86] speed=20 wait=0.0s (pre max|Δ|=1.45°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.28° (tol 3.0°) [VIA ] step 9 Δ= +0.2 -0.3 -1.4 -0.2 +1.0 -0.7 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.28°（関節 j2） [004] 推論 651ms 5ステップ先 Δ= +0.1 +1.2 -3.0 -0.2 +1.3 -0.4 グリッパー→閉 [VIA ] 中継点 2 点: step 2, 4 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk004_via02: send_angles (single) [ +12.11, +60.48, +18.81, -3.20, -61.87, -26.89] speed=20 wait=0.0s (pre max|Δ|=3.16°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.14° (tol 3.0°) [VIA ] step 2 Δ= +0.2 +1.5 -3.2 -0.2 +1.2 -0.3 着いた [SEND] pi05_chunk004_via04: send_angles (single) [ +12.07, +60.19, +18.94, -3.17, -61.95, -26.90] speed=20 wait=0.0s (pre max|Δ|=1.06°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.06° (tol 3.0°) [VIA ] step 4 Δ= +0.1 -1.0 -0.5 -0.1 +1.1 +0.8 着いた [Pi] GRIPPER: VALUE=20 speed=100 result= -1 [STATE ] 目標を渡す 目標と実測の最大差 1.24°（関節 j4） [005] 推論 1738ms 10ステップ先 Δ= +0.6 -10.2 +8.7 -0.3 -0.6 +0.7 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk005_via02: send_angles (single) [ +12.26, +58.75, +19.98, -2.53, -62.34, -27.20] speed=20 wait=0.0s (pre max|Δ|=2.42°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.36° (tol 3.0°) [VIA ] step 2 Δ= +0.7 -2.4 +1.2 +0.1 +0.9 +0.2 着いた [SEND] pi05_chunk005_via05: send_angles (single) [ +12.07, +55.71, +23.03, -2.76, -62.98, -27.04] speed=20 wait=0.0s (pre max|Δ|=4.40°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.50° (tol 3.0°) [VIA ] step 5 Δ= +0.7 -4.4 +4.0 -0.1 -0.6 -0.2 着いた [SEND] pi05_chunk005_via08: send_angles (single) [ +12.16, +52.02, +26.58, -2.80, -63.70, -26.94] speed=20 wait=0.0s (pre max|Δ|=5.19°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.62° (tol 3.0°) [VIA ] step 8 Δ= +0.7 -5.2 +4.2 +0.4 -1.9 -0.7 着いた [SEND] pi05_chunk005_via09: send_angles (single) [ +12.15, +50.96, +27.50, -2.83, -63.80, -26.92] speed=20 wait=0.0s (pre max|Δ|=2.82°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.55° (tol 3.0°) [VIA ] step 9 Δ= +0.3 -2.8 +1.3 +0.2 -2.7 -0.7 着いた [STATE ] 目標を渡す 目標と実測の最大差 2.55°（関節 j4） [006] 推論 791ms 10ステップ先 Δ=+13.2 -3.9 -10.7 +3.4 +3.2 -3.5 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk006_via02: send_angles (single) [ +11.81, +47.93, +30.51, -2.91, -64.53, -26.73] speed=20 wait=0.0s (pre max|Δ|=4.80°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.81° (tol 3.0°) [VIA ] step 2 Δ= -0.1 -4.8 +3.1 +0.2 -3.0 -0.4 着いた [SEND] pi05_chunk006_via05: send_angles (single) [ +16.73, +48.21, +25.92, -1.79, -62.56, -27.78] speed=20 wait=0.0s (pre max|Δ|=4.87°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.53° (tol 3.0°) [VIA ] step 5 Δ= +4.9 -1.5 -4.7 +1.4 +1.2 -1.4 着いた [SEND] pi05_chunk006_via08: send_angles (single) [ +23.39, +48.76, +18.56, -0.22, -59.34, -29.37] speed=20 wait=0.0s (pre max|Δ|=8.33°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.12° (tol 3.0°) [VIA ] step 8 Δ= +6.7 -1.0 -8.3 +2.0 +4.4 -2.6 着いた [SEND] pi05_chunk006_via09: send_angles (single) [ +25.04, +48.87, +16.77, +0.20, -58.06, -29.77] speed=20 wait=0.0s (pre max|Δ|=2.91°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.15° (tol 3.0°) [VIA ] step 9 Δ= +1.7 -0.9 -2.9 +1.2 +2.1 -1.2 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.15°（関節 j2） [007] 推論 622ms 10ステップ先 Δ=+17.1 +5.6 -20.6 +4.7 +6.8 -4.2 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > c [CORR ] 補正 1 に入りました (推論 7 のあと) [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉 r = 爪を開かずに締め直す / R = 爪を開いて閉じ直す (つかみ直し). どちらも閉じているときだけ Enter = 補正を終えてモデルの推論に戻る / b = 補正を取り消して始めの姿勢へ戻る ? = この一覧 [補正] 関節 j0 刻み 1.0° 補正目標 j0=+25.2 j1=+49.9 j2=+17.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [補正] 関節 j1 刻み 1.0° 補正目標 j0=+25.2 j1=+49.9 j2=+17.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [補正] 関節 j2 刻み 1.0° 補正目標 j0=+25.2 j1=+49.9 j2=+17.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [SEND] pi05_corr01_000: send_angles (single) [ +25.22, +49.92, +16.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=1.00°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.14° (tol 3.0°) [補正] j2 届いていない (目標 +16.9 / 実測 +17.9, ずれ 1.00° > 0.50°) [補正] 関節 j2 刻み 1.0° 補正目標 j0=+25.2 j1=+49.9 j2=+16.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [SEND] pi05_corr01_001: send_angles (single) [ +25.22, +49.92, +17.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=1.14°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.14° (tol 3.0°) [補正] j2 着いた (目標 +17.9 / 実測 +18.2, ずれ 0.27° ≤ 0.50°) [補正] 関節 j2 刻み 1.0° 補正目標 j0=+25.2 j1=+49.9 j2=+17.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [SEND] pi05_corr01_002: send_angles (single) [ +25.22, +49.92, +18.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=1.14°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.14° (tol 3.0°) [補正] j2 着いた (目標 +18.9 / 実測 +19.4, ずれ 0.50° ≤ 0.50°) [補正] 関節 j2 刻み 1.0° 補正目標 j0=+25.2 j1=+49.9 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [補正] 関節 j1 刻み 1.0° 補正目標 j0=+25.2 j1=+49.9 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [補正] 関節 j1 刻み 2.0° 補正目標 j0=+25.2 j1=+49.9 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [SEND] pi05_corr01_003: send_angles (single) [ +25.22, +51.92, +18.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=0.86°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.16° (tol 3.0°) [補正] j1 届いていない (目標 +51.9 / 実測 +53.1, ずれ 1.16° > 1.00°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+25.2 j1=+51.9 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [SEND] pi05_corr01_004: send_angles (single) [ +25.22, +53.92, +18.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=0.84°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.18° (tol 3.0°) [補正] j1 届いていない (目標 +53.9 / 実測 +55.1, ずれ 1.18° > 1.00°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+25.2 j1=+53.9 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [SEND] pi05_corr01_005: send_angles (single) [ +25.22, +55.92, +18.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=0.82°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.85° (tol 3.0°) [補正] j1 着いた (目標 +55.9 / 実測 +56.8, ずれ 0.85° ≤ 1.00°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+25.2 j1=+55.9 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [SEND] pi05_corr01_006: send_angles (single) [ +25.22, +53.92, +18.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=2.85°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.41° (tol 3.0°) [補正] j1 届いていない (目標 +53.9 / 実測 +56.3, ずれ 2.41° > 1.00°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+25.2 j1=+53.9 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [補正] 足りなかった 2.0° を足して送る (j1 を -4.0°) [SEND] pi05_corr01_007: send_angles (single) [ +25.22, +49.92, +18.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=6.41°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.84° (tol 3.0°) [補正] j1 届いていない (目標 +49.9 / 実測 +51.8, ずれ 1.84° > 1.00°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+25.2 j1=+49.9 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [補正] 足りなかった 1.8° を足して送る (j1 を -3.8°) [SEND] pi05_corr01_008: send_angles (single) [ +25.22, +46.08, +18.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=5.68°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.73° (tol 3.0°) [補正] j1 届いていない (目標 +46.1 / 実測 +47.8, ずれ 1.73° > 1.00°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+25.2 j1=+46.1 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [補正] 足りなかった 1.7° を足して送る (j1 を -3.7°) [SEND] pi05_corr01_009: send_angles (single) [ +25.22, +42.35, +18.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=5.46°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.68° (tol 3.0°) [補正] j1 届いていない (目標 +42.4 / 実測 +44.0, ずれ 1.68° > 1.00°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+25.2 j1=+42.4 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [補正] 足りなかった 1.7° を足して送る (j1 を -3.7°) [SEND] pi05_corr01_010: send_angles (single) [ +25.22, +38.67, +18.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=5.36°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.67° (tol 3.0°) [補正] j1 届いていない (目標 +38.7 / 実測 +40.3, ずれ 1.67° > 1.00°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+25.2 j1=+38.7 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [補正] 足りなかった 1.7° を足して送る (j1 を -3.7°) [SEND] pi05_corr01_011: send_angles (single) [ +25.22, +35.00, +18.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=5.34°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.73° (tol 3.0°) [補正] j1 届いていない (目標 +35.0 / 実測 +36.7, ずれ 1.73° > 1.00°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+25.2 j1=+35.0 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [補正] 足りなかった 1.7° を足して送る (j1 を -3.7°) [SEND] pi05_corr01_012: send_angles (single) [ +25.22, +31.27, +18.92, -0.17, -58.44, -29.35] speed=20 wait=0.0s (pre max|Δ|=5.46°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.68° (tol 3.0°) [補正] j1 届いていない (目標 +31.3 / 実測 +33.0, ずれ 1.68° > 1.00°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+25.2 j1=+31.3 j2=+18.9 j3=-0.2 j4=-58.4 j5=-29.4 グリッパー 閉 [CORR ] 補正 1 を終えました (13 刻み). この姿勢から推論に戻ります [STATE ] 実測を渡す (直前に送った目標がない) [008] 推論 691ms 10ステップ先 Δ=+13.0 +13.7 -8.7 +2.3 -4.5 -0.9 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > r [SEND] pi05_chunk008_via02: send_angles (single) [ +32.78, +33.88, +16.37, +0.87, -56.68, -29.78] speed=20 wait=0.0s (pre max|Δ|=7.74°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.73° (tol 3.0°) [VIA ] step 2 Δ= +7.7 +0.9 -3.1 +0.7 +1.6 -0.2 着いた [SEND] pi05_chunk008_via05: send_angles (single) [ +36.46, +37.89, +14.22, +1.58, -58.30, -30.03] speed=20 wait=0.0s (pre max|Δ|=3.88°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.51° (tol 3.0°) [VIA ] step 5 Δ= +3.7 +3.1 -3.9 +0.7 -1.2 -0.5 着いた [SEND] pi05_chunk008_via08: send_angles (single) [ +38.00, +44.79, +11.56, +2.33, -62.13, -30.31] speed=20 wait=0.0s (pre max|Δ|=5.86°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.84° (tol 3.0°) [VIA ] step 8 Δ= +1.7 +5.9 -4.2 +0.4 -5.0 -0.7 着いた [SEND] pi05_chunk008_via09: send_angles (single) [ +37.99, +46.63, +10.70, +2.48, -62.80, -30.41] speed=20 wait=0.0s (pre max|Δ|=2.51°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.69° (tol 3.0°) [VIA ] step 9 Δ= +0.3 +0.8 -2.2 +0.4 -2.5 -0.8 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.69°（関節 j2） [009] 推論 1581ms 10ステップ先 Δ= +2.7 +12.4 -6.8 +1.4 -9.2 -1.2 グリッパー→開 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk009_via02: send_angles (single) [ +39.81, +52.60, +7.51, +3.05, -64.97, -30.54] speed=20 wait=0.0s (pre max|Δ|=4.79°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.36° (tol 3.0°) [VIA ] step 2 Δ= +2.1 +4.8 -4.8 +0.8 -2.7 -0.7 着いた [SEND] pi05_chunk009_via05: send_angles (single) [ +40.39, +57.87, +6.26, +3.42, -69.02, -30.62] speed=20 wait=0.0s (pre max|Δ|=4.78°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.47° (tol 3.0°) [VIA ] step 5 Δ= +0.8 +4.2 -2.6 +0.3 -4.8 -0.6 着いた [SEND] pi05_chunk009_via08: send_angles (single) [ +40.39, +60.24, +5.63, +3.69, -71.44, -30.76] speed=20 wait=0.0s (pre max|Δ|=3.33°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.31° (tol 3.0°) [VIA ] step 8 Δ= -0.2 +1.3 -2.1 +0.5 -3.3 -0.5 着いた [SEND] pi05_chunk009_via09: send_angles (single) [ +40.39, +60.24, +5.63, +3.69, -71.44, -30.76] speed=20 wait=0.0s (pre max|Δ|=1.31°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.31° (tol 3.0°) [VIA ] step 9 Δ= -0.2 -1.1 -1.3 +0.3 +0.0 -0.4 着いた [Pi] GRIPPER: OPEN result= -1 [STATE ] 目標を渡す 目標と実測の最大差 1.31°（関節 j2） [010] 推論 562ms 10ステップ先 Δ= -2.6 -19.9 -0.8 -0.2 +19.7 -1.0 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk010_via02: send_angles (single) [ +40.36, +54.64, +5.87, +3.65, -66.08, -30.97] speed=20 wait=0.0s (pre max|Δ|=6.70°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.29° (tol 3.0°) [VIA ] step 2 Δ= -0.2 -6.7 -1.1 +0.2 +5.4 -0.6 着いた [SEND] pi05_chunk010_via05: send_angles (single) [ +40.71, +49.70, +6.03, +3.63, -61.01, -31.17] speed=20 wait=0.0s (pre max|Δ|=7.36°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.06° (tol 3.0°) [VIA ] step 5 Δ= +0.1 -6.9 -0.9 +0.2 +7.4 -0.5 着いた [SEND] pi05_chunk010_via08: send_angles (single) [ +39.55, +43.66, +6.07, +3.39, -54.35, -31.40] speed=20 wait=0.0s (pre max|Δ|=8.22°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.13° (tol 3.0°) [VIA ] step 8 Δ= -1.0 -8.1 -0.9 -0.0 +8.2 -0.6 着いた [SEND] pi05_chunk010_via09: send_angles (single) [ +38.02, +41.44, +6.14, +3.20, -51.76, -31.44] speed=20 wait=0.0s (pre max|Δ|=4.35°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.06° (tol 3.0°) [VIA ] step 9 Δ= -1.4 -4.3 -0.8 -0.7 +3.8 -0.2 着いた [STATE ] 目標を渡す 目標と実測の最大差 2.06°（関節 j1） [011] 推論 649ms 10ステップ先 Δ=-21.9 -25.4 -2.4 -3.9 +28.5 +0.2 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk011_via02: send_angles (single) [ +32.25, +34.81, +5.62, +2.35, -43.90, -31.34] speed=20 wait=0.0s (pre max|Δ|=8.69°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.01° (tol 3.0°) [VIA ] step 2 Δ= -5.7 -8.7 -1.3 -1.6 +8.6 -0.1 着いた [SEND] pi05_chunk011_via05: send_angles (single) [ +25.80, +27.78, +5.07, +1.37, -35.38, -31.21] speed=20 wait=0.0s (pre max|Δ|=9.18°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.92° (tol 3.0°) [VIA ] step 5 Δ= -6.4 -9.0 -1.9 -1.6 +9.2 +0.1 着いた [SEND] pi05_chunk011_via08: send_angles (single) [ +18.62, +20.50, +4.67, +0.46, -26.76, -31.09] speed=20 wait=0.0s (pre max|Δ|=9.20°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.09° (tol 3.0°) [VIA ] step 8 Δ= -7.6 -9.2 -2.3 -1.4 +9.0 +0.3 着いた [SEND] pi05_chunk011_via09: send_angles (single) [ +16.07, +18.05, +4.57, +0.16, -24.06, -31.04] speed=20 wait=0.0s (pre max|Δ|=4.36°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.10° (tol 3.0°) [VIA ] step 9 Δ= -2.6 -4.4 -2.2 -0.8 +3.1 +0.3 着いた [STATE ] 目標を渡す 目標と実測の最大差 2.10°（関節 j2） [012] 推論 587ms 10ステップ先 Δ=-13.9 -16.1 -3.4 +0.0 +17.1 +0.4 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk012_via02: send_angles (single) [ +9.90, +10.57, +3.98, +0.01, -15.24, -30.98] speed=20 wait=0.0s (pre max|Δ|=9.20°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.73° (tol 3.0°) [VIA ] step 2 Δ= -5.8 -9.2 -2.7 +0.1 +9.0 +0.4 着いた [SEND] pi05_chunk012_via05: send_angles (single) [ +3.83, +4.45, +3.37, -0.03, -8.11, -30.95] speed=20 wait=0.0s (pre max|Δ|=7.67°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.16° (tol 3.0°) [VIA ] step 5 Δ= -6.4 -7.7 -2.3 -0.3 +7.4 +0.4 着いた [SEND] pi05_chunk012_via08: send_angles (single) [ +1.48, +3.08, +3.26, -0.08, -6.61, -30.93] speed=20 wait=0.0s (pre max|Δ|=2.89°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.27° (tol 3.0°) [VIA ] step 8 Δ= -2.5 -2.9 -2.3 +0.0 +1.9 +0.4 着いた [SEND] pi05_chunk012_via09: send_angles (single) [ +1.85, +3.70, +3.30, -0.07, -7.20, -30.94] speed=20 wait=0.0s (pre max|Δ|=2.23°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.23° (tol 3.0°) [VIA ] step 9 Δ= +0.9 -0.7 -2.2 +0.0 -0.5 +0.4 着いた [STATE ] 目標を渡す 目標と実測の最大差 2.23°（関節 j2） [013] 推論 474ms 10ステップ先 Δ= -1.5 -1.5 -2.4 -0.0 +0.7 +0.5 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > h [SEND] pi05_home: send_angles (single) [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00] speed=20 wait=0.0s (pre max|Δ|=6.67°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.58° (tol 2.5°) [RECORD] 記録終了 コマ 1708 (8.824 fps) 実測角 996 回 (5.15 Hz) → /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_210954/recording 結果: y=成功 / n=失敗 / Enter=記録しない > y [DONE ] 記録: /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_210954 [CLIENT] run_policy_client.py が終わった (終了コード 0) (200 秒) もう1回やりますか? y=やる / 他=終わる > === かかった時間 1. Pod を作る 3 秒 2. Pod が立ち上がるまで待つ 38 秒 3a. スクリプトを送る 32 秒 3b. huggingface_hub を入れ, 鍵を渡す 10 秒 3c. モデルを取る (hf download) 8 秒 3d. openpi を取る (215abfb) 24 秒 3e. uv sync 20 秒 3f. addon・設定・assets 2 秒 4. サーバーの起動 40 秒 5. Mac の 8000 番へのトンネル 3 秒 6. デーモンの状態 2 秒 7. HOME → 爪を開く → 位置合わせ (1 回目) 43 秒 8. 実機推論 (1 回目) 57 秒 7. HOME → 爪を開く → 位置合わせ (2 回目) 23 秒 8. 実機推論 (2 回目) 200 秒 [LAB ] Pod は残しています (id m8is5nqx61gofi). 片付けは ./lab_stop.sh --log Log17 [TUNNEL] 8000 番のトンネルを閉じました ``` #### lab_stop.sh（4回目）lab/Log16/lab_stop_20261030_211425.txt ``` [STOP ] Log17 の Pod: id m8is5nqx61gofi GPU=NVIDIA A40 作った時刻 2026-10-30 21:04:38 [LOG ] serve.log を持ち帰った → /Volumes/KIOXIA/r2r2r-data/lab/Log17/serve.log (24 行) [STOP ] Terminate を送った (21:14:34) [STOP ] Pod (id m8is5nqx61gofi) が消えたことを API で確かめた [COST ] 借りていた時間 9 分 56 秒 (0.17 時間) × 1時間 0.59 ドル = 約 0.10 ドル (約 15 円. 1ドル=158円) [TUNNEL] 8000 番のトンネルは残っていません ``` #### サーバーのログ lab/Log16/serve_20261030_2114.log ``` INFO:root:Loading model... INFO:2026-10-30 12:07:13,471:jax._src.xla_bridge:925: Unable to initialize backend 'rocm': module 'jaxlib.xla_extension' has no attribute 'GpuAllocatorConfig' INFO:jax._src.xla_bridge:Unable to initialize backend 'rocm': module 'jaxlib.xla_extension' has no attribute 'GpuAllocatorConfig' INFO:2026-10-30 12:07:13,473:jax._src.xla_bridge:925: Unable to initialize backend 'tpu': INTERNAL: Failed to open libtpu.so: libtpu.so: cannot open shared object file: No such file or directory INFO:jax._src.xla_bridge:Unable to initialize backend 'tpu': INTERNAL: Failed to open libtpu.so: libtpu.so: cannot open shared object file: No such file or directory INFO:absl:orbax-checkpoint version: 0.11.13 INFO:absl:Created BasePyTreeCheckpointHandler: use_ocdbt=True, use_zarr3=False, pytree_metadata_options=PyTreeMetadataOptions(support_rich_types=False), array_metadata_store= INFO:absl:Restoring checkpoint from /workspace/ckpt/3999/params. INFO:absl:[thread=MainThread] Failed to get flag value for EXPERIMENTAL_ORBAX_USE_DISTRIBUTED_PROCESS_ID. INFO:absl:[process=0] /jax/checkpoint/read/bytes_per_sec: 1.2 GiB/s (total bytes: 6.3 GiB) (time elapsed: 5 seconds) (per-host) INFO:absl:Finished restoring checkpoint in 5.10 seconds from /workspace/ckpt/3999/params. INFO:openpi.shared.download:Downloading gs://big_vision/paligemma_tokenizer.model to /root/.cache/openpi/big_vision/paligemma_tokenizer.model INFO:tqdm_loggable.tqdm_logging:Progress on: -/4264023 rate:- remaining:? elapsed:00:00 postfix:- INFO:tqdm_loggable.tqdm_logging:Progress on: 4.07MiB/4.07MiB rate:4,261,022.0iB/s remaining:00:00 elapsed:00:01 postfix:- INFO:root:Loaded norm stats from /workspace/openpi/assets/pi05_mecharm_trim_lora/local/mecharm_redblock_box_teach_v1_trim2 INFO:root:Loaded norm stats from /workspace/ckpt/3999/assets/local/mecharm_redblock_box_teach_v1_trim2 INFO:root:Creating server (host: 15f5e69fe17f, ip: 172.27.0.2) INFO:websockets.server:server listening on 0.0.0.0:8000 INFO:websockets.server:connection open INFO:openpi.serving.websocket_policy_server:Connection from ('127.0.0.1', 53094) opened INFO:openpi.serving.websocket_policy_server:Connection from ('127.0.0.1', 53094) closed INFO:websockets.server:connection open INFO:openpi.serving.websocket_policy_server:Connection from ('127.0.0.1', 47254) opened INFO:openpi.serving.websocket_policy_server:Connection from ('127.0.0.1', 47254) closed ``` ## 追記2：翌朝、依頼39と40で、Pod の片付けと推論の確認のキーを直した 4回目のあとに出した依頼39と40は、別の依頼（サービスロボット導入シミュレーションの夜通しの録画）が終わるのを待ってから、翌朝（10月31日）の9時台に動きました。依頼箱は、1つずつ順に進める作りのためです。 | 依頼 | やったこと | 結果 | |---|---|---| | 39 | `lab_stop.sh` で serve.log を持ち帰れないときは、10秒おきに合わせて3回試し、それでもだめなら記録を残して Pod を消す。依頼36・37で残っていたファイルもコミットする | コミット 3fa44b5。Pod を借りずに、Runpod の API を偽物に差し替えて3通りを確かめた | | 40 | 推論の確認（補正の外）でも r で爪を締め直す。何も打たない Enter のときだけチャンクを実行し、知らない文字は聞き直す。確認の前にたまったキーを捨てる | コミット d91e718。テスト69件が通った。実機ではまだ試していない | - ねらい：4回目で、補正の外で押した r と、位置合わせを閉じたときの q が、意図せずチャンクを実行したり推論を終わらせたりした。キーの押し間違いで、アームが動かないようにする - 変わったこと：推論の確認でチャンクを実行するのは、何も打たない Enter だけになった。`c]`・`R`・大文字・空白だけの入力は「分からない入力」と出て、聞き直す。r は、補正の外でも爪の締め直しになる - 見込める効果：この日の3回目の `c]` と、4回目の r・q のような押し間違いで、チャンクが実行されることがなくなる見込み。実機の確認は次の回にする ### 依頼40の報告の「決めてほしいこと」2つ | 決めてほしいこと | Claude の案 | |---|---| | 推論の確認で、大文字（Q・H など）を受け付けなくした（R と r を分けるため） | これでよい。大文字は聞き直しになるだけで、アームは動かない | | 確認を出す前に毎回、たまった入力を捨てる（問いが出る前に打ったキーは消える） | これでよい。キーを押しっぱなしにしても、先へ進まない | この追記を書いた時点では、この2つの答えはまだもらっていません。 ### 依頼39：lab_stop.sh で serve.log を3回試してから Pod を消す #### 依頼文 20261030_39_lab_stopでserve_logを3回試して消すのと依頼36のコミット.md ```markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 30 --- # 依頼：lab_stop.sh で serve.log を持ち帰れないときは3回やり直してから Pod を消す・依頼36の分をコミットする（2026-10-30） 依頼 36（デーモンの自動起動・結果は依頼箱の 結果/20261030_36_*）と依頼 37（lab_start.sh・lab_stop.sh・コミット a043b60）の続き。 ## 依頼 37 の「決めてほしいこと」への答え（エイイチ） 1. Pod の保存領域を付けず、コンテナのディスク 200GB だけにした → このままでよい 2. 実機で通すときの Log 番号 → Log17（次の実機の試しで使う。この依頼では何もしない） 3. serve.log を持ち帰れないとき Pod を消さずに止まる → **3回やり直し、それでもだめなら消す**に変える 依頼 36 の「Mac からつながるまで約60秒」は、このままでよい（rc.local は変えない）。 ## A. lab_stop.sh - serve.log の持ち帰り（scp）に失敗したら、少し待って（例 10秒）やり直す。合わせて3回 - 3回とも失敗したら、「serve.log を持ち帰れなかった」とターミナルと lab_stop のログに残してから、Pod を Terminate まで消す（今の `--skip-log` と同じ流れ） - `--skip-log` はそのまま残す - 先頭の説明文（使い方）も、この動きに合わせて直す ## B. 依頼 36 の分をコミットする - 依頼 36 で作った tools/daemon_ctl.sh・tools/pi/mecharm_daemon_start.sh・tools/pi/mecharm-daemon.service・logs/20261030_2029.md と、依頼 37 の logs/20261030_2045.md がコミットされていない。A の変更と一緒にコミットする - git は個別ファイルを指定して add する（`git add .` は使わない）。ほかの変更中のファイル（dynamic_tracking_limits/・observe/ など）は入れない ## 確かめ - Pod は借りない。scp が失敗する場合の流れを、Pod なしで確かめられる形（関数を切り出してのテストや、存在しない宛先での試し）で確かめる - 鍵の文字列がログ・コミットに出ていないことを確かめる ## 報告に書くこと - 変えたファイル、コミット ``` #### Claude Code の報告 20261030_39_lab_stopでserve_logを3回試して消すのと依頼36のコミット_20261031_0924.md ```markdown # 結果：20261030_39_lab_stopでserve_logを3回試して消すのと依頼36のコミット - 状態：完了 - 始めた時刻：2026-10-31 09:24 - 終わった時刻：2026-10-31 09:26（2分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 6921c493-29c0-4abd-a948-2accd8f9a5c3` - 止められた操作：1件（いちばん下に一覧） ## Claude Code の報告 **やったこと** - lab_stop.sh の serve.log の持ち帰りを `fetch_serve_log` という関数にまとめ、うまくいかなければ10秒待ってやり直すようにしました。試すのは合わせて3回です。 - 3回ともだめなら、「[WARN ] serve.log を持ち帰れなかった (3 回試した). Pod はこのまま Terminate まで消します」とターミナルと lab_stop のログ（lab_stop_*.txt）に出し、そのまま Terminate へ進みます。 - 依頼にはありませんが、Pod の SSH の IP とポートが API から出ないときも「1回の失敗」として数えるようにしました。前はここで止まっていたので、Pod を消さずに止まる道が残らないようにするためです。 - `--skip-log` は前と同じ動きのままです（1回だけ試して、だめでも消す）。 - 試し用に、回数と待ち時間を環境変数 LAB_LOG_TRIES と LAB_LOG_WAIT で変えられるようにしました。何も指定しなければ3回・10秒です。 - 先頭の使い方の説明を新しい動きに合わせて直しました。 - 依頼36・37 で残っていたファイルと、この作業のログを、ファイルを一つずつ指定して add し、まとめてコミットしました。dynamic_tracking_limits/ や observe/ などほかの変更中のファイルは入れていません。 **結果** - Pod は借りずに確かめました。lab_stop.sh を一時フォルダにコピーし、Runpod の API の受け答えだけを偽物に差し替えて動かしました。 1. 本物の scp でつながらない宛先（127.0.0.1 の 1 番ポート）に向けた場合（待ちは2秒に縮めた）：3回失敗し、「持ち帰れなかった」を出してから Pod を消し、消えたことも確かめました。終了コードは 0、かかった時間は約4.9秒（待ちは2回）です。 2. `--skip-log` を付けた場合：1回だけ試して失敗し、そのまま消しました。終了コードは 0 です。 3. 1回目は失敗し2回目で持ち帰れるようにした場合（scp を差し替え、待ちは10秒のまま）：約10.2秒で serve.log を持ち帰ってから消しました。 - どの場合も、画面と同じ内容が lab_stop_*.txt に残りました。画面の IP とポートは ・ に置き換わっていました。 - Runpod の API キーと hf_token の文字列は、コミットしたファイルにもコミットの中身にも出ていません（どちらも 0 件）。 - コミット：3fa44b5（7ファイル、210行追加・16行削除） **変えたファイル** - lab_stop.sh（変更） - tools/daemon_ctl.sh、tools/pi/mecharm_daemon_start.sh、tools/pi/mecharm-daemon.service（依頼36で作ったものを今回コミット） - logs/20261030_2029.md、logs/20261030_2045.md（依頼36・37の作業ログを今回コミット） - logs/20261031_0926.md（今回の作業ログ、新規） **決めてほしいこと** なし ## 止められた操作 - {"tool_name": "Bash", "tool_use_id": "toolu_01NfyL6vEtxgUPBppp98yNRL", "tool_input": {"command": "T=/var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/tmp.bvMM6GAy3s; rm -f $T/deleted\ncat >> \"$T/lab_lib.sh\" </dev/null || echo 0); c=\\$((c+1)); echo \\$c > $T/cnt\n [ \\$c -ge 2 ] && { printf 'a\\nb\\n' > \"\\${@: -1}\"; return 0; }; echo \"scp: 失敗(試し)\"; return 1; }\nEOF\ntime bash \"$T/lab_stop.sh\" --log LogT; ls $T/lab/LogT; rm -rf \"$T\""}} ``` 報告の「止められた操作」は、Claude Code が確かめのために一時フォルダを消そうとした `rm -rf` が、依頼箱の決まりで止められたものです。報告では、3通りの確かめはどれも結果が出ています。 #### 作業記録 logs/20261031_0926.md ```markdown # 2026-10-31 09:26 依頼 39: lab_stop.sh で serve.log を3回試してから消す・依頼 36 の分をコミット ## やったこと - lab_stop.sh: serve.log の持ち帰りを関数 fetch_serve_log にまとめ, 失敗したら 10 秒待ってやり直す（合わせて 3 回） - 3 回ともだめなら「[WARN ] serve.log を持ち帰れなかった (3 回試した). Pod はこのまま Terminate まで消します」を画面と lab_stop_*.txt に出し, Terminate へ進む - SSH の IP とポートが API から出ないときも 1 回の失敗として数える（前は止まっていた） - --skip-log は前と同じ（1 回だけ試し, だめでも消す） - 回数と待ちは環境変数 LAB_LOG_TRIES / LAB_LOG_WAIT で変えられる（既定 3 回・10 秒. 試し用） - 先頭の使い方の説明を新しい動きに合わせた - 依頼 36 の tools/daemon_ctl.sh・tools/pi/mecharm_daemon_start.sh・tools/pi/mecharm-daemon.service・logs/20261030_2029.md と, 依頼 37 の logs/20261030_2045.md を一緒にコミット ## 確かめ（Pod は借りていない） lab_stop.sh を一時フォルダにコピーし, 本物の lab_lib.sh の後ろで Runpod API だけを偽物に差し替えて動かした 1. 本物の scp を 127.0.0.1 の 1 番ポートへ（つながらない）: 3 回失敗 → 「持ち帰れなかった」→ DELETE → 消えたのを確認, 終了コード 0. 待ち 2 秒で約 4.9 秒（待ちは 2 回） 2. --skip-log: 1 回だけ試して失敗 → DELETE, 終了コード 0 3. 1 回目失敗・2 回目成功（scp を差し替え）, 待ちは既定の 10 秒: 約 10.2 秒で serve.log を持ち帰り → DELETE - どの場合も lab_stop_*.txt に同じ行が残った. 画面の IP とポートは ・ に置き換わっていた ## 鍵 Runpod API キー・hf_token の文字列は, コミットするファイルに出ていない（grep -F で 0 件） ``` #### lab_stop.sh（コミット 3fa44b5 の時点） ```bash #!/bin/bash # lab_stop.sh — lab_start.sh で作った Pod を片付ける (依頼 37) # # ./lab_stop.sh --log Log17 serve.log を持ち帰り, Pod を Terminate まで消し, 費用の見込みを出す # ./lab_stop.sh --log なしは, 最後に lab_start.sh を動かした Log を使う # ./lab_stop.sh --check Pod が残っていないかを確かめるだけ (消さない. 残っていれば終了コード 1) # ./lab_stop.sh --log Log17 --skip-log serve.log の持ち帰りは1回だけ試し, だめでもやり直さずに消す # # 1. Pod の /workspace/logs/serve.log を /Volumes/KIOXIA/r2r2r-data/lab// へ持ち帰る # (serve.log がすでにあれば serve_<時刻>.log にする). 持ち帰れなければ 10 秒待ってやり直す (合わせて 3 回). # 3 回ともだめなら「serve.log を持ち帰れなかった」と画面と lab_stop のログに残し, そのまま 2 へ進んで消す # 2. Pod を Terminate (DELETE) する. 一覧からも消えたことを API で確かめる # 3. 借りていた時間 × 1時間の料金 で費用の見込みを出す (ドルと円. 1ドル=158円) # 4. 裏の 8000 番のトンネルが残っていれば閉じる set -u set -o pipefail HERE="$(cd "$(dirname "$0")" && pwd)" # shellcheck source=lab_lib.sh . "$HERE/lab_lib.sh" LOG=""; CHECK=0; SKIP_LOG=0 while [ $# -gt 0 ]; do case "$1" in --log) LOG="$2"; shift 2 ;; --check) CHECK=1; shift ;; --skip-log) SKIP_LOG=1; shift ;; -h|--help) sed -n '2,16p' "$0"; exit 0 ;; *) echo "[ERROR] 知らない引数: $1 (./lab_stop.sh --help)" >&2; exit 2 ;; esac done lab_need_key "$RUNPOD_KEY_FILE" "Runpod の API キー" close_tunnel() { local pids pids=$(lab_tunnel_pids) if [ -n "$pids" ]; then kill $pids 2>/dev/null echo "[TUNNEL] 残っていた 8000 番のトンネルを閉じました (pid $(echo $pids))" else echo "[TUNNEL] 8000 番のトンネルは残っていません" fi } list_pods() { # 残っている Pod を出す. 戻り値は数 local j n j=$(runpod_api GET /pods) || { echo "[ERROR] Pod の一覧を API から取れない (HTTP $RUNPOD_HTTP)" >&2; return 255; } n=$(jq 'length' <<<"$j") if [ "$n" -eq 0 ]; then echo "[CHECK ] 残っている Pod はありません" else echo "[CHECK ] 残っている Pod が $n 個あります:" jq -r '.[] | " \(.id) \(.name) \(.desiredStatus) \(.costPerHr) ドル/時"' <<<"$j" fi return "$n" } if [ $CHECK = 1 ]; then list_pods; n=$? if [ -n "$(lab_tunnel_pids)" ]; then echo "[CHECK ] 8000 番のトンネルが残っています (pid $(echo $(lab_tunnel_pids)))" fi [ $n = 0 ] && exit 0 || exit 1 fi if [ -z "$LOG" ] && [ -s "$LAB_CURRENT" ]; then LOG=$(cat "$LAB_CURRENT"); fi [ -n "$LOG" ] || { echo "[ERROR] --log Log17 のように Log 番号を付けてください" >&2; exit 2; } LOGDIR="$LAB_ROOT/$LOG" STATE="$LOGDIR/pod/lab_pod.json" if [ -d "$LOGDIR" ]; then # 画面に出しつつ / に残す exec > >(tee -a "$LOGDIR/lab_stop_$(date +%Y%m%d_%H%M%S).txt") 2>&1 fi [ -f "$STATE" ] || { echo "[ERROR] $STATE がありません (lab_start.sh で作った Pod の記録). ./lab_stop.sh --check で確かめる" >&2; exit 1; } POD_ID=$(jq -r '.id' "$STATE") COST_PER_HR=$(jq -r '.costPerHr' "$STATE") GPU_ID=$(jq -r '.gpu' "$STATE") POD_T0=$(jq -r '.created_epoch' "$STATE") echo "[STOP ] $LOG の Pod: id $POD_ID GPU=$GPU_ID 作った時刻 $(jq -r '.created_at' "$STATE")" lab_ssh_opts # ---- 1. serve.log を持ち帰る ---- LOG_TRIES=${LAB_LOG_TRIES:-3} # 合わせて何回試すか (--skip-log は 1 回) [ $SKIP_LOG = 1 ] && LOG_TRIES=1 LOG_WAIT=${LAB_LOG_WAIT:-10} # やり直す前に待つ秒数 fetch_serve_log() { # serve.log を持ち帰る. LOG_TRIES 回ともだめなら戻り値 1 local i dest dest="$LOGDIR/serve.log" [ -e "$dest" ] && dest="$LOGDIR/serve_$(date +%Y%m%d_%H%M%S).log" for i in $(seq 1 "$LOG_TRIES"); do if ! lab_pod_addr; then echo "[LOG ] ($i/$LOG_TRIES) Pod の SSH の IP とポートが API から出ない (状態 ${POD_STATUS:-?})" elif pod_scp "root@$POD_IP:/workspace/logs/serve.log" "$dest"; then echo "[LOG ] serve.log を持ち帰った → $dest ($(wc -l < "$dest" | tr -d ' ') 行)" return 0 else echo "[LOG ] ($i/$LOG_TRIES) serve.log の scp に失敗" rm -f "$dest" # 途中まで書いたものは残さない fi if [ "$i" -lt "$LOG_TRIES" ]; then echo "[LOG ] ${LOG_WAIT} 秒待ってやり直します" sleep "$LOG_WAIT" fi done return 1 } if runpod_api GET "/pods/$POD_ID" >/dev/null; then if ! fetch_serve_log; then echo "[WARN ] serve.log を持ち帰れなかった ($LOG_TRIES 回試した). Pod はこのまま Terminate まで消します" fi else echo "[STOP ] Pod (id $POD_ID) は API から見つかりません (HTTP $RUNPOD_HTTP). すでに消えている見込み" fi # ---- 2. Terminate ---- if runpod_api GET "/pods/$POD_ID" >/dev/null; then runpod_api DELETE "/pods/$POD_ID" >/dev/null || { echo "[ERROR] Pod を消せない (HTTP $RUNPOD_HTTP)"; exit 1; } T_END=$(date +%s) echo "[STOP ] Terminate を送った ($(date -r "$T_END" '+%H:%M:%S'))" else T_END=$(jq -r '.terminated_epoch // empty' "$STATE") # 前に消したときの時刻 (なければ今) [ -n "$T_END" ] || T_END=$(date +%s) fi gone=0 for _ in $(seq 1 30); do if pods=$(runpod_api GET /pods) \ && ! jq -e --arg id "$POD_ID" 'any(.[]; .id == $id and .desiredStatus != "TERMINATED")' <<<"$pods" >/dev/null; then gone=1; break fi sleep 5 done if [ $gone = 1 ]; then echo "[STOP ] Pod (id $POD_ID) が消えたことを API で確かめた" else echo "[ERROR] 2 分半待っても Pod (id $POD_ID) が API に残っています. Runpod の画面で確かめてください" fi # ---- 3. 費用の見込み ---- secs=$((T_END - POD_T0)) awk -v s="$secs" -v c="$COST_PER_HR" -v y="$YEN_PER_USD" 'BEGIN { h = s / 3600; usd = h * c printf "[COST ] 借りていた時間 %d 分 %d 秒 (%.2f 時間) × 1時間 %.2f ドル = 約 %.2f ドル (約 %d 円. 1ドル=%d円)\n", int(s / 60), s % 60, h, c, usd, int(usd * y + 0.5), y }' jq --argjson t "$T_END" --argjson s "$secs" \ '. + {terminated_epoch:$t, terminated_at:($t|strflocaltime("%Y-%m-%d %H:%M:%S")), rented_sec:$s}' \ "$STATE" > "$STATE.tmp" && mv "$STATE.tmp" "$STATE" # ---- 4. トンネル ---- close_tunnel list_pods >/dev/null; n=$? [ $gone = 1 ] || exit 1 [ $n = 0 ] || { echo "[WARN ] ほかの Pod が $n 個残っています (./lab_stop.sh --check)"; } ``` ### 依頼40：推論の確認で、知らない文字ではチャンクを実行しない #### 依頼文 20261030_40_補正の外でもrで締め直し知らない文字では実行しない.md ```markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 90 --- # 依頼：推論の確認（補正の外）でも r で爪を締め直す・知らない文字ではチャンクを実行しない・位置合わせのあとに残ったキーを捨てる（2026-10-30） 依頼 34・35（コミット be68f74・7aaf5df）と依頼 37（lab_start.sh・コミット a043b60）の続き。対象は pi05/run_policy_client.py（と、必要なら lab_start.sh）。 ## 実機で起きたこと（2026-10-30 21:09〜、lab_start.sh --log Log17。ターミナルの出力はエイイチが会話に貼った） - pi05_20261030_210954（成功）：補正1で j1 を持ち上げたあと、推論8回目の確認で r を押した。補正の外の確認では r は知らない文字なので Enter と同じに扱われ、推論8回目のチャンクを実行した。爪の締め直しは試せなかった - LOG407 の pi05_20261030_190659 でも、確認で `c]` と打ったら、補正に入らずにチャンクを実行した - pi05_20261030_210821：lab_start.sh で位置合わせ（camera_align.py）を q で閉じたあと、推論の最初の確認がすぐに終わり（結果も聞かれたが入力なしで進んだ）、57秒で終わった。位置合わせの画面を閉じるときに押したキーが、推論の確認の入力に残っていたと見ている ## A. 補正の外の確認でも r で締め直す - 推論のあとの確認（Enter=実行 / s / b / c / h / q）で r を押したら、補正の中の r と同じく、爪が閉じていれば閉じる指令をもう一度送る（チャンクは実行しない）。送ったあと、同じ確認をもう一度出す - 爪が開いているときは、補正の中と同じ表示（「爪が開いているので締め直しはしない（閉じるのは g）」）を出して、同じ確認をもう一度出す - 記録は補正の中の r と同じ形で log.jsonl に残す（どこで押したかが分かるように） - R は補正の中だけのまま（補正の外では、下の B の扱い） - 確認の一覧の表示に r を足す ## B. 補正の外の確認で、知らない文字ではチャンクを実行しない - 実行するのは、何も打たずに Enter を押したときだけにする - 知らない文字（例 `r` 以外の文字、`c]`、`R`）が来たら、実行せずに「分からない入力: ○○（Enter=実行 / …）」と出して、同じ確認をもう一度出す - 先頭の文字だけで判断して、後ろの文字を無視する作りにはしない（`c]` は c にしない。聞き直す） - 結果を聞く所（y / n / Enter）と、つかんだ区間の y / n も、同じ考え方で見直す（知らない文字で先へ進まない） - 新しい停止条件は足さない。聞き直すだけ ## C. 位置合わせのあとに残ったキーを捨てる - 推論の最初の確認を出す前に、標準入力にたまっている入力を捨てる（termios の tcflush など）。lab_start.sh から呼ぶときも、単独で呼ぶときも同じ - 2回目以降の確認の前に捨てるかは、Claude Code が判断してよい（押しっぱなしのキーで先へ進まないように）。決めたことを報告に書く ## 確かめ（実機なし） - `--dry-keys` で、確認の画面に r・知らない文字・`c]`・空の Enter を流し、A・B の動きを確かめるテストを足す - 既存の 63 件を含め、全部通ること - codex のレビューを通す（docs/ に依頼文と結果を残す） - git は個別ファイルを指定して add する ## 報告に書くこと - 変えたファイル、コミット、テストの件数 - 実機で最初に確かめる手順（短く） ``` #### Claude Code の報告 20261030_40_補正の外でもrで締め直し知らない文字では実行しない_20261031_0926.md ```markdown # 結果：20261030_40_補正の外でもrで締め直し知らない文字では実行しない - 状態：完了 - 始めた時刻：2026-10-31 09:26 - 終わった時刻：2026-10-31 09:47（21分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 959d7fc9-a768-4513-8baa-8feb68d4f252` ## Claude Code の報告 **やったこと** - A：推論のあとの確認（補正の外）で r を押すと、爪が閉じていれば閉じる指令をもう一度送ります。チャンクは実行せず、同じ確認をもう一度出します。爪が開いていれば「爪が開いているので締め直しはしない（閉じるのは g）」と出して、聞き直します。 - log.jsonl には補正の中と同じ形で残します（type=confirm、action=regrip_tighten、where=confirm、chunk）。補正の中の r と R の記録にも where=correction を足しました。 - 確認の一覧に「r=爪の締め直し」を足しました。 - B：実行するのは、何も打たずに Enter を押したときだけにしました。 - s / b / c / r / h / q のどれかとぴったり同じでない入力は、実行せずに「分からない入力: ○○（Enter=実行 / …）」と出して聞き直します。R、`c]`、空白だけ、大文字の Q / H / S / B / C もここに入ります。 - 先頭の文字だけでは判断しません。 - 大文字を受け付けなくしたのは、R と r を分けるためです。前は大文字も小文字として受けていました。 - 結果を聞く所（y / n / Enter）も、何も打たない Enter だけを「記録しない」にしました。知らない入力と空白だけは聞き直します。 - つかんだ区間の y / n は「その他=やめる」のままにしました。アームを動かさずに確認へ戻るので、先へは進みません。 - C：たまった入力は termios の tcflush で捨てます。 - 捨てるのは、推論の確認を出す前に毎回（1回目、2回目以降、聞き直すたび）と、結果を聞く前です。押しっぱなしのキーで先へ進まないように、2回目以降も捨てることにしました。 - 捨てる処理は run_policy_client.py の中にあるので、lab_start.sh から呼んでも単独で呼んでも同じに動きます。lab_start.sh は変えていません。 - 捨てられなかったときは警告を出します。 - codex のレビューに通しました。 **結果** - テストは 69 件すべて通りました（前からの 63 件に 6 件を足しました）。変更前の版と送信・記録を比べるテストも通っています。 - 足したテストは `--dry-keys` で r・R・`c]`・空白・知らない文字・空の Enter・大文字を流すものと、`--execute` での締め直しと結果の聞き直し、入力を捨てる順番を確かめるものです。 - codex は「必須の修正なし」でした。推奨のうち、警告を出すこと・説明に注意を書き足すこと・テスト 2 件を入れました。 - 擬似端末を使って本当に入力が捨てられるかのテストは、まだありません。実機の最初の確認で見ます。 - コミット：d91e718 - 一度、関係のない csv（logs/y_calibration_mid_20260516_204525.csv）をまちがえてコミットに入れました。直前の自分のコミットを直して外してあります。ファイルは消さず、コミットに入っていない元の状態のままです。 **実機で最初に確かめる手順** 1. `lab_start.sh` で位置合わせを q で閉じたあと、推論の最初の確認がひとりでに進まず、待っていること。 2. 確認で `x` や `R` を打つと「分からない入力」と出て、アームが動かないこと。 3. 補正で g で爪を閉じてから抜け、次の確認で r を押すと、爪の締め直しだけが起き、同じ確認がまた出ること。そのあと何も打たずに Enter で実行されること。 4. 結果の問いで空白だけを打つと聞き直され、y で記録されること。 **変えたファイル** - pi05/run_policy_client.py - pi05/test_rewind_correct.py - docs/review_request_20261031_confirm_keys_flush.md - docs/review_result_20261031_confirm_keys_flush_codex.md - logs/20261031_0947.md **決めてほしいこと** - 確認で大文字を受け付けなくしました（R と r を分けるため）。Q や H などの大文字も前のように受けたほうがよければ、教えてください。 - 確認の前に毎回入力を捨てるので、問いが出る前に打ったキーは消えます。押しっぱなしで先へ進まないための作りですが、この運用でよいかを確かめてください。 ``` #### レビューの依頼文 review_request_20261031_confirm_keys_flush.md ```markdown # コードレビュー依頼：推論の確認（補正の外）でも r で締め直す / 知らない入力ではチャンクを実行しない / たまったキーを捨てる（2026-10-31 依頼 40） 前提: 依頼 34・35（コミット be68f74・7aaf5df）と依頼 37（lab_start.sh・コミット a043b60）の続き。 過去のレビュー: docs/review_result_20261030_waits_carry_regrip_rewind_codex.md ほか 対象 (`git diff HEAD -- pi05/run_policy_client.py pi05/test_rewind_correct.py` で差分。ほかのファイルの未コミット差分は対象外): - `pi05/run_policy_client.py`（flush_stdin / CONFIRM_PROMPT / CONFIRM_KEYS / ask_outcome / main の推論後の確認ループ / run_correction の r・R の記録に where を追加 / 先頭の説明） - `pi05/test_rewind_correct.py`（TestConfirmKeys 5 件追加。計 68 件 OK。git HEAD 比較の test_unchanged_without_b_c も通る） - lab_start.sh は変えていない（run_policy_client.py 側で最初の確認の前に捨てるので、単独で呼んでも lab_start.sh から呼んでも同じ） ## 実機で起きたこと - 推論の確認で r を押した → 補正の外では知らない文字なので Enter と同じに扱われ、チャンクを実行した - 確認で `c]` と打った → 補正に入らずに実行した（以前は `.strip().lower()` で "b" 以外を素通しにし、知らない文字は実行に落ちていた） - lab_start.sh で camera_align.py を q で閉じた直後、推論の最初の確認と結果の問いが入力なしで進んだ（たまっていたキーと見ている） ## 変えたこと A. 確認で r：爪が閉じていれば arm.gripper(1)（補正の中の r と同じ送り口）。開いていれば「爪が開いているので締め直しはしない（閉じるのは g）」。どちらもチャンクは実行せず同じ確認を出し直す。log.jsonl に type=confirm / action=regrip_tighten / where=confirm / chunk / after_chunk / key / before / after / sent（/ skipped）。補正の中の r・R の記録にも where=correction を足した。R は補正の外では知らない入力 B. 実行は「何も打たずに Enter」（raw == ""）のときだけ。strip した値が s/b/c/r/h/q のどれかに完全一致しなければ「分からない入力: ○○（Enter=実行 / …）」で聞き直す。大文字（R・Q など）も知らない入力（以前は lower() で受けていた。R と r を分けるため）。空白だけも聞き直す。結果の問い（ask_outcome）も、空の Enter だけを「記録しない」にし、空白だけ・知らない入力は聞き直す。つかんだ区間 / R 補正の巻き戻しの y/n は「その他=やめる」（アームを動かさず確認に戻る）のままにした（先へ進まないため） C. flush_stdin()：端末なら termios.tcflush(TCIFLUSH)。推論後の確認を出す前に毎回（1 回目も、聞き直しのたびにも）と、結果を聞く前に呼ぶ ## 見てほしい点 1. 確認ループの分岐で、知らない入力・r が実行（Enter）や他の分岐に落ちる道が残っていないか。ans の値が break 後に下流（q / h / s / b / c / 実行）で正しく使われるか 2. r を確認の中で押したときの recorder の状態（wait のまま）・grip の扱い・grip_opened との関係（開始時の開放の前に r を押した場合など）で問題がないか 3. 毎回の flush で困る場面（人が素早く続けて打った入力を捨ててしまう等）が実機運用で問題になるか。KeyReader（補正の cbreak）から抜けた直後の端末状態で tcflush が効くか 4. 大文字を受け付けなくした変更の影響 5. テストが A/B/C を十分に確かめているか ファイル変更・実機操作はしないでください。読み取りだけでお願いします。日本語で、表（項目 / 判定 / 確認結果・修正案）と、必須 / 推奨 / 据え置きの分類で返してください。 ``` #### codex の答え review_result_20261031_confirm_keys_flush_codex.md ```markdown # codex レビュー結果：推論の確認の r・知らない入力・たまったキー（2026-10-31 依頼 40） 依頼文: docs/review_request_20261031_confirm_keys_flush.md（codex exec -s read-only） **必須修正に当たる不具合は、対象差分では見つかりませんでした。** A/Bは意図どおりです。Cは端末入力の破棄として妥当ですが、入力の先行・押しっぱなしを完全に防ぐ保証にはなりません。 | 項目 | 判定 | 確認結果・修正案 | |---|---|---| | 未知入力・rからの実行への落下 | 据え置き・OK | `raw == ""` だけが実行へ進みます。未知入力は `continue`、rも処理後に `continue`。空白だけ・R・`c]`から実行へ落ちる道はありません。 | | break後のans | 据え置き・OK | q/hは終了、sは次の推論、cは補正後に次の推論へ進みます。bは巻き戻し条件と必要な回答が揃った場合だけ抜けます。空のEnterではansも空になり、実行へ進みます。 | | rとrecorder・grip | 据え置き・OK | rの間はMODE_WAITを維持。`arm.gripper(1)`は記録側の爪状態も更新し、同じ閉指令・待ちを使います。gripは既に1なので再代入は不要です。開状態では送信しません。 | | 開始時の開放との関係 | 据え置き・OK | 初期値grip=0ではrをスキップ。補正で閉じた場合は既存処理が`grip_opened=True`にするため、確認でrを押してからEnterでも開始時の開放は入りません。ただしgripは指令上の状態であり、起動時の実際の爪状態の検出ではありません。 | | 毎回のflush・補正からの復帰 | 据え置き・妥当 | 先行入力を捨てるため、素早く打った次の回答が消えるのは仕様どおりです。KeyReaderは終了時に端末属性を復元し、端末では`os.read`を使うため、その経路にPythonの読み置きはありません。復帰後のtcflushを妨げる処理は見当たりません。 | | flushの保証範囲 | 推奨 | tcflush後に到着するキーリピートや、Python等の利用者側バッファに取り込まれた入力までは捨てられません。「プロンプトが出てから入力し、Enterを押しっぱなしにしない」という運用を明記すると確実です。 | | flush失敗の扱い | 推奨 | `except Exception: pass`で端末上の破棄失敗も無表示になります。失敗時に警告を出すと、入力が勝手に進む現象が再発した際に原因を追えます。 | | 大文字の扱い | 据え置き・意図どおり | 通常確認ではQ/H/S/B/C/Rをすべて拒否します。結果の問いは引き続きY/Nを受理し、巻き戻しのy/nも大文字を受理します。この違いは依頼内容と整合します。 | | A/Bのテスト | 推奨・概ね充足 | 開状態r、閉状態r、未知入力、Enter、結果の聞き直しを確認しています。追加するならQ/H/S/B/Cの拒否、r中のMODE_WAIT、初回実行前のrスキップ後の開放を明示的に検証すると補強できます。 | | C・ログのテスト | 推奨 | 現状はflushの呼び出し順とtcflushへの引数の確認です。実際の入力破棄・cbreak復帰は未検証なので、擬似端末テストが有効です。補正内Rの`where=correction`も追加確認を推奨します。 | ログの`sent=True`は既存補正と同様、**指令を送った意味で、締め直し成功の実測確認ではありません**。dry-keysでもTrueになる既存の表現を踏襲しています。 ファイル変更・実機操作はしていません。テストは一時ファイルを作成するため実行せず、読み取りのみで確認しました。申告の68件成功・HEAD比較成功は、このレビューでは再確認していません。 ## 反映（Claude Code） - 必須: なし - 推奨・反映: flush 失敗時に [WARN] を出す / 説明に「問いが出てから打つ（捨てたあとに届いたリピートは捨てられない）」を追記 / テスト追加: 大文字 Q・H・S・B・C の拒否、補正内 R の where=correction - 推奨・据え置き: 擬似端末で実際に入力が捨てられるかのテスト、r 中の MODE_WAIT の明示テスト（実機の最初の確認で見る） - テスト: 69 件 OK（既存 63 件 + 追加 6 件。git HEAD 比較の test_unchanged_without_b_c を含む） ``` #### 作業記録 logs/20261031_0947.md ```markdown # 2026-10-31 推論の確認でも r で締め直す・知らない入力では実行しない・たまったキーを捨てる（依頼 40） 依頼: 20261030_40_補正の外でもrで締め直し知らない文字では実行しない.md - A: 推論のあとの確認で r = 爪が閉じていれば閉じる指令をもう一度（チャンクは実行しない）。開いていれば「爪が開いているので締め直しはしない（閉じるのは g）」。どちらも同じ確認を出し直す。log.jsonl に type=confirm / action=regrip_tighten / where=confirm / chunk。補正の中の r・R の記録にも where=correction を追加 - B: 実行は何も打たずに Enter のときだけ。s/b/c/r/h/q に完全一致しない入力（R・c] ・大文字・空白だけ）は「分からない入力: ○○（…）」で聞き直す。結果の問いも空の Enter だけが「記録しない」。巻き戻しの y/n は「その他=やめる」のまま（アームを動かさず確認に戻る） - C: termios.tcflush で、推論の確認を出す前に毎回（1 回目・聞き直しも）と、結果を聞く前に、たまった入力を捨てる。run_policy_client.py の中で行うので lab_start.sh は変えていない - テスト 69 件 OK（追加 6 件）。codex: 必須なし、推奨のうち警告・説明・テスト 2 件を反映 ``` #### pi05/run_policy_client.py（コミット d91e718 の時点） ```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 範囲内 - グリッパー操作後は GRIPPER_SLEEP 待つ (2026-10-30 依頼 35 で 2.0 → 0.8 秒. --grip-wait で戻せる) - --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", ...} r = 爪を開かずに閉じる指令をもう一度 (締め直し) / R = 開いてから閉じ直す (つかみ直し). どちらも爪が閉じているときだけ (開いていれば何もしない. 閉じるのは g). 2026-10-30 依頼 34 1 刻みの「着いた」は, 止まるのを待ってから 動かした関節だけ を 刻みの半分 (下限 0.5°) で見る (表示と記録の joint_reached だけ. 送り方・再送・未到達のときの数え直しは今までどおり 3°) 届いていないときは, 同じ関節・同じ向きの次の 1 刻みに 足りなかった分 を足す (上限は刻みの大きさ. move 行の carry_deg). 届いた・向きを変えた・ほかの関節を選んだら足すのをやめる. 2026-10-30 依頼 35 R で開いてから閉じ終わるまでのコマは regrip (学習データに使わない). R だけを使った補正は b で巻き戻せる (つかんだ区間と同じく y=持っている / n=落とした を聞く). 依頼 35 - --execute の間は上カメラと実測角を 1 秒 10 コマで recording/ に残す (policy_recorder.py. お手本と同じ形). 1 コマごとに状態 (model / rewind / correct / home / wait) を付ける. wait = 推論のあとの確認で入力を待っている間 (人が手でブロックを置き直すので学習データに使わない) h / q で終えるときに 結果 (y / n / Enter=記録しない) を聞き, {run_stem}.json に残す - 推論のあとの確認 (2026-10-31 依頼 40): 実行するのは何も打たずに Enter を押したときだけ. 知らない入力 (R・c] ・空白だけ など) は実行せずに聞き直す. r は補正の中と同じく爪を締め直す (閉じているときだけ. チャンクは実行しない. log.jsonl に type=confirm / action=regrip_tighten). 確認と結果を聞く前には, 端末にたまっているキー (位置合わせを閉じたキーなど) を捨てる. 捨てたあとに届いたキー (押しっぱなしのリピート) は捨てられないので, 問いが出てから打つ - --dry-keys: ドライランでも 1 回ごとに止まり b / c を試せる (アームにも daemon にも触らない. 仮の姿勢を HOME から動かして流れだけ確かめる. Enter で「仮に実行」= 仮の姿勢が目標へ移る) 待ち時間 (2026-10-30 依頼 35. エイイチの判断で縮めた. 本線と共有の定数は変えず, ここで上書きする): --send-wait 0.0 (前 1.0) / --grip-wait 0.8 (前 2.0) / --grip-open-wait 0.0 (前 2.0) / --settle-wait 0.8 (前 2.0) 前の値に戻すには --send-wait 1.0 --grip-wait 2.0 --grip-open-wait 2.0 --settle-wait 2.0 送ったあとの到着の確認 (_send_and_wait_arrival の 3°) はそのまま行う. 未到達のときに続けるか止めるかは 今までどおり (--auto は止める. 手動の推論・巻き戻しは警告を出して続ける) 終了コード: 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 (待ちは下の SEND_WAIT で上書き) 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, GRASP_HELD, MODE_HOME, MODE_MODEL, MODE_REGRIP, MODE_REWIND, MODE_WAIT, correction_carry_used, correction_joint_reached, correction_reach_tol, correction_shortfall, 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 値 # ---- 待ち時間 (2026-10-30 依頼 35. エイイチの判断で縮めた. 「time.sleep() を短縮しない」より優先) ---- # 本線 (j0_sweep_scan.py / visual_servo_pick.py) と共有の定数は変えず, この中でだけ上書きする. # コマンドの引数 (--send-wait / --grip-wait / --grip-open-wait / --settle-wait) で前の値に戻せる. # 2026-10-30 の実機 (184245 / 185133 / 190659) では中継点 4 点のチャンクを送り終えるのに 10.5〜10.8 秒. GRIPPER_SLEEP = 0.8 # 爪の開け閉めのあと Mac で待つ秒 (前 2.0. DAY175 対策の daemon 側の 2 秒は Pi のまま) GRIP_OPEN_DAEMON_WAIT = 0.0 # 開くとき daemon が指令のあとで待つ秒 (前 2.0 = daemon_set_gripper の既定. # 前は開くたびに daemon 2 + Mac 2.0 = 4 秒待っていた. 閉じる方は daemon で待たない) SEND_WAIT = 0.0 # 送ったあと daemon が待つ秒 (前 SEND_WAIT_SEC 1.0). この待ちは daemon の # 「送信後の現在角度」の記録を遅らせるだけで, 着いたかは Mac が 3° で読んで確かめる. # 0 にするのは, 0 < wait のとき Mac の待ち上限が wait + 3 秒になり, 動いている間の # daemon の読み直し (最長 3.7 秒. LOG352) より短くなるため (0 なら上限 35 秒) 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 = 0.8 # 止まらなくてもここで待つのをやめ, そのときの値で判定する (前 2.0. 依頼 35) # --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) を直接呼ぶ. 中で再送しないので送信回数を正確に数えられる (上限を超えない). 送ったあとは 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) 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 gripper_cmd(state): """爪の指令と, そのあとの待ち (推論・補正・巻き戻しで共通). 閉 = daemon_set_gripper_value(GRIP_VALUE) / 開 = daemon_set_gripper(0, wait=GRIP_OPEN_DAEMON_WAIT). そのあと GRIPPER_SLEEP 待つ (依頼 35 で縮めた. 引数で前の値に戻せる).""" if state == 1: daemon_set_gripper_value(GRIP_VALUE) else: daemon_set_gripper(0, wait=GRIP_OPEN_DAEMON_WAIT) time.sleep(GRIPPER_SLEEP) 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°). グリッパーも推論と同じ gripper_cmd (閉 = 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, send_wait_sec=SEND_WAIT) 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 read_settled(self): """補正の 1 刻みの到着判定用: 動きが止まるのを待ってから読む (read_until_still). 戻り値: (角度 or None (読めない), 止まったか). 止まるのを待つのは目安 SETTLE_WAIT_SEC (読み取りの通信時間は含まない). ドライランは仮の姿勢をそのまま返す (止まった扱い).""" if self.execute: q, still, _ = read_until_still() return q, bool(still) return (None if self.sim_q is None else self.sim_q.copy()), 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 gripper_cmd(state) 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 は つかんだ区間 (グリッパーを閉じた推論) と R を使った補正 を巻き戻すときだけ渡す (依頼 27 / 35): GRASP_PLACED (y = 持っている): 送った目標を逆にたどって つかんだ位置 (最後に送った目標) まで 下ろす → 開いてブロックを置く → 残りを逆にたどって始めの姿勢へ. つかんだ位置まで下ろせなかったら開かない (宙で落とさない. 補正の取り消しと同じ決まり) GRASP_DROPPED (n = 落とした): 今の位置ですぐ開く (閉じた指が置き直したブロックに当たらないよう) → 逆にたどって始めの姿勢へ GRASP_HELD (y = 持っている. R を使った補正だけ): 閉じたまま逆にたどって始めの姿勢へ (開かない) 送り方・範囲チェックは 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: what = "つかんだ区間" if entry["kind"] == "chunk" else "R でつかみ直した補正" print(f"[REWIND] {name} ({what}) を巻き戻す: 落としたので先にグリッパーを開きます") 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 in (None, GRASP_HELD) 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 # 補正中に一度でも g でグリッパーを動かしたか (往復でも True. codex R1-2) regrip_used = False # R (つかみ直し) を使ったか. 始めと終わりはどちらも閉 → b で巻き戻せる (依頼 35) carry, carry_key = 0.0, None # 届かなかった分 と それを足す (関節, 向き) (依頼 35) 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": if val != joint: carry, carry_key = 0.0, None # ほかの関節を選んだら足すのをやめる (依頼 35) 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 carry, carry_key = 0.0, None # 実測から数え直すので足りない分はもう入っている if carry_key != (joint, val): carry, carry_key = 0.0, None # 向きを変えた (+ と - / ほかの関節) ら足すのをやめる step = CORRECTION_STEPS[si] if carry > 0: # codex 依頼 35 ⚠️: 止まる前に打ち切って測った不足は, 入力を待つ間に解消していることがある. # 送る直前の実測 qa で足りない分を測り直す (届いていれば 0) jr_now, _ = correction_joint_reached(qa, cur, joint, correction_reach_tol(step)) carry = correction_shortfall(qa, cur, joint, val, jr_now) # 依頼 35: 前の 1 刻みが届いていなければ, 足りなかった分 (上限は刻みの大きさ) を足して送る. # 範囲チェック (apply_limits / check_send_safe) はそのまま通す add = correction_carry_used(carry, step) t, notes, unsafe = plan_correction_step(cur, joint, val, step + add, qa, lo, hi, max_move) e = {"type": "correction", "action": "move", "correction_id": corr_id, "after_chunk": chunk, "joint": joint, "step_deg": step, "sign": val, "carry_deg": round(add, 3), "t": time.time(), "before": _fmt6(cur), "after": _fmt6(t), "measured_before": _fmt6(qa), "notes": notes, "sent": False} if add > 0: print(f" [補正] 足りなかった {add:.1f}° を足して送る (j{joint} を {val * (step + add):+.1f}°)") 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}") # 依頼 34: 「着いた」は動かした関節だけを, 刻みの半分 (下限 0.5°) で見る. # 送り方・再送・resync は今までどおり send の結果 (3°) で決める (表示と記録だけ) qs, settled = arm.read_settled() tol = correction_reach_tol(step) jr, jerr = correction_joint_reached(qs, t, joint, tol) e.update(sent=True, arrived=bool(ok), measured_after=_fmt6(qs), joint_reached=bool(jr), joint_err_deg=jerr, reach_tol_deg=tol, settled=settled) e["delta"] = round(float(t[joint] - cur[joint]), 3) cur = t sent.append(t.copy()) n_steps += 1 if not settled and jerr is not None: print(f" [補正] (動きが止まらないまま {SETTLE_WAIT_SEC:.1f} 秒で打ち切って測りました)") if jerr is None: print(f" [補正] j{joint} 届いたか分からない (関節角を読めない)") elif jr: print(f" [補正] j{joint} 着いた (目標 {t[joint]:+.1f} / 実測 {qs[joint]:+.1f}, " f"ずれ {jerr:.2f}° ≤ {tol:.2f}°)") else: print(f" [補正] j{joint} 届いていない (目標 {t[joint]:+.1f} / 実測 {qs[joint]:+.1f}, " f"ずれ {jerr:.2f}° > {tol:.2f}°)") # 依頼 35: 届いていなければ, 足りなかった分を次の同じ向きの 1 刻みに足す. 届いたらやめる carry = correction_shortfall(qs, t, joint, val, jr) carry_key = (joint, val) if carry > 0 else None 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 == "regrip": # 依頼 34: r = 開かずに閉じる指令をもう一度 / R = 開く → 閉じる (その場でつかみ直す). # 閉じる・開くは g と同じ arm.gripper (値 GRIP_VALUE・速さ・待ち GRIPPER_SLEEP も同じ) action = "regrip_tighten" if val == "tighten" else "regrip_reopen" e = {"type": "correction", "action": action, "where": "correction", "correction_id": corr_id, "after_chunk": chunk, "t": time.time(), "key": "r" if val == "tighten" else "R", "before": grip} if grip != 1: what = "締め直し" if val == "tighten" else "つかみ直し" print(f" [補正] 爪が開いているので{what}はしない（閉じるのは g）") e.update(after=grip, sent=False, skipped="gripper_open") elif val == "tighten": print(" [補正] 爪を締め直します (開かずに閉じる指令をもう一度)") arm.gripper(1) e.update(after=1, sent=True) else: print(" [補正] 爪を開いて, 閉じ直します (つかみ直し)") # 依頼 35: 開いてから閉じ終わるまでのコマは regrip (学習データに使わない) if recorder is not None: recorder.set_mode(MODE_REGRIP) arm.gripper(0) arm.gripper(1) if recorder is not None: recorder.set_mode(MODE_CORRECT) e.update(after=1, sent=True) # 始めも終わりも閉. g の往復とは分け, b で巻き戻せるようにする (y/n を聞く. 依頼 35) regrip_used = True log_f.write(json.dumps(e) + "\n"); log_f.flush() _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, "regrip_used": regrip_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, "regrip_used": regrip_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 flush_stdin(): """端末の入力にたまっているキーを捨てる (依頼 40: 位置合わせの画面を閉じたキーや, 送っている間に 押したキーで確認が先へ進まないように). 端末でないとき (テストでパイプから流すとき) は何もしない.""" try: tty = sys.stdin.isatty() except Exception: return if not tty: return try: import termios termios.tcflush(sys.stdin.fileno(), termios.TCIFLUSH) except Exception as ex: # 捨てられなかったことが分かるように残す (入力が勝手に進むのがまた起きたときに追えるように. codex 依頼 40) print(f" [WARN] たまった入力を捨てられませんでした ({ex})") CONFIRM_PROMPT = (" Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / r=爪の締め直し / " "h=HOMEに戻して終了 / q=終了 > ") CONFIRM_KEYS = ("s", "b", "c", "r", "h", "q") def ask_outcome(): while True: flush_stdin() raw = input(" 結果: y=成功 / n=失敗 / Enter=記録しない > ") if raw == "": return None # 何も打たずに Enter だけ「記録しない」(空白だけは聞き直す. 依頼 40) a = raw.strip().lower() if a in ("y", "n"): return a print(f" 分からない入力: {raw}（y=成功 / n=失敗 / Enter=記録しない）") def notify_recorder(sock, payload): try: payload = dict(payload, ts=time.time()) sock.sendto((json.dumps(payload) + "\n").encode(), RECORDER) except Exception: pass def apply_wait_args(args): """--send-wait などを この中の待ち (SEND_WAIT / GRIPPER_SLEEP / GRIP_OPEN_DAEMON_WAIT / SETTLE_WAIT_SEC) へ入れる. 指定がなければ今の値のまま. 決まった値は args にも書き戻す ({run_stem}.json の args に残る).""" global SEND_WAIT, GRIPPER_SLEEP, GRIP_OPEN_DAEMON_WAIT, SETTLE_WAIT_SEC for name, attr in (("send_wait", "SEND_WAIT"), ("grip_wait", "GRIPPER_SLEEP"), ("grip_open_wait", "GRIP_OPEN_DAEMON_WAIT"), ("settle_wait", "SETTLE_WAIT_SEC")): v = getattr(args, name, None) if v is None: setattr(args, name, globals()[attr]) continue if not np.isfinite(v) or v < 0: sys.exit(f"[ERROR] --{name.replace('_', '-')} は 0 以上の数で指定する (指定: {v})") globals()[attr] = float(v) print(f"[WAIT ] 送ったあと {SEND_WAIT:.1f}s / 爪のあと {GRIPPER_SLEEP:.1f}s (開くときの daemon " f"{GRIP_OPEN_DAEMON_WAIT:.1f}s) / 止まるのを待つ目安 {SETTLE_WAIT_SEC:.1f}s") 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 から始まる仮の姿勢を動かす") # 待ち時間 (依頼 35). 既定 None = この中の値 (縮めた値). 前の値に戻すときに指定する ap.add_argument("--send-wait", type=float, default=None, help=f"送ったあと daemon が待つ秒 (既定 {SEND_WAIT:.1f}. 前は {SEND_WAIT_SEC:.1f}). " "0 より大きいと Mac の応答待ちの上限が この値 + 3 秒になる") ap.add_argument("--grip-wait", type=float, default=None, help=f"爪の開け閉めのあと Mac で待つ秒 (既定 {GRIPPER_SLEEP:.1f}. 前は 2.0)") ap.add_argument("--grip-open-wait", type=float, default=None, help=f"爪を開くとき daemon が待つ秒 (既定 {GRIP_OPEN_DAEMON_WAIT:.1f}. 前は 2.0)") ap.add_argument("--settle-wait", type=float, default=None, help=f"動きが止まるのを待つ目安の秒 (補正の 1 刻み・settle. 既定 {SETTLE_WAIT_SEC:.1f}. 前は 2.0)") args = ap.parse_args() if args.dry_keys and (args.execute or args.auto): sys.exit("[ERROR] --dry-keys はドライラン用 (--execute / --auto と一緒には使えない)") apply_wait_args(args) 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 # 依頼 40: たまっているキー (位置合わせを閉じたキー・押しっぱなし) で先へ進まない flush_stdin() raw = input(CONFIRM_PROMPT) ans = raw.strip() if raw == "": break # 実行するのは何も打たずに Enter を押したときだけ (依頼 40) if ans not in CONFIRM_KEYS: # 知らない文字 (R・c] ・空白だけ など) は実行せずに聞き直す. 先頭の文字だけでは見ない print(f" 分からない入力: {raw}（{CONFIRM_PROMPT.strip().rstrip('>').strip()}）") continue if ans == "r": # 依頼 40: 補正の外でも r で爪を締め直す (補正の中の r と同じ. チャンクは実行しない) e = {"type": "confirm", "action": "regrip_tighten", "where": "confirm", "correction_id": None, "after_chunk": i, "chunk": i, "t": time.time(), "key": "r", "before": grip} if grip != 1: print(" 爪が開いているので締め直しはしない（閉じるのは g）") e.update(after=grip, sent=False, skipped="gripper_open") else: print(" 爪を締め直します (開かずに閉じる指令をもう一度)") arm.gripper(1) e.update(after=1, sent=True) log_f.write(json.dumps(e) + "\n"); log_f.flush() continue if ans != "b": break # 巻き戻せない場合は断って聞き直す (アームは動かさない) if not history: print(" [REWIND] 巻き戻せる区間がありません (まだ何も実行していない)") continue if history[-1].get("blocked"): print(" [REWIND] 直前の巻き戻し / 補正の取り消しが始めの姿勢まで戻りきれなかったので, " "これより前へは巻き戻しません (h で HOME に戻せます)") continue h_last = history[-1] if (h_last["kind"] == "correction" and h_last.get("regrip") and not h_last["grip_changed"]): # R を使った補正 (依頼 35): 始めも終わりも閉. つかんだ区間と同じく 1 文字で聞く a = input(f" [REWIND] 補正 {h_last['correction_id']} では R でつかみ直しました. " "y=持っている（閉じたまま戻る） / " "n=落とした（先に開いてから戻る） / その他=やめる > ").strip().lower() if a == "y": grasp = GRASP_HELD elif a == "n": grasp = GRASP_DROPPED else: print(" [REWIND] やめました (アームは動かしていません)") continue break if h_last["grip_changed"]: 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, send_wait_sec=SEND_WAIT) rec["home_requested"] = True rec["home_arrived"] = bool(ok_home) log_f.write(json.dumps(rec) + "\n"); log_f.flush() if recorder is not None: recorder.stop() outcome = ask_outcome() break 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 and entry["kind"] == "chunk": 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 (手が写る) if "start_frame" in rw: 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"] or cr.get("regrip_used"): # codex R2-1: 補正でグリッパーを動かしたら, 開始時の開放は済んだことにする # (このあと最初の推論の実行で勝手に開いて物を落とさない. R も同じ) 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"] or cr["regrip_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"], "regrip": cr["regrip_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) gripper_cmd(0) 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, send_wait_sec=SEND_WAIT) sent_any = True sent_targets.append(np.asarray(tgt, dtype=float).copy()) if args.via_stride > 0: rec["vias"][vi].update(called=True, success=bool(ok), retried=bool(retried), actual_after=_fmt6(read_angles())) 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) gripper_cmd(grip_cmd) 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, send_wait_sec=SEND_WAIT) 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/test_rewind_correct.py（コミット d91e718 の時点・テスト69件） ```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) と同じログになる (項目が増えるだけ) - 依頼 35: 待ちの引数 (前の値に戻せる) / 届かなかった分を次の刻みに足す / R を使った補正の巻き戻し (y / n) / regrip のコマを学習データから除く 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, correction_carry_used, correction_joint_reached, correction_reach_tol, correction_shortfall, 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.assertEqual(parse_correction_key("r"), ("regrip", "tighten")) # 依頼 34 self.assertEqual(parse_correction_key("R"), ("regrip", "reopen")) 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 # この名前で始まる送信では動かない self.offset = None # 送った目標からこれだけずれた所で止まる (依頼 34. 6 関節) 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 self.offset is not None: self.q = self.q + np.asarray(self.offset, 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, offset=None): world = FakeWorld() world.fail_after_label = fail_after_label world.stuck_prefix = stuck_prefix world.offset = offset 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) class TestRegripAndCorrectionReach(unittest.TestCase): """依頼 34: 補正の r (締め直し) / R (つかみ直し) と, 補正の 1 刻みの「着いた」の判定.""" EXEC = ["--execute", "--steps-per-chunk", "10", "--via-stride", "3", "--settle-retries", "0", "--ignore-framing", "--max-chunks", "6"] def setUp(self): self.td = Path(tempfile.mkdtemp()) time.sleep(1.1) def test_reach_tol_values(self): self.assertEqual(correction_reach_tol(1.0), 0.5) self.assertEqual(correction_reach_tol(2.0), 1.0) self.assertEqual(correction_reach_tol(0.5), 0.5) # 半分 (0.25°) は読み値のぶれより細かいので 0.5° self.assertEqual([correction_reach_tol(x) for x in CORRECTION_STEPS], [0.5, 0.5, 1.0]) def test_joint_reached_only_moved_joint(self): tgt = [5.0, 10.0, 0, 0, 0, -30] self.assertEqual(correction_joint_reached([5.4, 12.0, 1, 1, 1, -29], tgt, 0, 0.5), (True, 0.4)) self.assertEqual(correction_joint_reached([5.6, 10.0, 0, 0, 0, -30], tgt, 0, 0.5), (False, 0.6)) self.assertEqual(correction_joint_reached(None, tgt, 0, 0.5), (False, None)) def test_r_and_R_records(self): """g で閉じる → r → R: r は閉じる指令をもう一度だけ, R は 開く → 閉じる. 記録に前後の状態.""" code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys="grR\n") self.assertEqual(code, 0) v = ("v", rpc.GRIP_VALUE) self.assertEqual(world.grips[:4], [v, v, 0, v]) # g / r / R (開く → 閉じる) corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["gripper", "regrip_tighten", "regrip_reopen", "accept"]) r, R = corr[1], corr[2] self.assertEqual((r["key"], r["before"], r["after"], r["sent"]), ("r", 1, 1, True)) self.assertEqual((R["key"], R["before"], R["after"], R["sent"]), ("R", 1, 1, True)) self.assertIn("t", r) self.assertEqual(corr[-1]["grip_after"], 1) 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_r_and_R_dry_keys(self): """--execute なし (--dry-keys) でも r / R が流れる (アームにも daemon にも送らない).""" code, world, run, lines, out = run_client( rpc, ["--dry-keys", "--steps-per-chunk", "10", "--max-chunks", "3"], ["c", "", "q"], self.td, keys="rgrR1+\n") self.assertEqual(code, 0) self.assertEqual((world.grips, world.sends), ([], [])) corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["regrip_tighten", "gripper", "regrip_tighten", "regrip_reopen", "move", "accept"]) self.assertEqual([x["sent"] for x in corr[:4] if x["action"] != "gripper"], [False, True, True]) self.assertTrue(corr[4]["joint_reached"]) self.assertIn("グリッパー→開 (送らない)", out) def test_r_when_open_does_nothing(self): code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys="rR\n") self.assertEqual(code, 0) self.assertNotIn(("v", rpc.GRIP_VALUE), world.grips) # 閉じていない self.assertIn("爪が開いているので締め直しはしない（閉じるのは g）", out) corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["regrip_tighten", "regrip_reopen", "accept"]) self.assertTrue(all(not x["sent"] and x["skipped"] == "gripper_open" and x["before"] == 0 for x in corr[:2])) self.assertFalse(corr[-1]["grip_used"]) def test_cancel_after_R_restores_start_gripper(self): """開いた状態で入って g → R → b (取り消し): グリッパーは始めの「開」へ戻る.""" code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys="gR1+b") cancel = [x for x in lines if x.get("action") == "cancel"][0] self.assertTrue(cancel["finished"]) self.assertEqual(cancel["grip_after"], 0) v = ("v", rpc.GRIP_VALUE) self.assertEqual(world.grips[:4], [v, 0, v, 0]) def moves(self, keys, offset=None, stuck=None): code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys=keys, offset=offset, stuck_prefix=stuck) self.assertEqual(code, 0) return [x for x in lines if x.get("action") == "move"], out def test_reach_ignores_other_joints(self): mv, out = self.moves("1+\n", offset=[0.3, 0.9, 0, 0, 0, 0.8]) # j0 を動かす. ほかは 0.9° ずれ self.assertTrue(mv[0]["joint_reached"]) self.assertEqual((mv[0]["joint_err_deg"], mv[0]["reach_tol_deg"]), (0.3, 0.5)) self.assertTrue(mv[0]["settled"]) self.assertIn("着いた", out) def test_reach_tol_by_step(self): mv, out = self.moves("1+]+[[+\n", offset=[0.7, 0, 0, 0, 0, 0]) # 1° / 2° / 0.5° の刻みで 0.7° 足りない self.assertEqual([x["step_deg"] for x in mv], [1.0, 2.0, 0.5]) self.assertEqual([x["reach_tol_deg"] for x in mv], [0.5, 1.0, 0.5]) self.assertEqual([x["joint_reached"] for x in mv], [False, True, False]) self.assertTrue(all(x["arrived"] for x in mv)) # 送りの判定 (3°) は今までどおり着いた self.assertIn("届いていない", out) def test_not_reached_continues(self): """動かないときも「届いていない」と出して補正を続ける (止めない・新しい確認なし).""" mv, out = self.moves("1+++\n", stuck="pi05_corr") self.assertEqual(len(mv), 3) self.assertTrue(all(x["sent"] and not x["joint_reached"] for x in mv)) self.assertEqual(out.count("届いていない"), 3) self.assertIn("補正 1 を終えました", out) # ---- 依頼 35: 待ちを縮める / 届かなかった分を次の刻みに足す / R の補正も巻き戻す / regrip のコマ ---------- class ShortArm(rpc.ArmIO): """送った向きに frac だけしか動かない仮の腕. 送りの判定 (3°) は「着いた」を返す (実機の j1 持ち上げと同じ).""" def __init__(self, *a, frac=0.25, ok=True, **k): super().__init__(*a, **k) self.frac, self.ok = frac, ok def send(self, target, tag): t = np.asarray(target, dtype=float) self.sim_q = self.sim_q + (t - self.sim_q) * (1.0 - self.frac) return self.ok class TestCarry(unittest.TestCase): START = [0.0, 50.0, 0, 0, 0, -30] def run_keys(self, keys, frac=0.25, ok=True): arm = ShortArm(False, None, sim_q=self.START, frac=frac, ok=ok) log = io.StringIO() with mock.patch("sys.stdout", new=io.StringIO()) as out: res, _ = rpc.run_correction(arm, Keys(keys), LO, HI, 20.0, 0, 1, 0, log, None) mv = [json.loads(x) for x in log.getvalue().splitlines()] return [m for m in mv if m["action"] == "move"], out.getvalue() def test_shortfall_and_cap(self): self.assertEqual(correction_shortfall([0, 51.5, 0, 0, 0, 0], [0, 50.0, 0, 0, 0, 0], 1, -1, False), 1.5) self.assertEqual(correction_shortfall([0, 51.5, 0, 0, 0, 0], [0, 50.0, 0, 0, 0, 0], 1, -1, True), 0.0) self.assertEqual(correction_shortfall([0, 49.0, 0, 0, 0, 0], [0, 50.0, 0, 0, 0, 0], 1, -1, False), 0.0) self.assertEqual(correction_shortfall(None, [0, 50.0, 0, 0, 0, 0], 1, -1, False), 0.0) self.assertEqual(correction_shortfall([0, 48.5, 0, 0, 0, 0], [0, 50.0, 0, 0, 0, 0], 1, +1, False), 1.5) self.assertEqual(correction_carry_used(1.5, 2.0), 1.5) self.assertEqual(correction_carry_used(3.0, 2.0), 2.0) # 上限は刻みの大きさ self.assertEqual(correction_carry_used(0.0, 1.0), 0.0) def test_no_carry_when_reached(self): """2° 刻みで毎回 0.5° 足りない → 判定 (刻みの半分 1.0°) では「着いた」なので足さない.""" mv, out = self.run_keys("2]---\n") self.assertEqual([m["joint_reached"] for m in mv], [True, True, True]) self.assertEqual([m["carry_deg"] for m in mv], [0.0, 0.0, 0.0]) self.assertEqual([m["after"][1] for m in mv], [48.0, 46.0, 44.0]) self.assertNotIn("足りなかった", out) def test_carry_example_and_cap(self): """届かない量が刻みより大きくても, 足すのは刻みの大きさまで (1 回で動く量は刻みの 2 倍まで).""" mv, out = self.run_keys("2]---\n", frac=0.75) # 1 回目 50→48 の指令で 1.5° しか動かない → 足りない 1.5 → 2 回目は 48 - 2 - 1.5 = 44.5 self.assertEqual(mv[0]["after"][1], 48.0) self.assertEqual(mv[1]["carry_deg"], 1.5) self.assertEqual(mv[1]["after"][1], 44.5) self.assertIn("足りなかった 1.5° を足して送る", out) mv, _ = self.run_keys("--\n", frac=1.0) # 動かない. 刻み 1° self.assertEqual([m["carry_deg"] for m in mv], [0.0, 1.0]) mv, _ = self.run_keys("---\n", frac=1.0) self.assertEqual([m["carry_deg"] for m in mv], [0.0, 1.0, 1.0]) # 足りない 2° でも足すのは 1° self.assertTrue(all(abs(m["delta"]) <= 2.0 + 1e-9 for m in mv)) def test_carry_stops(self): mv, _ = self.run_keys("2]--+\n", frac=0.75) # 向きを変えた self.assertEqual([m["carry_deg"] for m in mv], [0.0, 1.5, 0.0]) mv, _ = self.run_keys("2]-1-2-\n", frac=0.75) # ほかの関節を選んで戻った self.assertEqual([m["joint"] for m in mv], [1, 0, 1]) self.assertEqual([m["carry_deg"] for m in mv], [0.0, 0.0, 0.0]) mv, _ = self.run_keys("2]--\n", frac=0.0) # 届いた self.assertEqual([m["carry_deg"] for m in mv], [0.0, 0.0]) mv, _ = self.run_keys("2]--\n", frac=0.75, ok=False) # 未到達 (3°) → 実測から数え直す. 足さない self.assertEqual(mv[1]["carry_deg"], 0.0) self.assertEqual(mv[1]["after"][1], 49.5 - 2.0) # 1 回目は 1.5° しか動かず実測 49.5 def test_carry_rechecked_before_send(self): """codex 依頼 35: 打ち切って測った不足が, 次に押すまでに解消していたら足さない.""" class LateArm(ShortArm): def read(self): q = super().read() if getattr(self, "arrive_late", False) and self.last is not None: self.sim_q = self.last.copy(); q = self.sim_q.copy() # 入力を待つ間に目標へ着いた return q def send(self, target, tag): self.last = np.asarray(target, dtype=float).copy() return super().send(target, tag) def read_settled(self): q, st = super().read_settled() self.arrive_late = True return q, False arm = LateArm(False, None, sim_q=self.START, frac=0.75) arm.last = None log = io.StringIO() with mock.patch("sys.stdout", new=io.StringIO()): rpc.run_correction(arm, Keys("2]--\n"), LO, HI, 20.0, 0, 1, 0, log, None) mv = [json.loads(x) for x in log.getvalue().splitlines() if '"move"' in x] self.assertFalse(mv[0]["joint_reached"]) # 打ち切った時点では届いていない self.assertEqual(mv[1]["carry_deg"], 0.0) # 送る直前には着いていたので足さない self.assertEqual(mv[1]["after"][1], 46.0) def test_carry_still_limited(self): """足した目標も apply_limits / check_send_safe を通る (安全範囲の端で止まる).""" arm = ShortArm(False, None, sim_q=[0.0, 87.0, 0, 0, 0, -30], frac=0.75) log = io.StringIO() with mock.patch("sys.stdout", new=io.StringIO()): rpc.run_correction(arm, Keys("2]++\n"), LO, HI, 20.0, 0, 1, 0, log, None) mv = [json.loads(x) for x in log.getvalue().splitlines() if '"move"' in x] self.assertEqual(mv[0]["after"][1], 88.0) # 87 + 2 → 88 (範囲の端) にクランプ self.assertTrue(all(m["after"][1] <= 88.0 for m in mv)) class TestWaitArgs(unittest.TestCase): NAMES = ("SEND_WAIT", "GRIPPER_SLEEP", "GRIP_OPEN_DAEMON_WAIT", "SETTLE_WAIT_SEC") def apply(self, argv): ap = argparse.Namespace(send_wait=None, grip_wait=None, grip_open_wait=None, settle_wait=None) for k, v in argv.items(): setattr(ap, k, v) with mock.patch("sys.stdout", new=io.StringIO()): rpc.apply_wait_args(ap) return ap, tuple(getattr(rpc, n) for n in self.NAMES) def setUp(self): self.patches = [mock.patch.object(rpc, n, getattr(rpc, n)) for n in self.NAMES] for p in self.patches: p.start() def tearDown(self): for p in self.patches: p.stop() def test_defaults_shortened(self): ap, vals = self.apply({}) self.assertEqual(vals, (0.0, 0.8, 0.0, 0.8)) self.assertEqual((ap.send_wait, ap.grip_wait, ap.grip_open_wait, ap.settle_wait), (0.0, 0.8, 0.0, 0.8)) def test_restore_previous_values(self): _, vals = self.apply({"send_wait": 1.0, "grip_wait": 2.0, "grip_open_wait": 2.0, "settle_wait": 2.0}) self.assertEqual(vals, (1.0, 2.0, 2.0, 2.0)) self.assertEqual(rpc.SEND_WAIT_SEC, 1.0) # 本線の定数は変えていない # 前の値が送り口とグリッパーまで届く sw, gr, sl = [], [], [] with mock.patch.object(rpc, "_send_and_wait_arrival", lambda *a, **k: (sw.append(k), (True, False))[1]), \ mock.patch.object(rpc, "daemon_set_gripper", lambda s, **k: gr.append((s, k))), \ mock.patch.object(rpc, "daemon_set_gripper_value", lambda v, **k: gr.append(("v", v))), \ mock.patch.object(rpc.time, "sleep", lambda x: sl.append(x)), \ mock.patch("sys.stdout", new=io.StringIO()): arm = rpc.ArmIO(True, None) arm.send([0, 0, 0, 0, 0, -30], "x") arm.gripper(0) arm.gripper(1) self.assertEqual(sw, [{"send_wait_sec": 1.0}]) self.assertEqual(gr, [(0, {"wait": 2.0}), ("v", rpc.GRIP_VALUE)]) self.assertEqual(sl, [2.0, 2.0]) def test_new_values_reach_senders(self): self.apply({}) sw, gr, sl, ds = [], [], [], [] with mock.patch.object(rpc, "_send_and_wait_arrival", lambda *a, **k: (sw.append(k), (True, False))[1]), \ mock.patch.object(rpc, "daemon_set_gripper", lambda s, **k: gr.append((s, k))), \ mock.patch.object(rpc, "daemon_send_angles", lambda *a, **k: ds.append(k)), \ mock.patch.object(rpc, "read_until_still", side_effect=[(np.array([0, 2.0, 0, 0, 0, -30]), True, None), (np.array([0, 0.0, 0, 0, 0, -30]), True, None)]), \ mock.patch.object(rpc.time, "sleep", lambda x: sl.append(x)), \ mock.patch("sys.stdout", new=io.StringIO()): arm = rpc.ArmIO(True, None) arm.send([0, 0, 0, 0, 0, -30], "x") arm.gripper(0) rpc.settle_target([0, 0, 0, 0, 0, -30], "x", 1.0, 2) self.assertEqual(sw, [{"send_wait_sec": 0.0}]) self.assertEqual(gr, [(0, {"wait": 0.0})]) self.assertEqual(sl, [0.8]) self.assertEqual(ds, [{"speed": rpc.SPEED, "wait": 0.0}]) def test_settle_wait_used(self): self.apply({"settle_wait": 0.3}) reads = iter([np.zeros(6) + k for k in range(100)]) # 動き続ける (止まらない) with mock.patch.object(rpc, "read_angles", lambda: next(reads)), \ mock.patch.object(rpc, "SETTLE_POLL_SEC", 0.05): t0 = time.time() q, still, _ = rpc.read_until_still() dt = time.time() - t0 self.assertFalse(still) self.assertLess(dt, 0.6) def test_negative_rejected(self): with self.assertRaises(SystemExit): self.apply({"grip_wait": -1.0}) class TestRegripRewind(unittest.TestCase): """R だけを使った補正を b で巻き戻す (y / n). g を往復した補正は今までどおり断る.""" EXEC = ["--execute", "--steps-per-chunk", "10", "--max-chunks", "6", "--settle-retries", "0", "--ignore-framing"] def setUp(self): self.td = Path(tempfile.mkdtemp()) time.sleep(1.1) def run_(self, inputs, keys): return run_client(rpc, self.EXEC, inputs, self.td, keys=keys) def test_rewind_R_correction_y(self): # 補正 1 で g (閉じる) / 補正 2 で R と j1 +2 刻み → b → y code, world, run, lines, out = self.run_(["c", "c", "b", "y", "q", ""], "g\nR2++\n") self.assertEqual(code, 0) v = ("v", rpc.GRIP_VALUE) self.assertEqual(world.grips, [v, 0, v]) # g / R (開く → 閉じる). 巻き戻しで開かない corr = [x for x in lines if x.get("action") == "accept"] self.assertEqual((corr[1]["grip_used"], corr[1]["regrip_used"]), (False, True)) rw = [x for x in lines if x.get("type") == "rewind"] self.assertEqual(len(rw), 1) self.assertEqual((rw[0]["kind"], rw[0]["correction_id"], rw[0]["grasp_undo"]), ("correction", 2, "held")) self.assertTrue(rw[0]["finished"]) self.assertFalse(rw[0]["gripper_opened"]) back = [s[1] for s in world.sends if s[0].startswith("pi05_rewind")] self.assertEqual(back[-1], corr[1]["start_pose"]) # 補正 2 を始めた姿勢へ戻る ch = [x for x in lines if x.get("type") is None and "chunk" in x] self.assertEqual(ch[-1]["state"][6], 1.0) # 閉じたまま self.assertNotIn("落としたので", out) def test_rewind_R_correction_n(self): code, world, run, lines, out = self.run_(["c", "c", "b", "n", "", "q", ""], "g\nR2++\n") self.assertEqual(code, 0) v = ("v", rpc.GRIP_VALUE) self.assertEqual(world.grips, [v, 0, v, 0]) # 巻き戻しの前にその場で開く k_open = max(k for k, e in enumerate(world.events) if e == ("grip", 0)) self.assertTrue(all(e[0] == "send" and e[1].startswith("pi05_rewind") for e in world.events[k_open + 1:])) # 開いてから戻る rw = [x for x in lines if x.get("type") == "rewind"][0] self.assertEqual(rw["grasp_undo"], "dropped") self.assertTrue(rw["gripper_opened"] and rw["finished"]) self.assertNotIn("start_frame", rw) # 置き直す間は wait (つかんだ区間の n と同じ) 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"]) ch = [x for x in lines if x.get("type") is None and "chunk" in x] self.assertEqual(ch[-1]["state"][6], 0.0) def test_rewind_R_correction_other_cancels(self): code, world, run, lines, out = self.run_(["c", "c", "b", "x", "q", ""], "g\nR2++\n") self.assertEqual([x for x in lines if x.get("type") == "rewind"], []) self.assertIn("やめました", out) def test_g_round_trip_still_refused(self): """g を使った補正 (R も使っていても) は今までどおり巻き戻さない.""" code, world, run, lines, out = self.run_(["c", "b", "q", ""], "gR2+\n") self.assertEqual([x for x in lines if x.get("type") == "rewind"], []) self.assertIn("でグリッパーを動かしたので巻き戻しません", out) def test_dry_keys_r_R_b(self): """--dry-keys でも r・R・b (y) が流れる (アームにも daemon にも送らない).""" code, world, run, lines, out = run_client( rpc, ["--dry-keys", "--steps-per-chunk", "10", "--max-chunks", "6"], ["c", "c", "b", "y", "q"], self.td, keys="g\nrR2-]-\n") self.assertEqual(code, 0) self.assertEqual((world.grips, world.sends), ([], [])) corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["gripper", "accept", "regrip_tighten", "regrip_reopen", "move", "move", "accept"]) self.assertEqual([x["carry_deg"] for x in corr if x["action"] == "move"], [0.0, 0.0]) rw = [x for x in lines if x.get("type") == "rewind"] self.assertEqual([(x["correction_id"], x["grasp_undo"], x["finished"]) for x in rw], [(2, "held", True)]) def test_regrip_frames_marked_and_excluded(self): code, world, run, lines, out = self.run_(["c", "q", ""], "grR\n") stages = [l.split(",")[2] for l in (run / "recording" / "states.csv").read_text().splitlines()[1:] if l.split(",")[1] == "stage"] self.assertEqual(stages.count("regrip"), 1) # R だけ. r は correct のまま k = stages.index("regrip") self.assertEqual(stages[k - 1:k + 2], ["correct", "regrip", "correct"]) # R の開く指令と閉じる指令は regrip の間に入る rows = [l.split(",") for l in (run / "recording" / "states.csv").read_text().splitlines()[1:]] ts_stage = [(float(r[0]), r[2]) for r in rows if r[1] == "stage"] t_in = [t for t, m in ts_stage if m == "regrip"][0] t_out = min(t for t, m in ts_stage if t > t_in) g_in = [(float(r[0]), r[10]) for r in rows if r[1] == "gripper_cmd" and t_in <= float(r[0]) <= t_out] self.assertEqual([g for _, g in g_in], ["0", "1"]) # build_correction_episodes の除き方: regrip は学習に使わない (wait / rewind と同じ) keep, m = step_keep_mask([0.5, 1.5, 2.5], [0.0, 1.0, 2.0], ["correct", "regrip", "correct"], []) self.assertEqual(list(keep), [True, False, True]) # ---- 依頼 40: 補正の外の r・知らない入力では実行しない・たまったキーを捨てる ---------------------- class _LoggedInputs: """input() に渡す答え. 何回目の input の前に flush_stdin が呼ばれたかを見るため, 渡した順を残す.""" def __init__(self, answers, events): self.answers, self.events = answers, events def __iter__(self): for a in self.answers: self.events.append(("input", a)) yield a class TestConfirmKeys(unittest.TestCase): EXEC = ["--execute", "--steps-per-chunk", "10", "--via-stride", "3", "--settle-retries", "0", "--max-chunks", "6"] def setUp(self): self.td = Path(tempfile.mkdtemp()) time.sleep(1.1) def test_dry_keys_r_unknown_and_enter(self): """--dry-keys: 開いているときの r・R・c]・空白だけ は実行しない. 空の Enter だけが実行する. g で閉じたあとの r は締め直す (チャンクは実行しない).""" code, world, run, lines, out = run_client( rpc, ["--dry-keys", "--steps-per-chunk", "10", "--max-chunks", "4"], ["r", "R", "c]", " ", "x", "", "c", "r", "q"], self.td, keys="g\n") self.assertEqual(code, 0) self.assertEqual((world.grips, world.sends), ([], [])) conf = [x for x in lines if x.get("type") == "confirm"] self.assertEqual([(x["action"], x["where"], x["chunk"], x["before"], x["sent"]) for x in conf], [("regrip_tighten", "confirm", 0, 0, False), ("regrip_tighten", "confirm", 2, 1, True)]) self.assertEqual(conf[0]["skipped"], "gripper_open") self.assertIn("爪が開いているので締め直しはしない（閉じるのは g）", out) for bad in ("R", "c]", " ", "x"): self.assertIn(f"分からない入力: {bad}（Enter=実行 /", out) ch = [x for x in lines if x.get("type") is None and "chunk" in x] self.assertEqual([x["chunk"] for x in ch if x.get("simulated")], [0]) # 空の Enter の 1 回だけ corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["gripper", "accept"]) # c] では補正に入らない self.assertIn("r=爪の締め直し", out) def test_execute_r_tightens_and_outcome_reasks(self): """--execute: 閉じているときの r は閉じる指令をもう一度送る (チャンクは送らない). 結果を聞く所も 知らない入力・空白だけ は聞き直す.""" code, world, run, lines, out = run_client( rpc, self.EXEC, ["c", "r", "R", "", "q", "yy", " ", "y"], self.td, keys="g\n") self.assertEqual(code, 0) v = ("v", rpc.GRIP_VALUE) self.assertEqual(world.grips[:2], [v, v]) # g で閉じる → r でもう一度閉じる (3 つ目は推論の開) conf = [x for x in lines if x.get("type") == "confirm"] self.assertEqual([(x["chunk"], x["before"], x["after"], x["sent"]) for x in conf], [(1, 1, 1, True)]) # r では実行せず, あとの空の Enter で推論 1 を実行した (推論 1 の行は r の記録より後に 1 つだけ) k_r = lines.index(conf[0]) ch1 = [(k, x) for k, x in enumerate(lines) if x.get("type") is None and x.get("chunk") == 1] self.assertEqual(len(ch1), 1) self.assertGreater(ch1[0][0], k_r) self.assertTrue(ch1[0][1]["executed"]) self.assertIn("分からない入力: yy（y=成功", out) self.assertIn("分からない入力: （y=成功", out) j = json.loads((run / f"{run.name}.json").read_text()) self.assertEqual((j["outcome"], j["judgments"]["place"]["human_verdict"]), ("success", "yes")) def test_regrip_in_correction_has_where(self): code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys="grR\n") corr = [x for x in lines if x.get("action") in ("regrip_tighten", "regrip_reopen")] self.assertEqual([(x["action"], x["where"]) for x in corr], [("regrip_tighten", "correction"), ("regrip_reopen", "correction")]) def test_uppercase_rejected(self): """大文字 (Q / H / S / B / C) は知らない入力. 終わらず・送らず・補正にも入らない.""" code, world, run, lines, out = run_client( rpc, ["--dry-keys", "--steps-per-chunk", "10", "--max-chunks", "2"], ["Q", "H", "S", "B", "C", "q"], self.td) self.assertEqual(code, 0) for bad in ("Q", "H", "S", "B", "C"): self.assertIn(f"分からない入力: {bad}（", out) ch = [x for x in lines if x.get("type") is None and "chunk" in x] self.assertEqual([x["chunk"] for x in ch], [0]) # 推論 0 の確認のまま q で終えた self.assertFalse(ch[0].get("simulated")) self.assertEqual([x for x in lines if x.get("type") in ("correction", "rewind")], []) def test_flush_before_each_prompt(self): """確認 (1 回目から毎回) と結果を聞く前に, たまったキーを捨てる.""" events = [] with mock.patch.object(rpc, "flush_stdin", lambda: events.append(("flush",))): code, *_ = run_client(rpc, self.EXEC, _LoggedInputs(["x", "", "q", "y"], events), self.td) self.assertEqual(code, 0) self.assertEqual(events, [("flush",), ("input", "x"), ("flush",), ("input", ""), ("flush",), ("input", "q"), ("flush",), ("input", "y")]) def test_flush_stdin_uses_tcflush_on_tty(self): import termios class Tty: def isatty(self): return True def fileno(self): return 0 with mock.patch.object(sys, "stdin", Tty()), mock.patch.object(termios, "tcflush") as tf: rpc.flush_stdin() tf.assert_called_once_with(0, termios.TCIFLUSH) with mock.patch.object(sys, "stdin", io.StringIO("")), mock.patch.object(termios, "tcflush") as tf: rpc.flush_stdin() # 端末でなければ何もしない tf.assert_not_called() if __name__ == "__main__": unittest.main() ``` ### 次に実機で確かめること 依頼40の報告にあった手順で、次の回に確かめます。 1. `lab_start.sh` で位置合わせを q で閉じたあと、推論の最初の確認がひとりでに進まず、待っていること 2. 確認で `x` や `R` を打つと「分からない入力」と出て、アームが動かないこと 3. 補正で g で爪を閉じてから抜け、次の確認で r を押すと、爪の締め直しだけが起き、同じ確認がまた出ること。そのあと何も打たずに Enter で実行されること 4. 結果の問いで空白だけを打つと聞き直され、y で記録されること そのあと、4回目（pi05_20261030_210954）の記録から、直した区間を含む学習データを作ります。 ## ターミナルの出力（全量） ### ターミナル1（Mac から Pod へ送る所。最初の1回は手順書の書き方のまま打って止まった） ``` Last login: Fri Oct 30 14:10:52 on ttys005 (base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % cd ~/r2r2r-scripts P=; IP=; K=~/.ssh/id_ed25519 ssh -p $P -i $K root@$IP "mkdir -p /workspace/r2r2r /workspace/logs /workspace/ckpt" scp -P $P -i $K -r ~/r2r2r-scripts/pi05 root@$IP:/workspace/r2r2r/ scp -P $P -i $K /Volumes/KIOXIA/r2r2r-data/lab/Log11/pod/add_trim_config.py root@$IP:/workspace/r2r2r/ ssh -p $P -i $K root@$IP zsh: parse error near `;' (base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % ``` ### ターミナル1（Pod の中。hf を入れ直した所と、鍵を確かめた所） `hf` のコマンドが見つからなかったので、`python3 -m pip` で入れ直しています。出力は、画面からコピーできた所だけです。 ``` which tmux hf pip python3 python3 -m pip install -U --break-system-packages huggingface_hub hash -r; which hf ``` ``` Collecting truststore>=0.10 (from httpx2<3,>=2.0.0->huggingface_hub) Downloading truststore-0.10.5-py3-none-any.whl.metadata (4.5 kB) Requirement already satisfied: h11>=0.16 in /usr/local/lib/python3.12/dist-packages (from httpcore2==2.16.0->httpx2<3,>=2.0.0->huggingface_hub) (0.16.0) Downloading huggingface_hub-2.4.0-py3-none-any.whl (857 kB) ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 857.0/857.0 kB 59.6 MB/s 0:00:00 Downloading click-8.5.0-py3-none-any.whl (125 kB) Downloading hf_xet-1.7.1-cp38-abi3-manylinux2014_x86_64.manylinux_2_17_x86_64.whl (4.2 MB) ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 4.2/4.2 MB 345.5 MB/s 0:00:00 Downloading httpx2-2.16.0-py3-none-any.whl (97 kB) Downloading httpcore2-2.16.0-py3-none-any.whl (85 kB) Downloading tqdm-4.70.1-py3-none-any.whl (80 kB) Downloading truststore-0.10.5-py3-none-any.whl (19 kB) Installing collected packages: truststore, tqdm, hf-xet, click, httpcore2, httpx2, huggingface_hub Successfully installed click-8.5.0 hf-xet-1.7.1 httpcore2-2.16.0 httpx2-2.16.0 huggingface_hub-2.4.0 tqdm-4.70.1 truststore-0.10.5 /usr/local/bin/hf root@723c5c4d27ed:/# [serve] 0:python3* ``` 鍵の文字数と、`hf auth whoami` の出力（1回目は貼り間違い、2回目は貼り直したあと）: ``` echo ${#HF_TOKEN} hf auth whoami ``` ``` hf auth whoami 51 Hint: The `hf-cli` skill is not installed. Run `hf skills add -g` to teach your AI agents how to use the `hf` CLI. Error: Invalid user token. The token from HF_TOKEN environment variable is invalid. Note that HF_TOKEN takes precedence over `hf auth login`. Hint: set HF_DEBUG=1 as environment variable for full traceback. root@723c5c4d27ed:/# ``` ``` ✓ Logged in user: PixelstoMotors root@723c5c4d27ed:/# ``` ### ターミナル2（トンネル） ``` Last login: Fri Oct 30 18:30:48 on ttys006 (base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % ssh -N -L 8000:localhost:8000 -p -i ~/.ssh/id_ed25519 root@ ``` ### サーバーのログ serve.log（KIOXIA の lab/Log16/serve.log） `Connection from … opened` が、クライアントを起動するごとに1組、3回分出ています。 ``` INFO:root:Loading model... INFO:2026-10-30 09:37:38,436:jax._src.xla_bridge:925: Unable to initialize backend 'rocm': module 'jaxlib.xla_extension' has no attribute 'GpuAllocatorConfig' INFO:jax._src.xla_bridge:Unable to initialize backend 'rocm': module 'jaxlib.xla_extension' has no attribute 'GpuAllocatorConfig' INFO:2026-10-30 09:37:38,438:jax._src.xla_bridge:925: Unable to initialize backend 'tpu': INTERNAL: Failed to open libtpu.so: libtpu.so: cannot open shared object file: No such file or directory INFO:jax._src.xla_bridge:Unable to initialize backend 'tpu': INTERNAL: Failed to open libtpu.so: libtpu.so: cannot open shared object file: No such file or directory INFO:absl:orbax-checkpoint version: 0.11.13 INFO:absl:Created BasePyTreeCheckpointHandler: use_ocdbt=True, use_zarr3=False, pytree_metadata_options=PyTreeMetadataOptions(support_rich_types=False), array_metadata_store= INFO:absl:Restoring checkpoint from /workspace/ckpt/3999/params. INFO:absl:[thread=MainThread] Failed to get flag value for EXPERIMENTAL_ORBAX_USE_DISTRIBUTED_PROCESS_ID. INFO:absl:[process=0] /jax/checkpoint/read/bytes_per_sec: 1.2 GiB/s (total bytes: 6.3 GiB) (time elapsed: 5 seconds) (per-host) INFO:absl:Finished restoring checkpoint in 5.25 seconds from /workspace/ckpt/3999/params. INFO:openpi.shared.download:Downloading gs://big_vision/paligemma_tokenizer.model to /root/.cache/openpi/big_vision/paligemma_tokenizer.model INFO:tqdm_loggable.tqdm_logging:Progress on: -/4264023 rate:- remaining:? elapsed:00:00 postfix:- INFO:tqdm_loggable.tqdm_logging:Progress on: 4.07MiB/4.07MiB rate:4,260,350.1iB/s remaining:00:00 elapsed:00:01 postfix:- INFO:root:Loaded norm stats from /workspace/openpi/assets/pi05_mecharm_trim_lora/local/mecharm_redblock_box_teach_v1_trim2 INFO:root:Loaded norm stats from /workspace/ckpt/3999/assets/local/mecharm_redblock_box_teach_v1_trim2 INFO:root:Creating server (host: 723c5c4d27ed, ip: 192.168.6.2) INFO:websockets.server:server listening on 0.0.0.0:8000 INFO:websockets.server:connection open INFO:openpi.serving.websocket_policy_server:Connection from ('127.0.0.1', 44890) opened INFO:openpi.serving.websocket_policy_server:Connection from ('127.0.0.1', 44890) closed INFO:websockets.server:connection open INFO:openpi.serving.websocket_policy_server:Connection from ('127.0.0.1', 40876) opened INFO:openpi.serving.websocket_policy_server:Connection from ('127.0.0.1', 40876) closed INFO:websockets.server:connection open INFO:openpi.serving.websocket_policy_server:Connection from ('127.0.0.1', 46190) opened INFO:openpi.serving.websocket_policy_server:Connection from ('127.0.0.1', 46190) closed ``` ### serve.log を Mac へ持ち帰った所 ``` (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % scp -P -i ~/.ssh/id_ed25519 root@:/workspace/logs/serve.log /Volumes/KIOXIA/r2r2r-data/lab/Log16/serve.log serve.log 100% 2722 4.9KB/s 00:00 (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % ``` ### ターミナル3（アーム。18時36分〜19時13分、3回分の実機推論。KIOXIA の lab/Log16/terminal_1836-1915.txt） ``` Last login: Fri Oct 30 18:36:48 on ttys007 (base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % cd ~/r2r2r-scripts ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 'pgrep -f mecharm_daemon.py >/dev/null && echo デーモン動いてる || (setsid nohup python3 /home/ubuntu/mecharm_daemon.py >/home/ubuntu/daemon.log 2>&1 &); sleep 8; tail -3 ~/daemon.log; ss -ltn | grep 5555' python3 dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py RAW=/Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes python3 -c "from visual_servo_pick import get_daemon_socket, daemon_set_gripper, close_daemon_socket; get_daemon_socket(); daemon_set_gripper(0); close_daemon_socket()" python3 pi05/camera_align.py --raw-dir $RAW --episode run_0001_20261019_194818 --scale 5 --alpha 0.7 デーモン動いてる [DAEMON 18:13:44] send_angles 後の現在角度: [-0.26, 1.49, 1.75, -0.7, -2.28, -29.09] [DAEMON 18:13:44] cmd received: get_angles [DAEMON 18:13:44] 切断: ('192.168.2.2', 62315) [INIT] daemon 接続中... Traceback (most recent call last): File "/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py", line 97, in main() File "/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py", line 37, in main get_daemon_socket() File "/Users/shimazueiichi/r2r2r-scripts/visual_servo_pick.py", line 528, in get_daemon_socket s.connect((DAEMON_HOST, DAEMON_PORT)) ConnectionRefusedError: [Errno 61] Connection refused Traceback (most recent call last): File "", line 1, in File "/Users/shimazueiichi/r2r2r-scripts/visual_servo_pick.py", line 528, in get_daemon_socket s.connect((DAEMON_HOST, DAEMON_PORT)) ConnectionRefusedError: [Errno 61] Connection refused [REF ] 基準画像: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes/run_0001_20261019_194818/frames/000000.jpg [REF ] お手本 run_0001_20261019_194818 (つかんだ瞬間の j0 = +13.2°) [REF ] 重なった画像のブロックに実物のブロックを合わせると, お手本と同じ位置に置ける [ALIGN ] |dy| <= 20 なら OK. アームは HOME に置いた状態で合わせる. q または Esc で終了 (ファイルには何も書かない) [ALIGN ] dy= +13.1 dx= -12.9 response=0.009 → 測れない (相関が弱く dx/dy が当てにならない (response=0.009 < 0.20)) [ALIGN ] dy= +16.4 dx= -13.7 response=0.008 → 測れない (相関が弱く dx/dy が当てにならない (response=0.008 < 0.20)) [ALIGN ] dy= +11.4 dx= -12.5 response=0.010 → 測れない (相関が弱く dx/dy が当てにならない (response=0.010 < 0.20)) [ALIGN ] dy= +9.3 dx= -11.6 response=0.011 → 測れない (相関が弱く dx/dy が当てにならない (response=0.011 < 0.20)) [ALIGN ] dy= +8.3 dx= -11.1 response=0.012 → 測れない (相関が弱く dx/dy が当てにならない (response=0.012 < 0.20)) [ALIGN ] dy= +9.4 dx= -11.7 response=0.011 → 測れない (相関が弱く dx/dy が当てにならない (response=0.011 < 0.20)) [ALIGN ] dy= +6.1 dx= -10.7 response=0.013 → 測れない (相関が弱く dx/dy が当てにならない (response=0.013 < 0.20)) [ALIGN ] dy= +3.8 dx= -10.6 response=0.014 → 測れない (相関が弱く dx/dy が当てにならない (response=0.014 < 0.20)) [ALIGN ] dy= +7.2 dx= -11.4 response=0.011 → 測れない (相関が弱く dx/dy が当てにならない (response=0.011 < 0.20)) [ALIGN ] dy= +4.4 dx= -10.7 response=0.013 → 測れない (相関が弱く dx/dy が当てにならない (response=0.013 < 0.20)) [ALIGN ] dy= +2.6 dx= -9.9 response=0.015 → 測れない (相関が弱く dx/dy が当てにならない (response=0.015 < 0.20)) [ALIGN ] dy= +4.6 dx= -10.2 response=0.014 → 測れない (相関が弱く dx/dy が当てにならない (response=0.014 < 0.20)) [ALIGN ] dy= -3.1 dx= -8.3 response=0.024 → 測れない (相関が弱く dx/dy が当てにならない (response=0.024 < 0.20)) [ALIGN ] dy= +2.5 dx= -9.8 response=0.016 → 測れない (相関が弱く dx/dy が当てにならない (response=0.016 < 0.20)) [ALIGN ] dy= +0.0 dx= -9.0 response=0.017 → 測れない (相関が弱く dx/dy が当てにならない (response=0.017 < 0.20)) [ALIGN ] dy= +1.2 dx= -9.5 response=0.017 → 測れない (相関が弱く dx/dy が当てにならない (response=0.017 < 0.20)) [ALIGN ] dy= -0.5 dx= -8.7 response=0.019 → 測れない (相関が弱く dx/dy が当てにならない (response=0.019 < 0.20)) [ALIGN ] dy= +1.1 dx= -9.4 response=0.018 → 測れない (相関が弱く dx/dy が当てにならない (response=0.018 < 0.20)) [ALIGN ] dy= +0.1 dx= -9.0 response=0.018 → 測れない (相関が弱く dx/dy が当てにならない (response=0.018 < 0.20)) [ALIGN ] dy= +1.3 dx= -9.1 response=0.016 → 測れない (相関が弱く dx/dy が当てにならない (response=0.016 < 0.20)) [ALIGN ] dy= -3.4 dx= -8.4 response=0.025 → 測れない (相関が弱く dx/dy が当てにならない (response=0.025 < 0.20)) [ALIGN ] dy= -5.6 dx= -7.7 response=0.041 → 測れない (相関が弱く dx/dy が当てにならない (response=0.041 < 0.20)) [ALIGN ] dy= +2.3 dx= -10.4 response=0.014 → 測れない (相関が弱く dx/dy が当てにならない (response=0.014 < 0.20)) [ALIGN ] dy= +15.5 dx= -14.2 response=0.008 → 測れない (相関が弱く dx/dy が当てにならない (response=0.008 < 0.20)) [ALIGN ] dy= +13.6 dx= -13.5 response=0.008 → 測れない (相関が弱く dx/dy が当てにならない (response=0.008 < 0.20)) [ALIGN ] dy= +10.7 dx= -12.1 response=0.010 → 測れない (相関が弱く dx/dy が当てにならない (response=0.010 < 0.20)) [ALIGN ] dy= +2.2 dx= -9.2 response=0.017 → 測れない (相関が弱く dx/dy が当てにならない (response=0.017 < 0.20)) [ALIGN ] dy= -0.5 dx= -9.0 response=0.020 → 測れない (相関が弱く dx/dy が当てにならない (response=0.020 < 0.20)) [ALIGN ] dy= +1.4 dx= -9.9 response=0.015 → 測れない (相関が弱く dx/dy が当てにならない (response=0.015 < 0.20)) [ALIGN ] dy= +3.9 dx= -10.3 response=0.015 → 測れない (相関が弱く dx/dy が当てにならない (response=0.015 < 0.20)) [ALIGN ] dy= +2.1 dx= -10.0 response=0.017 → 測れない (相関が弱く dx/dy が当てにならない (response=0.017 < 0.20)) [ALIGN ] dy= -2.4 dx= -8.6 response=0.022 → 測れない (相関が弱く dx/dy が当てにならない (response=0.022 < 0.20)) [ALIGN ] dy= +0.1 dx= -12.9 response=0.016 → 測れない (相関が弱く dx/dy が当てにならない (response=0.016 < 0.20)) [ALIGN ] dy= -3.1 dx= -11.2 response=0.019 → 測れない (相関が弱く dx/dy が当てにならない (response=0.019 < 0.20)) [ALIGN ] dy= -65.4 dx= +16.9 response=-0.004 → 測れない (相関が弱く dx/dy が当てにならない (response=-0.004 < 0.20)) [ALIGN ] dy= -5.7 dx= -12.1 response=0.039 → 測れない (相関が弱く dx/dy が当てにならない (response=0.039 < 0.20)) [ALIGN ] dy= -71.7 dx= +8.8 response=-0.004 → 測れない (相関が弱く dx/dy が当てにならない (response=-0.004 < 0.20)) [ALIGN ] dy= +25.7 dx= -21.8 response=0.006 → 測れない (相関が弱く dx/dy が当てにならない (response=0.006 < 0.20)) [ALIGN ] dy= +1.1 dx= -13.7 response=0.020 → 測れない (相関が弱く dx/dy が当てにならない (response=0.020 < 0.20)) [ALIGN ] dy= -4.4 dx= -14.5 response=0.032 → 測れない (相関が弱く dx/dy が当てにならない (response=0.032 < 0.20)) [ALIGN ] dy= +0.1 dx= -14.1 response=0.022 → 測れない (相関が弱く dx/dy が当てにならない (response=0.022 < 0.20)) [ALIGN ] dy= -5.3 dx= -14.9 response=0.042 → 測れない (相関が弱く dx/dy が当てにならない (response=0.042 < 0.20)) [ALIGN ] dy= -6.9 dx= -19.4 response=0.055 → 測れない (相関が弱く dx/dy が当てにならない (response=0.055 < 0.20)) [ALIGN ] dy= -2.1 dx= -20.5 response=0.050 → 測れない (相関が弱く dx/dy が当てにならない (response=0.050 < 0.20)) [ALIGN ] dy= -5.2 dx= -20.9 response=0.093 → 測れない (相関が弱く dx/dy が当てにならない (response=0.093 < 0.20)) [ALIGN ] dy= -2.7 dx= -21.5 response=0.100 → 測れない (相関が弱く dx/dy が当てにならない (response=0.100 < 0.20)) [ALIGN ] dy= +0.9 dx= -20.4 response=0.092 → 測れない (相関が弱く dx/dy が当てにならない (response=0.092 < 0.20)) [ALIGN ] dy= +4.6 dx= -18.4 response=0.072 → 測れない (相関が弱く dx/dy が当てにならない (response=0.072 < 0.20)) [ALIGN ] dy= +2.7 dx= -18.2 response=0.078 → 測れない (相関が弱く dx/dy が当てにならない (response=0.078 < 0.20)) [ALIGN ] dy= +5.0 dx= -13.4 response=0.048 → 測れない (相関が弱く dx/dy が当てにならない (response=0.048 < 0.20)) [ALIGN ] dy= +3.9 dx= -15.4 response=0.058 → 測れない (相関が弱く dx/dy が当てにならない (response=0.058 < 0.20)) [ALIGN ] dy= +2.6 dx= -14.8 response=0.101 → 測れない (相関が弱く dx/dy が当てにならない (response=0.101 < 0.20)) [ALIGN ] dy= +4.0 dx= -12.4 response=0.087 → 測れない (相関が弱く dx/dy が当てにならない (response=0.087 < 0.20)) [ALIGN ] dy= +2.9 dx= -13.8 response=0.081 → 測れない (相関が弱く dx/dy が当てにならない (response=0.081 < 0.20)) [ALIGN ] dy= +1.8 dx= -13.9 response=0.096 → 測れない (相関が弱く dx/dy が当てにならない (response=0.096 < 0.20)) [ALIGN ] dy= -0.7 dx= -14.8 response=0.102 → 測れない (相関が弱く dx/dy が当てにならない (response=0.102 < 0.20)) [ALIGN ] dy= +3.3 dx= -12.6 response=0.060 → 測れない (相関が弱く dx/dy が当てにならない (response=0.060 < 0.20)) [ALIGN ] dy= +0.3 dx= -13.8 response=0.090 → 測れない (相関が弱く dx/dy が当てにならない (response=0.090 < 0.20)) [ALIGN ] dy= +3.3 dx= -12.5 response=0.062 → 測れない (相関が弱く dx/dy が当てにならない (response=0.062 < 0.20)) [ALIGN ] dy= -0.3 dx= -13.9 response=0.104 → 測れない (相関が弱く dx/dy が当てにならない (response=0.104 < 0.20)) [ALIGN ] dy= +3.4 dx= -12.7 response=0.061 → 測れない (相関が弱く dx/dy が当てにならない (response=0.061 < 0.20)) [ALIGN ] dy= -0.0 dx= -13.9 response=0.097 → 測れない (相関が弱く dx/dy が当てにならない (response=0.097 < 0.20)) [ALIGN ] dy= +5.3 dx= -13.0 response=0.042 → 測れない (相関が弱く dx/dy が当てにならない (response=0.042 < 0.20)) [ALIGN ] dy= +0.3 dx= -14.1 response=0.086 → 測れない (相関が弱く dx/dy が当てにならない (response=0.086 < 0.20)) [ALIGN ] dy= +3.0 dx= -12.6 response=0.067 → 測れない (相関が弱く dx/dy が当てにならない (response=0.067 < 0.20)) [ALIGN ] dy= -0.1 dx= -13.9 response=0.112 → 測れない (相関が弱く dx/dy が当てにならない (response=0.112 < 0.20)) ^C [STOP ] Ctrl-C [DONE ] 終了 (何も書いていません) (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % 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 [SAFE ] 安全範囲 (学習 19 本 ±10°): j0[-72,+55] j1[-9,+88] j2[-46,+41] j3[-18,+27] j4[-90,+10] j5[-51,-14] [SAFE ] 肩 j1 の上限は J1_SAFE_MAX=88° で切った (これを超える指令はアームが黙って捨てる / LOG352) 元データ: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes [MODE ] EXECUTE steps/chunk=10 max_move=20.0° log=/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_183910 [CAMERA] 写り方 dy=-18.8 dx=+0.3 response=0.075 → 測れない (相関が弱く dx/dy が当てにならない (response=0.075 < 0.20)) [CAMERA] 基準: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes/run_0001_20261019_183454/frames/000000.jpg [WARN ] 写り方が『測れない』だが --ignore-framing が指定されたので続けます [DONE ] 記録: /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_183910 Traceback (most recent call last): File "/Users/shimazueiichi/r2r2r-scripts/pi05/run_policy_client.py", line 1359, in sys.exit(main()) ^^^^^^ File "/Users/shimazueiichi/r2r2r-scripts/pi05/run_policy_client.py", line 927, in main get_daemon_socket() File "/Users/shimazueiichi/r2r2r-scripts/visual_servo_pick.py", line 528, in get_daemon_socket s.connect((DAEMON_HOST, DAEMON_PORT)) ConnectionRefusedError: [Errno 61] Connection refused (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % cd ~/r2r2r-scripts python3 dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py [INIT] daemon 接続中... Traceback (most recent call last): File "/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py", line 97, in main() File "/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py", line 37, in main get_daemon_socket() File "/Users/shimazueiichi/r2r2r-scripts/visual_servo_pick.py", line 528, in get_daemon_socket s.connect((DAEMON_HOST, DAEMON_PORT)) ConnectionRefusedError: [Errno 61] Connection refused (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % cd ~/r2r2r-scripts ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 'pkill -f mecharm_daemon.py; sleep 2; (setsid nohup python3 /home/ubuntu/mecharm_daemon.py >/home/ubuntu/daemon.log 2>&1 &); sleep 8; tail -5 ~/daemon.log; ss -ltn | grep 5555' (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 'pkill -f "[m]echarm_daemon.py"; sleep 2; (setsid nohup python3 /home/ubuntu/mecharm_daemon.py >/home/ubuntu/daemon.log 2>&1 &); sleep 8; tail -5 ~/daemon.log; ss -ltn | grep 5555' (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 'pkill -f "[m]echarm_daemon.py"; echo 止めた' 止めた (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 '(setsid nohup python3 /home/ubuntu/mecharm_daemon.py >/home/ubuntu/daemon.log 2>&1 &); sleep 8; tail -5 ~/daemon.log; ss -ltn | grep 5555' [DAEMON 15:10:43] MyCobot 初期化中 (/dev/ttyAMA0, baud=1000000)... Note: This class is no longer maintained since v3.6.0, please refer to the project documentation: https://github.com/elephantrobotics/pymycobot/blob/main/README.md [DAEMON 15:10:45] ✓ MyCobot 初期化完了 [DAEMON 15:10:45] startup_warmup angles=[0.17, 93.42, -11.07, 2.02, -84.19, -30.05] [DAEMON 15:10:45] ポート 5555 で待ち受け中... LISTEN0 1 0.0.0.0:5555 0.0.0.0:* (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % cd ~/r2r2r-scripts python3 dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py [INIT] daemon 接続中... [WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment [INIT] fresh_mode=1 [PRE] 現在姿勢: [0.17, 93.42, -11.07, 2.02, -84.19, -30.05] [SEND] HOME へ移動指令 [WAIT] t=2.6s max_err=1.31° [DONE] HOME 到達 max_err=1.31° [POST] 最終姿勢: [0.17, 1.14, 0.61, 0.52, 1.31, -30.05] [WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment [EXIT] fresh_mode=0, socket close (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % python3 -c "from visual_servo_pick import get_daemon_socket, daemon_set_gripper, close_daemon_socket; get_daemon_socket(); daemon_set_gripper(0); close_daemon_socket()" [Pi] GRIPPER: OPEN result= -1 (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % python3 dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py RAW=/Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes python3 -c "from visual_servo_pick import get_daemon_socket, daemon_set_gripper, close_daemon_socket; get_daemon_socket(); daemon_set_gripper(0); close_daemon_socket()" python3 pi05/camera_align.py --raw-dir $RAW --episode run_0001_20261019_194818 --scale 5 --alpha 0.7 [INIT] daemon 接続中... [WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment [INIT] fresh_mode=1 [PRE] 現在姿勢: [0.17, 1.14, 0.70, 0.26, 0.08, -30.05] [SEND] HOME へ移動指令 [WAIT] t=2.0s max_err=1.14° [DONE] HOME 到達 max_err=1.14° [POST] 最終姿勢: [0.17, 1.14, 0.70, 0.26, 0.08, -30.05] [WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment [EXIT] fresh_mode=0, socket close [Pi] GRIPPER: OPEN result= -1 [REF ] 基準画像: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes/run_0001_20261019_194818/frames/000000.jpg [REF ] お手本 run_0001_20261019_194818 (つかんだ瞬間の j0 = +13.2°) [REF ] 重なった画像のブロックに実物のブロックを合わせると, お手本と同じ位置に置ける [ALIGN ] |dy| <= 20 なら OK. アームは HOME に置いた状態で合わせる. q または Esc で終了 (ファイルには何も書かない) [ALIGN ] dy= -0.5 dx= -1.9 response=0.967 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) ^C [STOP ] Ctrl-C [DONE ] 終了 (何も書いていません) (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % 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 [SAFE ] 安全範囲 (学習 19 本 ±10°): j0[-72,+55] j1[-9,+88] j2[-46,+41] j3[-18,+27] j4[-90,+10] j5[-51,-14] [SAFE ] 肩 j1 の上限は J1_SAFE_MAX=88° で切った (これを超える指令はアームが黙って捨てる / LOG352) 元データ: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes [MODE ] EXECUTE steps/chunk=10 max_move=20.0° log=/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_184245 [CAMERA] 写り方 dy=-6.6 dx=+4.9 response=0.575 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [CAMERA] 基準: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes/run_0001_20261019_183454/frames/000000.jpg [POLICY] 接続 ws://localhost:8000 metadata={} [RECORD] 上カメラと実測角を 1 秒 10 コマで記録します → /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_184245/recording [GRIP ] 開始時のグリッパー開放は, 最初の目標が検査を通ってから行います (爪が閉じていたら手で開けておく) [STATE ] 実測を渡す (直前に送った目標がない) [000] 推論 48251ms 10ステップ先 Δ= +4.4 +16.6 +11.0 -2.4 -24.6 +3.0 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [GRIP ] 開始時にグリッパーを開く [Pi] GRIPPER: OPEN result= -1 [SEND] pi05_chunk000_via02: send_angles (single) [ +0.86, +5.69, +2.97, -0.28, -6.38, -29.42] speed=20 wait=1.0s (pre max|Δ|=6.46°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.89° (tol 3.0°) [VIA ] step 2 Δ= +0.7 +4.5 +2.3 -0.5 -6.5 +0.6 着いた [SEND] pi05_chunk000_via05: send_angles (single) [ +2.46, +10.87, +6.60, -1.04, -14.43, -28.45] speed=20 wait=1.0s (pre max|Δ|=8.55°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.90° (tol 3.0°) [VIA ] step 5 Δ= +1.7 +4.4 +2.7 -1.0 -8.5 +1.4 着いた [SEND] pi05_chunk000_via08: send_angles (single) [ +4.16, +16.15, +10.60, -1.97, -22.35, -27.35] speed=20 wait=1.0s (pre max|Δ|=8.12°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.90° (tol 3.0°) [VIA ] step 8 Δ= +2.1 +4.4 +3.2 -0.7 -8.1 +1.6 着いた [SEND] pi05_chunk000_via09: send_angles (single) [ +4.59, +17.74, +11.68, -2.17, -24.53, -27.08] speed=20 wait=1.0s (pre max|Δ|=2.47°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.80° (tol 3.0°) [VIA ] step 9 Δ= +0.3 +0.7 +0.3 +0.2 -2.5 +0.6 着いた [STATE ] 目標を渡す 目標と実測の最大差 0.80°（関節 j1） [001] 推論 939ms 10ステップ先 Δ= +6.5 +18.2 +11.8 +0.2 -27.5 +0.7 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk001_via02: send_angles (single) [ +6.09, +22.96, +15.18, -2.38, -32.76, -26.72] speed=20 wait=1.0s (pre max|Δ|=7.45°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.85° (tol 3.0°) [VIA ] step 2 Δ= +1.8 +4.4 +2.9 -0.0 -7.5 +0.5 着いた [SEND] pi05_chunk001_via05: send_angles (single) [ +8.08, +28.79, +19.14, -2.22, -41.76, -26.64] speed=20 wait=1.0s (pre max|Δ|=9.77°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.91° (tol 3.0°) [VIA ] step 5 Δ= +2.3 +5.0 +3.2 +0.6 -9.8 +0.4 着いた [SEND] pi05_chunk001_via08: send_angles (single) [ +10.01, +34.78, +23.19, -2.28, -50.25, -26.62] speed=20 wait=1.0s (pre max|Δ|=8.86°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.99° (tol 3.0°) [VIA ] step 8 Δ= +2.5 +5.1 +3.4 +0.5 -8.9 +0.3 着いた [SEND] pi05_chunk001_via09: send_angles (single) [ +10.78, +36.76, +24.11, -2.21, -52.79, -26.57] speed=20 wait=1.0s (pre max|Δ|=2.87°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.94° (tol 3.0°) [VIA ] step 9 Δ= +0.9 +1.0 +0.4 +0.4 -2.9 +0.2 着いた [STATE ] 目標を渡す 目標と実測の最大差 0.94°（関節 j1） [002] 推論 2624ms 10ステップ先 Δ= +2.2 +7.7 +5.6 -0.3 -10.1 +0.2 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk002_via02: send_angles (single) [ +12.22, +42.80, +28.11, -2.63, -60.76, -26.56] speed=20 wait=1.0s (pre max|Δ|=8.03°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [VIA ] step 2 Δ= +1.3 +5.1 +4.4 -0.2 -8.0 +0.2 着いた [SEND] pi05_chunk002_via05: send_angles (single) [ +12.88, +45.66, +30.00, -2.74, -63.64, -26.57] speed=20 wait=1.0s (pre max|Δ|=3.09°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.09° (tol 3.0°) [VIA ] step 5 Δ= +0.8 +1.9 +1.7 -0.5 -3.1 +0.2 着いた [SEND] pi05_chunk002_via08: send_angles (single) [ +13.06, +45.63, +29.77, -2.76, -63.06, -26.58] speed=20 wait=1.0s (pre max|Δ|=1.27°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.27° (tol 3.0°) [VIA ] step 8 Δ= +0.1 -1.1 -0.6 -0.7 +1.3 +0.2 着いた [SEND] pi05_chunk002_via09: send_angles (single) [ +13.06, +45.36, +29.37, -2.78, -62.82, -26.57] speed=20 wait=1.0s (pre max|Δ|=1.51°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.39° (tol 3.0°) [VIA ] step 9 Δ= +0.1 -1.4 -1.0 -0.6 +1.5 +0.1 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.39°（関節 j1） [003] 推論 1079ms 10ステップ先 Δ= +0.8 +9.5 -9.6 -0.5 +2.5 +0.0 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk003_via02: send_angles (single) [ +13.28, +49.09, +26.88, -2.75, -62.60, -26.61] speed=20 wait=1.0s (pre max|Δ|=3.53°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.06° (tol 3.0°) [VIA ] step 2 Δ= +0.3 +2.3 -3.5 -0.4 +0.8 +0.1 着いた [SEND] pi05_chunk003_via05: send_angles (single) [ +13.70, +52.50, +23.94, -2.78, -61.75, -26.65] speed=20 wait=1.0s (pre max|Δ|=4.00°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.02° (tol 3.0°) [VIA ] step 5 Δ= +0.7 +2.6 -4.0 -0.3 +1.2 +0.1 着いた [SEND] pi05_chunk003_via08: send_angles (single) [ +13.81, +55.57, +21.44, -2.87, -61.24, -26.69] speed=20 wait=1.0s (pre max|Δ|=3.43°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.97° (tol 3.0°) [VIA ] step 8 Δ= +0.1 +2.1 -3.4 -0.2 +0.8 +0.0 着いた [SEND] pi05_chunk003_via09: send_angles (single) [ +13.79, +56.22, +20.77, -2.87, -61.05, -26.70] speed=20 wait=1.0s (pre max|Δ|=1.64°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.37° (tol 3.0°) [VIA ] step 9 Δ= +0.1 -0.3 -1.6 -0.2 +0.5 +0.0 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.37°（関節 j2） [004] 推論 2240ms 5ステップ先 Δ= +0.1 +1.6 -2.8 -0.4 +0.4 -0.0 グリッパー→閉 [VIA ] 中継点 2 点: step 2, 4 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk004_via02: send_angles (single) [ +13.85, +58.97, +19.22, -2.95, -60.78, -26.72] speed=20 wait=1.0s (pre max|Δ|=2.92°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.73° (tol 3.0°) [VIA ] step 2 Δ= +0.1 +1.9 -2.9 -0.3 +0.6 -0.0 着いた [SEND] pi05_chunk004_via04: send_angles (single) [ +13.78, +58.73, +19.34, -2.98, -60.98, -26.72] speed=20 wait=1.0s (pre max|Δ|=0.94°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.94° (tol 3.0°) [VIA ] step 4 Δ= +0.1 -0.9 -0.6 -0.4 +0.3 +0.1 着いた [Pi] GRIPPER: VALUE=20 speed=100 result= -1 [STATE ] 目標を渡す 目標と実測の最大差 1.59°（関節 j4） [005] 推論 1230ms 10ステップ先 Δ= -0.1 -9.3 +6.7 -0.3 -1.0 +0.2 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk005_via02: send_angles (single) [ +13.74, +58.17, +19.54, -2.76, -61.64, -27.06] speed=20 wait=1.0s (pre max|Δ|=1.50°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.33° (tol 3.0°) [VIA ] step 2 Δ= +0.5 -1.5 -0.3 -0.1 +0.6 +0.0 着いた [SEND] pi05_chunk005_via05: send_angles (single) [ +13.70, +55.40, +22.28, -2.80, -62.40, -26.92] speed=20 wait=1.0s (pre max|Δ|=4.10°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.02° (tol 3.0°) [VIA ] step 5 Δ= +0.4 -4.1 +2.4 -0.2 -0.4 +0.2 着いた [SEND] pi05_chunk005_via08: send_angles (single) [ +13.33, +51.27, +25.75, -2.85, -63.31, -26.75] speed=20 wait=1.0s (pre max|Δ|=5.59°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.90° (tol 3.0°) [VIA ] step 8 Δ= +0.1 -5.6 +3.9 +0.1 -2.9 +0.3 着いた [SEND] pi05_chunk005_via09: send_angles (single) [ +13.21, +50.32, +26.58, -2.83, -63.54, -26.74] speed=20 wait=1.0s (pre max|Δ|=2.85°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.97° (tol 3.0°) [VIA ] step 9 Δ= -0.1 -2.8 +0.7 +0.3 -1.8 +0.1 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.97°（関節 j1） [006] 推論 820ms 10ステップ先 Δ=+10.8 -4.3 -8.3 +2.9 +2.6 -2.5 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk006_via02: send_angles (single) [ +13.00, +47.21, +29.60, -2.89, -64.16, -26.60] speed=20 wait=1.0s (pre max|Δ|=5.08°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.74° (tol 3.0°) [VIA ] step 2 Δ= -0.3 -5.1 +3.0 +0.2 -1.7 +0.2 着いた [SEND] pi05_chunk006_via05: send_angles (single) [ +15.88, +46.95, +27.63, -2.24, -63.29, -27.20] speed=20 wait=1.0s (pre max|Δ|=2.61°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.00° (tol 3.0°) [VIA ] step 5 Δ= +2.6 -2.0 -2.0 +0.8 +1.3 -0.4 着いた [SEND] pi05_chunk006_via08: send_angles (single) [ +22.13, +47.71, +20.35, -0.72, -60.45, -28.76] speed=20 wait=1.0s (pre max|Δ|=8.56°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.24° (tol 3.0°) [VIA ] step 8 Δ= +6.4 -1.2 -8.6 +1.8 +4.1 -2.0 着いた [SEND] pi05_chunk006_via09: send_angles (single) [ +24.11, +47.97, +18.35, -0.26, -59.79, -29.28] speed=20 wait=1.0s (pre max|Δ|=3.09°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.07° (tol 3.0°) [VIA ] step 9 Δ= +2.1 -1.0 -3.1 +1.1 +1.6 -1.4 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.07°（関節 j2） [007] 推論 804ms 10ステップ先 Δ=+19.9 +6.2 -23.9 +4.8 +10.4 -5.3 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk007_via02: send_angles (single) [ +30.90, +49.12, +10.21, +1.41, -56.57, -30.86] speed=20 wait=1.0s (pre max|Δ|=9.21°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.21° (tol 3.0°) [VIA ] step 2 Δ= +6.8 +0.2 -9.2 +1.8 +3.6 -2.0 着いた [SEND] pi05_chunk007_via05: send_angles (single) [ +37.72, +50.88, +1.75, +3.06, -52.25, -32.52] speed=20 wait=1.0s (pre max|Δ|=9.67°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.15° (tol 3.0°) [VIA ] step 5 Δ= +7.0 +1.0 -9.7 +2.2 +4.8 -1.9 着いた [SEND] pi05_chunk007_via08: send_angles (single) [ +43.30, +53.70, -3.93, +4.26, -49.66, -33.87] speed=20 wait=1.0s (pre max|Δ|=6.83°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.21° (tol 3.0°) [VIA ] step 8 Δ= +5.6 +1.9 -6.8 +1.5 +3.0 -1.7 着いた [SEND] pi05_chunk007_via09: send_angles (single) [ +44.01, +55.15, -4.51, +4.44, -49.84, -34.01] speed=20 wait=1.0s (pre max|Δ|=1.79°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.44° (tol 3.0°) [VIA ] step 9 Δ= +0.9 +0.4 -1.8 -0.0 +0.1 -0.4 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.44°（関節 j2） [008] 推論 2302ms 10ステップ先 Δ= -0.6 +11.2 -2.0 +1.5 -11.0 -0.4 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk008_via02: send_angles (single) [ +43.84, +59.72, -4.49, +4.91, -53.91, -34.00] speed=20 wait=1.0s (pre max|Δ|=3.99°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.53° (tol 3.0°) [VIA ] step 2 Δ= -0.2 +3.4 -1.4 +0.5 -4.0 -0.3 着いた [SEND] pi05_chunk008_via05: send_angles (single) [ +43.66, +64.05, -4.72, +5.38, -57.52, -34.02] speed=20 wait=1.0s (pre max|Δ|=5.14°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.19° (tol 3.0°) [VIA ] step 5 Δ= -0.2 +3.2 -1.6 +0.7 -5.1 -0.4 着いた [SEND] pi05_chunk008_via08: send_angles (single) [ +43.52, +67.16, -5.01, +5.77, -60.46, -34.07] speed=20 wait=1.0s (pre max|Δ|=2.21°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.76° (tol 3.0°) [VIA ] step 8 Δ= -0.6 +2.2 -1.2 +0.8 -1.8 -0.5 着いた [SEND] pi05_chunk008_via09: send_angles (single) [ +43.47, +67.53, -5.10, +5.86, -60.97, -34.09] speed=20 wait=1.0s (pre max|Δ|=1.25°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.25° (tol 3.0°) [VIA ] step 9 Δ= -0.7 -0.3 -0.6 +0.4 +1.3 -0.5 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.25°（関節 j4） [009] 推論 922ms 2ステップ先 Δ= -0.9 -0.7 -0.6 +0.4 +1.3 -0.6 グリッパー→開 [VIA ] 中継点 1 点: step 1 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk009_via01: send_angles (single) [ +43.24, +67.31, -5.12, +5.98, -60.89, -34.17] speed=20 wait=1.0s (pre max|Δ|=1.24°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.98° (tol 3.0°) [VIA ] step 1 Δ= -0.9 -0.7 -0.6 +0.4 +1.2 -0.6 着いた [Pi] GRIPPER: OPEN result= -1 [STATE ] 目標を渡す 目標と実測の最大差 0.87°（関節 j4） [010] 推論 881ms 10ステップ先 Δ= +0.7 -13.1 +0.1 -0.5 +9.6 +0.1 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk010_via02: send_angles (single) [ +43.31, +64.10, -5.10, +6.09, -58.31, -34.52] speed=20 wait=1.0s (pre max|Δ|=3.92°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.81° (tol 3.0°) [VIA ] step 2 Δ= +0.3 -3.9 -0.2 -0.4 +1.8 +0.3 着いた [SEND] pi05_chunk010_via05: send_angles (single) [ +43.81, +60.78, -5.37, +6.07, -55.11, -34.66] speed=20 wait=1.0s (pre max|Δ|=5.13°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.32° (tol 3.0°) [VIA ] step 5 Δ= +0.8 -5.1 -0.9 +0.5 +2.2 +0.1 着いた [SEND] pi05_chunk010_via08: send_angles (single) [ +44.08, +56.72, -5.15, +6.26, -51.62, -34.82] speed=20 wait=1.0s (pre max|Δ|=6.38°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.16° (tol 3.0°) [VIA ] step 8 Δ= +0.3 -6.4 -0.8 +0.6 +2.0 -0.0 着いた [SEND] pi05_chunk010_via09: send_angles (single) [ +43.65, +54.89, -4.83, +6.28, -50.37, -34.79] speed=20 wait=1.0s (pre max|Δ|=3.99°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.15° (tol 3.0°) [VIA ] step 9 Δ= -0.0 -4.0 -0.5 +0.6 +0.7 +0.1 着いた [STATE ] 目標を渡す 目標と実測の最大差 2.15°（関節 j1） [011] 推論 2036ms 10ステップ先 Δ=-16.6 -27.3 +2.9 -3.3 +23.2 +2.1 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk011_via02: send_angles (single) [ +41.97, +49.52, -4.25, +5.69, -45.64, -34.58] speed=20 wait=1.0s (pre max|Δ|=7.52°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.07° (tol 3.0°) [VIA ] step 2 Δ= -1.7 -7.5 -0.0 -0.2 +4.7 +0.3 着いた [SEND] pi05_chunk011_via05: send_angles (single) [ +36.34, +41.96, -3.16, +4.45, -38.72, -33.89] speed=20 wait=1.0s (pre max|Δ|=9.63°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.98° (tol 3.0°) [VIA ] step 5 Δ= -5.8 -9.6 +0.5 -2.0 +7.2 +1.0 着いた [SEND] pi05_chunk011_via08: send_angles (single) [ +29.57, +33.03, -1.85, +2.87, -30.27, -33.07] speed=20 wait=1.0s (pre max|Δ|=10.91°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.95° (tol 3.0°) [VIA ] step 8 Δ= -7.1 -10.9 +0.8 -2.1 +8.4 +1.3 着いた [SEND] pi05_chunk011_via09: send_angles (single) [ +27.08, +29.70, -1.29, +2.38, -27.21, -32.81] speed=20 wait=1.0s (pre max|Δ|=5.28°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.76° (tol 3.0°) [VIA ] step 9 Δ= -2.6 -5.3 -0.1 -0.9 +3.3 +0.6 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.76°（関節 j1） [012] 推論 990ms 10ステップ先 Δ=-22.8 -29.5 +1.2 -2.0 +25.6 +2.0 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk012_via02: send_angles (single) [ +20.24, +21.42, -0.21, +1.05, -19.17, -31.94] speed=20 wait=1.0s (pre max|Δ|=10.04°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.69° (tol 3.0°) [VIA ] step 2 Δ= -6.6 -10.0 +0.3 -1.0 +8.3 +1.1 着いた [SEND] pi05_chunk012_via05: send_angles (single) [ +13.02, +12.19, +0.60, +0.26, -10.84, -31.32] speed=20 wait=1.0s (pre max|Δ|=10.92°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.52° (tol 3.0°) [VIA ] step 5 Δ= -7.3 -10.9 +0.2 -1.1 +8.4 +0.8 着いた [SEND] pi05_chunk012_via08: send_angles (single) [ +5.71, +3.91, +0.75, +0.13, -3.44, -31.12] speed=20 wait=1.0s (pre max|Δ|=9.80°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.36° (tol 3.0°) [VIA ] step 8 Δ= -7.5 -9.8 -0.5 -0.4 +7.7 +0.4 着いた [SEND] pi05_chunk012_via09: send_angles (single) [ +4.01, +1.96, +0.70, +0.11, -1.89, -31.06] speed=20 wait=1.0s (pre max|Δ|=3.31°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.29° (tol 3.0°) [VIA ] step 9 Δ= -2.1 -3.3 -0.5 -0.1 +1.9 +0.2 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.29°（関節 j1） [013] 推論 1193ms 10ステップ先 Δ= -2.0 -2.1 -0.4 -0.1 +0.4 +0.3 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > h [SEND] pi05_home: send_angles (single) [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=3.69°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.40° (tol 2.5°) [RECORD] 記録終了 コマ 2174 (9.034 fps) 実測角 1110 回 (4.61 Hz) → /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_184245/recording 結果: y=成功 / n=失敗 / Enter=記録しない > y [DONE ] 記録: /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_184245 (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % python3 dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py python3 -c "from visual_servo_pick import get_daemon_socket, daemon_set_gripper, close_daemon_socket; get_daemon_socket(); daemon_set_gripper(0); close_daemon_socket()" python3 pi05/camera_align.py --raw-dir $RAW --episode run_0001_20261019_194818 --scale 5 --alpha 0.7 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 [INIT] daemon 接続中... [WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment [INIT] fresh_mode=1 [PRE] 現在姿勢: [-0.43, 1.05, 1.23, 0.08, 0.17, -30.14] [SEND] HOME へ移動指令 [WAIT] t=2.0s max_err=1.23° [DONE] HOME 到達 max_err=1.23° [POST] 最終姿勢: [-0.43, 1.05, 1.23, 0.08, 0.17, -30.14] [WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment [EXIT] fresh_mode=0, socket close [Pi] GRIPPER: OPEN result= -1 [REF ] 基準画像: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes/run_0001_20261019_194818/frames/000000.jpg [REF ] お手本 run_0001_20261019_194818 (つかんだ瞬間の j0 = +13.2°) [REF ] 重なった画像のブロックに実物のブロックを合わせると, お手本と同じ位置に置ける [ALIGN ] |dy| <= 20 なら OK. アームは HOME に置いた状態で合わせる. q または Esc で終了 (ファイルには何も書かない) [ALIGN ] dy= +1.0 dx= -3.6 response=0.897 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +2.6 dx= +2.7 response=0.816 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +4.5 dx= +4.3 response=0.817 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +2.9 dx= +1.8 response=0.873 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +1.7 dx= +3.4 response=0.902 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +3.2 dx= +6.9 response=0.617 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +6.2 dx= +9.3 response=0.503 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +9.4 dx= +9.5 response=0.531 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +10.6 dx= +11.3 response=0.433 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +8.4 dx= +10.3 response=0.658 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +6.5 dx= +10.2 response=0.726 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +3.3 dx= +10.4 response=0.648 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +2.2 dx= +7.9 response=0.564 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [ALIGN ] dy= +0.8 dx= +4.4 response=0.921 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) ^C [STOP ] Ctrl-C [DONE ] 終了 (何も書いていません) [SAFE ] 安全範囲 (学習 19 本 ±10°): j0[-72,+55] j1[-9,+88] j2[-46,+41] j3[-18,+27] j4[-90,+10] j5[-51,-14] [SAFE ] 肩 j1 の上限は J1_SAFE_MAX=88° で切った (これを超える指令はアームが黙って捨てる / LOG352) 元データ: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes [MODE ] EXECUTE steps/chunk=10 max_move=20.0° log=/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_185133 [CAMERA] 写り方 dy=-5.2 dx=+10.9 response=0.552 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [CAMERA] 基準: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes/run_0001_20261019_183454/frames/000000.jpg [POLICY] 接続 ws://localhost:8000 metadata={} [RECORD] 上カメラと実測角を 1 秒 10 コマで記録します → /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_185133/recording [GRIP ] 開始時のグリッパー開放は, 最初の目標が検査を通ってから行います (爪が閉じていたら手で開けておく) [STATE ] 実測を渡す (直前に送った目標がない) [000] 推論 1778ms 10ステップ先 Δ= +4.2 +16.1 +11.8 -3.0 -23.2 +2.9 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [GRIP ] 開始時にグリッパーを開く [Pi] GRIPPER: OPEN result= -1 [SEND] pi05_chunk000_via02: send_angles (single) [ +0.40, +5.31, +3.90, -0.54, -6.04, -29.51] speed=20 wait=1.0s (pre max|Δ|=6.21°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.02° (tol 3.0°) [VIA ] step 2 Δ= +0.8 +4.3 +2.7 -0.6 -6.2 +0.6 着いた [SEND] pi05_chunk000_via05: send_angles (single) [ +1.86, +10.62, +7.89, -1.57, -13.51, -28.53] speed=20 wait=1.0s (pre max|Δ|=8.42°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.98° (tol 3.0°) [VIA ] step 5 Δ= +1.8 +4.5 +3.0 -1.3 -8.4 +1.2 着いた [SEND] pi05_chunk000_via08: send_angles (single) [ +3.47, +15.80, +12.00, -2.61, -20.91, -27.50] speed=20 wait=1.0s (pre max|Δ|=8.26°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.89° (tol 3.0°) [VIA ] step 8 Δ= +2.1 +4.4 +3.1 -1.2 -8.3 +1.2 着いた [SEND] pi05_chunk000_via09: send_angles (single) [ +3.81, +17.17, +13.01, -2.92, -23.08, -27.20] speed=20 wait=1.0s (pre max|Δ|=2.69°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.84° (tol 3.0°) [VIA ] step 9 Δ= +0.5 +0.5 +0.2 +0.1 -2.7 +0.5 着いた [STATE ] 目標を渡す 目標と実測の最大差 0.84°（関節 j1） [001] 推論 873ms 10ステップ先 Δ= +6.2 +17.9 +11.4 -0.7 -29.9 +1.1 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > b [REWIND] 推論 0 を巻き戻す: 中継点 4 点 → 始めの姿勢 -0.4 +1.1 +1.2 +0.1 +0.2 -30.1 [SEND] pi05_rewind_00: send_angles (single) [ +3.47, +15.80, +12.00, -2.61, -20.91, -27.50] speed=20 wait=1.0s (pre max|Δ|=2.30°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.79° (tol 3.0°) [BACK ] 1/4 → +3.5 +15.8 +12.0 -2.6 -20.9 -27.5 着いた [SEND] pi05_rewind_01: send_angles (single) [ +1.86, +10.62, +7.89, -1.57, -13.51, -28.53] speed=20 wait=1.0s (pre max|Δ|=8.46°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.51° (tol 3.0°) [BACK ] 2/4 → +1.9 +10.6 +7.9 -1.6 -13.5 -28.5 着いた [SEND] pi05_rewind_02: send_angles (single) [ +0.40, +5.31, +3.90, -0.54, -6.04, -29.51] speed=20 wait=1.0s (pre max|Δ|=8.19°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.46° (tol 3.0°) [BACK ] 3/4 → +0.4 +5.3 +3.9 -0.5 -6.0 -29.5 着いた [SEND] pi05_rewind_03: send_angles (single) [ -0.43, +1.05, +1.23, +0.08, +0.17, -30.14] speed=20 wait=1.0s (pre max|Δ|=6.67°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.40° (tol 3.0°) [BACK ] 4/4 → -0.4 +1.0 +1.2 +0.1 +0.2 -30.1 着いた [STATE ] 実測を渡す (直前に送った目標がない) [002] 推論 2338ms 10ステップ先 Δ= +4.6 +19.1 +12.3 -2.1 -27.5 +2.6 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > c [CORR ] 補正 1 に入りました (推論 2 のあと) [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉 Enter = 補正を終えてモデルの推論に戻る / b = 補正を取り消して始めの姿勢へ戻る ? = この一覧 [補正] 関節 j0 刻み 1.0° 補正目標 j0=+0.1 j1=+2.2 j2=+2.6 j3=-0.2 j4=+0.0 j5=-29.8 グリッパー 開 [SEND] pi05_corr01_000: send_angles (single) [ -0.92, +2.19, +2.63, -0.17, +0.00, -29.79] speed=20 wait=1.0s (pre max|Δ|=1.00°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.88° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-0.9 j1=+2.2 j2=+2.6 j3=-0.2 j4=+0.0 j5=-29.8 グリッパー 開 [SEND] pi05_corr01_001: send_angles (single) [ -1.92, +2.19, +2.63, -0.17, +0.00, -29.79] speed=20 wait=1.0s (pre max|Δ|=0.88°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.88° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-1.9 j1=+2.2 j2=+2.6 j3=-0.2 j4=+0.0 j5=-29.8 グリッパー 開 [SEND] pi05_corr01_002: send_angles (single) [ -2.92, +2.19, +2.63, -0.17, +0.00, -29.79] speed=20 wait=1.0s (pre max|Δ|=0.90°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.88° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-2.9 j1=+2.2 j2=+2.6 j3=-0.2 j4=+0.0 j5=-29.8 グリッパー 開 [SEND] pi05_corr01_003: send_angles (single) [ -1.92, +2.19, +2.63, -0.17, +0.00, -29.79] speed=20 wait=1.0s (pre max|Δ|=1.15°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.88° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-1.9 j1=+2.2 j2=+2.6 j3=-0.2 j4=+0.0 j5=-29.8 グリッパー 開 [SEND] pi05_corr01_004: send_angles (single) [ -0.92, +2.19, +2.63, -0.17, +0.00, -29.79] speed=20 wait=1.0s (pre max|Δ|=0.88°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.88° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-0.9 j1=+2.2 j2=+2.6 j3=-0.2 j4=+0.0 j5=-29.8 グリッパー 開 [SEND] pi05_corr01_005: send_angles (single) [ -1.92, +2.19, +2.63, -0.17, +0.00, -29.79] speed=20 wait=1.0s (pre max|Δ|=0.88°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.88° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-1.9 j1=+2.2 j2=+2.6 j3=-0.2 j4=+0.0 j5=-29.8 グリッパー 開 [SEND] pi05_corr01_006: send_angles (single) [ -2.92, +2.19, +2.63, -0.17, +0.00, -29.79] speed=20 wait=1.0s (pre max|Δ|=1.17°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.88° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-2.9 j1=+2.2 j2=+2.6 j3=-0.2 j4=+0.0 j5=-29.8 グリッパー 開 [CORR ] 補正 1 を終えました (7 刻み). この姿勢から推論に戻ります [STATE ] 実測を渡す (直前に送った目標がない) [003] 推論 1085ms 10ステップ先 Δ= +1.6 +19.4 +10.2 -2.5 -26.6 +2.6 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > c [CORR ] 補正 2 に入りました (推論 3 のあと) [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉 Enter = 補正を終えてモデルの推論に戻る / b = 補正を取り消して始めの姿勢へ戻る ? = この一覧 [補正] 関節 j0 刻み 1.0° 補正目標 j0=-3.2 j1=+3.1 j2=+3.3 j3=-0.1 j4=+0.1 j5=-29.8 グリッパー 開 [補正] 関節 j0 刻み 1.0° 補正目標 j0=-3.2 j1=+3.1 j2=+3.3 j3=-0.1 j4=+0.1 j5=-29.8 グリッパー 開 [補正] 関節 j0 刻み 1.0° 補正目標 j0=-3.2 j1=+3.1 j2=+3.3 j3=-0.1 j4=+0.1 j5=-29.8 グリッパー 開 [補正] 関節 j0 刻み 1.0° 補正目標 j0=-3.2 j1=+3.1 j2=+3.3 j3=-0.1 j4=+0.1 j5=-29.8 グリッパー 開 [SEND] pi05_corr02_000: send_angles (single) [ -4.16, +3.07, +3.33, -0.08, +0.08, -29.79] speed=20 wait=1.0s (pre max|Δ|=1.00°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.79° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-4.2 j1=+3.1 j2=+3.3 j3=-0.1 j4=+0.1 j5=-29.8 グリッパー 開 [SEND] pi05_corr02_001: send_angles (single) [ -5.16, +3.07, +3.33, -0.08, +0.08, -29.79] speed=20 wait=1.0s (pre max|Δ|=0.79°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.79° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-5.2 j1=+3.1 j2=+3.3 j3=-0.1 j4=+0.1 j5=-29.8 グリッパー 開 [SEND] pi05_corr02_002: send_angles (single) [ -6.16, +3.07, +3.33, -0.08, +0.08, -29.79] speed=20 wait=1.0s (pre max|Δ|=0.98°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.79° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-6.2 j1=+3.1 j2=+3.3 j3=-0.1 j4=+0.1 j5=-29.8 グリッパー 開 [SEND] pi05_corr02_003: send_angles (single) [ -5.16, +3.07, +3.33, -0.08, +0.08, -29.79] speed=20 wait=1.0s (pre max|Δ|=0.90°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.79° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-5.2 j1=+3.1 j2=+3.3 j3=-0.1 j4=+0.1 j5=-29.8 グリッパー 開 [SEND] pi05_corr02_004: send_angles (single) [ -4.16, +3.07, +3.33, -0.08, +0.08, -29.79] speed=20 wait=1.0s (pre max|Δ|=1.02°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.79° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-4.2 j1=+3.1 j2=+3.3 j3=-0.1 j4=+0.1 j5=-29.8 グリッパー 開 [CORR ] 補正 2 を終えました (5 刻み). この姿勢から推論に戻ります [STATE ] 実測を渡す (直前に送った目標がない) [004] 推論 1203ms 10ステップ先 Δ= -6.1 +21.0 +8.3 -1.4 -27.5 +2.1 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk004_via02: send_angles (single) [ -6.93, +10.04, +5.82, -0.24, -7.66, -29.34] speed=20 wait=1.0s (pre max|Δ|=8.01°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.94° (tol 3.0°) [VIA ] step 2 Δ= -2.5 +6.2 +1.8 -0.2 -8.0 +0.4 着いた [SEND] pi05_chunk004_via05: send_angles (single) [ -8.84, +16.48, +8.55, -0.99, -15.94, -28.55] speed=20 wait=1.0s (pre max|Δ|=7.51°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.01° (tol 3.0°) [VIA ] step 5 Δ= -2.1 +5.5 +1.8 -1.2 -7.5 +1.2 着いた [SEND] pi05_chunk004_via08: send_angles (single) [ -10.14, +23.01, +11.47, -1.38, -24.65, -27.86] speed=20 wait=1.0s (pre max|Δ|=9.01°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.89° (tol 3.0°) [VIA ] step 8 Δ= -1.8 +5.5 +2.1 -1.2 -9.0 +1.4 着いた [SEND] pi05_chunk004_via09: send_angles (single) [ -10.48, +24.82, +12.36, -1.37, -27.17, -27.71] speed=20 wait=1.0s (pre max|Δ|=2.74°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.93° (tol 3.0°) [VIA ] step 9 Δ= -0.2 +0.9 +0.2 -0.6 -2.7 +0.6 着いた [STATE ] 目標を渡す 目標と実測の最大差 0.93°（関節 j1） [005] 推論 1269ms 10ステップ先 Δ=-11.4 +22.9 +7.1 -1.1 -32.1 +0.6 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > c [CORR ] 補正 3 に入りました (推論 5 のあと) [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉 Enter = 補正を終えてモデルの推論に戻る / b = 補正を取り消して始めの姿勢へ戻る ? = この一覧 [補正] 関節 j0 刻み 1.0° 補正目標 j0=-10.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_000: send_angles (single) [ -9.28, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.00°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-9.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_001: send_angles (single) [ -8.28, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.21°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-8.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_002: send_angles (single) [ -7.28, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.24°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-7.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_003: send_angles (single) [ -6.28, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.19°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-6.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_004: send_angles (single) [ -5.28, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.22°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-5.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_005: send_angles (single) [ -4.28, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.16°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-4.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_006: send_angles (single) [ -3.28, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.11°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-3.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_007: send_angles (single) [ -2.28, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.23°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-2.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_008: send_angles (single) [ -1.28, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.18°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-1.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [補正] 関節 j0 刻み 1.0° 補正目標 j0=-1.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [補正] 関節 j0 刻み 1.0° 補正目標 j0=-1.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_009: send_angles (single) [ -0.28, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.21°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=-0.3 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_010: send_angles (single) [ +0.72, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.15°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+0.7 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_011: send_angles (single) [ +1.72, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.11°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+1.7 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_012: send_angles (single) [ +2.72, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.23°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+2.7 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_013: send_angles (single) [ +3.72, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.18°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+3.7 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_014: send_angles (single) [ +4.72, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.12°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+4.7 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_015: send_angles (single) [ +5.72, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.24°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+5.7 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_016: send_angles (single) [ +6.72, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.01°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+6.7 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_017: send_angles (single) [ +7.72, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.13°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+7.7 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_018: send_angles (single) [ +8.72, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.08°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+8.7 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_019: send_angles (single) [ +9.72, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.11°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+9.7 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [SEND] pi05_corr03_020: send_angles (single) [ +10.72, +25.75, +13.00, -1.05, -26.98, -27.94] speed=20 wait=1.0s (pre max|Δ|=1.23°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.96° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+10.7 j1=+25.8 j2=+13.0 j3=-1.1 j4=-27.0 j5=-27.9 グリッパー 開 [CORR ] 補正 3 を終えました (21 刻み). この姿勢から推論に戻ります [STATE ] 実測を渡す (直前に送った目標がない) [006] 推論 1194ms 10ステップ先 Δ= +5.7 +18.9 +11.8 -1.2 -27.6 +0.7 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk006_via02: send_angles (single) [ +12.52, +31.46, +16.84, -1.92, -33.41, -27.31] speed=20 wait=1.0s (pre max|Δ|=6.17°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.97° (tol 3.0°) [VIA ] step 2 Δ= +2.0 +4.8 +3.3 -0.7 -6.2 +0.5 着いた [SEND] pi05_chunk006_via05: send_angles (single) [ +14.33, +37.60, +20.80, -2.11, -43.20, -27.20] speed=20 wait=1.0s (pre max|Δ|=10.07°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.89° (tol 3.0°) [VIA ] step 5 Δ= +2.2 +5.2 +3.2 -0.9 -10.1 +0.2 着いた [SEND] pi05_chunk006_via08: send_angles (single) [ +15.76, +43.63, +24.48, -2.28, -52.14, -27.12] speed=20 wait=1.0s (pre max|Δ|=9.52°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.93° (tol 3.0°) [VIA ] step 8 Δ= +1.4 +5.1 +3.3 -0.6 -9.5 +0.3 着いた [SEND] pi05_chunk006_via09: send_angles (single) [ +16.24, +45.59, +25.33, -2.39, -54.83, -27.12] speed=20 wait=1.0s (pre max|Δ|=2.98°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.90° (tol 3.0°) [VIA ] step 9 Δ= +0.8 +1.0 +0.6 -0.6 -3.0 +0.2 着いた [STATE ] 目標を渡す 目標と実測の最大差 0.90°（関節 j1） [007] 推論 1074ms 10ステップ先 Δ= +0.3 +4.5 +0.8 -0.6 -3.9 +0.2 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > c [CORR ] 補正 4 に入りました (推論 7 のあと) [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉 Enter = 補正を終えてモデルの推論に戻る / b = 補正を取り消して始めの姿勢へ戻る ? = この一覧 [補正] 関節 j0 刻み 1.0° 補正目標 j0=+16.2 j1=+46.5 j2=+25.2 j3=-2.1 j4=-55.5 j5=-27.3 グリッパー 開 [SEND] pi05_corr04_000: send_angles (single) [ +17.17, +46.49, +25.22, -2.10, -55.54, -27.33] speed=20 wait=1.0s (pre max|Δ|=1.00°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.70° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+17.2 j1=+46.5 j2=+25.2 j3=-2.1 j4=-55.5 j5=-27.3 グリッパー 開 [SEND] pi05_corr04_001: send_angles (single) [ +16.17, +46.49, +25.22, -2.10, -55.54, -27.33] speed=20 wait=1.0s (pre max|Δ|=1.05°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.70° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+16.2 j1=+46.5 j2=+25.2 j3=-2.1 j4=-55.5 j5=-27.3 グリッパー 開 [SEND] pi05_corr04_002: send_angles (single) [ +15.17, +46.49, +25.22, -2.10, -55.54, -27.33] speed=20 wait=1.0s (pre max|Δ|=0.82°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.70° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+15.2 j1=+46.5 j2=+25.2 j3=-2.1 j4=-55.5 j5=-27.3 グリッパー 開 [SEND] pi05_corr04_003: send_angles (single) [ +14.17, +46.49, +25.22, -2.10, -55.54, -27.33] speed=20 wait=1.0s (pre max|Δ|=0.85°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.79° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+14.2 j1=+46.5 j2=+25.2 j3=-2.1 j4=-55.5 j5=-27.3 グリッパー 開 [SEND] pi05_corr04_004: send_angles (single) [ +13.17, +46.49, +25.22, -2.10, -55.54, -27.33] speed=20 wait=1.0s (pre max|Δ|=1.06°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.79° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+13.2 j1=+46.5 j2=+25.2 j3=-2.1 j4=-55.5 j5=-27.3 グリッパー 開 [SEND] pi05_corr04_005: send_angles (single) [ +12.17, +46.49, +25.22, -2.10, -55.54, -27.33] speed=20 wait=1.0s (pre max|Δ|=0.83°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.79° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+12.2 j1=+46.5 j2=+25.2 j3=-2.1 j4=-55.5 j5=-27.3 グリッパー 開 [CORR ] 補正 4 を終えました (6 刻み). この姿勢から推論に戻ります [STATE ] 実測を渡す (直前に送った目標がない) [008] 推論 1244ms 10ステップ先 Δ= +0.2 +5.8 -0.6 -0.1 -3.7 +0.0 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk008_via02: send_angles (single) [ +12.12, +50.63, +26.45, -2.07, -59.11, -27.33] speed=20 wait=1.0s (pre max|Δ|=3.35°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.63° (tol 3.0°) [VIA ] step 2 Δ= -0.0 +3.3 +1.2 -0.1 -3.3 +0.0 着いた [SEND] pi05_chunk008_via05: send_angles (single) [ +12.16, +51.46, +26.59, -1.94, -59.88, -27.29] speed=20 wait=1.0s (pre max|Δ|=2.40°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.83° (tol 3.0°) [VIA ] step 5 Δ= +0.0 -0.2 -0.1 -0.1 -2.4 +0.0 着いた [SEND] pi05_chunk008_via08: send_angles (single) [ +12.29, +52.35, +25.46, -2.01, -59.71, -27.28] speed=20 wait=1.0s (pre max|Δ|=1.25°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.08° (tol 3.0°) [VIA ] step 8 Δ= +0.2 +0.1 -1.3 -0.2 -0.6 +0.0 着いた [SEND] pi05_chunk008_via09: send_angles (single) [ +12.30, +53.06, +24.65, -2.03, -59.47, -27.30] speed=20 wait=1.0s (pre max|Δ|=1.89°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.18° (tol 3.0°) [VIA ] step 9 Δ= +0.2 -0.3 -1.9 -0.2 -0.1 +0.0 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.18°（関節 j2） [009] 推論 1156ms 8ステップ先 Δ= +0.9 +5.3 -6.1 -0.2 +0.9 -0.3 グリッパー→閉 [VIA ] 中継点 3 点: step 2, 5, 7 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk009_via02: send_angles (single) [ +12.52, +56.91, +21.64, -2.02, -58.89, -27.38] speed=20 wait=1.0s (pre max|Δ|=4.19°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.66° (tol 3.0°) [VIA ] step 2 Δ= +0.4 +3.1 -4.2 -0.2 +0.4 -0.2 着いた [SEND] pi05_chunk009_via05: send_angles (single) [ +12.92, +59.10, +19.61, -2.01, -58.34, -27.46] speed=20 wait=1.0s (pre max|Δ|=2.71°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.56° (tol 3.0°) [VIA ] step 5 Δ= +0.8 +1.5 -2.7 -0.2 +2.2 -0.0 着いた [SEND] pi05_chunk009_via07: send_angles (single) [ +13.02, +59.11, +19.68, -2.00, -58.38, -27.47] speed=20 wait=1.0s (pre max|Δ|=2.52°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.17° (tol 3.0°) [VIA ] step 7 Δ= +0.4 -0.6 -0.4 -0.6 +2.5 +0.3 着いた [Pi] GRIPPER: VALUE=20 speed=100 result= -1 [STATE ] 目標を渡す 目標と実測の最大差 1.91°（関節 j4） [010] 推論 1322ms 10ステップ先 Δ= +1.1 -10.2 +8.6 -0.7 +0.6 +0.6 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk010_via02: send_angles (single) [ +13.09, +56.31, +22.31, -2.14, -59.17, -27.35] speed=20 wait=1.0s (pre max|Δ|=3.19°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.40° (tol 3.0°) [VIA ] step 2 Δ= +0.6 -3.2 +3.0 -0.6 +1.1 +0.5 着いた [SEND] pi05_chunk010_via05: send_angles (single) [ +13.12, +53.31, +25.25, -2.29, -59.85, -27.18] speed=20 wait=1.0s (pre max|Δ|=4.34°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.02° (tol 3.0°) [VIA ] step 5 Δ= +0.6 -3.6 +4.3 -0.4 -1.9 +0.6 着いた [SEND] pi05_chunk010_via08: send_angles (single) [ +13.28, +49.79, +28.12, -2.26, -60.14, -27.10] speed=20 wait=1.0s (pre max|Δ|=4.52°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.31° (tol 3.0°) [VIA ] step 8 Δ= +0.8 -4.5 +3.9 +0.4 -2.3 +0.4 着いた [SEND] pi05_chunk010_via09: send_angles (single) [ +13.60, +49.26, +27.91, -2.14, -59.73, -27.23] speed=20 wait=1.0s (pre max|Δ|=1.90°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.90° (tol 3.0°) [VIA ] step 9 Δ= +0.7 -1.9 +0.3 +0.4 -1.9 -0.8 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.90°（関節 j4） [011] 推論 1190ms 10ステップ先 Δ=+18.6 -1.6 -20.3 +4.7 +7.0 -4.8 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > c [CORR ] 補正 5 に入りました (推論 11 のあと) [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉 Enter = 補正を終えてモデルの推論に戻る / b = 補正を取り消して始めの姿勢へ戻る ? = この一覧 [補正] 関節 j0 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-2.5 j4=-58.7 j5=-26.6 グリッパー 閉 [補正] 関節 j3 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-2.5 j4=-58.7 j5=-26.6 グリッパー 閉 [SEND] pi05_corr05_000: send_angles (single) [ +13.27, +50.97, +27.86, -1.46, -58.71, -26.63] speed=20 wait=1.0s (pre max|Δ|=1.00°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.49° (tol 3.0°) [補正] 関節 j3 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-1.5 j4=-58.7 j5=-26.6 グリッパー 閉 [SEND] pi05_corr05_001: send_angles (single) [ +13.27, +50.97, +27.86, -0.46, -58.71, -26.63] speed=20 wait=1.0s (pre max|Δ|=1.38°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.23° (tol 3.0°) [補正] 関節 j3 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-0.5 j4=-58.7 j5=-26.6 グリッパー 閉 [補正] 関節 j3 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-0.5 j4=-58.7 j5=-26.6 グリッパー 閉 [補正] 関節 j4 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-0.5 j4=-58.7 j5=-26.6 グリッパー 閉 [SEND] pi05_corr05_002: send_angles (single) [ +13.27, +50.97, +27.86, -0.46, -57.71, -26.63] speed=20 wait=1.0s (pre max|Δ|=1.79°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.26° (tol 3.0°) [補正] 関節 j4 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-0.5 j4=-57.7 j5=-26.6 グリッパー 閉 [SEND] pi05_corr05_003: send_angles (single) [ +13.27, +50.97, +27.86, -0.46, -56.71, -26.63] speed=20 wait=1.0s (pre max|Δ|=2.26°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.56° (tol 3.0°) [補正] 関節 j4 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-0.5 j4=-56.7 j5=-26.6 グリッパー 閉 [SEND] pi05_corr05_004: send_angles (single) [ +13.27, +50.97, +27.86, -0.46, -55.71, -26.63] speed=20 wait=1.0s (pre max|Δ|=2.56°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.94° (tol 3.0°) [補正] 関節 j4 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-0.5 j4=-55.7 j5=-26.6 グリッパー 閉 [SEND] pi05_corr05_005: send_angles (single) [ +13.27, +50.97, +27.86, -0.46, -54.71, -26.63] speed=20 wait=1.0s (pre max|Δ|=0.92°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.92° (tol 3.0°) [補正] 関節 j4 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-0.5 j4=-54.7 j5=-26.6 グリッパー 閉 [SEND] pi05_corr05_006: send_angles (single) [ +13.27, +50.97, +27.86, -0.46, -53.71, -26.63] speed=20 wait=1.0s (pre max|Δ|=1.92°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.52° (tol 3.0°) [補正] 関節 j4 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-0.5 j4=-53.7 j5=-26.6 グリッパー 閉 [SEND] pi05_corr05_007: send_angles (single) [ +13.27, +50.97, +27.86, -0.46, -52.71, -26.63] speed=20 wait=1.0s (pre max|Δ|=1.51°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.51° (tol 3.0°) [補正] 関節 j4 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-0.5 j4=-52.7 j5=-26.6 グリッパー 閉 [SEND] pi05_corr05_008: send_angles (single) [ +13.27, +50.97, +27.86, -0.46, -51.71, -26.63] speed=20 wait=1.0s (pre max|Δ|=5.24°, post-send timeout 8.0s) [SEND] [TIMEOUT] 8.0s 内に未到達: max|Δ|=4.71° actual=[ +13.79, +51.50, +27.50, -0.17, -56.42, -26.27] [RETRY] 1回目送信が無視された（pre→last max|Δ| 0.53°）。5.0s 待って再送します [RETRY] pi05_corr05_008 (retry): send_angles (single) [ +13.27, +50.97, +27.86, -0.46, -51.71, -26.63] speed=20 wait=1.0s (pre max|Δ|=4.71°, post-send timeout 8.0s) [RETRY] 失敗。ABORT へ. 8.0s 内に未到達: max|Δ|=4.71° actual=[ +13.79, +51.50, +27.50, -0.17, -56.42, -26.27] [WARN] 未到達. j4 の発熱などを確認してください [補正] 関節 j4 刻み 1.0° 補正目標 j0=+13.3 j1=+51.0 j2=+27.9 j3=-0.5 j4=-51.7 j5=-26.6 グリッパー 閉 [CORR ] 補正 5 を終えました (9 刻み). この姿勢から推論に戻ります [STATE ] 実測を渡す (直前に送った目標がない) [012] 推論 1140ms 10ステップ先 Δ=+12.5 -1.4 -10.8 +2.5 +3.9 -2.7 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk012_via02: send_angles (single) [ +14.07, +49.42, +29.45, -0.19, -57.08, -26.12] speed=20 wait=1.0s (pre max|Δ|=2.08°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.89° (tol 3.0°) [VIA ] step 2 Δ= +0.3 -2.1 +1.9 -0.0 -0.7 +0.2 着いた [SEND] pi05_chunk012_via05: send_angles (single) [ +18.38, +49.70, +25.54, +0.51, -55.69, -27.13] speed=20 wait=1.0s (pre max|Δ|=4.59°, post-send timeout 8.0s) [SEND] [TIMEOUT] 8.0s 内に未到達: max|Δ|=3.05° actual=[ +18.19, +50.44, +25.75, +0.79, -52.64, -27.42] [SEND] リトライしません: pre→last max|Δ| 4.40° >= 1.0° (部分移動/ストール疑い) [VIA ] step 5 Δ= +4.6 -0.7 -2.8 +0.6 -0.5 -0.8 未到達 [WARN] 未到達. j4 の発熱などを確認してください [SEND] pi05_chunk012_via08: send_angles (single) [ +24.32, +50.35, +18.50, +1.94, -53.32, -28.57] speed=20 wait=1.0s (pre max|Δ|=7.25°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.88° (tol 3.0°) [VIA ] step 8 Δ= +6.1 -0.1 -7.3 +1.2 -0.7 -1.2 着いた [SEND] pi05_chunk012_via09: send_angles (single) [ +26.28, +50.12, +16.74, +2.32, -52.49, -29.00] speed=20 wait=1.0s (pre max|Δ|=2.24°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.43° (tol 3.0°) [VIA ] step 9 Δ= +2.1 -1.0 -2.2 +0.9 -2.1 -1.0 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.43°（関節 j4） [013] 推論 1074ms 10ステップ先 Δ=+16.4 +8.3 -19.4 +3.7 +2.0 -4.6 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk013_via02: send_angles (single) [ +31.77, +51.47, +11.17, +3.07, -49.77, -29.91] speed=20 wait=1.0s (pre max|Δ|=6.75°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.38° (tol 3.0°) [VIA ] step 2 Δ= +5.4 +0.2 -6.7 +1.0 +2.9 -1.3 着いた [SEND] pi05_chunk013_via05: send_angles (single) [ +37.79, +54.22, +4.32, +4.43, -47.36, -31.38] speed=20 wait=1.0s (pre max|Δ|=7.89°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.04° (tol 3.0°) [VIA ] step 5 Δ= +5.7 +1.9 -7.9 +1.2 +3.8 -1.2 着いた [SEND] pi05_chunk013_via08: send_angles (single) [ +41.92, +58.16, -0.95, +5.67, -48.05, -32.64] speed=20 wait=1.0s (pre max|Δ|=6.31°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.03° (tol 3.0°) [VIA ] step 8 Δ= +4.0 +3.0 -6.3 +1.0 +0.1 -1.5 着いた [SEND] pi05_chunk013_via09: send_angles (single) [ +42.72, +59.44, -1.51, +5.89, -49.02, -32.78] speed=20 wait=1.0s (pre max|Δ|=1.59°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.29° (tol 3.0°) [VIA ] step 9 Δ= +1.0 +0.3 -1.6 +0.2 -1.0 -0.3 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.29°（関節 j1） [014] 推論 1147ms 8ステップ先 Δ= +0.0 +6.2 -2.5 +1.0 -7.3 -0.6 グリッパー→開 [VIA ] 中継点 3 点: step 2, 5, 7 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > h [SEND] pi05_home: send_angles (single) [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=60.73°, post-send timeout 11.1s) [SEND] arrival OK: max|Δ|=1.49° (tol 2.5°) [RECORD] 記録終了 コマ 6419 (8.944 fps) 実測角 5040 回 (7.02 Hz) → /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_185133/recording 結果: y=成功 / n=失敗 / Enter=記録しない > n [DONE ] 記録: /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_185133 (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % python3 dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py python3 -c "from visual_servo_pick import get_daemon_socket, daemon_set_gripper, close_daemon_socket; get_daemon_socket(); daemon_set_gripper(0); close_daemon_socket()" python3 pi05/camera_align.py --raw-dir $RAW --episode run_0001_20261019_194818 --scale 5 --alpha 0.7 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 [INIT] daemon 接続中... [WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment [INIT] fresh_mode=1 [PRE] 現在姿勢: [-0.43, 1.49, 0.70, 0.26, -0.26, -30.23] [SEND] HOME へ移動指令 [WAIT] t=2.0s max_err=1.49° [DONE] HOME 到達 max_err=1.49° [POST] 最終姿勢: [-0.43, 1.49, 0.70, 0.26, -0.26, -30.23] [WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment [EXIT] fresh_mode=0, socket close [Pi] GRIPPER: OPEN result= -1 [REF ] 基準画像: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes/run_0001_20261019_194818/frames/000000.jpg [REF ] お手本 run_0001_20261019_194818 (つかんだ瞬間の j0 = +13.2°) [REF ] 重なった画像のブロックに実物のブロックを合わせると, お手本と同じ位置に置ける [ALIGN ] |dy| <= 20 なら OK. アームは HOME に置いた状態で合わせる. q または Esc で終了 (ファイルには何も書かない) [ALIGN ] dy= +1.8 dx= +4.5 response=0.909 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) ^C [STOP ] Ctrl-C [DONE ] 終了 (何も書いていません) [SAFE ] 安全範囲 (学習 19 本 ±10°): j0[-72,+55] j1[-9,+88] j2[-46,+41] j3[-18,+27] j4[-90,+10] j5[-51,-14] [SAFE ] 肩 j1 の上限は J1_SAFE_MAX=88° で切った (これを超える指令はアームが黙って捨てる / LOG352) 元データ: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes [MODE ] EXECUTE steps/chunk=10 max_move=20.0° log=/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_190659 [CAMERA] 写り方 dy=-4.4 dx=+10.9 response=0.557 → OK (|dy|<=20 / |dx|<=20 / response>=0.20) [CAMERA] 基準: /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes/run_0001_20261019_183454/frames/000000.jpg [POLICY] 接続 ws://localhost:8000 metadata={} [RECORD] 上カメラと実測角を 1 秒 10 コマで記録します → /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_190659/recording [GRIP ] 開始時のグリッパー開放は, 最初の目標が検査を通ってから行います (爪が閉じていたら手で開けておく) [STATE ] 実測を渡す (直前に送った目標がない) [000] 推論 1512ms 10ステップ先 Δ= +4.4 +16.6 +11.0 -1.8 -24.0 +2.5 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [GRIP ] 開始時にグリッパーを開く [Pi] GRIPPER: OPEN result= -1 [SEND] pi05_chunk000_via02: send_angles (single) [ +0.65, +4.99, +3.03, -0.05, -5.61, -29.70] speed=20 wait=1.0s (pre max|Δ|=5.35°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.04° (tol 3.0°) [VIA ] step 2 Δ= +1.1 +3.5 +2.3 -0.3 -5.3 +0.5 着いた [SEND] pi05_chunk000_via05: send_angles (single) [ +1.90, +10.71, +6.68, -0.67, -14.41, -28.80] speed=20 wait=1.0s (pre max|Δ|=9.84°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.06° (tol 3.0°) [VIA ] step 5 Δ= +1.5 +4.8 +2.7 -0.8 -9.8 +1.1 着いた [SEND] pi05_chunk000_via08: send_angles (single) [ +3.44, +16.31, +10.53, -1.41, -22.24, -27.84] speed=20 wait=1.0s (pre max|Δ|=8.89°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.91° (tol 3.0°) [VIA ] step 8 Δ= +2.0 +4.7 +2.9 -0.7 -8.9 +1.2 着いた [SEND] pi05_chunk000_via09: send_angles (single) [ +3.96, +18.08, +11.66, -1.51, -24.28, -27.68] speed=20 wait=1.0s (pre max|Δ|=2.49°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.90° (tol 3.0°) [VIA ] step 9 Δ= +1.1 +0.9 +0.2 +0.2 -2.5 +0.3 着いた [STATE ] 目標を渡す 目標と実測の最大差 0.90°（関節 j1） [001] 推論 1257ms 10ステップ先 Δ= +5.4 +18.8 +12.0 +0.1 -28.6 +1.0 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk001_via02: send_angles (single) [ +5.58, +23.70, +15.35, -2.09, -32.57, -27.18] speed=20 wait=1.0s (pre max|Δ|=7.88°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.99° (tol 3.0°) [VIA ] step 2 Δ= +1.8 +4.7 +3.0 -0.3 -7.9 +0.7 着いた [SEND] pi05_chunk001_via05: send_angles (single) [ +7.19, +29.92, +19.32, -2.12, -41.80, -27.01] speed=20 wait=1.0s (pre max|Δ|=10.08°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.92° (tol 3.0°) [VIA ] step 5 Δ= +2.0 +5.2 +3.2 -0.5 -10.1 +0.4 着いた [SEND] pi05_chunk001_via08: send_angles (single) [ +8.67, +36.03, +23.38, -2.18, -50.95, -26.92] speed=20 wait=1.0s (pre max|Δ|=9.65°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.88° (tol 3.0°) [VIA ] step 8 Δ= +2.1 +5.2 +3.5 -0.5 -9.6 +0.3 着いた [SEND] pi05_chunk001_via09: send_angles (single) [ +9.15, +37.83, +24.32, -2.15, -53.25, -26.88] speed=20 wait=1.0s (pre max|Δ|=2.81°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.01° (tol 3.0°) [VIA ] step 9 Δ= +0.6 +0.9 +0.6 -0.7 -2.8 +0.3 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.01°（関節 j1） [002] 推論 806ms 10ステップ先 Δ= +1.8 +7.9 +5.3 -0.9 -10.0 +0.3 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > c [CORR ] 補正 1 に入りました (推論 2 のあと) [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉 Enter = 補正を終えてモデルの推論に戻る / b = 補正を取り消して始めの姿勢へ戻る ? = この一覧 [補正] 関節 j0 刻み 1.0° 補正目標 j0=+8.7 j1=+38.8 j2=+24.6 j3=-1.8 j4=-54.0 j5=-27.1 グリッパー 開 [SEND] pi05_corr01_000: send_angles (single) [ +9.70, +38.84, +24.60, -1.84, -53.96, -27.07] speed=20 wait=1.0s (pre max|Δ|=1.00°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.97° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+9.7 j1=+38.8 j2=+24.6 j3=-1.8 j4=-54.0 j5=-27.1 グリッパー 開 [SEND] pi05_corr01_001: send_angles (single) [ +10.70, +38.84, +24.60, -1.84, -53.96, -27.07] speed=20 wait=1.0s (pre max|Δ|=1.12°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.97° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+10.7 j1=+38.8 j2=+24.6 j3=-1.8 j4=-54.0 j5=-27.1 グリッパー 開 [SEND] pi05_corr01_002: send_angles (single) [ +11.70, +38.84, +24.60, -1.84, -53.96, -27.07] speed=20 wait=1.0s (pre max|Δ|=1.16°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.97° (tol 3.0°) [補正] 関節 j0 刻み 1.0° 補正目標 j0=+11.7 j1=+38.8 j2=+24.6 j3=-1.8 j4=-54.0 j5=-27.1 グリッパー 開 [CORR ] 補正 1 を終えました (3 刻み). この姿勢から推論に戻ります [STATE ] 実測を渡す (直前に送った目標がない) [003] 推論 821ms 10ステップ先 Δ= +1.5 +8.2 +4.8 -0.4 -8.9 +0.1 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk003_via02: send_angles (single) [ +12.60, +45.46, +28.37, -2.32, -61.35, -27.02] speed=20 wait=1.0s (pre max|Δ|=7.57°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.03° (tol 3.0°) [VIA ] step 2 Δ= +1.0 +5.7 +3.7 -0.4 -7.6 +0.0 着いた [SEND] pi05_chunk003_via05: send_angles (single) [ +13.06, +47.72, +30.08, -2.24, -63.14, -27.01] speed=20 wait=1.0s (pre max|Δ|=2.59°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.97° (tol 3.0°) [VIA ] step 5 Δ= +0.5 +1.2 +1.7 -0.5 -2.6 +0.1 着いた [SEND] pi05_chunk003_via08: send_angles (single) [ +13.09, +47.88, +29.83, -2.25, -62.82, -27.01] speed=20 wait=1.0s (pre max|Δ|=0.81°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.89° (tol 3.0°) [VIA ] step 8 Δ= +0.5 -0.8 -0.8 -0.6 +0.1 +0.1 着いた [SEND] pi05_chunk003_via09: send_angles (single) [ +13.14, +48.02, +29.46, -2.31, -62.70, -27.01] speed=20 wait=1.0s (pre max|Δ|=1.12°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.03° (tol 3.0°) [VIA ] step 9 Δ= +0.6 -0.8 -1.1 -0.6 +0.2 +0.1 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.03°（関節 j2） [004] 推論 914ms 10ステップ先 Δ= +1.3 +9.0 -9.2 -0.4 +0.9 -0.0 グリッパー→閉 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > [SEND] pi05_chunk004_via02: send_angles (single) [ +13.28, +52.32, +26.17, -2.28, -62.83, -27.01] speed=20 wait=1.0s (pre max|Δ|=4.32°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.94° (tol 3.0°) [VIA ] step 2 Δ= +0.7 +3.5 -4.3 -0.3 +0.1 +0.1 着いた [SEND] pi05_chunk004_via05: send_angles (single) [ +13.65, +56.20, +23.04, -2.36, -62.23, -27.07] speed=20 wait=1.0s (pre max|Δ|=4.03°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.92° (tol 3.0°) [VIA ] step 5 Δ= +0.4 +2.9 -4.0 -0.3 +0.7 +0.0 着いた [SEND] pi05_chunk004_via08: send_angles (single) [ +13.90, +57.75, +21.32, -2.36, -62.04, -27.11] speed=20 wait=1.0s (pre max|Δ|=2.58°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.97° (tol 3.0°) [VIA ] step 8 Δ= +0.6 +0.6 -2.6 -0.3 +0.9 -0.0 着いた [SEND] pi05_chunk004_via09: send_angles (single) [ +13.90, +57.75, +21.32, -2.36, -62.04, -27.11] speed=20 wait=1.0s (pre max|Δ|=0.97°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.82° (tol 3.0°) [VIA ] step 9 Δ= -0.1 -0.8 -0.8 -0.3 +1.0 +0.0 着いた [Pi] GRIPPER: VALUE=20 speed=100 result= -1 [STATE ] 目標を渡す 目標と実測の最大差 1.94°（関節 j4） [005] 推論 935ms 10ステップ先 Δ= +1.2 -10.9 +6.2 -0.3 +1.1 +0.1 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > c [CORR ] 補正 2 に入りました (推論 5 のあと) [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉 Enter = 補正を終えてモデルの推論に戻る / b = 補正を取り消して始めの姿勢へ戻る ? = この一覧 [補正] 関節 j0 刻み 1.0° 補正目標 j0=+13.6 j1=+58.4 j2=+22.0 j3=-2.1 j4=-62.7 j5=-27.4 グリッパー 閉 [補正] 関節 j1 刻み 1.0° 補正目標 j0=+13.6 j1=+58.4 j2=+22.0 j3=-2.1 j4=-62.7 j5=-27.4 グリッパー 閉 [SEND] pi05_corr02_000: send_angles (single) [ +13.62, +59.44, +21.97, -2.10, -62.66, -27.42] speed=20 wait=1.0s (pre max|Δ|=1.00°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.56° (tol 3.0°) [補正] 関節 j1 刻み 1.0° 補正目標 j0=+13.6 j1=+59.4 j2=+22.0 j3=-2.1 j4=-62.7 j5=-27.4 グリッパー 閉 [CORR ] 補正 2 を終えました (1 刻み). この姿勢から推論に戻ります [STATE ] 実測を渡す (直前に送った目標がない) [006] 推論 982ms 10ステップ先 Δ= +1.1 -11.6 +7.9 +0.0 -0.8 +0.0 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > c [CORR ] 補正 3 に入りました (推論 6 のあと) [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉 Enter = 補正を終えてモデルの推論に戻る / b = 補正を取り消して始めの姿勢へ戻る ? = この一覧 [補正] 関節 j0 刻み 1.0° 補正目標 j0=+13.6 j1=+58.9 j2=+21.8 j3=-2.2 j4=-62.7 j5=-27.4 グリッパー 閉 [補正] 関節 j0 刻み 2.0° 補正目標 j0=+13.6 j1=+58.9 j2=+21.8 j3=-2.2 j4=-62.7 j5=-27.4 グリッパー 閉 [補正] 関節 j1 刻み 2.0° 補正目標 j0=+13.6 j1=+58.9 j2=+21.8 j3=-2.2 j4=-62.7 j5=-27.4 グリッパー 閉 [SEND] pi05_corr03_000: send_angles (single) [ +13.62, +60.88, +21.79, -2.19, -62.66, -27.42] speed=20 wait=1.0s (pre max|Δ|=1.50°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.50° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+13.6 j1=+60.9 j2=+21.8 j3=-2.2 j4=-62.7 j5=-27.4 グリッパー 閉 [補正] 関節 j1 刻み 2.0° 補正目標 j0=+13.6 j1=+60.9 j2=+21.8 j3=-2.2 j4=-62.7 j5=-27.4 グリッパー 閉 [SEND] pi05_corr03_001: send_angles (single) [ +13.62, +62.88, +21.79, -2.19, -62.66, -27.42] speed=20 wait=1.0s (pre max|Δ|=4.22°, post-send timeout 8.0s) [SEND] [TIMEOUT] 8.0s 内に未到達: max|Δ|=5.36° actual=[ +13.53, +63.19, +21.35, -2.37, -68.02, -27.15] [SEND] リトライしません: pre→last max|Δ| 1.85° >= 1.0° (部分移動/ストール疑い) [WARN] 未到達. j4 の発熱などを確認してください [補正] 関節 j1 刻み 2.0° 補正目標 j0=+13.6 j1=+62.9 j2=+21.8 j3=-2.2 j4=-62.7 j5=-27.4 グリッパー 閉 [補正] 関節 j2 刻み 2.0° 補正目標 j0=+13.6 j1=+62.9 j2=+21.8 j3=-2.2 j4=-62.7 j5=-27.4 グリッパー 閉 [CORR ] 補正 3 を終えました (2 刻み). この姿勢から推論に戻ります [STATE ] 実測を渡す (直前に送った目標がない) [007] 推論 2147ms 10ステップ先 Δ= +0.8 -11.2 +7.3 -0.1 +0.6 +0.1 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > c] [SEND] pi05_chunk007_via02: send_angles (single) [ +13.62, +61.66, +23.09, -2.51, -69.39, -27.10] speed=20 wait=1.0s (pre max|Δ|=1.74°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.35° (tol 3.0°) [VIA ] step 2 Δ= +0.1 -1.7 +1.7 -0.1 -0.2 -0.0 着いた [SEND] pi05_chunk007_via05: send_angles (single) [ +13.46, +57.75, +26.12, -2.70, -69.35, -26.95] speed=20 wait=1.0s (pre max|Δ|=5.26°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.66° (tol 3.0°) [VIA ] step 5 Δ= -0.1 -5.3 +4.1 -0.3 -0.2 +0.1 着いた [SEND] pi05_chunk007_via08: send_angles (single) [ +13.80, +53.24, +28.63, -2.65, -68.88, -26.93] speed=20 wait=1.0s (pre max|Δ|=6.17°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.77° (tol 3.0°) [VIA ] step 8 Δ= +0.2 -6.2 +3.0 +0.1 -0.8 +0.0 着いた [SEND] pi05_chunk007_via09: send_angles (single) [ +14.35, +52.15, +28.66, -2.48, -68.34, -27.07] speed=20 wait=1.0s (pre max|Δ|=2.86°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.99° (tol 3.0°) [VIA ] step 9 Δ= +0.7 -2.9 +0.2 +0.3 -0.3 -0.6 着いた [STATE ] 目標を渡す 目標と実測の最大差 1.99°（関節 j1） [008] 推論 872ms 10ステップ先 Δ=+16.1 -3.8 -14.7 +3.2 +6.8 -4.0 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > c [CORR ] 補正 4 に入りました (推論 8 のあと) [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉 Enter = 補正を終えてモデルの推論に戻る / b = 補正を取り消して始めの姿勢へ戻る ? = この一覧 [補正] 関節 j0 刻み 1.0° 補正目標 j0=+14.3 j1=+54.1 j2=+28.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [補正] 関節 j2 刻み 1.0° 補正目標 j0=+14.3 j1=+54.1 j2=+28.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_000: send_angles (single) [ +14.32, +54.14, +29.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=1.00°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.87° (tol 3.0°) [補正] 関節 j2 刻み 1.0° 補正目標 j0=+14.3 j1=+54.1 j2=+29.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_001: send_angles (single) [ +14.32, +54.14, +30.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=0.87°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.87° (tol 3.0°) [補正] 関節 j2 刻み 1.0° 補正目標 j0=+14.3 j1=+54.1 j2=+30.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_002: send_angles (single) [ +14.32, +54.14, +31.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=0.98°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.87° (tol 3.0°) [補正] 関節 j2 刻み 1.0° 補正目標 j0=+14.3 j1=+54.1 j2=+31.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_003: send_angles (single) [ +14.32, +54.14, +32.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=1.36°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.87° (tol 3.0°) [補正] 関節 j2 刻み 1.0° 補正目標 j0=+14.3 j1=+54.1 j2=+32.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [補正] 関節 j2 刻み 2.0° 補正目標 j0=+14.3 j1=+54.1 j2=+32.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_004: send_angles (single) [ +14.32, +54.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=2.57°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.67° (tol 3.0°) [補正] 関節 j2 刻み 2.0° 補正目標 j0=+14.3 j1=+54.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_005: send_angles (single) [ +14.32, +54.14, +36.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=2.81°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.75° (tol 3.0°) [補正] 関節 j2 刻み 2.0° 補正目標 j0=+14.3 j1=+54.1 j2=+36.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_006: send_angles (single) [ +14.32, +54.14, +38.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.41°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.84° (tol 3.0°) [補正] 関節 j2 刻み 2.0° 補正目標 j0=+14.3 j1=+54.1 j2=+38.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_007: send_angles (single) [ +14.32, +54.14, +40.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.82°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.15° (tol 3.0°) [補正] 関節 j2 刻み 2.0° 補正目標 j0=+14.3 j1=+54.1 j2=+40.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_008: send_angles (single) [ +14.32, +54.14, +38.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=1.93°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.93° (tol 3.0°) [補正] 関節 j2 刻み 2.0° 補正目標 j0=+14.3 j1=+54.1 j2=+38.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_009: send_angles (single) [ +14.32, +54.14, +36.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=1.93°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=0.53° (tol 3.0°) [補正] 関節 j2 刻み 2.0° 補正目標 j0=+14.3 j1=+54.1 j2=+36.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_010: send_angles (single) [ +14.32, +54.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=1.47°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.50° (tol 3.0°) [補正] 関節 j2 刻み 2.0° 補正目標 j0=+14.3 j1=+54.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+54.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+54.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_011: send_angles (single) [ +14.32, +52.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=2.35°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.02° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+52.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_012: send_angles (single) [ +14.32, +50.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=2.85°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=2.90° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+50.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_013: send_angles (single) [ +14.32, +48.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.27°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.69° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+48.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_014: send_angles (single) [ +14.32, +46.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.69°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.58° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+46.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_015: send_angles (single) [ +14.32, +44.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.58°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.56° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+44.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_016: send_angles (single) [ +14.32, +42.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.56°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.54° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+42.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_017: send_angles (single) [ +14.32, +40.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.54°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.60° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+40.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_018: send_angles (single) [ +14.32, +38.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.60°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.49° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+38.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_019: send_angles (single) [ +14.32, +36.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.49°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.56° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+36.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_020: send_angles (single) [ +14.32, +34.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.56°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.45° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+34.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_021: send_angles (single) [ +14.32, +32.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.45°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.43° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+32.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_022: send_angles (single) [ +14.32, +30.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.43°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.32° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+30.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [SEND] pi05_corr04_023: send_angles (single) [ +14.32, +28.14, +34.47, -2.81, -67.85, -26.45] speed=20 wait=1.0s (pre max|Δ|=3.32°, post-send timeout 8.0s) [SEND] arrival OK: max|Δ|=1.39° (tol 3.0°) [補正] 関節 j1 刻み 2.0° 補正目標 j0=+14.3 j1=+28.1 j2=+34.5 j3=-2.8 j4=-67.8 j5=-26.4 グリッパー 閉 [CORR ] 補正 4 を終えました (24 刻み). この姿勢から推論に戻ります [STATE ] 実測を渡す (直前に送った目標がない) [009] 推論 808ms 10ステップ先 Δ=+15.5 +9.2 -12.9 +3.4 +3.2 -3.0 [VIA ] 中継点 4 点: step 2, 5, 8, 9 Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / h=HOMEに戻して終了 / q=終了 > h [SEND] pi05_home: send_angles (single) [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=67.67°, post-send timeout 12.2s) [SEND] arrival OK: max|Δ|=1.58° (tol 2.5°) [RECORD] 記録終了 コマ 2839 (8.785 fps) 実測角 1922 回 (5.95 Hz) → /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_190659/recording 結果: y=成功 / n=失敗 / Enter=記録しない > n [DONE ] 記録: /Volumes/KIOXIA/r2r2r-data/runs/pi05_policy/pi05_20261030_190659 (base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-scripts % ```