mechArm 270 Pi 実機データを土台にデジタルツインを作る Log#06 | pose_idで、姿勢と実機結果をつなぐ

整理から始めたつもりだった。Unity のデジタルツインで増やしてきた姿勢データが、中間 CSV の山になっていて、まずはそれを片付けるだけのつもりだった。

だが、片付けている途中で、もっと大きな問題に気づいた。姿勢データと、実機が動いた結果の記録が、どこにもつながっていない。同じ姿勢を使った試行のはずなのに、後から「どの姿勢だったか」を辿れない。

FK テーブルには、姿勢から爪先位置を引く地図がある。採用版 JSON には、実機が読む姿勢がある。台帳には、実機が動いた結果が残る。だが、その三つを同じ姿勢として結ぶ鍵がなかった。その鍵が pose_id になる。

この回は、中間ファイルの整理から始まり、姿勢に背番号を振り、j0_sweep_adopted の JSON・fk_scan_motion_based_v2.csv・pick_place_ledger_v3.csv を後から突き合わせられる状態にするまでの記録になる。コードに触れた箇所はすべて差分と外部レビューの結果を残す。

散らかりの正体は二層だった

最初の問いはシンプルだった。「ツインで作った姿勢データはバラバラの CSV になっているのか」。

調べてみると、姿勢データは二つの層に分かれていて、層ごとに状態がまるで違った。

ひとつは lookup テーブルの本体。fk_scan_motion_based_v2.csv（852,730 行）で、これは Mac 側 ~/r2r2r-scripts/ と Unity 側 Assets/Data/ に同一バイトで二重配置され、md5 も一致している。毎回のスキャンは「全行投入してから 6 軸キーの重複を除去する」方式でこの一本にマージされてきたので、テーブル自体は分裂していない。すでに統合済みだった。

もうひとつが、その v2 を作る過程で各回のスキャンが吐き出した中間ファイル群。こちらは DAY 別・バージョン別に散らばっていた。各スキャンの生出力、マージ直前の退避スナップショット、Unity に放り込んだ入力グリッド。これらが working ディレクトリにも archive にも溜まっていた。

つまり「バラバラの CSV」は、lookup テーブル本体ではなく、その素材側の話だった。本体はとっくに一本化されていて、散らかっていたのは過程の中間生成物の方だった。

棚卸しの結果は次のようになった。本流（触らない）が v2（852,730 行）と無印（512,088 行）の各 Mac/Unity ペア。整理候補が LOG252 期の motion_log v1〜v5、LOG254 期の probe / face80 入出力など、16 ファイル・約 3 MB で、いずれも v2 に統合済みかつ archive にも保全がある状態だった。

中間 CSV を archive に集約する

方針は単純にした。増やすのは構わない。ただし片付けは強制する。そのために「保全を先に埋めてから、保全が確認できたものだけを個別削除する」という順序を固定した。

ここで一点だけ手当てが必要だった。LOG254 の probe / probe40 / j0probe 出力が archive に未退避で、Unity の Assets/Data/ に裸で置かれていた。しかも probe 系は偵察用で v2 に取り込まれていない可能性が高く、つまり Assets/Data のそれが唯一のコピーだった。ここが消えたら復元できない。整理の判断とは無関係に、まずここを archive へコピーした。

Claude Code に渡した整理の結果がこれになる。

STEP 1: archive 退避
退避先: /Volumes/KIOXIA/r2r2r-data/archive/pick_place_ledger_v3_pre_pose_cols_20260626.csv
md5 (src=arch): 66713d7ee41a1e5052f899b7b5a2bfcd 一致
size: 50,384 bytes

Unity 側の削除は .csv と対の .csv.meta を両方消す必要がある。最終的に実ファイル 62 件を削除し、probe 系 11 件を新規退避した。本流の v2 / 無印 4 ファイルは md5 完全不変。working ディレクトリは「v2 ＋ 無印 ＋ 現役入力」だけのクリーンな状態になった。

Plastic SCM の check-in は Changeset 109 として確定した。コメントは次の通り。

[DAY255] Assets/Data 整理: v2統合済みの中間スキャンCSVを削除（DAY252 v1-v5入力/出力・DAY254 probe系、archive保全済み）

台帳が古い方を指していた

整理が一段落して、本題に入ろうとしたところで、別の問題に行き当たった。

実機の試行記録を貯める台帳がある。pick_place_ledger_v3.csv は、100 試行ぶんの既存記録を持つ 50 列の正本になっていた。これに姿勢を追跡するための列を足したかった。ところが、実機を動かす run_pick_place.py のコードを確認すると、台帳のパスがこうなっていた。

LEDGER_CSV_PATH = LEDGER_DIR / "pick_place_ledger_v2.csv"

実機が書き込む先は v2 だった。拡張しようとしている v3 は、誰も書き込んでいない空の器になりかねなかった。これは以前にも近い形で起きている。「拡張した先と、実機が実際に触る先が食い違う」という型の事故だ。

参照箇所を確認すると、ledger_v2 を指しているのはこの 1 行だけだった。直すのも 1 行で済む。

LEDGER_CSV_PATH = LEDGER_DIR / "pick_place_ledger_v3.csv"

ただし、この修正だけでは完結しない。パスを v3 に向けても、スクリプト側が台帳に書く列の定義（LEDGER_COLS）は古い 33 列のままで、v3 ファイルの 50 列とは食い違う。列を揃える作業は後段に回した。まずパスだけを正した。

live / twin / real をつなぐ鍵がなかった

ここからが本題になる。やりたいことは「ツインと実機の動きを比較できる状態」を作ることだった。

比較に必要なものは三つある。姿勢（関節角）、その姿勢でツインが予測する爪先位置、そして実機がその姿勢で実際に置いた爪先位置。この三つを同じ姿勢について並べられて初めて、「ツインの予測と実機の実際がどれだけずれるか」が測れる。

ところが、この三つはそれぞれ別の場所にいて、役割もまるで違う。

入力・変換器・出力。三つは役割が違うのに、これまで「同じ姿勢について」突き合わせる鍵がなかった。台帳には実機の実測座標は記録されているのに、「その試行がどの採用姿勢を使ったか」が残っていない。だから JSON の姿勢と台帳の実測を結びつけられない。

この鍵が pose_id になる。姿勢に背番号を振り、実機がどの背番号を使ったかを台帳に書く。同じ pose_id で三本がつながる。

台帳は入力ではなく、出力だった

当初は実機が読む正本を台帳（ledger）だと思い込んでいた。実際は逆で、台帳はアームが動いた後の結果を書く出力先であって、アームの挙動を決める入力ではない。アームの挙動を直接決めているのは採用版 JSON の方だ。

JSON が入力、FK テーブルが変換器、台帳が出力。この向きを正したことが、設計の向き全体を正す出発点になった。ファイルが増えるほど、何が入力で何が出力かは混ざりやすくなる。

姿勢の正本は五本、周りは選定の履歴

pose_id を振る前に、対象がいくつあるのかを数えた。最初は採用版 JSON が 5 本だと認識していたが、それが本当かを実ディスクで確認した。

実機が直接読む正本は adopted の 5 本だ。ただし、過去の sweep / classified / run ログまで含めると JSON は 254 本ある。そのうち姿勢として束ねる対象は adopted / classified / 未分類 sweep の 153 本で、run_* の 101 本は試行ログなので姿勢正本ではない。

姿勢の正本が 5 本だけなのには理由がある。Pick&Place の一連で姿勢を決める場面が、ちょうど五つだからだ。掴みに近づく（approach_pick）、掴む（pick）、置きに近づく（approach_place）、置く（place_withblock）、そしてブロック無しの置き（place_noblock）。工程が五つだから、姿勢の正本も五本だ。

そして 1 本の JSON は単一の点ではなく、j0 を振った 20 点の弧を持っている。土台の回転（j0）でブロックの横位置に合わせられるので、工程ごとに 1 本あれば横方向の位置変化には対応できる。残りの 248 本は、この 5 本を選ぶまでの試行錯誤の履歴だった。

ただし、これは「5 本で固定」という意味ではない。横方向は弧でカバーできるが、奥行き（Z）方向に位置が変われば、今の弧 1 本では届かない。実機が動けば新しい sweep データが生成され、その中から採用に格上げされるものが出てくる。採用版はこれから増え、差し替わる。pose_id は「5 本を区別するため」ではなく、「増えていく姿勢を追い続けるため」に要る。

pose_id を三つの経路で通す

姿勢データ全部に pose_id を振る。しかも、これから生成される分にも自動で入る仕組みにしないと、次の試行で追えない新規がすぐに生まれてしまう。三つの経路で進めた。

ひとつ目、採用版 5 本への付与。ファイル名がすでに系統を表しているので、{phase}_{condition}_DAY{NNN}_01 形式で振った。

二つ目、既存 148 本への遡及付与。adopted の 5 本は付与済みなのでスキップし、classified 72 本と未分類 76 本に振った。生成時刻がないのでファイルの mtime を代用し、base_angles が一致する姿勢には同じ pose_id を共有させた。結果、148 本は実質 18 個の姿勢に集約された。後で adopted を含めた全件を数え直すと、153 本すべてに pose_id が入り、ユニークな姿勢は 23 種だった。

この遡及付与は、元データの汚れも炙り出した。あるファイルは、ファイル名が place_withblock、JSON 内の phase が place、base_angles はかつて approach_place として記録された姿勢、という三重の表記不整合を抱えていた。pose_id は base_angles（実体）ベースで振っているので、その姿勢が最初に何と呼ばれたかを反映する。これはバグではなく、表記がばらついていた過去の記録を、実体で束ね直した結果だった。pose_id 体系が機能している証拠でもある。

三つ目、生成時の自動付与。これが本丸になる。sweep を吐く生成スクリプトに pose_id を埋め込んでおけば、以後の新規はすべて最初から pose_id 付きで生まれる。

sweep を生成する Python は 4 本あったが、参照を確認すると現役は本体 j0_sweep_scan.py の 1 本だけで、残り 3 本（simple / 20_scan / 20_scan_fixed）は参照ゼロで凍結していた。本体さえ直せば、本体を import している run_pick_place / vlm_framing_test / place_j0_sweep_20_scan の経路すべてに効く。旧 3 本は archive に退避して working ディレクトリから外した。

本体への改修は次の通り。datetime を import し、スイープ開始前にループ外スコープで pose_id を 1 回だけ確定する。異常終了パスの再書き出しでも同一値が使われる必要があるので、ループより外で固定するのが要点になる。

# pose_id はスイープ開始前に1回だけ確定する。異常終了パスでの再書き出しでも
# 同一値が使われる必要があるため、必ずループ外スコープで固定する（点ごとに
# datetime.now() を呼ぶと秒がズレて pose_id が動いてしまう）。
pose_id = "{}_{}".format(phase, datetime.now().strftime("%Y%m%d_%H%M%S"))
print("[INIT] pose_id = {}".format(pose_id))

そして output_data の phase 直後に 1 キー追加する。

output_data = {
    "phase": phase,
    "pose_id": pose_id,      # ← 追加（phase の直後）
    "detect_target": PHASE_DETECT_TARGET[phase],
    ...
}

この改修は実機が import するファイルに触れるので、外部レビューを通した。差分は +8 行（import 1 + pose_id ブロック 6 + output_data 1）。検算では py_compile 通過、5 回連続書き出しでも pose_id が一意（pick_20260626_123938）であることを確認した。

実機は JSON を直接読んでいない

台帳に pose_id を書く処理に入る前に、実機がどうやって姿勢を取得しているかを確認した。ここで構造がひとつ見えた。

run_pick_place.py は採用版 JSON を直接読んでいない。j0_lookup(phase, target_xz) という別モジュールに phase 名（"pick" など）を渡して、lookup の側が JSON を選び、姿勢を返している。実機本体はカウンターで「pick の姿勢をくれ」と注文するだけで、棚（JSON）を直接触らない。棚から取ってくるのは j0_lookup の役目だ。

この j0_lookup は単なる仲介ではなく、ちゃんと仕事をしていた。phase から採用版 JSON を 1 本に解決し、検出に成功した点だけを抽出し、弧の X 座標が単調かを検算し、ブロックの X 座標から弧上の 1 点を補間して選ぶ。「検出座標から安全に 1 姿勢を選ぶ」計算の本体がここにある。

だから pose_id を台帳に通すルートはこうなる。j0_lookup が JSON を開くときに pose_id も読み、戻り値に載せる。run_pick_place がそれを受け取って保持し、台帳に書く。j0_lookup の戻り値にはすでに source_file（参照した JSON 名）が入っていたので、その隣に pose_id を足すだけで済んだ。

そして lookup の対象を確認すると、place で実機が引くのは place_withblock の一本だった。place_noblock は検出が全失敗（0/20）のため XZ→j0 の対応を持てず、lookup 側で明示的に ValueError として弾かれる。「置く姿勢の実測はどれを使うか」という曖昧さは、ここで解消した。

台帳へ通す

j0_lookup と run_pick_place の二つに手を入れた。

j0_lookup 側は、_load_arc の戻り値を 3 要素から 4 要素にして pose_id を返すようにし、j0_lookup の result dict に pose_id を source_file の直後に追加した。

run_pick_place 側は三つ。LEDGER_COLS を 33 列から 50 列にして v3 ヘッダと完全一致させる。stage4_pick 直後に pick の pose_id と直近 move の post_actual（到達時の実測角）を log_record に保持する。stage8_place の戻り受けを ok, _ から ok, r_place に変え、place 側も同様に保持する。最後に _append_ledger の row に 14 列を書き込む。

新規に追加した 14 列の中身はこうなる。pick_pose_id / place_pose_id は保持した pose_id（取れなければ空文字）。pick_used_j0..j5 / place_used_j0..j5 は到達時の post_actual（小数 2 桁、None なら空欄）。align 系 3 列はコード内に出所がないので空欄固定とした。

ここで列名について、自分の指示ミスを実装側が正している。当初の指示で「既存の final_j1..j6 に合わせて 1 始まり（j1..j6）」と書いたが、v3 ヘッダはすでに pick_used_j0..j5（0 始まり）で確定していた。実装は「v3 ヘッダと順序一致」を優先し、0 始まりで通した。列名が食い違えば書き込みが壊れるので、これは正しい訂正になる。

実測角は新規に測っていない。post_actual は各 _do_move 内ですでに取得されて log_record に入っている。それを台帳に流すだけで、新規の get_angles も daemon I/O も増やしていない。送角・グリッパー・検出・stage 制御フローには触れていない。

検算結果は次の通り。

len(LEDGER_COLS) == 50                              ✅ 50
LEDGER_COLS == v3 ヘッダ（順序完全一致）            ✅
j0_lookup の result に pose_id（source_file 直後）  ✅ 4 phase 全件
place_noblock は ValueError（既存挙動）             ✅
単体テストで row のキー数 50                        ✅
pick_used_j0..j5 小数2桁                            ✅ -36.03 等
place_used_j0..j5 小数2桁                           ✅ 40.60 等
align_j0_final / align_red_x_mm / align_converged   ✅ 空欄固定
pose_used が None（pick or place 失敗）            ✅ pose_id="" + 6 軸全空
pose_id が None（旧 JSON 由来）                    ✅ pose_id=""

レビューを通す

姿勢決定に関わるコードは外部の目を通す決まりにしている。同じ書き手が書いて同じ書き手が確認すると、同じ盲点を持つからだ。pose_id 自動付与と、台帳連携の二回、コードレビューに出した。

台帳連携のレビュー結果を全量で残す。

レビュー結果：❌ブロッカーなし。実機前dry-runに進んでOK。

A1 ✅ _load_arc() 4要素化の受けは現行 j0_lookup.py 内で全修正済み。3要素受け残りなし。
A2 ✅ PICK/PLACE とも stage_lookup_and_move() 直後の moves[-1].post_actual を取得。別stage混入なし。
A3 ✅ place は "place_withblock" を lookup。place_noblock は ValueError 維持。
A4 ✅ LEDGER_COLS は50列。実物 pick_place_ledger_v3.csv ヘッダとも順序込み一致。row キー順も一致。
B1 ✅ 差分上、送角・グリッパー・検出・stage制御フロー変更なし。
B2 ✅ ok,_ → ok,r_place は後続の pose保存用途のみで、制御分岐影響なし。
B3 ✅ 既存 row フィールド、writeheader 条件、LEDGER_CSV_PATH=v3 問題なし。
C1 ✅ 新規 daemon I/O なし。post_actual は既存 _do_move() 内取得の再利用。
C2 ✅ pose_id 欠落は ""、post_actual=None は空欄。クラッシュしない。
C3 ✅ 補間ロジック・JOINT_LIMITS 検査は無傷。
D1 ✅ send_coords() 使用なし。
D2 ✅ release_all_servos() 使用なし。
D3 ✅ pose_id は JSON の実値のみ。推測値・ダミーなし。

検証 py_compile PASS、j0_lookup --self-check --phase all PASS。
4 phaseすべて採用点20個で max|Δj0|=0 でした。

--self-check で 4 phase すべて max|Δj0|=0 が出たのが効いている。pose_id を足したことで補間ロジックが 1 ミリも変わっていないことの実証になる。

レビューの軽微な指摘は二つ。j0_lookup の戻り値 docstring に pose_id の記載が漏れていたこと、構造化ログ _record_lookup に pose_id が入っていなかったこと。どちらも 1 行追加で対応した。docstring と実装が食い違っていると、後で見返すときに混乱する。全量ログにも pose_id を残しておけば、台帳とログの両方で突き合わせができる。

整理は追跡可能性の入口だった

一日の終わりに残ったものを並べると、流れがはっきりする。中間 CSV の整理から始まり、台帳が古い方を指していた発見を経て、姿勢に背番号を振り、実機の試行とツインの姿勢を一本の鍵で結ぶ基盤を作った。254 本の JSON は実質 23 個の姿勢に束ねられ、これから生成される分にも自動で pose_id が刻まれる。

ここで作ったのは、次の実機 run で pose_id を残すための配線であり、pose_id 付きの台帳行はまだ実機で確認していない。本当の目的——同じ pose_id で姿勢・予測・実測を並べ、ツインと実機のずれを測る比較テーブル——は、その実データが溜まってからだ。姿勢に背番号が付いたことで、次の run からは「どの姿勢で動き、ツインではどこに来るはずで、実機ではどこに来たのか」を同じ鍵で辿れる。

整理したかったのは CSV だった。実際に必要だったのは、姿勢と結果を結ぶ鍵だった。

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