--- title: "フィジカルAI研究室を作る Log#24 | π0.5 の失敗の原因を探ると、浅いお手本だった" slug: pi05-failures-traced-to-shallow-grasp-demos date: 2026-11-07 day: 460 meta_description: "mechArm 270 で Physical Intelligence の π0.5 が失敗した主な原因は、置き場所ではなく、ブロックが奥まで入る前に爪を閉じる浅いつかみ。学習データ30本中7本も同じ浅さだった。" tags: [フィジカルAI研究室, mechArm 270, π0.5, Physical Intelligence, R2R2R, Real2Render2Real, デジタルツイン, Unity, 模倣学習, 学習データ, Codex, Claude Code] series: "フィジカルAI研究室" --- mechArm 270 で Physical Intelligence の π0.5（ロボット用の AI のモデル）が失敗していた主な原因は、ブロックの置き場所の種類が少ないことではなく、ブロックが爪の奥まで入る前に閉じてしまう浅いつかみでした。**学習に使った実機のお手本30本のうち7本が同じくらい浅く、そのうち4本は π0.5 自身が浅くつかんだまま運べて成功と判定され、学習データに入っていたものでした。** 実機の記録を Unity のデジタルツイン（実機をそっくり写した3Dの模型）で再生し、上カメラの画を測って確かめています。 この回でやったことで、次のことが変わりました。 - 良くなること：次に直すものがはっきりした。置き場所を増やす前に、浅いお手本を外し、深いお手本を置き場所ごとに同じ本数そろえる。浅い7本と人の指が写った2本を外した学習データ（21本）はもうできている - もたらされること：お手本を足すたびに、爪を閉じて1秒後の画からつかみの深さを測り、浅い本を自動で外せるようになった。外した数と理由はデータの索引に残る。置き場所ごとの本数を数える表示も足した - 時間・費用：調べと道具づくりは、依頼箱の Claude Code（プログラムを書く AI）と Codex（別の会社の AI。計画の検証に使った）が、合わせて約2時間15分で行った（依頼7件。実機は動かしていない）。学習は回していないので、クラウドの GPU の費用は0円 きっかけは、このサイトのシリーズ「[デジタルツイン学習 R2R2R](https://pixelstomotors.com/series2)」で、Unity の中で試した動きを実機に移すやり方を続けてきたことでした。そのやり方を、いまの π0.5 の学習に活かせないかと考えたのが始まりです。名前のもとにした研究 [Real2Render2Real](https://arxiv.org/abs/2505.09601)（UC バークレーとトヨタ・リサーチ・インスティテュート、2025）は、スマホでスキャンした物と人の動画1本から、置き場所を変えたお手本の映像を何千本も描いて学習に使う手法です。効いていたのは映像のきれいさではなく、置き場所が散らばったお手本を作れることでした。 そこで最初は、置き場所を散らしたお手本を実機で集める計画を立てました。ところが、その計画を Codex に「間違っている前提で証拠を探して」と頼んで調べさせると、失敗の原因が置き場所だという証拠はない、という答えが返ってきました。調べ直すと、π0.5 を試した31回は、どれもほぼ同じ1か所にブロックを置いていました。失敗は、学習したことのある位置で起きていたのです。 やったことの全体は次のとおりです。 | 段階 | やったこと | 分かったこと | |---|---|---| | 計画（第1版） | R2R2R の論文から、置き場所を散らすお手本を実機で集める計画を立てた | 論文では、元の動きの再生だけだと π0.5 の仲間（π0-FAST）の成功率が80%から0%に落ちた | | Codex の敵対的検証（依頼106） | 計画を、間違っている前提で調べさせた | 失敗の原因が置き場所だという証拠はない。最新モデルはまだ実機で試していない | | 計画（第2版） | 置き場所を増やす前に、失敗がどこで起きているかを分ける順番に変えた | series2 のページにもこの流れを載せた | | Unity で失敗を再生（依頼107） | 実機の関節角を Unity のアームで再生し、横から見た。置き場所の候補と、Unity と実機の画の違いも確かめた | 31回とも同じ1か所。つかむ前に外れた失敗は0。Unity の画は本物とまったく違う | | 記録を細かく調べる（依頼110） | 爪を閉じたあとの持ち上げの指令と、閉じた瞬間の上カメラの画を測った | 12件中9件は、ブロックが爪の間に十分入る前に閉じていた（浅い7・空振り2）。持ち上げの指令は12件とも出ていた | | 学習データを調べる（依頼113） | お手本30本で、閉じる時刻とつかみの深さを同じ物差しで測った | 閉じる時刻のずれはない。浅いお手本が7本。うち4本は π0.5 自身が浅くつかんだ補正入りのお手本 | | 学習データを直す（依頼117） | 浅い7本と指の写った2本を外した版を作り、深さで採否を決める仕組みを足した | 21本・3,018コマ。置き場所ごとに数えると、位置1が0本、位置5が11本と偏っていた | | 本数をそろえる（依頼118） | 置き場所ごとに10本ずつそろえる手順と、本数を数える表示を足した | 実機で約30本を撮り足す手順ができた | 【図解】LOG460_grasp_depth.html 次は、実機で5か所に10本ずつの深いお手本をそろえ、学習し直して、補正なしで比べます。 GPU を買うか借りるかの判断は、Runpod で実際に借りて学習させた費用をもとに、別のページにまとめています。 【バナー】gpu-buy-or-rent この回の作業は、Claude（調べものと分析と依頼文を一緒に組み立てた AI）と進めました。方針の判断はこちらで行い、Unity とコードの作業は [Claude Code](https://claude.com/claude-code) に、計画の検証は [Codex](https://openai.com/codex/) に、依頼箱を通して頼んでいます。ここから下は、計画・検証・依頼文・報告・測った結果・コードの全量です。 ## 作業ログ ― R2R2R の考え方を π0.5 に活かす計画から、浅いお手本を外すまで この節より下で「見込める効果」と書いたものは、まだ実機で確かめていない見込みです。測った値は、測った値と分かるように書いています。 ### この回で使う言葉 | 言葉 | 意味 | |---|---| | R2R2R（Real2Render2Real） | 現実の物をスキャンし（Real）、画面の中で描き（Render）、現実のロボットに戻す（Real）流れ。UC バークレーとトヨタ・リサーチ・インスティテュートの論文の名前で、このサイトのシリーズ名のもとにもなった | | デジタルツイン | 実機と同じ形・同じ関節の動きをする、画面の中の模型。この回では Unity の中の mechArm 270 | | π0.5 | Physical Intelligence が公開している、ロボット用の AI のモデル。カメラの画と関節の角度を見て、次の動きを出す | | お手本 | π0.5 に学ばせる、実機で撮った1回分の動き（上カメラの画と関節の角度の並び）。このシリーズでは、手で教えた要所を再生したもの（教えた動き）と、π0.5 の動きに人がゲームパッドなどで手を加えたもの（補正入り）の2種類がある | | FK テーブル | 関節の角度と爪先の位置の対応表。mechArm 270 のものは Unity で作った150万行あまりの表 | | 敵対的検証 | 書いた本人とは別の AI に「この計画は間違っている」という前提で証拠を探させる確かめ方。同じ書き手が確かめると同じ見落としをするので、外の目を入れる | | 依頼箱 | Claude が書いた依頼を、Mac の見張り係が自動で Claude Code に渡し、報告を書き戻す仕組み（LOG390） | | はみ出し（px） | 爪を閉じて1秒後の上カメラの画で、ブロックが爪先の青い印の線からどれだけ先へ出ているか。大きいほど浅いつかみ。上カメラ 1920×1080 の画素で数え、24px はおよそ 9mm | ## R2R2R の論文で効いていたのは、置き場所が散らばったお手本だった 論文（[arXiv:2505.09601](https://arxiv.org/abs/2505.09601)）の要点を、このプロジェクトの調査メモ（research/paper_r2r2r_20261009.md）と原文で確かめた数字で並べます。 | 項目 | 内容 | |---|---| | 入力 | スマホで物をぐるっと撮った動画と、人が物を動かす動画1本 | | 作るもの | 物の置き場所を変えて動きを計算し直し、ロボットが作業する映像を描いたお手本。物理の計算はしない | | 速さ | RTX 4090 1枚で毎分約51本（人の遠隔操作は毎分約1.7本） | | π0-FAST の成功率（マグをコーヒーメーカーに置く） | 実機のお手本150本で73.3%、描いたお手本150本で33.3%、1,000本で80.0% | | 動きを作り直さず再生だけにすると | π0-FAST 80.0% → 0.0% | | 背景や机の模様を変えすぎると | π0-FAST 73.3% → 35.3% | | 実機150本と描いた1,000本を混ぜると | π0-FAST は 73.3%（描いた1,000本だけの80.0%より上がらない） | 1本あたりで比べれば実機のお手本のほうがずっと重く、描いたお手本は数で追いつく、という関係です。そして数を積んで効いたのは、置き場所が散らばった動きを作れたからでした。 ### Unity で描いたお手本は、いまの mechArm 270 では使いにくい 最初にこちらから聞いたのは、今のお手本をもとに Unity で大量にお手本を複製したら効果があるか、でした。答えは、同じ動きを Unity で何千回描いても、π0.5 が見る場面は5通りのままなので効かない、というものです。置き場所を変えて動きを作り直せば効く見込みはありますが、壁が3つありました。 1. π0.5 は上カメラの画を見て動く。Unity の画は、方眼のない暗いマット・濃い影・机も配線もない場面で、本物と違う（あとの依頼107の D で、並べて確かめた） 2. Unity の関節角を実機に送ると、爪先がずれる（昔の記録で40〜55mm前後。ただし肩が88°を超えて命令が捨てられていた影響が混ざっていて、純粋なずれは分からない） 3. 論文でも、実機と描いたお手本を混ぜて良くなるとは限らなかった 続けて、実機のお手本のほうが重要で、合成データは参考になりにくいのか、と聞きました。1本の重みでは実機が上で、合成の強みは数と広さにある。画と関節角の差が小さければ数の強みが効き、差が大きければ違う世界の練習を積むことになる、というのが答えです。いまの mechArm 270 では差が大きいので、Unity は画を描く工場としてではなく、どこで集めるべきか・どこで失敗しているかを先に探す道具として使うことにしました。 ## 計画の第1版：置き場所を散らしたお手本を実機で集める 論文から借りるのは、置き場所を変えて動きを作り直す部分だけ。映像は Unity で描かず、実機で撮る、という計画です。研究フォルダに置いた第1版の全文です。 ### research_r2r2r_to_pi05_plan_20261107_v1.md ``````markdown # R2R2R の考え方を π0.5 の学習に活かす計画（案） 作成: Claude（2026-11-07）　状態: 案。Codex の敵対的検証の前 元にした論文メモ: [paper_r2r2r_20261009.md](paper_r2r2r_20261009.md) ## 1. 何を解きたいか mechArm 270 Pi で動かしている π0.5（Physical Intelligence のロボット用 AI）は、お手本で見たブロックの位置の近くではつかめるが、位置が変わるとアームがブロックのところへ行かない。お手本の位置のばらつきが足りないことが原因だと見ている（2026-10-17 の見立て）。 R2R2R の論文（UC バークレー・トヨタ研究所、2025）で効いていたのは、きれいな映像を大量に描くことではなく、物の置き場所がばらついたお手本を作ることだった。元の動きをそのまま再生しただけのデータでは、π0-FAST の成功率は 80% から 0% に落ちている。逆に、背景や机の模様を変えすぎると 73% から 35% に落ちている。 そこでこの計画では、論文のうち「置き場所を変えて動きを作り直す」部分だけを借りる。映像は Unity で描かず、実機で撮る。 ## 2. 今の手元（2026-11-07 に Claude が確かめた値） | 項目 | 値 | 確かめた場所 | |---|---|---| | 最新の学習データ | 30本・4,301コマ（教えた動きの再生 19本＋ゲームパッドの補正つき 11本、うち補正 430コマ） | KIOXIA `datasets/mecharm_redblock_box_teach_v1_trim2_corr_v5/README.md` | | 教えた位置 | 5か所（位置ごとに 1〜5本） | `runs/experiments/record_teach_demo/20261019_LOG352/README.md` | | カメラ・画像 | 上のカメラ1台、384×216、10コマ/秒 | 各エピソードの `meta.json` | | 状態と行動 | 関節6つ＋爪の7つ。行動は 0.1 秒後の状態（絶対値） | 同上 | | 指示の言葉 | put the red block in the white box の1種類 | 同上 | | 最新のモデル | `checkpoints/pi05_mecharm_trim_corr_lora_v3`（2026-11-06、A40 で 1,500 ステップ・3.13 時間・約 292 円） | `lab/Log22/train/lab_train_20261106_195143.txt` | | 前に series2 の仕組みで作ったお手本 | scripted_v1：35本のうち使えたのは 20本。爪の開くラベルが約 2 秒早いずれあり | `datasets/mecharm_redblock_box_scripted_v1/` | | FK テーブル | `fk_tables/fk_scan_motion_based_v2.csv`（爪先の位置 xyz と関節6つ）。**Unity で作った表で、実機とはずれる**（LOG177）。実機では上カメラの視覚サーボと補正記憶でずれを詰めている | | ### 前の回答の訂正 チャットで「実測の FK テーブル」と書いたが、正しくは Unity で作った表。実機との差は、カメラで見て寄せる仕組み（視覚サーボ）と、位置ごとのずれを覚える補正記憶（correction_memory.json）で埋めている。 ### 実機で分かっている落とし穴（LOG352） - 肩（j1）が約 88° を超える命令は、アームが丸ごと捨てる - アームが動いている途中に爪の命令を出すと、その後の送信が無視されて固まる（daemon の再起動でしか戻らない） - ブロックを教えた位置から約 46mm ずらして置いた試行があった。置き場所の印がないと、人が置く位置がぶれる ## 3. やること ### 段階0　基準を測る（学習なし） - マットに置き場所の印を付ける。アームが届く範囲の中に 3×3 の9か所。教えた5か所とは別に、その間と外側を含める - 最新モデル（trim_corr_lora_v3）で、9か所×3回＝27回、補正なしで動かす - つかめたか・箱に入ったかを人が判定し、位置ごとの成功数を残す - 失敗がつかむ前か後か（ブロックへ行かない／落とす／箱にぶつかる）も分けて残す ### 段階1　置き場所を散らしたお手本を集める 3つの案を比べ、段階0の結果を見て決める。 | 案 | やり方 | 良い点 | 心配な点 | |---|---|---|---| | A　series2 の仕組みで自動 | `run_pick_place.py --record-demo`（カメラで見つけ、FK テーブルで姿勢を引き、視覚サーボで寄せ、補正記憶で直す）を9か所で回す | 新しいコードがほぼ要らない。人はブロックを置き直すだけ | 前回は 35本中 20本しか使えなかった。動きの速さ・止まり方が教えた動きと違い、混ぜると π0.5 が迷うかもしれない。爪のラベルのずれを直す必要がある | | B　教えた動きを置き場所に合わせて作り直す（論文に一番近い） | 教えた5本の要所（ブロック真上・つかむ位置）を、新しい置き場所の分だけ平行にずらす。ずらした爪先位置から FK テーブルで関節角を引き、要所の再生（`record_teach_demo.py`）で撮る | 動きの癖が今の学習データとそろう | Unity の表なので、ずらした先で実機の爪がブロックに合う保証がない。補正記憶をどう挟むかを決める必要がある。新しいコードが要る | | C　ゲームパッドで人が運ぶ | 9か所それぞれで、HOME からブロックの上まで人がゲームパッドで動かして撮る | 実機で確実にブロックに合う | 1本ごとに人の手間がかかる。動きの滑らかさは今の版が限界（LOG446） | まずは各案 9か所×3本＝27本を目安にする。今の 30本と合わせて 57本。 ### 段階2　学習する - 最新モデルから続けて学習する（corr_v5 のときと同じ形。Runpod の A40、`lab_train.sh` で寝ている間に回す） - 置き場所が広がると関節角の範囲も広がる。正規化の値（学習データの平均とばらつき）を今のまま使うか、作り直すかを決める ### 段階3　同じ条件で測り直す - 段階0と同じ 9か所×3回を、補正なしで動かす - さらに、格子の点と点の間（お手本にない位置）に 3か所足す - 合格の目安：お手本にない位置での成功数が、段階0より明らかに増えること（具体の数は段階0の結果を見て決める） ### 段階4（並べて進めてよい）　動く前に FK で見る - π0.5 が出した次の数十コマ分の関節角を FK に通し、爪先が白箱の縁より低いまま箱に近づく動きを見つける - 止めるためではなく、巻き戻しとゲームパッドの補正をどこで入れるかを示すために使う。最初は記録だけにする - 描画がいらないので軽い。ただし FK は Unity の表なので、実機との差の分だけ外れることがある ### やらないこと - Unity で映像を描いて学習に入れること（論文そのままのやり方）。実機のお手本が少ないうちは実機の映像のほうが有利で、カメラ1台のまま今よりよく動くことが条件だから。実機のお手本で頭打ちになったら考え直す ## 4. 置き場所 | もの | 場所 | |---|---| | 新しいお手本 | KIOXIA `datasets/mecharm_redblock_box_grid_v1/`（案ごとに分けるなら `_a` `_b` `_c` を付ける） | | 実機の推論・評価の記録 | KIOXIA `runs/pi05_policy/`、まとめは `lab/Log{NN}/`（番号はエイイチに確認） | | 学習済みモデル | KIOXIA `checkpoints/` と Hugging Face の非公開リポジトリ | ## 5. まだ決めていないこと - 段階1でどの案から始めるか（段階0の結果を見て決める） - scripted_v1 から教えた動きに切り替えた理由。案A がその理由で同じ失敗をしないか - 段階3の合格の数 `````` ## Codex の敵対的検証：失敗の原因が置き場所だという証拠はなかった（依頼106） 第1版を、Codex に「この計画は前提が間違っているか、やっても成功率が上がらないか、実機やデータを壊す危険がある、という前提で証拠を探して」と頼みました。依頼箱の Claude Code が Codex を起動し、Codex は記録を読むだけで調べています（11:46〜11:57）。 判定は、このままの計画で段階0（9か所×3回）に進むのは勧めない、でした。とくに大きかったのは次の4つです。 - 推論の記録に、ブロックを置いた位置が残っていない。人の判定がそろった31回のうち成功は19回で、そのうち補正なしで成功したのは2回だけ。最新の v3 はまだ実機で一度も試していない - 9か所×3回では差が言えない。1か所3回で0回成功でも、本当の成功率は0〜56%のどこかとしか言えない。集める位置と測る位置を分けないと、お手本にない位置での伸びは分からない - 推論のときは、学習データの関節の範囲に余裕を足した範囲で指令を止めている。古い範囲のまま新しい位置に置くと、アームが届かない - 第1版の書き間違い。補正入りの11本は「全部ゲームパッド」ではなく、ゲームパッド4本・前のやり方の補正7本。論文の「混ぜると良くなる」も π0-FAST では成り立っていない ### 依頼106：Codex に計画の敵対的検証を頼む ``````markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 150 --- # Codex に、R2R2R の考え方を π0.5 の学習に活かす計画の敵対的検証を頼む あなた（Claude Code）の仕事は **Codex を起動して検証を任せることだけ**。検証そのものはしない。どのファイルも変更しない（r2r2r-scripts も含む）。実機には接続しない。 ## 手順 1. Codex CLI があるか確かめる：`command -v codex || ls ~/.npm-global/bin/codex`。見つからない・ログインが切れている場合は、何もせずに止めて、報告の「決めてほしいこと」にその旨を書く。 2. 次のコマンドで Codex に渡す（そのまま実行。検証依頼書の中身はあなたが要約・書き換えしない）： ```bash REVIEW_DIR="$HOME/Library/Mobile Documents/com~apple~CloudDocs/デスクトップ/ADAPT DESIGN/じぶんの/PixelstoMotors/research" brctl download "$REVIEW_DIR/codex_request_r2r2r_to_pi05_20261107.md" 2>/dev/null brctl download "$REVIEW_DIR/research_r2r2r_to_pi05_plan_20261107.md" 2>/dev/null brctl download "$REVIEW_DIR/paper_r2r2r_20261009.md" 2>/dev/null brctl download "$REVIEW_DIR/pdf/paper_r2r2r_20261009.pdf" 2>/dev/null codex exec --skip-git-repo-check -C "$REVIEW_DIR" -s workspace-write \ "codex_request_r2r2r_to_pi05_20261107.md を読んで、その指示どおりに検証し、報告を codex_report_r2r2r_to_pi05_20261107.md としてこのフォルダに書いてください。対象フォルダはすべて読むだけで、書いてよいのはその報告と _codex_tmp_20261107/ の中だけです。" \ > "$REVIEW_DIR/_codex_tmp_20261107/codex_run.log" 2>&1 ``` 3. 終わったら `research/codex_report_r2r2r_to_pi05_20261107.md` ができたか確かめる。 - できていれば、報告には「できた」とそのパスと、最後のまとめの節（段階0に進んでよいか・直すべき項目・対案）だけを写す。 - できていなければ、`_codex_tmp_20261107/codex_run.log` の末尾 40 行を報告に写す。 ## 守ること - Codex の報告の中身をあなたが評価・手直ししない。 - `-s workspace-write` を外さない・`--dangerously-bypass-approvals-and-sandbox` などの制限を外す指定を足さない。 - 失敗しても自分で検証を代わりにやらない。 報告の最後の1行に「判定：合格／止めた（決めてほしいことあり）／失敗」を書く。 `````` ### 依頼106の報告 ``````markdown # 結果：20261107_106_Codexに R2R2R から π0.5 への計画の敵対的検証を頼む - 状態：完了 - 始めた時刻：2026-11-07 11:46 - 終わった時刻：2026-11-07 11:57（11分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 978e11a5-d4f9-483e-b839-dbeb0afd3ae3` ## Claude Code の報告 **やったこと** - Codex CLI があることと、ログインが有効なこと（ChatGPT でログイン済み）を確かめました。 - 依頼文のコマンドを書き換えずに実行しました。最初の実行は10分で止まる設定にしてしまったため、始まってすぐに止め、時間の上限を長くして同じコマンドで実行し直しました。終了コードは 0 です。 - 報告ファイルができたことを確かめ、最後のまとめの節だけをそのまま写しました。中身の評価や手直しはしていません。 - 依頼の「どのファイルも変更しない」を守るため、普段作っている作業記録（logs/ 内）は今回作っていません。 **結果** 「できた」：`research/codex_report_r2r2r_to_pi05_20261107.md`（35,088 バイト、11月7日 11:56 作成） Codex 報告の「最終回答」節を写します。 1. このまま段階0に進んでよいか ❌ このままの27回の実機実行は勧めない。9位置の座標・実機到達・視認性と、比較する設定が未定。既知位置で最新v3の無補正試行を少数確認する、という修正版の段階0にする。今回の読み取り検証では実機を動かしていない。 2. 進む前に直すべき項目（優先順） 1. 実行の安全条件と記録：肩88°、静止未確認時の爪命令、未到達継続、物体が見えない位置を扱う。モデルv3・実測／目標入力・指令制限・補正なしの定義を固定する。 2. 原因を断定しない：位置不足は仮説へ戻し、既知位置でも失敗するかと、保持・配置段階を分けて測る。31件の判定と19件の成功を無補正成功率に流用しない。 3. 評価設計：学習用と未学習評価用の位置を先に分け、追加3位置も学習前から測る。効果の大きさ・必要数・合格条件を事前に決める。 4. データの説明と同期：補正入り11本のうちゲームパッドは4本へ訂正。scriptedの爪ラベルを直す前に混ぜない。停止除去と時間軸を統一する。 5. 正規化と安全範囲：分位点・差分行動の統計、固定SHA、推論assets、`--raw-dir`による指令制限を一組で管理する。 6. 案Bと段階4の過大評価を直す：FK候補があることを到達・把持・衝突回避と同一視しない。連続姿勢、箱の形状、実機校正を追加する。 3. 対案の要点 まずv3の既知位置で失敗を分解し、見える・届く少数の新位置で、品質を揃えた教示と人の調整を足して比較する。効果が見えたところで自動収集を広げる。ブロックだけの画像編集で行動を据え置く方法は採らない。描画環境を作るより先に、同期・視認性・実行条件・評価の穴を塞ぐ方が今回の記録には合っている。 **変えたファイル** 私が手で変えたファイルはありません。Codex が書いたのは、報告 `codex_report_r2r2r_to_pi05_20261107.md` と実行ログ `_codex_tmp_20261107/codex_run.log` の2つです（ログは最初に止めた実行の分を上書きしています）。 **決めてほしいこと** なし。ただし、Codex の判定は「このままの段階0（27回の実機実行）は勧めない」です。段階0をどう直すか、いつ実機を動かすかは、エイイチが報告を読んで決めてください。 判定：合格 `````` ### Codex への検証依頼書（codex_request_r2r2r_to_pi05_20261107.md） ``````markdown # 検証依頼（敵対的検証）：R2R2R の考え方を π0.5 の学習に活かす計画 作成：Claude（2026-11-07）　依頼者：エイイチ 対象：`research_r2r2r_to_pi05_plan_20261107.md`（同じフォルダ）。まだ何も作っていない計画の段階 --- ## あなたの役割 この計画は Claude が書いた。同じ書き手の確認は同じ盲点を持つ。 あなたは **「この計画は前提が間違っているか、やっても成功率が上がらないか、実機やデータを壊す危険がある」という前提で、その証拠を探す** 役割を担う。 - 「たぶん大丈夫」は書かない。OK と判定する項目には、**実行したコマンド・数えた件数・読んだファイルと行番号** を証拠として添える - 計画の「2. 今の手元」の値を **信用しない**。自分で開いて数える - もっと良い方法があれば対案を出す。計画を守る必要はない - 報告は日本語で書く。英語の原文の引用は入れない（読み手は英語が読めない） --- ## 守ること - 下の対象フォルダは **すべて読むだけ**。変更・移動・削除・リネームをしない - 書いてよいのはこのフォルダ（`PixelstoMotors/research/`）の中の報告 `codex_report_r2r2r_to_pi05_20261107.md` と、`_codex_tmp_20261107/` の中の一時ファイルだけ - git 操作（commit・push・clean・checkout など）をしない - 実機（mechArm・Raspberry Pi）に接続しない。daemon・SSH・シリアルを使わない - `/Volumes/KIOXIA` の `keys/` と `*_key.txt`・`*_token.txt` は開かない - KIOXIA は外付け SSD で遅い。全体を何度も走査しない --- ## 対象フォルダ（Mac 上のパス） | 呼び名 | パス | |---|---| | iCloud | `~/Library/Mobile Documents/com~apple~CloudDocs/デスクトップ/ADAPT DESIGN/じぶんの/PixelstoMotors/` | | KIOXIA | `/Volumes/KIOXIA/r2r2r-data/`（datasets・runs・lab・checkpoints・fk_tables） | | コード | `~/r2r2r-scripts/`（学習は `pi05/`、教えた動きの再生は `dynamic_tracking_limits/drag_teaching/record_teach_demo.py`、series2 の本線は `run_pick_place.py`） | | 論文メモ | iCloud `research/paper_r2r2r_20261009.md`（原文 PDF は `research/pdf/paper_r2r2r_20261009.pdf`） | --- ## 確かめてほしいこと ### A. 前提は正しいか A1. 計画の「2. 今の手元」の表の値（本数・コマ数・位置の数・画像の大きさ・行動の定義・最新モデル・scripted_v1 の使えた数）を、実ファイルで数えて照合する A2. **一番大事な前提：失敗の主な原因は、お手本の置き場所のばらつきが足りないことか。** `runs/pi05_policy/` の各試行の JSON・log.jsonl・録画の記録から、失敗がつかむ前か後か、ブロックの位置と関係があるかを、記録から言える範囲で調べる。ブロックの位置が記録に残っていないなら、そう書く。前提が言えないなら、計画全体の順番がどう変わるかを書く A3. FK テーブル `fk_scan_motion_based_v2.csv` は Unity で作った表で実機とずれる、という計画の書き方は正しいか。どれくらいずれるかの記録（LOG177 前後、補正記憶 correction_memory.json）があれば、その大きさを示す A4. scripted_v1 から教えた動き（teach_v1）に切り替えた理由を、記録（datasets の README・runs/experiments・lab の記録・r2r2r-scripts の git log）から探す。案A がその理由で同じ失敗をしないか A5. 論文メモの数字（再生だけだと 80%→0%、背景を変えすぎると 73%→35%、実データ 150本と生成 1,000本を混ぜたときの結果）が原文 PDF と合っているか ### B. 計画の各段階に穴はないか B1. 段階0：3×3 の9か所はアームが届く範囲に置けるか。FK テーブルと「肩 j1 は約 88° まで」の制約で、届く範囲を実際に計算して示す。上カメラ1台でブロックと白箱が全部の位置で見えるか（記録の画像で確かめられる範囲で） B2. 段階1の案A・B・C それぞれについて、既存のコードで何がそのまま使えて、何を新しく書く必要があるか。とくに案B で、ずらした要所の関節角を FK テーブルで引いたとき、肩 88° 超え・動作中の爪命令で固まる件・爪先がブロックに合わない件にどう当たるか B3. 動きの癖が違うお手本（スクリプト・教えた動き・ゲームパッド）を混ぜたとき、π0.5 の学習で問題になりうる点。止まりの長さ・速さ・爪の開け閉めのタイミングの違いを、既存データで比べる B4. 段階2：置き場所を広げたとき、正規化の値（norm stats）を今のまま使うと何が起きるか。`pi05/` の学習・変換のコードで、どこで計算・保存・読み込みしているかを示す B5. 段階3：9か所×3回という回数で、成功率の差を言えるか。言えないなら、何回あれば言えるかを示す B6. 段階4：action chunk を FK に通して箱の縁との高さを見る、は今のコードのどこに入るか。白箱の位置と高さは何から取るか ### C. もっと良い方法はないか C1. 計画で見送った「Unity で映像を描いて学習に入れる」より、安く効きそうな方法（例：実機の画像の中でブロックを切り貼りして位置を増やす、など）があるか。あれば、行動（関節角）との食い違いをどう扱うかまで書く C2. 段階の順番を変えたほうがよいところ --- ## 報告の形 `codex_report_r2r2r_to_pi05_20261107.md` に書く。各項目に次のどれかを付ける。 - ✅ OK（証拠つき） - ⚠️ 要注意（何が心配か、具体的に） - ❌ NG（前提の誤り・明確な穴。証拠と直し方の案） 最後にまとめとして： 1. この計画のまま段階0に進んでよいか 2. 進む前に直すべき項目（優先順） 3. 対案があれば、その要点 `````` ### Codex の検証報告（codex_report_r2r2r_to_pi05_20261107.md） ``````markdown # R2R2R の考え方を π0.5 に活かす計画：敵対的検証報告 検証日：2026-11-07／検証者：Codex **総合判定：❌ 計画のまま実機で段階0を始めることは勧めない。位置の違うお手本を増やす仮説は残るが、それが現在の主原因だという証拠は不足している。評価位置・無補正の定義・既存コードの危険箇所・評価用の未学習位置を先に確定する。** 学習をせず現状を測る方針自体は妥当。ただし、まず記録と安全条件を整えた小規模な基準測定にする。 ## 検証範囲と読み方 以下で `D` は `/Volumes/KIOXIA/r2r2r-data/`、`S` は `/Users/shimazueiichi/r2r2r-scripts/`、`P` はこのフォルダの親 `PixelstoMotors/` を表す。行番号は今回読んだ現行ファイルの1始まり。JSON・NPZの全件集計は、ファイル名とキー・配列を証拠とし、再現用スクリプトと結果を `_codex_tmp_20261107/` に残した。 書き込みは本報告と指定一時フォルダだけ。対象コードの実行・インポート、学習、実機接続、daemon、SSH、シリアル、gitコマンドは実行していない。禁止された鍵・トークンファイルは開いていない。SSD全体の内容検索は行わず、必要なディレクトリに限定した。履歴は `.git/logs/HEAD` をテキストとして読んだもので、完全な `git log` の代わりとは扱っていない。 主な実行コマンド： ```sh python3 _codex_tmp_20261107/audit.py python3 _codex_tmp_20261107/further.py > _codex_tmp_20261107/further.txt python3 _codex_tmp_20261107/metrics.py > _codex_tmp_20261107/metrics.txt python3 _codex_tmp_20261107/statistics.py > _codex_tmp_20261107/statistics.txt ``` これらは対象のJSON・NPZ・CSV・画像を読み、一時フォルダにだけ集計・画像を書き出す。集計結果は `summary.json`、`episodes.json`、`runs.json`、`teach.json`、`grid.json`、`recording_metadata.json`。PDFはmacOSのPDFKitで全文抽出し、12・13ページを画像化して目視照合した（`pdftext.swift`、`render.swift`、`paper.txt`、`paper_page_12.png`、`paper_page_13.png`）。画像の一覧は `contact.jpg`、失敗動画の抽出一覧は `video_checks.json` と `failures_0.jpg`・`failures_6.jpg`。 本報告は「手元の記録から何が言えるか」の検証であり、実機の到達性・衝突回避を保証する試験ではない。記録にない原因を推定だけで確定しない。 ## A1. 今の手元の値 **⚠️ 要注意：本数などは合うが、「補正11本がすべてゲームパッド」は誤り。記録10Hzと実測10Hzも区別が必要。** | 項目 | 独立確認の結果 | 判定と証拠 | |---|---|---| | 最新データの本数・コマ | 30本、状態4,301行・行動4,301行・JPEG4,301枚。教示19本2,717行、補正入り11本1,584行。補正430行 | ✅ OK。`audit.py` で各 `raw_episodes/*/episode.npz` を開いて配列長を集計。`further.py` で全4,301枚を復号し、全枚216×384×3。README 8–13行とも一致 | | 補正11本の由来 | 7本は従来の補正、ゲームパッドを記録したものは4本。11本はモデルの動きも含む | ❌ NG。`runs.json` と各試行 `log.jsonl` の補正受理イベントを照合。2026-10-31の4本と11-01の3本には `pad` 行がなく、11-06の4本には順に26・26・19・16行ある。「教示19本＋人の補正入り11本（うちゲームパッド4本）」へ修正 | | 教えた位置 | 異なる教示ファイル5種類、採用本数1・5・4・5・4 | ✅ OK。ただし正確な物理位置5点の検証とは別。19試行の試行JSON `teach.file`・`teach.waypoints` を独立集計（`teach.json`）。`D/runs/experiments/record_teach_demo/20261019_LOG352/README.md` 10–16行と一致 | | 画像・カメラ | 上カメラ1系統。学習JPEGは384×216。全30本のメタデータは10Hz | ✅ OK（保存形式として）。全件集計と `S/pi05/openpi_addon/mecharm_policy.py` 4–6、42–55行で有効な画像入力を確認 | | 実際の観測頻度 | 10Hzの独立した実測画像・角度が毎回あるわけではない | ⚠️ 要注意。LOG352 README 36行は角度実測約1.6〜2Hz。例えば11-06 18:55:54の `recording/top_meta.json` は目標10Hzに対し有効8.249fps、角度8.51Hz。補間・同一画像の再使用を含む学習上の10Hzと区別する | | 状態・行動 | 7次元、関節6軸＋爪。最新30本では末尾を除いて行動が次の状態と一致 | ✅ OK（生データの定義）。全30本のNPZで数値比較、全件一致。`S/pi05/build_demo_episodes.py` 324–325行。ただし学習時は関節6軸をチャンク先頭状態との差分に変える（後述B4） | | 指示 | 全30本同じ、赤ブロックを白箱に入れる指示 | ✅ OK。全30個の `meta.json` の `task` を集合化し1種類 | | 最新モデル | `pi05_mecharm_trim_corr_lora_v3/1499` が存在。学習1,500ステップ。3.13時間はGPUを借りた時間 | ✅ OK（保存と実行記録の範囲）。現存ファイルは由来JSONを含め36個・6,335,852,509バイト。由来JSON3,883バイトを除くと35個・6,335,848,626バイトでログと一致。`D/lab/Log22/train/lab_train_20261106_195143.txt` 606、613–615、630、637–644行。約292円はログ記載の換算見積りで請求明細の検証ではない | | scripted_v1 | 索引35件中採用20件、実在エピソード20本・6,298コマ | ✅ OK。`index.json` の `used` を集計し実フォルダ・NPZと照合。不採用15件は、把持成功・配置失敗6件、把持成功・配置未判定4件、両方失敗3件、両方未判定2件。「15件すべて物理失敗」ではない | | scripted_v1の爪ラベル | 開くラベルが約2秒先行する設計が現行コードにも残る | ⚠️ 要注意。注記 `KNOWN_ISSUE_gripper_open_label_2sec_early.md` 7–46行と本線 `run_pick_place.py` 2273–2280、2317–2323行。各動画の全開放イベントを測った結果ではなく、通知順とdaemon待ちに基づく既知問題 | | FK表 | 1,705,187行。関節6軸と爪先3座標 | ✅ OK（表の構造）。CSVを1回読み込んで件数・列を確認し一時NPYに保持。生成由来・精度はA3参照 | ## A2. 「主原因は置き場所不足」か **❌ NG：主原因として確定できない。位置と成功率の関係を検証できる記録が不足している。** `D/runs/pi05_policy/` の実フォルダ130件を調査した。試行要約JSONは36件、把持・配置の人判定が両方あるのは31件。内訳は両方成功19件、両方失敗12件。残る99件は両方の判定が揃わず、失敗と数えられない。全ログのイベントは推論985行、画角確認100行、補正479行、ゲームパッド162行など。要約JSONの最上位キーとログのキーには、ブロックの実測初期座標・格子番号がなかった。画像中に位置情報はあるが、今回それを校正して130件の物理座標に変換してはいない。 成功19件中17件には補正回数が記録されている。補正・巻き戻し0件で成功しているのは `pi05_20261030_184245` と `pi05_20261031_185246` の2件。したがって19/31を「無補正成功率」とは呼べない。また、最新11-06の9試行はv2で、成功5件・失敗4件、全件に補正がある。v3はその日の22時台に保存されており、今回の試行群にはv3のモデル名がない。最新モデルで同じ症状が残るか自体が未測定である。 失敗12件の人判定はすべて把持も配置も失敗。ただしこれは「最初からつかめなかった12件」と同義ではない。例えば `pi05_20261106_184312/...json` 99–109行は両方失敗だが、同試行 `log.jsonl` 11行には閉じる命令がある。閉じる命令は把持成功の証拠ではない。`pi05_20261101_194042/log.jsonl` 13・15行には締め直し補正もある。把持不良・保持不良の両方を疑う余地がある。 録画あり36件の `top_meta.json` も読んだ。失敗12本の動画から各25%・50%・90%の計36枚を復号・目視確認した。ブロックが爪付近にある途中画像、爪から離れてマットにある画像、箱のそばまで腕が動いている画像が混在する。上からの静止画では高さ・保持力・落下瞬間が分からず、疎な抽出なので「つかむ前／つかんだ後」の件数は確定しない。全動画の連続再生・全イベントへの時刻対応までは行っていない。この限界を残して、位置だけの原因にまとめない。 2026-10-17の見立ては旧scripted系19本に対するもの。`P/デジタルツイン学習R2R2R/フィジカルAI研究室を作る/LOG348_article.md` 424–447、622–664行には、同じ置き場所でも爪が奥へずれること、学習回数を増やしても改善しないこと、夕方と夜の見え方の比較がある。これは位置不足の仮説を支持しても、現在の教示5位置＋補正入りモデルにそのまま適用できる因果証拠ではない。 **順番の修正：** v3の同一既知位置で再現性を先に測り、初期ブロック座標・箱座標・物体の見え方・閉じる時刻・持ち上げ後の保持・落下・接触を記録する。同じ位置でも失敗するなら、画角、実測と入力状態の差、爪タイミング、指令制限を先に疑う。既知位置が安定して近傍だけ落ちる場合に位置の拡張を主実験にする。 ## A3. FK表の由来と実機との差 **⚠️ 要注意：Unity由来という理解は支持される。ただし「実機との差が一律何mm」とは言えない。** `P/site_archive/code/r2r2r-content/articles/DAY177_article.md` 266–280行にはUnityのFK→CSV→実機という由来がある。計画の「LOG177」に対応する手元資料はこのDAY177の記事写しだった。CSVだけには生成来歴がなく、この写しから現在のv2全行の生成過程まで追跡できたわけではない。 この記事13行は約40mmのオフセットを述べるが、214–250行では肩120°などの命令を送り「下がらない」ことを根拠にしている。LOG352で分かった肩88°超の命令破棄が交絡する。**この記事だけで40mmを純粋なFK誤差の確定値とするのは不適切。** 近傍の `DAY179_article.md` 97–105行はFK予測と視覚実測の水平差54.5mm、124–136行は43.1mm、補正後のブロックとの距離13.5mmを記録している。こちらも当時の命令は肩110°・98°などで、現在の安全条件を満たす精密な校正試験ではない。視覚検出・命令未到達・座標原点の差を含む観測として扱う。 `D/diag/correction_memory.json` は38セル。保存補正ベクトルの大きさは中央値8.81mm、最大158.51mm。例は同ファイル12–15行のほぼZ方向−23.98mm、37–45行の約44.9mmと158.5mm。これは学習された補正値で、残差分布・FKの測定誤差ではない。現行本線1489–1498行では読み出した補正を上限処理して目標から引いている。保存された最大値がそのまま動作に使われるとも限らない。 直し方：Unity表は候補姿勢を探す道具と明記する。安全な命令範囲で「送った角度・到達実測角・視覚の爪先位置」を揃えて誤差を取り直し、それまでは補正記憶をそのまま案Bへ流用しない。 ## A4. scriptedからteachへ切り替えた理由と案A **❌ NG：案Aの「新しいコードがほぼ不要」は、前の詰まりを解消した説明になっていない。** LOG348記事654–688行は、スクリプトで成功データが集まる範囲が狭いこと、外側ではつかめずデータが作れないことを述べ、手で要所を教えて再生する方針に移っている。これとは別に爪ラベル約2秒先行の問題もある。`D/lab/Log10/README.md` は教示19本へ切り替え、旧モデルの続きではなく基盤モデルから学習し直した記録。実験READMEは肩超過・動作中の爪・検出誤りも示す。 履歴はgitコマンドを避け `S/.git/logs/HEAD` を直接読んだ。94–103行にはデモ記録導入、記録中のPLACE高さ記憶更新停止、肩安全制限・データ検査追加が並ぶ。ここには切替理由を直接確定する記述はなく、記事と実験記録を主証拠とした。 案Aは同じスクリプトで9位置に置くだけなら、狭い成功範囲という選択の偏りを再現する。新位置ごとの撮影・検出・到達・把持・配置を先に確かめ、失敗や不明を除外する必要がある。さらに通知順の修正または映像と対応した派生ラベル生成が必要。単に全試行へ2秒を足してはいけない（既知問題の注記64–73行は2回開放時の曖昧さを指摘）。 ## A5. 論文メモの数字 **✅ OK（数字の転記）。⚠️ 要注意（適用範囲と原論文内の不整合）。** ローカル原文PDFの12・13ページを抽出し、ページ画像でも表3〜5を確認した。メモ58–60行と照合： | 条件 | PDFの結果 | |---|---| | 動きを作り直す処理を外す、生成1,000本 | π0-FAST 80.0→0.0%、Diffusion Policy 53.3→6.7% | | 背景・机の変更を強める、各1,000本 | π0-FAST 73.3→35.3%、Diffusion Policy 53.3→33.3% | | 実150本・生成1,000本・両方 | π0-FAST 73.3／80.0／73.3%、Diffusion Policy 40.0／53.3／86.7% | 35.3%は原文にもあるためメモの転記ミスではない。ただし表4の説明は15試行で、35.3%は15回の単純成功率の刻みに合わない。原文だけで理由は分からない。表5本文はπ0-FASTの単独生成と混合を同程度と述べる一方、表の単独生成は80.0%、混合73.3%。厳密には「混ぜれば良くなる」は支持されない。 対象はマグをコーヒーメーカーに置く課題で、mechArmでもπ0.5でもない。位置への適応が重要という着想は使えるが、現在の主原因が位置不足であることや57本で改善することの根拠にはならない。 ## B1. 3×3の到達性とカメラ **❌ NG：計画には9位置の座標・間隔・高さがなく、実機で全部届くとは判定できない。以下は候補表の存在を調べた計算である。** CSV全1,705,187行のうち、肩j1≤88°は1,158,083行。さらに本線 `fk_lookup.py` 83–86行のPICK高さ帯10〜40mmに絞ると165,345行。表の座標はUnityのメートルで、水平はX・Z、高さはY。安全候補の水平外接範囲はX −325.824〜305.350mm、Z −110.343〜335.988mm。ただし外接長方形内部がすべて届くという意味ではない。 検証用にX={140,180,220}mm、Z={180,220,260}mmの候補格子を置き、高さ帯と肩制限を満たす行から水平最近傍を選んだ。指定されていない格子をこちらで仮定したもので、計画の確定位置ではない。 | X mm | Z mm | 表内の水平残差 mm | 表の高さY mm | 肩j1 ° | CSV行 | |---:|---:|---:|---:|---:|---:| |140|180|0.015|16.095|59.89|1349473| |140|220|0.450|29.526|72.39|1522701| |140|260|3.075|23.863|88.00|720323| |180|180|0.083|24.237|67.39|1529590| |180|220|0.094|38.110|77.39|1642941| |180|260|0.419|30.868|87.74|19031| |220|180|0.236|38.109|77.39|1530094| |220|220|3.837|27.959|87.74|19066| |220|260|16.688|36.133|87.74|16582| 最外点は16.7mm残る。表内誤差に実機誤差が加わり、高さも16〜38mmと一定でない。この検索は爪の向き・開口・腕の経路・衝突を条件に入れていない。本線の既定距離許容は45mm（`fk_lookup.py` 85行）で、候補が返ることと小さいブロックを挟めることは別である。 少し内側のX={120,160,200}、Z={180,220,260}mmなら、同じ検索の残差は全点0.23〜3.69mmになる。実機確認の候補としてはこちらがましだが、実機で届く証明ではない。全関節・爪姿勢・上空姿勢と経路を含めて確かめ、合わない点を除いて不規則な9位置にしてもよい。 視認性は教示5位置の各先頭画像を目視確認（`contact.jpg`）。位置1〜4ではブロックと箱が見え、位置5はブロックが腕・爪の陰にほぼ隠れる。LOG352記事14593行とも一致する。箱も動作中には腕で隠れる。未設置の9点全部が見える証拠はない。位置の採否はカメラ画像384×216で行い、物理到達性とは別に確認する。大きな格子印は新たな画像の違いにもなるため、初回基準測定から同じ印・照明・画角を使う。 ## B2. 案A・B・Cの実装と危険箇所 **⚠️ 要注意：既存部品は使えるが、どの案もそのまま27本収集へ進めない。特にBは姿勢の連続性と静止確認が不足。** | 案 | 使える部分 | 必要な追加・修正 | |---|---|---| | A | `S/dynamic_tracking_limits/place/place_modules/run_pick_place.py` の検出・lookup・視覚補正・記録、`pi05/build_demo_episodes.py` の変換 | A4の成功範囲の確認、爪通知と画像の同期修正、採否の確定。補正記憶・高さ記憶の状態を試行ごとに固定・記録し、モデル比較中に変化させない | | B | 教示JSONの要所抽出、`record_teach_demo.py` の再生・到達記録・撮影、`fk_lookup.py` の制約付き検索 | 教示の関節→爪先推定、物体位置の差分変換、姿勢を保つ候補選択、全区間の経路確認、範囲外の拒否、書き換えた要所を再生器へ渡す入力形式、位置別の実機調整が必要。既存表の最近傍検索だけでは足りない | | C | `run_policy_client.py` のゲームパッド補正と記録、`build_correction_episodes.py` の変換 | HOMEからの全行程を確実に記録できる操作手順・位置ID・初期状態の統一。現行はモデルと人の区間が混じる設計なので、人だけの全行程取得を事前確認。新位置の安全範囲を既存データ由来のまま制限しない | Bの具体的な穴： - 肩88°制限は既存lookupの候補除外で利用できる（`fk_lookup.py` 161–166行）。しかし教示再生の基礎 `waypoint_replay.py` 96–103行の肩上限は123°であり、同じガードではない。作った全要所・中間目標に88°制限を明示的に適用する必要がある。88°へ切り詰めるだけでは位置が変わるので不適合を返す。 - XYZだけ一致しても爪の向きは決まらない。元の関節に近い枝・爪姿勢・降下方向を保つ制約が必要。持ち上げ要所もPICKの移動に追従させ、固定した箱のPLACEへは安全な上空でつなぐ。全行程の平行移動は箱の目標までずらしてしまう。 - 再生器の爪前静止確認は安全停止を保証しない。`record_teach_demo.py` 2855–2868行は最大待ち後、確認できなくても警告だけで爪を動かす。未到達でも進む運用があるので、案Bに流用するなら「静止未確認時は人の確認まで次へ進まない」扱いが必要。 - 同ファイル2830–2854行では爪通知を呼び出し後へ変更済みだが、開放の自動読取り確認は取りやめ、人が判断する方針。データセットの旧注記82–84行の自動確認説明は現行コードと食い違う。古い説明だけを見て安全機能があると扱わない。 - 補正記憶は本線の狙い座標へ適用する設計（本線1489–1498行）で、教示再生器へ自動で効くものではない。FK表の系統誤差と、姿勢を変えたBの誤差が同じとも限らない。まず1位置1動作を実測し、補正の妥当性を判断する。 ## B3. 動きの癖が違うデータの混合 **⚠️ 要注意：時間処理を揃えず混ぜる比較では、置き場所の効果を分離できない。** NPZから各エピソードの数値を計算し、下表はその中央値。停止率は隣接状態の最大関節変化が0.1°未満（10Hz換算1°/秒未満）。速度も6関節中最大の差を10倍した値であり、爪先の速度ではない。補間・停止除去後の学習時系列の比較で、実機の真の速度とは区別する。 | データ | 本数 | 長さ 秒 | 停止率 | 速度中央値 °/秒 | 速度90%点 °/秒 | 閉じる／開く時刻 秒 | |---|---:|---:|---:|---:|---:|---| | scripted_v1 |20|28.8|25.9%|6.57|32.74|8.2／18.4| | teach_v1（途中停止除去前） |19|20.8|37.3%|12.74|29.48|7.8／16.6| | 最新データの教示部分 |19|14.0|7.6%|17.52|30.09|4.9／10.9| | 最新データの補正入り部分（全体） |11|13.5|14.0%|12.40|30.00|5.4／12.0| | うち従来補正の試行 |7|12.6|17.1%|12.43|30.00|5.0／11.4| | うちゲームパッド補正の試行 |4|18.3|6.1%|11.47|31.87|6.15／14.55| 証拠：`audit.py`・`metrics.py`・`statistics.py`、全対象NPZ。閉開時刻は最初の閉じる切替とその後の開く切替で、元撮影時刻ではない。補正入り11本は7本の従来補正と4本のゲームパッドを合わせたもの。最後の2行も各試行のモデル区間を含むため、人が操作した区間だけの速度比較ではない。4本の違いは位置・撮影日・停止処理とも交絡し、操作方法の因果効果を示さない。 同じ画像・状態の近くで、停止／移動／爪命令が異なる教師が混じると学習目標がばらつく。とくにscriptedの開く時刻は既知の誤ラベルを含む。元のteach19本も停止処理で3,949→2,717コマへ変わっている。したがって「スクリプト由来だから迷う」とは断定せず、まず同期・停止除去・速度・爪の待機条件を統一する。案別に保存し、同じ位置で少数を比較してから混ぜる。単なる30本＋27本ではなくフレーム数と位置ごとの寄与も管理する。 ## B4. 正規化 **❌ NG：「平均とばらつきを作り直すか決める」だけでは不十分。現行は分位点による正規化で、行動は差分化後に集計する。** コード上の経路： 1. `S/pi05/convert_to_lerobot.py` 55–64行は絶対状態・絶対行動をLeRobotへ移す。ここではopenpi用の正規化値を計算しない。 2. `S/pi05/openpi_addon/install_addon.py` 47–56行が関節6軸だけチャンク先頭状態との差分へ変える。爪は絶対値のまま。行動の正規化値を生の絶対角だけから計算してはいけない。 3. `/Users/shimazueiichi/openpi/scripts/compute_norm_stats.py` 89–113行がデータ変換後の状態・行動から統計を作り、設定のassetsとrepo_idの下へ保存する。既存手順 `S/pi05/RUNPOD_TRAIN.md` 103–113行にも計算コマンドがある。 4. ローカルopenpi `training/config.py` 179–195行で読み込み、π0以外は分位点を使う。`training/data_loader.py` 172–188行が学習時に適用、`training/checkpoints.py` 74–76行がチェックポイントassetsへ保存する。 5. `policies/policy_config.py` 59–64、80–86行は推論時にチェックポイントの統計を読み、入力正規化と出力の逆変換に使う。 現行v5の準備スクリプトは再計算しない。`pod_prep_trim_corr.sh` 48–59、89–95行でv2の値をコピーしSHAを固定する。今回v3現物の `1499/assets/local/mecharm_redblock_box_teach_v1_trim2_corr_v5/norm_stats.json` のSHA256は `80346837eb12ffa6e683416f0fa41a196519fa8459d1f0b666b06cd621c7f61b` で、スクリプトの固定値と一致した。 分位点の式は `transforms.py` 141–145行。範囲外は±1を超えるが、ここではクリップされない。したがって「範囲を広げると正規化で必ず切り捨てられる」は誤り。例として状態j0の下上分位点は−61.626°・44.620°で、60°は約1.29になる。爪や速度の変化も、差分行動の分布を変える。 一方、推論クライアント `run_policy_client.py` 182–195、248行以降には学習データの関節範囲＋余裕による指令制限が別にある。こちらは実際に目標を制限する。位置を広げたのに古い `--raw-dir` を使うと、正規化以前に動けない可能性がある。安全範囲と正規化は別々に比較・記録する。 提案：最初の小規模追加では旧統計を維持して継続学習との座標対応を保ち、新データの正規化後の外れ量を診断する。大きく外れる場合だけ、正しい変換経路で統計を再計算する別実験にする。統計だけ差し替えると入力・出力の意味が変わるため、学習と推論で同じ統計を必ず同梱し、再学習して比較する。ローカルopenpiはコード確認用であり、遠隔GPUの現行実装を再実行して確認したものではない。 ## B5. 9位置×3回で差を言えるか **❌ NG：予備的な故障分布を見る回数としては使えるが、任意の改善を判定できる設計ではない。後から合格数を決める点も修正が必要。** 二項比率の95% Wilson区間をPythonの標準数式で計算した。1位置3回で0成功でも真の成功率の区間は0〜56.1%、3成功でも43.9〜100%。全27回を独立な同一分布と仮定しても、14/27の区間は34.0〜69.3%。位置ごとの成功率が違い同じ日に連続すると、単純な27回合算の仮定にも注意がいる。大きな差なら27回でも検出できる場合はあるが、「明らかに増えた」の保証にはならない。 必要数は期待差による。両側有意水準5%・検出力80%・独立した2群・同数、通常の正規近似なら次の目安になる（端数切上げ、連続性補正なし）。 | 想定成功率の差 | 各モデルに必要な回数 | 9位置へ均等割りする目安 | |---|---:|---:| |20%→50%|39|各位置5回＝45| |50%→70%|93|各位置11回＝99| |50%→60%|388|各位置44回＝396| 計算式：平均率をm、差をdとして、各群n＝｛1.96√[2m(1−m)]＋0.842√[p0(1−p0)＋p1(1−p1)]｝²/d²。これは全体率の目安で、各位置別に同じ差を言いたいなら位置ごとに必要。格子内の繰り返しや対応ありの比較には別の検出力設計を使う。 **評価漏れもある。** 段階0と学習収集を同じ9点にすると、その9点は学習済み位置になる。段階3だけに足す3点には学習前の基準値がない。未学習位置での改善を主張するなら、この3点も学習前から測り、収集から外す。先に主評価・最小限意味のある差・回数・中止条件を定める。古いモデルと新モデルの順を日・位置の中で交互または無作為にし、補正なしで同じ安全設定を使う。途中で結果を見て合格線を変えない。 ## B6. FKで箱の縁を見る処理 **⚠️ 要注意：記録専用としては入れられるが、今ある位置→関節の検索器を逆向きに使うだけでは不十分。** 挿入候補は `S/pi05/run_policy_client.py` 1283行の推論配列取得後と、1289–1298行の中継点計画・記録生成後。生の全チャンクと、制限後に実際に送る中継点を両方記録する。さらに実測現在角から最初の点までと、各送信点間の経路も対象にする。現行のaction horizonは10コマ＝1秒（`install_addon.py` 76行）で、計画の「数十コマ」は現設定とは違う。 `fk_lookup.py` は位置から関節を探す器である。任意の6関節から爪先を返す評価器は別に要る。表を逆検索するなら関節空間での近さ・範囲外判定・推定不確実性を記録し、表にない動作を安全とみなさない。できればURDFの連続FKで評価し、実機の爪先・保持ブロックまでの変換を校正する。 箱の水平位置は上画像の白箱検出＋保存homographyから得られる。既存の `D/mecharm_calibration/teach_poses_LOG352/measure_*.json` に `box_xz_m` とhomographyがあるが、README 38–39行は箱検出の不安定さを認める。古い値を現在の真値として使わない。箱の外周・向き・縁高さ・ブロック寸法・座標原点を測定して保存する必要がある。上カメラの平面変換だけで縁の高さは測れない。本線コメントの箱高さ43mm（92–94行）は参考値で、今回の再測定ではない。 また `fk_lookup.py` 87–92行には、爪先が105.3mmでも前腕が箱に当たった記録がある。爪先だけ高いことは衝突回避の証明にならない。保持ブロック・爪・前腕を含めた形状と余裕が必要。段階4は最初は警告候補の記録に限定し、停止・安全保証と結びつけない。 ## C1. より安く効く可能性のある方法 **⚠️ 要注意：ブロックだけの切り貼りは、正しい関節角を作らない。** 画像中のブロックだけを動かし元の行動を付けると、ブロックが移動したのに腕は元の場所へ向かう教師になる。掴んだ後のブロック・爪の重なりや箱への軌道も破綻する。画像の切り貼りだけで新しい位置の実演を増やしたとは扱わない。行動を作り直すなら結局、校正されたFK・姿勢制約・接触段階に応じた物体と腕の整合・実機確認が必要で、安い画像編集だけでは済まない。 優先する対案は以下。 1. **記録済みデータの同期と実行条件を確認する。** 爪時刻・停止の長さ・目標状態を入力する設定と実測のずれ・クランプ量・カメラの写り方を点検する。9位置を増やす前に既知位置での失敗原因を小さく切り分ける。 2. **教示A案の狭さを再現しない収集をする。** 視認でき、実機で人が合わせられる近傍位置を2〜3点だけ増やし、既存の教示再生＋必要時の人の調整で質の揃った少数デモを取る。案Bの全自動化はこの効果が出てから投資する。 3. **見た目だけの軽い拡張は位置拡張と分ける。** 明るさ・色・センサー雑音の弱い変化、または画像全体の小さな平行移動なら、物体と腕の相対配置を維持できる。行動ラベルを変えずにカメラの見え方への頑健性を試す用途に限る。大きなクロップ・回転で必要物体が消えるものは除外し、物理位置の一般化の証拠とはしない。 4. **補正の多い状態へ絞って収集する。** 無補正試行のどの段階で外れるかを記録し、その直前状態からの回復を人が示す。新しい場所だけでなく保持・配置のデータが必要な場合にも対応できる。評価時の人の補正は成功へ混ぜない。 これらの費用対効果は今回の実測結果ではなく、実装量と既存の問題に基づく提案である。 ## C2. 段階の順番 **❌ NG：現状のまま0→大量収集→学習では、原因と改善を分離しにくい。** 推奨順序は次のとおり。 1. データとモデルの由来を固定し、v3のチェックポイント・正規化SHA・クライアント設定・カメラ配置・補正記憶を記録する。初期座標、保持確認、失敗段階、補正の有無の記録欄を用意する。 2. 視認性と安全な実機到達を人が確認した既知位置で、v3を少数回、補正なしで測る。動作中の爪や未到達時の継続を収集器から持ち込まない。 3. 学習用の位置と未学習の評価位置を先に分け、両方で学習前を測る。27回は予備測定と位置づけ、主評価の回数と合格条件を事前に決める。 4. 失敗が位置依存なら2〜3の新位置で教示・人の調整を使って少数データを取る。原因が保持・箱付近なら、その部分の記録と実行条件を先に直す。 5. 時間処理と統計を揃えて小規模学習を比較する。改善がなければ自動生成27本へ拡大しない。案A・Bは採用率・手直し時間・ラベル品質でも比較する。 6. 旧新モデルを同じ未学習位置・同じ条件で評価する。FKによる箱接近の記録は並行開発できるが、測定前から安全保証には使わない。 ## 最終回答 ### 1. このまま段階0に進んでよいか **❌ このままの27回の実機実行は勧めない。** 9位置の座標・実機到達・視認性と、比較する設定が未定。既知位置で最新v3の無補正試行を少数確認する、という修正版の段階0にする。今回の読み取り検証では実機を動かしていない。 ### 2. 進む前に直すべき項目（優先順） 1. **実行の安全条件と記録**：肩88°、静止未確認時の爪命令、未到達継続、物体が見えない位置を扱う。モデルv3・実測／目標入力・指令制限・補正なしの定義を固定する。 2. **原因を断定しない**：位置不足は仮説へ戻し、既知位置でも失敗するかと、保持・配置段階を分けて測る。31件の判定と19件の成功を無補正成功率に流用しない。 3. **評価設計**：学習用と未学習評価用の位置を先に分け、追加3位置も学習前から測る。効果の大きさ・必要数・合格条件を事前に決める。 4. **データの説明と同期**：補正入り11本のうちゲームパッドは4本へ訂正。scriptedの爪ラベルを直す前に混ぜない。停止除去と時間軸を統一する。 5. **正規化と安全範囲**：分位点・差分行動の統計、固定SHA、推論assets、`--raw-dir`による指令制限を一組で管理する。 6. **案Bと段階4の過大評価を直す**：FK候補があることを到達・把持・衝突回避と同一視しない。連続姿勢、箱の形状、実機校正を追加する。 ### 3. 対案の要点 **まずv3の既知位置で失敗を分解し、見える・届く少数の新位置で、品質を揃えた教示と人の調整を足して比較する。効果が見えたところで自動収集を広げる。** ブロックだけの画像編集で行動を据え置く方法は採らない。描画環境を作るより先に、同期・視認性・実行条件・評価の穴を塞ぐ方が今回の記録には合っている。 `````` ## 計画の第2版：置き場所を増やす前に、失敗がどこで起きているかを分ける Codex の指摘を受けて、順番を入れ替えました。まず Unity で過去の失敗を再生して分け、実機では最新モデルを教えた位置で補正なしで測る。置き場所で失敗が変わると分かってから、新しい位置を足す、という形です。 - ねらい：原因が分からないまま、実機で30本近いお手本を集める手間をかけない - 変わったこと：段階0を「9か所×3回」から「Unity で過去の失敗を分ける → 教えた位置と、評価用に取っておく位置で測る」に変えた - 見込める効果：置き場所が原因でなければ、集めるお手本の種類そのものが変わる（この回では実際にそうなった） ### research_r2r2r_to_pi05_plan_20261107.md（第2版） ``````markdown # R2R2R の考え方を π0.5 の学習に活かす計画（第2版） 作成: Claude（2026-11-07）　状態: Codex の敵対的検証（`codex_report_r2r2r_to_pi05_20261107.md`）を受けて書き直した版。第1版は `research_r2r2r_to_pi05_plan_20261107_v1.md` 元にした論文メモ: [paper_r2r2r_20261009.md](paper_r2r2r_20261009.md) ## 1. 何を確かめたいか mechArm 270 Pi で動かしている π0.5（Physical Intelligence のロボット用 AI）は、補正なしで成功した試行が今まで2回しかない。 見立ては「お手本の種類が少ない」こと。教えた動きの再生19本は、教えた記録5つ（ブロックの置き場所5か所）を繰り返したもので、π0.5 が見た場面は5通りしかない。R2R2R の論文でも、元の動きを再生しただけのデータでは成功率が 80% から 0% に落ちていて、効いていたのは置き場所の散らばりだった。 ただし、この見立てはまだ証拠がない。推論の記録にブロックを置いた位置が残っておらず、教えた位置の近くでも失敗している（Codex A2）。なので第2版では、置き場所を増やす前に、失敗がどこで起きているかを先に分ける。 ## 2. 今の手元（第1版からの訂正を含む） | 項目 | 値 | |---|---| | 最新の学習データ | 30本・4,301コマ。教えた動きの再生 19本＋補正入り 11本（**前のやり方の補正 7本・ゲームパッドの補正 4本**。第1版で「全部ゲームパッド」と書いたのは誤り） | | 教えた位置 | 5か所（位置ごとに 1〜5本）。位置5は上カメラから見てブロックが腕の陰にほぼ隠れる | | カメラ | 上の1台。学習は 384×216・10コマ/秒の形だが、実際に撮れているのは 8コマ/秒前後（間は埋めている） | | 最新のモデル | trim_corr_lora_v3（2026-11-06）。**実機ではまだ一度も試していない**（11-06 の9回は v2） | | 推論の記録 | 判定がそろった31回のうち成功19回。成功のうち17回は補正入りで、補正なしの成功は2回 | | FK テーブル | Unity で作った表。実機とのずれは昔の記録で 40〜55mm だが、肩が 88° を超えて命令が捨てられていた影響が混ざっていて、純粋なずれの大きさは分からない | | 正規化 | 分位点で割る形で、範囲の外の値も切り捨てない。ただし推論のときは別に、学習データの関節の範囲＋余裕で指令を止めている（`run_policy_client.py` の `--raw-dir`）。置き場所を広げたらこちらも合わせる | ## 3. 進め方 ### 段階0　失敗を分ける **0-1　過去の失敗を Unity で横から見る（依頼107。実機は使わない）** 推論の記録の関節角を Unity のアームで再生し、爪を閉じた瞬間の高さ、持ち上げたあと、箱との近さを横から見る。失敗をつかむ前・つかんだ後・箱に当たった・分からない、に分ける。同じ依頼で、次に試す置き場所の候補（届く・上から見える・箱に当たらない）と、Unity と実機の画の見比べも出す。 **0-2　記録を足す（r2r2r-scripts への依頼。0-1 のあと）** 推論の記録に、ブロックと箱の最初の位置（上カメラの最初のコマから）、爪を閉じた時刻、持ち上げたあとにブロックを持っているか（人が y/n）、失敗の段階（人が選ぶ）を足す。使うモデルと正規化の値（SHA）と指令の範囲も1回ごとに残す。 **0-3　実機で v3 を測る（エイイチ）** 補正なしで動かす。位置は2種類に分けて最初に決める。 - 教えた位置（見える4か所）：各3回。ここで失敗が多ければ、置き場所の前に直すものがある - 評価用の位置（お手本に使わない新しい位置2か所。0-1 で届く・見えると分かった点から選ぶ）：各3回。学習前の値として残す **判断** - 教えた位置でも失敗が多い → 置き場所ではなく、爪のタイミング・持ち上げ・箱の手前の動きを先に直す - 教えた位置は成功し、新しい位置で失敗する → 段階1へ ### 段階1　新しい置き場所のお手本を少し足す - 0-1 で使えると分かった点から 2〜3か所（評価用の位置とは別） - やり方は、今と同じ「教えた動きの再生」に、必要な所だけゲームパッドで人が合わせる形。動きの癖を今のお手本とそろえるため - 各5本ほど。止まりの詰め方・時間の扱い・爪の開け閉めの記録を、今の作り方（trim2・corr_v5）とそろえる - 自動で大量に集める仕組み（series2 の本線で回す案・教えた動きをずらして作り直す案）は、ここで効果が見えてから作る ### 段階2　学習する - v3 から続けて学習（corr_v5 と同じ形・Runpod の A40・1回約300円） - 正規化の値はいったん今のまま使い、新しいデータがどれだけ外にはみ出すかを測る。大きく外れたときだけ、計算し直す別の実験にする - 推論の指令の範囲（`--raw-dir`）は新しいデータに合わせる ### 段階3　同じ条件で比べる - 評価用の位置で、v3 と新しいモデルを同じ日に交互に、補正なしで動かす - 回数の目安：成功率が 20% から 60% に上がるくらいの大きな差なら、各モデル20回ほどで言える。小さな差を言うには何十回も要るので、まずは大きな差が出るかを見る - 合格の線は 0-3 のあとに、段階3を始める前に決める（結果を見てから変えない） ### 並べて進めること　箱の手前の動きを見る 0-1 の結果で「箱に当たる」が多ければ、π0.5 が出した次の 10コマ（1秒分）を Unity のアームで先に動かして、前腕と爪が箱の縁に近づくかを記録する。最初は記録だけで、止めない。 ### やらないこと（今は） Unity で映像を描いて学習に入れること。0-1 の画の見比べで、Unity の画が本物とどれだけ違うかを見てから考える。 ## 4. 置き場所 | もの | 場所 | |---|---| | Unity で確かめた結果 | KIOXIA `lab/twin/20261107_失敗の再生と置き場所/` | | 新しいお手本 | KIOXIA `datasets/`（名前は段階1で決める） | | 実機の推論・評価の記録 | KIOXIA `runs/pi05_policy/`、まとめは `lab/Log{NN}/`（番号はエイイチに確認） | | 学習済みモデル | KIOXIA `checkpoints/` と Hugging Face の非公開リポジトリ | ## 5. エイイチに決めてほしいこと - 0-3 の回数（教えた位置 4か所×3回＋評価用 2か所×3回＝18回で、補正なし）でよいか - 段階3の回数と合格の線（0-3 のあとで） `````` ### series2 のページに、π0.5 へのつなぎ方を載せた デジタルツイン学習 R2R2R のシリーズのページ（pixelstomotors.com/series2）の説明に、名前のもとにした研究と、ここで作った仕組みを π0.5 の学習に引き継いでいることを足し、その下に「このシリーズで作ったもの」と「π0.5 の学習へのつなぎ方（計画・検証中）」を2列で並べました。第1版で公開したあと、Codex の検証を受けて右の列を第2版に合わせています。 #### commit 4d537a4（p2m-site・series2 の説明と2列を足す） ``````diff commit 4d537a42c996863226231d6569b781db5b1ef887 Author: Claude Date: Sat Nov 7 11:47:24 2026 +0900 series2: R2R2R の由来と π0.5 へのつなぎ方を説明に足す（案・公開前） Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01LevEXWMtB3DzkTDGboCBeH diff --git a/src/lib/records.ts b/src/lib/records.ts index b72be1d..049de06 100644 --- a/src/lib/records.ts +++ b/src/lib/records.ts @@ -48,7 +48,11 @@ export const SERIES_PAGES: Record Date: Sat Nov 7 12:15:05 2026 +0900 series2: π0.5 へのつなぎ方を Codex の検証後の計画に合わせる Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01LevEXWMtB3DzkTDGboCBeH diff --git a/src/lib/records.ts b/src/lib/records.ts index 049de06..991477d 100644 --- a/src/lib/records.ts +++ b/src/lib/records.ts @@ -51,7 +51,7 @@ export const SERIES_PAGES: Record/dev/null | head -2; ls ~/.plastic4 2>/dev/null | head -3; O=\"/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所\"; mkdir -p \"$O/code\"; cp Assets/TwinCheck/Editor/*.cs \"$O/code/\"; cp \"Assets/TwinCheck/Python~/\"*.py \"$O/code/\"; ls \"$O/code\"; `````` ### KIOXIA lab/twin/20261107_失敗の再生と置き場所/README.md ``````markdown # mechArm のデジタルツイン（Unity）で、π0.5 の失敗した試行を横から見て、置き場所の候補を確かめる（2026-11-07・依頼107） 計画: `PixelstoMotors/research/research_r2r2r_to_pi05_plan_20261107.md` の段階0-1。 **実機は動かしていない**（Raspberry Pi・daemon・SSH・シリアルには触れていない）。実機の記録（`runs/`・`mecharm_calibration/`・`fk_tables/`）は読むだけ。 Unity は `Robotics_UX_New`（2022.3.62f3）を画面なし（`-batchmode`）で起動し、毎回終わらせた。既存のシーン・スクリプト・プレハブは変えていない。作業用のシーンは `R2R2R-20260224.unity` のファイルの写し（`Assets/TwinCheck/TwinCheck_Base.unity`）を開くだけで、保存していない。 ## 先に読むこと（結論） | | 結果 | |---|---| | A 関節角と爪先の対応 | 表から無作為に選んだ20行で、**ずれの最大 139.7mm・中央値 0.3mm**。腕がマット・箱・ブロックに触れない16行では最大 4.2mm・中央値 0.2mm。大きくずれた2行は、角度どおりに置くと腕がマットの下（爪先 y=-121mm）に入る姿勢で、表の値はマットの高さで止まっている。**置き方（角度をそのまま度で置く・符号 +・根元が原点）は合っていて、表の中に「物理で床などに当たって止まった値」が混ざっている**と見ている（200行でも同じ傾向。下の A 節） | | B 失敗12件の分類の候補 | つかむ前に外れた **0**・つかんだが持ち上げで落ちた **7**・箱に当たった **0**・分からない **5**（5件とも、爪を閉じたあと 15mm 以下しか持ち上げていない） | | C 置き場所の候補 | 14点（格子9＋教えた位置5）のうち **使えそう 12**・要注意 1（G_x160_z180、つかむ姿勢で腕がマットの下 3.5mm）・使えない 1（G_x120_z180、表の中で選べる行がない） | | D 実機と Unity の画 | `D/d_side_by_side_384x216.png`（左が実機、右が Unity。学習と同じ 384×216） | **大事な注意**: Unity の「角度 → 位置」と実機のずれは、この依頼では測っていない（昔の記録で 40〜55mm。肩 88° 超の命令が捨てられていた影響が混ざっていて、本当の大きさは分からない）。A で測ったのは Unity と表のずれだけ。B の分類は、失敗の値を**成功5件の値の幅**と比べて、大きく外れたときだけ決めている。 --- ## A. 関節角と爪先の対応を合わせる ### 置き方 `FKScanner_MotionBased.cs` と同じく、j0〜j5 を link1〜link6 の ArticulationBody にそのまま（度）対応させ、爪先は左右の `gripper_{left,right}1/…/Instance-Def-210103_4302_101/Brep` の中点、根元（base）からの差（Unity の m・高さは Y）とした。 ただし表を作ったときの `SetJointPositions`＋0.5 秒の物理は使わず、**ドライブ（stiffness 20,000・damping 2,000・target）には触れず、物理も回さず**、各リンクの Transform を「親のアンカー → X 軸まわりに角度だけ回す → 子のアンカーの逆」で置いた（ArticulationBody の回転関節と同じ式。`TwinArm.SetAngles`）。符号は + と − を試し、+ で合った（− は中央値 336mm）。シーンの20体どれで置いても同じ値（差 0.00mm）。 ### 結果 | 行の選び方 | 行数 | ずれの最大 | 中央値 | 腕が触れない行（数・最大・中央値） | 10mm を超えた行（うち腕が触れる） | |---|---|---|---|---|---| | 無作為 20行（乱数の種 107） | 20 | **139.7mm** | **0.3mm** | 16行・4.2mm・0.2mm | 2行（2行） | | 補足：無作為 200行（種 1070） | 200 | 294.2mm | 4.1mm | 137行・97.1mm・0.3mm | 54行（47行） | - 「腕が触れる」= 角度どおりに置くと、link1 から先の見た目のどこかがマットの下に入るか、シーンの白箱・赤ブロックにめり込む姿勢 - 20行で外れた2行は、どちらも肩 j1=110°台で、角度どおりなら爪先が y=−121mm・−63mm（マットの下）。表の ee_y は 10mm 前後（マットの上で止まった値） - 200行で、触れないのに大きくずれた行（最大 97mm）は j2 が 40〜62° と大きいものに集まっている。自分の腕どうしの当たりか、表を作った当時のシーンの物に当たった可能性があるが、確かめていない - 4mm 前後の小さなずれの行（j1 が整数の古い系統の行）は、表を作った当時のアンカーの値が今の写しと少し違う可能性がある → 置き方は合っていると判断し、B 以降に進んだ。**B・C は表ではなくこの置き方で直接計算**している。C で表から姿勢を選ぶときは、表と Unity の差が 5mm 以内の行だけを使った。 ファイル: `A/a_sample_rows.csv`（20行）・`A/a_unity_tip_20.csv`（Unity の値・20体分）・`A/a_result_20rows.csv`（1行ごとのずれと接触）・`A/a_*_200*.csv`（200行） --- ## B. 失敗した試行を再生して横から見る ### 対象 `runs/pi05_policy/` のうち `{run_stem}.json` の `judgments` に pick と place の人の判定が両方あるもの **31件（失敗12・成功19）**（Codex の数えと同じ）。失敗12件全部と、比べる成功5件（補正なしの成功2件全部＋補正ありの成功を日ごとに古い日から1件ずつ）。一覧は `B/selection.json`。 ### 使った記録 - 関節角: `recording/states.csv` の `event_type=actual`（j0〜j5）。前に残した行から全関節の差が 0.3° 未満で 2 秒以内の行は間引いた（17件で 2,085 行） - 爪: 同じファイルの `gripper_cmd`（0=開・1=閉）。閉じた瞬間は「最初の 1」の時刻 - 補正の区間: `stage=correct` の始めから次の stage までの区間（表の「補正の区間」列。秒は記録の始めから） - ブロック: 最初のコマでは HOME の爪の真下に半分隠れているため、**爪を閉じる命令より前のコマを1つおきに全部見て、赤がいちばん大きく見えたコマ**で位置を取った（本線と同じ HSV のしきい値、根元から 120mm 以内・細長い赤（配線）は外す） - 白箱: 本線の 06f（テンプレート＋輪郭）では、どの試行でも輪郭が1つに絞れずテンプレートの中心（箱の角に寄る）に頼っていたため、**白い四角の輪郭を自前で取り直し**、テンプレートの中心にいちばん近い四角を採った（`twin_prep.detect_box_white`）。向きは輪郭の長方形から求めた（どの試行も 87〜90°＝ほぼまっすぐ） - 画像の点 → マットの上の点: 各試行の最初のコマでマットの四隅を検出し、本線と同じ対応（`visual_servo_pick` の (±310, −89)・(±310, 361) mm）でホモグラフィを作った - **箱の高さは仮に 43mm**（本線のコメントの値。シーンの白箱もこの高さ） ### 検出で怪しいもの - `pi05_20261031_185246`（成功）: 上カメラの動画がはじめから終わりまでピンぼけ（鮮明さ 1〜2）で、ブロック・箱を検出しなかった。**ほかの試行の中央値で仮に置いた**（`trials.csv` の `*_pos_source` 列）。この試行は爪の命令の記録もない - 全試行で、ブロックは x≈−48〜−62mm・z≈240〜256mm、箱は x≈−194〜−210mm・z≈233〜240mm（同じ場所に置いていた）。教えた位置5（Unity で x=−70・z=253mm）のあたり - D で分かったこと: 画像の上の端はマットの z≈−55mm に当たり、本線の決め（−89mm）と約 34mm 違う。ブロックのあたりでは本線の座標が **約 9mm 手前**にずれる計算。また、ブロックの上面（高さ 24mm）を床の点として直しているので、さらに数 mm ずれる。B はすべての試行で同じずれ方なので、成功と比べる分類には効かない - Unity では、運んでいるブロックは描かない（ブロックは最初の位置のまま） ### 場面の決め方 - 閉じた瞬間: 爪を閉じる命令（gripper_cmd=1 の最初）にいちばん近い記録の行 - 持ち上げたあと: 閉じたあと、j0 が閉じたときから 8° 以上回る（箱へ運び始める）か、次の開く命令か、記録の終わりまでのうち、爪先がいちばん高かった行 - 箱にいちばん近い: 閉じたあと、爪先・爪・前腕（link3〜6）のどれかが箱の4つの壁（底板は除く）にいちばん近づいた行。距離は見た目のメッシュの頂点（間引き）と壁の直方体の距離、めり込むと負 ### 分類の候補の決め方 Unity と実機は数十 mm ずれうるので、**失敗の値を成功5件の幅と比べ、大きく外れたときだけ**決めた。 | 分類 | 条件 | |---|---| | つかむ前に外れた | 閉じた瞬間の爪先の高さか、ブロックとの水平のずれが、成功の最大（高さ 25.3mm・ずれ 39.4mm）より 15mm 以上大きい | | 箱に当たった | 閉じたあとに、爪先・爪・前腕が箱の壁に 10mm より深くめり込む。±10mm 以内は「縁すれすれ」と書くだけ | | つかんだが持ち上げで落ちた | 上の2つに当たらず、閉じたあとに爪先が 30mm 以上持ち上がっている（人の判定はどれも pick=no） | | 分からない | 上のどれでもない（爪の命令がない・持ち上げていない など） | ### 表（A で測ったずれ: 表との差は中央値 0.3mm・腕が触れない行で最大 4.2mm。実機とのずれは未測定で数十 mm ありうる） | 試行 | 結果 | 分類の候補 | 閉じた瞬間の爪先の高さ mm | ブロックとの水平のずれ mm | 持ち上げたあとの高さ mm（上がった分） | 箱の壁までの最短 mm（部位） | 箱の中に入った | 補正の区間（秒） | |---|---|---|---|---|---|---|---|---| | pi05_20261030_185133 | 失敗 | 分からない | 23.0 | 11.4 | 30.7（+7.7） | 1.1（爪） | 入った（深さ 5.1mm） | 5回 119-191;252-290;364-470;490-521;567-664 | | pi05_20261030_190659 | 失敗 | つかんだが持ち上げで落ちた | 28.0 | 19.9 | 132.1（+104.1） | 49.5（爪） | いいえ | 4回 50-72;115-126;131-179;202-318 | | pi05_20261031_180023 | 失敗 | つかんだが持ち上げで落ちた | 19.5 | 15.7 | 58.7（+39.2） | 45.3（爪） | いいえ | 3回 46-70;86-146;167-210 | | pi05_20261031_183219 | 失敗 | 分からない | 16.8 | 6.8 | 19.1（+2.3） | 79.0（爪） | いいえ | 1回 35-80 | | pi05_20261031_183426 | 失敗 | 分からない | 21.4 | 14.9 | 31.0（+9.6） | 19.0（爪） | いいえ | 5回 18-97;114-161;190-232;251-272;286-319 | | pi05_20261101_191547 | 失敗 | 分からない | 22.4 | 4.7 | 24.9（+2.5） | 69.2（爪） | いいえ | なし | | pi05_20261101_192758 | 失敗 | つかんだが持ち上げで落ちた | 18.3 | 10.5 | 77.2（+58.9） | 61.5（爪） | いいえ | 2回 25-45;76-132 | | pi05_20261101_194042 | 失敗 | つかんだが持ち上げで落ちた | 21.5 | 15.3 | 136.3（+114.8） | 33.6（爪） | いいえ | 1回 52-120 | | pi05_20261106_184312 | 失敗 | 分からない | 21.2 | 26.5 | 36.1（+14.9） | −1.0（爪） | 入った（深さ 4.9mm） | 2回 19-32;51-63 | | pi05_20261106_185050 | 失敗 | つかんだが持ち上げで落ちた | 21.3 | 21.7 | 206.3（+185.0） | 61.4（爪） | いいえ | 2回 18-31;49-65 | | pi05_20261106_185225 | 失敗 | つかんだが持ち上げで落ちた | 17.5 | 30.0 | 170.9（+153.4） | 63.8（爪） | いいえ | 3回 16-25;42-54;66-76 | | pi05_20261106_185415 | 失敗 | つかんだが持ち上げで落ちた | 22.9 | 22.5 | 180.5（+157.6） | 54.4（爪） | いいえ | 2回 17-27;46-64 | | pi05_20261030_184245 | 成功 | （比べる用） | 25.3 | 24.5 | 51.6（+26.3） | 10.4（爪） | いいえ | なし | | pi05_20261031_185246 | 成功 | （比べる用） | — | — | — | 213.4（爪） | いいえ | なし（爪の命令の記録なし） | | pi05_20261030_210954 | 成功 | （比べる用） | 20.5 | 26.4 | 52.2（+31.7） | 10.2（爪） | いいえ | 1回 70-136 | | pi05_20261031_175527 | 成功 | （比べる用） | 20.3 | 39.4 | 56.7（+36.4） | 1.3（爪） | いいえ | 2回 92-120;180-221 | | pi05_20261101_190347 | 成功 | （比べる用） | 23.3 | 10.0 | 58.2（+34.9） | 17.8（爪） | いいえ | 2回 109-137;180-232 | どの数字から決めたかは `B/b_table.csv` の `category_basis` 列。補正が閉じる前にあったか（`correct_before_close`）、爪を閉じる命令の回数（`n_grip_close_cmds`）もそこにある。 ### 読み取れること - **閉じた瞬間の爪先の位置は、失敗も成功もほぼ同じ幅**（高さ 17〜28mm 対 20〜25mm、水平のずれ 5〜30mm 対 10〜39mm）。Unity の解像度（実機とのずれが未測定）では、失敗を「爪を閉じた位置の違い」として見分けられなかった - 失敗の7件は、閉じたあと 39〜185mm 持ち上げている。人の判定はどれも pick=no なので、ブロックが爪から抜けたか、そもそも爪の間に入っていなかった。どちらかは Unity では分けられない（爪の開き具合の実測がない） - 失敗の5件（分からない）は、閉じたあと **2〜15mm しか持ち上げていない**。成功は 26〜36mm 持ち上げてから j0 を回している。「閉じたあと持ち上げない」はこの依頼の分類にない形なので「分からない」に入れた - 箱に 10mm より深く当たった試行はない。2件（185133・184312）は爪が箱の中に 5mm ほど入る（縁から ±1mm） ### 画像（試行ごと `B/images/{run}/`） | ファイル | 中身 | |---|---| | `1_close_side_note.png` | 爪を閉じる命令の瞬間、横から（正射影。マット 0mm・ブロック上面 24mm・箱の縁 43mm（仮）の線と、50mm ごとの目盛り） | | `1_close_front_note.png` | 同じ瞬間、ブロックの先から根元を見る向き | | `2_lift_side_note.png` | 持ち上げたあと、横から | | `3_box_side_note.png`・`3_box_front_note.png` | 箱にいちばん近づいたとき | | `*.png`（`_note` なし） | 字を書き込む前の元の画 | 「横」は根元から見た向き（ブロックまたは箱へ）に直角で、手前（+Z 側）から見ている。B の画だけ、腕が暗くならないよう補助の光を1つ足している（D は足していない）。`B/first_frames/{run}.jpg` は検出の確認用（黄＝マット四隅、赤丸＝ブロック、青＝白箱）。 --- ## C. 次に実機で試す置き場所の候補を確かめる ### 候補 - 格子の9点: Unity の X = 120・160・200mm、Z = 180・220・260mm（`G_x{X}_z{Z}`） - 教えた5か所（`T1`〜`T5`）: `runs/run_pick_place/{試行}/teach/drag_v2_*.json` の爪を閉じた瞬間の角度（`C/teach_pick_angles.csv`）を A の置き方で置いた爪先の位置（`C/teach_pick_fk.csv`） ### 姿勢の選び方（どれも肩 j1 ≤ 88°・表と Unity の差 5mm 以内の行だけ） | 姿勢 | 条件 | |---|---| | つかむ | 爪先の高さ 10〜40mm。水平のずれ ＋ 0.2×|高さ−20mm| が小さい順（20mm は B の成功の閉じた瞬間の高さ 20〜25mm に合わせた目安） | | ブロックの真上 | 高さ 60〜100mm・水平のずれ 15mm 以内のうち、つかむ姿勢との関節の差（j0〜j4 の絶対値の和）が小さい順 | | 持ち上げ | 高さ 80〜130mm・同じく | | 白箱の上 | 白箱の中心（B の検出の中央値 x=−203.8・z=235.9mm）の上、高さ 80〜130mm・水平 15mm 以内のうち、持ち上げ姿勢との関節の差が小さい順 | 道すじ: HOME [0,0,0,0,0,−30] → 真上 → つかむ（開）→ 持ち上げ（閉）→ 白箱の上 を、関節角の直線補間（区間ごとに 40 刻み）で動かして、各刻みで箱の壁・マット・ブロックとの近さを測った。 上から見えるか: D で合わせた上カメラから、つかむ直前（爪は開）を 960×540 で撮り、腕ありと腕なしで赤の画素を数えた比（50% 以上で「見える」）。 ### 表（1点1行。すべての姿勢の角度は `C/c_table.csv`） | 点 | 種類 | x, z mm | つかむ姿勢 [j0〜j5] | 肩 j1（つかむ／最大）° | 爪先の狙いからのずれ mm | 表と Unity の差 mm | 箱の壁までの最短 mm | マットまでの最短 mm | 上から見えるブロック | 判定 | 理由 | |---|---|---|---|---|---|---|---|---|---|---|---| | G_x120_z180 | 格子 | 120, 180 | — | — | — | — | — | — | — | 使えない | 表の中で、つかむ・真上・持ち上げを選べなかった（候補の行は、角度を置き直すと腕がマットの下 10mm に入る＝表を作ったときに床に当たって止まった値） | | G_x120_z220 | 格子 | 120, 220 | [−38.6, 77.4, 15.0, −14.9, −84.2, −29.4] | 77.4／77.4 | 1.2 | 0.3 | 56.2 | 7.1 | 100% | 使えそう | | | G_x120_z260 | 格子 | 120, 260 | [−24.5, 85.5, −10.0, 1.4, −70.5, −32.9] | 85.5／85.5 | 2.9 | 0.5 | 31.2 | 7.1 | 100% | 使えそう | | | G_x160_z180 | 格子 | 160, 180 | [−42.2, 69.9, 25.0, 0.1, −79.2, −29.4] | 69.9／69.9 | 0.5 | 0.3 | 52.2 | −3.5 | 100% | 要注意 | つかむ姿勢で腕がマットの下 3.5mm（±10mm 以内なので当たるとは決めない） | | G_x160_z220 | 格子 | 160, 220 | [−31.1, 77.4, 5.0, 10.1, −74.2, −29.4] | 77.4／77.4 | 0.5 | 0.3 | 58.4 | 8.1 | 100% | 使えそう | | | G_x160_z260 | 格子 | 160, 260 | [−30.0, 87.7, −19.5, 3.6, −70.1, −30.0] | 87.7／87.7 | 2.6 | 0.6 | 26.6 | 19.7 | 99% | 使えそう | | | G_x200_z180 | 格子 | 200, 180 | [−56.6, 77.4, 5.0, −14.9, −74.2, −29.4] | 77.4／77.4 | 1.8 | 0.3 | 56.9 | 7.6 | 100% | 使えそう | | | G_x200_z220 | 格子 | 200, 220 | [−42.5, 88.0, −15.0, 1.4, −70.5, −32.9] | 88.0／88.0 | 3.4 | 4.2 | 20.8 | 12.1 | 100% | 使えそう | | | G_x200_z260 | 格子 | 200, 260 | [−31.9, 87.7, −38.3, 18.6, −45.1, −30.0] | 87.7／87.7 | 2.4 | 4.4 | 20.5 | 22.2 | 97% | 使えそう | | | T1 | 教えた位置 | 161.6, 212.8 | [−46.1, 72.4, 10.0, −14.9, −74.2, −29.4] | 72.4／72.4 | 0.9 | 0.3 | 56.5 | 16.1 | 100% | 使えそう | | | T2 | 教えた位置 | 195.0, 260.5 | [−38.0, 87.0, −33.0, −4.0, −47.0, −32.0] | 87.0／87.7 | 3.9 | 4.2 | 34.3 | 16.4 | 100% | 使えそう | | | T3 | 教えた位置 | 104.5, 247.4 | [−31.6, 77.4, 5.0, −14.9, −74.2, −29.4] | 77.4／77.4 | 1.9 | 0.3 | 56.1 | 7.6 | 100% | 使えそう | | | T4 | 教えた位置 | 238.7, 127.2 | [−54.2, 77.4, 5.0, 15.1, −74.2, −29.4] | 77.4／77.4 | 1.0 | 0.3 | 39.7 | 7.5 | 100% | 使えそう | | | T5 | 教えた位置 | −70.0, 253.0 | [14.4, 76.7, 10.3, −1.4, −78.0, −29.5] | 76.7／76.7 | 2.1 | 0.4 | 36.5 | 6.4 | 93% | 使えそう | | - 「判定」の線（この依頼で置いた目安。エイイチが決め直してよい）: 使えない＝表で選べない・箱に 10mm より深く当たる・マットの下に 10mm より深く入る・上から 50% 以上隠れる・つかむ姿勢の爪先が狙いから 10mm 超離れる。要注意＝箱やマットから ±10mm 以内 - マットまでの最短が 6〜8mm の点は、つかむ姿勢で爪先がマットに近づいているだけ（24mm のブロックをつかむので当たり前） - 箱に当たる点はなかった。どの点も箱にいちばん近いのは最後の「白箱の上」の姿勢で、運んでいるブロックは描いていない - **T5（教えた位置5）は、つかむ直前には上から 93% 見える**。計画書の「位置5は上カメラから見てブロックが腕の陰にほぼ隠れる」は、HOME の最初のコマのこと（B の最初のコマでもブロックは爪の真下に半分隠れていた）と思われる - 肩が 88° ちょうど（G_x200_z220）や 87.7° の点は、実機で命令が捨てられる境目（約 88°）に近い 画像（1点1枚）: `C/c_{点}.png`（左: 上カメラから見たつかむ直前、中: 横から見たつかむ姿勢、右: 横から見た箱にいちばん近いとき）。元の画は `C/images/`。 --- ## D. Unity と実機の画を並べる ### 合わせ方 1. 実機の1コマ: `pi05_20261101_191547`（補正なしの失敗）の `top.mp4` の最初のコマ（アームが HOME、ブロックは爪の真下・白箱は右） 2. マットの四隅を本線の `mat_corner_detection.detect_corners` で検出（(326,0)・(1698,0)・(1704,935)・(305,922) px） 3. 画素は正方形・光軸は画像の中心・レンズのゆがみなしと仮定。**カメラがほぼ真下を向いているため、ホモグラフィ（四隅）だけでは焦点距離が決まらない**（ホモグラフィから解くと 22,534px という外れた値になった） 4. **上の2隅は画像の上の端で切れていて本当の角ではない**（v=0）。そこで次の 10 個の式で 9 個（焦点距離・位置3・向き3・上の2点の奥行き2）を最小二乗で解いた（`twin_camera_d.py`） - 下の2隅（本当の角）を (±310, 361) mm に - 上の2点を「マットの左右の縁 x=±310mm の上のどこか」に - HOME の爪先の青い印2つ（実機の画像で検出）を、Unity の HOME の左右の爪先（tip Brep の位置、高さ 235mm・左右の間 59mm）に 5. Unity のカメラにその位置・向き・縦の画角を入れ、シーンの光のまま（色・光・模様は調整しない）1920×1080 で撮り、学習と同じ 384×216 に縮めて並べた ### 結果 | 項目 | 値 | |---|---| | 焦点距離 | 1,457px（縦の画角 40.7°・横 66.7°。iPhone の広角として自然な値） | | カメラの位置（Unity の m） | x=−0.003・y=0.652・z=0.205（根元のほぼ真上から 20cm 手前、高さ 65cm） | | 向き | ほぼ真下（前向きの方向 (0.030, −0.999, −0.026)） | | 合わせた6点の再投影の誤差 | 最大 3.2px（下の2隅 3.1・3.2px、上の2点 2.8・1.0px、爪先の印 1.9・0.9px） | | 画像の上の端が当たるマットの位置 | z≈−58・−54mm（本線は −89mm と決めている） | | 残ったずれ（白箱の中心） | Unity が実機より右に 21.6px・上に 14.5px（およそ 12mm）。箱は B の位置（本線のやり方のホモグラフィ）に置いたので、上の z のずれが入っている | - 爪先の印の誤差が小さいのは、焦点距離をそこに合わせたから。独立した確かめは白箱だけ - 焦点距離は、Unity の爪の開き（左右の間 59mm）が実機と同じという前提に乗っている ### 画像 | ファイル | 中身 | |---|---| | **`D/d_side_by_side_384x216.png`** | 左: 実機・右: Unity（どちらも 384×216） | | `D/d_side_by_side_960x540.png` | 同じものを大きく | | `D/d_overlay_960x540.png` | 実機と Unity を半分ずつ重ねたもの（橙＝マット四隅、青＝爪先の印） | | `D/real_home_*.{jpg,png}`・`D/unity_home_*.png` | 元の画 | | `D/camera_fit.json` | 合わせたカメラの値すべて | | `D/residuals.json` | 残ったずれ | 素の見え方の違い: Unity のマット（0.8m 角の板）は実機のマットより広く、方眼の模様がない。光は上からの1灯で、影が濃い。机・配線・爪の青い印はない。 --- ## 使ったコード Unity（`Robotics_UX_New/Assets/TwinCheck/Editor/`、このフォルダの `code/` に写し） | ファイル | 役目 | |---|---| | `TwinArm.cs` | 1体のアームに関節角を直接置く（ドライブ・物理は使わない）・爪の開閉・爪先の位置 | | `TwinGeom.cs` | 腕のメッシュと箱の壁・ブロックの距離、箱の中に入ったか、腕のいちばん低い所 | | `TwinScene.cs` | シーンの写しを開き、1体目のアーム・ブロック・箱・床・光だけを見せる。RenderTexture で撮って PNG に書く | | `TwinCsv.cs` | CSV を読む | | `TwinCheckA.cs` | A（表の行の角度を置いて爪先を書き出す） | | `TwinCheckB.cs` | B の測定（`Metrics`）と、B・C・D の撮影（`Render`） | | `TwinCheckC.cs` | C の道すじの確かめと撮る場面 | | `TwinCheckD.cs` | D の手がかり（HOME の爪の点の 3D 位置） | | `TwinCheckDump.cs` | 下調べ（プレハブ・シーンの中身を書き出す） | Python（`Robotics_UX_New/Assets/TwinCheck/Python~/`、同じく `code/` に写し。`~` で終わるフォルダは Unity が読み込まない） | ファイル | 役目 | |---|---| | `twin_a.py` | A のまとめ | | `twin_prep.py` | B の下ごしらえ（試行の選び出し・時刻の並び・ブロックと箱の検出）。本線の検出の関数を読み取り専用で借りる | | `twin_table_b.py` | B の表・分類の候補・撮る場面 | | `twin_overlay.py` | B の画に字と目盛りを書く | | `twin_c.py` | C の姿勢の選び出し・表・画像 | | `twin_camera_d.py` | D のカメラ合わせ | | `twin_d.py` | D の撮る場面と並べた画 | ### シーンの扱い（2026-11-07 変更・依頼111） - 作業用のシーンの写しは記録（git・Unity Version Control）に入れない形にした。元のシーン `Assets/Scenes/R2R2R-20260224.unity` は読むだけで、開かない・保存しない - 依頼107 で使った写しは、このフォルダの **`code/R2R2R-20260224_cs110.unity`** に残した。Unity Version Control の **cs:110**（2026-07-17）の元のシーンと同じ中身（MD5 `b1613505dfd781275874030ce3da23d6`） - 動かすたびに `TwinCheckA.OpenWorkScene` が元のシーンを `Assets/TwinCheck/_work/TwinCheck_Base.unity` に写して開く（Unity は Assets の外のシーンを開けないため）。`TwinScene.Open`・`TwinCheckA.Run`・`TwinCheckDump.RunScene` はみなこれを通る - 写すたびに、出力フォルダ（`-twinOut`、なければ `-twinShots` のあるフォルダ）に `scene_source.json`（元のパス・MD5・changeset）を書く。changeset は Unity Version Control で読み込んでいる版の中身と MD5 が一致したときだけ書き、元のシーンにチェックインしていない変更があれば `null` になる - `.gitignore`・`ignore.conf` に `_work/`・`_work.meta`・`Python~/__pycache__/` を足した - やり直し方のコマンドは変わらない。D（`TwinCheckD.Points` → `twin_camera_d.py`）を一時フォルダでやり直し、`D/home_points_unity.json`・`D/camera_fit.json` と差の最大 0 を確かめた（このフォルダの結果は上書きしていない） - `code/` の `.cs` は依頼107 の時点のもの。この変更のあとのコードは git `736b3d3`・Unity Version Control cs:146 ## やり直し方 Unity の Editor でこのプロジェクトを開いていないことを確かめてから、`Robotics_UX_New` で: ```bash U=/Applications/Unity/Hub/Editor/2022.3.62f3/Unity.app/Contents/MacOS/Unity O="/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所" P="Assets/TwinCheck/Python~" run() { $U -batchmode -quit -projectPath "$PWD" -logFile /tmp/twin.log -executeMethod "$@"; } # A run TwinCheckA.Run -twinIn "$O/A/a_sample_rows.csv" -twinOut "$O/A/a_unity_tip_20.csv" -twinSign 1 python3 $P/twin_a.py "$O/A/a_sample_rows.csv" "$O/A/a_unity_tip_20.csv" "$O/A/a_result_20rows.csv" # B python3 $P/twin_prep.py run TwinCheckB.Metrics -twinTrials "$O/B/trials.csv" -twinTimeline "$O/B/timeline.csv" -twinOut "$O/B/timeline_metrics.csv" # 約3分 python3 $P/twin_table_b.py run TwinCheckB.Render -twinShots "$O/B/shots_b.csv" python3 $P/twin_overlay.py # D（C の上カメラに使うので C より先） run TwinCheckD.Points -twinOut "$O/D/home_points_unity.json" python3 $P/twin_camera_d.py python3 $P/twin_d.py shots run TwinCheckB.Render -twinShots "$O/D/shots_d.csv" python3 $P/twin_d.py compose # C run TwinCheckA.Run -twinIn "$O/C/teach_pick_angles.csv" -twinOut "$O/C/teach_pick_fk.csv" python3 $P/twin_c.py select run TwinCheckA.Run -twinIn "$O/C/c_candidates.csv" -twinOut "$O/C/c_candidates_fk.csv" python3 $P/twin_c.py finalize python3 -c "import json;d=json.load(open('$O/D/camera_fit.json'));p=d['cam_pos_unity_m'];q=d['cam_quat_unity_xyzw'];open('$O/C/cam_for_c.csv','w').write('cam_x,cam_y,cam_z,cam_qx,cam_qy,cam_qz,cam_qw,cam_fov\n'+','.join(f'{v:.6f}' for v in p+q+[d['fov_vertical_deg']])+'\n')" run TwinCheckC.Path -twinPoses "$O/C/c_poses.csv" -twinOut "$O/C/c_path.csv" -twinShots "$O/C/c_shots.csv" -twinCam "$O/C/cam_for_c.csv" -twinImgDir "$O/C/images" # 約3分 run TwinCheckB.Render -twinShots "$O/C/c_shots.csv" python3 $P/twin_c.py table ``` `teach_pick_angles.csv` は README の C 節の5つの教えた記録から、`gripper_events` の close の idx の角度を取り出したもの。 作業用のシーン `Assets/TwinCheck/TwinCheck_Base.unity` は `Assets/Scenes/R2R2R-20260224.unity`（2026-07-17 の書きかけの状態）のファイルの写し。開くだけで保存しない。 ## 気をつけること・分かっていないこと - Unity と実機の「角度 → 位置」のずれは測っていない。B の数 mm〜十数 mm の差では決めていない - 白箱は 80mm 角・高さ 43mm（仮）・まっすぐ置いた箱としている。実物の寸法は測っていない - ブロックの位置は本線のホモグラフィ（上の端を −89mm とする決め）で出しているので、D の結果からは z が約 9mm 手前にずれている見込み - 爪の開き具合は Unity の `GripperController` と同じ 0°（開）／40°（閉）で描いている。実機の開き具合の記録は使っていない - 表（`fk_scan_motion_based_v2.csv`）には、物理で何かに当たって止まった値が混ざっている疑いがある（200行のうち、角度どおりなら腕が何かに触れる行が 63 行・約 3 割、10mm 超ずれる行が 54 行）。表だけで姿勢を選ぶと、角度どおりなら床の下に入る姿勢を拾うおそれがある ## 決めたこと（エイイチ 2026-11-07） - A の大きなずれ：表（`fk_scan_motion_based_v2.csv`）には物理で床などに当たって止まった値が混ざっている、という見方でよい。表だけで姿勢を選ぶときは、Unity で角度を置き直して確かめた行だけを使う - C の「使えそう」の目安（上から 50% 以上見える・箱やマットから ±10mm 以内は要注意・つかむ姿勢の爪先が狙いから 10mm 以内）は、このまま使う - 上カメラの上の端のずれ（本線は −89mm、D では −55mm 前後）は調べる（依頼110 の F） `````` ### Robotics_UX_New commit 157516d（TwinCheck の Unity と Python のコード） ``````diff commit 157516d5095f4189d11c9f5d027d9c95a8609c06 Author: Shimazue Eiichi Date: Sat Nov 7 13:08:00 2026 +0900 [依頼107] デジタルツインで π0.5 の失敗の試行を再生し、置き場所の候補を確かめるコードを追加 Assets/TwinCheck/ の下に、実機の推論の記録の関節角を Unity のアームに直接置いて （ドライブ・物理は使わない）横から見る・箱との近さを測る・置き場所の候補の道すじを確かめる・ 上カメラを実機に合わせて並べる、Editor スクリプトと Python を置いた。 結果と README は KIOXIA の lab/twin/20261107_失敗の再生と置き場所/。 Co-Authored-By: Claude Opus 5.5 diff --git a/Assets/TwinCheck.meta b/Assets/TwinCheck.meta new file mode 100644 index 0000000..5f44e43 --- /dev/null +++ b/Assets/TwinCheck.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0bd33ee5bdb72451fab414dd9508c103 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor.meta b/Assets/TwinCheck/Editor.meta new file mode 100644 index 0000000..23d5cb1 --- /dev/null +++ b/Assets/TwinCheck/Editor.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 599790675b60e4368b067d6a2b8632e3 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinArm.cs b/Assets/TwinCheck/Editor/TwinArm.cs new file mode 100644 index 0000000..b057472 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinArm.cs @@ -0,0 +1,98 @@ +using UnityEngine; +using System.Collections.Generic; + +/// +/// TwinArm +/// 依頼107。シーンの写しの中の mechArm 1体に、関節角（度）を直接置く。 +/// ArticulationBody のドライブ（stiffness / damping / target）には一切触れず、 +/// 物理も回さない。親のアンカー → X軸まわりの回転 → 子のアンカーの逆、の順で +/// 各リンクの Transform を置く（ArticulationBody の回転関節と同じ式）。 +/// 爪先は FKScanner_MotionBased と同じく左右の tip Brep の中点、根元からの差で返す。 +/// +public class TwinArm +{ + public readonly GameObject root; + public readonly ArticulationBody baseBody; + public readonly ArticulationBody[] joints; // link1〜link6 + public readonly Transform leftTip, rightTip; + readonly Transform[] fingers = new Transform[6]; // left3, left2, left1, right3, right2, right1 + readonly Quaternion[] fingerInit = new Quaternion[6]; + public float sign = 1f; + + public TwinArm(GameObject go) + { + root = go; + var list = new List(); + foreach (var ab in go.GetComponentsInChildren(true)) + { + if (ab.isRoot) baseBody = ab; else list.Add(ab); + } + joints = list.GetRange(0, Mathf.Min(6, list.Count)).ToArray(); + leftTip = FindTip(go.transform, "left"); + rightTip = FindTip(go.transform, "right"); + string[] names = { "gripper_left3", "gripper_left2", "gripper_left1", "gripper_right3", "gripper_right2", "gripper_right1" }; + for (int i = 0; i < 6; i++) + { + fingers[i] = FindDeep(go.transform, names[i]); + if (fingers[i] != null) fingerInit[i] = fingers[i].localRotation; + } + } + + /// 関節角（度、j0〜j5）を置く。 + public void SetAngles(float[] deg) + { + for (int i = 0; i < joints.Length && i < deg.Length; i++) + { + var c = joints[i]; + var p = c.transform.parent; + Quaternion pr = p.rotation; + Vector3 jointPos = p.TransformPoint(c.parentAnchorPosition); + Quaternion jointRot = pr * c.parentAnchorRotation * Quaternion.AngleAxis(sign * deg[i], Vector3.right); + Quaternion childRot = jointRot * Quaternion.Inverse(c.anchorRotation); + Vector3 childPos = jointPos - childRot * Vector3.Scale(c.transform.lossyScale, c.anchorPosition); + c.transform.SetPositionAndRotation(childPos, childRot); + } + } + + /// 爪の開き。0 = 開き（GripperController の初期）、40 = 閉じ（GripperController.Close）。 + public void SetGrip(float a) + { + float[] s = { a, a, -a, -a, -a, a }; + for (int i = 0; i < 6; i++) + if (fingers[i] != null) fingers[i].localRotation = fingerInit[i] * Quaternion.Euler(0, s[i], 0); + } + + /// 爪先（左右 tip Brep の中点）の、根元からの位置（m、Unity 座標）。 + public Vector3 TipLocal() + { + Vector3 mid = (leftTip.position + rightTip.position) * 0.5f; + return mid - baseBody.transform.position; + } + + /// 腕の描画だけを切り替える（GameObject は消さない）。 + public void SetVisible(bool on) + { + foreach (var r in root.GetComponentsInChildren(true)) r.enabled = on; + } + + public Vector3 TipWorld() => (leftTip.position + rightTip.position) * 0.5f; + + static Transform FindTip(Transform root, string side) + { + var g = FindDeep(root, $"gripper_{side}1"); + if (g == null) return null; + var i = FindDeep(g, "Instance-Def-210103_4302_101"); + return i == null ? null : i.Find("Brep"); + } + + public static Transform FindDeep(Transform parent, string name) + { + foreach (Transform child in parent) + { + if (child.name == name) return child; + var r = FindDeep(child, name); + if (r != null) return r; + } + return null; + } +} diff --git a/Assets/TwinCheck/Editor/TwinArm.cs.meta b/Assets/TwinCheck/Editor/TwinArm.cs.meta new file mode 100644 index 0000000..97b4daf --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinArm.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 3f2c3f128c0034837ab674dc5ea05b33 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinCheckA.cs b/Assets/TwinCheck/Editor/TwinCheckA.cs new file mode 100644 index 0000000..0b4d62d --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckA.cs @@ -0,0 +1,69 @@ +using UnityEngine; +using UnityEditor; +using UnityEditor.SceneManagement; +using System.Collections.Generic; +using System.IO; +using System.Text; +using System.Globalization; + +/// +/// TwinCheckA +/// 依頼107 の A。FK テーブルから選んだ行の関節角を、シーンの写しの20体それぞれに置き、 +/// 爪先の位置（根元からの差）を書き出す。表の ee との比較は Python 側で行う。 +/// シーンは開くだけで保存しない。 +/// 実行: Unity -batchmode -quit -executeMethod TwinCheckA.Run -twinIn {行の CSV} -twinOut {出力 CSV} [-twinSign 1|-1] +/// +public static class TwinCheckA +{ + public const string ScenePath = "Assets/TwinCheck/TwinCheck_Base.unity"; + + public static void Run() + { + string inPath = TwinCheckArgs.Get("-twinIn", ""); + string outPath = TwinCheckArgs.Get("-twinOut", ""); + float sign = float.Parse(TwinCheckArgs.Get("-twinSign", "1"), CultureInfo.InvariantCulture); + + EditorSceneManager.OpenScene(ScenePath, OpenSceneMode.Single); + var arms = FindArms(); + + var lines = File.ReadAllLines(inPath); + var sb = new StringBuilder(); + sb.AppendLine("row_index,robot,tip_x,tip_y,tip_z,arm_min_y,near_box_mm,near_block_mm"); + for (int li = 1; li < lines.Length; li++) + { + var c = lines[li].Split(','); + if (c.Length < 10) continue; + var deg = new float[6]; + for (int k = 0; k < 6; k++) deg[k] = float.Parse(c[4 + k], CultureInfo.InvariantCulture); + for (int r = 0; r < arms.Count; r++) + { + arms[r].sign = sign; + arms[r].SetAngles(deg); + var t = arms[r].TipLocal(); + string extra = ""; + if (r == 0) + { + // 1体目だけ: 腕（link1〜）の見た目の一番低い所と、箱・ブロックへの近さ（コライダの外形どうしの距離） + var box = GameObject.Find("WhiteBox_arm1"); + var block = GameObject.Find("RedBlock_arm1"); + extra = string.Format(CultureInfo.InvariantCulture, ",{0:F6},{1:F1},{2:F1}", + TwinGeom.ArmMinY(arms[r]), TwinGeom.ArmToObjectMm(arms[r], box), TwinGeom.ArmToObjectMm(arms[r], block)); + } + sb.AppendLine(string.Format(CultureInfo.InvariantCulture, "{0},{1},{2:F6},{3:F6},{4:F6}", c[0], arms[r].root.name, t.x, t.y, t.z) + extra); + } + } + File.WriteAllText(outPath, sb.ToString()); + Debug.Log($"[TwinCheckA] wrote {outPath} arms={arms.Count}"); + } + + public static List FindArms() + { + var arms = new List(); + for (int r = 1; r <= 20; r++) + { + var go = GameObject.Find($"mecharm270mpi_{r}_0"); + if (go != null) arms.Add(new TwinArm(go)); + } + return arms; + } +} diff --git a/Assets/TwinCheck/Editor/TwinCheckA.cs.meta b/Assets/TwinCheck/Editor/TwinCheckA.cs.meta new file mode 100644 index 0000000..04dc29e --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckA.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 722a2ff69071f4426ad2c32f595edfd3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinCheckB.cs b/Assets/TwinCheck/Editor/TwinCheckB.cs new file mode 100644 index 0000000..46ac92e --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckB.cs @@ -0,0 +1,177 @@ +using UnityEngine; +using System.Collections.Generic; +using System.IO; +using System.Text; + +/// +/// TwinCheckB +/// 依頼107 の B。実機の推論の記録（実際に届いた関節角）を Unity のアームに置いて測り、撮る。 +/// 角度は TwinArm.SetAngles で直接置く（ドライブの値は変えない・物理は回さない）。 +/// +/// Metrics: trials.csv（ブロック・箱の位置）と timeline.csv（時刻ごとの j0〜j5・爪）から、 +/// 時刻ごとの爪先の位置・ブロックとの水平のずれ・箱の壁との近さを書く。 +/// 実行: Unity -batchmode -quit -executeMethod TwinCheckB.Metrics -twinTrials {trials.csv} -twinTimeline {timeline.csv} -twinOut {出力 CSV} +/// Render: shots.csv の1行ごとに姿勢・物の位置・カメラを決めて PNG を撮る。 +/// 実行: Unity -batchmode -quit -executeMethod TwinCheckB.Render -twinShots {shots.csv} +/// +public static class TwinCheckB +{ + public static void Metrics() + { + var trials = TwinCsv.Read(TwinCheckArgs.Get("-twinTrials", "")); + var timeline = TwinCsv.Read(TwinCheckArgs.Get("-twinTimeline", "")); + string outPath = TwinCheckArgs.Get("-twinOut", ""); + var sc = TwinScene.Open(); + + var byRun = new Dictionary>>(); + foreach (var r in timeline) + { + if (!byRun.TryGetValue(r["run"], out var l)) byRun[r["run"]] = l = new List>(); + l.Add(r); + } + + var sb = new StringBuilder(); + sb.AppendLine("run,i,t,grip,stage,tip_x,tip_y,tip_z,tip_h_mm,tip_block_xz_mm,d_block_grip_mm," + + "d_box_tip_mm,d_box_gripper_mm,d_box_forearm_mm,in_box_tip,in_box_gripper,in_box_forearm,in_box_depth_mm"); + var sw = System.Diagnostics.Stopwatch.StartNew(); + int n = 0; + foreach (var tr in trials) + { + if (!byRun.TryGetValue(tr["run"], out var rows)) continue; + sc.Place(TwinScene.F(tr["block_x_m"]), TwinScene.F(tr["block_z_m"]), Yaw(tr, "block_yaw_deg"), + TwinScene.F(tr["box_x_m"]), TwinScene.F(tr["box_z_m"]), Yaw(tr, "box_yaw_deg")); + Vector3 bc = sc.block.transform.position; + Vector3 xc = sc.box.transform.position; + foreach (var r in rows) + { + var deg = new float[6]; + for (int k = 0; k < 6; k++) deg[k] = TwinScene.F(r["j" + k]); + int grip = int.Parse(r["grip"]); + sc.arm.SetAngles(deg); + sc.arm.SetGrip(grip == 1 ? 40f : 0f); + Vector3 tip = sc.arm.TipWorld(); + float hBlock = new Vector2(tip.x - bc.x, tip.z - bc.z).magnitude * 1000f; + + // 箱の近くにいるときだけ、部位ごとの距離を細かく測る（遠いときは粗い値で足りる） + float far = new Vector2(tip.x - xc.x, tip.z - xc.z).magnitude; + string boxCols; + if (far < 0.22f) + { + float dT = TwinGeom.NearestMm(sc.arm, sc.box, TwinGeom.Part.Tip, out _); + float dG = TwinGeom.NearestMm(sc.arm, sc.box, TwinGeom.Part.Gripper, out _); + float dF = TwinGeom.NearestMm(sc.arm, sc.box, TwinGeom.Part.Forearm, out _); + int iT = TwinGeom.InsideBox(sc.arm, sc.box, TwinGeom.Part.Tip, out float d1); + int iG = TwinGeom.InsideBox(sc.arm, sc.box, TwinGeom.Part.Gripper, out float d2); + int iF = TwinGeom.InsideBox(sc.arm, sc.box, TwinGeom.Part.Forearm, out float d3); + boxCols = $"{dT:F1},{dG:F1},{dF:F1},{iT},{iG},{iF},{Mathf.Max(d1, Mathf.Max(d2, d3)):F1}"; + } + else boxCols = ",,,,,,"; + float dBlk = hBlock < 120f + ? Mathf.Min(TwinGeom.NearestMm(sc.arm, sc.block, TwinGeom.Part.Tip, out _), TwinGeom.NearestMm(sc.arm, sc.block, TwinGeom.Part.Gripper, out _)) + : float.NaN; + sb.AppendLine($"{r["run"]},{r["i"]},{r["t"]},{grip},{r["stage"]},{TwinScene.S(tip.x)},{TwinScene.S(tip.y)},{TwinScene.S(tip.z)}," + + $"{tip.y * 1000f:F1},{hBlock:F1},{(float.IsNaN(dBlk) ? "" : dBlk.ToString("F1"))},{boxCols}"); + n++; + } + } + File.WriteAllText(outPath, sb.ToString()); + Debug.Log($"[TwinCheckB] metrics rows={n} sec={sw.Elapsed.TotalSeconds:F1} -> {outPath}"); + } + + static float Yaw(Dictionary tr, string key) => + tr.TryGetValue(key, out var v) && !string.IsNullOrEmpty(v) ? TwinScene.F(v) : 0f; + + /// + /// shots.csv の列: out,j0..j5,grip,block_x,block_z,block_yaw,box_x,box_z,box_yaw,view,target_x,target_z,w,h + /// view = side（基準点を通る、根元からの向きに直角な水平の視線・正射影 0.24m 高）/ front（根元からの向きの先から根元を見る） + /// / top（真上からの仮の上カメラ）/ cam（cam_* 列のカメラ位置・向き・画角をそのまま使う） + /// + // 横から見た画の範囲（正射影）。twin_overlay.py の目盛りもこの値を使う + public const float SideCenterY = 0.10f; + public const float SideHalfHeight = 0.15f; + + public static void Render() + { + var shots = TwinCsv.Read(TwinCheckArgs.Get("-twinShots", "")); + var sc = TwinScene.Open(); + var camGo = new GameObject("TwinCheckCam"); + var cam = camGo.AddComponent(); + cam.clearFlags = CameraClearFlags.SolidColor; + cam.backgroundColor = new Color(0.82f, 0.84f, 0.88f); + cam.nearClipPlane = 0.01f; + cam.farClipPlane = 10f; + var main = GameObject.Find("Main Camera"); + if (main) main.SetActive(false); + + // B の確かめ用の補助の光（view=cam / top のときは消して、シーンの素の光だけで撮る） + var fill = new GameObject("TwinCheckFill").AddComponent(); + fill.type = LightType.Directional; + fill.intensity = 0.6f; + fill.shadows = LightShadows.None; + + foreach (var s in shots) + { + // 腕を隠すときは描画だけを切る（SetActive で消して戻すと、ArticulationBody がリンクの位置を戻してしまうため） + sc.arm.SetVisible(s.TryGetValue("hide_arm", out var ha) ? ha != "1" : true); + var deg = new float[6]; + for (int k = 0; k < 6; k++) deg[k] = TwinScene.F(s["j" + k]); + sc.arm.SetAngles(deg); + sc.arm.SetGrip(s["grip"] == "1" ? 40f : 0f); + sc.Place(TwinScene.F(s["block_x"]), TwinScene.F(s["block_z"]), TwinScene.F(s["block_yaw"]), + TwinScene.F(s["box_x"]), TwinScene.F(s["box_z"]), TwinScene.F(s["box_yaw"])); + sc.block.SetActive(s.TryGetValue("hide_block", out var hb) ? hb != "1" : true); + int w = int.Parse(s["w"]), h = int.Parse(s["h"]); + var tgt = new Vector3(TwinScene.F(s["target_x"]), 0f, TwinScene.F(s["target_z"])); + Vector3 dir = new Vector3(tgt.x, 0, tgt.z).normalized; + if (dir.sqrMagnitude < 1e-6f) dir = Vector3.forward; + string view = s["view"]; + fill.enabled = view == "side" || view == "front"; + cam.ResetProjectionMatrix(); + cam.aspect = (float)w / h; + if (view == "side") + { + Vector3 perp = new Vector3(dir.z, 0, -dir.x); // 根元からの向きに直角 + if (perp.z < 0) perp = -perp; // 手前（+Z 側）から見る + cam.orthographic = true; + cam.orthographicSize = SideHalfHeight; + cam.transform.position = tgt + perp * 0.8f + Vector3.up * SideCenterY; + cam.transform.rotation = Quaternion.LookRotation(-perp, Vector3.up); + fill.transform.rotation = Quaternion.LookRotation(-perp + Vector3.down * 0.3f, Vector3.up); + } + else if (view == "front") + { + cam.orthographic = false; + cam.fieldOfView = 40f; + cam.transform.position = tgt + dir * 0.45f + Vector3.up * 0.12f; + cam.transform.LookAt(new Vector3(tgt.x * 0.5f, 0.06f, tgt.z * 0.5f)); + fill.transform.rotation = cam.transform.rotation; + } + else if (view == "top") + { + cam.orthographic = false; + cam.fieldOfView = float.Parse(s.TryGetValue("cam_fov", out var fv) && fv != "" ? fv : "60", System.Globalization.CultureInfo.InvariantCulture); + cam.transform.position = new Vector3(0f, TwinScene.F(s.TryGetValue("cam_h", out var ch) && ch != "" ? ch : "0.75"), 0.136f); + cam.transform.rotation = Quaternion.LookRotation(Vector3.down, Vector3.back); + } + else // cam + { + cam.orthographic = false; + cam.transform.position = new Vector3(TwinScene.F(s["cam_x"]), TwinScene.F(s["cam_y"]), TwinScene.F(s["cam_z"])); + cam.transform.rotation = new Quaternion(TwinScene.F(s["cam_qx"]), TwinScene.F(s["cam_qy"]), TwinScene.F(s["cam_qz"]), TwinScene.F(s["cam_qw"])); + cam.fieldOfView = TwinScene.F(s["cam_fov"]); + if (s.TryGetValue("cam_cx", out var cx) && cx != "") + { + // 画像の中心のずれ（主点）も合わせるときは、投影行列を作り直す + float fy = 0.5f * h / Mathf.Tan(cam.fieldOfView * 0.5f * Mathf.Deg2Rad); + float cxp = TwinScene.F(cx), cyp = TwinScene.F(s["cam_cy"]); + float n = cam.nearClipPlane, f = cam.farClipPlane; + float l = -cxp * n / fy, r = (w - cxp) * n / fy; + float t = cyp * n / fy, b = -(h - cyp) * n / fy; + cam.projectionMatrix = Matrix4x4.Frustum(l, r, b, t, n, f); + } + } + TwinScene.Shoot(cam, w, h, s["out"]); + } + Debug.Log($"[TwinCheckB] rendered {shots.Count}"); + } +} diff --git a/Assets/TwinCheck/Editor/TwinCheckB.cs.meta b/Assets/TwinCheck/Editor/TwinCheckB.cs.meta new file mode 100644 index 0000000..a5ef4b3 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckB.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: fd0a190ce8e4e47b29a1af06f47477cd +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinCheckC.cs b/Assets/TwinCheck/Editor/TwinCheckC.cs new file mode 100644 index 0000000..b0c8ee1 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckC.cs @@ -0,0 +1,118 @@ +using UnityEngine; +using System.Collections.Generic; +using System.Globalization; +using System.IO; +using System.Text; + +/// +/// TwinCheckC +/// 依頼107 の C。置き場所の候補ごとに、HOME → ブロックの真上 → つかむ（開）→ つかむ（閉）→ 持ち上げ → 白箱の上 +/// を関節角の直線補間（区間ごとに 40 刻み）で動かし、箱の壁・床・ブロックとの近さを測る。 +/// 角度は TwinArm.SetAngles で直接置く（ドライブの値は変えない・物理は回さない）。運んでいるブロックは描かない。 +/// 実行: Unity -batchmode -quit -executeMethod TwinCheckC.Path -twinPoses {c_poses.csv} -twinOut {c_path.csv} -twinShots {c_shots.csv} -twinCam {camera_fit.json の値を並べた CSV} +/// +public static class TwinCheckC +{ + static readonly float[] Home = { 0, 0, 0, 0, 0, -30 }; + const int Steps = 40; + + public static void Path() + { + var poses = TwinCsv.Read(TwinCheckArgs.Get("-twinPoses", "")); + string outPath = TwinCheckArgs.Get("-twinOut", ""); + string shotsPath = TwinCheckArgs.Get("-twinShots", ""); + var cam = TwinCsv.Read(TwinCheckArgs.Get("-twinCam", ""))[0]; + string imgDir = TwinCheckArgs.Get("-twinImgDir", ""); + var sc = TwinScene.Open(); + + var byPoint = new Dictionary>>(); + var order = new List(); + foreach (var p in poses) + { + if (!byPoint.ContainsKey(p["point"])) { byPoint[p["point"]] = new Dictionary>(); order.Add(p["point"]); } + byPoint[p["point"]][p["phase"]] = p; + } + + var sb = new StringBuilder("point,complete,box_min_mm,box_min_part,box_min_segment,box_inside,box_inside_depth_mm,floor_min_mm,floor_min_segment," + + "block_min_before_grasp_mm,block_min_part\n"); + var shots = new StringBuilder("out,j0,j1,j2,j3,j4,j5,grip,block_x,block_z,block_yaw,box_x,box_z,box_yaw,view,target_x,target_z,w,h,hide_arm,hide_block," + + "cam_x,cam_y,cam_z,cam_qx,cam_qy,cam_qz,cam_qw,cam_fov\n"); + string camCols = $"{cam["cam_x"]},{cam["cam_y"]},{cam["cam_z"]},{cam["cam_qx"]},{cam["cam_qy"]},{cam["cam_qz"]},{cam["cam_qw"]},{cam["cam_fov"]}"; + + foreach (var pt in order) + { + var ph = byPoint[pt]; + if (!(ph.ContainsKey("above") && ph.ContainsKey("grasp") && ph.ContainsKey("lift") && ph.ContainsKey("box"))) + { + sb.AppendLine($"{pt},no,,,,,,,,,"); + continue; + } + var any = ph["grasp"]; + float bx = TwinScene.F(any["px"]), bz = TwinScene.F(any["pz"]); + float xx = TwinScene.F(any["box_x"]), xz = TwinScene.F(any["box_z"]); + sc.Place(bx, bz, 0f, xx, xz, 0f); + + // 区間: (名前, 始め, 終わり, 爪) + var segs = new List<(string, float[], float[], float)> + { + ("home_to_above", Home, J(ph["above"]), 0f), + ("above_to_grasp", J(ph["above"]), J(ph["grasp"]), 0f), + ("grasp_to_lift", J(ph["grasp"]), J(ph["lift"]), 40f), + ("lift_to_box", J(ph["lift"]), J(ph["box"]), 40f), + }; + float boxMin = float.MaxValue, floorMin = float.MaxValue, blkMin = float.MaxValue, depth = 0f; + string boxPart = "", boxSeg = "", floorSeg = "", blkPart = ""; + bool inside = false; + float[] boxAt = null; float boxGrip = 0f; + foreach (var (name, a, b, grip) in segs) + { + for (int s = 0; s <= Steps; s++) + { + float t = (float)s / Steps; + var q = new float[6]; + for (int k = 0; k < 6; k++) q[k] = Mathf.Lerp(a[k], b[k], t); + sc.arm.SetAngles(q); + sc.arm.SetGrip(grip); + foreach (var part in new[] { TwinGeom.Part.Tip, TwinGeom.Part.Gripper, TwinGeom.Part.Forearm }) + { + float d = TwinGeom.NearestMm(sc.arm, sc.box, part, out _); + if (d < boxMin) { boxMin = d; boxPart = part.ToString(); boxSeg = $"{name}@{s}"; boxAt = q; boxGrip = grip; } + if (TwinGeom.InsideBox(sc.arm, sc.box, part, out float dep) > 0) { inside = true; depth = Mathf.Max(depth, dep); } + // つかむ前にブロックへ当たらないか: HOME→真上は全部位、真上→つかむは前腕だけ + //（爪・爪先はブロックを挟みに行くので、近いのは当たり前） + if (name == "home_to_above" || (name == "above_to_grasp" && part == TwinGeom.Part.Forearm)) + { + float db = TwinGeom.NearestMm(sc.arm, sc.block, part, out _); + if (db < blkMin) { blkMin = db; blkPart = part.ToString(); } + } + } + float fy = TwinGeom.ArmMinY(sc.arm) * 1000f; + if (fy < floorMin) { floorMin = fy; floorSeg = $"{name}@{s}"; } + } + } + sb.AppendLine($"{pt},yes,{boxMin:F1},{boxPart},{boxSeg},{(inside ? "yes" : "no")},{depth:F1},{floorMin:F1},{floorSeg},{blkMin:F1},{blkPart}"); + + string common = $"{TwinScene.S(bx)},{TwinScene.S(bz)},0,{TwinScene.S(xx)},{TwinScene.S(xz)},0"; + var g = J(ph["grasp"]); + string gj = string.Join(",", System.Array.ConvertAll(g, v => v.ToString("F3", CultureInfo.InvariantCulture))); + // 上カメラ（D で合わせたもの）から、つかむ直前（爪は開）。腕あり・腕なし（隠れの割合を数えるため） + shots.AppendLine($"{imgDir}/{pt}_top_grasp.png,{gj},0,{common},cam,0,0,960,540,0,0,{camCols}"); + shots.AppendLine($"{imgDir}/{pt}_top_grasp_noarm.png,{gj},0,{common},cam,0,0,960,540,1,0,{camCols}"); + // 箱にいちばん近づいた刻みを横から + string bj = string.Join(",", System.Array.ConvertAll(boxAt, v => v.ToString("F3", CultureInfo.InvariantCulture))); + shots.AppendLine($"{imgDir}/{pt}_side_boxmin.png,{bj},{(boxGrip > 0 ? 1 : 0)},{common},side,{TwinScene.S(xx)},{TwinScene.S(xz)},960,720,0,1,{camCols}"); + // つかむ姿勢を横から + shots.AppendLine($"{imgDir}/{pt}_side_grasp.png,{gj},0,{common},side,{TwinScene.S(bx)},{TwinScene.S(bz)},960,720,0,0,{camCols}"); + } + File.WriteAllText(outPath, sb.ToString()); + File.WriteAllText(shotsPath, shots.ToString()); + Debug.Log($"[TwinCheckC] points={order.Count} -> {outPath}"); + } + + static float[] J(Dictionary r) + { + var q = new float[6]; + for (int k = 0; k < 6; k++) q[k] = TwinScene.F(r["j" + k]); + return q; + } +} diff --git a/Assets/TwinCheck/Editor/TwinCheckC.cs.meta b/Assets/TwinCheck/Editor/TwinCheckC.cs.meta new file mode 100644 index 0000000..253c25f --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckC.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9d97b95bd28fd40eeb1f51379cfc06cf +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinCheckD.cs b/Assets/TwinCheck/Editor/TwinCheckD.cs new file mode 100644 index 0000000..5ddf683 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckD.cs @@ -0,0 +1,48 @@ +using UnityEngine; +using System.IO; +using System.Text; + +/// +/// TwinCheckD +/// 依頼107 の D。上カメラ合わせの手がかりにする、HOME（[0,0,0,0,0,-30]）のときの爪の点の 3D 位置を書き出す。 +/// 左右の爪先（tip Brep の中心＝見た目の外形の中心）と、爪の付け根の上面の中心。 +/// 実行: Unity -batchmode -quit -executeMethod TwinCheckD.Points -twinOut {出力 json} +/// +public static class TwinCheckD +{ + public static void Points() + { + string outPath = TwinCheckArgs.Get("-twinOut", ""); + var sc = TwinScene.Open(); + sc.arm.SetAngles(new float[] { 0, 0, 0, 0, 0, -30 }); + sc.arm.SetGrip(0f); + var sb = new StringBuilder("{\n"); + void Add(string name, Vector3 p, bool last = false) => + sb.Append($" \"{name}\": [{p.x:F5}, {p.y:F5}, {p.z:F5}]{(last ? "" : ",")}\n"); + Add("left_tip_origin", sc.arm.leftTip.position); + Add("right_tip_origin", sc.arm.rightTip.position); + Add("left_tip_bounds_center", Bounds(sc.arm.leftTip).center); + Add("right_tip_bounds_center", Bounds(sc.arm.rightTip).center); + Add("left_tip_bounds_top", Top(Bounds(sc.arm.leftTip))); + Add("right_tip_bounds_top", Top(Bounds(sc.arm.rightTip))); + var gb = TwinArm.FindDeep(sc.arm.root.transform, "gripper_base"); + Add("gripper_base_bounds_top", Top(Bounds(gb))); + Add("gripper_base_bounds_center", Bounds(gb).center); + var b = Bounds(sc.arm.joints[5].transform); + Add("link6_bounds_min", b.min); + Add("link6_bounds_max", b.max); + Add("tip_mid", sc.arm.TipWorld(), true); + sb.Append("}\n"); + File.WriteAllText(outPath, sb.ToString()); + } + + static Bounds Bounds(Transform t) + { + var rs = t.GetComponentsInChildren(true); + var b = rs.Length > 0 ? rs[0].bounds : new Bounds(t.position, Vector3.zero); + foreach (var r in rs) b.Encapsulate(r.bounds); + return b; + } + + static Vector3 Top(Bounds b) => new Vector3(b.center.x, b.max.y, b.center.z); +} diff --git a/Assets/TwinCheck/Editor/TwinCheckD.cs.meta b/Assets/TwinCheck/Editor/TwinCheckD.cs.meta new file mode 100644 index 0000000..97b706d --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckD.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f85723c5a0f434882b8d2448a3735603 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinCheckDump.cs b/Assets/TwinCheck/Editor/TwinCheckDump.cs new file mode 100644 index 0000000..67b540f --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckDump.cs @@ -0,0 +1,100 @@ +using UnityEngine; +using UnityEditor; +using UnityEditor.SceneManagement; +using System.IO; +using System.Text; + +/// +/// TwinCheckDump +/// 依頼107 の下調べ。プレハブ mecharm270mpi_0_0 の関節（ArticulationBody）の並びと +/// アンカー・ドライブの値を書き出すだけ。シーン・プレハブは保存しない。 +/// 実行: Unity -batchmode -executeMethod TwinCheckDump.Run -twinOut {出力ファイル} +/// +public static class TwinCheckDump +{ + const string PrefabPath = "Assets/Prefabs/mecharm270mpi_0_0.prefab"; + + public static void Run() + { + string outPath = TwinCheckArgs.Get("-twinOut", "/tmp/twincheck/dump.txt"); + var sb = new StringBuilder(); + + EditorSceneManager.NewScene(NewSceneSetup.EmptyScene, NewSceneMode.Single); + var prefab = AssetDatabase.LoadAssetAtPath(PrefabPath); + var go = (GameObject)PrefabUtility.InstantiatePrefab(prefab); + go.transform.position = Vector3.zero; + + sb.AppendLine("== hierarchy =="); + DumpTree(go.transform, 0, sb); + + sb.AppendLine("== articulation bodies =="); + foreach (var ab in go.GetComponentsInChildren(true)) + { + var d = ab.xDrive; + sb.AppendLine($"{Path(ab.transform)} root={ab.isRoot} type={ab.jointType} " + + $"anchorPos={V(ab.anchorPosition)} anchorRot={Q(ab.anchorRotation)} " + + $"parentAnchorPos={V(ab.parentAnchorPosition)} parentAnchorRot={Q(ab.parentAnchorRotation)} " + + $"stiff={d.stiffness} damp={d.damping} target={d.target} lo={d.lowerLimit} hi={d.upperLimit} " + + $"twistLock={ab.twistLock} immovable={ab.immovable} gravity={ab.useGravity} " + + $"localPos={V(ab.transform.localPosition)} localRot={Q(ab.transform.localRotation)} enabled={ab.enabled}"); + } + + File.WriteAllText(outPath, sb.ToString()); + Debug.Log($"[TwinCheckDump] wrote {outPath}"); + } + + /// シーンの写しの、ロボット以外の物（床・ブロック・箱・カメラ・光）の位置と大きさを書き出す。 + public static void RunScene() + { + string outPath = TwinCheckArgs.Get("-twinOut", "/tmp/twincheck/scene.txt"); + var sb = new StringBuilder(); + EditorSceneManager.OpenScene(TwinCheckA.ScenePath, OpenSceneMode.Single); + foreach (var go in UnityEngine.SceneManagement.SceneManager.GetActiveScene().GetRootGameObjects()) + { + var t = go.transform; + var comps = new StringBuilder(); + foreach (var c in go.GetComponents()) if (c != null && !(c is Transform)) comps.Append(c.GetType().Name).Append(' '); + var rend = go.GetComponentInChildren(); + string b = rend != null ? $" bounds c={V(rend.bounds.center)} s={V(rend.bounds.size)} mat={(rend.sharedMaterial ? rend.sharedMaterial.name : "-")}" : ""; + sb.AppendLine($"{go.name} active={go.activeSelf} pos={V(t.position)} rot={t.eulerAngles} scale={V(t.lossyScale)} [{comps}]{b}"); + if (!go.name.StartsWith("mecharm")) + foreach (Transform ch in t) + { + var r2 = ch.GetComponent(); + sb.AppendLine($" {ch.name} pos={V(ch.position)} scale={V(ch.lossyScale)}" + (r2 ? $" bounds c={V(r2.bounds.center)} s={V(r2.bounds.size)}" : "")); + } + var cam = go.GetComponent(); + if (cam) sb.AppendLine($" camera fov={cam.fieldOfView} near={cam.nearClipPlane} far={cam.farClipPlane} ortho={cam.orthographic}"); + } + foreach (var ab in GameObject.Find("mecharm270mpi_1_0").GetComponentsInChildren(true)) + { + var d = ab.xDrive; + sb.AppendLine($"AB {ab.name} type={ab.jointType} lo={d.lowerLimit} hi={d.upperLimit} motion={ab.twistLock} swingY={ab.swingYLock} swingZ={ab.swingZLock} target={d.target} stiff={d.stiffness} damp={d.damping} limit={d.forceLimit} " + + $"parentAnchorPos={V(ab.parentAnchorPosition)} parentAnchorRot={Q(ab.parentAnchorRotation)} anchorRot={Q(ab.anchorRotation)} matchAnchors={ab.matchAnchors} collide={ab.GetComponentsInChildren(true).Length}"); + } + File.WriteAllText(outPath, sb.ToString()); + } + + static void DumpTree(Transform t, int depth, StringBuilder sb) + { + var comps = t.GetComponents(); + var names = new StringBuilder(); + foreach (var c in comps) if (c != null && !(c is Transform)) names.Append(c.GetType().Name).Append(' '); + sb.AppendLine($"{new string(' ', depth * 2)}{t.name} pos={V(t.position)} [{names}]"); + foreach (Transform c in t) DumpTree(c, depth + 1, sb); + } + + static string Path(Transform t) => t.parent == null ? t.name : Path(t.parent) + "/" + t.name; + static string V(Vector3 v) => $"({v.x:F5},{v.y:F5},{v.z:F5})"; + static string Q(Quaternion q) => $"({q.x:F5},{q.y:F5},{q.z:F5},{q.w:F5})"; +} + +public static class TwinCheckArgs +{ + public static string Get(string name, string def) + { + var a = System.Environment.GetCommandLineArgs(); + for (int i = 0; i < a.Length - 1; i++) if (a[i] == name) return a[i + 1]; + return def; + } +} diff --git a/Assets/TwinCheck/Editor/TwinCheckDump.cs.meta b/Assets/TwinCheck/Editor/TwinCheckDump.cs.meta new file mode 100644 index 0000000..8ecf220 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckDump.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 28d0f8a41a41742629330f8cbbba43c2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinCsv.cs b/Assets/TwinCheck/Editor/TwinCsv.cs new file mode 100644 index 0000000..c4a049e --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCsv.cs @@ -0,0 +1,46 @@ +using System.Collections.Generic; +using System.IO; +using System.Text; + +/// 依頼107。見出し付き CSV を読む（ダブルクォートの中のカンマに対応）。 +public static class TwinCsv +{ + public static List> Read(string path) + { + var rows = new List>(); + var lines = File.ReadAllLines(path, Encoding.UTF8); + if (lines.Length == 0) return rows; + var head = Split(lines[0]); + for (int i = 1; i < lines.Length; i++) + { + if (string.IsNullOrWhiteSpace(lines[i])) continue; + var c = Split(lines[i]); + var d = new Dictionary(); + for (int k = 0; k < head.Count && k < c.Count; k++) d[head[k]] = c[k]; + rows.Add(d); + } + return rows; + } + + static List Split(string line) + { + var o = new List(); + var sb = new StringBuilder(); + bool q = false; + for (int i = 0; i < line.Length; i++) + { + char ch = line[i]; + if (q) + { + if (ch == '"' && i + 1 < line.Length && line[i + 1] == '"') { sb.Append('"'); i++; } + else if (ch == '"') q = false; + else sb.Append(ch); + } + else if (ch == '"') q = true; + else if (ch == ',') { o.Add(sb.ToString()); sb.Clear(); } + else sb.Append(ch); + } + o.Add(sb.ToString()); + return o; + } +} diff --git a/Assets/TwinCheck/Editor/TwinCsv.cs.meta b/Assets/TwinCheck/Editor/TwinCsv.cs.meta new file mode 100644 index 0000000..0471298 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCsv.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d222e40e04e384729b8811bc20f576e8 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinGeom.cs b/Assets/TwinCheck/Editor/TwinGeom.cs new file mode 100644 index 0000000..14ed66a --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinGeom.cs @@ -0,0 +1,142 @@ +using UnityEngine; +using System.Collections.Generic; + +/// +/// TwinGeom +/// 依頼107。腕の見た目のメッシュ（頂点を間引いたもの）と、箱の壁・ブロックの直方体との +/// 距離を測る。物理は使わない。直方体は Transform の位置・回転・lossyScale（単位立方体）とみなす。 +/// +public static class TwinGeom +{ + public enum Part { Tip, Gripper, Forearm, Other } + + class Cached { public Renderer r; public Vector3[] v; public Part part; } + static readonly Dictionary> cache = new Dictionary>(); + + static List Get(TwinArm arm) + { + if (cache.TryGetValue(arm.root, out var list)) return list; + list = new List(); + foreach (var r in arm.joints[0].GetComponentsInChildren(true)) + { + var mf = r.GetComponent(); + if (mf == null || mf.sharedMesh == null) continue; + var all = mf.sharedMesh.vertices; + int stride = Mathf.Max(1, all.Length / 1500); + var v = new List(); + for (int i = 0; i < all.Length; i += stride) v.Add(all[i]); + list.Add(new Cached { r = r, v = v.ToArray(), part = Classify(arm, r.transform) }); + } + cache[arm.root] = list; + return list; + } + + static Part Classify(TwinArm arm, Transform t) + { + if (t == arm.leftTip || t == arm.rightTip || IsUnder(t, arm.leftTip?.parent) || IsUnder(t, arm.rightTip?.parent)) return Part.Tip; + for (var p = t; p != null; p = p.parent) + { + if (p.name.StartsWith("gripper")) return Part.Gripper; + if (p == arm.joints[2].transform || p == arm.joints[3].transform || p == arm.joints[4].transform || p == arm.joints[5].transform) return Part.Forearm; + if (p == arm.root.transform) break; + } + return Part.Other; + } + + static bool IsUnder(Transform t, Transform parent) + { + if (parent == null) return false; + for (var p = t; p != null; p = p.parent) if (p == parent) return true; + return false; + } + + public static float ArmMinY(TwinArm arm) + { + float m = float.MaxValue; + foreach (var c in Get(arm)) + { + var tr = c.r.transform; + foreach (var v in c.v) m = Mathf.Min(m, tr.TransformPoint(v).y); + } + return m - arm.baseBody.transform.position.y; + } + + /// 直方体（単位立方体を Transform で置いたもの）と点の距離（m）。中なら負（めり込みの深さ）。 + public static float BoxDistance(Transform box, Vector3 p) + { + Vector3 half = box.lossyScale * 0.5f; + Vector3 q = Quaternion.Inverse(box.rotation) * (p - box.position); + Vector3 d = new Vector3(Mathf.Abs(q.x) - half.x, Mathf.Abs(q.y) - half.y, Mathf.Abs(q.z) - half.z); + Vector3 o = Vector3.Max(d, Vector3.zero); + float outside = o.magnitude; + float inside = Mathf.Min(Mathf.Max(d.x, Mathf.Max(d.y, d.z)), 0f); + return outside + inside; + } + + static IEnumerable Solids(GameObject obj) + { + if (obj.GetComponent() != null) yield return obj.transform; + // 箱の「縁」を見るため、底板（Bottom）は距離の相手に入れない + foreach (Transform ch in obj.transform) if (ch.GetComponent() != null && ch.name != "Bottom") yield return ch; + } + + /// 腕（link1 から先）と物の、いちばん近い距離（mm）。 + public static float ArmToObjectMm(TwinArm arm, GameObject obj) => NearestMm(arm, obj, null, out _); + + /// 部位ごとの、物へのいちばん近い距離（mm）。part を null にすると全部。 + public static float NearestMm(TwinArm arm, GameObject obj, Part? part, out Vector3 nearestPoint) + { + float m = float.MaxValue; + nearestPoint = Vector3.zero; + if (obj == null) return m; + var solids = new List(Solids(obj)); + foreach (var c in Get(arm)) + { + if (part.HasValue && c.part != part.Value) continue; + var tr = c.r.transform; + foreach (var v in c.v) + { + var p = tr.TransformPoint(v); + foreach (var s in solids) + { + float d = BoxDistance(s, p); + if (d < m) { m = d; nearestPoint = p; } + } + } + } + return m * 1000f; + } + + /// + /// 箱の内側（壁の内面より内・底より上・壁の上端より下）に入っている頂点の数と、 + /// 入り込んだ深さ（壁の上端から下へ、mm）の最大。箱は Bottom/Front/Back/Left/Right の5枚でできている前提。 + /// + public static int InsideBox(TwinArm arm, GameObject box, Part? part, out float maxDepthMm) + { + maxDepthMm = 0f; + var f = box.transform.Find("Front"); var b = box.transform.Find("Back"); + var l = box.transform.Find("Left"); var r = box.transform.Find("Right"); + if (f == null || b == null || l == null || r == null) return -1; + float xMin = Mathf.Min(l.position.x, r.position.x) + l.lossyScale.x * 0.5f; + float xMax = Mathf.Max(l.position.x, r.position.x) - r.lossyScale.x * 0.5f; + float zMin = Mathf.Min(f.position.z, b.position.z) + f.lossyScale.z * 0.5f; + float zMax = Mathf.Max(f.position.z, b.position.z) - b.lossyScale.z * 0.5f; + float top = f.position.y + f.lossyScale.y * 0.5f; + int n = 0; + foreach (var c in Get(arm)) + { + if (part.HasValue && c.part != part.Value) continue; + var tr = c.r.transform; + foreach (var v in c.v) + { + var p = tr.TransformPoint(v); + if (p.x > xMin && p.x < xMax && p.z > zMin && p.z < zMax && p.y < top) + { + n++; + maxDepthMm = Mathf.Max(maxDepthMm, (top - p.y) * 1000f); + } + } + } + return n; + } +} diff --git a/Assets/TwinCheck/Editor/TwinGeom.cs.meta b/Assets/TwinCheck/Editor/TwinGeom.cs.meta new file mode 100644 index 0000000..722ce63 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinGeom.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4946c894ed6444426bba68e21b54b7e3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinScene.cs b/Assets/TwinCheck/Editor/TwinScene.cs new file mode 100644 index 0000000..564a578 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinScene.cs @@ -0,0 +1,72 @@ +using UnityEngine; +using UnityEditor.SceneManagement; +using System.Globalization; +using System.IO; + +/// +/// TwinScene +/// 依頼107。シーンの写し（Assets/TwinCheck/TwinCheck_Base.unity）を開き、 +/// 1体目のアーム（mecharm270mpi_1_0、根元が原点）・赤ブロック・白箱・床・光だけを残して見せる。 +/// 他の物は SetActive(false) にするだけで、シーンは保存しない。 +/// +public class TwinScene +{ + public TwinArm arm; + public GameObject block, box, floor; + + public static TwinScene Open() + { + EditorSceneManager.OpenScene(TwinCheckA.ScenePath, OpenSceneMode.Single); + var s = new TwinScene(); + var keep = new[] { "mecharm270mpi_1_0", "RedBlock_arm1", "WhiteBox_arm1", "Plane1", "Directional Light", "Main Camera" }; + foreach (var go in UnityEngine.SceneManagement.SceneManager.GetActiveScene().GetRootGameObjects()) + go.SetActive(System.Array.IndexOf(keep, go.name) >= 0); + s.arm = new TwinArm(GameObject.Find("mecharm270mpi_1_0")); + s.block = GameObject.Find("RedBlock_arm1"); + s.box = GameObject.Find("WhiteBox_arm1"); + s.floor = GameObject.Find("Plane1"); + // 物理で動かないように（物理は回さないが念のため） + var rb = s.block.GetComponent(); + if (rb) rb.isKinematic = true; + return s; + } + + /// ブロック（24mm 角、マットの上）と白箱（80mm 角・高さ 43mm は仮）を置く。yaw は画像から求めた向き（度）。 + public void Place(float blockX, float blockZ, float blockYaw, float boxX, float boxZ, float boxYaw) + { + block.transform.SetPositionAndRotation(new Vector3(blockX, 0.012f, blockZ), Quaternion.Euler(0, -blockYaw, 0)); + box.transform.SetPositionAndRotation(new Vector3(boxX, 0f, boxZ), Quaternion.Euler(0, -Wrap45(boxYaw), 0)); + } + + public static float Wrap45(float yaw) + { + yaw %= 90f; + if (yaw > 45f) yaw -= 90f; + if (yaw < -45f) yaw += 90f; + return yaw; + } + + /// カメラから RenderTexture に撮って PNG に書く。 + public static void Shoot(Camera cam, int w, int h, string path) + { + var rt = new RenderTexture(w, h, 24, RenderTextureFormat.ARGB32) { antiAliasing = 4 }; + var prev = cam.targetTexture; + cam.targetTexture = rt; + cam.Render(); + var act = RenderTexture.active; + RenderTexture.active = rt; + var tex = new Texture2D(w, h, TextureFormat.RGB24, false); + tex.ReadPixels(new Rect(0, 0, w, h), 0, 0); + tex.Apply(); + RenderTexture.active = act; + cam.targetTexture = prev; + Directory.CreateDirectory(Path.GetDirectoryName(path)); + File.WriteAllBytes(path, tex.EncodeToPNG()); + Object.DestroyImmediate(tex); + rt.Release(); + Object.DestroyImmediate(rt); + } + + public static float F(string s) => float.Parse(s, CultureInfo.InvariantCulture); + public static string S(float v, string fmt = "F4") => v.ToString(fmt, CultureInfo.InvariantCulture); +} diff --git a/Assets/TwinCheck/Editor/TwinScene.cs.meta b/Assets/TwinCheck/Editor/TwinScene.cs.meta new file mode 100644 index 0000000..c9f93fd --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinScene.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6b85b5c066bb54764ac32b5cc08c37b2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Python~/twin_a.py b/Assets/TwinCheck/Python~/twin_a.py new file mode 100644 index 0000000..e6ab934 --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_a.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +"""依頼107 A: FK テーブルの行の関節角を Unity に置いた爪先（TwinCheckA の出力）と、表の ee のずれをまとめる。 + + python3 twin_a.py {行の CSV} {TwinCheckA の出力} {まとめの CSV} +腕（link1 から先）がマットの下に入る・箱・ブロックに触れる姿勢は「接触あり」として分けて数える +（表は物理を 0.5 秒回してから測っているので、接触すると角度どおりの位置で止まらない）。 +""" +import csv +import math +import statistics as st +import sys + + +def main(rows_csv, tip_csv, out_csv): + ref = {r["row_index"]: r for r in csv.DictReader(open(rows_csv))} + out = [] + per_robot = {} + for r in csv.DictReader(open(tip_csv)): + e = ref[r["row_index"]] + d = math.dist([float(r["tip_x"]), float(r["tip_y"]), float(r["tip_z"])], + [float(e["ee_x"]), float(e["ee_y"]), float(e["ee_z"])]) * 1000 + per_robot.setdefault(r["robot"], []).append(d) + if r["robot"] != "mecharm270mpi_1_0": + continue + contact = float(r["arm_min_y"]) < 0 or float(r["near_box_mm"]) < 0 or float(r["near_block_mm"]) < 0 + out.append(dict(row_index=r["row_index"], **{k: e[k] for k in ("ee_x", "ee_y", "ee_z", "j0", "j1", "j2", "j3", "j4", "j5")}, + unity_x=r["tip_x"], unity_y=r["tip_y"], unity_z=r["tip_z"], diff_mm=round(d, 2), + arm_min_y_mm=round(float(r["arm_min_y"]) * 1000, 1), near_box_mm=r["near_box_mm"], + near_block_mm=r["near_block_mm"], contact="yes" if contact else "no")) + with open(out_csv, "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=list(out[0].keys())) + w.writeheader() + w.writerows(out) + ds = [o["diff_mm"] for o in out] + free = [o["diff_mm"] for o in out if o["contact"] == "no"] + print(f"行 {len(ds)}: 最大 {max(ds):.1f}mm 中央値 {st.median(ds):.1f}mm / 接触なし {len(free)} 行: 最大 {max(free):.1f}mm 中央値 {st.median(free):.1f}mm " + f"/ 10mm 超 {sum(d > 10 for d in ds)} 行（うち接触あり {sum(o['diff_mm'] > 10 and o['contact'] == 'yes' for o in out)}）") + spread = max(abs(max(v) - max(per_robot['mecharm270mpi_1_0'])) for v in per_robot.values()) + print(f"20体のあいだの差（最大のずれの差）: {spread:.2f}mm") + + +if __name__ == "__main__": + main(*sys.argv[1:4]) diff --git a/Assets/TwinCheck/Python~/twin_c.py b/Assets/TwinCheck/Python~/twin_c.py new file mode 100644 index 0000000..3158c1f --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_c.py @@ -0,0 +1,246 @@ +#!/usr/bin/env python3 +"""依頼107 C: 次に実機で試す置き場所の候補を、FK テーブルと Unity で確かめる。 + + select : 各点で、肩 j1 <= 88° の行から 4 つの姿勢（ブロックの真上・つかむ・持ち上げ・白箱の上）の候補を + 上位 K 個ずつ選び、C/c_candidates.csv に書く（Unity で角度を置き直して表の ee と比べるため） + finalize : Unity で置き直した爪先（C/c_candidates_fk.csv）が表の ee と 5mm 以内の候補のうち、 + いちばん順位の高いものを採り、C/c_poses.csv（Unity の道すじの確かめ・撮影の入力）を書く + table : Unity の結果（C/c_path.csv・C/images/*）から、1点1行の表 C/c_table.csv と各点の画像を作る + +姿勢の選び方（どれも肩 j1 <= 88°） + つかむ : 爪先の高さ 10〜40mm（Codex の条件）。水平のずれ + 0.2×|高さ−20mm| が小さい順 + （20mm は B の成功5件の閉じた瞬間の爪先の高さ 20〜25mm に合わせた目安） + 真上 : 高さ 60〜100mm。水平のずれ 15mm 以内のうち、つかむ姿勢との関節の差（j0〜j4 の絶対値の和）が小さい順 + 持ち上げ : 高さ 80〜130mm。同じく、つかむ姿勢との関節の差が小さい順 + 白箱の上 : 白箱の中心（B の検出の中央値）の上、高さ 80〜130mm。水平のずれ 15mm 以内のうち、持ち上げ姿勢との関節の差が小さい順 +""" +import csv +import json +import math +import sys +from pathlib import Path + +import numpy as np + +OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所") +FK = Path("/Volumes/KIOXIA/r2r2r-data/fk_tables/fk_scan_motion_based_v2.csv") +J1_MAX = 88.0 +K = 8 +GRID_X = [120, 160, 200] +GRID_Z = [180, 220, 260] +HOME = [0, 0, 0, 0, 0, -30] +FK_OK_MM = 5.0 + + +def box_center(): + xs, zs = [], [] + for r in csv.DictReader(open(OUT / "B/trials.csv")): + if r["box_pos_source"] == "detected": + xs.append(float(r["box_x_m"])) + zs.append(float(r["box_z_m"])) + return float(np.median(xs)), float(np.median(zs)) + + +def points(): + pts = [dict(point=f"G_x{x}_z{z}", kind="grid", x=x / 1000, z=z / 1000) for x in GRID_X for z in GRID_Z] + for r in csv.DictReader(open(OUT / "C/teach_pick_fk.csv")): + if r["robot"] != "mecharm270mpi_1_0": + continue + pts.append(dict(point=f"T{r['row_index']}", kind="teach", x=float(r["tip_x"]), z=float(r["tip_z"]))) + return pts + + +def load_fk(): + a = np.loadtxt(FK, delimiter=",", skiprows=1) + a = a[a[:, 4] <= J1_MAX] + return a # ee_x, ee_y, ee_z, j0..j5 + + +def select(): + a = load_fk() + bx, bz = box_center() + rows = [] + for p in points(): + dxz = np.hypot(a[:, 0] - p["x"], a[:, 2] - p["z"]) + y = a[:, 1] + m = (y >= 0.010) & (y <= 0.040) + score = dxz * 1000 + 0.2 * np.abs(y - 0.020) * 1000 + idx = np.where(m)[0] + g = idx[np.argsort(score[idx])][:K] + for rank, i in enumerate(g): + rows.append((p, "grasp", rank, i)) + gj = a[g[0], 3:8] + for phase, lo, hi, ref, cx, cz in (("above", 0.060, 0.100, gj, p["x"], p["z"]), + ("lift", 0.080, 0.130, gj, p["x"], p["z"])): + d2 = np.hypot(a[:, 0] - cx, a[:, 2] - cz) + mm = (a[:, 1] >= lo) & (a[:, 1] <= hi) & (d2 <= 0.015) + idx = np.where(mm)[0] + if len(idx) == 0: + mm = (a[:, 1] >= lo) & (a[:, 1] <= hi) + idx = np.where(mm)[0] + order = idx[np.argsort(d2[idx])][:K] + else: + jd = np.abs(a[idx, 3:8] - ref).sum(axis=1) + order = idx[np.argsort(jd)][:K] + for rank, i in enumerate(order): + rows.append((p, phase, rank, i)) + if phase == "lift": + lj = a[order[0], 3:8] + d2 = np.hypot(a[:, 0] - bx, a[:, 2] - bz) + mm = (a[:, 1] >= 0.080) & (a[:, 1] <= 0.130) & (d2 <= 0.015) + idx = np.where(mm)[0] + if len(idx): + jd = np.abs(a[idx, 3:8] - lj).sum(axis=1) + order = idx[np.argsort(jd)][:K] + for rank, i in enumerate(order): + rows.append((p, "box", rank, i)) + with open(OUT / "C/c_candidates.csv", "w") as f: + f.write("row_index,ee_x,ee_y,ee_z,j0,j1,j2,j3,j4,j5,point,phase,rank,target_x,target_z\n") + for k, (p, phase, rank, i) in enumerate(rows): + r = a[i] + tx, tz = (bx, bz) if phase == "box" else (p["x"], p["z"]) + f.write(f"{k}," + ",".join(f"{v:.6f}" for v in r[:3]) + "," + ",".join(f"{v:.3f}" for v in r[3:9]) + + f",{p['point']},{phase},{rank},{tx:.4f},{tz:.4f}\n") + print("候補", len(rows), "白箱の中心", bx, bz) + + +def finalize(): + cand = {r["row_index"]: r for r in csv.DictReader(open(OUT / "C/c_candidates.csv"))} + fk = {r["row_index"]: r for r in csv.DictReader(open(OUT / "C/c_candidates_fk.csv")) if r["robot"] == "mecharm270mpi_1_0"} + best = {} + for k, c in cand.items(): + f = fk[k] + d = math.dist([float(c["ee_x"]), float(c["ee_y"]), float(c["ee_z"])], + [float(f["tip_x"]), float(f["tip_y"]), float(f["tip_z"])]) * 1000 + c["table_vs_fk_mm"] = d + c.update(fk_x=f["tip_x"], fk_y=f["tip_y"], fk_z=f["tip_z"], arm_min_y=f["arm_min_y"]) + key = (c["point"], c["phase"]) + if d <= FK_OK_MM and (key not in best or int(c["rank"]) < int(best[key]["rank"])): + best[key] = c + bx, bz = box_center() + pts = points() + out = [] + for p in pts: + for phase in ("above", "grasp", "lift", "box"): + c = best.get((p["point"], phase)) + if c is None: + continue + out.append(dict(point=p["point"], kind=p["kind"], px=p["x"], pz=p["z"], phase=phase, rank=c["rank"], + fk_row=c["row_index"], **{f"j{i}": c[f"j{i}"] for i in range(6)}, + ee_x=c["ee_x"], ee_y=c["ee_y"], ee_z=c["ee_z"], fk_x=c["fk_x"], fk_y=c["fk_y"], fk_z=c["fk_z"], + table_vs_fk_mm=f"{c['table_vs_fk_mm']:.1f}", + target_x=c["target_x"], target_z=c["target_z"], box_x=f"{bx:.4f}", box_z=f"{bz:.4f}")) + with open(OUT / "C/c_poses.csv", "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=list(out[0].keys())) + w.writeheader() + w.writerows(out) + miss = [(p["point"], ph) for p in pts for ph in ("above", "grasp", "lift", "box") if (p["point"], ph) not in best] + print("採った姿勢", len(out), "選べなかった", miss) + + +VIS_OK = 0.5 # 上から見えるブロックの割合がこれ以上なら「見える」 +BOX_HIT_MM = -10.0 # 箱の壁にこれより深くめり込めば「当たる」 +NEAR_MM = 10.0 # 箱の壁・床から ±10mm 以内は「すれすれ」（数 mm の差で決めない） +GRASP_XZ_OK_MM = 10.0 + + +def red_pixels(path): + import cv2 + im = cv2.imread(str(path)).astype(int) + b, g, r = im[..., 0], im[..., 1], im[..., 2] + return int(((r > 60) & (r > 2 * g) & (r > 2 * b)).sum()) + + +def table(): + import cv2 + from PIL import Image, ImageDraw, ImageFont + font = ImageFont.truetype("/System/Library/Fonts/ヒラギノ角ゴシック W3.ttc", 24) + small = ImageFont.truetype("/System/Library/Fonts/ヒラギノ角ゴシック W3.ttc", 19) + poses = {} + for r in csv.DictReader(open(OUT / "C/c_poses.csv")): + poses.setdefault(r["point"], {})[r["phase"]] = r + path = {r["point"]: r for r in csv.DictReader(open(OUT / "C/c_path.csv"))} + rows = [] + for p in points(): + pt = p["point"] + ph = poses.get(pt, {}) + pa = path.get(pt, {}) + row = dict(point=pt, kind="教えた位置" if p["kind"] == "teach" else "格子の候補", + target_x_mm=round(p["x"] * 1000, 1), target_z_mm=round(p["z"] * 1000, 1)) + reasons = [] + if pa.get("complete") != "yes": + row.update(usable="使えない", reason="表の中で、つかむ・真上・持ち上げの姿勢を選べなかった（候補の行は、角度を置き直すと腕が床の下に入る＝表を作ったときに床に当たって止まった値）") + rows.append(row) + continue + for phase in ("above", "grasp", "lift", "box"): + r = ph[phase] + row[f"{phase}_angles"] = "[" + ", ".join(f"{float(r[f'j{i}']):.1f}" for i in range(6)) + "]" + g = ph["grasp"] + row["shoulder_j1_grasp_deg"] = float(g["j1"]) + row["shoulder_j1_max_deg"] = max(float(ph[k]["j1"]) for k in ph) + gx, gy, gz = float(g["fk_x"]), float(g["fk_y"]), float(g["fk_z"]) + row["grasp_tip_xz_err_mm"] = round(math.hypot(gx - p["x"], gz - p["z"]) * 1000, 1) + row["grasp_tip_h_mm"] = round(gy * 1000, 1) + row["table_vs_unity_max_mm"] = max(float(ph[k]["table_vs_fk_mm"]) for k in ph) + bxz = [float(ph["box"]["fk_x"]), float(ph["box"]["fk_z"])] + row["above_box_xz_err_mm"] = round(math.hypot(bxz[0] - float(ph["box"]["box_x"]), bxz[1] - float(ph["box"]["box_z"])) * 1000, 1) + row["above_box_tip_h_mm"] = round(float(ph["box"]["fk_y"]) * 1000, 1) + bm = float(pa["box_min_mm"]) + row.update(box_min_mm=bm, box_min_part=pa["box_min_part"], box_min_where=pa["box_min_segment"], + box_hit="当たる" if bm < BOX_HIT_MM else ("すれすれ" if bm <= NEAR_MM else "当たらない"), + floor_min_mm=float(pa["floor_min_mm"]), floor_min_where=pa["floor_min_segment"], + block_min_before_grasp_mm=float(pa["block_min_before_grasp_mm"])) + n_arm = red_pixels(OUT / f"C/images/{pt}_top_grasp.png") + n_free = red_pixels(OUT / f"C/images/{pt}_top_grasp_noarm.png") + vis = n_arm / n_free if n_free else 0.0 + row["top_visible_ratio"] = round(vis, 2) + row["top_visible"] = "見える" if vis >= VIS_OK else "隠れる" + if row["grasp_tip_xz_err_mm"] > GRASP_XZ_OK_MM: + reasons.append(f"つかむ姿勢の爪先が狙いから {row['grasp_tip_xz_err_mm']}mm 離れる") + if row["box_hit"] == "当たる": + reasons.append(f"{row['box_min_part']}が箱に当たる") + elif row["box_hit"] == "すれすれ": + reasons.append(f"{row['box_min_part']}が箱の縁すれすれ（{bm:+.0f}mm）") + if row["floor_min_mm"] < -NEAR_MM: + reasons.append(f"腕が床の下 {-row['floor_min_mm']:.0f}mm に入る") + elif row["floor_min_mm"] < 0: + # つかむ姿勢で爪先がマットの近く（0〜10mm）に来るのは当たり前なので、床の下に入るときだけ書く + reasons.append(f"腕が床の下に少し入る（{row['floor_min_mm']:+.1f}mm、{row['floor_min_where']}。±{NEAR_MM:.0f}mm 以内なので当たるとは決めない）") + if vis < VIS_OK: + reasons.append(f"上から見てブロックが {100 * (1 - vis):.0f}% 隠れる") + hard = [x for x in reasons if "当たる" in x and "決めない" not in x or "mm に入る" in x or "隠れる" in x or "離れる" in x] + row["usable"] = "使えない" if hard else ("要注意" if reasons else "使えそう") + row["reason"] = " / ".join(reasons) if reasons else "どの条件にも当たらない" + rows.append(row) + + # 画像（1点1枚）: 上カメラから見たつかむ直前 / 横から見たつかむ姿勢 / 箱にいちばん近づいたとき + top = cv2.imread(str(OUT / f"C/images/{pt}_top_grasp.png")) + s1 = cv2.resize(cv2.imread(str(OUT / f"C/images/{pt}_side_grasp.png")), (720, 540)) + s2 = cv2.resize(cv2.imread(str(OUT / f"C/images/{pt}_side_boxmin.png")), (720, 540)) + canvas = cv2.cvtColor(np.hstack([top, s1, s2]), cv2.COLOR_BGR2RGB) + im = Image.fromarray(canvas) + d = ImageDraw.Draw(im) + d.rectangle([0, 0, 960, 68], fill=(255, 255, 255)) + d.rectangle([0, 502, 960, 540], fill=(255, 255, 255)) + d.text((10, 8), f"{pt}（{row['kind']}・x={row['target_x_mm']} z={row['target_z_mm']}mm） 判定: {row['usable']}", font=font, fill=(0, 0, 0)) + d.text((10, 40), f"上カメラ（D で合わせたもの）から、つかむ直前。ブロックが見える割合 {vis:.0%}", font=small, fill=(0, 0, 0)) + d.text((970, 8), f"横から、つかむ姿勢（肩 j1={row['shoulder_j1_grasp_deg']:.1f}°・爪先の高さ {row['grasp_tip_h_mm']}mm）", font=small, fill=(0, 0, 0)) + d.text((1690, 8), f"横から、箱にいちばん近いとき（{row['box_min_part']} {bm:.0f}mm）", font=small, fill=(0, 0, 0)) + d.text((10, 510), row["reason"][:70], font=small, fill=(150, 0, 0)) + im.save(OUT / f"C/c_{pt}.png") + + cols = ["point", "kind", "target_x_mm", "target_z_mm", "usable", "reason", + "above_angles", "grasp_angles", "lift_angles", "box_angles", "shoulder_j1_grasp_deg", "shoulder_j1_max_deg", + "grasp_tip_xz_err_mm", "grasp_tip_h_mm", "table_vs_unity_max_mm", "above_box_xz_err_mm", "above_box_tip_h_mm", + "box_min_mm", "box_min_part", "box_min_where", "box_hit", "floor_min_mm", "floor_min_where", + "block_min_before_grasp_mm", "top_visible_ratio", "top_visible"] + with open(OUT / "C/c_table.csv", "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=cols, extrasaction="ignore") + w.writeheader() + w.writerows(rows) + for r in rows: + print(r["point"], r["usable"], r.get("top_visible_ratio"), r.get("grasp_tip_xz_err_mm"), r.get("shoulder_j1_max_deg"), r.get("box_min_mm"), r.get("floor_min_mm"), r["reason"]) + + +if __name__ == "__main__": + {"select": select, "finalize": finalize, "table": table}[sys.argv[1]]() diff --git a/Assets/TwinCheck/Python~/twin_camera_d.py b/Assets/TwinCheck/Python~/twin_camera_d.py new file mode 100644 index 0000000..af2cd47 --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_camera_d.py @@ -0,0 +1,176 @@ +#!/usr/bin/env python3 +"""依頼107 D: Unity のカメラを実機の上カメラに合わせる。 + +合わせ方 + 1. 実機の1コマ（アームが HOME の最初のコマ）でマットの四隅を検出する（本線の mat_corner_detection.detect_corners） + 2. 画素は正方形・画像の中心が光軸・レンズのゆがみなしと仮定する + 3. マットの上の2隅は画像の上の端で切れていて本当の角ではない（v=0）。そこで、 + - 下の2隅（本当の角）は (±310, 361) mm の点として合わせる + - 上の2点は「マットの左右の縁 x=±310mm の上のどこか」（奥行き z は未知）として合わせる + - HOME の爪先の青い印2つ（実機の画像）を、Unity の HOME の左右の爪先（tip Brep の位置）に合わせる + 焦点距離・カメラの位置3・向き3・上の2点の奥行き2 の 9 個を、10 個の式の最小二乗で解く（scipy）。 + ホモグラフィだけ・四隅だけでは焦点距離が決まらない（ほぼ真下向きのため。試すと 2 万 px 台の外れた値になる） + 4. 残ったずれ: 各点の再投影の誤差（px）。爪先の印は Unity と実機のアームの姿勢のずれも含む +出力: {OUT}/D/camera_fit.json（Unity のカメラの位置・回転・画角） +""" +import json +import math +import sys +from pathlib import Path + +import cv2 +import numpy as np + +OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所") +RUNS = Path("/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy") +sys.path.insert(0, str(Path.home() / "r2r2r-scripts")) +MAT_CORNERS_MM = [[310, -89], [-310, -89], [-310, 361], [310, 361]] +RUN = "pi05_20261101_191547" # 補正なしの失敗。最初のコマはアームが HOME、ブロックと箱が見える + + +def focal_from_h(G, cx, cy): + """G: マットの点(X,Z,1) → 画素 の 3x3。K=[f,0,cx;0,f,cy;0,0,1] として列の直交と長さの等しさから f を求める。""" + T = np.array([[1, 0, -cx], [0, 1, -cy], [0, 0, 1]], float) + A = T @ G # = diag(f,f,1) [r1 r2 t] + h1, h2 = A[:, 0], A[:, 1] + # 直交: (h1x h2x + h1y h2y)/f^2 + h1z h2z = 0 + # 等長: (h1x^2+h1y^2 - h2x^2-h2y^2)/f^2 + (h1z^2 - h2z^2) = 0 + a = np.array([h1[0] * h2[0] + h1[1] * h2[1], h1[0] ** 2 + h1[1] ** 2 - h2[0] ** 2 - h2[1] ** 2]) + b = -np.array([h1[2] * h2[2], h1[2] ** 2 - h2[2] ** 2]) + inv_f2 = float(a @ b) / float(b @ b) # a * (1/f^2) = b ... を 1/f^2 について解く + # a/f^2 = b → 1/f^2 = (a·b)/(a·a) のほうが正しい向き + inv_f2 = float(b @ a) / float(a @ a) + return 1.0 / math.sqrt(inv_f2) if inv_f2 > 0 else None + + +def main(): + import mat_corner_detection as mcd + cap = cv2.VideoCapture(str(RUNS / RUN / "recording/top.mp4")) + ok, frame = cap.read() + cap.release() + h, w = frame.shape[:2] + corners = np.array(mcd.detect_corners(frame), np.float64) + mm = np.array(MAT_CORNERS_MM, np.float64) + H = cv2.getPerspectiveTransform(corners.astype(np.float32), mm.astype(np.float32)) # 画素 → mm + G = np.linalg.inv(H) + G /= G[2, 2] + G_m = G @ np.diag([1000.0, 1000.0, 1.0]) # m → 画素 + cx, cy = w / 2.0, h / 2.0 + f_h = focal_from_h(G_m, cx, cy) + # Unity (x, y=0, z) → 右手系 (x, y, -z) + obj = np.array([[p[0] / 1000.0, 0.0, -p[1] / 1000.0] for p in MAT_CORNERS_MM]) + hp = json.load(open(OUT / "D/home_points_unity.json")) + tips3 = np.array([[p[0], p[1], -p[2]] for p in (hp["left_tip_origin"], hp["right_tip_origin"])]) + dots = blue_dots(frame) + # 爪先の印は画像の下のほうの2つ（v が大きい2つ）。Unity の左（x 負）は画像の右（u 大） + fingers = sorted(sorted(dots, key=lambda d: -d[1])[:2], key=lambda d: -d[0]) + fingers = np.array(fingers, np.float64) + + from scipy.optimize import least_squares + bottom3 = np.array([[-0.310, 0.0, -0.361], [0.310, 0.0, -0.361]]) # 右手系（z 反転）。LR=(-310,361), LL=(310,361) + bottom_px = corners[[2, 3]] + top_px = corners[[0, 1]] # UL は x=+310 の縁、UR は x=-310 の縁 + + def unpack(p): + fv, rv, tv, zl, zr = p[0], p[1:4], p[4:7], p[7], p[8] + K = np.array([[fv, 0, cx], [0, fv, cy], [0, 0, 1]]) + return K, rv, tv, zl, zr + + def resid(p): + K, rv, tv, zl, zr = unpack(p) + pts = np.vstack([bottom3, [[0.310, 0, -zl], [-0.310, 0, -zr]], tips3]) + pr, _ = cv2.projectPoints(pts, rv, tv, K, None) + pr = pr.reshape(-1, 2) + tgt = np.vstack([bottom_px, top_px, fingers]) + return (pr - tgt).ravel() + + # 初期値: 焦点距離 1400px（iPhone の広角の目安）で、四隅から解いた姿勢 + K0 = np.array([[1400.0, 0, cx], [0, 1400.0, cy], [0, 0, 1]]) + _, rv0, tv0 = cv2.solvePnP(obj, corners, K0, None, flags=cv2.SOLVEPNP_IPPE) + p0 = np.concatenate([[1400.0], rv0.ravel(), tv0.ravel(), [-0.089, -0.089]]) + sol = least_squares(resid, p0, x_scale="jac") + K, rvec, tvec, z_ul, z_ur = unpack(sol.x) + f = float(K[0, 0]) + r = sol.fun.reshape(-1, 2) + errs = np.linalg.norm(r, axis=1) + tip_err = errs[4:6] + rvec = rvec.reshape(3, 1) + tvec = tvec.reshape(3, 1) + R, _ = cv2.Rodrigues(rvec) + proj, _ = cv2.projectPoints(obj, rvec, tvec, K, None) + reproj = np.linalg.norm(proj.reshape(-1, 2) - corners, axis=1) + C = (-R.T @ tvec).ravel() + fwd = R.T @ np.array([0, 0, 1.0]) + up = R.T @ np.array([0, -1.0, 0]) + to_u = np.array([1, 1, -1.0]) + C_u, fwd_u, up_u = C * to_u, fwd * to_u, up * to_u + fov_v = math.degrees(2 * math.atan(cy / f)) + + # Unity の LookRotation(fwd, up) を四元数にする（Unity は左手系なので、右手系の式で x,y,z を組んだ行列から求める） + zc = fwd_u / np.linalg.norm(fwd_u) + xc = np.cross(up_u, zc) # Unity: right = up × forward（左手系） + xc /= np.linalg.norm(xc) + yc = np.cross(zc, xc) + M = np.column_stack([xc, yc, zc]) + q = rot_to_quat(M) + + res = dict(run=RUN, frame=0, image_w=w, image_h=h, mat_corners_px=corners.tolist(), mat_corners_mm=MAT_CORNERS_MM, + focal_px=f, principal_px=[cx, cy], fov_vertical_deg=fov_v, + cam_pos_unity_m=C_u.tolist(), cam_forward_unity=fwd_u.tolist(), cam_up_unity=up_u.tolist(), + cam_quat_unity_xyzw=q, reproj_px=reproj.tolist(), reproj_max_px=float(reproj.max()), + rvec=rvec.ravel().tolist(), tvec=tvec.ravel().tolist(), K=K.tolist(), + focal_from_homography_only_px=f_h, + fit_points=["下の左右の隅（本当の角）", "上の2点（左右の縁の上・奥行き未知）", "HOME の爪先の印2つ"], + fit_err_px=errs.tolist(), top_points_z_mm=[float(-z_ul * 1000) * -1, float(-z_ur * 1000) * -1], blue_dots_px=[list(map(float, d)) for d in dots], + finger_dots_px=fingers.tolist(), finger_reproj_px=tip_err.tolist(), + fov_horizontal_deg=math.degrees(2 * math.atan(cx / f)), + assumption="画素は正方形・光軸は画像の中心・レンズのゆがみなし・マット四隅の4点だけで解いた") + (OUT / "D").mkdir(parents=True, exist_ok=True) + json.dump(res, open(OUT / "D/camera_fit.json", "w"), indent=1, ensure_ascii=False) + print(json.dumps({k: res[k] for k in ("focal_px", "focal_from_homography_only_px", "fov_vertical_deg", "fov_horizontal_deg", + "cam_pos_unity_m", "cam_forward_unity", "reproj_max_px", "finger_reproj_px")}, indent=1)) + + +def blue_dots(frame): + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + m = cv2.inRange(hsv, np.array([95, 120, 80]), np.array([130, 255, 255])) + cnts, _ = cv2.findContours(m, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + out = [] + for c in cnts: + if cv2.contourArea(c) > 100: + (x, y), _r = cv2.minEnclosingCircle(c) + out.append((x, y)) + return out + + +def rot_to_quat(M): + t = M[0, 0] + M[1, 1] + M[2, 2] + if t > 0: + s = math.sqrt(t + 1.0) * 2 + w = 0.25 * s + x = (M[2, 1] - M[1, 2]) / s + y = (M[0, 2] - M[2, 0]) / s + z = (M[1, 0] - M[0, 1]) / s + elif M[0, 0] > M[1, 1] and M[0, 0] > M[2, 2]: + s = math.sqrt(1.0 + M[0, 0] - M[1, 1] - M[2, 2]) * 2 + w = (M[2, 1] - M[1, 2]) / s + x = 0.25 * s + y = (M[0, 1] + M[1, 0]) / s + z = (M[0, 2] + M[2, 0]) / s + elif M[1, 1] > M[2, 2]: + s = math.sqrt(1.0 + M[1, 1] - M[0, 0] - M[2, 2]) * 2 + w = (M[0, 2] - M[2, 0]) / s + x = (M[0, 1] + M[1, 0]) / s + y = 0.25 * s + z = (M[1, 2] + M[2, 1]) / s + else: + s = math.sqrt(1.0 + M[2, 2] - M[0, 0] - M[1, 1]) * 2 + w = (M[1, 0] - M[0, 1]) / s + x = (M[0, 2] + M[2, 0]) / s + y = (M[1, 2] + M[2, 1]) / s + z = 0.25 * s + return [x, y, z, w] + + +if __name__ == "__main__": + main() diff --git a/Assets/TwinCheck/Python~/twin_d.py b/Assets/TwinCheck/Python~/twin_d.py new file mode 100644 index 0000000..cd8bb0c --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_d.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +"""依頼107 D: 実機の上カメラの1コマと、合わせた Unity のカメラの1コマを並べる。 + + shots : python3 twin_d.py shots → D/shots_d.csv（Unity で撮る場面。アーム HOME・ブロックと箱は B で求めた位置） + compose : python3 twin_d.py compose → D/d_side_by_side_384x216.png（左: 実機 / 右: Unity、どちらも 384×216） + D/d_side_by_side_960x540.png・D/d_overlay_960x540.png（重ねた確認用） +色・光・模様は調整しない（Unity はシーンの光だけで撮る）。 +""" +import csv +import json +import sys +from pathlib import Path + +import cv2 +import numpy as np + +OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所") +RUNS = Path("/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy") + + +def shots(): + cam = json.load(open(OUT / "D/camera_fit.json")) + tr = {r["run"]: r for r in csv.DictReader(open(OUT / "B/trials.csv"))}[cam["run"]] + q = cam["cam_quat_unity_xyzw"] + p = cam["cam_pos_unity_m"] + base = dict(j0=0, j1=0, j2=0, j3=0, j4=0, j5=-30, grip=0, + block_x=tr["block_x_m"], block_z=tr["block_z_m"], block_yaw=tr["block_yaw_deg"] or 0, + box_x=tr["box_x_m"], box_z=tr["box_z_m"], box_yaw=tr["box_yaw_deg"] or 0, + view="cam", target_x=0, target_z=0, cam_x=p[0], cam_y=p[1], cam_z=p[2], + cam_qx=q[0], cam_qy=q[1], cam_qz=q[2], cam_qw=q[3], cam_fov=cam["fov_vertical_deg"]) + rows = [dict(base, out=str(OUT / "D/unity_home_1920x1080.png"), w=1920, h=1080), + dict(base, out=str(OUT / "D/unity_home_384x216.png"), w=384, h=216)] + with open(OUT / "D/shots_d.csv", "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=list(rows[0].keys())) + w.writeheader() + w.writerows(rows) + + +def compose(): + cam = json.load(open(OUT / "D/camera_fit.json")) + capv = cv2.VideoCapture(str(RUNS / cam["run"] / "recording/top.mp4")) + ok, real = capv.read() + capv.release() + cv2.imwrite(str(OUT / "D/real_home_1920x1080.jpg"), real) + uni = cv2.imread(str(OUT / "D/unity_home_1920x1080.png")) + real_s = cv2.resize(real, (384, 216), interpolation=cv2.INTER_AREA) + # 学習と同じ 384×216。Unity は 1920×1080 で撮ったものを同じやり方で縮める（384×216 で直接撮ったものも残す） + uni_s = cv2.resize(uni, (384, 216), interpolation=cv2.INTER_AREA) + cv2.imwrite(str(OUT / "D/real_home_384x216.png"), real_s) + cv2.imwrite(str(OUT / "D/d_side_by_side_384x216.png"), np.hstack([real_s, uni_s])) + big = np.hstack([cv2.resize(real, (960, 540)), cv2.resize(uni, (960, 540))]) + cv2.imwrite(str(OUT / "D/d_side_by_side_960x540.png"), big) + ov = cv2.addWeighted(cv2.resize(real, (960, 540)), 0.5, cv2.resize(uni, (960, 540)), 0.5, 0) + for (u, v) in cam["mat_corners_px"]: + cv2.circle(ov, (int(u / 2), int(v / 2)), 8, (0, 200, 255), 2) + for (u, v) in cam["finger_dots_px"]: + cv2.circle(ov, (int(u / 2), int(v / 2)), 8, (255, 0, 0), 2) + cv2.imwrite(str(OUT / "D/d_overlay_960x540.png"), ov) + + +if __name__ == "__main__": + {"shots": shots, "compose": compose}[sys.argv[1]]() diff --git a/Assets/TwinCheck/Python~/twin_overlay.py b/Assets/TwinCheck/Python~/twin_overlay.py new file mode 100644 index 0000000..0f81e6f --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_overlay.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +"""依頼107 B: Unity で撮った画に、場面の名前・数字・高さの目盛り（横から見た画だけ）を書き込む。 +元の PNG は残し、書き込んだものを同じフォルダに *_note.png で置く。 + +横から見た画は正射影で、画面の中心の高さ 0.10m・上下に 0.15m（TwinCheckB.SideCenterY / SideHalfHeight）。 +""" +import csv +from pathlib import Path + +from PIL import Image, ImageDraw, ImageFont + +OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所") +FONT = "/System/Library/Fonts/ヒラギノ角ゴシック W3.ttc" +SIDE_CENTER_Y = 0.10 +SIDE_HALF_H = 0.15 +SCENE = {"1_close": "爪を閉じる命令の瞬間", "2_lift": "持ち上げたあと（いちばん高い所）", "3_box": "箱にいちばん近づいたとき"} + + +def y_px(y_m, h): + return h / 2 - (y_m - SIDE_CENTER_Y) / SIDE_HALF_H * (h / 2) + + +def main(): + rows = {r["run"]: r for r in csv.DictReader(open(OUT / "B/b_table.csv"))} + shots = list(csv.DictReader(open(OUT / "B/shots_b.csv"))) + font = ImageFont.truetype(FONT, 22) + small = ImageFont.truetype(FONT, 18) + for s in shots: + p = Path(s["out"]) + if not p.exists(): + continue + run = p.parent.name + r = rows[run] + name = p.stem.rsplit("_", 1)[0] + view = p.stem.rsplit("_", 1)[1] + im = Image.open(p).convert("RGB") + d = ImageDraw.Draw(im) + w, h = im.size + if view == "side": + for y_m, label, col in ((0.0, "マット 0mm", (40, 40, 40)), (0.024, "ブロック上面 24mm", (170, 0, 0)), + (0.043, "箱の縁 43mm（仮）", (0, 70, 170))): + yy = y_px(y_m, h) + d.line([(0, yy), (w, yy)], fill=col, width=1) + d.text((w - 230, yy - 22), label, font=small, fill=col) + for mm in range(50, 260, 50): + yy = y_px(mm / 1000, h) + d.line([(0, yy), (14, yy)], fill=(60, 60, 60), width=2) + d.text((18, yy - 11), f"{mm}mm", font=small, fill=(60, 60, 60)) + lines = [f"{run}（{'失敗' if r['outcome'] == 'failure' else '成功'}） {SCENE.get(name, name)} t={float(s['t']):.1f}s {'横' if view == 'side' else '正面寄り'}", + f"爪: {'閉' if s['grip'] == '1' else '開'}"] + if name == "1_close": + lines.append(f"爪先の高さ {r['close_tip_h_mm']}mm / ブロックとの水平のずれ {r['close_tip_block_xz_mm']}mm") + elif name == "2_lift": + lines.append(f"爪先の高さ {r['lift_tip_h_mm']}mm（閉じたときから +{r['lift_rise_mm']}mm）") + else: + lines.append(f"箱の壁までの最短 {r['box_min_mm']}mm（{r['box_min_part']}）/ 箱の中に入った: {r['box_inside']}") + lines.append("Unity と実機の角度→位置は数十mm ずれうる。ブロックは最初の位置のまま描いている") + y = 8 + for i, t in enumerate(lines): + d.text((10, y), t, font=font if i == 0 else small, fill=(0, 0, 0)) + y += 28 if i == 0 else 23 + im.save(p.with_name(p.stem + "_note.png")) + + +if __name__ == "__main__": + main() diff --git a/Assets/TwinCheck/Python~/twin_prep.py b/Assets/TwinCheck/Python~/twin_prep.py new file mode 100644 index 0000000..eeaa06b --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_prep.py @@ -0,0 +1,370 @@ +#!/usr/bin/env python3 +"""依頼107 B の下ごしらえ（実機・daemon には触れない。記録を読むだけ）。 + +- 対象の試行を選ぶ（judgments の pick / place に人の判定が両方あるもの。失敗は全部・成功は5件） +- states.csv の event_type=actual（j0〜j5）と爪の命令（gripper_cmd）、補正の区間（stage=correct）を + 時刻の並びにして、Unity に渡す CSV（timeline.csv）にする +- top.mp4 の最初のコマから、マット四隅・赤ブロック・白箱を見つけ、マットの上の位置（Unity の m）に直す。 + 検出は r2r2r-scripts の本線と同じ関数を読み取り専用で借りる + （mat_corner_detection.detect_corners / 06f の evaluate_frame_gray と detect_box_center_by_contour） + +出力: {OUT}/B/trials.csv, {OUT}/B/timeline.csv, {OUT}/B/first_frames/{run}.jpg（検出位置に印） +""" +import csv +import json +import math +import os +import sys +from pathlib import Path + +import cv2 +import numpy as np + +RUNS = Path("/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy") +OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所") +SCRIPTS = Path.home() / "r2r2r-scripts" +PLACE_MODULES = SCRIPTS / "dynamic_tracking_limits/place/place_modules" + +# 成功から比べる5件：補正なしで成功した2件と、その他の日から1件ずつ（各日のいちばん早いもの） +SUCCESS_PICK_RULE = "補正なしの成功は全部、残りは補正ありの成功を日ごとに古い日から1件ずつ" +N_SUCCESS = 5 + +MAT_CORNERS_MM = [[310, -89], [-310, -89], [-310, 361], [310, 361]] # visual_servo_pick と同じ +BLOCK_MIN_DIST_FROM_BASE_MM = 120.0 # record_teach_demo と同じ（根元のそばの赤いケーブルを拾わない） +BOX_CONTOUR_TMPL_SANITY_MAX_PX = 60 # 06f と同じ +BOX_HEIGHT_MM_ASSUMED = 43.0 # 仮の値（本線のコメントの値） + +# 時間の並びを間引く：前に残した行から全関節の差が 0.3° 未満で、2 秒以内なら捨てる +KEEP_DEG = 0.3 +KEEP_SEC = 2.0 + + +def load_detectors(): + sys.path.insert(0, str(SCRIPTS)) + sys.path.insert(0, str(PLACE_MODULES)) + import importlib + import contextlib + with contextlib.redirect_stdout(sys.stderr): + mcd = importlib.import_module("mat_corner_detection") + m06f = importlib.import_module("06f_top_white_box_detect_v7") + templates = m06f._load_white_box_templates() + return mcd, m06f, templates + + +def select_trials(): + fails, succ = [], [] + for d in sorted(os.listdir(RUNS)): + p = RUNS / d / f"{d}.json" + if not p.exists(): + continue + j = json.load(open(p)) + jd = j.get("judgments") or {} + pk, pl = jd.get("pick") or {}, jd.get("place") or {} + if not (pk.get("human_verdict") and pl.get("human_verdict")): + continue + row = dict(run=d, outcome=j.get("outcome"), pick=pk.get("human_verdict"), + place=pl.get("human_verdict"), corrections=j.get("corrections"), + model=(j.get("args") or {}).get("model", "")) + (fails if j.get("outcome") == "failure" else succ).append(row) + chosen = [s for s in succ if s["corrections"] == 0] + days = {} + for s in succ: + if s in chosen: + continue + days.setdefault(s["run"][5:13], s) + for day in sorted(days): + if len(chosen) >= N_SUCCESS: + break + chosen.append(days[day]) + return fails, chosen, len(fails) + len(succ) + + +def first_frame(run): + cap = cv2.VideoCapture(str(RUNS / run / "recording/top.mp4")) + ok, frame = cap.read() + cap.release() + return frame if ok else None + + +def px_to_mm(H, px, py): + r = cv2.perspectiveTransform(np.array([[[float(px), float(py)]]], np.float32), H) + return float(r[0][0][0]), float(r[0][0][1]) + + +def yaw_from_pts(H, pts): + """画像上の長方形の1辺を mm に直し、Unity の Y 軸まわりの向き（度、0〜90）にする。""" + a = px_to_mm(H, *pts[0]) + b = px_to_mm(H, *pts[1]) + ang = math.degrees(math.atan2(b[1] - a[1], b[0] - a[0])) + return ang % 90.0 + + +def detect_box(frame, mat_bbox, H, m06f, templates): + import contextlib + with contextlib.redirect_stdout(sys.stderr): + res = m06f.evaluate_frame_gray(frame, mat_bbox, templates) + best = res.get("overall_best") + if best is None or best.get("best_score") is None: + return None, ["箱未検出（テンプレ）"] + score = float(best["best_score"]) + tcx, tcy = best["cx"], best["cy"] + note = [] + try: + with contextlib.redirect_stdout(sys.stderr): + cr = m06f.detect_box_center_by_contour(frame, mat_bbox) + except Exception as e: # 本線と同じく fallback + cr = None + note.append(f"輪郭で例外 {type(e).__name__}") + yaw = None + if cr is not None and math.hypot(cr["cx"] - tcx, cr["cy"] - tcy) <= BOX_CONTOUR_TMPL_SANITY_MAX_PX: + cx, cy, src = cr["cx"], cr["cy"], "contour" + yaw = yaw_from_pts(H, cr["rect_pts"]) + else: + cx, cy, src = tcx, tcy, "template_fallback" + note.append("輪郭が取れずテンプレ中心") + if score < 0.5: + note.append(f"テンプレのスコアが低い({score:.2f})") + return dict(px=(cx, cy), mm=px_to_mm(H, cx, cy), score=score, source=src, yaw=yaw), note + + +def red_candidates(frame, mat_bbox, H): + """赤の塊のうち、ブロックらしいもの（根元から120mm以上・縦横比1.8以下・外接長方形の埋まり0.6以上）。""" + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + m = cv2.inRange(hsv, np.array([0, 100, 100]), np.array([10, 255, 255])) | \ + cv2.inRange(hsv, np.array([160, 100, 100]), np.array([180, 255, 255])) + m = cv2.morphologyEx(m, cv2.MORPH_OPEN, np.ones((5, 5), np.uint8)) + cnts, _ = cv2.findContours(m, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + out = [] + for c in cnts: + a = cv2.contourArea(c) + if a < 500: + continue + (cx, cy), (rw, rh), _ang = rect = cv2.minAreaRect(c) + if not (mat_bbox[0] <= cx <= mat_bbox[1] and mat_bbox[2] <= cy <= mat_bbox[3]): + continue + aspect = max(rw, rh) / max(1e-6, min(rw, rh)) + extent = a / max(1e-6, rw * rh) + mm = px_to_mm(H, cx, cy) + if math.hypot(*mm) < BLOCK_MIN_DIST_FROM_BASE_MM or aspect > 1.8 or extent < 0.6: + continue + out.append(dict(area=a, px=(cx, cy), mm=mm, yaw=yaw_from_pts(H, cv2.boxPoints(rect).tolist()))) + return out + + +def detect_block_over_frames(run, mat_bbox, H, t_states0, close_ts): + """爪を閉じる命令より前のコマを順に見て、赤がいちばん大きく見えたコマでブロックの位置を取る。 + 最初のコマでは HOME の爪の真下に半分隠れていることが多いため。""" + log = list(csv.DictReader(open(RUNS / run / "recording/top_frame_log.csv"))) + t_limit = (close_ts if close_ts is not None else 1e9) + t_states0 + cap = cv2.VideoCapture(str(RUNS / run / "recording/top.mp4")) + best = None + n_seen = 0 + for k, row in enumerate(log): + ok, f = cap.read() + if not ok: + break + if float(row["capture_ts"]) > t_limit: + break + if k % 2: + continue + cands = red_candidates(f, mat_bbox, H) + if cands: + n_seen += 1 + c = max(cands, key=lambda c: c["area"]) + if best is None or c["area"] > best["area"]: + best = dict(c, frame=k, t=float(row["capture_ts"]) - t_states0) + cap.release() + note = [] + if best is None: + return None, ["閉じる前のコマでブロックが見つからない"] + if best["area"] < 1800: + note.append(f"ブロックの見え方が小さい({best['area']:.0f}px)・隠れている疑い") + return best, note + + +def detect_box_white(frame, mat_bbox, H, tmpl_px): + """白箱を白い四角の輪郭で取る（06f の輪郭検出が1個に絞れないことが多いため、ここではテンプレ中心に + いちばん近い四角を採る）。根元から150mm以内（腕）は外す。""" + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + m = cv2.inRange(hsv, np.array([0, 0, 165]), np.array([180, 60, 255])) + mask = np.zeros_like(m) + mask[mat_bbox[2]:mat_bbox[3], mat_bbox[0]:mat_bbox[1]] = 255 + m &= mask + k = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (9, 9)) + m = cv2.morphologyEx(cv2.morphologyEx(m, cv2.MORPH_CLOSE, k), cv2.MORPH_OPEN, k) + cnts, _ = cv2.findContours(m, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + cands = [] + for c in cnts: + a = cv2.contourArea(c) + if not (15000 <= a <= 120000): + continue + rect = cv2.minAreaRect(c) + (cx, cy), (rw, rh), _ = rect + aspect = max(rw, rh) / max(1e-6, min(rw, rh)) + extent = a / max(1e-6, rw * rh) + mm = px_to_mm(H, cx, cy) + if aspect > 1.5 or extent < 0.75 or math.hypot(*mm) < 150: + continue + d = math.hypot(cx - tmpl_px[0], cy - tmpl_px[1]) if tmpl_px else 0 + cands.append(dict(px=(cx, cy), mm=mm, area=a, aspect=aspect, extent=extent, d_tmpl=d, + yaw=yaw_from_pts(H, cv2.boxPoints(rect).tolist()), pts=cv2.boxPoints(rect).tolist())) + if not cands: + return None + return min(cands, key=lambda c: c["d_tmpl"]) + + +def build_timeline(run): + rows = list(csv.DictReader(open(RUNS / run / "recording/states.csv"))) + rows.sort(key=lambda r: float(r["ts"])) + t0 = float(rows[0]["ts"]) + stage = "model" + grip = 0 + out = [] + last = None + events = [] + for r in rows: + ts = float(r["ts"]) - t0 + et = r["event_type"] + if et == "stage": + if r["stage"] != stage: + events.append(("stage", ts, r["stage"])) + stage = r["stage"] + elif et == "gripper_cmd": + g = int(float(r["gripper_state"])) + events.append(("grip", ts, g)) + grip = g + last = None # 爪の変わり目は必ず残す + elif et == "actual": + try: + j = [float(r[f"j{i}"]) for i in range(6)] + except ValueError: + continue + keep = last is None or ts - last[0] >= KEEP_SEC or \ + max(abs(a - b) for a, b in zip(j, last[1])) >= KEEP_DEG + if keep: + out.append(dict(t=ts, j=j, grip=grip, stage=stage)) + last = (ts, j) + # 補正の区間 + corr = [] + cur = None + for kind, ts, v in events: + if kind != "stage": + continue + if v == "correct" and cur is None: + cur = ts + elif v != "correct" and cur is not None: + corr.append((cur, ts)) + cur = None + if cur is not None: + corr.append((cur, out[-1]["t"] if out else cur)) + grips = [(ts, v) for kind, ts, v in events if kind == "grip"] + return out, grips, corr, t0 + + +def main(): + mcd, m06f, templates = load_detectors() + fails, succs, n_judged = select_trials() + print(f"判定がそろった試行 {n_judged} 件 / 失敗 {len(fails)} 件 / 比べる成功 {len(succs)} 件", file=sys.stderr) + (OUT / "B/first_frames").mkdir(parents=True, exist_ok=True) + + trials_rows = [] + tl_rows = [] + for tr in fails + succs: + run = tr["run"] + frame = first_frame(run) + note = [] + block = box = None + mat_bbox = None + corners = mcd.detect_corners(frame) if frame is not None else None + if corners is not None: + corners = [[float(x), float(y)] for x, y in corners] + sharp = cv2.Laplacian(cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY), cv2.CV_64F).var() if frame is not None else 0 + if sharp < 20: + note.append(f"上カメラの映像がピンぼけ(鮮明さ{sharp:.0f})で検出しない") + corners = None + if corners is None: + note.append("マット四隅を検出できず") + H = None + else: + H = cv2.getPerspectiveTransform(np.array(corners, np.float32), np.array(MAT_CORNERS_MM, np.float32)) + mat_bbox = (int(min(corners[0][0], corners[3][0])), int(max(corners[1][0], corners[2][0])), + int(min(corners[0][1], corners[1][1])), int(max(corners[2][1], corners[3][1]))) + tmpl, n2 = detect_box(frame, mat_bbox, H, m06f, templates) + note += n2 + tl, grips, corr, t0 = build_timeline(run) + close_ts = next((ts for ts, g in grips if g == 1), None) + if H is not None: + block, n1 = detect_block_over_frames(run, mat_bbox, H, t0, close_ts) + note += n1 + wb = detect_box_white(frame, mat_bbox, H, tmpl["px"] if tmpl else None) + if wb is not None: + box = dict(px=wb["px"], mm=wb["mm"], yaw=wb["yaw"], score=tmpl["score"] if tmpl else float("nan"), + source="white_contour", d_tmpl=wb["d_tmpl"], pts=wb["pts"]) + if tmpl and wb["d_tmpl"] > 80: + note.append(f"白の四角とテンプレ中心が{wb['d_tmpl']:.0f}px離れている") + elif tmpl: + box = dict(tmpl, yaw=None) + note.append("白の四角が取れずテンプレ中心（箱の角に寄りがち）") + if frame is not None: + img = frame.copy() + if corners is not None: + cv2.polylines(img, [np.array(corners, np.int32)], True, (0, 200, 255), 3) + if block: + cv2.circle(img, tuple(int(v) for v in block["px"]), 25, (0, 0, 255), 4) + if box: + cv2.circle(img, tuple(int(v) for v in box["px"]), 12, (255, 0, 0), -1) + if box.get("pts"): + cv2.polylines(img, [np.array(box["pts"], np.int32)], True, (255, 0, 0), 4) + cv2.imwrite(str(OUT / f"B/first_frames/{run}.jpg"), cv2.resize(img, (960, 540))) + trials_rows.append(dict( + run=run, outcome=tr["outcome"], human_pick=tr["pick"], human_place=tr["place"], + corrections_json=tr["corrections"], model=tr["model"], + corners_px=json.dumps([[round(x, 1), round(y, 1)] for x, y in corners]) if corners else "", + block_x_m=f"{block['mm'][0] / 1000:.4f}" if block else "", + block_z_m=f"{block['mm'][1] / 1000:.4f}" if block else "", + block_yaw_deg=f"{block['yaw']:.1f}" if block else "", + block_frame=block["frame"] if block else "", block_t=f"{block['t']:.2f}" if block else "", + block_area_px=f"{block['area']:.0f}" if block else "", + box_x_m=f"{box['mm'][0] / 1000:.4f}" if box else "", + box_z_m=f"{box['mm'][1] / 1000:.4f}" if box else "", + box_yaw_deg=f"{box['yaw']:.1f}" if box and box["yaw"] is not None else "", + box_score=f"{box['score']:.3f}" if box else "", + box_source=box["source"] if box else "", + box_height_mm_assumed=BOX_HEIGHT_MM_ASSUMED, + close_t=f"{close_ts:.2f}" if close_ts is not None else "", + n_grip_cmd=len(grips), grip_cmds=json.dumps([[round(ts, 2), g] for ts, g in grips]), + n_correct=len(corr), correct_intervals=json.dumps([[round(a, 2), round(b, 2)] for a, b in corr]), + n_samples=len(tl), duration_s=f"{tl[-1]['t']:.1f}" if tl else "", + detect_note=" / ".join(note))) + for i, s in enumerate(tl): + tl_rows.append(dict(run=run, i=i, t=f"{s['t']:.3f}", **{f"j{k}": f"{s['j'][k]:.3f}" for k in range(6)}, + grip=s["grip"], stage=s["stage"])) + print(run, tr["outcome"], "block", trials_rows[-1]["block_x_m"], trials_rows[-1]["block_z_m"], + "box", trials_rows[-1]["box_x_m"], trials_rows[-1]["box_z_m"], trials_rows[-1]["box_source"], + "samples", len(tl), note, file=sys.stderr) + + # 検出できなかった試行は、ほかの試行の中央値で仮に置く（仮であることを列に残す） + for key in ("block", "box"): + xs = sorted(float(r[f"{key}_x_m"]) for r in trials_rows if r[f"{key}_x_m"]) + zs = sorted(float(r[f"{key}_z_m"]) for r in trials_rows if r[f"{key}_z_m"]) + for r in trials_rows: + r[f"{key}_pos_source"] = "detected" + if not r[f"{key}_x_m"] and xs: + r[f"{key}_x_m"] = f"{xs[len(xs) // 2]:.4f}" + r[f"{key}_z_m"] = f"{zs[len(zs) // 2]:.4f}" + r[f"{key}_pos_source"] = "median_of_other_trials(仮)" + with open(OUT / "B/trials.csv", "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=list(trials_rows[0].keys())) + w.writeheader() + w.writerows(trials_rows) + with open(OUT / "B/timeline.csv", "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=list(tl_rows[0].keys())) + w.writeheader() + w.writerows(tl_rows) + with open(OUT / "B/selection.json", "w") as f: + json.dump(dict(n_judged=n_judged, failures=[t["run"] for t in fails], + successes=[t["run"] for t in succs], success_rule=SUCCESS_PICK_RULE), f, ensure_ascii=False, indent=1) + + +if __name__ == "__main__": + main() diff --git a/Assets/TwinCheck/Python~/twin_table_b.py b/Assets/TwinCheck/Python~/twin_table_b.py new file mode 100644 index 0000000..f0e0719 --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_table_b.py @@ -0,0 +1,182 @@ +#!/usr/bin/env python3 +"""依頼107 B: 時刻ごとの測定（timeline_metrics.csv）から、試行ごとの表と分類の候補、撮る場面（shots_b.csv）を作る。 + +使う場面 + 閉じた瞬間 : 爪を閉じる命令（gripper_cmd=1 の最初）にいちばん近い記録の行 + 持ち上げたあと: 閉じたあと、j0 が閉じたときから 8° 以上回る（箱へ運び始める）か、次の開く命令か、 + 記録の終わりまでのうち、爪先がいちばん高かった行 + 箱にいちばん近い: 閉じたあと（閉じていなければ全部）で、爪先・爪・前腕のどれかが箱の壁にいちばん近づいた行 + +分類の候補（Unity と実機は数十 mm ずれうるので、成功5件の値の幅と比べて大きく外れたときだけ決める） + つかむ前に外れた : 閉じた瞬間の爪先の高さか、ブロックとの水平のずれが、成功の最大より 15mm 以上大きい + 箱に当たった : 閉じたあとに、爪先・爪・前腕が箱の壁に 10mm より深くめり込む（距離 < -10mm）。 + ±10mm 以内は「縁すれすれ」として根拠の欄に書くだけにする（数 mm の差で決めない） + つかんだが持ち上げで落ちた : 上の2つに当たらず、閉じたあとに爪先が 30mm 以上持ち上がっている（人の判定は pick=no） + 分からない : 爪を閉じる命令がない・記録が足りない など +""" +import csv +import json +import math +import statistics as st +from pathlib import Path + +OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所") +MARGIN_MM = 15.0 +LIFT_MIN_MM = 30.0 +BOX_HIT_MM = -10.0 +BOX_NEAR_MM = 10.0 +J0_TRANSPORT_DEG = 8.0 + + +def f(v): + return float(v) if v not in ("", None) else None + + +def main(): + trials = list(csv.DictReader(open(OUT / "B/trials.csv"))) + tl = {} + for r in csv.DictReader(open(OUT / "B/timeline.csv")): + tl.setdefault(r["run"], []).append(r) + met = {} + for r in csv.DictReader(open(OUT / "B/timeline_metrics.csv")): + met.setdefault(r["run"], []).append(r) + + rows = [] + shots = [] + for tr in trials: + run = tr["run"] + T = tl.get(run, []) + M = met.get(run, []) + grips = json.loads(tr["grip_cmds"] or "[]") + corr = json.loads(tr["correct_intervals"] or "[]") + close_t = f(tr["close_t"]) + res = dict(run=run, outcome=tr["outcome"], human_pick=tr["human_pick"], human_place=tr["human_place"], + model=tr["model"].split(" ")[0], block_pos_source=tr["block_pos_source"], + box_pos_source=tr["box_pos_source"], detect_note=tr["detect_note"]) + idx_close = idx_lift = idx_box = None + if close_t is not None and M: + idx_close = min(range(len(M)), key=lambda i: abs(float(M[i]["t"]) - close_t)) + m = M[idx_close] + res.update(close_t_s=round(close_t, 1), close_tip_h_mm=f(m["tip_h_mm"]), close_tip_block_xz_mm=f(m["tip_block_xz_mm"]), + close_grip_block_mm=f(m["d_block_grip_mm"])) + j0c = float(T[idx_close]["j0"]) + next_open = next((g[0] for g in grips if g[1] == 0 and g[0] > close_t), None) + end = len(M) + for i in range(idx_close, len(M)): + if abs(float(T[i]["j0"]) - j0c) >= J0_TRANSPORT_DEG or (next_open is not None and float(M[i]["t"]) >= next_open): + end = i + break + win = range(idx_close, max(end, idx_close + 1)) + idx_lift = max(win, key=lambda i: float(M[i]["tip_h_mm"])) + res.update(lift_t_s=round(float(M[idx_lift]["t"]), 1), lift_tip_h_mm=f(M[idx_lift]["tip_h_mm"]), + lift_rise_mm=round(f(M[idx_lift]["tip_h_mm"]) - res["close_tip_h_mm"], 1)) + after = range(idx_close, len(M)) + else: + after = range(len(M)) + best = None + for i in after: + for part in ("tip", "gripper", "forearm"): + d = f(M[i][f"d_box_{part}_mm"]) + if d is not None and (best is None or d < best[0]): + best = (d, i, part) + inside_any = [i for i in after if M[i]["in_box_tip"] not in ("", "0") or M[i]["in_box_gripper"] not in ("", "0") + or M[i]["in_box_forearm"] not in ("", "0")] + if best: + idx_box = best[1] + m = M[idx_box] + res.update(box_min_mm=round(best[0], 1), box_min_part=best[2], box_min_t_s=round(float(m["t"]), 1), + box_d_tip_mm=f(m["d_box_tip_mm"]), box_d_gripper_mm=f(m["d_box_gripper_mm"]), box_d_forearm_mm=f(m["d_box_forearm_mm"])) + res["box_inside"] = "yes" if inside_any else "no" + res["box_inside_depth_mm"] = max((f(M[i]["in_box_depth_mm"]) or 0) for i in inside_any) if inside_any else 0.0 + res["n_correct"] = len(corr) + res["correct_intervals_s"] = ";".join(f"{a:.0f}-{b:.0f}" for a, b in corr) + res["correct_before_close"] = "yes" if close_t is not None and any(a < close_t for a, b in corr) else "no" + res["n_grip_close_cmds"] = sum(1 for g in grips if g[1] == 1) + res["_idx"] = (idx_close, idx_lift, idx_box) + rows.append(res) + + succ = [r for r in rows if r["outcome"] == "success" and r.get("close_tip_h_mm") is not None] + ref_h = max(r["close_tip_h_mm"] for r in succ) + ref_xz = max(r["close_tip_block_xz_mm"] for r in succ) + for r in rows: + why = [] + if r.get("close_tip_h_mm") is None: + cat = "分からない" + why.append("爪を閉じる命令の記録がない") + else: + miss_h = r["close_tip_h_mm"] > ref_h + MARGIN_MM + miss_xz = r["close_tip_block_xz_mm"] > ref_xz + MARGIN_MM + hit = r.get("box_min_mm") is not None and r["box_min_mm"] < BOX_HIT_MM + near = r.get("box_min_mm") is not None and abs(r["box_min_mm"]) <= BOX_NEAR_MM + lifted = r.get("lift_rise_mm") is not None and r["lift_rise_mm"] >= LIFT_MIN_MM + if miss_h or miss_xz: + cat = "つかむ前に外れた" + if miss_h: + why.append(f"閉じた瞬間の爪先の高さ {r['close_tip_h_mm']:.0f}mm > 成功の最大 {ref_h:.0f}+{MARGIN_MM:.0f}") + if miss_xz: + why.append(f"閉じた瞬間のブロックとの水平のずれ {r['close_tip_block_xz_mm']:.0f}mm > 成功の最大 {ref_xz:.0f}+{MARGIN_MM:.0f}") + elif hit: + cat = "箱に当たった" + why.append(f"{r['box_min_part']}が箱の壁に {-r['box_min_mm']:.0f}mm めり込む（{r['box_min_t_s']}s）") + near = False + elif lifted: + cat = "つかんだが持ち上げで落ちた" + why.append(f"閉じた瞬間は成功の幅の中（高さ {r['close_tip_h_mm']:.0f}・ずれ {r['close_tip_block_xz_mm']:.0f}mm）、" + f"そのあと {r['lift_rise_mm']:.0f}mm 持ち上げ、箱には当たらない。人の判定は pick={r['human_pick']}") + else: + cat = "分からない" + why.append(f"閉じた瞬間は成功の幅の中、そのあとの持ち上げが {r.get('lift_rise_mm')}mm しかない（j0 を回し始めるまで・記録の終わりまで）") + if near: + why.append(f"{r['box_min_part']}が箱の縁すれすれ（{r['box_min_mm']:+.0f}mm、{r['box_min_t_s']}s。±{BOX_NEAR_MM:.0f}mm 以内なので当たったとは決めない）") + if hit and cat != "箱に当たった": + why.append(f"（ほかに、{r['box_min_part']}が箱の壁にめり込む {-r['box_min_mm']:.0f}mm）") + # 成功は比べるためだけに載せる（分類の名前は付けない） + r["category"] = cat if r["outcome"] == "failure" else "（成功・比べる用）" + r["category_basis"] = " / ".join(why) + + cols = ["run", "outcome", "human_pick", "human_place", "model", "category", "category_basis", + "close_t_s", "close_tip_h_mm", "close_tip_block_xz_mm", "close_grip_block_mm", + "lift_t_s", "lift_tip_h_mm", "lift_rise_mm", + "box_min_mm", "box_min_part", "box_min_t_s", "box_d_tip_mm", "box_d_gripper_mm", "box_d_forearm_mm", + "box_inside", "box_inside_depth_mm", + "n_correct", "correct_intervals_s", "correct_before_close", "n_grip_close_cmds", + "block_pos_source", "box_pos_source", "detect_note"] + with open(OUT / "B/b_table.csv", "w", newline="") as fo: + w = csv.DictWriter(fo, fieldnames=cols, extrasaction="ignore") + w.writeheader() + w.writerows(rows) + + # 撮る場面 + trmap = {t["run"]: t for t in trials} + for r in rows: + tr = trmap[r["run"]] + T = tl[r["run"]] + base = dict(block_x=tr["block_x_m"], block_z=tr["block_z_m"], block_yaw=tr["block_yaw_deg"] or "0", + box_x=tr["box_x_m"], box_z=tr["box_z_m"], box_yaw=tr["box_yaw_deg"] or "0", w=960, h=720) + for name, idx, tgt, views in (("1_close", r["_idx"][0], "block", ("side", "front")), + ("2_lift", r["_idx"][1], "block", ("side",)), + ("3_box", r["_idx"][2], "box", ("side", "front"))): + if idx is None: + continue + s = T[idx] + tx, tz = (tr["block_x_m"], tr["block_z_m"]) if tgt == "block" else (tr["box_x_m"], tr["box_z_m"]) + for v in views: + shots.append(dict(base, out=str(OUT / f"B/images/{r['run']}/{name}_{v}.png"), + **{f"j{k}": s[f"j{k}"] for k in range(6)}, grip=s["grip"], view=v, + target_x=tx, target_z=tz, t=s["t"])) + with open(OUT / "B/shots_b.csv", "w", newline="") as fo: + w = csv.DictWriter(fo, fieldnames=list(shots[0].keys())) + w.writeheader() + w.writerows(shots) + + fails = [r for r in rows if r["outcome"] == "failure"] + from collections import Counter + print("成功の基準: 閉じた瞬間の高さ最大", ref_h, "ずれ最大", ref_xz) + print(Counter(r["category"] for r in fails)) + for r in rows: + print(r["run"], r["outcome"], r["category"], r.get("close_tip_h_mm"), r.get("close_tip_block_xz_mm"), + r.get("lift_rise_mm"), r.get("box_min_mm"), r.get("box_min_part"), r["box_inside"], r["n_correct"]) + + +if __name__ == "__main__": + main() `````` ### 依頼111：作業用のシーンの写しを残さない形にする 依頼107で作った作業用のシーンは、元のシーンとバイト単位で同じでした。元のシーンの7/17の状態は Unity Version Control にチェックイン済みだったので、写しを記録に入れても守るものは増えません。写しは KIOXIA にコードと一緒に置き、動かすたびに元のシーンを一時フォルダへ写して開く形にしました。直したあとにやり直した結果は、前と差0でした（14:22〜14:26）。 #### 依頼111 ``````markdown --- 作業フォルダ: /Users/shimazueiichi/Robotics_UX/Robotics_UX_New 上限時間: 60 --- # TwinCheck のシーンの写しを残さない形にする（依頼107 の続き・2026-11-07） 依頼107 で作った `Assets/TwinCheck/TwinCheck_Base.unity` は `Assets/Scenes/R2R2R-20260224.unity` とバイト単位で同じで、元のシーンの 7/17 の状態は Unity Version Control にチェックイン済み（`.plastic/plastic.wktree` の指紋と一致）。写しを記録に入れても守るものが増えず、同じシーンが2つ並ぶと迷うので、エイイチの決め（2026-11-07）で次の形にする。 **実機には触れない。** 元のシーン `Assets/Scenes/R2R2R-20260224.unity` は読むだけ（開かない・保存しない）。ArticulationBody の Stiffness・Damping は変えない。Unity の Editor でこのプロジェクトが開いていたら何もせず止める。 ## やること 1. **写しを KIOXIA に残す。** 今の `Assets/TwinCheck/TwinCheck_Base.unity` を `/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/code/` に写す。ファイル名には元のシーンの名前と Unity Version Control の版を入れる（例 `R2R2R-20260224_cs{番号}.unity`）。版は `cm` で、元のシーンのこの内容が入った changeset を調べる 2. **動かすたびに元のシーンを写して開く形にする。** `TwinCheckA.ScenePath` を使う所（`TwinCheckA.cs`・`TwinScene.cs`・`TwinCheckDump.cs`）を、元のシーンを `Assets/TwinCheck/_work/TwinCheck_Base.unity` に写してから開く1つの関数にまとめる（Unity は Assets の外のシーンを開けないため、`_work/` に置く）。写すたびに元のシーンの MD5 を出力フォルダの `scene_source.json` に書く（元のパス・MD5・調べられれば changeset） 3. **`_work/` を記録に入れない。** `.gitignore` と Unity Version Control の `ignore.conf` の最後に `Assets/TwinCheck/_work/` と `Assets/TwinCheck/_work.meta` と `Assets/TwinCheck/Python~/__pycache__/` の行を足す（今ある行は変えない） 4. **今の写しを外す。** `Assets/TwinCheck/TwinCheck_Base.unity` と `.meta` を `Assets/TwinCheck/_work/` へ移す（消さなくてよい。2 で上書きされる） 5. **同じ結果が出るか確かめる。** D（`TwinCheckD.Points` → `twin_camera_d.py`）だけをやり直し、出力を一時フォルダに書いて、依頼107 の `D/home_points_unity.json`・`D/camera_fit.json` と値が同じか（差の最大）を比べる。KIOXIA の依頼107 の結果は上書きしない 6. **記録する。** 変えた TwinCheck のファイルと `.gitignore` を1つずつ指定して git に commit、同じものと `ignore.conf` を Unity Version Control にチェックインする（ほかの変更は含めない）。KIOXIA の README.md の「使ったコード」の節の下に「シーンの扱い（2026-11-07 変更）」を足し、写しの置き場所・版・やり直し方の変わった所を書く ## 報告 KIOXIA に写したファイル名と changeset、5 の差の最大、commit と changeset の番号だけを書く。最後の1行に「判定：合格／止めた（決めてほしいことあり）／失敗」を書く。 `````` #### 依頼111の報告 ``````markdown # 結果：20261107_111_TwinCheckのシーンの写しを残さない形にする - 状態：完了 - 始めた時刻：2026-11-07 14:22 - 終わった時刻：2026-11-07 14:26（4分） - 作業フォルダ：/Users/shimazueiichi/Robotics_UX/Robotics_UX_New - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 7de262b9-5bd6-4e28-b0eb-7c6e50c05f62` ## Claude Code の報告 やったこと： - 始める前に、Unity の Editor が動いていないことを確かめました（動いていたのは Unity Hub だけ）。元のシーンは読んだだけで、開いても保存してもいません。MD5 は作業の前も後も `b1613505…` で同じです。実機と ArticulationBody の値には触れていません。 - 依頼107 の写しを KIOXIA の `code/` に写しました。Unity Version Control の各版の中身の MD5 を元のシーンと比べ、一致した版の番号をファイル名に入れました。 - `TwinCheckA.OpenWorkScene` を作り、`TwinScene.Open`・`TwinCheckA.Run`・`TwinCheckDump.RunScene` がみなこの関数を通るようにしました。この関数は、元のシーンを `_work/` に写してから開き、出力フォルダに `scene_source.json`（元のパス・MD5・changeset）を書きます。 - `.gitignore` と `ignore.conf` の最後に行を足し、今の写しと `.meta` を `_work/` へ移しました。 - D をやり直して一時フォルダ `/tmp/twin111/out` に書き、依頼107 の結果と比べました。KIOXIA の依頼107 の結果は上書きしていません。 - git に commit し、Unity Version Control にチェックインしました。どちらも指定したファイルだけを入れています。KIOXIA の README に「シーンの扱い（2026-11-07 変更・依頼111）」の節を足しました。 結果： - KIOXIA に写したファイル：`code/R2R2R-20260224_cs110.unity`（changeset は **cs:110**、2026-07-17。MD5 が一致） - 5 の差の最大：`home_points_unity.json` は 33 個の値で **0**（ファイルもバイト単位で同じ）、`camera_fit.json` は 78 個の値で **0**。数でない項目にも違いはありません。 - `scene_source.json` には changeset 110 が出ました。 - git の commit は **736b3d3**、Unity Version Control の changeset は **cs:146** です。 変えたファイル： - `Assets/TwinCheck/Editor/TwinCheckA.cs`・`TwinScene.cs`・`TwinCheckDump.cs` - `.gitignore`、`ignore.conf`（Unity Version Control だけ） - `Assets/TwinCheck/TwinCheck_Base.unity` と `.meta`（`_work/` へ移した） - KIOXIA の `README.md` と `code/R2R2R-20260224_cs110.unity` 決めてほしいこと： - `ignore.conf` に依頼文どおりの行（先頭に `/` がない形）を足すと、Unity Version Control ではその行が効かず、`_work/` などが記録に入る側のままでした。そこで、今ある行（`/Assets/ServiceSim/Store/old~`）と同じ形で `/Assets/TwinCheck/_work`・`/Assets/TwinCheck/_work.meta`・`/Assets/TwinCheck/Python~/__pycache__` と書き、cs:146 に入れました。こう書くと記録に入らなくなることを確かめています。`.gitignore` は依頼文どおりの行のままで、効いています。この書き方でよいか確かめてください。 - KIOXIA の `code/` にある `.cs` は依頼107 の時点のまま残し、上書きしていません。新しいコードは git の 736b3d3 にあると README に書きました。`code/` の `.cs` も新しくするか決めてください。 - git のほうの `ignore.conf` には、前からある commit していない変更（`.git` などの2行）が残っています。今回の commit には入れていません。 判定：止めた（決めてほしいことあり） `````` #### Robotics_UX_New commit 736b3d3 ``````diff commit 736b3d3bb8e62ad214895b0664483913c11f41b1 Author: Shimazue Eiichi Date: Sat Nov 7 14:25:45 2026 +0900 [依頼111] TwinCheck のシーンの写しを記録に残さず、動かすたびに元のシーンから写して開く形にする - TwinCheckA.OpenWorkScene: 元のシーン Assets/Scenes/R2R2R-20260224.unity を Assets/TwinCheck/_work/TwinCheck_Base.unity に写して開く（保存しない）。 元のパス・MD5・changeset（Unity Version Control の版の中身と MD5 が一致したときだけ）を 出力フォルダの scene_source.json に書く - TwinScene.Open・TwinCheckA.Run・TwinCheckDump.RunScene をこの関数に揃えた - .gitignore に _work/・_work.meta・Python~/__pycache__/ を足した Co-Authored-By: Claude Opus 5.5 diff --git a/.gitignore b/.gitignore index 09a16a6..6805b63 100644 --- a/.gitignore +++ b/.gitignore @@ -46,3 +46,6 @@ Packages/packages-lock.json Assets/GaussianAssets/*.bytes .tmp.driveupload/ +Assets/TwinCheck/_work/ +Assets/TwinCheck/_work.meta +Assets/TwinCheck/Python~/__pycache__/ diff --git a/Assets/TwinCheck/Editor/TwinCheckA.cs b/Assets/TwinCheck/Editor/TwinCheckA.cs index 0b4d62d..ad87476 100644 --- a/Assets/TwinCheck/Editor/TwinCheckA.cs +++ b/Assets/TwinCheck/Editor/TwinCheckA.cs @@ -10,12 +10,90 @@ using System.Globalization; /// TwinCheckA /// 依頼107 の A。FK テーブルから選んだ行の関節角を、シーンの写しの20体それぞれに置き、 /// 爪先の位置（根元からの差）を書き出す。表の ee との比較は Python 側で行う。 -/// シーンは開くだけで保存しない。 +/// シーンは開くだけで保存しない。シーンの写しは OpenWorkScene で毎回作る（依頼111）。 /// 実行: Unity -batchmode -quit -executeMethod TwinCheckA.Run -twinIn {行の CSV} -twinOut {出力 CSV} [-twinSign 1|-1] /// public static class TwinCheckA { - public const string ScenePath = "Assets/TwinCheck/TwinCheck_Base.unity"; + // 元のシーン（読むだけ）と、動かすたびに作る写し（_work/ は記録に入れない。Unity は Assets の外のシーンを開けない） + public const string SourceScenePath = "Assets/Scenes/R2R2R-20260224.unity"; + public const string ScenePath = "Assets/TwinCheck/_work/TwinCheck_Base.unity"; + + /// + /// 元のシーンを _work/ に写して開く（保存しない）。元のパス・MD5・changeset を + /// 出力フォルダ（-twinOut、なければ -twinShots のあるフォルダ）の scene_source.json に書く。 + /// changeset は Unity Version Control で読み込んでいる版の中身と MD5 が一致したときだけ書く。 + /// + public static void OpenWorkScene() + { + Directory.CreateDirectory(Path.GetDirectoryName(ScenePath)); + File.Copy(SourceScenePath, ScenePath, true); + AssetDatabase.ImportAsset(ScenePath, ImportAssetOptions.ForceUpdate); + string md5 = Md5(SourceScenePath); + string cs = Changeset(SourceScenePath, md5, out string note); + + string outArg = TwinCheckArgs.Get("-twinOut", ""); + if (outArg == "") outArg = TwinCheckArgs.Get("-twinShots", ""); + if (outArg != "") + { + string dir = Path.GetDirectoryName(Path.GetFullPath(outArg)); + Directory.CreateDirectory(dir); + var sb = new StringBuilder("{\n"); + sb.Append($" \"source\": \"{SourceScenePath}\",\n"); + sb.Append($" \"md5\": \"{md5}\",\n"); + sb.Append($" \"changeset\": {(cs == null ? "null" : cs)},\n"); + sb.Append($" \"changeset_note\": \"{note}\",\n"); + sb.Append($" \"copied_to\": \"{ScenePath}\",\n"); + sb.Append($" \"copied_at\": \"{System.DateTime.Now:yyyy-MM-ddTHH:mm:ss}\"\n"); + sb.Append("}\n"); + File.WriteAllText(Path.Combine(dir, "scene_source.json"), sb.ToString()); + } + EditorSceneManager.OpenScene(ScenePath, OpenSceneMode.Single); + Debug.Log($"[TwinCheck] scene {SourceScenePath} md5={md5} cs={cs ?? "-"} ({note})"); + } + + static string Md5(string path) + { + using (var m = System.Security.Cryptography.MD5.Create()) + using (var f = File.OpenRead(path)) + return System.BitConverter.ToString(m.ComputeHash(f)).Replace("-", "").ToLowerInvariant(); + } + + /// cm で読み込んでいる版の番号を調べ、その版の中身の MD5 が今のファイルと同じなら番号を返す。 + static string Changeset(string path, string md5, out string note) + { + string full = Path.GetFullPath(path); + string cs = Cm($"ls \"{full}\" --format={{changeset}}").Trim(); + if (!int.TryParse(cs, out _)) { note = "cm で版を調べられなかった"; return null; } + string tmp = Path.Combine(Path.GetTempPath(), $"twincheck_scene_cs{cs}.unity"); + Cm($"cat \"{full}#cs:{cs}\" --file=\"{tmp}\""); + if (!File.Exists(tmp)) { note = $"cs:{cs} の中身を取り出せなかった"; return null; } + bool same = Md5(tmp) == md5; + File.Delete(tmp); + if (!same) { note = $"読み込んでいる版 cs:{cs} から変更あり（チェックインされていない）"; return null; } + note = $"cs:{cs} の中身と MD5 が一致"; + return cs; + } + + static string Cm(string args) + { + foreach (var exe in new[] { "/usr/local/bin/cm", "cm" }) + { + try + { + var psi = new System.Diagnostics.ProcessStartInfo(exe, args) + { RedirectStandardOutput = true, RedirectStandardError = true, UseShellExecute = false }; + using (var p = System.Diagnostics.Process.Start(psi)) + { + string o = p.StandardOutput.ReadToEnd(); + p.WaitForExit(60000); + return o; + } + } + catch (System.Exception) { } + } + return ""; + } public static void Run() { @@ -23,7 +101,7 @@ public static class TwinCheckA string outPath = TwinCheckArgs.Get("-twinOut", ""); float sign = float.Parse(TwinCheckArgs.Get("-twinSign", "1"), CultureInfo.InvariantCulture); - EditorSceneManager.OpenScene(ScenePath, OpenSceneMode.Single); + OpenWorkScene(); var arms = FindArms(); var lines = File.ReadAllLines(inPath); diff --git a/Assets/TwinCheck/Editor/TwinCheckDump.cs b/Assets/TwinCheck/Editor/TwinCheckDump.cs index 67b540f..4144ecb 100644 --- a/Assets/TwinCheck/Editor/TwinCheckDump.cs +++ b/Assets/TwinCheck/Editor/TwinCheckDump.cs @@ -48,7 +48,7 @@ public static class TwinCheckDump { string outPath = TwinCheckArgs.Get("-twinOut", "/tmp/twincheck/scene.txt"); var sb = new StringBuilder(); - EditorSceneManager.OpenScene(TwinCheckA.ScenePath, OpenSceneMode.Single); + TwinCheckA.OpenWorkScene(); foreach (var go in UnityEngine.SceneManagement.SceneManager.GetActiveScene().GetRootGameObjects()) { var t = go.transform; diff --git a/Assets/TwinCheck/Editor/TwinScene.cs b/Assets/TwinCheck/Editor/TwinScene.cs index 564a578..ee2a600 100644 --- a/Assets/TwinCheck/Editor/TwinScene.cs +++ b/Assets/TwinCheck/Editor/TwinScene.cs @@ -1,11 +1,10 @@ using UnityEngine; -using UnityEditor.SceneManagement; using System.Globalization; using System.IO; /// /// TwinScene -/// 依頼107。シーンの写し（Assets/TwinCheck/TwinCheck_Base.unity）を開き、 +/// 依頼107。元のシーンの写し（TwinCheckA.OpenWorkScene が毎回 Assets/TwinCheck/_work/ に作る）を開き、 /// 1体目のアーム（mecharm270mpi_1_0、根元が原点）・赤ブロック・白箱・床・光だけを残して見せる。 /// 他の物は SetActive(false) にするだけで、シーンは保存しない。 /// @@ -16,7 +15,7 @@ public class TwinScene public static TwinScene Open() { - EditorSceneManager.OpenScene(TwinCheckA.ScenePath, OpenSceneMode.Single); + TwinCheckA.OpenWorkScene(); var s = new TwinScene(); var keep = new[] { "mecharm270mpi_1_0", "RedBlock_arm1", "WhiteBox_arm1", "Plane1", "Directional Light", "Main Camera" }; foreach (var go in UnityEngine.SceneManagement.SceneManager.GetActiveScene().GetRootGameObjects()) `````` ## 爪を閉じたとき、ブロックが爪の間に十分入っていなかった（依頼110） Unity では、失敗と成功で、爪を閉じた瞬間の爪先の位置に差が見えませんでした。そこで記録そのものを細かく読みました（15:01〜15:31）。 | 問い | 答え | |---|---| | 閉じたあと、π0.5 は持ち上げを出していたか | 12件とも出していた。範囲の制限で削られたものは0。持ち上げが少なく見えたのは、浅くつかんでいるのを見て人が途中で止めていたため | | 閉じたとき、アームは止まっていたか | ほぼ止まっていた。爪の命令のあとに送信が無視されて固まった件（LOG352 の形）は0 | | 閉じた瞬間、ブロックは爪の間にあったか | 7件は浅い（ブロックが爪先の印の線から12〜15mmはみ出したまま閉じた）。2件は空振り（爪がブロックの手前で閉じた）。3件は成功と見分けがつかない | 【画像】LOG460_03_grasp_failures.jpg π0.5 の失敗12件の、爪を閉じる直前・閉じて1秒後・持ち上げた後の上カメラの切り出し。多くはブロックが爪先からはみ出したまま閉じている 【画像】LOG460_04_grasp_successes.jpg π0.5 の成功の同じ切り出し。ブロックが爪の奥まで入ってから閉じている 同じ依頼で、series2 の本線（視覚サーボ）の上カメラのずれも調べました。上カメラは毎回付け外ししていて、10/19 からはマットの上の端が画像から切れています。本線は画像の上の端をマットの上の角（根元から89mm先）として扱っていたので、位置が4〜19mmずれる計算でした。補正記憶は10/16が最後の更新で、このずれは入っていません。π0.5 は画像をそのまま見て動くので、こちらの影響は受けません。 ### 依頼110：π0.5 の失敗のつかみと持ち上げを記録から調べ、上カメラの上の端のずれを調べる ``````markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 150 --- # π0.5 の失敗12件のつかみと持ち上げを記録から調べる／本線の上カメラの上の端のずれを調べる（2026-11-07） **読むだけの依頼。** コードは変えない。実機・Raspberry Pi・daemon・SSH・シリアルには触れない。KIOXIA の `runs/`・`datasets/`・`mecharm_calibration/` は読むだけ。 書いてよいのは `/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/E_つかみと持ち上げ/` と `F_上カメラの上の端/` の中だけ（調べるためのスクリプトもここに置く。r2r2r-scripts には置かない）。KIOXIA がつながっていなければ止める。 ## これまで分かったこと（依頼107。`lab/twin/20261107_失敗の再生と置き場所/README.md`） - 推論の31試行は、どれもブロックをほぼ同じ1か所（教えた位置5のあたり）に置いていた。失敗12件は学習した位置で起きている - 失敗12件のうち、つかむ前に外れたものは0。7件は爪を閉じて 39〜185mm 持ち上げたのにブロックを持てていない。5件は閉じたあと 2〜15mm しか持ち上げていない（成功は 26〜36mm 持ち上げてから j0 を回す）。箱に当たったものは0 - 閉じた瞬間の爪先の位置は、成功も失敗も同じくらいの幅で、Unity では見分けられなかった - 対象の試行の一覧は `B/selection.json`、1件ごとの値は `B/b_table.csv` ## E. つかみと持ち上げを記録から調べる 対象：失敗12件と、比べる成功5件（`B/selection.json` と同じ）。各試行の `runs/pi05_policy/{run}/` の `log.jsonl`（推論のかたまりごとの `actions`・`state`・`state_source`・`measured_state`）、`recording/states.csv`（送った指令と届いた角度・爪の命令）、`{run}.json`、`recording/top.mp4`・`frames/`。読むコードは `pi05/run_policy_client.py`。 1. **爪を閉じたあとに、π0.5 は持ち上げを出していたか。** 閉じる命令のあとの数かたまりについて、π0.5 が出した行動（生の値）・範囲の制限（`--raw-dir` の学習データの範囲＋余裕）をかけたあとの値・実際に送った値・届いた角度を並べる。持ち上げ（肩 j1 を戻す・爪先が上がる向き）を出していたのに、制限で削られた／送ったのに届かなかった／そもそも出していない、のどれかを試行ごとに分ける。とくに持ち上げの少ない5件 2. **爪を閉じたとき、アームは止まっていたか。** 閉じる命令の前後で、届いた角度が動き続けていなかったか。爪の命令のあとに送信が無視されて固まる件（LOG352）に当たっていないか 3. **π0.5 に渡した状態が実際とずれていなかったか。** `state_source` が `target`（前に送った目標を状態として渡す）のかたまりで、`measured_state` との差がどれくらいあったか。閉じる直前と直後で見る 4. **閉じた瞬間、ブロックは爪の間にあったか。** 上カメラの、閉じる命令の直前と直後のコマを17件並べた一覧画像を作る（ブロックと爪のあたりを切り出して大きく）。成功と失敗で見え方に違いがあるかを書く。上からの画で言えないことは「言えない」と書く 5. **爪の開き具合の記録があるか**（爪の値の読み返しなど）。あれば成功と失敗で比べる。なければ、ないと書く 6. まとめ：失敗12件それぞれについて、いちばんありそうな原因を1つ（持ち上げを出していない・制限で削られた・送っても届かない・動きながら閉じた・爪の間に入っていない・分からない）と、その根拠の数字 ## F. 本線の上カメラの上の端のずれを調べる 依頼107 の D で、上カメラの画像の上の端はマットの z≈−55mm に当たり、本線の決め（マットの上の2隅を (±310, −89) mm とする。`visual_servo_pick` まわり）と約 34mm 違った。画像の上の端でマットが切れていて、本当の角が写っていないのに、上の端の点を本当の角として扱っている疑いがある。 1. 本線で、マットの四隅をどう検出し、どの点を (±310, −89) に対応させているかを、ファイル名と行番号で示す 2. 上カメラは毎回付け外ししている。`runs/run_pick_place/`・`runs/pi05_policy/`・`datasets/` の最初のコマを日ごとに1枚ずつ見て、マットの上の端が画像の中に写っている日と、切れている日を分ける（四隅の検出の座標で判断する。v≈0 なら切れている） 3. 切れている日に本線で測った位置が、どれくらいずれる計算になるかを、マットの中の何点か（教えた位置5のあたり・マットの中央・手前）で示す 4. 補正記憶（`diag/correction_memory.json`）や台帳の値に、このずれが入り込んでいそうかを書く（読むだけ） 5. 直し方の案を2つ以内で書く（直すのは別の依頼。本線の変更は外部レビューを挟む決まり） ## 報告 - E・F それぞれのフォルダに README.md（表・画像の一覧・調べ方）を置く - 依頼箱の報告には、E の6のまとめの数（原因ごとの件数）と、F の「切れている日の数・ずれの大きさ・直し方の案の要点」だけを書く - 最後の1行に「判定：合格／止めた（決めてほしいことあり）／失敗」を書く `````` ### 依頼110の報告 ``````markdown # 結果：20261107_110_π0.5の失敗のつかみと持ち上げを記録から調べ上カメラの上端のずれを調べる - 状態：完了 - 始めた時刻：2026-11-07 15:01 - 終わった時刻：2026-11-07 15:31（30分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 40ceff6f-fa4b-47cd-b549-e78d2c496b8f` ## Claude Code の報告 E と F の両方を調べ終えました。どちらのフォルダにも README.md・表・画像・調べたスクリプトを置いています。r2r2r-scripts のファイルは何も変えていません（本線の `mat_corner_detection.detect_corners` は読み込んで使っただけです）。 **やったこと** - E：失敗12件と成功5件について、閉じたあとの持ち上げを並べました（π0.5 の生の値 → 範囲の制限のあと → 送った値 → 届いた角度）。あわせて、閉じたときの動き・送信が無視されたか・渡した状態のずれ・上カメラの閉じる前後のコマを調べ、原因を1つずつ付けました。 - F：本線の四隅の扱いを行番号つきで示しました。30日分の最初のコマを本線の検出で分け、マットの方眼（1cm）を数えて画像の上の端の本当の z を出し、ずれを計算しました。補正記憶と台帳は読むだけです。 **結果** - E の原因ごとの件数（失敗12件）: - 爪の間に入っていない：9件。うち「浅い」7件は、ブロックが爪先から約 12〜15mm はみ出したまま閉じたものです。「空振り」2件は、爪がブロックの手前で閉じたものです。 - 分からない：3件。 - 持ち上げを出していない・制限で削られた・送っても届かない・動きながら閉じた：いずれも0件。 - π0.5 は12件とも、閉じたあとに持ち上げる値を出していました。持ち上げが少なく見えたのは、失敗のうち10件で、人がそのかたまりを送らずに止めていたためです。 - F: - 切れている日：30日のうち11日です。大きく切れているのは8日（10/19・10/21・10/22・10/24・10/30・10/31・11/01・11/06）、わずかなのは3日（7/11・7/13・10/29）です。10/19 からはずっと切れています。 - ずれの大きさ：大きく切れている日は、画像の上の端が本当は z=−54〜−73mm で、本線の決め（−89mm）より 16〜35mm 根元寄りです。本線の位置は根元の側へずれ、教えた位置5のあたりで 4〜9mm、マットの中央で 8〜19mm、手前で 1〜3mm になります。 - 補正記憶などへの影響：補正記憶・置く高さの記憶・置き場所の補正は、どれも 10/16 が最後の更新で、大きく切れている日の値は入っていません。台帳の 10/19 の39行と 10/22 の4行のブロック・箱の座標は、切れた画のままの値です（5.5〜22mm ずれています）。 - 直し方の案の要点： - 案1：上の端が切れていたら、上の2点の z をマットの方眼から出して対応させる。 - 案2：切れていたら止めて、カメラをマット全体が写る位置に付け直す。ただし、π0.5 の学習データは 10/19 から切れた画角です。 **変えたファイル** フォルダは `/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/` の下です。 - `E_つかみと持ち上げ/`：README.md、e_analyze.py・e_images.py・e_grasp_metrics.py・e_track.py・e_cause.py、e_*.csv、e_grasp_failures.jpg・e_grasp_successes.jpg、e_fk_fit.json - `F_上カメラの上の端/`：README.md、f_corners.py・f_shift.py・f_montage.py、f_days.csv・f_ztop.csv・f_shift.csv、f_days_montage.jpg、f_first_frames/ **決めてほしいこと** - F の直し方を、次の依頼で案1・案2のどちらにするか（本線の変更なので外部レビューを挟みます）。 判定：合格 `````` ### E_つかみと持ち上げ/README.md ``````markdown # E. π0.5 の失敗12件のつかみと持ち上げを記録から調べる（2026-11-07・依頼110） **読むだけ。** 実機・Raspberry Pi・daemon・SSH・シリアルには触れていない。`runs/pi05_policy/` の記録を読み、このフォルダにだけ書いた。r2r2r-scripts のコードは変えていない（`pi05/run_policy_client.py` は読むだけ）。 対象: 失敗12件と比べる成功5件（依頼107 の `B/selection.json` と同じ）。成功の `pi05_20261031_185246` は爪を閉じる命令の記録がなく（推論1回・送っていない）、上カメラもピンぼけなので、比べる成功は実質4件。 ## 先に読むこと（結論） | 問い | 答え | |---|---| | 1 閉じたあと π0.5 は持ち上げを出したか | **12件とも出していた**。閉じた次のかたまりの生の値で爪先 +9〜35mm、その次で +25〜71mm（成功は +25〜37mm → +56〜62mm）。**範囲の制限で削られたものは0**（閉じたあと人が手を入れるまでのかたまりに制限の記録なし）。送った値に届かない分（肩 j1 で 0.5〜2.2°・手首 j4 で 0.4〜2.7°）は**成功も同じ**で、失敗だけの特徴ではない。違いは、**失敗の10件で、人が π0.5 の持ち上げのかたまりを送らずに止めた**こと（成功は4件とも、j0 を回し始めるまで止めていない）。依頼107 で「持ち上げが 2〜15mm しかない」とした5件は、3件（185133・183219・191547）が人が止めた／記録を終えたもの、2件（183426・184312）が π0.5 が 16〜33mm 上げながら j0 を 8° 以上回し始めたもの | | 2 閉じたときアームは止まっていたか | ほぼ止まっていた。閉じる前 0.3 秒の関節の速さ 0〜2.6°/s（成功 0〜0.3°/s）、爪を待つ 0.8 秒の間の動き 0〜1.1°（j4）。2.4〜2.6°/s の2件（184312・185050）は最後の送信の 0.09〜0.2 秒あとに閉じている。**爪の命令のあと送信が無視されて固まった件（LOG352 の形）は0**（送信の記録がある8件で、閉じたあと最初の送信のあと腕は 2 秒以内に 1.4〜8.1° 動いた。中継点の送り直しも0） | | 3 渡した状態は実際とずれていたか | `state_source=target` のかたまりで、目標と実測の差は閉じるかたまり 0.9〜2.9°・次のかたまり 0.8〜2.9°（成功 0.9〜2.1°・1.2〜1.6°）。肩 j1 はおおむね目標の方が 0.2〜1.0° 小さい（上がっている）。**成功と同じ幅で、失敗の原因とは見えない** | | 4 閉じた瞬間、ブロックは爪の間にあったか | **2件は爪の間にない（空振り）**: 閉じた直後の爪先の青い印の間が 73〜75px（ブロックを挟んだ件は 99〜100px）。**7件は浅い**: ブロックが爪先の印の線から 32〜40px（約 12〜15mm）先へはみ出したまま閉じている（成功 −14〜18px）。3件は成功と見分けがつかない。上からの画なので、**ブロックが爪といっしょに持ち上がったか（高さ）は言えない** | | 5 爪の開き具合の記録 | **ない**。`states.csv` の `gripper_state` は送った命令（0/1）だけで、爪の値の読み返しは記録していない。代わりに上の画の青い印の間（4）で比べた | | 6 原因（失敗12件） | **爪の間に入っていない 9件**（浅い 7・空振り 2）・**分からない 3件**・持ち上げを出していない 0・制限で削られた 0・送っても届かない 0・動きながら閉じた 0 | ## 1. 閉じたあとの持ち上げ（生の値 → 制限のあと → 送った値 → 届いた角度） 爪先の高さは角度から近似式で出した（依頼107 B の Unity の爪先の高さ 2,085 行に j1〜j4 の三角関数を当てはめた。別に取り分けた 585 行で誤差の中央値 0.8mm・最大 5.4mm。`e_fk_fit.json`）。**実機とのずれは測っていない**ので、値は「閉じた瞬間からどれだけ上がったか」の比べ合いにだけ使う。 - 生の値: π0.5 が出した行動のうち、実際に使うステップ（10 ステップ目。爪を切り替えるかたまりは切り替えの1つ前） - 制限のあと: `--raw-dir` の学習データの範囲 ±10°・肩 88°・j4 の安全範囲と、1回 ±20° の移動量の制限をかけた値（ログの `vias[].planned_target`） - 送った値: `sent_targets` の最後 - 届いた: 送ったあと読んだ角度（`vias[].actual_after`）。次のかたまりの実測ともほぼ同じ（0.3° 以内） - 窓: 閉じてから、j0 が 8° 回るか・次に開くか・HOME まで（依頼107 B と同じ）。「人」は人の補正（キーボード／ゲームパッド）で、前後の爪先の高さ | 試行 | 結果 | 閉じた次のかたまり（c+1） | その次（c+2） | そのあと | |---|---|---|---|---| | pi05_20261030_185133 | 失敗 | 生 22.0 / 制限 22.0 / 送 22.0 / 届 9.7 | 生 71.0（j0 +19°）**送らず** | 人 11.3→3.8（下げた）、c+3 生 35.8→届 22.4 | | pi05_20261030_190659 | 失敗 | 生 31.7 **送らず** | 人 0.4→−1.1、生 30.7 **送らず** | 人 −1.1→−2.9、c+3 生 25.6→届 15.4 | | pi05_20261031_180023 | 失敗 | 生 18.1 **送らず** | 人 2.5→14.8、生 32.9→届 20.5 | 生 53.5 送らず、人 20.5→24.5 | | pi05_20261031_183219 | 失敗 | 生 29.6 **送らず** | （記録を終えた） | | | pi05_20261031_183426 | 失敗 | 生 35.1 / 送 35.1 / 届 15.6（j0 +9°。ここで窓が終わる） | | | | pi05_20261101_191547 | 失敗 | 生 16.1 / 送 16.1 / 届 5.5 | 生 46.9（j0 +15°）**送らず**（記録を終えた） | | | pi05_20261101_192758 | 失敗 | 生 22.5 **送らず** | 人 1.9→11.7、生 35.0 **送らず** | | | pi05_20261101_194042 | 失敗 | 生 17.5 **送らず** | 人 2.4→115.7 | | | pi05_20261106_184312 | 失敗 | 生 21.6 / 送 21.6 / 届 8.2 | 生 41.5 / 送 41.5 / 届 32.7（j0 +17°） | | | pi05_20261106_185050 | 失敗 | 生 18.2 / 送 18.2 / 届 5.6 | 生 35.8 **送らず** | 人 5.9→147.3 | | pi05_20261106_185225 | 失敗 | 生 13.8 / 送 13.8 / 届 7.1 | 生 36.5 **送らず** | 人 7.6→8.7、人 9.9→63.3 | | pi05_20261106_185415 | 失敗 | 生 8.8 / 送 8.8 / 届 4.1 | 生 25.3 / 送 25.3 / 届 7.7 | 生 39.0 送らず、人 9.4→64.4 | | pi05_20261030_184245 | 成功 | 生 29.9 / 送 29.9 / 届 17.1 | 生 59.5 / 送 59.5 / 届 51.8（j0 +11°） | | | pi05_20261030_210954 | 成功 | 生 25.2 / 送 25.2 / 届 9.8 | 生 56.3 / 送 56.3 / 届 47.7（j0 +13°） | | | pi05_20261031_175527 | 成功 | 生 25.0 / 送 25.0 / 届 9.8 | 生 57.7 / 送 57.7 / 届 48.7（j0 +15°） | | | pi05_20261101_190347 | 成功 | 生 36.5 / 送 36.5 / 届 21.8 | 生 62.0 / 送 62.0 / 届 56.3（j0 +17°） | | （単位 mm。閉じた瞬間の爪先の高さからの差） 読み取れること - **持ち上げを出していない試行はない**。c+1 の生の値は失敗 8.8〜35.1mm（中央値 20）・成功 25.0〜36.5mm（中央値 27.5）で、失敗の方がやや小さいが重なる。c+2 では失敗も 25〜71mm を出している - **範囲の制限で削られた値はない**（制限のあとの値＝生の値。制限の記録 `notes` が出たのは、194042 の人の補正のあとの j4 の移動量の制限だけ） - **送った値と届いた角度の差は成功にもある**（c+1 で失敗 5〜20mm・成功 13〜15mm、c+2 で失敗 9〜18mm・成功 6〜9mm）。関節で見ると肩 j1 が 0.5〜2.2°、手首 j4 が 0.4〜2.7° 足りない（`_send_and_wait_arrival` は 3° 以内で着いたとする）。次のかたまりの実測でも同じ差が残っていて、動き続けてはいない - **失敗の10件は、π0.5 の持ち上げのかたまりを人が送らずに止めた**（上の表の「送らず」。184312 と 183426 だけは、j0 を回し始めるまで π0.5 のかたまりを送っている）。成功の4件は、j0 を回し始めるまで止めていない。止めた理由は記録にないが、4 の画で多くはブロックを浅くつかんでいた - 依頼107 の「閉じたあと 2〜15mm しか持ち上げていない」5件（185133・183219・183426・191547・184312）は、π0.5 が上げを出していなかったのではなく、185133・183219・191547 は人が止めた（183219・191547 は記録を終えた）、183426・184312 は 15〜33mm 上げながら j0 が 8° 回った（依頼107 の数え方では、そこで持ち上げの窓が終わる） ファイル: `e_chunks.csv`（閉じるかたまりの1つ前から窓の終わりまでの、かたまりと人の補正の1行ずつ。列は下の「列の意味」）・`e_summary.csv`（試行ごと） ## 2. 閉じたときアームは止まっていたか・爪の命令のあと送信が無視されたか | 試行 | 結果 | 閉じる前 0.3 秒の速さ °/s | 爪を待つ 0.8 秒の動き °（関節） | 最後の送信から閉じるまで 秒 | そのときの目標と実測の差 °（関節） | 閉じたあと最初の送信 | |---|---|---|---|---|---|---| | 185133 | 失敗 | 1.2 | 0.0 | 記録なし | — | 送信の記録なし（10/30・10/31 は `send_angles` の行がない。中継点の送り直しは0） | | 190659 | 失敗 | 0.6 | 0.0 | 記録なし | — | 同上 | | 180023 | 失敗 | 2.0 | 0.0 | 記録なし | — | 同上 | | 183219 | 失敗 | 1.2 | 0.0 | 記録なし | — | 同上 | | 183426 | 失敗 | 0.3 | 0.0 | 記録なし | — | 同上 | | 191547 | 失敗 | 0.3 | 0.0 | 1.65 | 2.0（j4） | 7.0 秒後・2 秒で 2.5° 動いた | | 192758 | 失敗 | 1.5 | 0.0 | 1.58 | 1.2（j4） | 人の補正 13.7 秒後・1.4° 動いた | | 194042 | 失敗 | 0.3 | 0.0 | 1.65 | 1.7（j4） | 人の補正 14.3 秒後・1.8° 動いた | | 184312 | 失敗 | **2.4** | **1.1（j4）** | **0.20** | 1.4（j4） | 3.0 秒後・8.1° 動いた | | 185050 | 失敗 | **2.6** | **1.0（j4）** | **0.09** | 1.7（j4） | 7.3 秒後・4.6° 動いた | | 185225 | 失敗 | 0.0 | 0.5（j2） | 0.09 | 0.8（j4） | 3.7 秒後・6.2° 動いた | | 185415 | 失敗 | 0.0 | 0.4（j4） | 0.21 | 1.0（j2） | 2.7 秒後・6.5° 動いた | | 184245 | 成功 | 0.3 | 0.0 | 記録なし | — | 同上 | | 210954 | 成功 | 0.3 | 0.0 | 記録なし | — | 同上 | | 175527 | 成功 | 0.3 | 0.0 | 記録なし | — | 同上 | | 190347 | 成功 | 0.0 | 0.0 | 5.98 | 1.7（j4） | 5.4 秒後・2.7° 動いた | - 閉じる命令は、かたまりの最後の送信が「3° 以内に着いた」と返ったすぐあとに出る（`run_policy_client.py` 1598〜1604 行）。11/06 の4件は最後の送信から 0.09〜0.21 秒で閉じている。そのうち 184312・185050 は閉じる前に 2.4〜2.6°/s で動いていて、爪を待つ間に j4 が約 1° 動いた。**j4 の 1° は爪先で数 mm で、原因と言えるほどではない**と見た（この2件のうち 185050 は浅いつかみ、184312 は成功と同じ形で閉じている） - 爪の命令（`set_gripper_value`）の返事は 0.01 秒で、爪が閉じきるのを待っていない（0.8 秒待つだけ。`GRIPPER_SLEEP`）。閉じきったかどうかの記録はない - 「送信が無視された」は「目標が 2° 以上離れているのに、2 秒たっても 0.5° 未満しか動かない」で数えた。**該当0**。中継点の送り直し（`retried`）も、閉じたあとのかたまりで0。人の補正の1刻みで「着いた」と判定されなかったものは4件に1回ずつあるが、どれも腕は動いていて（0.5〜18°）、固まってはいない - 10/30・10/31 の記録には送信（`send_angles`）の行がないので、最後の送信の時刻は分からない ファイル: `e_close.csv` ## 3. π0.5 に渡した状態と実際のずれ `--state-from-target` で、前に送った目標を状態として渡している（目標と実測の差が 6° を超えたら実測。`STATE_TARGET_MAX_GAP_DEG`）。 | | 閉じるかたまり（c+0）の差 ° | うち肩 j1 °（目標−実測） | 次のかたまり（c+1）の差 ° | うち肩 j1 ° | |---|---|---|---|---| | 失敗 12件 | 0.0〜2.9（target 10件・measured 2件） | −0.9〜0.0 | 0.8〜2.9（12件とも target） | −0.9〜+0.5 | | 成功 4件 | 0.9〜2.1（4件とも target） | −0.9〜−0.5 | 0.0〜1.6（target 3件・measured 1件） | −1.0〜0.0 | 差がいちばん大きい関節はたいてい j4（2 の「目標と実測の差」と同じ、着いたとみなす 3° の中に残る分）。**失敗も成功も同じ幅**。 ## 4. 閉じた瞬間、ブロックは爪の間にあったか 一覧画像（1行1試行。左から 閉じる直前・閉じて 1.0 秒後・π0.5 の持ち上げ1かたまり目を送り終えた直後・爪先が閉じたときより 40mm 上がった 0.5 秒後）: - **`e_grasp_failures.jpg`**（失敗12件。赤い字） - **`e_grasp_successes.jpg`**（成功5件。緑の字。185246 は閉じる命令がないので空） ブロックのあたり（閉じる直前のコマの赤の重心）を 420×315px で切り出し 1.25 倍。コマの時刻は `top_frame_log.csv` の `capture_ts`。カメラの映像の遅れは測っていない。使ったコマは `e_frames.csv`。 閉じた直後のコマを数えた値（`e_grasp_metrics.csv`）: | 試行 | 結果 | 青い印の間 px | ブロックのはみ出し px（爪先の印の線から先） | 目安 mm | |---|---|---|---|---| | 185133 | 失敗 | 99.5 | 15.7 | 6 | | 190659 | 失敗 | 100.0 | 34.5 | 13 | | 180023 | 失敗 | 99.4 | 36.6 | 14 | | **183219** | 失敗 | **74.5** | 78.8 | 30 | | 183426 | 失敗 | 99.8 | 40.3 | 15 | | **191547** | 失敗 | **72.6** | 75.2 | 29 | | 192758 | 失敗 | 99.2 | 35.3 | 14 | | 194042 | 失敗 | 99.3 | 32.3 | 12 | | 184312 | 失敗 | 99.9 | 20.8 | 8 | | 185050 | 失敗 | 99.8 | 35.0 | 13 | | 185225 | 失敗 | 99.3 | 8.7 | 3 | | 185415 | 失敗 | 100.1 | 33.0 | 13 | | 184245 | 成功 | 100.2 | 6.7 | 3 | | 210954 | 成功 | 100.2 | 12.0 | 5 | | 175527 | 成功 | 99.5 | −13.5 | −5 | | 190347 | 成功 | 99.3 | 17.5 | 7 | - 「青い印」は爪先の2つの青い丸。閉じる前はどれも 135〜138px。ブロックを挟むと 99〜100px で止まり、空振りでは 73〜75px まで閉じる - 「はみ出し」は、選んだ赤の塊の下の端（画像の下＝根元から遠い側）と、青い印2つの縦の平均との差。mm の目安はブロックの上面（約 62px／24mm）から 2.6px/mm とした - 閉じる直前と直後で、ブロックを挟んだ試行のはみ出しは 18〜29px 減る（爪の閉じ方で爪先が前に出るため。成功も同じ）。空振りの2件は減らない（−7px・+17px） 見え方の違い（画を見て） - **成功4件**: ブロックが両方の爪の真ん中に入り、ブロックの奥の端が爪の付け根の近くまで入っている。40mm 上がったときも、ブロックは爪先の印の間にあり、j0 を回すといっしょに動く - **空振り 2件（183219・191547）**: 閉じる直前、ブロックは爪先より 1 つ分以上先にある。閉じたあと、爪はブロックの手前（根元側）で閉じ、191547 ではブロックが押されて約 30° 回っている - **浅い 7件（190659・180023・183426・192758・194042・185050・185415）**: ブロックは左右の爪の間にあるが、爪先の線から 12〜15mm 先へはみ出していて、爪がブロックの根元側の端だけをつかんでいる。185415 は 40mm 上がったとき、ブロックが爪先の印より根元側に写り、爪から抜けて床に残ったように見える - **成功と見分けられない3件（185133・184312・185225）**: 閉じた形ははみ出し 9〜21px・印の間 99〜100px で成功の幅。185133 は閉じてから 150 秒ほどブロックを爪の間に入れたまま人の補正を続け、最後は箱の外のマットにあった。184312 はブロックを爪の間に入れたまま箱の上まで回っている。185225 は人の補正で 72mm 上げたときに爪から離れた - 上からの画では、**ブロックが床から浮いたか（爪といっしょに持ち上がったか）は言えない**。言えるのは「爪の間に横から見て入っているか」と「横に離れたか」だけ ブロックが爪から横に離れた時刻（`e_track.csv`・`e_track_series.csv`。0.5 秒おきのコマで、ブロックの重心と爪先の印の中点の距離が閉じた直後より 40px 以上離れたコマが2つ続いた最初）: | 試行 | 離れた時刻（閉じてから）秒 | そのときの爪先の高さ mm | j0 の回り ° | それまでの最高 mm | |---|---|---|---|---| | 185133 | 155.5 | 26.4 | 13.8 | 26.4 | | 190659 | 192.5 | 52.1 | 0.3 | 52.1 | | 180023 | 離れていない（HOME まで） | — | — | 39.4 | | 183219 | 離れていない（空振り。はじめから離れている） | — | — | 2.3 | | 183426 | 144.0 | 115.0 | 9.2 | 115.0 | | 191547 | 離れていない（空振り） | — | — | 7.5 | | 192758 | 59.5 | 12.9 | 0.3 | 61.3 | | 194042 | 88.5 | 11.1 | 7.8 | 115.7 | | 184312 | 離れていない（HOME まで） | — | — | 157.0 | | 185050 | 22.0 | 145.3 | 0.8 | 146.7 | | 185225 | 16.5 | 72.1 | 0.1 | 72.3 | | 185415 | 22.5 | 60.2 | 1.7 | 60.2 | | 成功4件 | 離れていない（開く命令まで） | | | 78.5〜128.3 | 浅い7件のうち 4件（190659・183426・185050・185415）は、人の補正で 52〜145mm 上げたあたりで離れている。192758・194042 は 61・116mm まで上げたあと下げ、低い所（11〜13mm）で離れた。180023 は HOME まで離れていない（上げたのは 39mm まで） ## 5. 爪の開き具合の記録 **ない**。`recording/states.csv` の `gripper_cmd` 行の `gripper_state` は送った命令（0=開・1=閉）で、`rpc` 行も `set_gripper_value` の返事の時間だけ。`log.jsonl` の `grip_state`（ゲームパッド）・`grip_before/after`（補正）も命令の状態。daemon に爪の値や電流を読む `read_servo` を足したが、Pi にはまだ置いていない（git `32a4280`）ので、この17件には入っていない。代わりに 4 の青い印の間で比べた。 ## 6. まとめ（失敗12件の原因） 決め方（`e_cause.py`。上から順に見て最初に当たったもの。線は成功4件の幅から置いたこの依頼での目安）: 1. 爪の間に入っていない（空振り）: 閉じた直後の青い印の間 < 90px 2. 爪の間に入っていない（浅い）: はみ出し > 成功の最大（17.5px）+ 10px 3. 動きながら閉じた: 閉じる前の速さ > 5°/s か、爪を待つ間に 3° 以上動いた 4. 持ち上げを出していない: 人の補正より前のかたまりで、生の値が 20mm 以上上げていない 5. 制限で削られた: 生の値は 20mm 以上、制限のあとで 20mm 未満 6. 送っても届かない: 送った値は 20mm 以上で届いた角度が 20mm 未満、かつ成功より大きく足りない 7. 分からない | 試行 | いちばんありそうな原因 | 根拠の数字 | |---|---|---| | pi05_20261030_185133 | 分からない | 閉じた形は成功の幅（はみ出し 16px・印の間 100px）。π0.5 は c+1 で 22mm 上げを送り届いたのは 10mm、c+2（71mm・j0 +19°）は人が止めて下げた。そのあと 150 秒ほど人の補正を続け、ブロックは箱の外のマットに残った | | pi05_20261030_190659 | 爪の間に入っていない（浅い） | はみ出し 34px（約 13mm。成功 −14〜18px）。π0.5 の c+1（32mm）・c+2（31mm）は人が止めた。人の補正で 52mm 上げたあたりで離れた | | pi05_20261031_180023 | 爪の間に入っていない（浅い） | はみ出し 37px（約 14mm）。c+1（18mm）は人が止めた | | pi05_20261031_183219 | 爪の間に入っていない（空振り） | 青い印の間 74px（成功 99〜100px）、ブロックが印の線から 79px 先。c+1（30mm）は人が止めて記録を終えた | | pi05_20261031_183426 | 爪の間に入っていない（浅い） | はみ出し 40px（約 15mm）。π0.5 は c+1 で 35mm 上げを送り届いたのは 16mm（j0 +9°）。115mm 上げたあたりで離れた | | pi05_20261101_191547 | 爪の間に入っていない（空振り） | 青い印の間 73px、ブロックが 75px 先で約 30° 回された。c+1 は 16mm を送り 6mm、c+2（47mm）は人が止めて記録を終えた | | pi05_20261101_192758 | 爪の間に入っていない（浅い） | はみ出し 35px（約 14mm）。c+1（23mm）・c+2（35mm）は人が止めた | | pi05_20261101_194042 | 爪の間に入っていない（浅い） | はみ出し 32px（約 12mm）。c+1（18mm）は人が止め、人の補正で 116mm 上げた | | pi05_20261106_184312 | 分からない | 閉じた形は成功の幅（はみ出し 21px）。π0.5 は c+1・c+2 で 22・42mm 上げを送り、届いたのは 8・33mm（成功と同じくらい）、j0 +17°。ブロックを爪の間に入れたまま箱の上まで運んでいる。閉じる 0.2 秒前まで動いていた（2.4°/s・爪を待つ間に j4 1.1°） | | pi05_20261106_185050 | 爪の間に入っていない（浅い） | はみ出し 35px（約 13mm）。c+1 は 18mm を送り 6mm、c+2（36mm）は人が止め、人の補正（j1 を −15° ずつ押し続け）で 145mm 上げたところで離れた。閉じる 0.09 秒前まで動いていた（2.6°/s・j4 1.0°） | | pi05_20261106_185225 | 分からない | 閉じた形は成功の幅（はみ出し 9px）。c+1 は 14mm を送り 7mm、c+2（37mm）は人が止めた。人の補正で 72mm 上げたときに離れた | | pi05_20261106_185415 | 爪の間に入っていない（浅い） | はみ出し 33px（約 13mm）。c+1・c+2 は 9・25mm を送り 4・8mm、c+3（39mm）は人が止め、人の補正で 60mm 上げたときに離れた | **原因ごとの件数: 爪の間に入っていない 9（浅い 7・空振り 2）・分からない 3・持ち上げを出していない 0・制限で削られた 0・送っても届かない 0・動きながら閉じた 0** 気をつけること - 「浅い」は「爪の間に入っていない」に入れたが、ブロックの一部（根元側の 9〜12mm 前後）は爪の間にある。上からの画では、爪がブロックのどの高さをつかんだかは分からない - 浅い・空振りのどちらも、**閉じる前の位置が成功より根元側に寄っている**（閉じる直前のはみ出し: 浅い 55〜64px・空振り 58〜86px、成功 16〜45px）。π0.5 が爪を閉じると決めた位置が、ブロックの 1〜2cm 手前だった形 - 爪先の高さは Unity から作った近似式。実機の爪先の高さは測っていない - 人が π0.5 のかたまりを止めた理由は記録にない。ここでは画から「浅くつかんでいた」ことだけを言っている ## 列の意味（`e_chunks.csv`） `rel_to_close` 閉じたかたまりからの番号（0=閉じたかたまり）／`executed` 送ったか／`after_window` j0 が 8° 回ったあと（窓の外）か／`state_meas_gap_*` 渡した状態と実測の差／`h_*_mm` 閉じた瞬間からの爪先の高さの差（`raw_used` 使うステップの生の値・`raw_horizon_max` 10 ステップの中の最大・`clipped` 制限のあと・`target_from_actual` 送る直前の実測からかけ直した値・`sent` 送った値・`actual_after` 届いた角度）／`j0_raw_used_minus_close` 生の値の j0 − 閉じたときの j0／`notes` 制限の記録／`via_retried_steps` 中継点で送り直したステップ。`kind=correction` の行は人の補正（`h_state_mm` 始め、`h_actual_after_mm` 終わり） ## やり直し方 ```bash cd "/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/E_つかみと持ち上げ" python3 -I e_analyze.py # e_fk_fit.json・e_chunks.csv・e_close.csv・e_summary.csv python3 -I e_images.py # e_grasp_failures.jpg・e_grasp_successes.jpg・e_frames.csv python3 -I e_grasp_metrics.py # e_grasp_metrics.csv python3 -I e_track.py # e_track.csv・e_track_series.csv（数分） python3 -I e_cause.py # e_cause.csv ``` `````` ### E のスクリプト e_analyze.py ``````python """依頼110 E: π0.5 の失敗12件・成功5件の, 爪を閉じたあとの持ち上げとつかみを記録から調べる (読むだけ). 入力 (読むだけ): runs/pi05_policy/{run}/log.jsonl・recording/states.csv・recording/top_frame_log.csv・{run}.json lab/twin/20261107_失敗の再生と置き場所/B/{selection.json, timeline.csv, timeline_metrics.csv} 出力 (このフォルダ): e_fk_fit.json 爪先の高さの近似式 (B の Unity の値に当てはめ) e_chunks.csv 閉じる前後のかたまり・補正ごとの値 (1行 = 1つの出来事) e_close.csv 閉じた瞬間の動き・送信無視・状態のずれ (1行 = 1試行) e_summary.csv 試行ごとのまとめと原因の候補 """ import csv import json from pathlib import Path import numpy as np ROOT = Path("/Volumes/KIOXIA/r2r2r-data") RUNS = ROOT / "runs" / "pi05_policy" TW = ROOT / "lab" / "twin" / "20261107_失敗の再生と置き場所" B = TW / "B" OUT = TW / "E_つかみと持ち上げ" J0_TURN_DEG = 8.0 # B と同じ: 閉じたときから j0 が 8° 回ったら「箱へ運び始めた」 LIFT_OUT_MM = 20.0 # π0.5 が「持ち上げを出した」とみる高さ (成功の持ち上げ 26〜36mm より少し下) STILL_DEG_S = 2.0 # 閉じる前 0.3 秒の関節の速さがこれ未満なら「止まっていた」 GAP_DEG = 1.5 # 閉じた瞬間の, 最後に送った目標と届いた角度の差がこれを超えたら「着ききっていない」 # ---------------------------------------------------------------- 爪先の高さの近似式 def _feats(J): J = np.atleast_2d(np.asarray(J, dtype=float)) j1, j2, j3, j4 = [np.radians(J[:, k]) for k in (1, 2, 3, 4)] b = j1 + j2 return np.stack([np.ones_like(j1), np.sin(j1), np.cos(j1), np.sin(b), np.cos(b), np.sin(b) * np.cos(j4), np.cos(b) * np.cos(j4), np.sin(b) * np.sin(j4) * np.cos(j3), np.cos(b) * np.sin(j4) * np.cos(j3), np.sin(j4) * np.sin(j3)], 1) def fit_height(): ang = {(r["run"], r["i"]): r for r in csv.DictReader(open(B / "timeline.csv"))} X, y = [], [] for r in csv.DictReader(open(B / "timeline_metrics.csv")): a = ang[(r["run"], r["i"])] X.append([float(a[f"j{k}"]) for k in range(6)]); y.append(float(r["tip_h_mm"])) X, y = np.array(X), np.array(y) idx = np.random.default_rng(110).permutation(len(y)); tr, te = idx[:1500], idx[1500:] c, *_ = np.linalg.lstsq(_feats(X[tr]), y[tr], rcond=None) e = np.abs(_feats(X[te]) @ c - y[te]) c, *_ = np.linalg.lstsq(_feats(X), y, rcond=None) info = {"n": int(len(y)), "holdout_n": int(len(te)), "holdout_abs_err_median_mm": round(float(np.median(e)), 2), "holdout_abs_err_p95_mm": round(float(np.percentile(e, 95)), 2), "holdout_abs_err_max_mm": round(float(e.max()), 2), "coef": [float(v) for v in c], "note": "B の Unity の爪先の高さ (マットから mm) を j1〜j4 の三角関数で当てはめた. 実機とのずれは測っていない"} json.dump(info, open(OUT / "e_fk_fit.json", "w"), ensure_ascii=False, indent=1) return lambda q: float((_feats(np.asarray(q, dtype=float)[:6]) @ c)[0]), info # ---------------------------------------------------------------- 記録を読む def load_states(run): act, sends, grips, stages = [], [], [], [] for r in csv.DictReader(open(RUNS / run / "recording" / "states.csv")): t = float(r["ts"]) if r["event_type"] == "actual" and r["j0"] and r["stage"] != "home": # HOME に戻る動きは数えない act.append([t] + [float(r[f"j{k}"]) for k in range(6)]) elif r["event_type"] == "send_angles": sends.append((t, np.array([float(r[f"j{k}"]) for k in range(6)]), r["stage"])) elif r["event_type"] == "gripper_cmd": grips.append((t, int(r["gripper_state"]), r["stage"])) elif r["event_type"] == "stage": stages.append((t, r["stage"])) return np.array(act), sends, grips, stages def q_at(act, t): """t 以前でいちばん新しい届いた角度.""" k = np.searchsorted(act[:, 0], t, side="right") - 1 return None if k < 0 else act[k, 1:] def load_log(run): return [json.loads(l) for l in open(RUNS / run / "log.jsonl")] def frame_log(run): p = RUNS / run / "recording" / "top_frame_log.csv" return [(int(r["frame_idx"]), float(r["capture_ts"])) for r in csv.DictReader(open(p))] if p.exists() else [] def r1(v): return None if v is None else round(float(v), 1) def main(): H, fit = fit_height() sel = json.load(open(B / "selection.json")) runs = [(r, "failure") for r in sel["failures"]] + [(r, "success") for r in sel["successes"]] btab = {r["run"]: r for r in csv.DictReader(open(B / "b_table.csv"))} ev_rows, close_rows, sum_rows = [], [], [] for run, outcome in runs: act, sends, grips, stages = load_states(run) log = load_log(run) tj = json.load(open(RUNS / run / f"{run}.json")) closes = [g for g in grips if g[1] == 1] if not closes: sum_rows.append({"run": run, "outcome": outcome, "cause": "記録なし", "basis": "爪を閉じる命令の記録がない (log.jsonl も推論1回で送っていない)"}) continue t_close, _, close_stage = closes[0] q_close = q_at(act, t_close) h_close = H(q_close) j0_close = q_close[0] opens_after = [g[0] for g in grips if g[1] == 0 and g[0] > t_close] t_open = opens_after[0] if opens_after else np.inf # j0 が 8° 回った時刻 after = act[(act[:, 0] > t_close) & (act[:, 0] < t_open)] turn = after[np.abs(after[:, 1] - j0_close) >= J0_TURN_DEG] t_turn = turn[0, 0] if len(turn) else np.inf t_end_win = min(t_open, t_turn, act[-1, 0] + 1) win = act[(act[:, 0] >= t_close) & (act[:, 0] <= t_end_win)] lift_total = max(H(q) for q in win[:, 1:]) - h_close if len(win) else 0.0 # ---- 出来事の並び (かたまり と 補正 と 巻き戻し) events = [] chunk_recs = [d for d in log if "chunk" in d and d.get("type") is None] close_chunk = None for d in chunk_recs: if d.get("grip_cmd") == 1 and d.get("executed") and d.get("t_exec_end") \ and abs(d["t_exec_end"] - t_close) < 5: close_chunk = d["chunk"] for d in log: t = d.get("type") if t is None and "chunk" in d: events.append(("chunk", d.get("t_exec_start") or d["t"], d)) elif t == "correction" and d.get("action") in ("accept", "cancel"): events.append(("correction", d.get("t_start", d.get("t")), d)) elif t == "rewind": events.append(("rewind", d.get("t", 0), d)) events.sort(key=lambda e: e[1]) # 閉じたかたまりの1つ前から, 窓の終わりまで model_lift_raw, model_lift_clip, model_lift_sent, model_lift_act = [], [], [], [] corr_lift = [] state_gaps = [] note_clamps = [] first_corr_seen = False for kind, t0, d in events: if kind == "chunk": c = d["chunk"] if close_chunk is None or c < close_chunk - 1: continue if d["t"] > t_end_win: # 窓の終わりのあとの推論は, 出していた値だけ見る (送らない分) pass rel = c - close_chunk acts = np.asarray(d["actions"], dtype=float) st = np.asarray(d["state"], dtype=float)[:6] ms = np.asarray(d["measured_state"], dtype=float)[:6] gap = np.abs(st - ms) vias = d.get("vias") or [] raw_last = np.asarray(vias[-1]["raw"]) if vias else acts[min(d["used_step"], len(acts) - 1), :6] plan_last = np.asarray(vias[-1]["planned_target"]) if vias else np.asarray(d["target"]) tgt_last = np.asarray(vias[-1]["target"]) if vias else np.asarray(d["target"]) sent = d.get("sent_targets") or [] sent_last = np.asarray(sent[-1]) if sent else None aa = [v.get("actual_after") for v in vias if v.get("actual_after")] act_last = np.asarray(aa[-1]) if aa else None if act_last is None and d.get("executed") and d.get("t_exec_end"): act_last = q_at(act, d["t_exec_end"] + 0.3) h_raw_horizon = [H(a) for a in acts[:, :6]] notes = sorted({n for v in vias for n in (v.get("notes") or [])} | set(d.get("notes") or [])) retried = [v["step"] for v in vias if v.get("retried")] failed = [v["step"] for v in vias if v.get("called") and v.get("success") is False] row = { "run": run, "outcome": outcome, "kind": "chunk", "chunk": c, "rel_to_close": rel, "executed": bool(d.get("executed")), "grip_cmd": d.get("grip_cmd"), "after_window": bool(d["t"] > t_end_win), "state_source": d.get("state_source"), "state_meas_gap_max_deg": r1(gap.max()), "state_meas_gap_joint": f"j{int(np.argmax(gap))}", "state_meas_gap_j1_deg": r1(st[1] - ms[1]), "state_meas_gap_j2_deg": r1(st[2] - ms[2]), "h_state_mm": r1(H(st) - h_close), "h_measured_mm": r1(H(ms) - h_close), "j1_measured": r1(ms[1]), "j1_raw_used": r1(raw_last[1]), "j1_clipped": r1(plan_last[1]), "j1_sent": r1(None if sent_last is None else sent_last[1]), "j1_actual_after": r1(None if act_last is None else act_last[1]), "h_raw_used_mm": r1(H(raw_last) - h_close), "h_raw_horizon_max_mm": r1(max(h_raw_horizon) - h_close), "h_raw_horizon_last_mm": r1(h_raw_horizon[-1] - h_close), "h_clipped_mm": r1(H(plan_last) - h_close), "h_target_from_actual_mm": r1(H(tgt_last) - h_close), "h_sent_mm": r1(None if sent_last is None else H(sent_last) - h_close), "h_actual_after_mm": r1(None if act_last is None else H(act_last) - h_close), "j0_raw_used_minus_close": r1(raw_last[0] - j0_close), "j0_raw_horizon_last_minus_close": r1(acts[-1, 0] - j0_close), "notes": " / ".join(notes), "via_retried_steps": " ".join(map(str, retried)), "via_failed_steps": " ".join(map(str, failed)), "used_step": d.get("used_step"), } ev_rows.append(row) if rel in (-1, 0, 1, 2): state_gaps.append((rel, d.get("state_source"), gap.max(), st[1] - ms[1])) if rel >= 1 and not first_corr_seen and d["t"] <= t_end_win: model_lift_raw.append(max(h_raw_horizon) - h_close) model_lift_clip.append(H(plan_last) - h_close) if d.get("executed") and sent_last is not None: model_lift_sent.append(H(sent_last) - h_close) if act_last is not None: model_lift_act.append(H(act_last) - h_close) if any("クランプ" in n or "制限" in n for n in notes): note_clamps.append((c, notes)) elif kind in ("correction", "rewind"): if t0 is None or t0 < t_close or t0 > t_end_win: continue first_corr_seen = True sp = d.get("start_pose"); ma = d.get("measured_after") hl = (H(ma) - H(sp)) if (sp and ma) else None corr_lift.append(hl if hl is not None else 0.0) ev_rows.append({"run": run, "outcome": outcome, "kind": kind, "chunk": d.get("after_chunk"), "rel_to_close": None, "executed": True, "h_state_mm": r1(None if not sp else H(sp) - h_close), "h_actual_after_mm": r1(None if not ma else H(ma) - h_close), "j1_measured": r1(None if not sp else sp[1]), "j1_actual_after": r1(None if not ma else ma[1]), "notes": f"人の補正 ({d.get('device') or 'キーボード'}) {d.get('action')}" + (f" grip {d.get('grip_before')}→{d.get('grip_after')}"), }) # ---- 閉じた瞬間の動き (2) def speed(t_a, t_b): qa, qb = q_at(act, t_a), q_at(act, t_b) return None if qa is None or qb is None else float(np.max(np.abs(qb - qa)) / (t_b - t_a)) v_before = speed(t_close - 0.3, t_close) moved_during = (lambda qa, qb: None if qa is None or qb is None else float(np.max(np.abs(qb - qa))))( q_at(act, t_close), q_at(act, t_close + 0.8)) moved_during_j = (lambda qa, qb: None if qa is None or qb is None else int(np.argmax(np.abs(qb - qa))))( q_at(act, t_close), q_at(act, t_close + 0.8)) moved_2s = (lambda qa, qb: None if qa is None or qb is None else float(np.max(np.abs(qb - qa))))( q_at(act, t_close), q_at(act, t_close + 2.0)) last_send = [s for s in sends if s[0] <= t_close] ls_t, ls_q = (last_send[-1][0], last_send[-1][1]) if last_send else (None, None) gap_close = None if ls_q is None else np.abs(ls_q - q_close) settled_q = q_at(act, t_close + 2.0) gap_settled = None if ls_q is None or settled_q is None else float(np.max(np.abs(ls_q - settled_q))) h_still = H(settled_q) - h_close if settled_q is not None else None # 届いた角度で, 閉じた瞬間から先に何 mm 爪先が動いたか (前後の位置は j0 と高さで) # 送信無視 (LOG352 / DAY228): 閉じたあと最初の送信に, 腕が向かい始めたか s_after = [s for s in sends if s[0] > t_close] ignored = None; first_send_info = "" if s_after: ts, qt, stg = s_after[0] q0 = q_at(act, ts) q2 = q_at(act, ts + 2.0) if q0 is not None and q2 is not None: d0 = float(np.max(np.abs(qt - q0))); d2 = float(np.max(np.abs(qt - q2))) moved = float(np.max(np.abs(q2 - q0))) # 無視された = 目標が 2° 以上離れているのに, 2 秒たっても 0.5° 未満しか動かない ignored = bool(d0 >= 2.0 and moved < 0.5) first_send_info = (f"{stg} 送信 {ts - t_close:.1f}s 後: 目標まで {d0:.1f}°→2秒後 {d2:.1f}° " f"(動いた {moved:.1f}°)") # 補正の1刻み (パッド・キーボード) で届かなかったもの corr_unreached = 0 for d in log: if d.get("type") in ("pad", "correction") and (d.get("t_unix") or d.get("t") or 0) > t_close: tt = d.get("t_unix") if d.get("type") == "pad" else d.get("t") if not isinstance(tt, (int, float)) or tt > t_end_win: continue if d.get("sent") and (d.get("arrived") is False or d.get("reached") is False): corr_unreached += 1 sg = {rel: (src, g, g1) for rel, src, g, g1 in state_gaps} close_rows.append({ "run": run, "outcome": outcome, "close_stage": close_stage, "close_chunk": close_chunk, "t_close_s": round(t_close - act[0, 0], 2), "n_close_cmds": len(closes), "h_close_unity_mm": r1(h_close), "j1_close": r1(q_close[1]), "j2_close": r1(q_close[2]), "last_send_before_close_s": None if ls_t is None else round(t_close - ls_t, 2), "gap_lastsend_vs_actual_at_close_deg": None if gap_close is None else r1(gap_close.max()), "gap_joint": None if gap_close is None else f"j{int(np.argmax(gap_close))}", "gap_lastsend_vs_actual_2s_after_deg": r1(gap_settled), "speed_0.3s_before_close_deg_s": r1(v_before), "moved_during_grip_wait_0.8s_deg": r1(moved_during), "moved_during_grip_wait_joint": None if moved_during_j is None else f"j{moved_during_j}", "moved_0_to_2s_deg": r1(moved_2s), "h_change_0_to_2s_mm": r1(h_still), "first_send_after_close": first_send_info, "first_send_ignored": ignored, "corr_steps_unreached_after_close": corr_unreached, "state_src_close_chunk": sg.get(0, (None,))[0], "state_gap_close_chunk_deg": r1(sg.get(0, (0, None))[1]), "state_gap_close_chunk_j1_deg": r1(sg.get(0, (0, None, None))[2]), "state_src_next_chunk": sg.get(1, (None,))[0], "state_gap_next_chunk_deg": r1(sg.get(1, (0, None))[1]), "state_gap_next_chunk_j1_deg": r1(sg.get(1, (0, None, None))[2]), "state_src_prev_chunk": sg.get(-1, (None,))[0], "state_gap_prev_chunk_deg": r1(sg.get(-1, (0, None))[1]), }) # ---- 持ち上げの区分 (1) raw = max(model_lift_raw) if model_lift_raw else None clip = max(model_lift_clip) if model_lift_clip else None sentm = max(model_lift_sent) if model_lift_sent else None actm = max(model_lift_act) if model_lift_act else None if raw is None: lift_cls = "推論なし (閉じてすぐ人の補正)" elif raw < LIFT_OUT_MM: lift_cls = "出していない" elif clip is not None and clip < LIFT_OUT_MM: lift_cls = "制限で削られた" elif sentm is None: lift_cls = "出したが送らなかった (人が止めた)" elif actm is not None and actm < LIFT_OUT_MM and sentm >= LIFT_OUT_MM: lift_cls = "送ったのに届かなかった" else: lift_cls = "出して届いた" sum_rows.append({ "run": run, "outcome": outcome, "close_chunk": close_chunk, "lift_total_mm": r1(lift_total), "lift_by_model_actual_mm": r1(actm), "lift_by_correction_mm": r1(max(corr_lift) if corr_lift else None), "n_model_chunks_before_correction": len(model_lift_raw), "model_raw_max_mm": r1(raw), "model_clipped_max_mm": r1(clip), "model_sent_max_mm": r1(sentm), "model_actual_max_mm": r1(actm), "lift_class": lift_cls, "clamp_notes": " | ".join(f"c{c}: {'/'.join(n)}" for c, n in note_clamps), "b_category": btab.get(run, {}).get("category"), "b_close_tip_block_xz_mm": btab.get(run, {}).get("close_tip_block_xz_mm"), "b_lift_rise_mm": btab.get(run, {}).get("lift_rise_mm"), "judge_pick": tj.get("judgments", {}).get("pick", {}).get("human_verdict"), }) def wcsv(name, rows): keys = [] for r in rows: for k in r: if k not in keys: keys.append(k) with open(OUT / name, "w", newline="") as f: w = csv.DictWriter(f, fieldnames=keys); w.writeheader(); w.writerows(rows) wcsv("e_chunks.csv", ev_rows) wcsv("e_close.csv", close_rows) wcsv("e_summary.csv", sum_rows) print("fit", fit["holdout_abs_err_median_mm"], fit["holdout_abs_err_max_mm"]) if __name__ == "__main__": main() `````` ### E のスクリプト e_images.py ``````python """依頼110 E-4: 閉じる命令の直前・直後 (+1.0 秒)・π0.5 の持ち上げ1かたまり目を送り終えた直後・ 爪先が閉じたときより 40mm 上がった (近似式. 送り手は π0.5 でも人の補正でもよい) 0.5 秒後 の上カメラのコマを並べる. ブロックのあたり (直前のコマの赤の重心) を 420×315 px で切り出し, 1.25 倍にして並べる. 出力: e_grasp_failures.jpg (失敗12件) / e_grasp_successes.jpg (成功5件) / e_frames.csv (使ったコマ) """ import csv import json from pathlib import Path import cv2 import numpy as np ROOT = Path("/Volumes/KIOXIA/r2r2r-data") RUNS = ROOT / "runs" / "pi05_policy" TW = ROOT / "lab" / "twin" / "20261107_失敗の再生と置き場所" OUT = TW / "E_つかみと持ち上げ" CW, CH, SC = 420, 315, 1.25 FALLBACK = (1140, 690) # 赤が見つからないときの中心 (17件の直前のコマの赤の位置のおおよそ) def red_center(img): hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) m = cv2.inRange(hsv, (0, 120, 70), (10, 255, 255)) | cv2.inRange(hsv, (170, 120, 70), (180, 255, 255)) m[:, :700] = 0; m[:, 1400:] = 0; m[:350] = 0 n, lab, st, cen = cv2.connectedComponentsWithStats(m) best = None for k in range(1, n): x, y, w, h, a = st[k] if a < 300 or max(w, h) > 3 * min(w, h): # 細長い赤 (配線) は外す continue if best is None or a > best[0]: best = (a, cen[k]) return None if best is None else (int(best[1][0]), int(best[1][1])) def grab(cap, idx): cap.set(cv2.CAP_PROP_POS_FRAMES, idx) ok, f = cap.read() return f if ok else None def crop(f, c): x0 = int(np.clip(c[0] - CW // 2, 0, f.shape[1] - CW)); y0 = int(np.clip(c[1] - CH // 2, 0, f.shape[0] - CH)) im = f[y0:y0 + CH, x0:x0 + CW] return cv2.resize(im, (int(CW * SC), int(CH * SC)), interpolation=cv2.INTER_CUBIC) def label(img, text, color=(255, 255, 255)): cv2.rectangle(img, (0, 0), (img.shape[1], 30), (0, 0, 0), -1) cv2.putText(img, text, (8, 22), cv2.FONT_HERSHEY_SIMPLEX, 0.62, color, 2, cv2.LINE_AA) return img def height_fn(): c = np.array(json.load(open(OUT / "e_fk_fit.json"))["coef"]) def H(q): j1, j2, j3, j4 = [np.radians(q[k]) for k in (1, 2, 3, 4)] b = j1 + j2 f = np.array([1, np.sin(j1), np.cos(j1), np.sin(b), np.cos(b), np.sin(b) * np.cos(j4), np.cos(b) * np.cos(j4), np.sin(b) * np.sin(j4) * np.cos(j3), np.cos(b) * np.sin(j4) * np.cos(j3), np.sin(j4) * np.sin(j3)]) return float(f @ c) return H def t_rise(rec, tc, H, rise=40.0): """閉じたあと, 爪先が閉じたときより rise mm 上がった最初の時刻 (届いた角度. 次に開くまで).""" act, t_open = [], np.inf for r in csv.DictReader(open(rec / "states.csv")): if r["event_type"] == "gripper_cmd" and r["gripper_state"] == "0" and float(r["ts"]) > tc: t_open = min(t_open, float(r["ts"])) if r["event_type"] == "actual" and r["j0"] and r["stage"] != "home": # HOME に戻る動きは数えない act.append((float(r["ts"]), [float(r[f"j{k}"]) for k in range(6)])) before = [q for t, q in act if t <= tc] if not before: return None h0 = H(before[-1]) for t, q in act: if tc < t < t_open and H(q) - h0 >= rise: return t return None def main(): H = height_fn() close = {r["run"]: r for r in csv.DictReader(open(OUT / "e_close.csv"))} sel = json.load(open(TW / "B" / "selection.json")) rows_out = [] for group, runs in (("failures", sel["failures"]), ("successes", sel["successes"])): tiles = [] for run in runs: W, Hh = int(CW * SC), int(CH * SC) blank = np.full((Hh, W, 3), 40, np.uint8) rec = RUNS / run / "recording" fl = [(int(r["frame_idx"]), float(r["capture_ts"])) for r in csv.DictReader(open(rec / "top_frame_log.csv"))] gr = [float(r["ts"]) for r in csv.DictReader(open(rec / "states.csv")) if r["event_type"] == "gripper_cmd" and r["gripper_state"] == "1"] name = run.replace("pi05_", "") if not gr: tiles.append(np.hstack([label(blank.copy(), f"{name} no close cmd")] + [blank] * 3)) rows_out.append({"run": run, "note": "爪を閉じる命令の記録なし"}) continue tc = gr[0] ts = np.array([t for _, t in fl]) i_before = int(np.searchsorted(ts, tc, side="right") - 1) i_after = int(np.searchsorted(ts, tc + 1.0)) # π0.5 の持ち上げ1かたまり目 (閉じたかたまり+1) を送り終えた直後 cc = int(close[run]["close_chunk"]) recs = {d["chunk"]: d for d in map(json.loads, open(RUNS / run / "log.jsonl")) if d.get("type") is None and "chunk" in d} nx = recs.get(cc + 1) i_lift = None if nx and nx.get("executed") and nx.get("t_exec_end"): i_lift = int(np.searchsorted(ts, nx["t_exec_end"] + 0.3)) cap = cv2.VideoCapture(str(rec / "top.mp4")) fb = grab(cap, fl[i_before][0]); fa = grab(cap, fl[min(i_after, len(fl) - 1)][0]) c = red_center(fb) or red_center(fa) or FALLBACK t_b = ts[i_before] - tc; t_a = ts[min(i_after, len(fl) - 1)] - tc col = (90, 90, 255) if group == "failures" else (120, 255, 120) tb = label(crop(fb, c), f"{name} before {t_b:+.2f}s", col) ta = label(crop(fa, c), f"after {t_a:+.2f}s") if i_lift is not None and i_lift < len(fl): fl_img = grab(cap, fl[i_lift][0]); t_l = ts[i_lift] - tc tl = label(crop(fl_img, c), f"lift chunk {cc + 1} end {t_l:+.1f}s") else: tl = label(blank.copy(), f"chunk {cc + 1} not sent (human)") t_l = None tr = t_rise(rec, tc, H) i_rise = None if tr is None else int(np.searchsorted(ts, tr + 0.5)) if i_rise is not None and i_rise < len(fl): t_r = ts[i_rise] - tc tr_img = label(crop(grab(cap, fl[i_rise][0]), c), f"tip +40mm {t_r:+.1f}s") else: t_r = None tr_img = label(blank.copy(), "tip never +40mm") tiles.append(np.hstack([tb, ta, tl, tr_img])) rows_out.append({"run": run, "group": group, "frame_before": fl[i_before][0], "t_before_s": round(t_b, 2), "frame_after": fl[min(i_after, len(fl) - 1)][0], "t_after_s": round(t_a, 2), "frame_lift": None if i_lift is None else fl[i_lift][0], "t_lift_s": None if t_l is None else round(t_l, 2), "frame_rise40": None if t_r is None else fl[i_rise][0], "t_rise40_s": None if t_r is None else round(t_r, 2), "crop_center_px": f"{c[0]},{c[1]}"}) cap.release() cv2.imwrite(str(OUT / f"e_grasp_{group}.jpg"), np.vstack(tiles), [cv2.IMWRITE_JPEG_QUALITY, 88]) keys = sorted({k for r in rows_out for k in r}, key=lambda k: list(rows_out[1].keys()).index(k) if k in rows_out[1] else 99) with open(OUT / "e_frames.csv", "w", newline="") as f: w = csv.DictWriter(f, fieldnames=keys); w.writeheader(); w.writerows(rows_out) if __name__ == "__main__": main() `````` ### E のスクリプト e_grasp_metrics.py ``````python """依頼110 E-4: 閉じた直後 (+1.0 秒) のコマで, 爪先の青い印2つと赤いブロックの位置を数える. center_dx_px : ブロックの重心 − 青い印2つの中点 (画像の横. + が右) protrude_px : ブロックの下の端 (画像の下 = 根元から遠い側) − 青い印2つの縦の平均 marker_gap_px: 青い印2つの間 (爪の閉じ具合の目安. ブロックの幅で止まれば成功と同じくらい) red_area_px : 見えている赤の面積 (爪に隠れる分は減る) 画素 → mm の目安: マットの上で約 2.2 px/mm (マット 620mm が約 1,370 px). ブロックの上面 (高さ 24mm) では少し大きく写る. 出力: e_grasp_metrics.csv """ import csv from pathlib import Path import cv2 import numpy as np ROOT = Path("/Volumes/KIOXIA/r2r2r-data") RUNS = ROOT / "runs" / "pi05_policy" OUT = ROOT / "lab" / "twin" / "20261107_失敗の再生と置き場所" / "E_つかみと持ち上げ" def blobs(mask, min_area): n, lab, st, cen = cv2.connectedComponentsWithStats(mask) return [(st[k], cen[k]) for k in range(1, n) if st[k][4] >= min_area] def measure(f, c): x0, y0 = c[0] - 260, c[1] - 260 roi = f[y0:y0 + 520, x0:x0 + 520] hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV) red = cv2.inRange(hsv, (0, 120, 70), (10, 255, 255)) | cv2.inRange(hsv, (170, 120, 70), (180, 255, 255)) blue = cv2.inRange(hsv, (100, 150, 80), (130, 255, 255)) rb = [b for b in blobs(red, 300) if max(b[0][2], b[0][3]) < 3 * min(b[0][2], b[0][3])] bb = blobs(blue, 60) if not rb or len(bb) < 2: return None r = max(rb, key=lambda b: b[0][4]) # 赤にいちばん近い青2つ (爪先の印). 爪の付け根の上の印 (腕の上) は赤から遠い bb = sorted(bb, key=lambda b: np.hypot(*(b[1] - r[1])))[:2] (b1, c1), (b2, c2) = bb mid = (c1 + c2) / 2 bottom = r[0][1] + r[0][3] # 選んだ赤の塊の下の端 return {"center_dx_px": round(float(r[1][0] - mid[0]), 1), "protrude_px": round(float(bottom - mid[1]), 1), "marker_gap_px": round(float(np.hypot(*(c1 - c2))), 1), "red_area_px": int(r[0][4])} def main(): rows = [] for r in csv.DictReader(open(OUT / "e_frames.csv")): if not r.get("frame_after"): rows.append({"run": r["run"], "note": "閉じる命令の記録なし"}); continue cap = cv2.VideoCapture(str(RUNS / r["run"] / "recording" / "top.mp4")) c = tuple(int(v) for v in r["crop_center_px"].split(",")) out = {"run": r["run"], "group": r["group"]} for tag in ("before", "after"): cap.set(cv2.CAP_PROP_POS_FRAMES, int(r[f"frame_{tag}"])); ok, f = cap.read() m = measure(f, c) if ok else None for k, v in (m or {}).items(): out[f"{tag}_{k}"] = v rows.append(out) keys = [] for r in rows: keys += [k for k in r if k not in keys] with open(OUT / "e_grasp_metrics.csv", "w", newline="") as fo: w = csv.DictWriter(fo, fieldnames=keys); w.writeheader(); w.writerows(rows) for r in rows: print(r) if __name__ == "__main__": main() `````` ### E のスクリプト e_track.py ``````python """依頼110 E-4 補足: 閉じたあと, ブロックが爪先から離れた時刻を上カメラで追う. 0.5 秒おきのコマで, 赤いブロックの重心と, 赤にいちばん近い青い印2つ (間が 60〜150px のもの = 爪先の印) の 中点の距離 sep を測る. 閉じた直後 (+1.0〜2.0 秒) の sep の中央値を基準にし, 基準より 40px 以上離れたコマが 2つ続いた最初の時刻を「離れた」とする. HOME に戻る区間と, 次に爪を開く命令のあとは見ない. 真上からの画なので, ブロックが爪といっしょに持ち上がったか (高さ) は分からない. 分かるのは横に離れたことだけ. 出力: e_track.csv (試行ごと) / e_track_series.csv (コマごと) """ import csv import json from pathlib import Path import cv2 import numpy as np ROOT = Path("/Volumes/KIOXIA/r2r2r-data") RUNS = ROOT / "runs" / "pi05_policy" OUT = ROOT / "lab" / "twin" / "20261107_失敗の再生と置き場所" / "E_つかみと持ち上げ" SEP_JUMP_PX = 40 def height_fn(): c = np.array(json.load(open(OUT / "e_fk_fit.json"))["coef"]) def H(q): j1, j2, j3, j4 = [np.radians(q[k]) for k in (1, 2, 3, 4)] b = j1 + j2 f = np.array([1, np.sin(j1), np.cos(j1), np.sin(b), np.cos(b), np.sin(b) * np.cos(j4), np.cos(b) * np.cos(j4), np.sin(b) * np.sin(j4) * np.cos(j3), np.cos(b) * np.sin(j4) * np.cos(j3), np.sin(j4) * np.sin(j3)]) return float(f @ c) return H def find(f): hsv = cv2.cvtColor(f, cv2.COLOR_BGR2HSV) red = cv2.inRange(hsv, (0, 120, 70), (10, 255, 255)) | cv2.inRange(hsv, (170, 120, 70), (180, 255, 255)) blue = cv2.inRange(hsv, (100, 150, 80), (130, 255, 255)) n, _, st, cen = cv2.connectedComponentsWithStats(red) rs = [(st[k][4], cen[k]) for k in range(1, n) if st[k][4] >= 300 and max(st[k][2], st[k][3]) < 3 * min(st[k][2], st[k][3])] n, _, st, cen = cv2.connectedComponentsWithStats(blue) bs = [cen[k] for k in range(1, n) if st[k][4] >= 60] if not rs or len(bs) < 2: return None rc = max(rs, key=lambda x: x[0])[1] best = None for i in range(len(bs)): for j in range(i + 1, len(bs)): g = np.hypot(*(bs[i] - bs[j])) if 60 <= g <= 150: mid = (bs[i] + bs[j]) / 2 d = np.hypot(*(rc - mid)) if best is None or d < best[0]: best = (d, g) return None if best is None else (best[0], best[1], rc) def main(): H = height_fn() sel = json.load(open(OUT.parent / "B" / "selection.json")) res, series = [], [] for run in sel["failures"] + sel["successes"]: rec = RUNS / run / "recording" st = list(csv.DictReader(open(rec / "states.csv"))) gr = [float(r["ts"]) for r in st if r["event_type"] == "gripper_cmd" and r["gripper_state"] == "1"] if not gr: res.append({"run": run, "note": "閉じる命令の記録なし"}); continue tc = gr[0] t_open = min([float(r["ts"]) for r in st if r["event_type"] == "gripper_cmd" and r["gripper_state"] == "0" and float(r["ts"]) > tc] or [np.inf]) t_home = min([float(r["ts"]) for r in st if r["event_type"] == "stage" and r["stage"] == "home" and float(r["ts"]) > tc] or [np.inf]) act = [(float(r["ts"]), [float(r[f"j{k}"]) for k in range(6)]) for r in st if r["event_type"] == "actual" and r["j0"]] at = np.array([a[0] for a in act]) def q_at(t): k = max(0, int(np.searchsorted(at, t, side="right") - 1)); return act[k][1] h0, j00 = H(q_at(tc)), q_at(tc)[0] fl = [(int(r["frame_idx"]), float(r["capture_ts"])) for r in csv.DictReader(open(rec / "top_frame_log.csv"))] ts = np.array([t for _, t in fl]) cap = cv2.VideoCapture(str(rec / "top.mp4")) t_end = min(t_open, t_home, ts[-1]) base, lost, run_hits = [], None, 0 for t in np.arange(tc + 1.0, t_end, 0.5): i = int(np.searchsorted(ts, t)) if i >= len(fl): break cap.set(cv2.CAP_PROP_POS_FRAMES, fl[i][0]); ok, f = cap.read() m = find(f) if ok else None q = q_at(t) row = {"run": run, "t_s": round(t - tc, 1), "h_rise_mm": round(H(q) - h0, 1), "j0_turn_deg": round(q[0] - j00, 1), "sep_px": None if m is None else round(m[0], 1), "marker_gap_px": None if m is None else round(m[1], 1)} series.append(row) if m is None: continue if t - tc <= 2.0: base.append(m[0]); continue b = float(np.median(base)) if base else None if b is not None and lost is None: run_hits = run_hits + 1 if m[0] > b + SEP_JUMP_PX else 0 if run_hits >= 2: lost = row res.append({"run": run, "sep_base_px": None if not base else round(float(np.median(base)), 1), "lost_t_s": None if lost is None else lost["t_s"], "lost_h_rise_mm": None if lost is None else lost["h_rise_mm"], "lost_j0_turn_deg": None if lost is None else lost["j0_turn_deg"], "lost_sep_px": None if lost is None else lost["sep_px"], "max_h_rise_until_lost_or_end_mm": max([s["h_rise_mm"] for s in series if s["run"] == run and (lost is None or s["t_s"] <= lost["t_s"])] or [None]), "watched_until_s": round(t_end - tc, 1), "end_reason": "開く命令" if t_end == t_open else ("HOME" if t_end == t_home else "記録の終わり")}) cap.release() for name, rows in (("e_track.csv", res), ("e_track_series.csv", series)): keys = [] for r in rows: keys += [k for k in r if k not in keys] with open(OUT / name, "w", newline="") as fo: w = csv.DictWriter(fo, fieldnames=keys); w.writeheader(); w.writerows(rows) for r in res: print(r) if __name__ == "__main__": main() `````` ### E のスクリプト e_cause.py ``````python """依頼110 E-6: 失敗12件それぞれに, いちばんありそうな原因を1つ付ける (e_summary / e_close / e_grasp_metrics / e_track をまとめる). 順に見て, 最初に当たったものを原因にする (線は成功4件の幅から置いた. この依頼での目安): 1 爪の間に入っていない (空振り) 閉じた直後の青い印の間 < 90px (成功は 99〜100px = ブロックの幅で止まる) 2 爪の間に入っていない (浅い) 閉じた直後, ブロックが爪先の印の線から先へはみ出した量 > 成功の最大 + 10px 3 動きながら閉じた 閉じる前 0.3 秒の関節の速さ > 5°/s か, 爪を待つ 0.8 秒に 3° 以上動いた 4 持ち上げを出していない 閉じたあと, 人の補正より前のかたまりで π0.5 の生の値が 20mm 以上上げていない 5 制限で削られた 生の値は 20mm 以上なのに, 範囲の制限のあとで 20mm 未満 6 送っても届かない 送った値は 20mm 以上なのに届いた角度で 20mm 未満, かつ成功より大きく足りない 7 分からない 上のどれでもない 出力: e_cause.csv """ import csv from pathlib import Path OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/E_つかみと持ち上げ") def rd(name): return {r["run"]: r for r in csv.DictReader(open(OUT / name))} def f(v): try: return float(v) except (TypeError, ValueError): return None def main(): S, C, G, T = rd("e_summary.csv"), rd("e_close.csv"), rd("e_grasp_metrics.csv"), rd("e_track.csv") succ = [r for r in G.values() if r.get("group") == "successes"] pr_max = max(f(r["after_protrude_px"]) for r in succ) rows = [] for run, s in S.items(): if s["outcome"] != "failure": continue c, g, t = C[run], G[run], T[run] gap, pr = f(g.get("after_marker_gap_px")), f(g.get("after_protrude_px")) v, mv = f(c.get("speed_0.3s_before_close_deg_s")), f(c.get("moved_during_grip_wait_0.8s_deg")) raw, clip, sent, act = (f(s.get(k)) for k in ("model_raw_max_mm", "model_clipped_max_mm", "model_sent_max_mm", "model_actual_max_mm")) if gap is not None and gap < 90: cause = "爪の間に入っていない（空振り）" basis = f"閉じた直後の青い印の間 {gap:.0f}px（成功 99〜100px）、ブロックが印の線から {pr:.0f}px 先" elif pr is not None and pr > pr_max + 10: cause = "爪の間に入っていない（浅い）" basis = (f"閉じた直後、ブロックが爪先の印の線から {pr:.0f}px（約 {pr / 2.6:.0f}mm）先へはみ出す" f"（成功 {min(f(r['after_protrude_px']) for r in succ):.0f}〜{pr_max:.0f}px）。印の間 {gap:.0f}px") elif (v is not None and v > 5) or (mv is not None and mv >= 3): cause = "動きながら閉じた"; basis = f"閉じる前の速さ {v}°/s・爪を待つ間 {mv}°" elif raw is not None and raw < 20: cause = "持ち上げを出していない"; basis = f"π0.5 の生の値の持ち上げ最大 {raw}mm" elif raw is not None and clip is not None and clip < 20: cause = "制限で削られた"; basis = f"生の値 {raw}mm → 制限のあと {clip}mm" else: cause = "分からない" basis = f"閉じた形は成功の幅（はみ出し {pr:.0f}px・印の間 {gap:.0f}px）" lost = t.get("lost_t_s") rows.append({ "run": run, "cause": cause, "basis": basis, "after_marker_gap_px": gap, "after_protrude_px": pr, "speed_before_close_deg_s": v, "moved_during_grip_wait_deg": mv, "model_raw_lift_max_mm": raw, "model_clipped_max_mm": clip, "model_sent_max_mm": sent, "model_reached_max_mm": act, "n_model_chunks_before_human": s.get("n_model_chunks_before_correction"), "lift_by_human_correction_mm": s.get("lift_by_correction_mm"), "block_left_fingers_t_s": lost or "", "block_left_fingers_tip_rise_mm": t.get("lost_h_rise_mm") or "", "max_tip_rise_mm": t.get("max_h_rise_until_lost_or_end_mm"), "state_gap_close_deg": c.get("state_gap_close_chunk_deg"), "state_gap_next_deg": c.get("state_gap_next_chunk_deg"), }) keys = list(rows[0].keys()) with open(OUT / "e_cause.csv", "w", newline="") as fo: w = csv.DictWriter(fo, fieldnames=keys); w.writeheader(); w.writerows(rows) from collections import Counter print(Counter(r["cause"] for r in rows)) for r in rows: print(r["run"], r["cause"], "|", r["basis"], "| lost", r["block_left_fingers_t_s"], r["block_left_fingers_tip_rise_mm"]) if __name__ == "__main__": main() `````` ### F_上カメラの上の端/README.md ``````markdown # F. 本線の上カメラの上の端のずれを調べる（2026-11-07・依頼110） **読むだけ。** 本線のコード（`visual_servo_pick.py`・`mat_corner_detection.py`）は変えていない（四隅の検出の関数を読み取り専用で借りただけ）。`runs/`・`datasets/`・`diag/` は読むだけ。実機には触れていない。 ## 先に読むこと（結論） | 問い | 答え | |---|---| | 1 本線の四隅の扱い | `mat_corner_detection.detect_corners` が、黒いマットのいちばん大きい輪郭の凸包を4頂点に近似し、x+y・x−y で左上・右上・右下・左下に並べる。`visual_servo_pick.setup_homography_and_mat_range` が、その**左上・右上を (+310, −89)・(−310, −89) mm**、右下・左下を (−310, +361)・(+310, +361) mm に対応させてホモグラフィを作る。**検出した点が画像の端（v=0）にあるかは見ていない**。マットが画像の上の端で切れていると、凸包の頂点は「マットの縁と画像の上の端が交わる点」になり、それがそのまま z=−89（マットの物理的な上の端）として扱われる | | 2 切れている日 | 見た 30 日のうち **11 日**（大きく切れている 8 日: 10/19・10/21・10/22・10/24・10/30・10/31・11/01・11/06／わずか 3 日: 7/11・7/13・10/29）。写っている日 19 日（6/19〜7/20 のうち 7/11・7/13 以外の 16 日と 10/15・10/16・10/17）。**10/19 からはずっと切れている**（10/19 の教えたデモから π0.5 の推論の日まで） | | 3 ずれの大きさ | 大きく切れている日、画像の上の端は本当は z=−54〜−73mm（本線の決めより 16〜35mm 根元寄り）。本線で出す位置は**根元の側（z が小さい側）へずれる**。教えた位置5のあたりで 4〜9mm、マットの中央で 8〜19mm、手前（z=330）で 1〜3mm、奥（z=0）で 13〜31mm。x は 0（左右対称と置いたため） | | 4 補正記憶・台帳 | **補正記憶（`diag/correction_memory.json`）・置く高さの記憶・置き場所の補正は、どれも 10/16 が最後の更新で、大きく切れている日の値は入っていない**。わずかに切れた 7/11・7/13 の値は入っている見込み（ずれは 2mm 以下）。**台帳（`runs/pick_place_ledger_v3.csv`）の 10/19 の 39 行・10/22 の 4 行のブロックと箱の座標は、切れた画のままの値**（ブロック 5.5〜22.3mm・箱 7.2〜10.0mm、いずれも根元寄り）。依頼107 の B のブロック・白箱の位置も同じ | | 5 直し方の案 | ①上の端が切れていたら、上の2点の z をマットの方眼から出して対応させる（このフォルダのやり方）。②上の端が切れていたら止める・警告し、カメラをマット全体が写る位置に付け直す（ただし π0.5 の学習データは 10/19 から切れた画）。どちらも本線の変更なので、外部レビューを挟む | ## 1. 本線で、マットの四隅をどう検出し、どの点を (±310, −89) に対応させているか | ファイル:行 | 中身 | |---|---| | `mat_corner_detection.py:42-141` `detect_corners` | 灰色にして大津の二値化（マット=黒を前景）→ 15×15 の閉じ・開き → いちばん大きい輪郭 → 凸包（`:98`）→ `approxPolyDP` で 4 頂点（`:109`、ならなければ係数を変えて `:119`）→ `sort_corners` | | `mat_corner_detection.py:144-158` `sort_corners` | x+y 最小=左上・x−y 最大=右上・x+y 最大=右下・x−y 最小=左下 | | `mat_corner_detection.py:161-191` `remove_outliers_and_average` | 複数コマの四隅を中央値 ±2σ（最低 5px）で外れを除いて平均 | | `visual_servo_pick.py:262-290` `setup_homography_and_mat_range` | 四隅を検出して平均 → **`:274-279` で 左上=(+310, −89)・右上=(−310, −89)・右下=(−310, +361)・左下=(+310, +361)** → `:281` `cv2.getPerspectiveTransform`。`:284-290` で四隅の画素の範囲を `_mat_px_range` に残す（切れている日は上が 0） | | `visual_servo_pick.py:293-301` `pixel_to_unity` | 画素 → このホモグラフィ → mm → Unity の m | | `visual_servo_pick.py:818-821` `mm_to_px`・`:848-867` `capture_rectified` | 同じ (±310, −89)・(±310, +361) で真上から見た画を作る | | `dynamic_tracking_limits/place/place_modules/run_pick_place.py:136-151・682・791・810` | 本線の `setup_homography_and_mat_range`・`pixel_to_unity` をそのまま使う（独自のホモグラフィは持たない） | −89・+361 の出どころは「アームの根元はマットの左右の中央・上の端から 89mm・下の端から 361mm」（`r2r2r-content/methodology/arm_kinematics.md:194`、DAY164 の記事。マットは 620×450mm）。つまり (±310, −89) は**マットの物理的な上の2隅**で、画像に上の端が写っている前提の値。 同じ (±310, −89) を使う古いスクリプトも残っている（`calibrate_visual_servo.py`・`recalibrate_homography.py`・`verify_dynamic_pick_auto_correct.py`・`debug_detection.py`）。 ## 2. マットの上の端が写っている日と、切れている日 日ごとの最初のコマ（それぞれの置き場で最も早い試行。真っ暗なコマは飛ばしてその日の次の試行）に本線の `detect_corners` をかけ、左上・右上の点の v が 6px 以下なら「切れている」とした。384×216 のコマは 5 倍に広げてから検出した。 一覧画像: **`f_days_montage.jpg`**（緑の枠=写っている・赤の枠=切れている。黄の丸が検出した四隅）。1枚ずつは `f_first_frames/{置き場}_{日}.jpg`。値はすべて `f_days.csv`。 | 日 | 置き場（使ったコマ） | 左上 v・右上 v（px） | 判定 | 画像の上の端の z（mm） | |---|---|---|---|---| | 6/19〜7/7（12 日） | run_pick_place（上カメラの jpg） | 30〜137 | 写っている | — | | 7/10 | run_pick_place | 27・42 | 写っている | — | | **7/11** | run_pick_place | **0**・45 | 切れている（左上の隅だけ。方眼では上の端が −94 で、ほぼマットの縁） | −93.7 | | 7/12 | run_pick_place | 26・28 | 写っている | — | | **7/13** | run_pick_place | **0・0** | 切れている（わずか） | −87.4 | | 7/15・7/20 | run_pick_place | 96〜116 | 写っている | — | | 10/15 | run_pick_place（jpg）・pi05_policy（384×216）・datasets | 79〜105 | 写っている | — | | 10/16 | run_pick_place（top.mp4）・datasets | 86〜104 | 写っている | — | | 10/17 | pi05_policy（384×216） | 96・107 | 写っている | — | | **10/19** | run_pick_place（top.mp4）・datasets（teach_v1） | **0・0** | 切れている | −64.0（方眼）／−60.9（比） | | **10/21** | pi05_policy（384×216。最初の試行は真っ暗で次の試行） | **0・0** | 切れている | −54.4（比） | | **10/22** | pi05_policy（384×216） | **0・0** | 切れている | −71.2（比） | | **10/24** | pi05_policy（384×216） | **0・0** | 切れている | −72.8（比） | | **10/29** | pi05_policy（384×216） | **0・0** | 切れている（わずか） | −83.1（比） | | **10/30** | pi05_policy（top.mp4） | **0・0** | 切れている | −56.7（方眼） | | **10/31** | pi05_policy（top.mp4）・datasets（corr_v1） | **0・0** | 切れている | −55.8（方眼）／−54.0（比） | | **11/01** | pi05_policy（top.mp4）・datasets（corr_v4） | **0・0** | 切れている | −53.8（方眼）／−54.8（比） | | **11/06** | pi05_policy（top.mp4）・datasets（corr_v5） | **0・0** | 切れている | −63.5（方眼）／−64.5（比） | - 同じ日の置き場どうしで判定は食い違わなかった - 左・右・下の隅が画像の端にかかった日はなかった - 6〜7月は腕がつかむ途中のコマ（上カメラの jpg）だが、四隅は検出できている ### 画像の上の端が、マットの本当はどこに当たるか **方眼を数えた（1920×1080 のコマ）**: マットには 1cm の方眼と 5cm ごとの数字が刷ってあり、いちばん下の線が「0」（マットの下の端から約 10mm）。左の縁の内側（縁から 70〜220px）の帯で横線を拾って下から数え、画像の上の端（v=0）と検出した左上の点が何 cm の線に当たるかを出した。「k cm」の線は z = 351 − 10k mm。 - **確かめ**: 上の端が写っている 1920×1080 の日（7/10 を除く 17 日）で、検出した左上の隅は 43.6〜44.0cm 相当 = **z −85〜−89mm**（本線の決め −89mm と 4mm 以内）。7/10 だけは線の数え違いで 40.9cm になった（下の端から「0」の線まで 38mm と出ていて、ほかの日の 7〜11mm と合わない） - 下の端から「0」の線までは 7〜11mm（7/10 の 38mm・7/20 の 20.5mm を除く） **比から推し量った（384×216 だけの日）**: 下の辺の画素の幅（= 620mm）と、下の端から画像の上の端までの画素の高さの比に、1920×1080 の切れている 5 日で合わせた係数（1.009）をかけた。合わせた 5 日での残りは最大 2.7mm。同じ日に方眼の値もある 10/19・10/31・11/01・11/06 では、方眼との差は 1〜3mm。 依頼107 の D（カメラの位置と向きを解いた方）では 11/01 の上の端が z≈−58・−54mm（左・右）で、方眼の −53.8mm（左）と 4mm 以内。 ## 3. 切れている日に、本線で測った位置がどれだけずれるか 本当の対応（上の2点 = マットの左右の縁 x=±310mm の、上の端の z）と、本線の対応（上の2点 = (±310, −89)）でホモグラフィを2つ作り、マットの上の点を「本当の対応で画素へ → 本線の対応で mm へ」戻したときの差（本線 − 本当）。左右の上の点の z は同じとした（D では左右で約 4mm 違った）。x のずれは、この置き方では 0。値は `f_shift.csv`、上の端の z は `f_ztop.csv`。 z のずれ（mm。− は根元の側＝画像の上の側へずれる）: | 日（上の端の z） | 教えた位置5 (−70, 253) | π0.5 のブロックのあたり (−55, 248) | 白箱の中心 (−204, 236) | 教えた位置1 (162, 213) | マットの中央 (0, 136) | 手前 (0・±250, 330) | 奥の中央 (0, 0) | |---|---|---|---|---|---|---|---| | 7/11（−93.7） | +1.1 | +1.2 | +1.3 | +1.5 | +2.3 | +0.3 | +3.7 | | 7/13（−87.4） | −0.4 | −0.4 | −0.4 | −0.5 | −0.8 | −0.1 | −1.3 | | 10/19（−64.0） | −6.4 | −6.6 | −7.4 | −8.7 | −13.2 | −1.8 | −21.2 | | 10/21（−54.4・比） | −9.0 | −9.4 | −10.4 | −12.4 | −18.8 | −2.6 | −30.1 | | 10/22（−71.2・比） | −4.5 | −4.7 | −5.2 | −6.1 | −9.3 | −1.3 | −14.9 | | 10/24（−72.8・比） | −4.0 | −4.2 | −4.7 | −5.5 | −8.4 | −1.2 | −13.5 | | 10/29（−83.1・比） | −1.4 | −1.5 | −1.6 | −2.0 | −3.0 | −0.4 | −4.8 | | 10/30（−56.7） | −8.4 | −8.7 | −9.7 | −11.5 | −17.4 | −2.4 | −27.9 | | 10/31（−55.8） | −8.6 | −9.0 | −10.0 | −11.8 | −17.9 | −2.5 | −28.8 | | 11/01（−53.8） | −9.2 | −9.6 | −10.6 | −12.6 | −19.1 | −2.6 | −30.6 | | 11/06（−63.5） | −6.5 | −6.8 | −7.5 | −8.9 | −13.5 | −1.9 | −21.7 | - 下の2隅（z=361）は本当の角なので、手前ほどずれが小さく、奥ほど大きい - 本線は「画像の上の端 = −89」として上下に引き伸ばすので、マットの上の点は**根元の側に寄って**出る。依頼107 の README は「ブロックのあたりでは本線の座標が約 9mm 手前にずれる」と書いているが、計算では z が小さい側（根元の側）へのずれ。大きさ（約 9mm）は合っている - ブロックの上面（高さ 24mm）や爪の印（さらに高い）を床の点として変換するときの、高さによるずれはこの表に入っていない（依頼107 で「さらに数 mm」としたもの） ## 4. 補正記憶・台帳にこのずれが入り込んでいそうか（読むだけ） | ファイル | 最後の更新 | 切れている日の値 | |---|---|---| | `diag/correction_memory.json`（つかむ位置の補正。マス目ごとの dx・dz） | 10/16 19:43 | 大きく切れている日（10/19 以降）は**入っていない**。7/11（補正のマスの行 37）・7/13（32 行）は入っている見込みだが、その日のずれはつかむ位置のあたりで 2mm 以下 | | `diag/place_height_memory.json` | 10/16 17:53 | 同上 | | `diag/place_release_offset.json` | 10/16 19:15 | 同上 | | `runs/pick_place_ledger_v3.csv`（台帳） | 10/22 17:34 | **10/19 の 39 行・10/22 の 4 行の `block_pick_x/z`・`box_x/z` は切れた画のホモグラフィの値**。この日は補正（`tip_block_dx_mm`・`corr_cell_key`）は使っていない。その日の最初のコマの四隅で計算し直すと、ブロックの位置（10/19 は z=−41〜241 とばらつく）は 10/19 が 6.6〜22.3mm・10/22 が 5.5〜5.7mm、箱（z=178〜191）は 10/19 が 9.5〜10.0mm・10/22 が 7.2mm、いずれも根元の側にずれている | | `runs/handover_ledger_v1.csv` | 7/20 | 写っている日だけ | - 補正記憶の dx・dz は、同じ画の中の爪先とブロックの差から作っているので、切れた日のものでも差の部分のずれは打ち消し合う方向（打ち消しきるかは高さが違うので分からない）。ただし、どのマスに入れるか（ブロックの位置）はずれる - π0.5（`pi05/run_policy_client.py`）は画をそのまま渡していて、このホモグラフィは使っていない。影響を受けたのは、本線（`run_pick_place.py`）を 10/19 以降に走らせた試行と、依頼107 の B（ブロック・白箱の位置） - **これから本線を今のカメラの付け方で走らせると**、ブロックの位置が教えた位置5のあたりで 6〜9mm 根元寄りに出て、補正記憶（写っている日に作ったもの）のマスと目標がその分ずれる見込み ## 5. 直し方の案（直すのは別の依頼。本線の変更は外部レビューを挟む） 1. **上の端が切れていたら、上の2点の z をマットの方眼から出す**: `setup_homography_and_mat_range` で、左上・右上の v が数 px 以下なら、(±310, −89) ではなく、方眼を数えて出した z（このフォルダの `f_corners.py` の `grid_lines`・`cm_at` と同じ考え）を上の2点に対応させる。カメラの付け直しがそのまま使え、π0.5 の学習データの画角も変えない。方眼の数え違い（7/10 のように 3cm 外れる）を見つける検査が要る（下の端から「0」の線までが 7〜11mm に入るか、など） 2. **上の端が切れていたら止める（または警告する）**: 検出した四隅のどれかが画像の端にかかっていたら、ホモグラフィを作らずに止め、カメラをマット全体が写る位置（6/19〜10/17 の付け方）に付け直す。仕組みは簡単だが、10/19 以降の π0.5 の学習データ（教えたデモ・補正のデータ）は切れた画角なので、π0.5 を使う日と本線を使う日でカメラの付け方を変えることになる ## やり直し方 ```bash cd "/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/F_上カメラの上の端" python3 -I f_corners.py # f_days.csv・f_first_frames/（本線の detect_corners を r2r2r-scripts から読み取り専用で import） python3 -I f_shift.py # f_ztop.csv・f_shift.csv python3 -I f_montage.py # f_days_montage.jpg ``` `f_corners.py` は `python3 -I` でも `sys.path` に `/Users/shimazueiichi/r2r2r-scripts` を足して `mat_corner_detection` を読む（関数を借りるだけで、ファイルは変えない）。 `````` ### F のスクリプト f_corners.py ``````python """依頼110 F-2: 上カメラの日ごとの最初のコマで, 本線の四隅の検出 (mat_corner_detection.detect_corners) をかけ, マットの上の端が画像の中に写っている日と, 画像の上の端で切れている日を分ける (読むだけ). 対象 (日ごとに1枚・それぞれの置き場の中で最も早いもの): runs/run_pick_place/ … recording/top.mp4 の最初のコマ, なければ上カメラの jpg (stage4_pick_tip → pick → place_pre → place) runs/pi05_policy/ … recording/top.mp4 の最初のコマ, なければ frames/0000.jpg (384×216) datasets/*/raw_episodes/ … frames/000000.jpg (384×216) 384×216 は 5 倍に広げてから検出する (本線の検出は 1920×1080 向けのしきい値). 切れている = 左上・右上の検出点の v が 6px 以下 (1920×1080 に直した値). 真っ暗なコマ (平均の明るさ 25 未満) は飛ばし, その日の次の試行を使う. さらに, 1920×1080 のコマでは, マットに刷ってある 1cm の方眼の横線を左の端の内側で数え, 画像の上の端 (v=0) と, 上の2隅として検出された点が, マットの何 cm の線に当たるかを出す. 方眼の「0」の線 = マットの下の端から約 10mm 上 (下の端は z=+361mm) → 「k cm」の線は z = 351 − 10k mm (450mm のマットに 0〜43cm の方眼. 本線の決めの上の端 z=−89mm は「43cm の線より 10mm 上」) 出力: f_days.csv / f_first_frames/{source}_{day}.jpg (検出点を描いたもの) """ import csv import re import sys from pathlib import Path import cv2 import numpy as np sys.path.insert(0, "/Users/shimazueiichi/r2r2r-scripts") from mat_corner_detection import detect_corners # noqa: E402 本線の検出を読むだけで使う ROOT = Path("/Volumes/KIOXIA/r2r2r-data") OUT = ROOT / "lab" / "twin" / "20261107_失敗の再生と置き場所" / "F_上カメラの上の端" IMG_DIR = OUT / "f_first_frames" CUT_V = 6.0 DAY = re.compile(r"_(20\d{6})_(\d{6})") def first_mp4_frame(p): cap = cv2.VideoCapture(str(p)); ok, f = cap.read(); cap.release() return f if ok else None def candidates(): """(source, day, stamp, path, kind) を返す.""" out = [] rp = ROOT / "runs" / "run_pick_place" for d in sorted(rp.iterdir()): m = DAY.search(d.name) if not m or not d.is_dir(): continue mp4 = d / "recording" / "top.mp4" if mp4.exists(): out.append(("run_pick_place", m.group(1), m.group(2), mp4, "mp4")); continue for lab in ("stage4_pick_tip", "pick", "place_pre", "place"): j = d / f"{d.name}_{lab}.jpg" if j.exists(): out.append(("run_pick_place", m.group(1), m.group(2), j, f"jpg:{lab}")); break pp = ROOT / "runs" / "pi05_policy" for d in sorted(pp.iterdir()): m = DAY.search(d.name) if not m or not d.is_dir(): continue mp4, j = d / "recording" / "top.mp4", d / "frames" / "0000.jpg" if mp4.exists(): out.append(("pi05_policy", m.group(1), m.group(2), mp4, "mp4")) elif j.exists(): out.append(("pi05_policy", m.group(1), m.group(2), j, "small")) for ds in sorted((ROOT / "datasets").iterdir()): re_ = ds / "raw_episodes" if not re_.is_dir(): continue for e in sorted(re_.iterdir()): m = DAY.search(e.name); j = e / "frames" / "000000.jpg" if m and j.exists(): out.append(("datasets", m.group(1), m.group(2), j, f"small:{ds.name}")) return out def load(path, kind): if kind == "mp4": return first_mp4_frame(path), 1.0 im = cv2.imread(str(path)) if im is None: return None, 1.0 if im.shape[1] < 1000: s = 1920 / im.shape[1] return cv2.resize(im, (1920, round(im.shape[0] * s)), interpolation=cv2.INTER_LINEAR), s return im, 1.0 def grid_lines(img, c): """左の端の内側の帯で方眼の横線を拾う. 戻り値: [(v, k cm)] (下の「0」の線から数えた番号) と周期 px.""" (ulx, uly), (llx, lly) = c[0], c[3] vb = int(lly) prof = [] for v in range(0, vb): xe = ulx + (llx - ulx) * (v - uly) / max(1e-6, (lly - uly)) x0 = int(xe + 70); x1 = int(xe + 220) prof.append(img[v, x0:x1, 2].astype(float).mean() - img[v, x0:x1, 0].astype(float).mean() * 0.5) prof = np.array(prof) k = 31 base = np.convolve(prof, np.ones(k) / k, mode="same") d = prof - base peaks = [v for v in range(2, len(d) - 2) if d[v] == d[v - 2:v + 3].max() and d[v] > 1.5] # 近すぎる山はまとめる merged = [] for v in peaks: if merged and v - merged[-1] < 12: if d[v] > d[merged[-1]]: merged[-1] = v continue merged.append(v) if len(merged) < 10: return None, None sp = np.diff(merged) p = float(np.median(sp[(sp > 15) & (sp < 35)])) if np.any((sp > 15) & (sp < 35)) else None if p is None: return None, None # 下の端から 12px 以上上の, いちばん下の線を「0」とし, 上へ番号を振る lines = [v for v in merged if v < vb - 12] lines = lines[::-1] out = [(lines[0], 0)] for v in lines[1:]: kk = out[-1][1] + max(1, int(round((out[-1][0] - v) / p))) out.append((v, kk)) return out, p def cm_at(lines, v): """v の位置が何 cm の線に当たるか (線の間は直線で補う. 端の外は端の2本で延ばす).""" vs = np.array([a for a, _ in lines], float); ks = np.array([b for _, b in lines], float) o = np.argsort(vs); vs, ks = vs[o], ks[o] if v <= vs[0]: return float(ks[0] + (vs[0] - v) * (ks[0] - ks[1]) / (vs[1] - vs[0])) return float(np.interp(v, vs, ks)) def main(): IMG_DIR.mkdir(exist_ok=True) seen, rows, skipped = set(), [], [] for src, day, stamp, path, kind in candidates(): if (src, day) in seen: continue img, s = load(path, kind) if img is None: continue if float(img.mean()) < 25: # 真っ暗なコマ (カメラの映像が出ていない) はその日の次の試行を使う skipped.append(f"{src} {day} {path.name if hasattr(path, 'name') else path}: 暗い (平均 {img.mean():.0f})") continue c = detect_corners(img) row = {"source": src, "day": day, "first_run_time": stamp, "path": str(path).replace(str(ROOT) + "/", ""), "kind": kind, "scale_to_1920": round(s, 2)} if c is None: row["status"] = "四隅を検出できない" rows.append(row); seen.add((src, day)); continue c = np.asarray(c, float) row.update({f"{n}_{a}": round(float(c[i][j]), 1) for i, n in enumerate(("UL", "UR", "LR", "LL")) for j, a in enumerate(("u", "v"))}) top_v = min(c[0][1], c[1][1]) row["top_cut"] = bool(top_v <= CUT_V) others = [] if min(c[0][0], c[3][0]) <= CUT_V: others.append("左") if max(c[1][0], c[2][0]) >= img.shape[1] - 1 - CUT_V: others.append("右") if max(c[2][1], c[3][1]) >= img.shape[0] - 1 - CUT_V: others.append("下") row["other_cut"] = "・".join(others) if kind == "mp4" or kind.startswith("jpg"): lines, p = grid_lines(img, c) if lines: k0 = cm_at(lines, 0.0) kul = cm_at(lines, float(c[0][1])) row.update({"grid_period_px": round(p, 1), "grid_lines": len(lines), "grid_top_line_cm": lines[-1][1], "bottom_edge_to_0line_mm": round((c[3][1] - lines[0][0]) / p * 10, 1), "image_top_cm": round(k0, 2), "image_top_z_mm": round(351 - 10 * k0, 1), "UL_point_cm": round(kul, 2), "UL_point_z_mm": round(351 - 10 * kul, 1)}) row["status"] = "切れている" if row["top_cut"] else "写っている" vis = img.copy() for i, (u, v) in enumerate(c): cv2.circle(vis, (int(u), int(v)), 14, (0, 255, 255), -1) cv2.putText(vis, f"{src} {day} {row['status'] == '切れている' and 'TOP CUT' or 'top visible'}", (20, 60), cv2.FONT_HERSHEY_SIMPLEX, 1.6, (0, 255, 255), 4) cv2.imwrite(str(IMG_DIR / f"{src}_{day}.jpg"), cv2.resize(vis, (960, round(vis.shape[0] / 2))), [cv2.IMWRITE_JPEG_QUALITY, 85]) rows.append(row); seen.add((src, day)) keys = [] for r in rows: keys += [k for k in r if k not in keys] with open(OUT / "f_days.csv", "w", newline="") as f: w = csv.DictWriter(f, fieldnames=keys); w.writeheader(); w.writerows(rows) print("飛ばしたコマ:", skipped) for r in rows: print(r["source"], r["day"], r["kind"][:20], r["status"], r.get("UL_v"), r.get("UR_v"), r.get("other_cut"), "top_z", r.get("image_top_z_mm"), "toplines", r.get("grid_top_line_cm"), "p", r.get("grid_period_px"), "bm", r.get("bottom_edge_to_0line_mm")) if __name__ == "__main__": main() `````` ### F のスクリプト f_shift.py ``````python """依頼110 F-3: 上の端が切れている日に, 本線のホモグラフィ (上の2点を (±310, −89) とする) で測った位置が どれだけずれるかを計算する (読むだけ). 正しい対応: 上の2点は「マットの左右の縁 x=±310 の, 方眼で数えた z (f_days.csv の image_top_z_mm)」. 1920×1080 のコマがない日 (384×216 だけの日) は, 下の辺の幅と高さの比から z を推し量る (1920×1080 の切れている日で係数を合わせる. 粗い見積もり). 左右の上の点の z は同じとした (依頼107 D のカメラ合わせでは左右で約 4mm 違った). 本線の対応: 上の2点を (±310, −89). ずれ = 本線で出る位置 − 本当の位置 (x, z mm. z は + が手前 = 根元から遠い向き). 出力: f_shift.csv (日 × 点) / f_ztop.csv (日ごとの上の端の z) """ import csv from pathlib import Path import cv2 import numpy as np OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/F_上カメラの上の端") MAIN_TOP_Z = -89.0 POINTS = [ # 名前, x, z (本線のマット座標 mm. Unity の x・z と同じ向き) ("教えた位置5 (T5)", -70.0, 253.0), ("π0.5 のブロックのあたり (依頼107 B)", -55.0, 248.0), ("白箱の中心 (依頼107 B)", -204.0, 236.0), ("教えた位置1 (T1)", 161.6, 212.8), ("マットの中央", 0.0, 136.0), ("手前の中央", 0.0, 330.0), ("手前の左", 250.0, 330.0), ("手前の右", -250.0, 330.0), ("奥の中央", 0.0, 0.0), ] def H_of(c, ztop_l, ztop_r): src = np.array(c, np.float32) dst = np.array([[310, ztop_l], [-310, ztop_r], [-310, 361], [310, 361]], np.float32) return cv2.getPerspectiveTransform(src, dst) def apply(H, p): return cv2.perspectiveTransform(np.array([[p]], np.float32), H)[0][0] def main(): days = [r for r in csv.DictReader(open(OUT / "f_days.csv")) if r.get("status") == "切れている"] # 1920×1080 の日で, 高さ/幅 の比から z を出す係数を合わせる def ratio_est(r): w = float(r["LR_u"]) - float(r["LL_u"]); h = float(r["LL_v"]) - 0.0 return h * 620.0 / w fit = [(ratio_est(r), 361 - float(r["image_top_z_mm"])) for r in days if r.get("image_top_z_mm")] fit = [(a, b) for a, b in fit if b < 440] # 7月のわずかに切れた日 (上の端がほぼマットの縁) は外す k = float(np.mean([b / a for a, b in fit])) resid = [b - k * a for a, b in fit] zrows, srows = [], [] for r in days: c = [(float(r[f"{n}_u"]), float(r[f"{n}_v"])) for n in ("UL", "UR", "LR", "LL")] if r.get("image_top_z_mm"): zt, how = float(r["image_top_z_mm"]), "方眼を数えた (1920×1080)" else: zt, how = 361 - k * ratio_est(r), f"幅と高さの比から推し量った (係数 {k:.3f}・合わせた{len(fit)}日の残り最大 {max(abs(x) for x in resid):.1f}mm)" note = "" if r.get("other_cut"): note = f"上のほかに {r['other_cut']} も画像の端で切れている (下の2隅も本当の角ではない)" zrows.append({"source": r["source"], "day": r["day"], "kind": r["kind"], "image_top_z_mm": round(zt, 1), "top_shift_vs_main_mm": round(zt - MAIN_TOP_Z, 1), "how": how, "note": note}) if note: continue Hm = H_of(c, MAIN_TOP_Z, MAIN_TOP_Z) Ht = H_of(c, zt, zt) Ht_inv = np.linalg.inv(Ht) for name, x, z in POINTS: px = apply(Ht_inv, (x, z)) xm, zm = apply(Hm, px) srows.append({"source": r["source"], "day": r["day"], "image_top_z_mm": round(zt, 1), "point": name, "true_x_mm": x, "true_z_mm": z, "main_x_mm": round(float(xm), 1), "main_z_mm": round(float(zm), 1), "dx_mm": round(float(xm - x), 1), "dz_mm": round(float(zm - z), 1), "dist_mm": round(float(np.hypot(xm - x, zm - z)), 1), "pixel_u": round(float(px[0]), 1), "pixel_v": round(float(px[1]), 1), "in_image": bool(px[1] >= 0)}) for name, rows in (("f_ztop.csv", zrows), ("f_shift.csv", srows)): keys = [] for r in rows: keys += [kk for kk in r if kk not in keys] with open(OUT / name, "w", newline="") as f: w = csv.DictWriter(f, fieldnames=keys); w.writeheader(); w.writerows(rows) print("k", round(k, 4), "fit days", len(fit), "resid", [round(x, 1) for x in resid]) for r in zrows: print(r) for r in srows: if r["point"] in ("教えた位置5 (T5)", "マットの中央", "手前の中央", "奥の中央"): print(r["source"][:8], r["day"], r["image_top_z_mm"], r["point"], r["dx_mm"], r["dz_mm"]) if __name__ == "__main__": main() `````` ### F のスクリプト f_montage.py ``````python """依頼110 F-2: f_first_frames/ の日ごとの最初のコマ (検出した四隅を黄の丸で描いたもの) を1枚に並べる. 出力: f_days_montage.jpg""" import csv from pathlib import Path import cv2 import numpy as np OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/F_上カメラの上の端") rows = list(csv.DictReader(open(OUT / "f_days.csv"))) tiles = [] for r in rows: p = OUT / "f_first_frames" / f"{r['source']}_{r['day']}.jpg" im = cv2.imread(str(p)) if p.exists() else np.full((270, 480, 3), 40, np.uint8) im = cv2.resize(im, (384, 216)) col = (80, 80, 255) if r.get("status") == "切れている" else (120, 255, 120) cv2.rectangle(im, (0, 0), (383, 215), col, 4) z = r.get("image_top_z_mm") or "" cv2.rectangle(im, (0, 190), (384, 216), (0, 0, 0), -1) cv2.putText(im, f"{r['source'][:8]} {r['day']} {'CUT' if r.get('status') == '切れている' else 'ok'} {('top z=' + z) if z and r.get('status') == '切れている' else ''}", (6, 209), cv2.FONT_HERSHEY_SIMPLEX, 0.5, col, 1, cv2.LINE_AA) tiles.append(im) while len(tiles) % 5: tiles.append(np.zeros((216, 384, 3), np.uint8)) grid = np.vstack([np.hstack(tiles[i:i + 5]) for i in range(0, len(tiles), 5)]) cv2.imwrite(str(OUT / "f_days_montage.jpg"), grid, [cv2.IMWRITE_JPEG_QUALITY, 85]) print(grid.shape) `````` ## 学習データの30本のうち7本が、同じくらい浅いお手本だった（依頼113） π0.5 がブロックの1〜2cm手前で閉じる癖を、学習データから覚えたのではないかを調べました（17:18〜17:34）。最初に疑ったのは、お手本の中で爪を閉じたことにする時刻が、アームが止まる前になっていることでした。前に作ったスクリプトのお手本で、爪を開いたことにする時刻が約2秒早いずれが見つかっていたためです。 | 問い | 答え | |---|---| | 閉じる時刻は早かったか | 早くなかった。30本とも、アームが止まってから閉じている。止まりを詰める処理でも、閉じる時刻はずれていない | | お手本は浅くつかんでいたか | 30本中7本が、π0.5 の浅い失敗と同じくらい浅い（教えた動き3・補正入り4） | | どこが浅いか | π0.5 を試している置き場所のお手本15本のうち4本が浅く、4本とも π0.5 自身が閉じた補正入りのお手本。同じ置き場所の教えた動き4本は、成功と同じ深さ | | 人の補正は奥へ入れていたか | 入れていなかった。補正は横の位置合わせだけ | π0.5 が浅くつかんでも、そのまま運べれば成功と判定され、それがお手本として学習データに入っていました。**浅くつかむ癖を、自分の成功で強めていたことになります。** ほかに、教えた動きの2本（191347・191441）では、閉じる瞬間に人の指がブロックを爪の間へ押し込んでいて、その手が学習の画に写っていました。 【画像】LOG460_05_training_grasp.jpg 学習データ30本の、爪を閉じる瞬間の上カメラの切り出しの一覧 ### 依頼113：π0.5 の学習データで、爪を閉じる時刻とつかみの深さを調べる ``````markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 150 --- # π0.5 の学習データで、爪を閉じる時刻とつかみの深さを調べる（依頼110 の続き・2026-11-07） **読むだけの依頼。** コードは変えない。実機・Raspberry Pi・daemon・SSH・シリアルには触れない。KIOXIA の `runs/`・`datasets/` は読むだけ。 書いてよいのは `/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/G_お手本の閉じる時刻/` の中だけ（調べるためのスクリプトもここに置く）。KIOXIA がつながっていなければ止める。 ## これまで分かったこと（依頼110 の E。`lab/twin/20261107_失敗の再生と置き場所/E_つかみと持ち上げ/README.md`） - π0.5 の失敗12件のうち9件は、ブロックが爪の間に十分入る前に爪を閉じていた（浅い7件：ブロックが爪先の青い印の線から 32〜40px＝約 12〜15mm はみ出したまま閉じた。空振り2件：爪がブロックの手前で閉じた）。成功は −14〜18px - 閉じる直前のはみ出しも、失敗は成功より大きい（浅い 55〜64px・空振り 58〜86px、成功 16〜45px）。π0.5 が「閉じる」と決めた位置が 1〜2cm 手前だった形 - 持ち上げは12件とも出していた。動きながら閉じた・制限で削られた・命令が無視された、はどれも0 - 測り方は E のスクリプト（`e_grasp_metrics.py`・`e_images.py`）にある。同じ測り方を使う ## 確かめたいこと π0.5 が早めに閉じる癖を、**学習データから覚えたのではないか**。とくに、お手本の中で爪のラベル（`state[:,6]` の 0→1）が、アームがブロックの奥まで進みきる前に切り替わっていないか。前に scripted_v1 では、開くラベルが約2秒早いずれが見つかっている（`datasets/mecharm_redblock_box_scripted_v1/KNOWN_ISSUE_gripper_open_label_2sec_early.md`）。 対象：v3 の学習に使った `datasets/mecharm_redblock_box_teach_v1_trim2_corr_v5`（30本。教えた動きの再生19本＋補正入り11本）。比べるために、元の `datasets/mecharm_redblock_box_teach_v1`（詰める前）と `mecharm_redblock_box_scripted_v1` も見る。元の記録は、教えた動きが `runs/run_pick_place/{run}/`、補正入りが `runs/pi05_policy/{run}/`（`recording/states.csv`・`top.mp4`・`top_frame_log.csv`）。作ったコードは `pi05/build_demo_episodes.py`・`pi05/build_correction_episodes.py`・詰める処理（trim・trim2・止まりの詰め・角度の抜けの埋め）。 ## やること 1. **ラベルの切り替わりとアームの動き。** 30本それぞれで、爪のラベルが 0→1 になるコマを見つける。そのコマの前後 1.5 秒で、関節（とくに j1・j2・j4）がどれだけ動いたか、爪先がブロックの方へまだ進んでいたか（E の爪先の近似式 `e_fk_fit.json` を使ってよい）を出す。元の記録の爪の命令（`gripper_cmd`）の時刻と、アームが止まった時刻（届いた角度が落ち着いた時刻）と、ラベルの時刻の3つを並べる 2. **詰める処理でずれていないか。** 止まりの詰め・角度の抜けの埋めで、閉じる前後のコマが消えたり、ラベルの位置がずれたりしていないかを、詰める前（teach_v1）と後（trim2・corr_v5）で比べる。コードのどこで決まるかをファイル名と行番号で示す 3. **お手本のつかみの深さ。** E と同じ測り方（爪先の青い印の間・ブロックのはみ出し px）で、30本の「ラベルが 1 になる直前のコマ」と「直後のコマ」を測る。学習データの画は 384×216 に縮めてあるので、元の `top.mp4` の同じ時刻（`top_frame_log.csv`）のコマで測り、E と同じ大きさにそろえる。E の成功・失敗の値と同じ表に並べる 4. **行動のラベル。** 行動は「0.1 秒あとの状態」。ラベルが 1 になるコマの前後で、行動（次の目標）がまだブロックへ進む向きを示しているか 5. **補正入り11本。** 人の補正（ゲームパッド・キーボード）で、閉じる前にどこまで奥へ入れたか。教えた動きの19本と比べて深さがどう違うか 6. **まとめ。** 学習データが「早く閉じる・浅く閉じる」を教えている証拠があるか。あるなら、どの本で・どれだけ。直し方の案を2つ以内（例：ラベルの時刻を止まった時刻に合わせ直す・浅いお手本を外す／撮り直す）。直すのは別の依頼 ## 報告 - G のフォルダに README.md（表・画像の一覧・調べ方・やり直し方）を置く。30本のつかむ瞬間を並べた一覧画像を1枚作る（E の e_grasp_successes.jpg と同じ形） - 依頼箱の報告には、6 のまとめ（証拠があるか・何本・どれだけ）と直し方の案の要点だけを書く - 最後の1行に「判定：合格／止めた（決めてほしいことあり）／失敗」を書く `````` ### 依頼113の報告 ``````markdown # 結果：20261107_113_学習データで爪を閉じる時刻とつかみの深さを調べる - 状態：完了 - 始めた時刻：2026-11-07 17:18 - 終わった時刻：2026-11-07 17:34（17分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 6dafb23b-ff98-465d-984f-1020e945ea71` - 止められた操作：1件（いちばん下に一覧） ## Claude Code の報告 やったこと - v3 の学習に使った `teach_v1_trim2_corr_v5` の 30 本（教えた動き 19・補正入り 11）を調べました。比べるために `teach_v1`、`scripted_v1`、E の失敗 12 件・成功 4 件も同じ形で並べました。読むだけで、コード・データセット・runs・実機には触れていません。 - 各コマが元の記録のどの時刻かを、データを作ったコードと同じ計算で戻しました。作り直した状態は 69 本すべてで元のデータと一致しました（差 0.000°）。 - E の画の測り方は爪が画の真下を向く記録用です。10/15〜10/19 の記録は爪が斜めなので、爪の向きに沿って測る形を足し、E の 16 件も測り直しました。成功の最大＋10px（24px）の線で分けると、E の「浅い 7 件」とまったく同じ 7 件になり、物差しがそろっていることを確かめました。 結果（6 のまとめ） - **「早く閉じる」（時刻）を学習データが教えている証拠はありません。** - 30 本とも、ラベルはアームが止まったあとに 0 から 1 へ切り替わっています。教えた動きは、着いてから約 1〜2 秒待って閉じる命令を出し、ラベルはその 0.01〜0.1 秒あとです。閉じる前の 0.5 秒にアームは動いていません。 - 詰める処理でもラベルの位置は動いていません（30 本とも差 0.0 秒）。 - 変わったのは閉じる前の待ちです。ラベルの前後 1.5 秒は、教えた動きで 31 コマから 10〜14 コマ、補正入り（10/31〜11/01）で 2〜4 コマに減りました。学習の並びでは「下ろし切って 2〜3 コマで閉じる」になっています。 - 閉じる行動のコマで、まだブロックへ進む向きを示す本は 0 本です。 - **「浅く閉じる」を学習データが教えている証拠はあります。** 閉じて 1.0 秒後に、ブロックがまだ爪先の印から 24px を超えてはみ出しているのが 30 本中 7 本です（教えた 3・補正入り 4）。 - π0.5 を試す置き場所のお手本 15 本のうち 4 本（27%）が浅く、4 本とも補正入りで π0.5 自身が閉じたものです（181946・192101・184452・184906。33〜49px、約 13〜19mm）。 - E の浅い失敗は 30〜38px、成功は −19〜14px です。同じ置き場所の教えた動き 4 本は 8〜15px で、成功と同じ深さでした。 - 184452・184906 は閉じる直前のはみ出しが 74〜75px で、E の空振り（63〜85px）と同じくらい手前で閉じています。 - 人の補正は横の位置合わせだけで、奥へ押し込んではいません（爪先の伸び −3.5〜+0.1mm）。 - 別に分かったこと: 教えた動きの 191347・191441 では、閉じる瞬間に人の指がブロックを押し込んでいて、その手が学習の画に写っています。 直し方の案（直すのは別の依頼） 1. 浅いお手本 7 本（とくに試す置き場所の補正入り 4 本）を外すか、撮り直します。補正入りを足すときの採否に「閉じたときの深さ」を入れます。 2. 試す置き場所の深いお手本を増やします。今、この置き場所の教えた動きは 4 本だけで、残り 11 本は π0.5 自身の動きです。 - 「ラベルの時刻を止まった時刻に合わせ直す」は、時刻のずれがなかったので効かないと見ています。 変えたファイル - r2r2r-scripts のコードは変えていません。 - `G_お手本の閉じる時刻/` に次を置きました。 - README.md - `g_analyze.py`・`g_images.py`・`g_report.py` - `g_timing.csv`・`g_steps.csv`・`g_table.csv`・`g_grasp_metrics.csv`・`g_fit.json` - `g_grasp_main.jpg`（30 本のつかむ瞬間の一覧）・`g_grasp_scripted.jpg` - `_check/`（途中で見た確認用の画） - 作業の記録（`~/r2r2r-scripts/logs/`）は、依頼で書いてよい場所の外なので書いていません。 決めてほしいこと - 直し方の案の 1・2 のどちらを（または両方を）次の依頼にするか。 - 「浅い」とみなす線を 24px（E の成功 4 件の最大＋10px。この調べでの仮の目安）のままにするか。 - 人の手が写っている教えた動き 2 本（191347・191441）を学習に残すか。 判定：合格 ## 止められた操作 - {"tool_name": "Bash", "tool_use_id": "toolu_01XQK7QwmZpbyQyMACeS1uQf", "tool_input": {"command": "cd \"/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/G_お手本の閉じる時刻\" && python3 - <<'EOF'\np=\"g_report.py\";s=open(p).read()\ns=s.replace('''「手」: 閉じる前のコマに人の手が写る本 (肌を赤として拾うので閉じる前の値は使えない). 画で確かめて列に書いた.''','''「手」: 閉じる前のコマに人の手が写る本 (画で確かめた. HAND). 肌を赤として拾うと値が 100px を超えるので,\n閉じる前の値で 100px を超えるものは使わない (表では「—」).''')\ns=s.replace(''' # scripted_v1: 閉じる前 1.5 秒〜直前のどれかで手が写り, 値が 100px を超えたもの''',''' `````` ### G_お手本の閉じる時刻/README.md ``````markdown # G. π0.5 の学習データで、爪を閉じる時刻とつかみの深さを調べる（2026-11-07・依頼113。依頼110 E の続き） **読むだけ。** 実機・Raspberry Pi・daemon・SSH・シリアルには触れていない。`datasets/`・`runs/` は読むだけで、書いたのはこのフォルダだけ。r2r2r-scripts のコードは変えていない（`pi05/build_demo_episodes.py`・`pi05/build_correction_episodes.py`・`record_teach_demo.py` は読むだけ。データと対のコードの写し `datasets/mecharm_redblock_box_corr_v5/code/` は、`python3 -B` で読み込んだだけで書いていない）。 対象: v3 の学習に使った `mecharm_redblock_box_teach_v1_trim2_corr_v5` の 30 本（教えた動きの再生 19 本・補正入り 11 本。どちらも元の `teach_v1_trim2` / `corr_v5` とファイルが同じことを確かめた）。比べるために `teach_v1`（詰める前の 19 本）と `scripted_v1`（20 本）、E の失敗 12 件・成功 4 件。 ## 先に読むこと（結論） | 問い | 答え | |---|---| | ラベルは早く切り替わっていたか | **いいえ。30 本とも、アームが止まってから閉じている。** 教えた 19 本: 爪の命令はアームが最後の姿勢に着いた実測の 0.98〜1.02 秒あと（その実測の前は 0.3〜1.1 秒の抜け）、ラベルは命令の 0.01〜0.10 秒あと（10Hz の次のコマ）。閉じる前の 0.5 秒の爪先の動きは伸び ±0.1mm・高さ −0.4〜+0.2mm、0.3 秒の速さ 0.02〜0.53°/s。`record_teach_demo.py` は止まったのを確かめてから閉じ、daemon が返ったあとにラベルの時刻を残す作りで（2862・2899 行）、scripted_v1 の開くラベルのような 2 秒のずれはない。補正入り 11 本も閉じたのは全部 π0.5（model）で、命令はアームが着いた実測の 0.11〜4.44 秒あと | | 詰める処理でラベルがずれたか | **ずれていない。** 30 本とも、エピソードで最初に 1 になるコマの元の時刻は、詰める前の 10Hz の並びで最初に 1 になる時刻と同じ（差 0.0 秒）。閉じる前後のコマ（i−1, i, i+1）も残っている。**変わったのは「閉じる前に止まって待つ時間」が短くなったこと**: 教えた動きはラベルの前後 1.5 秒に 31 コマ → 10〜14 コマ、補正入りの 10/31〜11/01 は 16〜31 コマ → 2〜4 コマ。学習の並びでは、下ろし終えてから 2〜3 コマ（0.2〜0.3 秒ぶん）で閉じることになる | | お手本は浅くつかんでいたか | **一部は浅い。** E の測り方（爪先の青い印の線からのブロックのはみ出し。ここでは爪の向きに沿って測った）で、閉じた 1.0 秒後のはみ出しは 教えた 19 本 4〜39px（中央値 16）・補正入り 11 本 −19〜49px（中央値 20）。π0.5 の成功 4 件は −19〜14px（中央値 6）、浅い失敗 7 件は 30〜38px。E と同じ線（成功の最大 14px＋10px = 24px）を超えるお手本が **30 本中 7 本**（教えた 3・補正入り 4）。うち 5 本は浅い失敗の幅（30px 以上）か、それより浅い | | どこが浅いか | **π0.5 を試す置き場所（ブロックが画の中央・j0 +13° 前後）のお手本 15 本のうち 4 本が浅い。4 本とも補正入りの、π0.5 が自分で閉じたもの**（181946・192101・184452・184906。閉じたあと 33〜49px ≒ 13〜19mm）。同じ置き場所の教えた動き 4 本（194818〜195120）は 8〜15px で、π0.5 の成功と同じ深さ。教えた動きの浅い 3 本（183454・192343・192633）は別の置き場所 | | 行動（0.1 秒あとの状態）は奥へ向いていたか | 教えた 19 本: 閉じる行動のコマ（i−1）で関節の変化 0.01〜0.06°（止まっている）。補正入りの 10/31〜11/01: 0.0〜1.2°・爪先の伸び 0〜2.1mm だが、これは閉じている間の実測の変化（上へ 1〜4mm）を抜けの埋め方（speed20）が閉じるコマへ前倒しで入れたもので、ブロックへ進む向きではない。**閉じるコマで「まだ奥へ進め」を示す行動は 0 本** | | 人の補正は奥へ入れていたか | **入れていない。** 閉じる前の補正（11 本とも 2〜38 コマ）は j0 の横の位置合わせ（+1〜+6°）と少しの下げ（高さ −8〜+1mm）で、爪先の伸びは −3.5〜+0.1mm。閉じる位置を決めたのは補正のあとの π0.5 のかたまり | | 学習データが「早く閉じる・浅く閉じる」を教えている証拠 | **「早く（時刻）」の証拠はない。「浅く」の証拠はある**: 試す置き場所の 15 本のうち 4 本（27%）が、失敗と同じくらい浅いところで閉じた π0.5 自身の動き（成功の判定だったので学習に入った）。とくに 11/06 の 184452・184906 は閉じる直前のはみ出しが 74〜75px で、E の空振り（63〜85px）と同じくらい手前で閉じる判断をしている | ## 1. ラベルの切り替わりとアームの動き 元の記録の時刻の戻し方: 教えた動きは `build_demo_episodes.build_one` と同じ `t0 = max(上カメラの最初, 実測の最初)` に `t_orig` を足した。補正入りは、データと対で残したコード（`corr_v5/code/`）の `select_steps` を同じ引数（`--gap-fill speed20 --trim-hold`）でもう一度計算した（ファイルは書かない）。**戻した時刻で作り直した state は、69 本（30＋teach_v1 19＋scripted 20）すべてでエピソードの state と一致（差 0.000°）**。 - 「止まった時刻」= 閉じる命令の直前の実測の姿勢から 0.5° の中に、命令までずっと入っていた最初の実測。実測は約 3Hz で、送信中は抜ける（教えた動き 約 1 秒・補正入りの 10/31〜11/01 約 1.6 秒）ので、着いたのはその実測より抜けの分だけ前のこともある（「抜け」の列） - 爪先の伸び・高さは近似式（高さは E の `e_fk_fit.json`。伸び＝台の真上からの横の距離は、同じ B の Unity の値に同じ形の式を当てはめた `g_fit.json`。別に取り分けた 585 行で誤差の中央値 3.5mm・95% 10.6mm）。**差（どれだけ動いたか）を見るためだけに使う** | 群 | ラベル − 命令 秒 | 命令 − 止まった実測 秒（その前の抜け 秒） | 閉じる前 0.3 秒 °/s | 前 1.5 秒の j1 / j2 / j4 ° | 前 1.5 秒の伸び / 高さ mm | 最後 0.5 秒の伸び / 高さ mm | 命令のあと 1.5 秒の動き ° | |---|---|---|---|---|---|---|---| | 教えた 19 本 | 0.01〜0.10 | 0.98〜1.02（0.3〜1.1） | 0.02〜0.53 | +0.7〜+8.6 / −8.6〜+0.5 / −0.9〜+6.9 | −8.5〜+3.8 / −31〜−14 | −0.1〜+0.1 / −0.4〜+0.2 | 実測なし（爪を待つ 2 秒は読まない） | | 補正入り 10/31〜11/01（7 本） | 0.00〜0.09 | 1.61〜4.44（1.54〜1.66） | 0.0〜1.2 | −0.3〜0 / −0.6〜0 / −1.2〜+0.1 | 0〜+2.2 / 0〜+3.8 ※ | 0〜+2.2 / 0〜+3.8 ※ | 0.0（3 本。4 本は実測なし） | | 補正入り 11/06（4 本） | 0.05〜0.08 | 0.11〜0.36（0.02〜0.30） | 0.0〜**5.4** | +0.8〜+10.9 / −7.1〜−1.0 / −1.6〜0 | +1.5〜+4.6 / −28〜−0.3 | −0.2〜+4.4 / −2.5〜−0.3 | 1.1〜1.8 | | （比べる）scripted_v1 20 本 | 0.00〜0.09 | 0.59〜0.97（0.02〜2.7） | 0.06〜0.36 | −0.6〜+1.3 / −0.4〜+3.5 / +3.5〜+7.3 | −10〜−3 / −33〜−14 | 0〜+0.1 / −0.1〜+0.3 | 実測なし | ※ 10/31〜11/01 の伸び・高さの変化は、閉じたあと 1.6 秒の抜けのあとに読んだ姿勢（爪が閉じる間に j2 が −0.6° ほど動いた。上へ 1〜4mm）を、speed20 の埋め方が抜けの頭（＝ラベルのコマ）に置いたもの。例: 175527 は命令 078.52 → 次の実測 080.12 で j1 −0.17・j2 −0.62。ブロックへ進む動きではない。 読み取れること - 教えた動きは下ろし終えてから約 1〜2 秒、scripted は 0.6 秒以上止まって待ち、そのあとで閉じている。閉じる前 0.5 秒にアームは動いていない - 11/06 の補正入り 4 本は、送ったらすぐ返る daemon で記録したもので、**閉じる 0.1〜0.4 秒前まで動いていた**（183926・184452・184650 は閉じる前 0.3 秒に 3.2〜5.4°/s、184452・184906 は閉じる前の 1.5 秒で 22〜28mm 下げ、命令のあと 1.5 秒でも 1.1〜1.8° 動いた）。画でも 184452・184906 は閉じる 0.5 秒前から直前までにはみ出しが 8px（約 3mm）減っていて、閉じる直前までわずかに奥へ進んでいた。E の失敗 184312・185050（閉じる前 2.4〜2.6°/s）と同じ形 - 全体の表は `g_table.csv`（1 本 1 行）と `g_timing.csv`（細かい列） ## 2. 詰める処理でずれていないか | データ | ラベルの前後 1.5 秒の元の 10Hz のコマ | エピソードに残ったコマ | エピソードの 15 コマ前は何秒前か | ラベルの位置のずれ | |---|---|---|---|---| | teach_v1（詰める前） | 31 | 30〜31 | 1.5〜3.1 秒 | 0.0 秒 | | trim2（＝v3 の教えた 19 本） | 31 | **10〜14** | **2.2〜7.1 秒** | 0.0 秒 | | corr_v5 10/31〜11/01（7 本） | 16〜31（model / correct のコマ） | **2〜4** | **16.5〜29.2 秒** | 0.0 秒 | | corr_v5 11/06（4 本） | 22〜29 | 7〜14 | 3.0〜12.6 秒 | 0.0 秒 | - **ラベルの位置は動いていない。** ラベルは 10Hz の並びを作った時点で、爪の命令の時刻から決まる（`build_demo_episodes.py` 318〜324 行・`build_correction_episodes.py` 372〜377 行の `np.searchsorted(gt, ti, side="right") - 1`）。そのあとの詰める処理はコマを抜くだけで、残ったコマの state[:,6] は変えない - 閉じる前後のコマは必ず残る: `gripper_guard_mask`（`build_demo_episodes.py` 192 行。`GRIPPER_GUARD_STEPS = 1` は 64 行）で i−1, i, i+1 を守り、止まりの区間でも爪の切り替えの直前・直後の区間は先頭 2 コマを残す（`MID_IDLE_KEEP_STEPS = 2` 60 行、`mid_idle_keep_mask` 209 行・353 行、補正入りは `hold_keep_mask` の `keep_grip` 238・265 行、`HOLD_KEEP_GRIP` 79 行） - **短くなったのは閉じる前の待ち。** 教えた動きの「着いてから約 1 秒待って閉じる」は、`mid_idle_keep_mask`（`--trim-mid-idle`。`build_demo_episodes.py` 352〜361 行）で 2 コマ＋守りの 1 コマになった。補正入りは送信の確かめを待つ止まりを `HOLD_KEEP_MODEL = 0`（77 行）で消すので、10/31〜11/01 は閉じるコマの 1 つ前（i−1）が実時刻で約 3 秒前のコマになっている（175527・184016 で i−2 が −3.1 秒）。止まりの抜けの埋め方は `fill_motion_gaps`（147 行。呼び出し 367 行） - この結果、**学習の並びでは「下ろす動き → 2〜3 コマで閉じる」がつながる**。閉じる前の 10 コマの行動を足すと、trim2 では高さ −22〜−47mm（下ろす動きがまるごと入る）、詰める前の teach_v1 では −1.0〜+0.2mm（止まって待っているだけ）。1 秒（10 コマ）のかたまりの中で「下ろし切ってすぐ閉じる」を教えていることになる。ただし閉じるのはアームが止まった姿勢のコマで、**閉じる位置（深さ）そのものは変えていない** ## 3. お手本のつかみの深さ 測り方: E の `e_grasp_metrics.py` の `measure`（赤の塊の下の端 − 爪先の青い印 2 つの縦の平均）は、爪が画の真下を向いている 10/30 以降の記録のための測り方。10/15〜10/19 の記録はブロックが画の左寄りで、爪が斜め（−19〜−55°）を向いているので、**同じ量を爪の向きに沿って測る形**（`g_images.py` の `measure_axis`。爪の向き＝胴の青い印 → 爪先の印 2 つの中点。はみ出し＝赤の画素を爪の向きに投げた最大 − 爪先の印の中点）にした。E の 16 件もこの形で測り直した（E の値との差 0〜7px。E の「浅い 7 件」の分け方はこの形でも同じ 7 件になる）。 - 測ったコマ: 元の `top.mp4` の、爪の命令の 1.5 秒前・0.5 秒前・直前（E の before と同じ）・エピソードでラベルが 1 になったコマに使った画に近いコマ・命令の 1.0 秒後（E の after と同じ）。画素は元の 1920×1080 のまま（E と同じ大きさ） - 10/19 の一部はブロックが薄いピンクに写り、E の赤の幅（彩度 120 以上）ではブロックの一部しか拾えないので、彩度 60 で拾った面積の半分未満なら彩度 60 の値を使った（`g_grasp_metrics.csv` の `*_axis_s_min`）。183454 は胴の印が写らないので、爪先の印 2 つを結ぶ線に直角な向きを使った（`axis_from=tips`） - **人の手が写るコマがある**: 教えた 191347・191441 は閉じる直前〜ラベルのコマで人の指がブロックを爪の間へ押している。scripted は 182552・185536・175444 が閉じる直前まで、183310・184725・190925 は 1.5 秒前に手が写る。手（肌）を赤として拾うと 100px を超えるので、その値は使わない（表で「—」） - 1.5 秒前・0.5 秒前の値は、爪の高さが違うと上からの見え方が変わるので、閉じる直前とは比べられない（奥へ進んだかは 1 の関節・近似式で見た） | 本 | 種類 | 1.5 秒前 px | 直前 px | ラベルのコマ px | 1.0 秒後 px | 浅い（> 24px） | |---|---|---|---|---|---|---| | 20261031_175527 | 補正入り | 8.5 | 8.5 | 8.5 | −19.4 | | | 20261031_180435 | 補正入り | 41.0 | 40.7 | 40.7 | 20.2 | | | 20261031_181946 | 補正入り | 60.9 | 61.4 | 61.4 | 37.8 | **浅い** | | 20261031_184016 | 補正入り | 44.0 | 44.0 | 44.0 | 15.9 | | | 20261101_190347 | 補正入り | 43.7 | 44.7 | 44.7 | 14.1 | | | 20261101_190854 | 補正入り | 47.0 | 47.0 | 47.0 | 21.4 | | | 20261101_192101 | 補正入り | 57.5 | 57.4 | 57.4 | 33.4 | **浅い** | | 20261106_183926 | 補正入り | 42.2 | 42.1 | 38.5 | 14.1 | | | 20261106_184452 | 補正入り | 67.5 | 75.0 | 75.1 | 49.2 | **浅い** | | 20261106_184650 | 補正入り | 54.9 | 50.3 | 50.3 | 19.8 | | | 20261106_184906 | 補正入り | 73.4 | 73.8 | 73.8 | 47.1 | **浅い** | | 20261019_183454 | 教えた（置き場所 A） | 59.0 | 67.2 | 67.2 | 38.9 | **浅い** | | 20261019_191257 | 教えた（B） | 17.7 | 38.1 | 38.1 | 8.1 | | | 20261019_191347 | 教えた（B） | — | —（手） | — | 19.7 | | | 20261019_191441 | 教えた（B） | — | —（手） | — | 14.5 | | | 20261019_191541 | 教えた（B） | 26.0 | 44.5 | 44.5 | 15.9 | | | 20261019_191635 | 教えた（B） | 25.0 | 43.5 | 44.2 | 18.2 | | | 20261019_192343 | 教えた（C） | 54.8 | 54.5 | 54.5 | 27.6 | **浅い** | | 20261019_192438 | 教えた（C） | 47.6 | 47.9 | 47.9 | 17.9 | | | 20261019_192539 | 教えた（C） | 43.5 | 43.6 | 43.6 | 12.7 | | | 20261019_192633 | 教えた（C） | 59.1 | 59.0 | 59.0 | 28.9 | **浅い** | | 20261019_193404 | 教えた（D） | 32.9 | 51.2 | 51.2 | 21.6 | | | 20261019_193459 | 教えた（D） | 33.8 | 47.6 | 47.6 | 18.8 | | | 20261019_193617 | 教えた（D） | 23.6 | 40.3 | 40.5 | 11.6 | | | 20261019_193825 | 教えた（D） | 15.7 | 28.1 | 28.1 | 4.4 | | | 20261019_193932 | 教えた（D） | 36.0 | 51.8 | 51.8 | 22.1 | | | 20261019_194818 | 教えた（E＝試す置き場所） | 35.5 | 40.2 | 40.2 | 15.3 | | | 20261019_194913 | 教えた（E） | 32.2 | 34.8 | 34.8 | 8.3 | | | 20261019_195023 | 教えた（E） | 36.7 | 40.7 | 41.2 | 13.1 | | | 20261019_195120 | 教えた（E） | 34.5 | 40.2 | 40.2 | 12.7 | | 置き場所 A〜E は教えたファイル（`teach/drag_v2_*.json`）の違い: A `20221013_144348`（1 本）・B `230205`（5 本）・C `231249`（4 本）・D `232345`（5 本）・E `233323`（4 本。PICK の j0 +13.1°。π0.5 を試すときのブロックの位置と同じ）。 E の値と並べる（閉じた 1.0 秒後。爪の向きに沿う測り方。px の目安 2.6px/mm は E と同じ）: | 群 | 閉じる直前 px | 閉じた 1.0 秒後 px（中央値） | 浅い（> 24px） | |---|---|---|---| | E 成功 4 件（π0.5） | 8〜45 | −19〜14（6） | 0 | | E 失敗 12 件（π0.5） | 33〜85 | 4〜76（32）。浅い 7 件は 30〜38、空振り 2 件は 75〜76（印の間 73〜75px） | 7 | | 学習データ 教えた 19 本 | 28〜67 | 4〜39（16） | 3（183454・192343・192633） | | 　うち試す置き場所 E の 4 本 | 35〜41 | 8〜15（13） | 0 | | 学習データ 補正入り 11 本（試す置き場所） | 8〜75 | −19〜49（20） | 4（181946・192101・184452・184906） | | （比べる）scripted_v1 20 本（開いた印の間を 137px にそろえた） | 26〜94 | 1〜41（21） | 7 | - 閉じきった印の間は 30 本とも 97.7〜101px（ブロックを挟んでいる。空振りは 0 本） - 補正入りの 175527 と 190347 は E の成功と同じ記録（値も E の測り直しと同じ） 画像 - **`g_grasp_main.jpg`**: 30 本のつかむ瞬間（1 行 1 本。上から補正入り 11 本・教えた 19 本。左から 命令の 1.5 秒前・直前・ラベルが 1 のコマ・1.0 秒後。字の `pr` が爪の向きに沿うはみ出し px・`gap` が爪先の印の間 px。名前の字の色は 補正入り＝水色・教えた＝緑）。E の `e_grasp_successes.jpg` と同じ切り出し（420×315px を 1.25 倍） - `g_grasp_scripted.jpg`: scripted_v1 20 本（同じ形） - `_check/`: 調べる途中で見た確認用の画（画の全体・手の写る行・191257 の元の画） ## 4. 行動のラベル 行動は「次のコマの state」なので、爪を閉じる行動は i−1 のコマ（`actions[i-1, 6] = 1`）に入る。 | 群 | i−1 の関節の変化 ° | i−1 の爪先の伸び mm | 閉じる前 10 コマの行動の合計 伸び / 高さ mm | |---|---|---|---| | 教えた 19 本（trim2） | 0.01〜0.06 | 0.0〜+0.1 | −8〜+7 / −47〜−22 | | 　（詰める前 teach_v1） | 0.01〜0.06 | 0.0〜+0.1 | −0.3〜+0.1 / −1.0〜+0.2 | | 補正入り 10/31〜11/01（7 本） | 0.0〜1.2 | 0.0〜+2.1（1 の ※ の前倒し。上向き） | −5〜+15 / −32〜−1 | | 補正入り 11/06（4 本） | 0.0〜0.18 | −0.2〜0.0 | +1〜+12 / −28〜−6 | - **閉じる行動のコマで、関節の目標がまだブロックへ進む向きを示している本はない。** 補正入りの 10/31〜11/01 の 0.5〜1.2° は爪が閉じる間の姿勢の変化（上向き） - 閉じる前の 10 コマ（1 かたまりぶん）には、詰めたことで下ろす動き（−22〜−47mm）がまるごと入っている（2 の最後） - 細かい値は `g_steps.csv`（ラベルの前後 ±15 コマ。`act_d_reach_mm`・`act_d_h_mm` が行動 − 状態） ## 5. 補正入り 11 本 - 11 本とも、**爪を閉じたのは π0.5（model のコマ）**。人の補正（10/31〜11/01 の 7 本はキーボード、11/06 の 4 本はゲームパッド）は閉じる前に 2〜38 コマあり、最後の補正から閉じるまで 5.2〜41.8 秒 - 補正の中身は j0 の横の位置合わせ（+1.0〜+6.1°）と少しの下げで、補正の間の爪先の伸び −3.5〜+0.1mm・高さ −8.2〜+0.6mm（近似式）。**奥へ押し込む補正はしていない**。閉じる位置を決めたのは、補正のあとの π0.5 のかたまり - 深さ: 閉じた 1.0 秒後 −19〜49px（中央値 20）。同じ置き場所の教えた 4 本（8〜15px・中央値 13）より浅く、E の成功（中央値 6）より浅い。浅い 4 本（181946・192101・184452・184906）は 33〜49px で、E の浅い失敗（30〜38px）と同じか浅い。どれも人が「成功」とした run なので学習に入った - 11/06 の 4 本は閉じる直前まで動いていて（1）、うち 184452・184906 は閉じる直前のはみ出しが 74〜75px（E の空振りの直前 63〜85px と同じくらい） ## 6. まとめ **学習データが「早く閉じる」（時刻）を教えている証拠はない。「浅く閉じる」を教えている証拠はある。** - 時刻: 30 本ともラベルはアームが止まってから切り替わり、詰める処理でも位置は動いていない（差 0.0 秒）。scripted_v1 の開くラベルのような時刻のずれはない - 深さ: 30 本中 **7 本**が、π0.5 の成功の幅を 10px 以上超えて浅い（閉じた 1.0 秒後 28〜49px ≒ 11〜19mm 先へはみ出したまま）。とくに **π0.5 を試す置き場所のお手本 15 本のうち 4 本（27%）**が浅く、4 本とも π0.5 自身が閉じた補正入りの run（181946・192101・184452・184906。33〜49px ≒ 13〜19mm）。同じ置き場所の教えた動き 4 本は成功と同じ深さ（8〜15px）で、浅いお手本は補正入りから入っている - 補正入りを学習に足すと、π0.5 が「浅いまま閉じても成功の扱い」になった動きを、そのまま次の学習が覚える形になっている。E で見た π0.5 の失敗の「閉じる直前のはみ出し 55〜64px（浅い）・58〜86px（空振り）」は、学習データの補正入り 184452・184906 の 74〜75px、181946・192101 の 57〜61px と重なる - もう 1 つ: 詰めたことで、学習の並びでは「下ろし切って 0.2〜0.3 秒で閉じる」になり、閉じる前に止まって待つ時間（元は 1〜3 秒）がほぼ消えている。これが早く閉じる癖に効くかは、このデータだけでは言えない（閉じる位置そのものは止まった姿勢） 直し方の案（直すのは別の依頼） 1. **浅いお手本を外す（または撮り直す）**: 閉じた 1.0 秒後のはみ出しが 24px を超える 7 本（とくに試す置き場所の補正入り 4 本）を外した corr_v6 を作る。補正入りを足すときは、`build_correction_episodes.py` の採否に「閉じたときの深さ」を入れる（この G の `measure_axis` を使う）か、浅く閉じた run は「成功」でも使わない決まりにする 2. **試す置き場所の深いお手本を増やす**: 今は試す置き場所の教えた動きが 4 本しかなく、残り 11 本は π0.5 自身の動き。ブロックの奥まで入れてから閉じる教えた動き（または、閉じる前に人が奥へ押し込む補正）を足す 「ラベルの時刻を止まった時刻に合わせ直す」は、時刻のずれが見つからなかったので効かないと見た。 ## 気をつけること - 「浅い」の線（24px）は、E の成功 4 件の最大＋10px というこの調べでの目安。成功が 4 件しかない - 上からの画なので、爪がブロックのどの高さをつかんだかは分からない（E と同じ） - 爪先の伸び・高さは Unity から作った近似式。実機とのずれは測っていない - 教えた動きの 191347・191441（と scripted の 3 本）は、閉じる瞬間に人の指がブロックを爪の間へ押している。閉じたあとの深さは測れるが、閉じる直前の画には手が写ったまま学習に入っている - 補正入りで閉じる前後の実測が抜けている（約 1.6 秒）区間は、`fill_motion_gaps`（speed20）の埋め方しだいで行動の値が変わる（1 の ※） ## ファイル | ファイル | 中身 | |---|---| | `g_analyze.py` | 1・2・4・5 の計算。出力 `g_timing.csv`（1 本 1 行。69 本）・`g_steps.csv`（ラベルの前後 ±15 コマ）・`g_fit.json`（爪先の伸びの近似式） | | `g_images.py` | 3 の測りと画像。出力 `g_grasp_metrics.csv`（50 本＋E の 16 件）・`g_grasp_main.jpg`・`g_grasp_scripted.jpg` | | `g_report.py` | `g_table.csv`（1 本 1 行のまとめ）と表を出す | `g_timing.csv` の主な列: `t_cmd` 爪を閉じる命令の時刻／`t_label_minus_cmd_s` ラベル − 命令／`t_cmd_minus_stop_s` 命令 − 止まった実測／`stop_gap_before_s` 止まった実測の前の抜け／`speed_pre0p3_deg_s` 閉じる前 0.3 秒の速さ／`d_j*_pre`・`d_j*_post` ラベルの前・後 1.5 秒の関節の変化／`d_reach_*`・`d_h_*` 爪先の伸び・高さの変化／`grid_steps_pm1p5` 詰める前の前後 1.5 秒のコマ数／`ep_steps_pm1p5` エピソードに残ったコマ数／`t_label_minus_grid_first1_s` ラベルの位置のずれ／`act±k_*` 行動 − 状態／`correct_*` 閉じる前の人の補正。`g_grasp_metrics.csv`: `{tag}_protrude_px` 等は E の測り方そのまま、`{tag}_axis_*` は爪の向きに沿う測り方（tag = pre1p5 / pre0p5 / before / ep_label / after）。 ## やり直し方 ```bash cd "/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/G_お手本の閉じる時刻" python3 -I -B g_analyze.py # g_timing.csv・g_steps.csv・g_fit.json（数秒） python3 -I -B g_images.py # g_grasp_metrics.csv・g_grasp_main.jpg・g_grasp_scripted.jpg（約 1 分） python3 -I -B g_report.py # g_table.csv と表 ``` `-B` は、読み込む `datasets/mecharm_redblock_box_corr_v5/code/` と `E_つかみと持ち上げ/` に `__pycache__` を書かないため（必ず付ける）。 `````` ### G のスクリプト g_analyze.py ``````python """依頼113 G: 学習データ (teach_v1_trim2_corr_v5 の 30 本) で, 爪のラベル (state[:,6]) が 0→1 になる時刻と アームの動き・元の記録の爪の命令 (gripper_cmd)・アームが止まった時刻を並べる. 比べるために teach_v1 (詰める前) と scripted_v1 (20 本) も同じ形で出す. 読むだけ (データセット・runs には書かない). 時刻の戻し方 (エピソードの各コマが元の記録のどの時刻か): 教えた動き (run_pick_place) : build_demo_episodes と同じ t0 = max(上カメラの最初, 実測の最初) に t_orig (trim2) / t (teach_v1・scripted_v1) を足す 補正入り (pi05_policy) : データと対で残したコード (corr_v5/code/) の select_steps を同じ引数 (speed20・trim_hold) で もう一度計算し (ファイルは書かない), 残ったコマの元の時刻を取る どちらも, 戻した時刻で作り直した state がエピソードの state と一致することを確かめる (state_check_max_deg) 出力: g_timing.csv (1 本 1 行) / g_steps.csv (ラベルの前後 ±15 コマのエピソードの中身) / g_fit.json (爪先の近似式) """ import csv import json import sys from pathlib import Path import numpy as np ROOT = Path("/Volumes/KIOXIA/r2r2r-data") DS = ROOT / "datasets" TW = ROOT / "lab" / "twin" / "20261107_失敗の再生と置き場所" OUT = TW / "G_お手本の閉じる時刻" MAIN = "mecharm_redblock_box_teach_v1_trim2_corr_v5" CODE = DS / "mecharm_redblock_box_corr_v5" / "code" sys.path.insert(0, str(CODE)) import build_demo_episodes as bde # noqa: E402 import build_correction_episodes as bce # noqa: E402 from rewind_correct import excluded_windows # noqa: E402 WIN = 1.5 # ラベルの前後で見る秒 STOP_TOL_DEG = 0.5 # 閉じる命令のときの姿勢からこの中にずっと入っていれば「止まっていた」 FPS = 10 # ---------------------------------------------------------------- 爪先の近似式 (高さは E の式. 横の伸びはここで当てはめ) def _feats(J): J = np.atleast_2d(np.asarray(J, dtype=float)) j1, j2, j3, j4 = [np.radians(J[:, k]) for k in (1, 2, 3, 4)] b = j1 + j2 return np.stack([np.ones_like(j1), np.sin(j1), np.cos(j1), np.sin(b), np.cos(b), np.sin(b) * np.cos(j4), np.cos(b) * np.cos(j4), np.sin(b) * np.sin(j4) * np.cos(j3), np.cos(b) * np.sin(j4) * np.cos(j3), np.sin(j4) * np.sin(j3)], 1) def fit_tip(): ch = np.array(json.load(open(TW / "E_つかみと持ち上げ" / "e_fk_fit.json"))["coef"]) B = TW / "B" ang = {(r["run"], r["i"]): r for r in csv.DictReader(open(B / "timeline.csv"))} X, y = [], [] for r in csv.DictReader(open(B / "timeline_metrics.csv")): a = ang[(r["run"], r["i"])] X.append([float(a[f"j{k}"]) for k in range(6)]) y.append(1000 * np.hypot(float(r["tip_x"]), float(r["tip_z"]))) # 台の真上からの横の距離 (Unity) X, y = np.array(X), np.array(y) idx = np.random.default_rng(113).permutation(len(y)); tr, te = idx[:1500], idx[1500:] c, *_ = np.linalg.lstsq(_feats(X[tr]), y[tr], rcond=None) e = np.abs(_feats(X[te]) @ c - y[te]) c, *_ = np.linalg.lstsq(_feats(X), y, rcond=None) info = {"reach": {"n": int(len(y)), "holdout_n": int(len(te)), "holdout_abs_err_median_mm": round(float(np.median(e)), 2), "holdout_abs_err_p95_mm": round(float(np.percentile(e, 95)), 2), "coef": [float(v) for v in c], "note": "B の Unity の爪先 (tip_x, tip_z) の台からの横の距離を E と同じ j1〜j4 の三角関数で当てはめた. " "台の中心は Unity の原点とした. 差 (どれだけ伸びたか) を見るためだけに使う"}, "height": "E_つかみと持ち上げ/e_fk_fit.json の coef をそのまま使う"} json.dump(info, open(OUT / "g_fit.json", "w"), ensure_ascii=False, indent=1) return (lambda q: float((_feats(q) @ ch)[0])), (lambda q: float((_feats(q) @ c)[0])) H, R = fit_tip() # ---------------------------------------------------------------- 元の時刻を戻す def demo_grid(rec): """build_demo_episodes.build_one と同じ前処理. 戻り値: 10Hz の時刻 t, 関節 q, ラベル g, 実測, 爪の命令.""" at, aq, gt, gs = bde.load_states(rec / "states.csv") o = np.argsort(at); at, aq = at[o], aq[o] at, aq = bde.drop_spikes(at, aq) fidx, fts = bde.load_frame_log(rec / "top_frame_log.csv") t0, t1 = max(fts[0], at[0]), min(fts[-1], at[-1]) t = np.arange(t0, t1, 1.0 / FPS) q = np.stack([np.interp(t, at, aq[:, k]) for k in range(6)], axis=1) return t, q, at, aq, gt, gs def label_of(t, gt, gs): g = np.zeros(len(t)) if len(gt): o = np.argsort(gt); gt, gs = gt[o], gs[o] for i, ti in enumerate(t): k = np.searchsorted(gt, ti, side="right") - 1 g[i] = gs[k] if k >= 0 else 0.0 return g def corr_grid(run_dir): """build_correction_episodes.build_one と同じ計算 (corr_v5 の引数). 戻り値は demo_grid と同じ + 選んだ添字と状態.""" rec = run_dir / "recording" at, aq, gt, gs = bde.load_states(rec / "states.csv") o = np.argsort(at); at, aq = at[o], aq[o] at, aq = bde.drop_spikes(at, aq) fidx, fts = bde.load_frame_log(rec / "top_frame_log.csv") t0, t1 = max(fts[0], at[0]), min(fts[-1], at[-1]) t = np.arange(t0, t1, 1.0 / FPS) q, _ = bce.fill_motion_gaps(t, at, aq, mode="speed20") g = label_of(t, gt, gs) log = bce.load_log(run_dir / "log.jsonl") mode_ts, modes = bce.load_stage_times(rec / "states.csv") sel = bce.select_steps(t, q, g, mode_ts, modes, excluded_windows(log), trim_mid_idle=True, trim_hold=True) # 詰める前 (model / correct だけを残し, 止まりを詰める前) の添字 from rewind_correct import step_keep_mask keep0, m_all = step_keep_mask(t, mode_ts, modes, excluded_windows(log)) return t, q, at, aq, gt, gs, sel, np.flatnonzero(keep0), m_all # ---------------------------------------------------------------- 1 本を調べる def interp_q(t, q, x): return np.array([np.interp(x, t, q[:, k]) for k in range(6)]) def analyze(name, ep_dir, kind, ds_name): z = np.load(ep_dir / "episode.npz") meta = json.loads((ep_dir / "meta.json").read_text()) st, ac = z["state"], z["actions"] tor = z["t_orig"] if "t_orig" in z.files else z["t"] if kind == "corr": run_dir = ROOT / "runs" / "pi05_policy" / name t, q, at, aq, gt, gs, sel, idx0, m_all = corr_grid(run_dir) absT = t[sel["idx"]] modes = meta.get("step_modes") or sel["modes"] pre_idx = idx0 # 詰める前に残っていたコマ (model / correct) else: run_dir = ROOT / "runs" / "run_pick_place" / name t, q, at, aq, gt, gs = demo_grid(run_dir / "recording") absT = t[0] + tor modes = ["teach"] * len(st) pre_idx = np.arange(len(t)) rec = run_dir / "recording" chk = float(np.max(np.abs(np.stack([interp_q(t, q, x) for x in absT]) - st[:, :6]))) sw = np.flatnonzero((st[1:, 6] == 1) & (st[:-1, 6] == 0)) + 1 row = {"dataset": ds_name, "run": name, "kind": kind, "steps": len(st), "state_check_max_deg": round(chk, 3), "n_close_labels": len(sw)} if not len(sw): row["note"] = "ラベルが 0→1 にならない" return row, [] i = int(sw[0]) tl = float(absT[i]) closes = sorted(float(x) for x, s in zip(gt, gs) if s == 1) tc = max([x for x in closes if x <= tl + 1e-6], default=np.nan) # アームが止まった時刻: 閉じる命令のときの姿勢 (直前の実測) から STOP_TOL_DEG の中に, 命令までずっと入っていた最初の実測 kb = np.flatnonzero(at <= tc) qc = aq[kb[-1]] s = kb[-1] while s - 1 >= 0 and np.max(np.abs(aq[s - 1] - qc)) <= STOP_TOL_DEG: s -= 1 t_stop = float(at[s]) gap_before_stop = float(at[s] - at[s - 1]) if s > 0 else np.nan # 止まった時刻の不確かさ (実測の間) after = (at > tc) & (at <= tc + WIN) move_after = float(np.max(np.abs(aq[after] - qc))) if after.any() else np.nan qa, q0, qb = interp_q(t, q, tl - WIN), interp_q(t, q, tl), interp_q(t, q, tl + WIN) qh = interp_q(t, q, tl - 0.5) # 閉じる前 0.3 秒の速さ (E と同じ見方. 実測の差の最大 / 0.3 秒) q03 = interp_q(t, q, tc - 0.3) qtc = interp_q(t, q, tc) row.update({ "label_step": i, "label_mode": modes[i], "label_mode_prev": modes[i - 1], "t_cmd": round(tc, 3), "t_label_minus_cmd_s": round(tl - tc, 3), "t_cmd_minus_stop_s": round(tc - t_stop, 2), "stop_gap_before_s": round(gap_before_stop, 2), "move_after_cmd_1p5s_deg": None if np.isnan(move_after) else round(move_after, 2), "speed_pre0p3_deg_s": round(float(np.max(np.abs(qtc - q03))) / 0.3, 2), **{f"d_j{k}_pre": round(float(q0[k] - qa[k]), 2) for k in (1, 2, 4)}, **{f"d_j{k}_post": round(float(qb[k] - q0[k]), 2) for k in (1, 2, 4)}, "d_reach_pre_mm": round(float(R(q0) - R(qa)), 1), "d_reach_last0p5_mm": round(float(R(q0) - R(qh)), 1), "d_reach_post_mm": round(float(R(qb) - R(q0)), 1), "d_h_pre_mm": round(float(H(q0) - H(qa)), 1), "d_h_last0p5_mm": round(float(H(q0) - H(qh)), 1), "d_h_post_mm": round(float(H(qb) - H(q0)), 1), "h_close_mm": round(float(H(q0)), 1), "reach_close_mm": round(float(R(q0)), 1), }) # 詰める処理: ラベルの前後 1.5 秒に元の 10Hz で何コマあったか / エピソードに何コマ残ったか near_pre = (t[pre_idx] >= tl - WIN) & (t[pre_idx] <= tl + WIN) near_ep = (absT >= tl - WIN - 1e-6) & (absT <= tl + WIN + 1e-6) row["grid_steps_pm1p5"] = int(near_pre.sum()) row["ep_steps_pm1p5"] = int(near_ep.sum()) lo = max(0, i - 15) row["ep_15steps_before_span_s"] = round(float(tl - absT[lo]), 1) # エピソードの 15 コマ前が何秒前か row["ep_15steps_d_reach_mm"] = round(float(R(st[i, :6]) - R(st[lo, :6])), 1) row["ep_15steps_d_h_mm"] = round(float(H(st[i, :6]) - H(st[lo, :6])), 1) # ラベルの位置がずれていないか: エピソードで最初に 1 になるコマの元の時刻が, 10Hz の並びで最初に 1 になる時刻と同じか g_full = label_of(t, gt, gs) first_full = t[np.flatnonzero((g_full[1:] == 1) & (g_full[:-1] == 0))[0] + 1] if (g_full == 1).any() else np.nan row["t_label_minus_grid_first1_s"] = round(tl - float(first_full), 2) # 行動: ラベルの前後のコマで「次の目標」がどちらへ向いているか steps = [] for k in range(max(0, i - 15), min(len(st), i + 16)): steps.append({"dataset": ds_name, "run": name, "rel": k - i, "t_rel_s": round(float(absT[k] - tl), 2), "mode": modes[k], "grip_state": int(st[k, 6]), "grip_action": int(ac[k, 6]), "act_max_joint_deg": round(float(np.max(np.abs(ac[k, :6] - st[k, :6]))), 2), "act_d_reach_mm": round(float(R(ac[k, :6]) - R(st[k, :6])), 2), "act_d_h_mm": round(float(H(ac[k, :6]) - H(st[k, :6])), 2), **{f"act_d_j{j}": round(float(ac[k, j] - st[k, j]), 2) for j in (1, 2, 4)}, "reach_mm": round(float(R(st[k, :6])), 1), "h_mm": round(float(H(st[k, :6])), 1)}) by = {s["rel"]: s for s in steps} for r in (-3, -2, -1, 0, 1): if r in by: row[f"act{r:+d}_d_reach_mm"] = by[r]["act_d_reach_mm"] row[f"act{r:+d}_max_joint_deg"] = by[r]["act_max_joint_deg"] pre = [s for s in steps if -10 <= s["rel"] < 0] row["act_pre10_sum_d_reach_mm"] = round(sum(s["act_d_reach_mm"] for s in pre), 1) row["act_pre10_sum_d_h_mm"] = round(sum(s["act_d_h_mm"] for s in pre), 1) row["frame_ts_before"] = None # 上カメラ: 爪の命令の直前のコマ (E と同じ) / エピソードのラベル直前・直後のコマに近いコマ fl = [(int(r["frame_idx"]), float(r["capture_ts"])) for r in csv.DictReader(open(rec / "top_frame_log.csv"))] fts = np.array([x for _, x in fl]) def frame_at_or_before(x): return fl[max(0, int(np.searchsorted(fts, x, side="right") - 1))] def frame_after(x): return fl[min(len(fl) - 1, int(np.searchsorted(fts, x)))] def nearest(x): k = int(np.argmin(np.abs(fts - x))) return fl[k] row.update({ "frame_pre1p5": frame_at_or_before(tc - WIN)[0], "frame_pre0p5": frame_at_or_before(tc - 0.5)[0], "frame_before": frame_at_or_before(tc)[0], "frame_after": frame_after(tc + 1.0)[0], "frame_ep_prev": nearest(float(absT[i - 1]))[0], "frame_ep_label": nearest(tl)[0], "t_before_s": round(frame_at_or_before(tc)[1] - tc, 2), "t_after_s": round(frame_after(tc + 1.0)[1] - tc, 2), }) del row["frame_ts_before"] # 補正入り: 閉じる前に人の補正があったか (ラベルより前の correct のコマ) と, その間の伸び if kind == "corr": ci = [k for k in range(i) if modes[k] == "correct"] row["correct_steps_before_label"] = len(ci) if ci: k0, k1 = ci[0], ci[-1] row["correct_last_to_label_s"] = round(float(tl - absT[k1]), 1) row["correct_d_reach_mm"] = round(float(R(st[min(k1 + 1, len(st) - 1), :6]) - R(st[k0, :6])), 1) row["correct_d_h_mm"] = round(float(H(st[min(k1 + 1, len(st) - 1), :6]) - H(st[k0, :6])), 1) dj = st[min(k1 + 1, len(st) - 1), :6] - st[k0, :6] row["correct_d_joints"] = " ".join(f"{v:+.1f}" for v in dj) return row, steps def episodes(ds): return sorted(p for p in (DS / ds / "raw_episodes").iterdir() if (p / "episode.npz").exists()) def main(): rows, steps = [], [] for p in episodes(MAIN): kind = "corr" if p.name.startswith("pi05_") else "teach" r, s = analyze(p.name, p, kind, "corr_v5_main"); rows.append(r); steps += s for ds, tag in (("mecharm_redblock_box_teach_v1", "teach_v1"), ("mecharm_redblock_box_scripted_v1", "scripted_v1")): for p in episodes(ds): r, s = analyze(p.name, p, "teach", tag); rows.append(r); steps += s keys = [] for r in rows: keys += [k for k in r if k not in keys] with open(OUT / "g_timing.csv", "w", newline="") as f: w = csv.DictWriter(f, fieldnames=keys); w.writeheader(); w.writerows(rows) with open(OUT / "g_steps.csv", "w", newline="") as f: w = csv.DictWriter(f, fieldnames=list(steps[0])); w.writeheader(); w.writerows(steps) print(f"rows={len(rows)} steps={len(steps)} state_check_max_deg={max(r['state_check_max_deg'] for r in rows)}") if __name__ == "__main__": main() `````` ### G のスクリプト g_images.py ``````python """依頼113 G-3: 学習データ 30 本 (と scripted_v1 20 本) の「ラベルが 1 になる直前・直後」を, 元の top.mp4 の 同じ時刻のコマで E と同じ測り方 (E_つかみと持ち上げ/e_grasp_metrics.py の measure・e_images.py の red_center) で測る. 測るコマ (g_timing.csv の frame_* 列. 時刻は top_frame_log.csv の capture_ts): pre1p5 / pre0p5 : 爪の命令の 1.5 秒前・0.5 秒前 (まだ奥へ進んでいたか) before : 爪の命令の直前 (E の before と同じ) ep_label : エピソードでラベルが 1 になったコマに使われた画に近いコマ after : 爪の命令の 1.0 秒後 (E の after と同じ. 爪が閉じきった形) 出力: g_grasp_metrics.csv / g_grasp_main.jpg (30 本. 1 行 1 本. 左から 1.5 秒前・直前・ラベルのコマ・1.0 秒後) g_grasp_scripted.jpg (scripted_v1) """ import csv import sys from pathlib import Path import cv2 import numpy as np ROOT = Path("/Volumes/KIOXIA/r2r2r-data") TW = ROOT / "lab" / "twin" / "20261107_失敗の再生と置き場所" OUT = TW / "G_お手本の閉じる時刻" sys.path.insert(0, str(TW / "E_つかみと持ち上げ")) from e_grasp_metrics import measure # noqa: E402 from e_images import FALLBACK, CW, CH, SC, crop, label, red_center # noqa: E402 RED_LO1, RED_HI1, RED_LO2, RED_HI2 = (0, 120, 70), (10, 255, 255), (170, 120, 70), (180, 255, 255) # E と同じ BLUE_LO, BLUE_HI = (100, 150, 80), (130, 255, 255) # E と同じ HALF = 350 # 爪の向きを測るときの切り出し (胴の青い印まで入るように E の 260 より広い) def _red_mask(hsv, s_min): return cv2.inRange(hsv, (0, s_min, 70), (10, 255, 255)) | cv2.inRange(hsv, (170, s_min, 70), (180, 255, 255)) def red_center_wide(img, s_min=120): """E の red_center と同じ赤の決まり (面積 300 以上・細長くない) で, 探す範囲を画の全体に広げたもの. 10/15〜10/19 の記録はブロックが画の左寄り (x 450〜800) にあり, E の範囲 (x 700〜1400) から外れるため. 画の全体では赤い配線も拾うので, 爪先の青い印 2 つが 200px 以内にある赤の塊だけを候補にする.""" hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) m = _red_mask(hsv, s_min) m[:, :40] = 0; m[:, -40:] = 0; m[:40] = 0; m[-40:] = 0 n, lab, st, cen = cv2.connectedComponentsWithStats(m) nb, _, stb, cenb = cv2.connectedComponentsWithStats(cv2.inRange(hsv, BLUE_LO, BLUE_HI)) blues = [cenb[k] for k in range(1, nb) if stb[k][4] >= 60] best = None for k in range(1, n): x, y, w, h, a = st[k] if a < 300 or max(w, h) > 3 * min(w, h): continue d = sorted(np.hypot(*(b - cen[k])) for b in blues) if len(d) < 2 or d[1] > 200: continue if best is None or a > best[0]: best = (a, cen[k]) return None if best is None else (int(best[1][0]), int(best[1][1])) def _axis_once(roi, s_min): hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV) n, lab, st, cen = cv2.connectedComponentsWithStats(_red_mask(hsv, s_min)) rb = [k for k in range(1, n) if st[k][4] >= 300 and max(st[k][2], st[k][3]) < 3 * min(st[k][2], st[k][3])] nb, _, stb, cenb = cv2.connectedComponentsWithStats(cv2.inRange(hsv, BLUE_LO, BLUE_HI)) bb = [k for k in range(1, nb) if stb[k][4] >= 60] if not rb or len(bb) < 2: return None r = max(rb, key=lambda k: st[k][4]) bb = sorted(bb, key=lambda k: np.hypot(*(cenb[k] - cen[r]))) c1, c2 = cenb[bb[0]], cenb[bb[1]] mid = (c1 + c2) / 2 tip_dir = (c2 - c1) / max(1e-6, np.linalg.norm(c2 - c1)) body = None for k in bb[2:]: v = cenb[k] - mid; d = np.linalg.norm(v) if 120 <= d <= 400 and abs(np.dot(v / d, tip_dir)) < 0.5: body = cenb[k]; break if body is not None: u = (mid - body) / np.linalg.norm(mid - body); src = "body" else: # 胴の印が写らないとき: 爪先の印 2 つを結ぶ線に直角で, 画の下向き (どの記録も爪先は画の下寄りを向く) u = np.array([-tip_dir[1], tip_dir[0]]) u = u if u[1] > 0 else -u src = "tips" ys, xs = np.nonzero(lab == r) proj = (np.stack([xs, ys], 1) - mid) @ u lat = float((cen[r] - mid) @ np.array([-u[1], u[0]])) return {"axis_protrude_px": round(float(proj.max()), 1), "axis_lateral_px": round(lat, 1), "axis_marker_gap_px": round(float(np.linalg.norm(c1 - c2)), 1), "axis_deg": round(float(np.degrees(np.arctan2(u[0], u[1]))), 1), "axis_from": src, "axis_red_area": int(st[r][4]), "axis_s_min": s_min} def measure_axis(f, c, s_min=120): """E の measure を爪の向きに沿って測るようにしたもの. 爪の向き u = 胴の青い印 → 爪先の青い印 2 つの中点 (画の上で. 爪先へ向かう向き). 胴の印が写らないときは, 爪先の印 2 つを結ぶ線に直角で画の下向き (axis_from=tips). axis_protrude_px : ブロックの赤の画素を u に投げた最大 − 爪先の印の中点 (E の protrude_px と同じ意味. 爪が画の真下を向いていれば E の値とほぼ同じになる) axis_lateral_px : ブロックの重心の, u に直角な向きのずれ axis_marker_gap_px: 爪先の印の間 (E と同じ) 赤の決まりは E と同じ (彩度 120 以上). ただしブロックが薄いピンクに写る回 (10/19 の一部) は彩度 120 では ブロックの一部しか拾えないので, 彩度 60 で拾った面積の半分未満なら彩度 60 の値を使う (axis_s_min=60).""" H_, W_ = f.shape[:2] x0, y0 = max(0, c[0] - HALF), max(0, c[1] - HALF) roi = f[y0:min(H_, c[1] + HALF), x0:min(W_, c[0] + HALF)] m = _axis_once(roi, s_min) if s_min == 120: m60 = _axis_once(roi, 60) if m60 and (m is None or m["axis_red_area"] < 0.5 * m60["axis_red_area"]): m = m60 return m TAGS = ("pre1p5", "pre0p5", "before", "ep_label", "after") SHOW = (("pre1p5", "-1.5s"), ("before", "before"), ("ep_label", "label=1 frame"), ("after", "after +1.0s")) def rec_of(run): sub = "pi05_policy" if run.startswith("pi05_") else "run_pick_place" return ROOT / "runs" / sub / run / "recording" def main(): rows = list(csv.DictReader(open(OUT / "g_timing.csv"))) out, tiles = [], {"main": [], "scripted": []} W, Hh = int(CW * SC), int(CH * SC) for r in rows: if r["dataset"] == "teach_v1": # trim2 と同じ記録・同じコマ (g_timing で確かめた) なので測らない continue cap = cv2.VideoCapture(str(rec_of(r["run"]) / "top.mp4")) fr = {} for tag in TAGS: cap.set(cv2.CAP_PROP_POS_FRAMES, int(r[f"frame_{tag}"])); ok, f = cap.read() fr[tag] = f if ok else None ce = red_center(fr["before"]) or red_center(fr["after"]) # E と同じ探し方 cw = None if ce else (red_center_wide(fr["before"]) or red_center_wide(fr["after"])) s_min = 120 if not ce and not cw: # ブロックが薄いピンクに写る回 (10/19 の一部). 彩度の下限だけ 60 に下げる s_min = 60 cw = red_center_wide(fr["before"], 60) or red_center_wide(fr["after"], 60) c = ce or cw or FALLBACK o = {"dataset": r["dataset"], "run": r["run"], "kind": r["kind"], "crop_center_px": f"{c[0]},{c[1]}", "center_by": "E" if ce else ("wide" if cw and s_min == 120 else ("wide_s60" if cw else "none"))} for tag in TAGS: m = measure(fr[tag], c) if fr[tag] is not None else None for k, v in (m or {}).items(): o[f"{tag}_{k}"] = v m = measure_axis(fr[tag], c, s_min) if fr[tag] is not None else None for k, v in (m or {}).items(): o[f"{tag}_{k}"] = v out.append(o) name = r["run"].replace("run_0001_", "").replace("pi05_", "p") col = (255, 220, 120) if r["kind"] == "corr" else (120, 255, 120) row_t = [] for j, (tag, txt) in enumerate(SHOW): img = crop(fr[tag], c) if fr[tag] is not None else np.full((Hh, W, 3), 40, np.uint8) p = o.get(f"{tag}_axis_protrude_px"); g = o.get(f"{tag}_axis_marker_gap_px") extra = "" if p is None else f" pr {p:+.0f} gap {g:.0f}" row_t.append(label(img, (f"{name} " if j == 0 else "") + txt + extra, col if j == 0 else (255, 255, 255))) tiles["main" if r["dataset"] == "corr_v5_main" else "scripted"].append(np.hstack(row_t)) cap.release() # E の失敗・成功 16 件も同じ「爪の向きに沿う」測り方で測る (E の e_frames.csv の before / after のコマ) E = TW / "E_つかみと持ち上げ" grp = {r["run"]: r for r in csv.DictReader(open(E / "e_grasp_metrics.csv"))} for r in csv.DictReader(open(E / "e_frames.csv")): if not r.get("frame_after"): continue cap = cv2.VideoCapture(str(ROOT / "runs" / "pi05_policy" / r["run"] / "recording" / "top.mp4")) c = tuple(int(v) for v in r["crop_center_px"].split(",")) o = {"dataset": "E_" + r["group"], "run": r["run"], "kind": "E", "crop_center_px": r["crop_center_px"], "center_by": "E"} for tag in ("before", "after"): cap.set(cv2.CAP_PROP_POS_FRAMES, int(r[f"frame_{tag}"])); ok, f = cap.read() o[f"{tag}_protrude_px"] = grp[r["run"]].get(f"{tag}_protrude_px") o[f"{tag}_marker_gap_px"] = grp[r["run"]].get(f"{tag}_marker_gap_px") m = measure_axis(f, c) if ok else None for k, v in (m or {}).items(): o[f"{tag}_{k}"] = v cap.release() out.append(o) for k, v in tiles.items(): cv2.imwrite(str(OUT / f"g_grasp_{k}.jpg"), np.vstack(v), [cv2.IMWRITE_JPEG_QUALITY, 85]) keys = [] for o in out: keys += [k for k in o if k not in keys] with open(OUT / "g_grasp_metrics.csv", "w", newline="") as f: w = csv.DictWriter(f, fieldnames=keys); w.writeheader(); w.writerows(out) print(len(out), "rows") if __name__ == "__main__": main() `````` ### G のスクリプト g_report.py ``````python """依頼113 G: g_timing.csv と g_grasp_metrics.csv を 1 本 1 行にまとめ (g_table.csv), README 用の表を出す (標準出力). 深さの物差しは「爪の向きに沿うはみ出し」(g_images.measure_axis. axis_protrude_px). E の値 (縦の差) とは 0〜7px 違うので, E の 16 件も同じ物差しで測り直した値 (dataset=E_failures / E_successes) と並べる. scripted_v1 はカメラが少し引いていて爪先の印の間 (開き) が 122〜127px (ほかは 134〜140px) なので, px を「開いた印の間 137px」にそろえた値 (scaled) も出す. 「手」: 閉じる前のコマに人の手が写る本 (画で確かめた. HAND). 肌を赤として拾うと値が 100px を超えるので, 閉じる前の値で 100px を超えるものは使わない (表では「—」). """ import csv from pathlib import Path import numpy as np OUT = Path(__file__).resolve().parent HAND = {"run_0001_20261019_191347", "run_0001_20261019_191441", # scripted_v1: 182552・185536・175444 は直前まで, 183310・184725・190925 は 1.5 秒前のコマだけ手が写る "run_0001_20261015_182552", "run_0001_20261015_183310", "run_0001_20261015_185536", "run_0001_20261016_175444", "run_0001_20261016_184725", "run_0001_20261016_190925"} SUCCESS_MAX_AFTER = None # E の成功の閉じたあとの最大 (下で計算) def f(v): try: return float(v) except (TypeError, ValueError): return None def main(): tim = {(r["dataset"], r["run"]): r for r in csv.DictReader(open(OUT / "g_timing.csv"))} gm = list(csv.DictReader(open(OUT / "g_grasp_metrics.csv"))) e_succ = [f(r["after_axis_protrude_px"]) for r in gm if r["dataset"] == "E_successes"] e_shallow = [f(r["after_axis_protrude_px"]) for r in gm if r["dataset"] == "E_failures" and f(r["after_axis_marker_gap_px"]) > 90 and f(r["after_axis_protrude_px"]) > max(e_succ) + 10] line = max(e_succ) + 10 rows = [] for g in gm: t = tim.get((g["dataset"], g["run"]), {}) gap_open = f(g.get("before_axis_marker_gap_px")) or 137.0 sc = 137.0 / gap_open if g["dataset"] == "scripted_v1" else 1.0 o = {"dataset": g["dataset"], "run": g["run"], "kind": g["kind"], "hand_in_frame": g["run"] in HAND, "pre1p5_px": g.get("pre1p5_axis_protrude_px"), "pre0p5_px": g.get("pre0p5_axis_protrude_px"), "before_px": g.get("before_axis_protrude_px"), "label_frame_px": g.get("ep_label_axis_protrude_px"), "after_px": g.get("after_axis_protrude_px"), "after_gap_px": g.get("after_axis_marker_gap_px"), "scale_to_137": round(sc, 3), "E_before_px": g.get("before_protrude_px"), "E_after_px": g.get("after_protrude_px")} a = f(o["after_px"]) o["after_px_scaled"] = None if a is None else round(a * sc, 1) o["after_mm_approx"] = None if a is None else round(a * sc / 2.6, 1) o["shallow_like_failures"] = (a is not None and a * sc > line and (f(o["after_gap_px"]) or 0) * sc > 90) for k in ("label_mode", "t_label_minus_cmd_s", "t_cmd_minus_stop_s", "stop_gap_before_s", "speed_pre0p3_deg_s", "move_after_cmd_1p5s_deg", "d_j1_pre", "d_j2_pre", "d_j4_pre", "d_reach_pre_mm", "d_reach_last0p5_mm", "d_h_pre_mm", "d_h_last0p5_mm", "grid_steps_pm1p5", "ep_steps_pm1p5", "t_label_minus_grid_first1_s", "act-1_d_reach_mm", "act-1_max_joint_deg", "correct_steps_before_label", "correct_d_reach_mm", "correct_d_h_mm", "correct_d_joints"): o[k] = t.get(k) rows.append(o) with open(OUT / "g_table.csv", "w", newline="") as fo: w = csv.DictWriter(fo, fieldnames=list(rows[0])); w.writeheader(); w.writerows(rows) print(f"E 成功の閉じたあと最大 {max(e_succ)} px → 浅いとみなす線 {line:.1f} px / E 浅い失敗 {sorted(e_shallow)}") def ok(v): v = f(v) return "—" if v is None or v > 100 else v def short(r): return r.replace("run_0001_", "").replace("pi05_", "") print("\n| 本 | 種類 | 閉じたモード | 命令−止まり 秒 | 閉じる前 0.3 秒 °/s | 前 1.5 秒 j1/j2/j4 ° | 前 1.5 秒 伸び/高さ mm | " "最後 0.5 秒 伸び/高さ mm | 1.5 秒前 px | 直前 px | ラベルのコマ px | 1.0 秒後 px | 浅い |") print("|" + "---|" * 13) for o in rows: if o["dataset"] != "corr_v5_main": continue hand = "（手）" if o["hand_in_frame"] else "" print(f"| {short(o['run'])} | {'補正入り' if o['kind'] == 'corr' else '教えた'} | {o['label_mode']} | " f"{o['t_cmd_minus_stop_s']} | {o['speed_pre0p3_deg_s']} | {o['d_j1_pre']}/{o['d_j2_pre']}/{o['d_j4_pre']} | " f"{o['d_reach_pre_mm']}/{o['d_h_pre_mm']} | {o['d_reach_last0p5_mm']}/{o['d_h_last0p5_mm']} | " f"{ok(o['pre1p5_px'])} | {ok(o['before_px'])}{hand} | " f"{ok(o['label_frame_px'])} | {o['after_px']} | {'**浅い**' if o['shallow_like_failures'] else ''} |") print("\n群ごと (閉じたあと px. 手の本も閉じたあとは数える)") groups = {"教えた 19": lambda o: o["dataset"] == "corr_v5_main" and o["kind"] == "teach", "教えた うち試す置き場所 (194818〜195120) 4": lambda o: o["dataset"] == "corr_v5_main" and o["run"][-6:] in ("194818", "194913", "195023", "195120"), "補正入り 11": lambda o: o["dataset"] == "corr_v5_main" and o["kind"] == "corr", "scripted_v1 20 (137px にそろえた)": lambda o: o["dataset"] == "scripted_v1", "E 成功 4": lambda o: o["dataset"] == "E_successes", "E 失敗 12": lambda o: o["dataset"] == "E_failures"} for name, fn in groups.items(): sel = [o for o in rows if fn(o)] af = [o["after_px_scaled"] for o in sel if o["after_px_scaled"] is not None] bf = [f(o["before_px"]) * o["scale_to_137"] for o in sel if f(o["before_px"]) is not None and f(o["before_px"]) <= 100] n_sh = sum(o["shallow_like_failures"] for o in sel) print(f"- {name}: 直前 {min(bf):.0f}〜{max(bf):.0f} (中央値 {np.median(bf):.0f}) / 1.0 秒後 {min(af):.0f}〜{max(af):.0f} " f"(中央値 {np.median(af):.0f}) / 浅い (> {line:.0f}px) {n_sh} 本: " + ", ".join(short(o["run"]) for o in sel if o["shallow_like_failures"])) if __name__ == "__main__": main() `````` ## 浅い7本と指の写った2本を外し、深さで採否を決める仕組みを足した（依頼117） 外す線は、π0.5 の成功4件のはみ出しの最大（14px）に10pxを足した24px（およそ9mm）にしました。この線で、実機の失敗のうち浅い7件をそのまま言い当てられます。外した9本は次のとおりです（20:40〜20:58）。 | run | 種類 | 閉じて1秒後のはみ出し px | 理由 | |---|---|---|---| | pi05_20261031_181946 | 補正入り | 37.8 | 浅い | | pi05_20261101_192101 | 補正入り | 33.4 | 浅い | | pi05_20261106_184452 | 補正入り | 49.2 | 浅い | | pi05_20261106_184906 | 補正入り | 47.1 | 浅い | | run_0001_20261019_183454 | 教えた動き | 38.9 | 浅い | | run_0001_20261019_192343 | 教えた動き | 27.6 | 浅い | | run_0001_20261019_192633 | 教えた動き | 28.9 | 浅い | | run_0001_20261019_191347 | 教えた動き | 19.7 | 人の指が写っている | | run_0001_20261019_191441 | 教えた動き | 14.5 | 人の指が写っている | - ねらい：浅くつかむお手本を学習に入れない。これから足すお手本も、撮るたびに同じ線で選べるようにする - 変わったこと：学習データは30本から21本（3,018コマ）になった。お手本を作るスクリプトに、はみ出しが線を超えた本を外す引数を足した。引数を付けなければ、前と1バイトも違わないデータができる - 見込める効果：π0.5 が浅く閉じる癖を覚え直す材料が減る。確かめるのは、深いお手本を足して学習し直したあとの、補正なしの成功率 ### 依頼117：浅いお手本と指の写ったお手本を外した学習データを作り、つかみの深さで採否を決められるようにする ``````markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 150 --- # 浅いお手本と指の写ったお手本を外した学習データを作り、これから足すお手本をつかみの深さで採否を決められるようにする（依頼113 の続き・2026-11-07） **実機には触れない。** Raspberry Pi・daemon・SSH・シリアルは使わない。学習（Runpod）もまだ回さない（深いお手本を実機で足してから回す）。 ## これまで分かったこと（`/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/G_お手本の閉じる時刻/README.md`） - π0.5 の失敗の多くは、ブロックが爪の奥まで入る前に閉じた「浅い」つかみ - v3 の学習データ `mecharm_redblock_box_teach_v1_trim2_corr_v5`（30本）のうち、閉じて 1.0 秒後にブロックが爪先の印の線から 24px を超えてはみ出しているものが7本。試している置き場所の4本は、π0.5 が自分で浅く閉じた補正入りのお手本だった - 教えた動きの2本は、閉じる瞬間に人の指がブロックを押し込んでいて、学習の画に写っている - 測り方は G の `g_images.py` の `measure_axis`（爪の向きに沿ったはみ出し。元の `top.mp4` の 1920×1080 で測る）と `g_grasp_metrics.csv` エイイチの決め（2026-11-07）：浅い7本と指の写った2本を外す。浅いとみなす線は 24px（約 9mm）。これから足すお手本は、つかみの深さで採否を決める。 ## A. 外した学習データを作る 外すのは次の9本（G の `g_table.csv` の `corr_v5_main` で、`after_px` > 24 か `hand_in_frame` = True）。 | run | 種類 | 閉じて 1.0 秒後のはみ出し px | 理由 | |---|---|---|---| | pi05_20261031_181946 | 補正入り | 37.8 | 浅い | | pi05_20261101_192101 | 補正入り | 33.4 | 浅い | | pi05_20261106_184452 | 補正入り | 49.2 | 浅い | | pi05_20261106_184906 | 補正入り | 47.1 | 浅い | | run_0001_20261019_183454 | 教えた動き | 38.9 | 浅い | | run_0001_20261019_192343 | 教えた動き | 27.6 | 浅い | | run_0001_20261019_192633 | 教えた動き | 28.9 | 浅い | | run_0001_20261019_191347 | 教えた動き | 19.7 | 人の指が写っている | | run_0001_20261019_191441 | 教えた動き | 14.5 | 人の指が写っている | - 元のデータセット（trim2・corr_v5・teach_v1_trim2_corr_v5）は変えない。`pi05/merge_raw_episodes.py` など今ある作り方で、外した版を新しい名前で作る：`mecharm_redblock_box_teach_v1_trim2_corr_v5_deep`（教えた動き 14本・補正入り 7本の 21本になるはず） - README に、外した9本と理由・測った値・G の README への参照を書く。index.json にも外した理由を残す - check_dataset を通し、本数・コマ数・補正のコマ数を報告する。Pod に送る tar も作る（exFAT の `._` は入れない） ## B. これから足すお手本を、つかみの深さで採否を決められるようにする - G の `measure_axis` を、`pi05/` の中の小さな部品（例 `pi05/grasp_depth.py`）にする。入力は run のフォルダ（`recording/top.mp4`・`top_frame_log.csv`・`states.csv` の爪の命令の時刻）、出力は閉じる直前と閉じて 1.0 秒後のはみ出し px と、人の手が写っていそうかの印 - `pi05/build_demo_episodes.py` と `pi05/build_correction_episodes.py` に、はみ出しが線（既定 24px）を超える本を外す引数を足す（例 `--max-protrude-px 24`）。**引数を付けないときの出力は今までと同じにする**（今までのデータセットが同じ入力から同じように作れること。テストで確かめる）。外した本は index.json に値と理由を残す - 測れなかった本（印が見えない・ブロックが見つからない）は外さずに「測れなかった」と残し、報告に数を書く - G で測った30本の値と、この部品で測った値が同じになるか（差の最大）を確かめる - テストを足す ## C. 深いお手本を足す実機の手順を書く（コードは変えない） エイイチが実機で、試している置き場所（教えた位置5のあたり。教えた記録は LOG352 の位置5、`run_0001_20261019_194818`〜`195120` の4本はどれも深い）のお手本を5〜10本足すための手順を、`pi05/` の手順書（RUNPOD_TRAIN.md の近く、または README）に1節で書く。 - 使うスクリプトとコマンド（今の `record_teach_demo.py` で、位置5の教えた記録を再生する形でよいか。同じ動きの繰り返しになる点も書く） - 閉じる前にブロックが奥まで入っているかを、人がどこで確かめるか（今の作りで止まって確かめられる所があるか。なければ、ない と書く） - 撮ったあとに B の部品で深さを測り、線を超えた本を外す流れ - 依頼116（爪と肩の電流を記録に足す）が入っていれば、爪の電流でつかめたかを見る手順も足す ## 守ること - git は変えたファイルを1つずつ指定して add・commit する。`git add .`・`git clean` は使わない - KIOXIA の `datasets/` は新しいフォルダを作るだけ。今あるフォルダは上書き・移動・削除しない - 依頼116 が同じ時期に `policy_recorder` まわりを変えている。そちらのファイルには触れない ## 報告 A の本数・コマ数、B の差の最大と「引数なしで今までと同じ」の確かめ、C の手順の置き場所だけを書く。最後の1行に「判定：合格／止めた（決めてほしいことあり）／失敗」を書く。 `````` ### 依頼117の報告 ``````markdown # 結果：20261107_117_浅いお手本と指の写ったお手本を外した学習データを作りつかみの深さで採否を決める - 状態：完了 - 始めた時刻：2026-11-07 20:40 - 終わった時刻：2026-11-07 20:58（18分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 70e0bb25-6eff-4035-84b8-93b12ecfdd0b` - 止められた操作：1件（いちばん下に一覧） ## Claude Code の報告 **やったこと** - A: 浅い7本と指の写った2本を外した学習データ `mecharm_redblock_box_teach_v1_trim2_corr_v5_deep` を新しく作りました。`merge_raw_episodes.py` に、外す本と理由を渡す `--exclude-json` を足して作っています。元のデータセット3つは読んだだけで、変えていません。外した9本の理由・測った値・G の README への参照は、README と index.json の `excluded` に残しました。Pod に送る tar も作りました。 - B: G の `measure_axis` を `pi05/grasp_depth.py` にしました。`build_demo_episodes.py` と `build_correction_episodes.py` に `--max-protrude-px` を足しました。測れなかった本は外さず「測れなかった」と残します。テストは `pi05/test_grasp_depth.py` に12件足しました。 - C: 位置5の深いお手本を足す手順を `pi05/DEEP_DEMOS.md` に1節で書きました。 **結果** - A の本数とコマ数: 21本・3018コマです（教えた動き 14本 1989コマ、補正入り 7本 1029コマ）。補正のコマは282です。 - check_dataset は FAIL なしで、WARN が2つ出ました。 - 1つ目の「2. 停止」は、元のデータと同じ WARN です（190854 の 22.8%）。 - 2つ目の「5. ばらつき」は、外したことで新しく出ました。j0 +15° 付近が 11/21本（52.4%）で、半分を超えています（元は 15/30本 50.0% で PASS）。 - tar は 82MB で、`._` は入っていません（0個）。 - B の差の最大: G で測った30本と比べて、閉じる直前・閉じて1.0秒後とも 0.0px でした。閉じる命令の時刻と、測ったコマも同じです。浅い7本と手の写った2本の印も G と一致しました。 - 引数なしで今までと同じか: trim2 の19本と corr_v5 の11本を、作ったときと同じ引数で /tmp に作り直しました。npz・meta.json・画のバイト列まで30本とも今のデータセットと同じでした。 - `--max-protrude-px 24` を付けると7本が外れ、残りの23本も同じでした。 - pi05 のテスト11本は、新しいものも含めてすべて通りました。 - C の手順の置き場所: `pi05/DEEP_DEMOS.md` です。 - 閉じる前に止まって確かめる所は、今の作りにはありません。 - 依頼116の電流は実機推論の記録にだけ残ります。教えた動きの再生の記録には残らないので、この手順では爪の電流で確かめられません。 - この手順はまだ実機で通していません。 **変えたファイル** - 変更: `pi05/build_demo_episodes.py`・`pi05/build_correction_episodes.py`・`pi05/merge_raw_episodes.py` - 新規: `pi05/grasp_depth.py`・`pi05/test_grasp_depth.py`・`pi05/DEEP_DEMOS.md`・`pi05/excludes/teach_v1_trim2_corr_v5_deep.json`・`pi05/excludes/teach_v1_trim2_corr_v5_deep_note.md`・`logs/20261107_2058.md` - KIOXIA: `datasets/mecharm_redblock_box_teach_v1_trim2_corr_v5_deep/` を新しく作りました。 - commit は f65fc21 です。依頼116の `policy_recorder` まわりには触れていません。 **決めてほしいこと** 1. 手が写っていそうな本を、自動で外すかどうか。今は印を出して数と名前を表示するだけで、外すのは線を超えた本だけです。 2. 位置5の同じ動きを10本足すと、j0 +15° 付近が 31本中21本（68%）になり、check_dataset の WARN が強まります。このまま足すか、他の置き場所も混ぜるか。 判定：止めた（決めてほしいことあり） ## 止められた操作 - {"tool_name": "Bash", "tool_use_id": "toolu_01APQLDRvN9befAb7Z4ZBgcY", "tool_input": {"command": "cat > /tmp/gd117/same.py <<'EOF'\n\"\"\"引数なし / --max-protrude-px 24 で作り直したエピソードが, 今あるデータセットと同じかを比べる (書く先は /tmp だけ).\"\"\"\nimport json, sys, hashlib, argparse\nfrom pathlib import Path\nimport numpy as np\nsys.path.insert(0, \"/Users/shimazueiichi/r2r2r-scripts/pi05\")\nimport build_demo_episodes as bde, build_correction_episodes as bce\nDS = Path(\"/Volumes/KIOXIA/r2r2r-data/datasets\"); RUNS = Pat `````` ### r2r2r-scripts commit f65fc21（grasp_depth.py・--max-protrude-px・--exclude-json・DEEP_DEMOS.md） ``````diff commit f65fc211ba4d3f287c1a639582c06ee06864adb9 Author: Eiichi Shimazu Date: Sat Nov 7 20:58:32 2026 +0900 浅いお手本 7 本と指の写った 2 本を外した学習データ mecharm_redblock_box_teach_v1_trim2_corr_v5_deep (21 本 / 3018 コマ) を merge_raw_episodes.py の --exclude-json (理由つき. index.json の excluded と README に残す) で作る. G の measure_axis を pi05/grasp_depth.py にし (G の 30 本と差 0.0px), build_demo_episodes / build_correction_episodes に --max-protrude-px を足す (引数なしでは測らず, trim2 19 本・corr_v5 11 本を作り直して npz・meta・画まで同じ. 測れなかった本は外さない). 位置 5 の深いお手本を足す手順を pi05/DEEP_DEMOS.md に書く (依頼 117) Co-Authored-By: Claude Opus 5.5 diff --git a/logs/20261107_2058.md b/logs/20261107_2058.md new file mode 100644 index 0000000..0cd2346 --- /dev/null +++ b/logs/20261107_2058.md @@ -0,0 +1,25 @@ +# 2026-11-07 依頼 117：浅いお手本と指の写ったお手本を外した学習データを作り、つかみの深さで採否を決める + +実機・Pi・daemon・SSH・シリアル・Runpod には触れていない。 + +## A. 外した学習データ +- `datasets/mecharm_redblock_box_teach_v1_trim2_corr_v5_deep`（新しいフォルダ。元の trim2・corr_v5・teach_v1_trim2_corr_v5 は読んだだけ） +- 21 本 / 3018 コマ（教えた動き 14 本 1989 コマ・補正入り 7 本 1029 コマ、うち補正 282 コマ）。元は 30 本 / 4301 コマ / 補正 430 +- 外した 9 本: 浅い 7 本（181946・192101・184452・184906・183454・192343・192633）、指 2 本（191347・191441）。index.json の excluded・README の表と G への参照 +- 作り方: `merge_raw_episodes.py` に `--exclude-json`（理由つきの外す一覧）・`--readme-note` を足した。一覧は `pi05/excludes/teach_v1_trim2_corr_v5_deep.json` +- check_dataset: FAIL なし・WARN 2（2. 停止 = 元と同じ 190854 の 22.8%、5. ばらつき = j0 +15° 付近が 11/21 = 52.4% で半分を超えた。元は 15/30 = 50.0% で PASS） +- tar 82MB（`._` 0 個・episode.npz 21 個） + +## B. grasp_depth.py +- `pi05/grasp_depth.py`: G の measure_axis・E の red_center・G の red_center_wide をそのまま写し、run のフォルダから閉じる直前・1.0 秒後のはみ出し・手の印（閉じる前のコマで 100px 超）を返す。測れないときは unmeasured と理由 +- G の 30 本と比べて: after・before の差の最大 0.0px、閉じる命令の時刻の差 0、測ったコマ番号も同じ。浅い 7 本と手の 2 本の印も G と同じ +- `build_demo_episodes.py` / `build_correction_episodes.py` に `--max-protrude-px`。引数なしでは測らない +- 引数なしの確かめ: trim2 の 19 本（--trim-mid-idle）と corr_v5 の 11 本（--gap-fill speed20 --trim-hold）を /tmp に作り直し、今のデータセットと npz・meta.json・画のバイト列まで 30 本とも同じ。--max-protrude-px 24 では 7 本が外れ、残り 23 本も同じ +- テスト: `pi05/test_grasp_depth.py` 12 件 OK。pi05/test_*.py 11 本すべて OK + +## C. 手順 +- `pi05/DEEP_DEMOS.md`（位置 5 の `drag_v2_20221013_233323.json` を record_teach_demo.py で再生・閉じる前に止まって確かめる所はない・撮ったあと grasp_depth と --max-protrude-px・電流は教えた動きの記録には残らない） + +## 決めてほしいこと +- 手が写っていそうな本は印を出すだけで外していない（線を超えた本だけ外す）。手の本も自動で外すか +- 位置 5 の同じ動きを 5〜10 本足すと、j0 +15° 付近が 21/31 = 68% になる（check_dataset 5. の WARN が強まる） diff --git a/pi05/DEEP_DEMOS.md b/pi05/DEEP_DEMOS.md new file mode 100644 index 0000000..416fda7 --- /dev/null +++ b/pi05/DEEP_DEMOS.md @@ -0,0 +1,38 @@ +# 深いお手本を足す（試している置き場所・教えた位置 5。2026-11-07 依頼 117） + +学習（`RUNPOD_TRAIN.md`）の前に、π0.5 を試している置き場所（ブロックが画の中央・PICK の j0 +13°）で、ブロックが爪の奥まで入ってから閉じるお手本を 5〜10 本足す手順。今のデータは `mecharm_redblock_box_teach_v1_trim2_corr_v5_deep`（21 本。浅い 7 本・指の写った 2 本を外した版）。**この手順はまだ実機で通していない**（書いただけ）。 + +**1. 使うスクリプト**: 今の `dynamic_tracking_limits/drag_teaching/record_teach_demo.py` で、LOG352 の位置 5 の教えた記録 `drag_v2_20221013_233323.json` を再生する（新しく教え直さないので、Pi の `drag_teach_v2.py teach` は飛ばす）。10/19 の `run_0001_20261019_194818`〜`195120` の 4 本はこの形で撮ったもので、閉じて 1.0 秒後のはみ出しは 8〜15px（深い）。 + +``` +# ブロックを位置 5 に置く（10/19 の measure は block_xz (+45.7, −28.5)。teach_poses_LOG352/measure_20261019_194751.json） +python3 dynamic_tracking_limits/place/place_modules/recorder.py --top --no-side # 別のターミナルで常駐 +python3 dynamic_tracking_limits/drag_teaching/record_teach_demo.py measure # 1 本ごとに置き直したら測り直す (保存先 teach_poses_{日付}/) +python3 dynamic_tracking_limits/drag_teaching/record_teach_demo.py run --teach drag_v2_20221013_233323.json # ドライラン（計画だけ） +python3 dynamic_tracking_limits/drag_teaching/record_teach_demo.py run --teach drag_v2_20221013_233323.json --execute # 再生＋記録 +``` + +- **同じ動きの繰り返しになる**: 再生するのは教えたときの関節の角度そのもので、ブロックの位置を見て動きを変えない。5〜10 本とも同じ PICK の姿勢（`pose_used.pick_used` = [13.18, 60.02, 19.51, −2.46, −61.87, −26.71]）で閉じる。画の違いは、人が置いたブロックの数 mm のずれと光だけになる。ブロックを毎回同じ所に置くほど深くつかめるが、学習には同じ画と同じ動きが増える（check_dataset の 5. で j0 +15° 付近が今でも 21 本中 11 本・52%。10 本足すと 31 本中 21 本・68%） +- `run` が確かめるのは「measure のあとでブロックが動いていないか」（5mm）だけで、教えたときの位置と同じかは見ない。位置 5 に置けたかは、measure の block_xz を 10/19 の値と人が見比べる。10/19 の 4 本はどれも再生直前の検出で WARN（ブロックを見つけられない・赤いケーブルを拾う）が出ていたが、完走している + +**2. 閉じる前に奥まで入っているかを確かめる所**: **今の作りにはない。** `run --execute` は、始める前の「周囲に手・物が無いか確認して Enter」のあとは止まらない。PICK に着くと、止まったのを最大 8 秒待ってから（止まらなくても WARN だけで）そのまま閉じる。人が見られるのは、Enter の前の画（ブロックの置き方）と、終わったあとの y/n と動画だけ。閉じる前に人がブロックを押し込むと、指が学習の画に写る（191347・191441 で起きた。外した）。浅いと思ったら Enter の前に置き直すか、撮ったあとで外す。 + +**3. 撮ったあとに深さを測って、線を超えた本を外す** + +``` +# 1 本ずつの深さを見る（読むだけ） +python3 pi05/grasp_depth.py /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_YYYYMMDD_HHMMSS +# 新しい本だけのデータセットを作る（新しい名前。線を超えた本は外し、index.json の grasp_depth に値と理由を残す） +python3 pi05/build_demo_episodes.py --dataset mecharm_redblock_box_teach_pos5_deep_v1 --trim-mid-idle \ + --max-protrude-px 24 --run run_0001_YYYYMMDD_HHMMSS --run ... +# 今の deep とまとめる（元は変えない。外す本が増えたら --exclude-json に理由と一緒に書く） +python3 pi05/merge_raw_episodes.py --src mecharm_redblock_box_teach_v1_trim2_corr_v5_deep \ + --src mecharm_redblock_box_teach_pos5_deep_v1 --dataset <新しい名前> --tar +python3 pi05/check_dataset.py /Volumes/KIOXIA/r2r2r-data/datasets/<新しい名前>/raw_episodes +``` + +- `build_demo_episodes.py` の最後に `[DEPTH]` の行が出る: 浅くて外した本・**測れなかった本（外していない）**・**手が写っていそうな本（外していない）** の数と名前。測れなかった本と手の本は、`top.mp4` の閉じる前後を人が見て、外すなら merge の `--exclude-json` に書く +- 線は 24px（閉じて 1.0 秒後のはみ出し。約 9mm）。値の意味は `lab/twin/20261107_失敗の再生と置き場所/G_お手本の閉じる時刻/README.md` +- 補正入りの run（`run_policy_client.py`）を足すときも同じで、`build_correction_episodes.py ... --max-protrude-px 24` を付ける + +**4. 爪の電流でつかめたかを見る（依頼 116）**: 依頼 116 で電流を残すようになったのは実機推論の記録（`pi05/policy_recorder.py` → `{run}/recording/currents.csv`。`run_policy_client.py` で動かしたとき）だけ。**`record_teach_demo.py` の再生の記録（`recorder.py`）には電流は残らない**ので、この手順の教えた動きでは電流で見られない。補正入りの run を足すときは、`column -s, -t < {run}/recording/currents.csv | less` で閉じたあとの `gripper_current` を見る（依頼 116 の確かめ方では、つかむと数百〜990、放すと −50 前後になるはず。まだ実機で確かめていない。daemon に read_servo を置いていなければ `top_meta.json` の `currents_mode` が `get_angles_fallback` で、電流は空）。 diff --git a/pi05/build_correction_episodes.py b/pi05/build_correction_episodes.py index 1eaf821..e5bce79 100644 --- a/pi05/build_correction_episodes.py +++ b/pi05/build_correction_episodes.py @@ -27,6 +27,10 @@ mid_idle_keep_mask. 既定で詰める. --no-trim-mid-idle で前者だけ) - --trim-hold (corr_v3. 依頼 48): 中継点・補正の 1 刻みごとの「着いたあとの止まり」を, 区間ごとに 残す数を変えて詰める (hold_keep_mask. 止まりの判定とずれの切り方は mid_idle_keep_mask と同じ) + - --max-protrude-px (2026-11-07 依頼 117): grasp_depth.py で閉じて 1.0 秒後のブロックのはみ出しを測り, + それを超える本 (浅いつかみ) を外す. 閉じる命令は, エピソードで最初に爪が 1 になったコマの時刻かそれより前の + いちばん遅い命令. 値と理由は index.json の grasp_depth に残す. 測れなかった本は外さない. + 付けないときは測らず, 出力は今までと同じ - 元の記録は読むだけ (上書き・移動・削除しない) Usage: @@ -385,6 +389,18 @@ def build_one(run_dir, out_dir, args): return {"run": stem, "used": False, "reason": f"使えるステップが少ない ({len(sel['idx'])})"} tt = t[sel["idx"]] + # 依頼 117: つかみの深さで採否を決める (--max-protrude-px のときだけ. 既定では測らない) + depth = None + max_pr = getattr(args, "max_protrude_px", None) + if max_pr is not None: + import grasp_depth + drop, why, depth = grasp_depth.judge(run_dir, max_pr, + grasp_depth.first_close_label_time(tt, sel["state"][:, 6])) + if drop: + if out_dir.exists(): + why += " (前に作ったエピソードが残っている. 消していない)" + return {"run": stem, "used": False, "reason": why, "grasp_depth": depth} + # codex R1-4: 画像の候補も同じ決まりで絞る (除いた区間・rewind / home の画像を, 境界の # 最近傍として拾わない). 候補から選ぶので, 除いた時刻の画像は入らない fkeep, _ = step_keep_mask(fts, mode_ts, modes, windows) @@ -465,10 +481,13 @@ def build_one(run_dir, out_dir, args): "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"], - "correction_rows": meta["correction_rows"], "max_step_deg": meta["max_step_deg"]} + res = {"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"], + "correction_rows": meta["correction_rows"], "max_step_deg": meta["max_step_deg"]} + if depth is not None: + res["grasp_depth"] = depth + return res def check_compat(ours, ref): @@ -517,7 +536,8 @@ def write_readme(ds, results, args): "- 作成: `python3 pi05/build_correction_episodes.py`" + ("" if not args.no_trim_mid_idle else " --no-trim-mid-idle") + f" --gap-fill {args.gap_fill}" - + (" --trim-hold" if getattr(args, "trim_hold", False) else ""), + + (" --trim-hold" if getattr(args, "trim_hold", False) else "") + + (f" --max-protrude-px {args.max_protrude_px:g}" if getattr(args, "max_protrude_px", None) is not None else ""), f"- 元データ: `runs/pi05_policy//` (読むだけ。上書き・移動・削除しない)", "", "## 作り方", "", "- 結果が「成功」(y) の run だけ使う (`{run_stem}.json` の outcome)", @@ -534,7 +554,13 @@ def write_readme(ds, results, args): + ("前後の実測角をまっすぐ結ぶ (linear)" if args.gap_fill == "linear" else f"抜けの頭から速度 20 の速さ ({SPEED20_DEG_PER_SEC:.0f}°/秒. いちばん大きく動く関節) で前後の実測角を結ぶ" "直線を進み, 着いたら止まる (speed20)"), - "- 戻りきれなかった巻き戻しがある run は使わない (前後の姿勢が続かない)", "", + "- 戻りきれなかった巻き戻しがある run は使わない (前後の姿勢が続かない)", + ] + if getattr(args, "max_protrude_px", None) is not None: + lines.append(f"- 閉じて 1.0 秒後のブロックのはみ出し (`pi05/grasp_depth.py`) が {args.max_protrude_px:g}px を超える" + " run は使わない (浅いつかみ。値は index.json の grasp_depth。測れなかった run は外さない)") + lines += [ + "", "## 数", "", 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)})", "", @@ -550,12 +576,14 @@ def write_readme(ds, results, args): (ds / "README.md").write_text("\n".join(lines) + "\n") -def copy_code(ds): +def copy_code(ds, with_depth=False): """データと対のコード: このデータを作ったスクリプト (と読み込む部品) を ds/code/ に写す.""" dest = ds / "code" dest.mkdir(parents=True, exist_ok=True) for name in ("build_correction_episodes.py", "rewind_correct.py", "build_demo_episodes.py"): shutil.copy2(_HERE / name, dest / name) + if with_depth: + shutil.copy2(_HERE / "grasp_depth.py", dest / "grasp_depth.py") return dest @@ -573,6 +601,9 @@ def main(): "(corr_v2. 依頼 46)") ap.add_argument("--trim-hold", action="store_true", help="中継点・補正の 1 刻みごとの着いたあとの止まりを区間ごとに詰める (corr_v3. 依頼 48)") + ap.add_argument("--max-protrude-px", type=float, default=None, + help="閉じて 1.0 秒後のブロックのはみ出し (grasp_depth.py) がこれを超える run を外す. " + "例 24 (エイイチの決め 2026-11-07). 既定: 測らない (依頼 117)") ap.add_argument("--data-root", help=argparse.SUPPRESS) # テスト用 (KIOXIA の代わり) args = ap.parse_args() if args.trim_hold and args.no_trim_mid_idle: @@ -610,9 +641,11 @@ def main(): old.update(results) index_path.write_text(json.dumps(old, indent=2, ensure_ascii=False)) write_readme(ds, old, args) - print(f"[CODE] データと対のコード → {copy_code(ds)}") + print(f"[CODE] データと対のコード → {copy_code(ds, args.max_protrude_px is not None)}") 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.max_protrude_px is not None: + bde.print_depth_summary(list(results.values()), args.max_protrude_px) if args.check_with: probs = check_compat(out_root, args.check_with) print("[CHECK] お手本と一緒に使えるか: " + ("OK" if not probs else f"NG {len(probs)} 件")) diff --git a/pi05/build_demo_episodes.py b/pi05/build_demo_episodes.py index 669f520..d15fc31 100644 --- a/pi05/build_demo_episodes.py +++ b/pi05/build_demo_episodes.py @@ -26,6 +26,9 @@ 元の時刻を t_orig に残す. オプション未指定時の出力は従来と同一. - グリッパー切替ステップの前後 (i-1, i, i+1) は どちらの停止詰めでも必ず残す. - 採用条件: pick と place の人の判定がどちらも yes (既定). VLM 判定は使わない. + - --max-protrude-px を付けると (2026-11-07 依頼 117), grasp_depth.py で閉じて 1.0 秒後のブロックのはみ出しを測り, + それを超える本 (浅いつかみ) を外す. 測った値と理由は index.json の grasp_depth に残す. + 測れなかった本は外さない (grasp_depth.status=unmeasured). 付けないときは測らず, 出力は今までと同じ. Usage: python3 pi05/build_demo_episodes.py # 2026-10-15 以降の全試行 @@ -324,6 +327,17 @@ def build_one(run_dir, out_dir, args): state = np.concatenate([q, g[:, None]], axis=1).astype(np.float32) actions = np.concatenate([state[1:], state[-1:]], axis=0) + # 依頼 117: つかみの深さで採否を決める (--max-protrude-px のときだけ. 既定では測らない) + depth = None + max_pr = getattr(args, "max_protrude_px", None) + if max_pr is not None: + import grasp_depth + drop, why, depth = grasp_depth.judge(run_dir, max_pr, grasp_depth.first_close_label_time(t, g)) + if drop: + if out_dir.exists(): + why += " (前に作ったエピソードが残っている. 消していない)" + return {"run": stem, "used": False, "reason": why, "grasp_depth": depth} + # 停止区間を詰める motion = np.max(np.abs(actions - state), axis=1) keep = np.ones(len(t), dtype=bool) @@ -421,10 +435,13 @@ def build_one(run_dir, out_dir, args): "human_verdict": {"pick": pick_h, "place": place_h}, } (out_dir / "meta.json").write_text(json.dumps(meta, indent=2, ensure_ascii=False)) - return {"run": stem, "used": True, "reason": "ok", "steps": int(len(t)), - "idle_trimmed_sec": meta["idle_trimmed_sec"], - "steps_before_mid_trim": steps_before_mid_trim, - "mid_idle_trimmed_steps": mid_idle_trimmed_steps} + res = {"run": stem, "used": True, "reason": "ok", "steps": int(len(t)), + "idle_trimmed_sec": meta["idle_trimmed_sec"], + "steps_before_mid_trim": steps_before_mid_trim, + "mid_idle_trimmed_steps": mid_idle_trimmed_steps} + if depth is not None: + res["grasp_depth"] = depth + return res def assert_trim_target_is_new(ds): @@ -459,6 +476,9 @@ def main(): ap.add_argument("--trim-mid-idle", action="store_true", help="途中の停止区間を先頭 %d ステップだけ残して詰める (既定: しない)" % MID_IDLE_KEEP_STEPS) + ap.add_argument("--max-protrude-px", type=float, default=None, + help="閉じて 1.0 秒後のブロックのはみ出し (grasp_depth.py) がこれを超える本を外す. " + "例 24 (エイイチの決め 2026-11-07). 既定: 測らない (依頼 117)") args = ap.parse_args() ensure_ssd() @@ -498,6 +518,22 @@ def main(): 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)}" f" → {out_root}") + if args.max_protrude_px is not None: + print_depth_summary(results, args.max_protrude_px) + + +def print_depth_summary(results, line_px): + """依頼 117: つかみの深さで外した本・測れなかった本・手が写っていそうな本の数を出す.""" + ds_ = [r for r in results if r.get("grasp_depth")] + shallow = [r for r in ds_ if r["grasp_depth"].get("shallow")] + unmeasured = [r for r in ds_ if r["grasp_depth"].get("status") != "ok"] + hand = [r for r in ds_ if r["grasp_depth"].get("hand_suspect")] + print(f"[DEPTH] 線 {line_px:g}px: 測った {len(ds_)} 本 / 浅くて外した {len(shallow)} 本 / " + f"測れなかった {len(unmeasured)} 本 (外していない) / 手が写っていそう {len(hand)} 本 (外していない)") + for r in unmeasured: + print(f" 測れなかった: {r['run']} {r['grasp_depth'].get('reason')}") + for r in hand: + print(f" 手が写っていそう: {r['run']} (画で確かめること)") if __name__ == "__main__": diff --git a/pi05/excludes/teach_v1_trim2_corr_v5_deep.json b/pi05/excludes/teach_v1_trim2_corr_v5_deep.json new file mode 100644 index 0000000..c3557d5 --- /dev/null +++ b/pi05/excludes/teach_v1_trim2_corr_v5_deep.json @@ -0,0 +1,299 @@ +{ + "pi05_20261031_181946": { + "reason": "浅い: 閉じて 1.0 秒後のはみ出し 37.8px > 24px", + "kind": "補正入り", + "after_px": 37.8, + "before_px": 61.4, + "hand_in_frame": false, + "line_px": 24.0, + "measured_by": "依頼113 G g_table.csv (corr_v5_main) / pi05/grasp_depth.py で同じ値", + "grasp_depth": { + "status": "ok", + "reason": null, + "t_close": 1793438589.926, + "frame_before": 1763, + "frame_after": 1773, + "before_px": 61.4, + "after_px": 37.8, + "after_gap_px": 99.0, + "hand_suspect": false, + "pre_px": { + "pre1.5s": 60.9, + "pre0.5s": 61.0, + "before": 61.4 + }, + "center_px": [ + 1148, + 699 + ], + "center_by": "E", + "axis_from": "body", + "max_protrude_px": 24.0, + "shallow": true + } + }, + "pi05_20261101_192101": { + "reason": "浅い: 閉じて 1.0 秒後のはみ出し 33.4px > 24px", + "kind": "補正入り", + "after_px": 33.4, + "before_px": 57.4, + "hand_in_frame": false, + "line_px": 24.0, + "measured_by": "依頼113 G g_table.csv (corr_v5_main) / pi05/grasp_depth.py で同じ値", + "grasp_depth": { + "status": "ok", + "reason": null, + "t_close": 1793528521.445, + "frame_before": 507, + "frame_after": 517, + "before_px": 57.4, + "after_px": 33.4, + "after_gap_px": 101.0, + "hand_suspect": false, + "pre_px": { + "pre1.5s": 57.5, + "pre0.5s": 57.4, + "before": 57.4 + }, + "center_px": [ + 1146, + 695 + ], + "center_by": "E", + "axis_from": "body", + "max_protrude_px": 24.0, + "shallow": true + } + }, + "pi05_20261106_184452": { + "reason": "浅い: 閉じて 1.0 秒後のはみ出し 49.2px > 24px", + "kind": "補正入り", + "after_px": 49.2, + "before_px": 75.0, + "hand_in_frame": false, + "line_px": 24.0, + "measured_by": "依頼113 G g_table.csv (corr_v5_main) / pi05/grasp_depth.py で同じ値", + "grasp_depth": { + "status": "ok", + "reason": null, + "t_close": 1793958333.441, + "frame_before": 321, + "frame_after": 332, + "before_px": 75.0, + "after_px": 49.2, + "after_gap_px": 99.5, + "hand_suspect": false, + "pre_px": { + "pre1.5s": 67.5, + "pre0.5s": 82.9, + "before": 75.0 + }, + "center_px": [ + 1147, + 701 + ], + "center_by": "E", + "axis_from": "body", + "max_protrude_px": 24.0, + "shallow": true + } + }, + "pi05_20261106_184906": { + "reason": "浅い: 閉じて 1.0 秒後のはみ出し 47.1px > 24px", + "kind": "補正入り", + "after_px": 47.1, + "before_px": 73.8, + "hand_in_frame": false, + "line_px": 24.0, + "measured_by": "依頼113 G g_table.csv (corr_v5_main) / pi05/grasp_depth.py で同じ値", + "grasp_depth": { + "status": "ok", + "reason": null, + "t_close": 1793958582.267, + "frame_before": 276, + "frame_after": 286, + "before_px": 73.8, + "after_px": 47.1, + "after_gap_px": 99.4, + "hand_suspect": false, + "pre_px": { + "pre1.5s": 73.4, + "pre0.5s": 82.2, + "before": 73.8 + }, + "center_px": [ + 1141, + 700 + ], + "center_by": "E", + "axis_from": "body", + "max_protrude_px": 24.0, + "shallow": true + } + }, + "run_0001_20261019_183454": { + "reason": "浅い: 閉じて 1.0 秒後のはみ出し 38.9px > 24px", + "kind": "教えた動き", + "after_px": 38.9, + "before_px": 67.2, + "hand_in_frame": false, + "line_px": 24.0, + "measured_by": "依頼113 G g_table.csv (corr_v5_main) / pi05/grasp_depth.py で同じ値", + "grasp_depth": { + "status": "ok", + "reason": null, + "t_close": 1792402514.214, + "frame_before": 476, + "frame_after": 501, + "before_px": 67.2, + "after_px": 38.9, + "after_gap_px": 99.7, + "hand_suspect": false, + "pre_px": { + "pre1.5s": 59.0, + "pre0.5s": 67.2, + "before": 67.2 + }, + "center_px": [ + 608, + 594 + ], + "center_by": "wide", + "axis_from": "tips", + "max_protrude_px": 24.0, + "shallow": true + } + }, + "run_0001_20261019_191347": { + "reason": "人の指が写っている: 閉じる直前〜ラベルのコマで指がブロックを押し込む (閉じて 1.0 秒後 19.7px)", + "kind": "教えた動き", + "after_px": 19.7, + "before_px": 189.5, + "hand_in_frame": true, + "line_px": 24.0, + "measured_by": "依頼113 G g_table.csv (corr_v5_main) / pi05/grasp_depth.py で同じ値", + "grasp_depth": { + "status": "ok", + "reason": null, + "t_close": 1792404847.237, + "frame_before": 466, + "frame_after": 491, + "before_px": null, + "after_px": 19.7, + "after_gap_px": 100.1, + "hand_suspect": true, + "pre_px": { + "pre1.5s": 359.9, + "pre0.5s": 55.4, + "before": 189.5 + }, + "center_px": [ + 549, + 683 + ], + "center_by": "wide", + "axis_from": "body", + "max_protrude_px": 24.0, + "shallow": false + } + }, + "run_0001_20261019_191441": { + "reason": "人の指が写っている: 閉じる直前〜ラベルのコマで指がブロックを押し込む (閉じて 1.0 秒後 14.5px)", + "kind": "教えた動き", + "after_px": 14.5, + "before_px": 177.3, + "hand_in_frame": true, + "line_px": 24.0, + "measured_by": "依頼113 G g_table.csv (corr_v5_main) / pi05/grasp_depth.py で同じ値", + "grasp_depth": { + "status": "ok", + "reason": null, + "t_close": 1792404906.418, + "frame_before": 598, + "frame_after": 623, + "before_px": null, + "after_px": 14.5, + "after_gap_px": 99.5, + "hand_suspect": true, + "pre_px": { + "pre1.5s": 206.8, + "pre0.5s": 68.1, + "before": 177.3 + }, + "center_px": [ + 500, + 791 + ], + "center_by": "wide", + "axis_from": "body", + "max_protrude_px": 24.0, + "shallow": false + } + }, + "run_0001_20261019_192343": { + "reason": "浅い: 閉じて 1.0 秒後のはみ出し 27.6px > 24px", + "kind": "教えた動き", + "after_px": 27.6, + "before_px": 54.5, + "hand_in_frame": false, + "line_px": 24.0, + "measured_by": "依頼113 G g_table.csv (corr_v5_main) / pi05/grasp_depth.py で同じ値", + "grasp_depth": { + "status": "ok", + "reason": null, + "t_close": 1792405442.394, + "frame_before": 452, + "frame_after": 477, + "before_px": 54.5, + "after_px": 27.6, + "after_gap_px": 100.0, + "hand_suspect": false, + "pre_px": { + "pre1.5s": 54.8, + "pre0.5s": 54.7, + "before": 54.5 + }, + "center_px": [ + 747, + 649 + ], + "center_by": "E", + "axis_from": "body", + "max_protrude_px": 24.0, + "shallow": true + } + }, + "run_0001_20261019_192633": { + "reason": "浅い: 閉じて 1.0 秒後のはみ出し 28.9px > 24px", + "kind": "教えた動き", + "after_px": 28.9, + "before_px": 59.0, + "hand_in_frame": false, + "line_px": 24.0, + "measured_by": "依頼113 G g_table.csv (corr_v5_main) / pi05/grasp_depth.py で同じ値", + "grasp_depth": { + "status": "ok", + "reason": null, + "t_close": 1792405612.535, + "frame_before": 450, + "frame_after": 475, + "before_px": 59.0, + "after_px": 28.9, + "after_gap_px": 99.9, + "hand_suspect": false, + "pre_px": { + "pre1.5s": 59.1, + "pre0.5s": 59.0, + "before": 59.0 + }, + "center_px": [ + 775, + 665 + ], + "center_by": "E", + "axis_from": "body", + "max_protrude_px": 24.0, + "shallow": true + } + } +} \ No newline at end of file diff --git a/pi05/excludes/teach_v1_trim2_corr_v5_deep_note.md b/pi05/excludes/teach_v1_trim2_corr_v5_deep_note.md new file mode 100644 index 0000000..001f409 --- /dev/null +++ b/pi05/excludes/teach_v1_trim2_corr_v5_deep_note.md @@ -0,0 +1,23 @@ +## 外した理由（2026-11-07 依頼 117。エイイチの決め） + +`mecharm_redblock_box_teach_v1_trim2_corr_v5`（v3 の学習データ 30 本）から、**浅いつかみの 7 本**と**人の指が写った 2 本**を外した版。元の `teach_v1_trim2`・`corr_v5`・`teach_v1_trim2_corr_v5` は変えていない（読んで写しただけ）。 + +- 調べた記録: `lab/twin/20261107_失敗の再生と置き場所/G_お手本の閉じる時刻/README.md`（依頼 113 G）。値は同じフォルダの `g_table.csv`（dataset = `corr_v5_main`）と `g_grasp_metrics.csv` +- 測り方: G の `g_images.py` の `measure_axis`（爪の向きに沿ったはみ出し。ブロックの赤の画素を爪の向きに投げた最大 − 爪先の青い印 2 つの中点。元の `top.mp4` の 1920×1080 で測る。約 2.6px/mm）。同じ測り方を部品にした `pi05/grasp_depth.py` で測り直し、30 本とも G と同じ値（差 0.0px） +- 浅いとみなす線: 閉じる命令の 1.0 秒後のはみ出しが **24px（約 9mm）を超える**（π0.5 の成功 4 件の最大 14px ＋ 10px） +- 指: 教えた動きの 191347・191441 は、閉じる瞬間に人の指がブロックを爪の間へ押し込んでいて、学習の画に写っている（G で画を見て確かめた。肌を赤として拾うので閉じる直前の値は 177〜190px になり使えない） + +| エピソード | 種類 | 閉じる直前 px | 閉じて 1.0 秒後 px | 理由 | +| --- | --- | --- | --- | --- | +| pi05_20261031_181946 | 補正入り | 61.4 | 37.8 | 浅い | +| pi05_20261101_192101 | 補正入り | 57.4 | 33.4 | 浅い | +| pi05_20261106_184452 | 補正入り | 75.0 | 49.2 | 浅い | +| pi05_20261106_184906 | 補正入り | 73.8 | 47.1 | 浅い | +| run_0001_20261019_183454 | 教えた動き | 67.2 | 38.9 | 浅い | +| run_0001_20261019_192343 | 教えた動き | 54.5 | 27.6 | 浅い | +| run_0001_20261019_192633 | 教えた動き | 59.0 | 28.9 | 浅い | +| run_0001_20261019_191347 | 教えた動き | —（指） | 19.7 | 人の指が写っている | +| run_0001_20261019_191441 | 教えた動き | —（指） | 14.5 | 人の指が写っている | + +- 外した本の一覧と測った値は `index.json` の `excluded`、作るときに渡した一覧は `code/teach_v1_trim2_corr_v5_deep.json`（r2r2r-scripts の `pi05/excludes/` と同じもの） +- 試している置き場所（教えた位置 5 = `drag_v2_20221013_233323`）の教えた動き 4 本（194818〜195120。8〜15px）は残っている。補正入りは浅い 4 本が抜けて 7 本 diff --git a/pi05/grasp_depth.py b/pi05/grasp_depth.py new file mode 100644 index 0000000..37bfbbf --- /dev/null +++ b/pi05/grasp_depth.py @@ -0,0 +1,344 @@ +#!/usr/bin/env python3 +"""grasp_depth.py — 1 本の記録 (run のフォルダ) で, 爪を閉じたときにブロックが爪の奥まで入っていたかを測る (2026-11-07 依頼 117). + +依頼 113 G (lab/twin/20261107_失敗の再生と置き場所/G_お手本の閉じる時刻/g_images.py の measure_axis) と同じ測り方を, +学習データを作るスクリプト (build_demo_episodes.py / build_correction_episodes.py の --max-protrude-px) から使える形にした. +赤の決まり・青の印の決まり・ブロックの探し方は E (e_images.red_center) と G (red_center_wide・measure_axis) のまま. + +入力 (run のフォルダ. runs/run_pick_place/{run}/ でも runs/pi05_policy/{run}/ でもよい): + recording/states.csv 爪の命令 (event_type=gripper_cmd. gripper_state=1 が閉じる) の時刻 + recording/top_frame_log.csv 上カメラのコマの時刻 (frame_idx, capture_ts) + recording/top.mp4 上カメラ (元の 1920x1080 のまま測る) + +測るコマ (G と同じ): + before : 閉じる命令の時刻か, それより前でいちばん近いコマ (閉じる直前) + after : 閉じる命令の 1.0 秒後か, それよりあとでいちばん近いコマ (爪が閉じきった形) + pre1p5 / pre0p5 : 命令の 1.5 秒前・0.5 秒前 (人の手が写っていないかを見るためだけに使う) + +はみ出し (protrude_px): 爪の向き u (胴の青い印 → 爪先の青い印 2 つの中点) に沿って, ブロックの赤の画素を u に投げた +最大 − 爪先の印の中点. 大きいほどブロックが爪先から先へ出ている (= 浅い). 約 2.6px/mm (E の目安). +人の手: 肌を赤として拾うと値が HAND_PX (100px) を超える (G で確かめた). 閉じる前のコマのどれかでそうなったら +hand_suspect=True にし, その回の before_px は使わない (None). after_px は爪が閉じたあとなので残す. + +測れなかった本 (爪の命令がない・コマがない・印が見えない・ブロックが見つからない) は status="unmeasured" と +理由 (reason) を返す. 学習データを作る側は, 測れなかった本は外さずに「測れなかった」と残す. + + python3 pi05/grasp_depth.py /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_194818 + python3 pi05/grasp_depth.py RUN_DIR [RUN_DIR ...] --csv out.csv # 何本でも. 1 本 1 行 + +読むだけ (記録には何も書かない). +""" +import argparse +import csv +import json +import sys +from pathlib import Path + +import cv2 +import numpy as np + +AFTER_SEC = 1.0 # 閉じてから何秒後のコマで測るか (E・G と同じ) +PRE_SECS = (1.5, 0.5) # 手が写っていないかを見る閉じる前のコマ (G の pre1p5 / pre0p5) +DEFAULT_MAX_PROTRUDE_PX = 24.0 # 浅いとみなす線 (E の成功の最大 14px + 10px. エイイチの決め 2026-11-07) +HAND_PX = 100.0 # 閉じる前のはみ出しがこれを超えたら, 人の手 (肌) を赤として拾っている印 (G) +HALF = 350 # 爪の向きを測るときの切り出しの半分 (胴の青い印まで入るように) +# 赤・青の色の幅 (E と同じ) +BLUE_LO, BLUE_HI = (100, 150, 80), (130, 255, 255) + + +def _red_mask(hsv, s_min): + return cv2.inRange(hsv, (0, s_min, 70), (10, 255, 255)) | cv2.inRange(hsv, (170, s_min, 70), (180, 255, 255)) + + +def red_center(img): + """E の red_center と同じ. 画の x 700〜1400・y 350 より下で, 面積 300 以上・細長くない赤の塊のいちばん大きいもの.""" + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + m = _red_mask(hsv, 120) + m[:, :700] = 0; m[:, 1400:] = 0; m[:350] = 0 + n, lab, st, cen = cv2.connectedComponentsWithStats(m) + best = None + for k in range(1, n): + x, y, w, h, a = st[k] + if a < 300 or max(w, h) > 3 * min(w, h): # 細長い赤 (配線) は外す + continue + if best is None or a > best[0]: + best = (a, cen[k]) + return None if best is None else (int(best[1][0]), int(best[1][1])) + + +def red_center_wide(img, s_min=120): + """G の red_center_wide と同じ. 探す範囲を画の全体に広げ, 爪先の青い印 2 つが 200px 以内にある赤の塊だけを候補にする + (10/15〜10/19 の記録はブロックが画の左寄りにあるため).""" + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + m = _red_mask(hsv, s_min) + m[:, :40] = 0; m[:, -40:] = 0; m[:40] = 0; m[-40:] = 0 + n, lab, st, cen = cv2.connectedComponentsWithStats(m) + nb, _, stb, cenb = cv2.connectedComponentsWithStats(cv2.inRange(hsv, BLUE_LO, BLUE_HI)) + blues = [cenb[k] for k in range(1, nb) if stb[k][4] >= 60] + best = None + for k in range(1, n): + x, y, w, h, a = st[k] + if a < 300 or max(w, h) > 3 * min(w, h): + continue + d = sorted(np.hypot(*(b - cen[k])) for b in blues) + if len(d) < 2 or d[1] > 200: + continue + if best is None or a > best[0]: + best = (a, cen[k]) + return None if best is None else (int(best[1][0]), int(best[1][1])) + + +def _axis_once(roi, s_min): + hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV) + n, lab, st, cen = cv2.connectedComponentsWithStats(_red_mask(hsv, s_min)) + rb = [k for k in range(1, n) if st[k][4] >= 300 and max(st[k][2], st[k][3]) < 3 * min(st[k][2], st[k][3])] + nb, _, stb, cenb = cv2.connectedComponentsWithStats(cv2.inRange(hsv, BLUE_LO, BLUE_HI)) + bb = [k for k in range(1, nb) if stb[k][4] >= 60] + if not rb or len(bb) < 2: + return None + r = max(rb, key=lambda k: st[k][4]) + bb = sorted(bb, key=lambda k: np.hypot(*(cenb[k] - cen[r]))) + c1, c2 = cenb[bb[0]], cenb[bb[1]] + mid = (c1 + c2) / 2 + tip_dir = (c2 - c1) / max(1e-6, np.linalg.norm(c2 - c1)) + body = None + for k in bb[2:]: + v = cenb[k] - mid; d = np.linalg.norm(v) + if 120 <= d <= 400 and abs(np.dot(v / d, tip_dir)) < 0.5: + body = cenb[k]; break + if body is not None: + u = (mid - body) / np.linalg.norm(mid - body); src = "body" + else: + # 胴の印が写らないとき: 爪先の印 2 つを結ぶ線に直角で, 画の下向き (どの記録も爪先は画の下寄りを向く) + u = np.array([-tip_dir[1], tip_dir[0]]) + u = u if u[1] > 0 else -u + src = "tips" + ys, xs = np.nonzero(lab == r) + proj = (np.stack([xs, ys], 1) - mid) @ u + lat = float((cen[r] - mid) @ np.array([-u[1], u[0]])) + return {"axis_protrude_px": round(float(proj.max()), 1), "axis_lateral_px": round(lat, 1), + "axis_marker_gap_px": round(float(np.linalg.norm(c1 - c2)), 1), + "axis_deg": round(float(np.degrees(np.arctan2(u[0], u[1]))), 1), "axis_from": src, + "axis_red_area": int(st[r][4]), "axis_s_min": s_min} + + +def measure_axis(f, c, s_min=120): + """G の measure_axis と同じ. c (ブロックのおおよその中心) のまわりで, 爪の向きに沿うはみ出しを測る. + 彩度 120 で拾った赤が彩度 60 で拾った面積の半分未満なら, 彩度 60 の値を使う (薄いピンクに写る回).""" + H_, W_ = f.shape[:2] + x0, y0 = max(0, c[0] - HALF), max(0, c[1] - HALF) + roi = f[y0:min(H_, c[1] + HALF), x0:min(W_, c[0] + HALF)] + m = _axis_once(roi, s_min) + if s_min == 120: + m60 = _axis_once(roi, 60) + if m60 and (m is None or m["axis_red_area"] < 0.5 * m60["axis_red_area"]): + m = m60 + return m + + +def find_center(f_before, f_after): + """G と同じ順で探す: E の探し方 → 画の全体 (彩度 120) → 画の全体 (彩度 60). + 戻り値 (中心, 彩度の下限, 探し方). 見つからなければ (None, 120, "none").""" + for f in (f_before, f_after): + c = red_center(f) if f is not None else None + if c: + return c, 120, "E" + for f in (f_before, f_after): + c = red_center_wide(f) if f is not None else None + if c: + return c, 120, "wide" + for f in (f_before, f_after): + c = red_center_wide(f, 60) if f is not None else None + if c: + return c, 60, "wide_s60" + return None, 120, "none" + + +# ---------------------------------------------------------------- 記録を読む +def load_gripper_cmds(states_csv): + """states.csv の gripper_cmd 行. 戻り値 (時刻の並び, 状態の並び 0/1). 時刻の順に並べる. + 読み方は build_demo_episodes.load_states と同じ (gripper_state が 1 なら閉じる).""" + ts, ss = [], [] + with Path(states_csv).open(newline="") as f: + for row in csv.DictReader(f): + if row.get("event_type") != "gripper_cmd": + continue + try: + t = float(row["ts"]) + s = 1.0 if int(float(row["gripper_state"])) == 1 else 0.0 + except Exception: + continue + ts.append(t); ss.append(s) + ts, ss = np.array(ts), np.array(ss) + o = np.argsort(ts, kind="stable") + return ts[o], ss[o] + + +def close_time(gt, gs, t_label=None): + """閉じる命令の時刻. + t_label を渡したとき: その時刻か, それより前でいちばん遅い閉じる命令 (G と同じ. エピソードで最初に爪が 1 になった + コマの時刻を渡す). 渡さないとき: 開いた状態 (または始め) から最初に閉じた命令.""" + closes = [float(t) for t, s in zip(gt, gs) if s == 1] + if t_label is not None: + c = [t for t in closes if t <= t_label + 1e-6] + return max(c) if c else None + prev = 0.0 + for t, s in zip(gt, gs): + if s == 1 and prev == 0: + return float(t) + prev = s + return None + + +def load_frame_log(path): + idx, ts = [], [] + with Path(path).open(newline="") as f: + for row in csv.DictReader(f): + idx.append(int(row["frame_idx"])); ts.append(float(row["capture_ts"])) + return np.array(idx), np.array(ts) + + +def _read(cap, idx): + cap.set(cv2.CAP_PROP_POS_FRAMES, int(idx)) + ok, f = cap.read() + return f if ok else None + + +def measure_run(run_dir, t_label=None, max_protrude_px=None): + """1 本を測る. 戻り値は dict (JSON にそのまま書ける値だけ). + status : "ok" / "unmeasured" + reason : unmeasured のときの理由 + t_close : 閉じる命令の時刻 (states.csv の ts) + frame_before / frame_after : 測った top.mp4 のコマ番号 + before_px / after_px : 閉じる直前・閉じて 1.0 秒後のはみ出し px (after が採否に使う値) + after_gap_px : 閉じて 1.0 秒後の爪先の印の間 px (空振りなら 75px 前後. つかめば 98〜101px) + hand_suspect : 閉じる前のコマで人の手が写っていそうか (はみ出しが HAND_PX を超えた) + shallow : max_protrude_px を渡したとき, after_px がそれを超えたか (測れなければ None)""" + run_dir = Path(run_dir) + rec = run_dir / "recording" + out = {"status": "unmeasured", "reason": None, "t_close": None, "frame_before": None, "frame_after": None, + "before_px": None, "after_px": None, "after_gap_px": None, "hand_suspect": False, + "pre_px": {}, "center_px": None, "center_by": None, "axis_from": None} + if max_protrude_px is not None: + out["max_protrude_px"] = float(max_protrude_px) + out["shallow"] = None + 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: + out["reason"] = "missing:" + ",".join(missing) + return out + gt, gs = load_gripper_cmds(rec / "states.csv") + tc = close_time(gt, gs, t_label) + if tc is None: + out["reason"] = "閉じる命令がない" + return out + out["t_close"] = round(tc, 3) + fidx, fts = load_frame_log(rec / "top_frame_log.csv") + if not len(fts): + out["reason"] = "上カメラのコマがない" + return out + o = np.argsort(fts, kind="stable"); fidx, fts = fidx[o], fts[o] + + def at_or_before(x): + return int(fidx[max(0, int(np.searchsorted(fts, x, side="right") - 1))]) + + def after(x): + k = int(np.searchsorted(fts, x)) + return None if k >= len(fts) else int(fidx[k]) + + if fts[0] > tc: + out["reason"] = "閉じる前のコマがない" + return out + fb, fa = at_or_before(tc), after(tc + AFTER_SEC) + if fa is None: + out["reason"] = f"閉じて {AFTER_SEC:.1f} 秒後のコマがない" + return out + out["frame_before"], out["frame_after"] = fb, fa + cap = cv2.VideoCapture(str(rec / "top.mp4")) + try: + img_b, img_a = _read(cap, fb), _read(cap, fa) + if img_a is None: + out["reason"] = f"top.mp4 のコマ {fa} が読めない" + return out + c, s_min, by = find_center(img_b, img_a) + out["center_by"] = by + if c is None: + out["reason"] = "ブロックが見つからない" + return out + out["center_px"] = [int(c[0]), int(c[1])] + ma = measure_axis(img_a, c, s_min) + if ma is None: + out["reason"] = "閉じたあとのコマで爪先の印かブロックが見えない" + return out + out["after_px"] = ma["axis_protrude_px"] + out["after_gap_px"] = ma["axis_marker_gap_px"] + out["axis_from"] = ma["axis_from"] + pre = {} + for sec in PRE_SECS: + if fts[0] <= tc - sec: + img = _read(cap, at_or_before(tc - sec)) + m = measure_axis(img, c, s_min) if img is not None else None + pre[f"pre{sec:g}s"] = None if m is None else m["axis_protrude_px"] + mb = measure_axis(img_b, c, s_min) if img_b is not None else None + bpx = None if mb is None else mb["axis_protrude_px"] + pre["before"] = bpx + out["pre_px"] = pre + out["hand_suspect"] = any(v is not None and v > HAND_PX for v in pre.values()) + out["before_px"] = None if (bpx is None or bpx > HAND_PX) else bpx + finally: + cap.release() + out["status"] = "ok" + if max_protrude_px is not None: + out["shallow"] = bool(out["after_px"] > max_protrude_px) + return out + + +def first_close_label_time(t_abs, grip): + """エピソードの並び (元の時刻 t_abs と爪の列 grip) で最初に 0 → 1 になったコマの時刻. なければ None.""" + sw = np.flatnonzero((np.asarray(grip[1:]) == 1) & (np.asarray(grip[:-1]) == 0)) + 1 + return float(t_abs[sw[0]]) if len(sw) else None + + +def judge(run_dir, max_protrude_px, t_label=None): + """学習データを作る側から呼ぶ. 戻り値 (外すか, 理由の文字, 測った値の dict). + 測れなかった本は外さない (理由 grasp_depth_unmeasured を値に残す).""" + m = measure_run(run_dir, t_label=t_label, max_protrude_px=max_protrude_px) + if m["status"] != "ok": + return False, f"grasp_depth_unmeasured:{m['reason']}", m + if m["shallow"]: + return True, f"shallow_grasp after_px={m['after_px']} > {max_protrude_px:g}", m + return False, "ok", m + + +def main(): + ap = argparse.ArgumentParser(description=__doc__.split("\n")[0]) + ap.add_argument("run_dir", nargs="+", help="run のフォルダ (recording/ がある所)") + ap.add_argument("--max-protrude-px", type=float, default=DEFAULT_MAX_PROTRUDE_PX, + help=f"浅いとみなす線 (閉じて 1.0 秒後のはみ出し px. 既定 {DEFAULT_MAX_PROTRUDE_PX:g})") + ap.add_argument("--csv", help="1 本 1 行の CSV に書く (書く先はこのファイルだけ)") + args = ap.parse_args() + rows = [] + for d in args.run_dir: + m = measure_run(d, max_protrude_px=args.max_protrude_px) + name = Path(d).name + if m["status"] == "ok": + flag = "浅い" if m["shallow"] else "深い" + hand = " 手が写っていそう" if m["hand_suspect"] else "" + print(f"[{flag}] {name} 閉じる直前 {m['before_px']}px 閉じて 1.0 秒後 {m['after_px']}px " + f"(線 {args.max_protrude_px:g}px) 爪先の印の間 {m['after_gap_px']}px{hand}") + else: + print(f"[測れない] {name} {m['reason']}") + rows.append({"run": name, **{k: (json.dumps(v, ensure_ascii=False) if isinstance(v, (dict, list)) else v) + for k, v in m.items()}}) + if args.csv: + keys = [] + for r in rows: + keys += [k for k in r if k not in keys] + with open(args.csv, "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=keys); w.writeheader(); w.writerows(rows) + print(f"[CSV ] {args.csv}") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/pi05/merge_raw_episodes.py b/pi05/merge_raw_episodes.py index a91cfa4..eba2841 100644 --- a/pi05/merge_raw_episodes.py +++ b/pi05/merge_raw_episodes.py @@ -15,6 +15,12 @@ eval_policy_offline.py) も「学習に使ったデータ」を 1 つのフォ datasets/{DATASET}/code/merge_raw_episodes.py このスクリプトの写し (データと対のコード) datasets/{DATASET}/{DATASET}_raw_episodes.tar (--tar) Pod へ送る形. ._* (AppleDouble) は入れない +外す本 (--exclude-json. 2026-11-07 依頼 117): + {"エピソード名": {"reason": "外す理由", ...測った値など}, ...} の JSON を渡すと, その本は写さない. + index.json の excluded に中身をそのまま残し, README に表を書く. 元のどれにもない名前があれば何も書かずに止まる. + --readme-note に Markdown のファイルを渡すと, README の最後にそのまま足す (外した理由の説明など). + 付けないときの出力は今までと同じ. + 決まり: - 元のデータセットは読むだけ (上書き・移動・削除しない) - 写す前に, 全部の元が お手本 (最初の --src) と一緒に使えるか (キー・型・FPS・画像の大きさ) を確かめ, @@ -69,6 +75,8 @@ def main(): ap.add_argument("--dataset", required=True, help="まとめた先のデータセット名 (新しい名前)") ap.add_argument("--force", action="store_true", help="まとめた先を作り直す (元には触らない)") ap.add_argument("--tar", action="store_true", help="Pod へ送る tar も作る") + ap.add_argument("--exclude-json", help="外す本と理由の JSON ({エピソード名: {reason: ..., ...}}). 依頼 117") + ap.add_argument("--readme-note", help="README の最後に足す Markdown のファイル") ap.add_argument("--data-root", help=argparse.SUPPRESS) # テスト用 (KIOXIA の代わり) args = ap.parse_args() if args.data_root: @@ -99,6 +107,14 @@ def main(): if ep.name in names: sys.exit(f"[ABORT] エピソード名が重なる: {ep.name} ({names[ep.name]} と {s})") names[ep.name] = s + excluded = json.loads(Path(args.exclude_json).read_text()) if args.exclude_json else {} + unknown = [k for k in excluded if k not in names] + if unknown: + sys.exit(f"[ABORT] 外す本が元のどれにもない: {', '.join(unknown)}") + no_reason = [k for k, v in excluded.items() if not (isinstance(v, dict) and v.get("reason"))] + if no_reason: + sys.exit(f"[ABORT] 外す本に理由 (reason) がない: {', '.join(no_reason)}") + note = Path(args.readme_note).read_text() if args.readme_note else None ds = dsroot / args.dataset if ds.exists(): @@ -109,7 +125,11 @@ def main(): out.mkdir(parents=True) index = {"sources": {}, "episodes": {}} for src_name, s in zip(args.src, srcs): - eps = episodes(s) + eps = [ep for ep in episodes(s) if ep.name not in excluded] + for ep in episodes(s): + if ep.name in excluded: + excluded[ep.name] = dict(excluded[ep.name], from_dataset=src_name, **{ + k: v for k, v in ep_summary(ep).items() if k in ("steps", "correct_steps")}) for ep in eps: shutil.copytree(ep, out / ep.name, ignore=_ignore_appledouble) index["episodes"][ep.name] = dict(ep_summary(ep), from_dataset=src_name) @@ -129,22 +149,34 @@ def main(): total_steps = sum(v["steps"] for v in index["episodes"].values()) index.update(total_episodes=total_eps, total_steps=total_steps, repo_id=f"local/{args.dataset}") + if excluded: + index["excluded"] = excluded (ds / "index.json").write_text(json.dumps(index, indent=2, ensure_ascii=False)) lines = [ f"# {args.dataset}", "", "いくつかの raw_episodes を 1 つにまとめたデータセット (元は読むだけ。写しを置く)。", "", "- 作成: `python3 pi05/merge_raw_episodes.py " - + " ".join(f"--src {s}" for s in args.src) + f" --dataset {args.dataset}`", + + " ".join(f"--src {s}" for s in args.src) + f" --dataset {args.dataset}" + + (f" --exclude-json {Path(args.exclude_json).name}" if excluded else "") + "`", f"- LeRobot に変換するときの repo_id: `local/{args.dataset}`", "", f"合計 {total_eps} 本 / {total_steps} コマ", "", "| 元 | 本数 | コマ | うち補正 (correct) |", "| --- | --- | --- | --- |", ] for k, v in index["sources"].items(): lines.append(f"| {k} | {v['episodes']} | {v['steps']} | {v['correct_steps']} |") + if excluded: + lines += ["", f"## 外した本 ({len(excluded)} 本. `index.json` の `excluded`)", "", + "| エピソード | 元 | コマ | 理由 |", "| --- | --- | --- | --- |"] + for k, v in excluded.items(): + lines.append(f"| {k} | {v['from_dataset']} | {v['steps']} | {v['reason']} |") + if note: + lines += ["", note.rstrip("\n")] (ds / "README.md").write_text("\n".join(lines) + "\n") code = ds / "code" code.mkdir(exist_ok=True) shutil.copy2(_HERE, code / _HERE.name) # データと対のコード + if excluded: + shutil.copy2(args.exclude_json, code / Path(args.exclude_json).name) # 外した本の一覧も対で残す print(f"\n合計 {total_eps} 本 / {total_steps} コマ → {out}") if args.tar: t = make_tar(ds, args.dataset) diff --git a/pi05/test_grasp_depth.py b/pi05/test_grasp_depth.py new file mode 100644 index 0000000..48da962 --- /dev/null +++ b/pi05/test_grasp_depth.py @@ -0,0 +1,336 @@ +#!/usr/bin/env python3 +"""test_grasp_depth.py — つかみの深さの部品 (grasp_depth.py) と, 学習データを作る側の --max-protrude-px の確かめ (2026-11-07 依頼 117). + + python3 pi05/test_grasp_depth.py -v + +合成の画と記録だけ (KIOXIA にもアームにも触らない). 確かめること: + - 爪先の青い印 2 つ・胴の印・赤いブロックを描いた画で, 爪の向きに沿うはみ出しが描いた値になる (斜めの爪でも) + - run のフォルダから, 閉じる命令の直前と 1.0 秒後のコマを選んで測る. 線を超えたら shallow + - 閉じる前のコマで大きな赤 (人の手の肌) を拾ったら hand_suspect. その回の閉じる直前の値は使わない + - 測れない本 (閉じる命令がない・ブロックが見つからない) は status=unmeasured と理由を返す + - build_demo_episodes / build_correction_episodes: 引数なしでは測らず, 出力 (npz・meta・画・戻り値) が今までと同じ. + 線を超えた本は外して index に値と理由を残す. 測れなかった本は外さない +""" +import argparse +import json +import sys +import tempfile +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 build_correction_episodes as bce # noqa: E402 +import build_demo_episodes as bde # noqa: E402 +import grasp_depth as gd # noqa: E402 +import test_pad_build as tpb # noqa: E402 + +W, H = 1920, 1080 +RED, BLUE = (40, 30, 220), (220, 60, 20) # BGR. HSV で 赤 (H≒178, S≒220) / 青 (H≒112, S≒231) +BG = (90, 110, 100) + + +def scene(protrude, deg=0.0, tip=(1100, 640), gap=136, hand=False): + """上カメラの合成の画. 爪の向き (画の真下から deg 度) に沿って, ブロックの先が爪先の印の中点から protrude px 先.""" + img = np.full((H, W, 3), BG, np.uint8) + a = np.radians(deg) + u = np.array([np.sin(a), np.cos(a)]) # 爪先へ向かう向き (deg=0 で画の真下) + v = np.array([-u[1], u[0]]) # 爪先の印 2 つを結ぶ向き + mid = np.array(tip, float) + if hand: # 肌を赤として拾う大きな塊 (爪の先まで 150px 出る. 印は隠さない) + cv2.rectangle(img, (int(mid[0]) - 110, int(mid[1]) - 100), (int(mid[0]) + 110, int(mid[1]) + 150), RED, -1) + for p in (mid + v * gap / 2, mid - v * gap / 2, mid - u * 260): # 爪先の印 2 つ・胴の印 + cv2.circle(img, tuple(int(round(x)) for x in p), 9, BLUE, -1) + # ブロック: 爪の向きに 60px・横に 60px の四角. 先の辺が mid + u * protrude + c = mid + u * (protrude - 30) + box = np.array([c + u * 30 + v * 30, c + u * 30 - v * 30, c - u * 30 - v * 30, c - u * 30 + v * 30]) + cv2.fillConvexPoly(img, np.round(box).astype(np.int32), RED) + return img + + +def write_video_run(run_dir, before_px, after_px, close_at=2.0, hand=False, blank=False, with_close=True): + """grasp_depth が読む記録 (states.csv の爪の命令・top.mp4・top_frame_log.csv) を作る. 10Hz で 4 秒.""" + rec = run_dir / "recording" + rec.mkdir(parents=True) + base = 1.8e9 + rows = ["ts,event_type,stage,j0,j1,j2,j3,j4,j5,speed,gripper_state", + f"{base + 0.2:.6f},gripper_cmd,,,,,,,,,0"] + if with_close: + rows.append(f"{base + close_at:.6f},gripper_cmd,,,,,,,,,1") + (rec / "states.csv").write_text("\n".join(rows) + "\n") + w = cv2.VideoWriter(str(rec / "top.mp4"), cv2.VideoWriter_fourcc(*"mp4v"), 10.0, (W, H)) + lines = ["frame_idx,capture_ts"] + for k, t in enumerate(np.arange(0, 4.0, 0.1)): + if blank: + img = np.full((H, W, 3), BG, np.uint8) + elif t < close_at + 0.5: + img = scene(before_px, hand=hand and t > close_at - 0.6) + else: + img = scene(after_px, gap=100) + w.write(img) + lines.append(f"{k},{base + t + 0.003:.6f}") + w.release() + (rec / "top_frame_log.csv").write_text("\n".join(lines) + "\n") + return base + + +class TestMeasureAxis(unittest.TestCase): + def test_straight_down(self): + for p in (-15, 0, 14, 24, 40, 75): + img = scene(p) + m = gd.measure_axis(img, gd.red_center(img)) + self.assertAlmostEqual(m["axis_protrude_px"], p, delta=1.0, msg=p) + self.assertEqual(m["axis_from"], "body") + self.assertAlmostEqual(m["axis_marker_gap_px"], 136, delta=1.0) + + def test_slanted_gripper(self): + """10/15〜10/19 の記録のように爪が斜め (−19〜−55°) を向いていても, 爪の向きに沿って測る.""" + for deg in (-20, -45): + img = scene(30, deg=deg, tip=(700, 600)) + c = gd.red_center(img) or gd.red_center_wide(img) + m = gd.measure_axis(img, c) + self.assertAlmostEqual(m["axis_protrude_px"], 30, delta=1.5, msg=deg) + self.assertAlmostEqual(m["axis_deg"], deg, delta=1.0) + + def test_no_marker(self): + img = np.full((H, W, 3), BG, np.uint8) + cv2.rectangle(img, (1070, 600), (1130, 660), RED, -1) + self.assertIsNone(gd.measure_axis(img, (1100, 630))) + + +class TestCloseTime(unittest.TestCase): + def test_first_close_and_label(self): + gt = np.array([1.0, 2.0, 2.05, 5.0, 7.0]) + gs = np.array([0.0, 1.0, 1.0, 0.0, 1.0]) + self.assertEqual(gd.close_time(gt, gs), 2.0) # 開いた状態から最初に閉じた命令 + self.assertEqual(gd.close_time(gt, gs, t_label=2.1), 2.05) # ラベルの時刻かその前のいちばん遅い命令 (G と同じ) + self.assertEqual(gd.close_time(gt, gs, t_label=7.0), 7.0) + self.assertIsNone(gd.close_time(gt, gs, t_label=1.5)) + self.assertIsNone(gd.close_time(np.array([1.0]), np.array([0.0]))) + + def test_label_time_of_episode(self): + t = np.arange(10) * 0.1 + 100.0 + g = np.array([0, 0, 0, 1, 1, 1, 0, 0, 1, 1]) + self.assertAlmostEqual(gd.first_close_label_time(t, g), 100.3) + self.assertIsNone(gd.first_close_label_time(t, np.zeros(10))) + + +class TestMeasureRun(unittest.TestCase): + def setUp(self): + self.td = Path(tempfile.mkdtemp()) + + def test_deep_and_shallow(self): + for name, after, shallow in (("deep", 10, False), ("shallow", 40, True)): + d = self.td / name + base = write_video_run(d, before_px=45, after_px=after) + m = gd.measure_run(d, max_protrude_px=24) + self.assertEqual(m["status"], "ok", m) + self.assertAlmostEqual(m["t_close"], base + 2.0, places=3) + self.assertEqual(m["frame_before"], 19) # 1.903 秒のコマ (閉じる命令の直前) + self.assertEqual(m["frame_after"], 30) # 3.003 秒のコマ (閉じる命令の 1.0 秒後かそのあと) + self.assertAlmostEqual(m["before_px"], 45, delta=3) # mp4 の圧縮で縁が少しにじむ + self.assertAlmostEqual(m["after_px"], after, delta=3) + self.assertAlmostEqual(m["after_gap_px"], 100, delta=3) + self.assertIs(m["shallow"], shallow) + self.assertFalse(m["hand_suspect"]) + drop, why, _ = gd.judge(d, 24) + self.assertIs(drop, shallow) + self.assertTrue(why.startswith("shallow_grasp") if shallow else why == "ok") + json.dumps(m) # index.json にそのまま書ける + + def test_hand(self): + d = self.td / "hand" + write_video_run(d, before_px=45, after_px=15, hand=True) + m = gd.measure_run(d, max_protrude_px=24) + self.assertEqual(m["status"], "ok", m) + self.assertTrue(m["hand_suspect"]) + self.assertIsNone(m["before_px"]) # 手を拾った閉じる直前の値は使わない + self.assertGreater(m["pre_px"]["before"], gd.HAND_PX) + self.assertAlmostEqual(m["after_px"], 15, delta=3) + self.assertFalse(m["shallow"]) + + def test_unmeasured(self): + d = self.td / "blank" + write_video_run(d, 0, 0, blank=True) + m = gd.measure_run(d, max_protrude_px=24) + self.assertEqual((m["status"], m["reason"]), ("unmeasured", "ブロックが見つからない")) + self.assertIsNone(m["shallow"]) + drop, why, _ = gd.judge(d, 24) + self.assertFalse(drop) # 測れなかった本は外さない + self.assertTrue(why.startswith("grasp_depth_unmeasured")) + d = self.td / "noclose" + write_video_run(d, 45, 10, with_close=False) + self.assertEqual(gd.measure_run(d)["reason"], "閉じる命令がない") + self.assertEqual(gd.measure_run(self.td / "nothing")["reason"], + "missing:states.csv,top.mp4,top_frame_log.csv") + + +def _same_episode(a, b): + za, zb = np.load(a / "episode.npz"), np.load(b / "episode.npz") + assert sorted(za.files) == sorted(zb.files) + for k in za.files: + np.testing.assert_array_equal(za[k], zb[k]) + assert json.loads((a / "meta.json").read_text()) == json.loads((b / "meta.json").read_text()) + fa = sorted(p.name for p in (a / "frames").glob("*.jpg")) + assert fa == sorted(p.name for p in (b / "frames").glob("*.jpg")) + for n in fa: + assert (a / "frames" / n).read_bytes() == (b / "frames" / n).read_bytes() + + +def _fake_measure(after_px, status="ok"): + calls = [] + + def f(run_dir, t_label=None, max_protrude_px=None): + calls.append(t_label) + m = {"status": status, "reason": None if status == "ok" else "ブロックが見つからない", + "after_px": after_px if status == "ok" else None, "hand_suspect": False, + "max_protrude_px": max_protrude_px} + m["shallow"] = (after_px > max_protrude_px) if status == "ok" else None + return m + return f, calls + + +class TestBuilders(unittest.TestCase): + """合成の記録 (test_pad_build.write_run. 8.0 秒に爪を閉じる) で, 学習データを作る 2 本の --max-protrude-px を見る.""" + + def setUp(self): + self.td = Path(tempfile.mkdtemp()) + self.run = self.td / "runs" / "pi05_20991107_000000" + tpb.write_run(self.run) + + def demo_args(self, mp=None, **kw): + a = argparse.Namespace(include_failed=False, force=True, trim_mid_idle=True, **kw) + if mp is not None: + a.max_protrude_px = mp + return a + + def corr_args(self, mp=None, **kw): + a = argparse.Namespace(force=True, no_trim_mid_idle=False, gap_fill="speed20", trim_hold=True, **kw) + if mp is not None: + a.max_protrude_px = mp + return a + + def check(self, build, make_args): + out = self.td / "ds" + # 1. 引数なし (属性がない今までの Namespace / None) は測らない. 出力は同じ + boom = mock.patch.object(gd, "measure_run", side_effect=AssertionError("測ってはいけない")) + with boom: + r0 = build(self.run, out / "a" / self.run.name, make_args()) + r1 = build(self.run, out / "b" / self.run.name, make_args(max_protrude_px=None)) + self.assertTrue(r0["used"], r0) + self.assertEqual(r0, r1) + self.assertNotIn("grasp_depth", r0) + _same_episode(out / "a" / self.run.name, out / "b" / self.run.name) + # 2. 線の内側 (深い) なら同じエピソードができ, 値が戻り値 (= index.json) に残る + f, calls = _fake_measure(10.0) + with mock.patch.object(gd, "measure_run", f): + r2 = build(self.run, out / "c" / self.run.name, make_args(24.0)) + self.assertTrue(r2["used"]) + self.assertEqual(r2["grasp_depth"]["after_px"], 10.0) + self.assertEqual({k: v for k, v in r2.items() if k != "grasp_depth"}, r0) + _same_episode(out / "a" / self.run.name, out / "c" / self.run.name) + # エピソードで爪が 1 になった最初のコマの時刻を渡している (8.0 秒の命令の次の 10Hz のコマ) + self.assertGreaterEqual(calls[0], tpb.BASE + 8.0) + self.assertLess(calls[0], tpb.BASE + 8.1 + 1e-6) + self.assertAlmostEqual(gd.close_time(*gd.load_gripper_cmds(self.run / "recording" / "states.csv"), + t_label=calls[0]), tpb.BASE + 8.0, places=3) + # 3. 線を超えたら外す. 値と理由が残る. エピソードは書かない + f, _ = _fake_measure(40.0) + with mock.patch.object(gd, "measure_run", f): + r3 = build(self.run, out / "d" / self.run.name, make_args(24.0)) + self.assertFalse(r3["used"]) + self.assertEqual(r3["reason"], "shallow_grasp after_px=40.0 > 24") + self.assertEqual(r3["grasp_depth"]["after_px"], 40.0) + self.assertFalse((out / "d" / self.run.name).exists()) + # 4. 測れなかった本は外さず, 測れなかったと残す + f, _ = _fake_measure(None, status="unmeasured") + with mock.patch.object(gd, "measure_run", f): + r4 = build(self.run, out / "e" / self.run.name, make_args(24.0)) + self.assertTrue(r4["used"]) + self.assertEqual(r4["grasp_depth"]["status"], "unmeasured") + _same_episode(out / "a" / self.run.name, out / "e" / self.run.name) + # 5. 本物の部品で測る (合成の画は灰色だけなので「ブロックが見つからない」→ 外さない) + r5 = build(self.run, out / "f" / self.run.name, make_args(24.0)) + self.assertTrue(r5["used"]) + self.assertEqual(r5["grasp_depth"]["reason"], "ブロックが見つからない") + + def test_demo(self): + self.check(bde.build_one, self.demo_args) + + def test_correction(self): + self.check(bce.build_one, self.corr_args) + + +class TestMergeExclude(unittest.TestCase): + """merge_raw_episodes.py --exclude-json: 外した本は写さず, index.json と README に理由を残す. 付けなければ今までと同じ.""" + + def setUp(self): + self.root = Path(tempfile.mkdtemp()) + ds = self.root / "datasets" + for src, names in (("A", ["run_1", "run_2"]), ("B", ["pi05_1", "pi05_2"])): + for n in names: + ep = ds / src / "raw_episodes" / n + (ep / "frames").mkdir(parents=True) + st = np.zeros((3, 7), np.float32) + np.savez_compressed(ep / "episode.npz", t=np.arange(3) / 10.0, state=st, actions=st) + cv2.imwrite(str(ep / "frames" / "000000.jpg"), np.zeros((216, 384, 3), np.uint8)) + for k in (1, 2): + cv2.imwrite(str(ep / "frames" / f"{k:06d}.jpg"), np.zeros((216, 384, 3), np.uint8)) + (ep / "meta.json").write_text(json.dumps({ + "steps": 3, "fps": 10, "image_size": [384, 216], "state_dim": 7, "task": "t", + "state_names": list("abcdefg"), "correct_steps": 1 if src == "B" else 0})) + + def merge(self, name, *extra): + import subprocess + r = subprocess.run([sys.executable, str(_HERE / "merge_raw_episodes.py"), "--src", "A", "--src", "B", + "--dataset", name, "--data-root", str(self.root), *extra], + capture_output=True, text=True) + return r, self.root / "datasets" / name + + def test_exclude(self): + r, ds = self.merge("plain") + self.assertEqual(r.returncode, 0, r.stderr) + plain = json.loads((ds / "index.json").read_text()) + self.assertNotIn("excluded", plain) + self.assertNotIn("外した本", (ds / "README.md").read_text()) + ex = self.root / "ex.json" + ex.write_text(json.dumps({"run_2": {"reason": "浅い", "after_px": 30.0}, + "pi05_1": {"reason": "指", "after_px": 10.0}}, ensure_ascii=False)) + note = self.root / "note.md" + note.write_text("## 外した理由\n\nG を見る\n") + r, ds = self.merge("deep", "--exclude-json", str(ex), "--readme-note", str(note)) + self.assertEqual(r.returncode, 0, r.stderr) + idx = json.loads((ds / "index.json").read_text()) + self.assertEqual(sorted(idx["episodes"]), ["pi05_2", "run_1"]) + self.assertEqual(sorted(p.name for p in (ds / "raw_episodes").iterdir()), ["pi05_2", "run_1"]) + self.assertEqual(idx["total_episodes"], 2) + self.assertEqual(idx["excluded"]["run_2"], {"reason": "浅い", "after_px": 30.0, "from_dataset": "A", + "steps": 3, "correct_steps": 0}) + self.assertEqual(idx["sources"]["B"], {"episodes": 1, "steps": 3, "correct_steps": 1}) + txt = (ds / "README.md").read_text() + self.assertIn("| run_2 | A | 3 | 浅い |", txt) + self.assertIn("G を見る", txt) + self.assertTrue((ds / "code" / "ex.json").exists()) + # 外していない本は同じ形で入っている (plain と同じ) + self.assertEqual(idx["episodes"]["run_1"], plain["episodes"]["run_1"]) + + def test_bad_exclude(self): + ex = self.root / "bad.json" + ex.write_text(json.dumps({"run_9": {"reason": "x"}})) + r, ds = self.merge("bad", "--exclude-json", str(ex)) + self.assertNotEqual(r.returncode, 0) + self.assertIn("元のどれにもない", r.stderr) + self.assertFalse(ds.exists()) + ex.write_text(json.dumps({"run_1": {"after_px": 1}})) + r, ds = self.merge("bad2", "--exclude-json", str(ex)) + self.assertIn("理由 (reason) がない", r.stderr) + self.assertFalse(ds.exists()) + + +if __name__ == "__main__": + unittest.main() `````` ## 置き場所ごとに10本ずつそろえる手順と、本数を数える表示（依頼118） 外した版の21本を置き場所ごとに数えると、位置1が0本、位置2が3本、位置3が2本、位置4が5本、位置5（π0.5 を試している所）が11本でした。半分以上が位置5です。 SO-101 で π0.5 に靴下とゴミを取り分けさせた例（LOG443）では、靴下80本・ゴミ50本だと多いほうに寄ってしまい、各100本にそろえて直していました。同じ考え方で、5か所を10本ずつにそろえることにしました。足すのは約30本で、教えた記録を再生して撮るので、人がやるのはブロックを置き直すことだけです。各位置の1本目を撮ったら深さを測り、浅ければ撮り続けずに教え直します。 依頼118で、手順書（pi05/DEEP_DEMOS.md）をこの形に直し、置き場所ごとの本数を数える表示（pi05/count_by_position.py）を足しました（21:59〜22:10）。教えた動きは教えた記録のファイル名から、補正入りは上カメラで見つけたブロックの位置から、5か所のどれかに割り当てます。外した版の21本で数え直すと、0・3・2・5・11本で、手で数えた値と同じでした。 | 位置 | 今の本数 | 教えた動き | 補正入り | 外した本 | 10本にそろえるには | |---|---|---|---|---|---| | 1 | 0 | 0 | 0 | 浅い 1 | ＋10 | | 2 | 3 | 3 | 0 | 指 2 | ＋7 | | 3 | 2 | 2 | 0 | 浅い 2 | ＋8 | | 4 | 5 | 5 | 0 | 0 | ＋5 | | 5 | 11 | 4 | 7 | 浅い 4 | ＋0 | - ねらい：π0.5 が半分以上を位置5で覚えている偏りをなくす - 変わったこと：撮り足す本数が位置ごとに決まり、撮るたびに表で確かめられるようになった。データセットを作り終えると、この表が README に自動で入る - 見込める効果：どの位置でも同じだけ深いつかみを見ることになり、位置5以外での失敗が減る見込み。確かめるのは、学び直したあとの評価用の位置での成功率 ### 依頼118：深いお手本の手順書を置き場所ごとに本数をそろえる形に直し、位置ごとの本数を数える表示を足す ``````markdown --- 作業フォルダ: /Users/shimazueiichi/r2r2r-scripts 上限時間: 90 --- # 深いお手本の手順書を、置き場所ごとに本数をそろえる形に直し、位置ごとの本数を数える表示を足す（依頼117 の続き・2026-11-07） **実機には触れない。** Raspberry Pi・daemon・SSH・シリアルは使わない。学習も回さない。KIOXIA の `datasets/`・`runs/` は読むだけ。 ## 背景 依頼117 で作った `mecharm_redblock_box_teach_v1_trim2_corr_v5_deep`（21本）を置き場所ごとに数えると、位置1 が 0本・位置2 が 3本・位置3 が 2本・位置4 が 5本・位置5 が 11本（教えた動き 4・補正入り 7）で、半分以上が位置5 に寄っている。SO-101 で π0.5 を学習した例（LOG443）では、タスクごとの本数の偏りがそのまま失敗の形になり、本数をそろえて直していた。 エイイチの決め（2026-11-07）：**置き場所ごとの本数をそろえる。** 目安は各位置 10本（位置1〜5。LOG352 の教えた5か所）。 ## A. 手順書 `pi05/DEEP_DEMOS.md` を直す - 目的を「位置5に足す」から「位置1〜5 を各10本にそろえる」に改める。今の本数と足す本数の表を載せる（位置1 ＋10・位置2 ＋7・位置3 ＋8・位置4 ＋5・位置5 ＋0。B の表示で出る値に合わせる） - 各位置の教えた記録（LOG352 の README の表：位置1 `drag_v2_20221013_144348.json`・位置2 `230205`・位置3 `231249`・位置4 `232345`・位置5 `233323`）を再生して撮る手順を、位置ごとに書く - **各位置の1本目を撮ったら、`pi05/grasp_depth.py` で深さを測り、24px を超えたら撮り続けずに止める**流れを入れる。位置1 は前の1本（183454）が 38.9px で浅かったので、教えた記録そのものが浅い可能性があると書き、浅ければ深くつかむように教え直す手順（肩 j1 は約 88° まで・肩を倒さず肘と手首で下げる、LOG352 の分かったこと）に進む - ブロックを教えた位置からずらして置かない注意（LOG352 で 46mm ずれた例）。マットに印を付けておく - 撮り終えたら、B の表示で位置ごとの本数を見て、浅くて外れた分を撮り足して各10本にそろえる流れ - 1本あたりの目安時間と、全体の目安（約30本・2時間前後） ## B. 位置ごとの本数を数える表示を足す - 新しい小さなスクリプト（例 `pi05/count_by_position.py`）。入力はデータセット名（複数可）または raw_episodes のフォルダ。出力は位置1〜5 と「分からない」ごとの本数（教えた動き・補正入りの内訳つき）、目標本数（既定 10）との差、浅くて外れた本・手が写っていそうな本の数 - 位置の決め方：教えた動きは、試行フォルダの `teach/` にある教えた記録のファイル名（LOG352 の表と同じ対応）から決める。補正入り（π0.5 の推論）は、上カメラの最初のほうのコマで見つけたブロックの位置（依頼107 の `twin_prep` や `grasp_depth.py` の見つけ方を流用してよい）を、教えた5か所の位置のいちばん近いものに割り当て、離れすぎていれば「分からない」にする。決め方と距離の線を README とスクリプトの頭に書く - 依頼117 の21本で、位置2 が 3・位置3 が 2・位置4 が 5・位置5 が 11（位置1 は 0）になるか確かめる。ならなければ、どの本がどう違うかを報告する - `merge_raw_episodes.py` でデータセットを作り終えたときに、この表を README に足す（引数で切れるようにし、引数なしでも作るデータは今までと同じにする） - テストを足す ## 守ること - git は変えたファイルを1つずつ指定して add・commit する。`git add .`・`git clean` は使わない - 今あるデータセットは上書き・移動・削除しない ## 報告 A の手順書の置き場所と要点、B の21本での位置ごとの数（確かめた結果）だけを書く。最後の1行に「判定：合格／止めた（決めてほしいことあり）／失敗」を書く。 `````` ### 依頼118の報告 ``````markdown # 結果：20261107_118_深いお手本の手順書を置き場所ごとに本数をそろえる形にし位置ごとの本数を数える表示を足す - 状態：完了 - 始めた時刻：2026-11-07 21:59 - 終わった時刻：2026-11-07 22:10（11分） - 作業フォルダ：/Users/shimazueiichi/r2r2r-scripts - この続きを Claude Code で話すとき：作業フォルダで `claude --resume 55a5cbd2-d866-4185-ba70-dfa15fa46906` ## Claude Code の報告 **やったこと** A. 手順書 `pi05/DEEP_DEMOS.md` を直しました。目的を「位置1〜5 を各10本にそろえる」に改めました。要点は次のとおりです。 - 今の本数と足す本数の表を載せました。位置1 ＋10・位置2 ＋7・位置3 ＋8・位置4 ＋5・位置5 ＋0 で、合わせて ＋30 本です。B の表示で出る値と同じです。 - 位置ごとに、教えた記録（LOG352 の表の5つ）を再生して撮る手順を書きました。前に撮った本の深さも位置ごとに添えました。 - 各位置の1本目を撮ったら `pi05/grasp_depth.py` で深さを測ります。閉じて 1.0 秒後のはみ出しが 24px を超えたら、その位置は撮り続けずに止めます。 - 位置1 は前の1本（183454）が 38.9px で浅かったため、教えた記録そのものが浅い可能性があると書きました。浅ければ教え直す手順に進みます（肩 j1 は約 88° まで、肩を倒さず肘と手首で下げる）。 - ブロックを教えた位置からずらして置かない注意を入れました（46mm ずらした 193729 の例）。マットの印は、LOG352 で撮った試行の上カメラの1コマめで合わせます。再生直前に撮った画 `prerun_*.jpg` には、ブロックが箱の中に写っている回があるので、目安に使わないよう書きました。 - 撮り終えたら B の表示で位置ごとの本数を見て、浅くて外れた分を撮り足す流れを書きました。 - 目安時間は 1本あたり約3分で、全体で約30本・2時間前後です。 B. 位置ごとの本数を数える `pi05/count_by_position.py` を足しました。 - 教えた動きは、試行フォルダの `teach/` にある教えた記録のファイル名で位置を決めます。 - 補正入りは、上カメラで見つけたブロックの位置を、教えた5か所のいちばん近いものに割り当てます。30mm を超えて離れていたら「分からない」にします。 - 教えた5か所の基準の位置は、同じ見つけ方を LOG352 の19本にかけた値です。ただし位置1 の基準は 183454 の1本だけから取った値で、ブロックが爪の陰で小さく写っています。 - `merge_raw_episodes.py` は、作り終えたらこの表を README に足します。`--no-position-table` で足さないようにできます。raw_episodes と index.json は、表があってもなくても同じです。 **結果** 依頼117 の21本を数えた結果は、依頼の値と同じでした。 | 位置 | 本数 | 教えた動き | 補正入り | 外した本 | | --- | --- | --- | --- | --- | | 1 | 0 | 0 | 0 | 浅い 1 | | 2 | 3 | 3 | 0 | 指 2 | | 3 | 2 | 2 | 0 | 浅い 2 | | 4 | 5 | 5 | 0 | 0 | | 5 | 11 | 4 | 7 | 浅い 4 | | 分からない | 0 | 0 | 0 | 0 | - 補正入り7本は、どれも位置5 の基準から 0.5〜16.3mm の所にありました。 - 46mm ずらして置いた 193729 は、位置4 から 48mm 離れていて「分からない」になります。 - テストは、新しく足した7件・merge の27件・深さの12件がすべて通りました。 - 実機・Pi・daemon には触れていません。今あるデータセットも変えていません。 **変えたファイル** - `pi05/DEEP_DEMOS.md` - `pi05/merge_raw_episodes.py` - `pi05/count_by_position.py`（新規） - `pi05/test_count_by_position.py`（新規） - `logs/20261107_2210.md`（新規・作業の記録） コミットは df54374 と 1b66c05 です。 **決めてほしいこと** - 位置1 を教え直した場合、新しい教えた記録のファイル名はまだ表にないため、そのままでは「分からない」に数えられます。位置1 として `count_by_position.py` の表（`TEACH_FILES`）に1行足すかを決めてください。 判定：止めた（決めてほしいことあり） `````` ### r2r2r-scripts commit df54374（DEEP_DEMOS.md・count_by_position.py・merge_raw_episodes.py） ``````diff commit df54374b757ab7b5c568604362a4f2fd2c15c4a7 Author: Eiichi Shimazu Date: Sat Nov 7 22:10:26 2026 +0900 深いお手本の手順書 pi05/DEEP_DEMOS.md を位置 1〜5 を各 10 本にそろえる形に直し (今の本数と足す本数・位置ごとの撮り方・1 本目で深さを測り 24px 超で止める・位置 1 の教え直し・印・目安時間), 置き場所ごとの本数を数える pi05/count_by_position.py を足す (教えた動きは teach/ のファイル名, 補正入りは上カメラのブロックの位置をいちばん近い教えた位置へ. 30mm 超は分からない). merge_raw_episodes.py は作り終えたら表を README に足す (--no-position-table で足さない. データは同じ). 21 本で 0・3・2・5・11 を確かめた (依頼 118) Co-Authored-By: Claude Opus 5.5 diff --git a/pi05/DEEP_DEMOS.md b/pi05/DEEP_DEMOS.md index 416fda7..7db6663 100644 --- a/pi05/DEEP_DEMOS.md +++ b/pi05/DEEP_DEMOS.md @@ -1,38 +1,107 @@ -# 深いお手本を足す（試している置き場所・教えた位置 5。2026-11-07 依頼 117） +# 深いお手本で置き場所ごとの本数をそろえる（位置 1〜5 を各 10 本。2026-11-07 依頼 117・118） -学習（`RUNPOD_TRAIN.md`）の前に、π0.5 を試している置き場所（ブロックが画の中央・PICK の j0 +13°）で、ブロックが爪の奥まで入ってから閉じるお手本を 5〜10 本足す手順。今のデータは `mecharm_redblock_box_teach_v1_trim2_corr_v5_deep`（21 本。浅い 7 本・指の写った 2 本を外した版）。**この手順はまだ実機で通していない**（書いただけ）。 +学習（`RUNPOD_TRAIN.md`）の前に、LOG352 で教えた 5 か所（位置 1〜5）それぞれで、ブロックが爪の奥まで入ってから閉じるお手本を撮り足し、**どの位置も 10 本**にそろえる手順。**この手順はまだ実機で通していない**（書いただけ）。 -**1. 使うスクリプト**: 今の `dynamic_tracking_limits/drag_teaching/record_teach_demo.py` で、LOG352 の位置 5 の教えた記録 `drag_v2_20221013_233323.json` を再生する（新しく教え直さないので、Pi の `drag_teach_v2.py teach` は飛ばす）。10/19 の `run_0001_20261019_194818`〜`195120` の 4 本はこの形で撮ったもので、閉じて 1.0 秒後のはみ出しは 8〜15px（深い）。 +**なぜそろえるか**: 今のデータ `mecharm_redblock_box_teach_v1_trim2_corr_v5_deep`（21 本。浅い 7 本・指の写った 2 本を外した版）は半分以上が位置 5 に寄っている。SO-101 で π0.5 を学習した例（LOG443）では、タスクごとの本数の偏りがそのまま失敗の形になり、本数をそろえて直していた。エイイチの決め（2026-11-07）: 置き場所ごとの本数をそろえる。目安は各位置 10 本。 + +## 0. 今の本数と足す本数 + +`python3 pi05/count_by_position.py mecharm_redblock_box_teach_v1_trim2_corr_v5_deep` の表（2026-11-07）: + +| 位置 | 教えた記録 | 今の本数 | うち教えた動き | うち補正入り | 足す本数 | 外した本（浅い・指） | +| --- | --- | --- | --- | --- | --- | --- | +| 1 | `drag_v2_20221013_144348.json` | 0 | 0 | 0 | **＋10** | 浅い 1（183454） | +| 2 | `drag_v2_20221013_230205.json` | 3 | 3 | 0 | **＋7** | 指 2（191347・191441） | +| 3 | `drag_v2_20221013_231249.json` | 2 | 2 | 0 | **＋8** | 浅い 2（192343・192633） | +| 4 | `drag_v2_20221013_232345.json` | 5 | 5 | 0 | **＋5** | 0 | +| 5 | `drag_v2_20221013_233323.json` | 11 | 4 | 7 | **＋0** | 浅い 4（補正入り） | +| 合計 | | 21 | 14 | 7 | **＋30** | 7 | + +- 位置の決め方（`count_by_position.py` の頭にも書いた）: 教えた動きは試行フォルダの `teach/` にある教えた記録のファイル名。補正入りは上カメラで見つけたブロックの位置（マットの上の mm）をいちばん近い教えた位置に割り当て、30mm を超えたら「分からない」 +- 位置 5 は足さない（今の 11 本で目安を超えている）。位置 5 の補正入りを減らすかどうかは決めていない + +**目安時間**: 1 本あたり約 3 分（置き直し・measure・再生 約 1 分・y/n・深さを測る。10/19 の位置 4 の 5 本は 1〜2 分おきに撮れていた）。位置を替えるときに印の確認と 1 本目の確かめで 5〜10 分。**全体で約 30 本・2 時間前後**（30 本 × 3 分 ＋ 位置の替え 4 回）（浅くて撮り直す分と、位置 1 を教え直す場合はその分だけ延びる）。 + +## 1. 撮る前に（全部の位置で 1 回） + +- **ブロックを教えた位置からずらして置かない。** 再生するのは教えたときの関節の角度そのもので、ブロックの位置を見て動きを変えない。LOG352 では位置 4 から約 46mm ずらして置いた 193729 が使えなかった（`count_by_position.py` の見つけ方でも位置 4 から 48mm 離れて「分からない」になる） +- **マットに 5 か所の印を付けておく**（ブロックの四隅が分かる小さな印。上カメラに大きく写る色・赤は使わない）。印の場所は LOG352 で採った試行の上カメラの最初のコマ（HOME で、ブロックが置かれた画）で合わせる: `/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_{183454（位置 1）・191541（位置 2）・192438（位置 3）・193459（位置 4）・194913（位置 5）}/recording/top.mp4` の 1 コマめ（`teach_poses_LOG352/prerun_*.jpg` はブロックが箱の中に写っている回があり、目安にならない）。`count_by_position.py` の `TEACH_REF_MM`（同じ見つけ方での 10/19 のブロックの中心）も目安になる: 位置 1 (182.7, 188.8)・位置 2 (215.5, 242.2)・位置 3 (123.6, 235.6)・位置 4 (240.8, 101.3)・位置 5 (−48.8, 246.4) mm +- `measure` の `block_xz` は根元近くの赤いケーブルを拾うことがあり当てにならない（LOG352 の分かったこと）。置けたかは印と画で人が見る +- 閉じる前に人がブロックを押し込まない（指が学習の画に写る。191347・191441 で起きて外した）。`run --execute` は Enter のあとは止まらないので、浅いと思ったら Enter の前に置き直すか、撮ったあとで外す ``` -# ブロックを位置 5 に置く（10/19 の measure は block_xz (+45.7, −28.5)。teach_poses_LOG352/measure_20261019_194751.json） python3 dynamic_tracking_limits/place/place_modules/recorder.py --top --no-side # 別のターミナルで常駐 -python3 dynamic_tracking_limits/drag_teaching/record_teach_demo.py measure # 1 本ごとに置き直したら測り直す (保存先 teach_poses_{日付}/) -python3 dynamic_tracking_limits/drag_teaching/record_teach_demo.py run --teach drag_v2_20221013_233323.json # ドライラン（計画だけ） -python3 dynamic_tracking_limits/drag_teaching/record_teach_demo.py run --teach drag_v2_20221013_233323.json --execute # 再生＋記録 ``` -- **同じ動きの繰り返しになる**: 再生するのは教えたときの関節の角度そのもので、ブロックの位置を見て動きを変えない。5〜10 本とも同じ PICK の姿勢（`pose_used.pick_used` = [13.18, 60.02, 19.51, −2.46, −61.87, −26.71]）で閉じる。画の違いは、人が置いたブロックの数 mm のずれと光だけになる。ブロックを毎回同じ所に置くほど深くつかめるが、学習には同じ画と同じ動きが増える（check_dataset の 5. で j0 +15° 付近が今でも 21 本中 11 本・52%。10 本足すと 31 本中 21 本・68%） -- `run` が確かめるのは「measure のあとでブロックが動いていないか」（5mm）だけで、教えたときの位置と同じかは見ない。位置 5 に置けたかは、measure の block_xz を 10/19 の値と人が見比べる。10/19 の 4 本はどれも再生直前の検出で WARN（ブロックを見つけられない・赤いケーブルを拾う）が出ていたが、完走している +## 2. 位置ごとに撮る（位置 1 → 2 → 3 → 4 の順。位置 5 は撮らない） + +どの位置も同じ流れ。`{教えた記録}` を位置の表のファイル名に替える。 + +``` +# ブロックを位置 N の印に置く +python3 dynamic_tracking_limits/drag_teaching/record_teach_demo.py measure # 1 本ごとに置き直したら測り直す +python3 dynamic_tracking_limits/drag_teaching/record_teach_demo.py run --teach {教えた記録} # ドライラン（計画だけ） +python3 dynamic_tracking_limits/drag_teaching/record_teach_demo.py run --teach {教えた記録} --execute # 再生＋記録 +``` -**2. 閉じる前に奥まで入っているかを確かめる所**: **今の作りにはない。** `run --execute` は、始める前の「周囲に手・物が無いか確認して Enter」のあとは止まらない。PICK に着くと、止まったのを最大 8 秒待ってから（止まらなくても WARN だけで）そのまま閉じる。人が見られるのは、Enter の前の画（ブロックの置き方）と、終わったあとの y/n と動画だけ。閉じる前に人がブロックを押し込むと、指が学習の画に写る（191347・191441 で起きた。外した）。浅いと思ったら Enter の前に置き直すか、撮ったあとで外す。 +| 位置 | `{教えた記録}` | 撮る本数（外れなければ） | 前に撮った本の深さ（閉じて 1.0 秒後。依頼 113 G の g_table.csv） | +| --- | --- | --- | --- | +| 1 | `drag_v2_20221013_144348.json` | 10 | 183454 が 38.9px（浅い）。**教えた記録そのものが浅い可能性がある**（下の 2-1） | +| 2 | `drag_v2_20221013_230205.json` | 7 | 残っている 3 本は 8.1・15.9・18.2px（深い） | +| 3 | `drag_v2_20221013_231249.json` | 8 | 4 本中 2 本が 27.6・28.9px（浅い）、残り 2 本は 12.7・17.9px。線のすぐ上なので置き方で変わる可能性 | +| 4 | `drag_v2_20221013_232345.json` | 5 | 5 本とも 24px 以下（4.4〜22.1px。21.6・22.1px は線に近い） | -**3. 撮ったあとに深さを測って、線を超えた本を外す** +### 2-0. 各位置の 1 本目を撮ったら、深さを測ってから続ける ``` -# 1 本ずつの深さを見る（読むだけ） python3 pi05/grasp_depth.py /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_YYYYMMDD_HHMMSS -# 新しい本だけのデータセットを作る（新しい名前。線を超えた本は外し、index.json の grasp_depth に値と理由を残す） -python3 pi05/build_demo_episodes.py --dataset mecharm_redblock_box_teach_pos5_deep_v1 --trim-mid-idle \ +``` + +- **閉じて 1.0 秒後のはみ出しが 24px（約 9mm）を超えたら、その位置は撮り続けずに止める。** 置き方を印に合わせ直して 1 本だけ撮り直し、それでも 24px を超えたら、教えた記録が浅いとみて 2-1 に進む（その位置の残りは撮らない） +- 24px 以下なら、その位置の残りを撮る。2 本目からも 1 本ずつ測ってよい（`grasp_depth.py` は何本でも渡せる） +- `[測れない]` が出たら（ブロックが見つからない・爪先の印が見えない）、`top.mp4` の閉じる前後を人が見て決める。測れなかった本は学習データを作るときも外されない +- `手が写っていそう` と出たら、その本は使わない方向で画を見る + +### 2-1. 教えた記録が浅いとき（位置 1 で起きそう）: 深くつかむように教え直す + +位置 1 の前の 1 本（183454）は 38.9px で浅かった。置き方ではなく、教えた PICK の姿勢が浅い可能性がある。教え直しはエイイチが手で行う。 + +1. ブロックを位置 1 の印に置く → 【Mac】`record_teach_demo.py measure` +2. 【Pi】daemon 停止 → `drag_teach_v2.py teach` → daemon 起動（`record_teach_demo.py` の頭の「1 本のお手本を作る流れ」と同じ） +3. 教えるときの決まり（LOG352 の分かったこと）: + - **肩 j1（0 始まり）は約 88° まで。** 88° を超えた命令はアームが丸ごと捨てて降りない（LOG352 で肩 +95.53〜+99.22° の 4 つが動かなかった。手で教えるときはモーターの力が抜けているので超えて曲げられてしまう） + - **肩を倒さず、肘と手首で下げる。** 本線は同じ場所を肩 88〜89°・肘 −33°・手首 −52° 前後でつかんでいる + - 爪の奥（爪先の印より手前）までブロックが入ってから閉じる +4. 新しい教えた記録で 1 本撮り（ドライランで PICK の肩が 88° 以下かを見てから `--execute`）、2-0 で深さを測る。24px 以下になったら残りを撮る +5. **新しい教えた記録のファイル名は `count_by_position.py` の `TEACH_FILES` にまだない**（そのままだと「分からない」に数えられる）。位置 1 として足すかはエイイチが決め、足すなら `TEACH_FILES` に 1 行足す + +## 3. 撮り終えたら: データにして、位置ごとの本数を見て、足りない分を撮り足す + +``` +# 1 本ずつの深さ（読むだけ） +python3 pi05/grasp_depth.py /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_YYYYMMDD_HHMMSS ... +# 新しい本だけのデータセットを作る（新しい名前。24px を超えた本は外し、index.json の grasp_depth に値と理由を残す） +python3 pi05/build_demo_episodes.py --dataset mecharm_redblock_box_teach_pos1to4_deep_v1 --trim-mid-idle \ --max-protrude-px 24 --run run_0001_YYYYMMDD_HHMMSS --run ... +# 位置ごとの本数を見る（今の deep と新しい本を合わせて数える。読むだけ） +python3 pi05/count_by_position.py mecharm_redblock_box_teach_v1_trim2_corr_v5_deep mecharm_redblock_box_teach_pos1to4_deep_v1 +``` + +- `build_demo_episodes.py` の最後に `[DEPTH]` の行が出る: 浅くて外した本・**測れなかった本（外していない）**・**手が写っていそうな本（外していない）**の数と名前。測れなかった本と手の本は `top.mp4` の閉じる前後を人が見て、外すなら merge の `--exclude-json` に書く +- `count_by_position.py` の表の「目標 10 本まで」が ＋ の位置だけ、2. に戻って撮り足す（浅くて外れた分）。全部の位置が ＋0 になったら次へ +- 「分からない」が出たら、表の下の理由を見る（teach/ がない・教え直した記録が `TEACH_FILES` にない・補正入りのブロックが教えた位置から 30mm を超える など） + +``` # 今の deep とまとめる（元は変えない。外す本が増えたら --exclude-json に理由と一緒に書く） python3 pi05/merge_raw_episodes.py --src mecharm_redblock_box_teach_v1_trim2_corr_v5_deep \ - --src mecharm_redblock_box_teach_pos5_deep_v1 --dataset <新しい名前> --tar + --src mecharm_redblock_box_teach_pos1to4_deep_v1 --dataset <新しい名前> --tar python3 pi05/check_dataset.py /Volumes/KIOXIA/r2r2r-data/datasets/<新しい名前>/raw_episodes ``` -- `build_demo_episodes.py` の最後に `[DEPTH]` の行が出る: 浅くて外した本・**測れなかった本（外していない）**・**手が写っていそうな本（外していない）** の数と名前。測れなかった本と手の本は、`top.mp4` の閉じる前後を人が見て、外すなら merge の `--exclude-json` に書く -- 線は 24px（閉じて 1.0 秒後のはみ出し。約 9mm）。値の意味は `lab/twin/20261107_失敗の再生と置き場所/G_お手本の閉じる時刻/README.md` +- `merge_raw_episodes.py` は作り終えたときに、位置ごとの本数の表を README の最後に足す（依頼 118。`--no-position-table` で足さない。raw_episodes と index.json は表があってもなくても同じ）。目標は `--target`（既定 10） +- 線 24px の意味は `/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/G_お手本の閉じる時刻/README.md` - 補正入りの run（`run_policy_client.py`）を足すときも同じで、`build_correction_episodes.py ... --max-protrude-px 24` を付ける -**4. 爪の電流でつかめたかを見る（依頼 116）**: 依頼 116 で電流を残すようになったのは実機推論の記録（`pi05/policy_recorder.py` → `{run}/recording/currents.csv`。`run_policy_client.py` で動かしたとき）だけ。**`record_teach_demo.py` の再生の記録（`recorder.py`）には電流は残らない**ので、この手順の教えた動きでは電流で見られない。補正入りの run を足すときは、`column -s, -t < {run}/recording/currents.csv | less` で閉じたあとの `gripper_current` を見る（依頼 116 の確かめ方では、つかむと数百〜990、放すと −50 前後になるはず。まだ実機で確かめていない。daemon に read_servo を置いていなければ `top_meta.json` の `currents_mode` が `get_angles_fallback` で、電流は空）。 +## 4. 爪の電流でつかめたかを見る（依頼 116） + +依頼 116 で電流を残すようになったのは実機推論の記録（`pi05/policy_recorder.py` → `{run}/recording/currents.csv`。`run_policy_client.py` で動かしたとき）だけ。**`record_teach_demo.py` の再生の記録（`recorder.py`）には電流は残らない**ので、この手順の教えた動きでは電流で見られない。補正入りの run を足すときは、`column -s, -t < {run}/recording/currents.csv | less` で閉じたあとの `gripper_current` を見る（依頼 116 の確かめ方では、つかむと数百〜990、放すと −50 前後になるはず。まだ実機で確かめていない。daemon に read_servo を置いていなければ `top_meta.json` の `currents_mode` が `get_angles_fallback` で、電流は空）。 diff --git a/pi05/count_by_position.py b/pi05/count_by_position.py new file mode 100644 index 0000000..08c9299 --- /dev/null +++ b/pi05/count_by_position.py @@ -0,0 +1,378 @@ +#!/usr/bin/env python3 +"""count_by_position.py — 学習データの本数を, ブロックの置き場所 (LOG352 の教えた位置 1〜5) ごとに数える (2026-11-07 依頼 118). + +エイイチの決め (2026-11-07): 置き場所ごとの本数をそろえる. 目安は各位置 10 本. +撮り足す本数を決めるときと, merge_raw_episodes.py でデータセットを作り終えたときの README の表に使う. + +入力 (どちらでも. いくつでも): + データセット名 datasets/{名前}/raw_episodes を数える + raw_episodes のフォルダ そのフォルダを数える (ひとつ上の index.json があれば外した本も数える) + +位置の決め方: + 教えた動き (run_pick_place の run): + 試行フォルダの teach/ にある教えた記録のファイル名で決める (LOG352 の README の表と同じ対応. TEACH_FILES). + teach/ がない・表にないファイル名なら「分からない」. + 補正入り (pi05_policy の run. meta.json の source が pi05_policy で始まる): + 上カメラ (recording/top.mp4) の最初のコマでマットの四隅を見つけ, マットの上の mm に直す対応を作る + (依頼 107 twin_prep と同じ. visual_servo_pick の四隅 (±310, −89)・(±310, 361) mm). + 爪を閉じる命令 (いちばん初め) より前のコマを 3 つおきに見て, ブロックらしい赤 (twin_prep.red_candidates と同じ. + 根元から 120mm 以内・細長い・埋まりの少ない赤は外す) がいちばん大きく写ったコマの中心を, ブロックの位置とする + (HOME の最初のコマでは爪の下に半分隠れていることがあるため). + その位置から TEACH_REF_MM のいちばん近い位置を選び, 離れが MAX_DIST_MM (30mm) を超えたら「分からない」. + 四隅・ブロックが見つからないときも「分からない」(理由を残す). + TEACH_REF_MM: 同じ見つけ方を LOG352 で採った 19 本 (10/19) にかけた値の位置ごとの中央値. + 位置 1 は 183454 の 1 本だけ (爪の陰で赤が小さく写る. 866px). 位置 2 は 191257 が見つからず 4 本, + 位置 3 は 192539 が見つからず 3 本. 位置 4 から 46mm ずらして置いた 193729 は (237.4, 52.6) で, 位置 4 から 48mm + (使わない試行. この線なら「分からない」になる). + 教えた位置どうしのいちばん近い間は位置 1〜2 の 63mm. 線 30mm はその半分より少し下. + 座標はマットの上の mm (visual_servo_pick の向き). 依頼 107 C の Unity の値 (T1〜T5) とは原点・向きが違う. + +外した本・手の本 (データセットの index.json があるとき): + merge_raw_episodes.py の index.json の excluded / build_*_episodes.py の index.json の used=false の本を, + 同じ決め方で位置に割り当て, 「浅くて外した」(grasp_depth.shallow か理由が「浅い」「shallow」で始まる) と + 「手で外した」(hand_in_frame か grasp_depth.hand_suspect) を数える. + データに入っている本の「手が写っていそう」は index に値があるときだけ数える (--measure-depth を付けると + grasp_depth.py で 1 本ずつ測る. 遅い). + + python3 pi05/count_by_position.py mecharm_redblock_box_teach_v1_trim2_corr_v5_deep + python3 pi05/count_by_position.py A B --target 10 --json out.json + +読むだけ (記録・データセットには何も書かない. --json のファイルだけ書く). +""" +import argparse +import contextlib +import json +import math +import sys +from pathlib import Path + +import cv2 +import numpy as np + +_HERE = Path(__file__).resolve() +sys.path.insert(0, str(_HERE.parent)) +sys.path.insert(0, str(_HERE.parent.parent)) # mat_corner_detection.py (r2r2r-scripts の直下) + +import grasp_depth as gd # noqa: E402 + +DATA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data") +RUN_SUBDIRS = ("run_pick_place", "pi05_policy") +POSITIONS = (1, 2, 3, 4, 5) +UNKNOWN = "unknown" +DEFAULT_TARGET = 10 + +# LOG352 の README の表 (runs/experiments/record_teach_demo/20261019_LOG352/README.md) +TEACH_FILES = { + "drag_v2_20221013_144348.json": 1, + "drag_v2_20221013_230205.json": 2, + "drag_v2_20221013_231249.json": 3, + "drag_v2_20221013_232345.json": 4, + "drag_v2_20221013_233323.json": 5, +} +# 教えた位置のブロックの中心 (マットの上の mm). 決め方は頭の説明 +TEACH_REF_MM = { + 1: (182.7, 188.8), + 2: (215.5, 242.2), + 3: (123.6, 235.6), + 4: (240.8, 101.3), + 5: (-48.8, 246.4), +} +MAX_DIST_MM = 30.0 +MAT_CORNERS_MM = [[310, -89], [-310, -89], [-310, 361], [310, 361]] # visual_servo_pick と同じ (twin_prep) +BLOCK_MIN_DIST_FROM_BASE_MM = 120.0 # record_teach_demo と同じ (twin_prep) +FRAME_STEP = 3 + + +# ---------------------------------------------------------------- 位置を決める +def teach_position(run_dir): + """教えた動きの run: teach/ の教えた記録のファイル名から. 戻り値 (位置 or UNKNOWN, 根拠の文字).""" + tdir = Path(run_dir) / "teach" + files = sorted(p.name for p in tdir.glob("[!.]*.json")) if tdir.is_dir() else [] + if not files: + return UNKNOWN, "teach/ に教えた記録がない" + pos = {TEACH_FILES[f] for f in files if f in TEACH_FILES} + if len(pos) != 1: + return UNKNOWN, f"teach/ のファイルが表と合わない: {', '.join(files)}" + return pos.pop(), f"teach/{files[0]}" + + +def nearest_position(mm, max_dist=MAX_DIST_MM): + """ブロックの位置 (mm) → (位置 or UNKNOWN, いちばん近い位置, 離れ mm).""" + p, d = min(((k, math.hypot(mm[0] - v[0], mm[1] - v[1])) for k, v in TEACH_REF_MM.items()), key=lambda x: x[1]) + return (p if d <= max_dist else UNKNOWN), p, round(d, 1) + + +def _px_to_mm(H, x, y): + r = cv2.perspectiveTransform(np.array([[[float(x), float(y)]]], np.float32), H) + return float(r[0][0][0]), float(r[0][0][1]) + + +def red_candidates(frame, mat_bbox, H): + """twin_prep.red_candidates と同じ (向きは使わないので出さない).""" + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + m = cv2.inRange(hsv, np.array([0, 100, 100]), np.array([10, 255, 255])) | \ + cv2.inRange(hsv, np.array([160, 100, 100]), np.array([180, 255, 255])) + m = cv2.morphologyEx(m, cv2.MORPH_OPEN, np.ones((5, 5), np.uint8)) + cnts, _ = cv2.findContours(m, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + out = [] + for c in cnts: + a = cv2.contourArea(c) + if a < 500: + continue + (cx, cy), (rw, rh), _ang = cv2.minAreaRect(c) + if not (mat_bbox[0] <= cx <= mat_bbox[1] and mat_bbox[2] <= cy <= mat_bbox[3]): + continue + aspect = max(rw, rh) / max(1e-6, min(rw, rh)) + extent = a / max(1e-6, rw * rh) + mm = _px_to_mm(H, cx, cy) + if math.hypot(*mm) < BLOCK_MIN_DIST_FROM_BASE_MM or aspect > 1.8 or extent < 0.6: + continue + out.append({"area": float(a), "px": (float(cx), float(cy)), "mm": mm}) + return out + + +def detect_corners(frame): + with contextlib.redirect_stdout(sys.stderr): + import mat_corner_detection as mcd + return mcd.detect_corners(frame) + + +def block_mm(run_dir, corners_fn=None): + """補正入りの run: 上カメラで見つけたブロックの位置. 戻り値 (dict or None, 理由).""" + corners_fn = corners_fn or detect_corners + rec = Path(run_dir) / "recording" + if not (rec / "top.mp4").exists() or not (rec / "top_frame_log.csv").exists(): + return None, "top.mp4 か top_frame_log.csv がない" + tc = None + if (rec / "states.csv").exists(): + gt, gs = gd.load_gripper_cmds(rec / "states.csv") + tc = gd.close_time(gt, gs) + _, fts = gd.load_frame_log(rec / "top_frame_log.csv") + cap = cv2.VideoCapture(str(rec / "top.mp4")) + try: + ok, f0 = cap.read() + if not ok: + return None, "top.mp4 が読めない" + corners = corners_fn(f0) + if corners is None: + return None, "マットの四隅が見つからない" + co = [[float(x), float(y)] for x, y in corners] + H = cv2.getPerspectiveTransform(np.array(co, np.float32), np.array(MAT_CORNERS_MM, np.float32)) + bbox = (int(min(co[0][0], co[3][0])), int(max(co[1][0], co[2][0])), + int(min(co[0][1], co[1][1])), int(max(co[2][1], co[3][1]))) + best, k, f = None, 0, f0 + while ok: + if k < len(fts) and tc is not None and fts[k] > tc: + break + if k % FRAME_STEP == 0: + cs = red_candidates(f, bbox, H) + if cs: + c = max(cs, key=lambda c: c["area"]) + if best is None or c["area"] > best["area"]: + best = dict(c, frame=k) + ok, f = cap.read() + k += 1 + finally: + cap.release() + if best is None: + return None, "閉じる前のコマでブロックが見つからない" + return best, "ok" + + +def find_run_dir(stem, runs_root): + for sub in RUN_SUBDIRS: + d = Path(runs_root) / sub / stem + if d.is_dir(): + return d + return None + + +def is_correction(meta, stem): + src = (meta or {}).get("source", "") + if src: + return src.startswith("pi05_policy") + return stem.startswith("pi05_") + + +def classify(stem, meta, runs_root, corners_fn=None): + """1 本の位置. 戻り値 dict(stem, kind, position, how, ...).""" + kind = "correction" if is_correction(meta, stem) else "teach" + out = {"stem": stem, "kind": kind, "position": UNKNOWN, "how": None} + rd = find_run_dir(stem, runs_root) + if rd is None: + out["how"] = "runs/ に試行フォルダがない" + return out + if kind == "teach": + out["position"], out["how"] = teach_position(rd) + return out + try: + b, why = block_mm(rd, corners_fn) + except Exception as e: # 見つけ方で落ちても数えるのは止めない + b, why = None, f"exception:{type(e).__name__}:{e}" + if b is None: + out["how"] = why + return out + pos, near, d = nearest_position(b["mm"]) + out.update(position=pos, how=f"top.mp4 のブロック (コマ {b['frame']}) → 位置 {near} まで {d}mm", + block_mm=[round(v, 1) for v in b["mm"]], nearest=near, dist_mm=d) + if pos == UNKNOWN: + out["how"] += f" (線 {MAX_DIST_MM:g}mm を超える)" + return out + + +# ---------------------------------------------------------------- 外した本・手の本 +def _shallow(v): + g = v.get("grasp_depth") or {} + r = str(v.get("reason", "")) + return bool(g.get("shallow")) or r.startswith("浅い") or r.startswith("shallow") + + +def _hand(v): + g = v.get("grasp_depth") or {} + return bool(v.get("hand_in_frame")) or bool(g.get("hand_suspect")) + + +def excluded_entries(ds_dir): + """データセットの index.json から, 外した本 {名前: 中身}. merge の excluded と build の used=false の両方.""" + p = Path(ds_dir) / "index.json" + if not p.exists(): + return {}, {} + idx = json.loads(p.read_text()) + out, kept_info = {}, {} + for k, v in (idx.get("excluded") or {}).items(): + out[k] = v + for k, v in idx.items(): + if isinstance(v, dict) and "used" in v and "run" in v: + if v["used"]: + kept_info[k] = v + elif v.get("grasp_depth") or v.get("hand_in_frame"): + out[k] = v + return out, kept_info + + +# ---------------------------------------------------------------- 数える +def resolve_input(x, data_root): + p = Path(x) + if p.is_dir() and (p.name == "raw_episodes" or list(p.glob("[!.]*/episode.npz"))): + return p, p.parent + d = Path(data_root) / "datasets" / x + return d / "raw_episodes", d + + +def count(inputs, data_root=DATA_ROOT, target=DEFAULT_TARGET, measure_depth=False, corners_fn=None, log=print): + runs_root = Path(data_root) / "runs" + eps, excl, kept_info, seen = [], [], {}, set() + for x in inputs: + raw, ds = resolve_input(x, data_root) + if not raw.is_dir(): + raise SystemExit(f"[ABORT] raw_episodes がない: {raw}") + for ep in sorted(p.parent for p in raw.glob("[!.]*/episode.npz")): + if ep.name in seen: + continue + seen.add(ep.name) + meta = json.loads((ep / "meta.json").read_text()) if (ep / "meta.json").exists() else {} + stem = meta.get("run_stem", ep.name) + eps.append((ep.name, stem, meta)) + ex, ki = excluded_entries(ds) + kept_info.update(ki) + for k, v in ex.items(): + if k not in seen: + seen.add(k) + excl.append((k, v)) + rows = [] + for name, stem, meta in eps: + r = classify(stem, meta, runs_root, corners_fn) + r["name"] = name + g = (kept_info.get(stem) or {}).get("grasp_depth") + if measure_depth: + rd = find_run_dir(stem, runs_root) + g = gd.measure_run(rd) if rd else None + r["hand_suspect"] = bool(g and g.get("hand_suspect")) + r["depth_known"] = g is not None + rows.append(r) + log(f"[POS ] {name} {'教えた動き' if r['kind'] == 'teach' else '補正入り'} 位置 {r['position']} {r['how']}") + ex_rows = [] + for name, v in excl: + kind_meta = {"source": "pi05_policy"} if (v.get("kind") == "補正入り" or name.startswith("pi05_")) else {} + r = classify(name, kind_meta, runs_root, corners_fn) + r.update(name=name, shallow=_shallow(v), hand=_hand(v), reason=v.get("reason")) + ex_rows.append(r) + log(f"[EXCL] {name} 位置 {r['position']} {r.get('reason')}") + return summarize(rows, ex_rows, target) + + +def summarize(rows, ex_rows, target): + keys = list(POSITIONS) + [UNKNOWN] + table = {} + for k in keys: + rs = [r for r in rows if r["position"] == k] + es = [r for r in ex_rows if r["position"] == k] + n = len(rs) + table[k] = { + "episodes": n, + "teach": sum(r["kind"] == "teach" for r in rs), + "correction": sum(r["kind"] == "correction" for r in rs), + "to_add": (max(0, target - n) if k != UNKNOWN else None), + "excluded_shallow": sum(r["shallow"] for r in es), + "excluded_hand": sum(r["hand"] for r in es), + "hand_suspect_in_data": sum(r["hand_suspect"] for r in rs), + } + return {"target": target, "max_dist_mm": MAX_DIST_MM, "table": table, + "total": len(rows), "depth_known": sum(r["depth_known"] for r in rows), + "episodes": rows, "excluded": ex_rows} + + +def table_lines(res): + """Markdown の表 (README と端末で同じもの).""" + t = res["table"] + lines = [f"| 位置 | 本数 | 教えた動き | 補正入り | 目標 {res['target']} 本まで | 浅くて外した | 手で外した | " + "入っている本で手が写っていそう |", + "| --- | --- | --- | --- | --- | --- | --- | --- |"] + for k, v in t.items(): + name = f"位置 {k}" if k != UNKNOWN else "分からない" + add = "—" if v["to_add"] is None else f"+{v['to_add']}" + hand_in = v["hand_suspect_in_data"] if res["depth_known"] else "—" + lines.append(f"| {name} | {v['episodes']} | {v['teach']} | {v['correction']} | {add} | " + f"{v['excluded_shallow']} | {v['excluded_hand']} | {hand_in} |") + lines.append(f"| 合計 | {res['total']} | {sum(v['teach'] for v in t.values())} | " + f"{sum(v['correction'] for v in t.values())} | " + f"+{sum(v['to_add'] or 0 for v in t.values())} | " + f"{sum(v['excluded_shallow'] for v in t.values())} | {sum(v['excluded_hand'] for v in t.values())} | " + f"{sum(v['hand_suspect_in_data'] for v in t.values()) if res['depth_known'] else '—'} |") + return lines + + +def readme_section(res): + unk = [r for r in res["episodes"] + res["excluded"] if r["position"] == UNKNOWN] + lines = ["## 置き場所ごとの本数 (`pi05/count_by_position.py`。依頼 118)", "", + "教えた動きは `teach/` の教えた記録のファイル名 (LOG352 の表) で、補正入りは上カメラで見つけたブロックの位置を" + f"いちばん近い教えた位置に割り当てた ({res['max_dist_mm']:g}mm を超えたら「分からない」)。" + "「浅くて外した」「手で外した」は、このデータセットの `index.json` で外したエピソードの数 (データには入っていない)。", ""] + lines += table_lines(res) + if not res["depth_known"]: + lines += ["", "- 入っている本の「手が写っていそう」は測っていない (`--measure-depth` で測る)"] + for r in unk: + lines.append(f"- 分からない: {r['name']} ({r['how']})") + return lines + + +def main(): + ap = argparse.ArgumentParser(description=__doc__.split("\n")[0]) + ap.add_argument("inputs", nargs="+", help="データセット名 (datasets/ の下) か raw_episodes のフォルダ") + ap.add_argument("--target", type=int, default=DEFAULT_TARGET, help=f"位置ごとの目標本数 (既定 {DEFAULT_TARGET})") + ap.add_argument("--measure-depth", action="store_true", + help="入っている本を grasp_depth.py で測り, 手が写っていそうな本を数える (遅い)") + ap.add_argument("--json", help="結果を JSON に書く (書く先はこのファイルだけ)") + ap.add_argument("--data-root", default=str(DATA_ROOT), help=argparse.SUPPRESS) + args = ap.parse_args() + res = count(args.inputs, args.data_root, args.target, args.measure_depth) + print() + print("\n".join(table_lines(res))) + if args.json: + Path(args.json).write_text(json.dumps(res, indent=2, ensure_ascii=False, default=str)) + print(f"[JSON] {args.json}") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/pi05/merge_raw_episodes.py b/pi05/merge_raw_episodes.py index eba2841..7e85eed 100644 --- a/pi05/merge_raw_episodes.py +++ b/pi05/merge_raw_episodes.py @@ -21,6 +21,11 @@ eval_policy_offline.py) も「学習に使ったデータ」を 1 つのフォ --readme-note に Markdown のファイルを渡すと, README の最後にそのまま足す (外した理由の説明など). 付けないときの出力は今までと同じ. +置き場所ごとの本数 (2026-11-07 依頼 118): + 作り終えたら count_by_position.py で, 教えた位置 1〜5 ごとの本数 (教えた動き・補正入り)・目標本数 (--target. 既定 10) + までの差・外した本の数を README の最後に表で足す. runs/ を読むだけで, raw_episodes・index.json は変わらない. + --no-position-table で足さない (README も今までと同じ). 数えるところで落ちても, データは作り終えたまま止めない. + 決まり: - 元のデータセットは読むだけ (上書き・移動・削除しない) - 写す前に, 全部の元が お手本 (最初の --src) と一緒に使えるか (キー・型・FPS・画像の大きさ) を確かめ, @@ -68,6 +73,18 @@ def make_tar(ds, name): return out +def position_table(raw, root, target): + """依頼 118: 置き場所ごとの本数の表 (README の行). 数えるところで落ちたら, そのことだけ書いて続ける.""" + import count_by_position as cbp + try: + res = cbp.count([str(raw)], data_root=root, target=target) + except (Exception, SystemExit) as e: + print(f"[WARN] 置き場所ごとの本数を数えられなかった: {type(e).__name__}: {e}") + return ["", f"(置き場所ごとの本数は数えられなかった: {type(e).__name__})"] + print("\n".join(cbp.table_lines(res))) + return [""] + cbp.readme_section(res) + + def main(): ap = argparse.ArgumentParser() ap.add_argument("--src", action="append", required=True, @@ -77,6 +94,9 @@ def main(): ap.add_argument("--tar", action="store_true", help="Pod へ送る tar も作る") ap.add_argument("--exclude-json", help="外す本と理由の JSON ({エピソード名: {reason: ..., ...}}). 依頼 117") ap.add_argument("--readme-note", help="README の最後に足す Markdown のファイル") + ap.add_argument("--no-position-table", action="store_true", + help="置き場所ごとの本数の表 (count_by_position.py) を README に足さない. 依頼 118") + ap.add_argument("--target", type=int, default=10, help="置き場所ごとの表の目標本数 (既定 10)") ap.add_argument("--data-root", help=argparse.SUPPRESS) # テスト用 (KIOXIA の代わり) args = ap.parse_args() if args.data_root: @@ -171,10 +191,14 @@ def main(): lines.append(f"| {k} | {v['from_dataset']} | {v['steps']} | {v['reason']} |") if note: lines += ["", note.rstrip("\n")] + if not args.no_position_table: + lines += position_table(out, root, args.target) (ds / "README.md").write_text("\n".join(lines) + "\n") code = ds / "code" code.mkdir(exist_ok=True) shutil.copy2(_HERE, code / _HERE.name) # データと対のコード + if not args.no_position_table: + shutil.copy2(_HERE.parent / "count_by_position.py", code / "count_by_position.py") # 表を作ったコード if excluded: shutil.copy2(args.exclude_json, code / Path(args.exclude_json).name) # 外した本の一覧も対で残す print(f"\n合計 {total_eps} 本 / {total_steps} コマ → {out}") diff --git a/pi05/test_count_by_position.py b/pi05/test_count_by_position.py new file mode 100644 index 0000000..852e35b --- /dev/null +++ b/pi05/test_count_by_position.py @@ -0,0 +1,218 @@ +#!/usr/bin/env python3 +"""test_count_by_position.py — 置き場所ごとの本数 (count_by_position.py) と merge_raw_episodes.py の表の確かめ (2026-11-07 依頼 118). + + python3 pi05/test_count_by_position.py -v + +一時フォルダの合成の記録だけ (KIOXIA にもアームにも触らない). 確かめること: + - 教えた動き: teach/ の教えた記録のファイル名で位置 1〜5. ない・表にないなら「分からない」 + - いちばん近い教えた位置. 線 (30mm) を超えたら「分からない」(位置 4 から 48mm ずらした 193729 の値) + - 補正入り: 上カメラの合成の動画で, マットの四隅からの mm に直したブロックの位置 → 位置. 閉じたあとのコマは見ない + - データセットを数える: 位置ごとの本数・教えた動き/補正入りの内訳・目標までの差・外した本 (浅い・手) の数 + - merge_raw_episodes.py: 引数なしで README に表が足され, --no-position-table では足さない. + どちらでも raw_episodes と index.json は同じ +""" +import csv +import filecmp +import json +import sys +import tempfile +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 count_by_position as cbp # noqa: E402 +import merge_raw_episodes as mre # noqa: E402 +from test_merge_and_config import write_ep # noqa: E402 + +# 合成のマット: 画の (100,100)→(310,−89)mm・(1340,100)→(−310,−89)・(1340,1000)→(−310,361)・(100,1000)→(310,361). 0.5mm/px +CORNERS = [(100, 100), (1340, 100), (1340, 1000), (100, 1000)] +W, H = 1440, 1080 + + +def mm_to_px(mm): + return int(round(100 + 2 * (310 - mm[0]))), int(round(100 + 2 * (mm[1] + 89))) + + +def fake_corners(_frame): + return CORNERS + + +def write_policy_run(d, block_mm, close_after=None, late_block_mm=None, n=9): + """補正入りの run の合成の記録. 閉じる命令は close_after コマめの時刻の直後 (None なら命令なし). + late_block_mm を渡すと, 閉じたあとのコマにだけそこへ大きな赤を描く (見ないことの確かめ).""" + rec = d / "recording" + rec.mkdir(parents=True) + vw = cv2.VideoWriter(str(rec / "top.mp4"), cv2.VideoWriter_fourcc(*"mp4v"), 10, (W, H)) + ts = [1000.0 + 0.1 * i for i in range(n)] + for i in range(n): + img = np.full((H, W, 3), (60, 60, 60), np.uint8) + late = close_after is not None and i > close_after + if late and late_block_mm is not None: + x, y = mm_to_px(late_block_mm) + cv2.rectangle(img, (x - 50, y - 50), (x + 50, y + 50), (30, 30, 220), -1) + elif not late: + x, y = mm_to_px(block_mm) + cv2.rectangle(img, (x - 30, y - 30), (x + 30, y + 30), (30, 30, 220), -1) + vw.write(img) + vw.release() + with (rec / "top_frame_log.csv").open("w", newline="") as f: + w = csv.writer(f) + w.writerow(["frame_idx", "capture_ts"]) + w.writerows([i, t] for i, t in enumerate(ts)) + with (rec / "states.csv").open("w", newline="") as f: + w = csv.writer(f) + w.writerow(["ts", "event_type", "gripper_state"]) + w.writerow([ts[0] - 1, "gripper_cmd", 0]) + if close_after is not None: + w.writerow([ts[close_after] + 0.05, "gripper_cmd", 1]) + + +def write_teach_run(d, teach_file): + (d / "teach").mkdir(parents=True) + if teach_file: + (d / "teach" / teach_file).write_text("{}") + + +class TestPosition(unittest.TestCase): + def setUp(self): + self._td = tempfile.TemporaryDirectory() + self.root = Path(self._td.name) + + def tearDown(self): + self._td.cleanup() + + def test_teach_file_mapping(self): + for f, pos in cbp.TEACH_FILES.items(): + d = self.root / f"t{pos}" + write_teach_run(d, f) + self.assertEqual(cbp.teach_position(d)[0], pos) + write_teach_run(self.root / "none", None) + self.assertEqual(cbp.teach_position(self.root / "none")[0], cbp.UNKNOWN) + write_teach_run(self.root / "other", "drag_v2_20221013_999999.json") + self.assertEqual(cbp.teach_position(self.root / "other")[0], cbp.UNKNOWN) + self.assertEqual(cbp.teach_position(self.root / "missing")[0], cbp.UNKNOWN) + + def test_nearest_and_line(self): + for pos, mm in cbp.TEACH_REF_MM.items(): + self.assertEqual(cbp.nearest_position(mm)[0], pos) + self.assertEqual(cbp.nearest_position((-48.3, 246.2))[0], 5) # 補正入り 184650 の値 + pos, near, d = cbp.nearest_position((237.4, 52.6)) # 位置 4 から ずらした 193729 + self.assertEqual((pos, near), (cbp.UNKNOWN, 4)) + self.assertGreater(d, cbp.MAX_DIST_MM) + + def test_block_from_video(self): + d = self.root / "p5" + write_policy_run(d, (-50.0, 240.0), close_after=5, late_block_mm=cbp.TEACH_REF_MM[4]) + b, why = cbp.block_mm(d, fake_corners) + self.assertEqual(why, "ok") + self.assertAlmostEqual(b["mm"][0], -50.0, delta=2) + self.assertAlmostEqual(b["mm"][1], 240.0, delta=2) + self.assertLessEqual(b["frame"], 5) # 閉じたあとのコマは見ない + r = cbp.classify("pi05_x", {"source": "pi05_policy (model + correct)"}, self.root, fake_corners) + self.assertEqual(r["position"], cbp.UNKNOWN) # runs/ にない + self.assertIn("ない", r["how"]) + + def test_block_not_found(self): + d = self.root / "corner_none" + write_policy_run(d, (-50.0, 240.0), close_after=5) + b, why = cbp.block_mm(d, lambda f: None) + self.assertIsNone(b) + self.assertIn("四隅", why) + + +class TestCount(unittest.TestCase): + def setUp(self): + self._td = tempfile.TemporaryDirectory() + self.root = Path(self._td.name) + runs = self.root / "runs" + ds = self.root / "datasets" + files = {v: k for k, v in cbp.TEACH_FILES.items()} + # 教えた動き: 位置 2 が 2 本・位置 4 が 1 本・teach/ のない 1 本 + for stem, pos in [("run_a", 2), ("run_b", 2), ("run_c", 4), ("run_d", None)]: + write_teach_run(runs / "run_pick_place" / stem, files.get(pos)) + write_ep(ds / "a" / "raw_episodes" / stem, n=3) + # 補正入り: 位置 5 が 1 本・どこからも遠い 1 本 + write_policy_run(runs / "pi05_policy" / "pi05_1", cbp.TEACH_REF_MM[5], close_after=5) + write_policy_run(runs / "pi05_policy" / "pi05_far", (0.0, 350.0), close_after=5) + for stem in ("pi05_1", "pi05_far"): + write_ep(ds / "b" / "raw_episodes" / stem, n=4, source="pi05_policy (model + correct)") + # 外した本 (merge の excluded と同じ形): 位置 1 の浅い本・位置 2 の手の本・位置 5 の浅い補正入り + write_teach_run(runs / "run_pick_place" / "run_s", files[1]) + write_teach_run(runs / "run_pick_place" / "run_h", files[2]) + write_policy_run(runs / "pi05_policy" / "pi05_s", cbp.TEACH_REF_MM[5], close_after=5) + for stem in ("run_s", "run_h"): + write_ep(ds / "a" / "raw_episodes" / stem, n=3) + write_ep(ds / "b" / "raw_episodes" / "pi05_s", n=4, source="pi05_policy (model + correct)") + self.excl = self.root / "excl.json" + self.excl.write_text(json.dumps({ + "run_s": {"reason": "浅い: 閉じて 1.0 秒後のはみ出し 38.9px > 24px"}, + "run_h": {"reason": "人の指が写っている", "hand_in_frame": True}, + "pi05_s": {"reason": "浅い: 33px", "kind": "補正入り", "grasp_depth": {"shallow": True}}})) + self.patch = mock.patch.object(cbp, "detect_corners", fake_corners) + self.patch.start() + + def tearDown(self): + self.patch.stop() + self._td.cleanup() + + def merge(self, name, *extra): + argv = ["merge_raw_episodes.py", "--data-root", str(self.root), "--src", "a", "--src", "b", + "--dataset", name, "--exclude-json", str(self.excl), *extra] + with mock.patch.object(sys, "argv", argv), mock.patch("sys.stdout"): + return mre.main() + + def test_count_merged(self): + self.assertEqual(self.merge("ab"), 0) + res = cbp.count(["ab"], data_root=self.root, target=3, log=lambda *a: None) + t = res["table"] + self.assertEqual(res["total"], 6) + self.assertEqual([t[p]["episodes"] for p in cbp.POSITIONS], [0, 2, 0, 1, 1]) + self.assertEqual(t[cbp.UNKNOWN]["episodes"], 2) # teach/ なし・遠い補正入り + self.assertEqual((t[5]["teach"], t[5]["correction"]), (0, 1)) + self.assertEqual([t[p]["to_add"] for p in cbp.POSITIONS], [3, 1, 3, 2, 2]) + self.assertEqual((t[1]["excluded_shallow"], t[2]["excluded_hand"], t[5]["excluded_shallow"]), (1, 1, 1)) + self.assertEqual(sum(v["excluded_shallow"] for v in t.values()), 2) + # raw_episodes のフォルダを渡しても同じ + res2 = cbp.count([str(self.root / "datasets" / "ab" / "raw_episodes")], data_root=self.root, target=3, + log=lambda *a: None) + self.assertEqual(res2["table"], t) + lines = cbp.table_lines(res) + self.assertIn("| 位置 1 | 0 | 0 | 0 | +3 | 1 | 0 | — |", lines) + self.assertIn("| 分からない | 2 | 1 | 1 | — | 0 | 0 | — |", lines) + + def test_merge_readme_table_and_same_data(self): + self.assertEqual(self.merge("with"), 0) + self.assertEqual(self.merge("without", "--no-position-table"), 0) + ds = self.root / "datasets" + rw, rwo = (ds / "with" / "README.md").read_text(), (ds / "without" / "README.md").read_text() + self.assertIn("## 置き場所ごとの本数", rw) + self.assertIn("| 位置 2 | 2 | 2 | 0 | +8 |", rw) + self.assertNotIn("置き場所ごとの本数", rwo) + self.assertEqual(rw.split("\n## 置き場所ごとの本数")[0].rstrip("\n").replace("with", "X"), + rwo.rstrip("\n").replace("without", "X")) + iw = json.loads((ds / "with" / "index.json").read_text()) + iwo = json.loads((ds / "without" / "index.json").read_text()) + iw.pop("repo_id"); iwo.pop("repo_id") + self.assertEqual(iw, iwo) + cmp = filecmp.dircmp(ds / "with" / "raw_episodes", ds / "without" / "raw_episodes") + self.assertEqual((cmp.left_only, cmp.right_only, cmp.diff_files), ([], [], [])) + for sub in cmp.common_dirs: + c = filecmp.dircmp(ds / "with" / "raw_episodes" / sub, ds / "without" / "raw_episodes" / sub) + self.assertEqual(c.diff_files, []) + self.assertTrue((ds / "with" / "code" / "count_by_position.py").exists()) + self.assertFalse((ds / "without" / "code" / "count_by_position.py").exists()) + + def test_merge_survives_count_error(self): + with mock.patch.object(cbp, "count", side_effect=RuntimeError("x")): + self.assertEqual(self.merge("err"), 0) + self.assertIn("数えられなかった", (self.root / "datasets" / "err" / "README.md").read_text()) + + +if __name__ == "__main__": + unittest.main() `````` ## 次は実機で5か所に10本ずつの深いお手本をそろえる 学習データの準備は終わりました。残りは実機で約30本を撮り足すことで、別のチャットで行います。位置1を教え直したときは、新しい教えた記録のファイル名を、本数を数える表示の表に1行足します。そろったら、外した版に足して学習し直し、補正なしで比べます。比べるときは、学習に使う位置と、評価のためだけに使う位置を分けておきます。 Unity の画を本物に近づけて、お手本を描いて増やすのは、つかみが安定してからにします。置き場所を何十か所にも広げたくなったとき、写真から作る3D（ガウシアン・スプラッティング）で机のまわりを Unity に入れるのが有力です。 ### 出典 - Real2Render2Real: Scaling Robot Data Without Dynamics Simulation or Robot Hardware（Yu, Fu ほか、2025）：https://arxiv.org/abs/2505.09601 - 同論文（CoRL 2025 / PMLR）：https://proceedings.mlr.press/v305/yu25a.html - デジタルツイン学習 R2R2R のシリーズのページ：https://pixelstomotors.com/series2 - LOG352（手で教えた動きを再生して π0.5 のお手本を集める。肩 j1 の 88° の件） - LOG390（依頼箱の仕組み） - LOG443（SO-101 で π0.5 に靴下とゴミを取り分けさせた例の分析） - LOG461（mechArm 270 の爪と肩の電流を読む）