補正では解決不能と確定した。固定オフセットが効かない理由と、根本修正への方針転換

Phase C v2（2点実測アフィン変換）を実機で初検証する日だった。dry-runでパイプライン全体が正常動作することを確認し、PICK/PLACEの方向バグを修正し、lookup自体はdist=0.2mmという完璧な精度で動いた。問題はその先にあった。グリッパーの爪がブロックに届かない。固定補正を複数パターン試した。全部ダメだった。最終的に判明したのは「J0姿勢によって爪の向きが変わるため、固定オフセットでは構造的に解決不能」という事実だった。方針を転換する。Unity FKをグリッパー爪位置で作り直す。

💡 Key Takeaway
lookupテーブル自体はdist=0.2mmという誤差で動作している。カメラ→Unity座標変換も正常。6軸全部lookup結果が送信されていることも確認した。それでもグリッパーがブロックに届かない。原因はUnity FKスキャンが記録するEE座標がリンク先端（手首）の位置であり、グリッパーの爪はそこからさらに先にあるためだ。オフセットはJ0の向きに依存して変わるため、単一の固定補正値では補えない。解決策はただ一つ、EEの定義をグリッパー爪先端に変えてFKを再生成すること。

dry-runでパイプライン全体を検証する

—— 213,312点ロード、ee_yフィルタ後156,603点、パイプライン全体正常動作。

r2r2r_loop.pyのSSH・Pi送信部分をスキップして、カメラ検出→Phase C v2アフィン変換→lookup→結果表示だけをMac単体で走らせるr2r2r_dryrun.pyを使って最初の検証をした。

起動するとFKテーブル213,312点をロードし、ee_yフィルタ（PICK高さ27mm±20mm）で156,603点に絞り込む。2点キャリブレーション（赤ブロック・白箱を同時検出してスケール係数と原点を計算）が完了すると、1秒ごとにブロック位置を検出してUnity座標に変換し、lookup結果を表示し続ける。

赤ブロック pixel: (617, 725)
白箱       pixel: (959, 539)
scale_x = +0.00099 m/px  [WARN: 正の値—座標系確認]
scale_z = +0.00026 m/px

scale_xが正になったのは、今回のカメラ配置で「画像右=Unity X+」になっていたため。引き継ぎ書の想定と逆だったが、ブロックを右に動かすとUnity xがより負になることを目視で確認し、座標変換は正しく機能していると判断した。J0角度がブロックの位置に応じて-5°、15°、48°、53°、56°と変化し、全件dist < 100mmに収まっていた。動的追従は機能している。

PICK/PLACEの逆転バグを修正する

—— コードを読むまで原因がわからなかった。

実機でr2r2r_loop.pyを走らせると、アームが白箱に向かってからブロックに向かった。PICK/PLACEが逆になっていた。detect_both_targets()の内部は「赤→pick_unity、白→place_unity」と正しく実装されていたが、line 678の受け取り順がセッション中に誤修正されたまま残っていた。

# 誤: place_unity, pick_unity, frame = detect_both_targets(cap)
# 正: pick_unity, place_unity, frame = detect_both_targets(cap)  ← 元に戻す

1行直して正しい順序に戻った。アームは赤ブロックに向かってからPLACEに移るようになった。

lookupは完璧に動いている。問題はその先にある

—— dist=0.2mm。爪がブロックに届かない。

PICK/PLACEの方向が正しくなったあと、ログを確認した。

PICK Unity座標: (-0.1450, 0.2030)
PICK lookup成功: ee=(-0.1851, 0.0310, 0.1029) dist=0.2mm
PICK angles=[60.0, 98.7, -35.3, -4.4, -64.1, -30.0]

distは0.2mm。lookupはブロックの位置に最も近いEE座標をほぼ完璧に見つけている。6軸全部の角度がlookup結果から来ていることも確認した。「APPROACH_PICK_BASEの固定値で差し替わっているのでは」という仮説は外れた。

それでもグリッパーは赤ブロックに届かなかった。アームはブロックの方向を向いて、正しい高さで止まっている。ただ、爪がブロックから離れた位置で止まる。

原点は明確だった。test_ee_offset.pyで実測した結果がある。

グリッパー爪 Unity: (-0.1745, 0.0481)
lookup EE期待:      (-0.1688, 0.1384)
オフセット: dx=0.006m  dz=0.090m

lookupテーブルのee_x、ee_zはUnityのリンク先端（手首関節）の座標だ。実機のグリッパー爪はそこからさらに先にある。Unity FKスキャンは爪の位置を記録していなかった。

固定補正を複数パターン試す。全部ダメだった

—— z-0.100で100mm手前、z+0.100でlookup失敗。どっちも解決しない。

dz=90mmを元に固定補正を複数パターン試した。

まずz-0.0903。lookupクエリをブロック位置より90mm手前に投げれば、EE（手首）がそこに来て、爪先がブロック位置に届くはずという発想だった。結果、爪は確かに近づいたが、yが少し変わっただけで根本的な改善はなかった。

次にx=-0.040、z=-0.100の組み合わせ補正。lookupは成功（dist=0.2mm）したが、カメラ画像を見ると爪がブロックから約100mm手前で止まっていた。補正が逆方向だった可能性がある。

ではz+0.100に反転してみる。

PICK lookup失敗: dist=135.0mm > 100mm → 固定角度fallbackに移行

lookupテーブルのカバレッジ外に出た。z+方向はマット奥（ロボットから遠い方向）で、テーブルが存在しないゾーンだった。

どの補正値を試してもうまくいかない。ここで構造的な問題に気づいた。

固定補正が効かない理由

—— J0が変わると爪の向きが変わる。補正値は一定にならない。

test_ee_offset.pyの計測はJ0=48°の姿勢で行った。しかし実際のPICK動作ではlookup結果によってJ0=60°になる。J0が12°違えば、アームの向きが変わり、グリッパー爪がどの方向に伸びているかも変わる。

「EEから爪まで90mmずれている」という事実は変わらない。しかしそのズレがUnity座標上のどの方向に出るかは、J0が変わるたびに違う値になる。単一の固定補正値（dx=c1、dz=c2）では、どの姿勢でも正しく補正することはできない。

補正なし:   爪がブロックより手前で止まる（100mm程度）
z-0.100:   lookup成功、爪が100mm手前
z+0.100:   lookupカバレッジ外、fallback動作
固定補正:  どのパターンも構造的に不可能

Unity FKを爪位置で作り直す

—— 根本解決は一つだけ。EEの定義を変える。

問題の根本はUnity FKスキャンが記録するEE座標がリンク先端（手首）だということだ。グリッパーを含めた爪先端の座標を記録していれば、lookupが返す座標とアームが実際に届く位置が一致する。

解決策は単純だ。Unityの URDF でエンドエフェクタのTransformをグリッパー爪先端に変更し、213,312点のFKスキャンを再生成する。新しいlookupテーブルに入れ替えれば、固定補正なしで爪がブロック位置に届くようになる。

補正で逃げ続けてきた。その終わりが確定した。

※ プロジェクト名「R2R2R」は、UC Berkeley・Toyota Research Instituteの研究「Real2Render2Real」（Yu, Fu et al., 2025）の概念に触発されています。現実空間をデジタルツイン化し、仮想空間での試行から得た知見を実世界のモーター駆動へ還元する——その設計と実装過程を毎日記録しています。技術アプローチおよび目的は独自のものです。