mechArm 270 Pi 実機データを土台にデジタルツインを作る Log#03 | 現行PICKはFKテーブルを読んでいなかった

PICK の被覆を、弧から面に広げるつもりで作業に入った。

前回、FK テーブルの被覆を数えた。全点を重ねれば面に見えるが、運用している姿勢ごとに見ると、実体は j0 スイープが作る細い弧だった。ならば次は、その弧を増やして面にする。引き継ぎ書のスコープもそうなっていた。

だが、拡充する前に一つ確認していなかった。現行の PICK は、本当にその FK テーブルを読んでいるのか。

コードを辿ると、前提は崩れた。現行の通し Pick&Place は、fk_scan_motion_based_v2.csv も無印 512K の fk_scan_motion_based.csv も読んでいなかった。読んでいたのは j0_sweep_adopted の JSON だった。つまり、FK テーブルを面で増やしても、ライブの PICK は変わらない。

この記録は、間違った対象を拡充しかけたところから、現行 PICK の参照元を特定し、FK 側には被覆ビューア用の面を足すところまでを、省略せずに残す。外した推測も、訂正した過程も消さない。

拡充する前に、読んでいるファイルを確かめる

引き継ぎ書は「APPROACH_PICK / PICK の被覆を弧から面にする」ことをスコープに置いていた。そのために最初に潰すべき一点があった。被覆を測る対象テーブルが、記録ごとに食い違っていた。

arm_kinematics.md と handoff_template.md（どちらも LOG222 更新）は「現行は fk_scan_motion_based_v2.csv（710,704点）、無印 512K は退避温存・変更禁止」と書いている。一方、前回の被覆解析は無印 512K を対象にしていた。memory も「無印＝active」と記録していた。

LOG222 の文書自身に「v2 が不調なら参照先を無印へ戻してロールバック可」とある。v2 を無印へロールバックして、その変更が文書に反映されないまま memory 側だけ「無印＝active」を拾った、という筋はあり得る。ただしこれは推測なので、勘で確定させない。実コードが開くファイル名を直接見る。誤ると、ライブでないテーブルを拡充して作業が丸ごと無駄になる。

読み取りのみ・実機を動かさないコマンドで確認する。

cd ~/r2r2r-scripts && grep -rn "fk_scan_motion_based" . --include=*.py
ls -la ~/r2r2r-scripts/fk_scan_motion_based*.csv
wc -l ~/r2r2r-scripts/fk_scan_motion_based*.csv

最初の実行で、肝心の grep が zsh のグロブ展開で落ちた。

zsh: no matches found: --include=*.py
-rw-r--r--  1 shimazueiichi  staff  52124841  6月  5 20:02 /Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based_v2.csv
-rw-r--r--@ 1 shimazueiichi  staff  37507933  4月 25 15:10 /Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based.csv
  710705 /Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based_v2.csv
  512088 /Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based.csv
 1222793 total

--include=*.py の *.py をコマンド実行前に zsh が展開しようとして、カレント直下に該当が無く "no matches" でコマンドごと中断していた。クォートで止める。

grep -rn "fk_scan_motion_based" ~/r2r2r-scripts --include='*.py'

ls と wc -l は通っていて、両ファイルが文書どおりのサイズで存在することは分かった。fk_scan_motion_based_v2.csv = 710,705 行（ヘッダ1＋710,704点）、最終更新 6/5。無印 = 512,088 行（ヘッダ1＋512,087点）、最終更新 4/25。

更新日は判断材料にしない。CSV の mtime は「最後に書き込んだ日」であって「最後に読まれた日」ではない。loader がどちらを open しているかは mtime からは出ない。grep が文字列として開いているファイル名を直接見せる。それが唯一の確定根拠になる。

無印か v2 か、ではなかった

クォートで止めた grep が通って、答えが出た。ただし単一の答えではなく、分裂が出た。CSV_PATH = ... で読み込んでいる行を、どちらのテーブルを開いているかで仕分けると、テーブルは2系統に割れていた。

トップ階層のスクリプトは全部無印、place_modules の番号付きモジュールは全部 v2。memory が「無印＝active」と言い、LOG222 文書が「v2＝現行」と言って食い違っていた正体はこれで、両方とも正しい。読んでいるスクリプトが違うだけだった。

memory が「無印」を拾った理由も見える。11_fk_load_test.py:4 のコメントが fk_scan_motion_based_v2.csv（512,087 点） と書いている。ファイル名は v2 なのに点数は無印の数（512,087）。この食い違ったコメントが混乱の発生源に見える。ただし wc -l は v2=710,705 行・無印=512,088 行で、中身は名前どおり。コメントが古いだけで、ファイルの実体は健全だった。コメントは信用せず、CSV_PATH 文字列と実行数だけ信じる。

ただ、この時点での見立て、「現行通しは v2 が濃厚」も、次で外れることになる。問題は、無印か v2 か、ではなかった。

現行 PICK は FK 最近傍 lookup をしていない

どちらがライブの PICK lookup か。決め手は「現行の v3 台帳を書いている通し実行が、どの descend を通るか」。LOG250 記事の台帳行は run_0001_20260618_144228 形式＝run_pick_place 系の出力だった。そこで通し実行のオーケストレータを直接見る。

sed -n '1,80p' ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules/run_pick_place.py
grep -n "import\|approach\|descend\|lookup\|CSV_PATH\|\.csv" ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules/run_pick_place.py
grep -n "CSV_PATH" ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules/15_descend_move.py

ここで、想定と違う層が出てきた。run_pick_place.py の冒頭が決定的だった。これは run_0001_20260618_144228 形式＝LOG250 の台帳行を出した現行の通し本体で、その lookup 方式が冒頭に書いてある。赤ブロックを上カメラで検出し、j0 対応表で各 phase の j0 を引き、単発 send_angles で基準姿勢へ移動する。

実体は stage_lookup_and_move（964行〜）が r = j0_lookup(phase, target_xz) を呼び、approach_pick も pick も全部これを通る（1108・1117行）。現行の PICK lookup は j0_lookup。j0 だけを (phase, x) で引き、j1〜j5 は phase ごとの固定基準姿勢。6軸の最近傍を fk_scan_motion_based から引く方式ではない。run_pick_place.py が fk_scan_motion_based に触れているのは21行の「書き換えない」という禁止ルールのコメントだけで、ロードしていない。

前段の見立ては外れていた。15_descend_move.py（v2 を読む）と 14_approach_move.py（v2）は、run_pick_place.py の import に入っていない。import は j0_lookup / j0_sweep_scan / detect_positions_once / visual_servo_pick だけ。しかも 14_ / 15_ は数字始まりで import 不可。つまりライブ経路ではない。旧モジュラ設計の残骸だった。トップ階層の無印読み（r2r2r_loop.py 等）も旧世代。

3世代が同居しているだけで、現行通しの実体は j0_lookup 一本だった。問題は、無印か v2 か、ではなかった。現行 PICK は、そのどちらも読んでいなかった。

読んでいたのは j0_sweep_adopted の JSON だった

j0_lookup が fk_scan から派生生成されているのか、独立の事前計算対応表なのか。ここで設計が分岐する。前者なら FK 拡充は意味を持つ。後者なら拡充対象が丸ごと変わる。j0_lookup.py の中身を見る。

F=$(find ~/r2r2r-scripts -name "j0_lookup.py" | head -1); echo "== $F =="; sed -n '1,140p' "$F"; echo "== data refs =="; grep -n "open(\|\.csv\|expanduser\|read_csv\|np.load\|load(\|base_angles\|対応表\|BASE\|table" "$F"

確定した事実は、現行 PICK は FK テーブルを一切使っていない、ということだった。

j0_lookup.py が読むのは logs/j0_sweep/j0_sweep_adopted/{phase}_*_adopted_*.json。fk_scan_motion_based（無印も v2 も）一切読まない。grep の data refs に .csv も expanduser も np.load も出ない。JSON だけ。

各 phase の採用版 JSON が持つのは2つ。base_angles（j1〜j5 を固定する基準姿勢・長さ6）と、sweep[]（(detect_xz_m, j0_cmd) の列＝j0 スイープの実測弧）。lookup の動作は、target の x だけで sweep の隣接2点を選び、内分して j0 を出す（267〜271行）。返すのは target_angles = [j0] + base_angles[1:]。z は使わない（B案・LOG236 で確定した方式）。

つまり現行 PICK の姿勢は、phase ごとに base_angles で j1〜j5 を固定し、j0 だけを実測スイープ弧から x マッチで決める。前回数えた「FK 点群の弧」は、この j0 スイープ弧そのものの EE 軌跡だった。弧が一本なのは当たり前で、j0 しか振っていないからだった。

採用版 JSON の実体を見る。

ls -la ~/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_adopted/
P=$(ls ~/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_adopted/pick_*_adopted_*.json | head -1); echo "== $P =="; python3 -c "import json,sys; d=json.load(open('$P')); print('base_angles=',d.get('base_angles')); s=d.get('sweep') or []; ok=[p for p in s if p.get('detect_ok') and p.get('detect_xz_m') and p.get('j0_cmd') is not None]; xs=[p['detect_xz_m'][0] for p in ok]; print('sweep_total=',len(s),'usable=',len(ok)); print('x_range=',(min(xs),max(xs)) if xs else None); print('j0_range=',(min(p['j0_cmd'] for p in ok),max(p['j0_cmd'] for p in ok)) if ok else None)"

== /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_adopted/pick_NA_20pt_adopted_DAY235_01.json ==
base_angles= [-36.03, 88.0, -21.44, 1.4, -70.48, -32.87]
sweep_total= 20 usable= 20
x_range= (-0.3056231994628906, 0.30662908935546873)
j0_range= (-85.0, 85.0)

phase ごとに採用版 JSON が1本ずつ（pick / approach_pick / approach_place / place_noblock / place_withblock）。pick の base_angles は1姿勢、sweep は20点、x レンジは ±306mm、j0 は ±85°。これが弧一本の正体だった。引き継ぎ書が「最重要」に挙げた「どの FK テーブルがライブか」は、問い自体が外れていた。正しい問いは「現行 PICK は FK テーブルではなく j0_sweep_adopted の JSON を引いている」だった。grep を3段まで掘って初めて出た。勘で v2 を拡充していたら、スキャンが丸ごと空振りになっていた。読まれていないファイルをいくら拡充しても、実機は1mmも変わらない。

足りないのは X ではなく Z だった

被覆を面にする前に、どこに足りないかを数で出す。現行台帳の実体を確定する。runs/pick_place_ledger_v3.csv（50KB・6/24更新）がライブで、v2 は archive 行きだった。memory の「v3＝canonical」が正しい。台帳の block_pick_x / block_pick_z（実際にブロックを置いた位置）の分布を出す。

python3 - <<'EOF'
import csv, collections
rows=list(csv.DictReader(open('/Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv')))
xs=[float(r['block_pick_x']) for r in rows if r.get('block_pick_x','').strip()]
zs=[float(r['block_pick_z']) for r in rows if r.get('block_pick_z','').strip()]
print('rows:',len(rows),' usable_x:',len(xs),' usable_z:',len(zs))
print('x_mm min/max:',round(min(xs),1),'/',round(max(xs),1))
print('z_mm min/max:',round(min(zs),1),'/',round(max(zs),1))
print('z_hist(50mm bin):',dict(sorted(collections.Counter(int(z//50)*50 for z in zs).items())))
print('x_hist(50mm bin):',dict(sorted(collections.Counter(int(x//50)*50 for x in xs).items())))
EOF

rows: 100  usable_x: 95  usable_z: 95
x_mm min/max: -110.1 / 274.2
z_mm min/max: 134.9 / 320.0
z_hist(50mm bin): {100: 2, 150: 19, 200: 36, 250: 29, 300: 9}
x_hist(50mm bin): {-150: 1, 0: 1, 50: 8, 100: 5, 150: 24, 200: 54, 250: 2}

ブロックが実際に置かれた範囲は z（奥行き）が 134.9〜320.0mm。50mm 刻みで 150/200/250 帯に84点が集中（19+36+29）。x は −110〜274mm で、現行の弧（±306mm）の内側に全部収まっている。x 方向は足りている。問題は z だった。z が 135〜320mm の約186mm に散っているのに、現行の pick base は z 固定の弧1本。1本の弧は z 上のほぼ一点しか通らない。残りの z 帯に来たブロックは、弧から z 方向に外れた位置で掴みにいくことになる。掴めない試行の相当数はここで説明がつく。

面にするには、複数の弧が要る

弧が1本なのは、base_angles で j1〜j5 を凍結して j0 だけ振っているから。1 base = 1 弧。これは構造的な話で、データが薄いのではなく、振っている軸が1本しかない。面にするには base を複数持つ。各 base が違う到達半径を持ち、それぞれで j0 を振れば、半径違いの弧が入れ子になって帯＝面になる。

半径を決めるのは腕の伸び、主に j1（肩ピッチ）と j2（肘）。現行の pick base [-36.03, 88.0, -21.44, 1.4, -70.48, -32.87] から、この2つを近い側・遠い側に振って伸縮させた姿勢を作る。j0 は触らない（j0 は弧の上を動く担当）。j1/j2 を変えると手首の向きがずれるので、爪先が床へ垂直に降りるよう j3/j4 は連れて動かす。姿勢は勘で作らない。base から j1/j2 を段階的にずらし、各候補が床まで降りるか（descend 後に ee_y が床高さに来るか）を確認して、降りたものだけ採る。これが「実在から採る」の中身で、arm_kinematics の拡充手順 step2 と同じ精神になる。

データと不可分な制約が一つある。現行の lookup は x だけでマッチして1 base の j1〜j5 を返す。弧が複数になると「どの弧か」を先に z で選ぶ段が要る。これが無いと、増やした弧をコードが選べない。さらに同じ x に複数 z が乗ると _assert_x_monotonic が例外を吐く（弧をまたいで単調性を検査すると折り返し扱いになる）。「PICK を面で増やす」は「lookup に z 選択を1段足す」とセットでしか成立しない。これは関節制御フローに効くので、その実装に進むときは codex/gemini レビュー必須ラインになる。

ここで作業の指示は明確だった。

PICKのデータを面で増やす方法だけ考えて。他には触らないで。

面の生成だけに集中する。EE 座標は Unity のデジタルツイン内でグリッパー先端 Brep が来る位置で、一般の運動学計算（ikpy）では再現できない。同じ関節角でも 300mm 以上ずれる。設計側が出せるのはスキャン入力＝関節角グリッドで、これは純粋な角度の組み合わせなので生成できる。Unity の FKScanner_MotionBased（MotionLog モード）が各行の ee を計算する。

最初に作ったグリッドは、各1,225点だった。pick base の j1〜j5 を固定し、j0 を −90〜+90（25点）、j1 を ±15°（7点）、j2 を ±15°（7点）で振る。j0 全振りで x 幅を弧で埋め、j1/j2 格子で reach を伸縮させて弧を z 方向にずらす。approach も同じ振り方。

Unity 側では一度、ファイルが見つからないエラーで止まった。原因はファイル名ではなく、Inspector の Motion Log Csv Path の末尾に入っていた空白だった。Python では正しいパスを見ていたため存在確認は通るが、Unity は ....csv を探していた。ファイル側ではなく、Inspector 値側に混入した不可視文字だった。ファイル側だけをテストして値側をテストしていなかったので、切り分けに時間を使った。

最初のグリッドの結果は、pick が床近 575点・うち赤枠内（ee_x 100〜250 / ee_z 180〜320）92点、approach が床近 200点・赤枠内 32点。弧1本のときに比べれば面の芽は出たが、赤枠の中はまだ薄かった。

赤枠に集中して、FK 側の面を増やす

最初のグリッドは j0 を −90〜+90 まで全振りしていた。赤枠は x の片側に寄っているので、全振りの3分の2は赤枠の外に飛んでいた。実測から、赤枠（x +95〜260）に届く j0 は PICK で −90〜−22、A_PICK で −60〜−22 と分かった。さらに、最初のグリッドでは赤枠床近の z が最大283mm までしか伸びず、台帳の z 上限320mm に届いていなかった。奥に届かせるには腕をもっと伸ばす＝j1 を高くする。

j0 を赤枠帯に絞り、j1 を上に伸ばし、j1/j2 の刻みを倍にした。

このスキャンも一度では通らなかった。Inspector で出力名だけ _v2_out に変え、入力欄を前のままにして Play してしまい、「v2 という名前のファイルに v1 の結果」が入る空振りを pick・approach それぞれで起こした。出力の行数（1,225 のままか 11,130 になったか）と j0 レンジ（−90〜+90 か −92〜−14 か）で検出して、入力欄を v2 に直して回し直した。

結果を数字で並べる。

v1: PICK 赤枠内 92 / A_PICK 32
v2: PICK 赤枠内 2,863 / A_PICK 2,065

v2 の高密度スキャン結果から、赤枠内（x 95〜260 / z 185〜330 / 床近 ee_y<40）の点だけを抜いて、既存の被覆ビューア（FK_density_4poses.html）の PICK / A_PICK に合算する。最初の改修では別ボタンを2つ足してしまい、作り直した。

何これ？なんで項目増えてんの？A_PICKとPICKに一緒に合成してよ。

別ソースとして並べるのではなく、既存ソースの中に合算して、そのボタンの点数が増える形にする。ボタンは増やさない。

PICK と A_PICK のボタンで「床近くだけ」を ON にすると、赤枠の中が弧一本のスカスカから埋まっている。ここで面にしたのは、FK ビューア上の被覆である。現行 PICK の制御面は、まだ弧のままだ。

FK テーブルは1本に統合した

ここまでで FK 関連のファイルは分裂していた。現行 v2 テーブル（710,704点）と、今回のスキャン出力2本（pick 11,130点・approach 9,408点）が別々にある。これを v2 テーブルに追記して1本にまとめる。新ファイルは作らない。

これはライブ PICK を変えるための統合ではない。被覆ビューアと将来の lookup 拡張のために、FK 側のデータを一本に戻す作業だ。現行 PICK は引き続き j0_sweep_adopted の JSON を読む。

列順（9列）と単位（ee は m）と行末（LF）が揃うことを確認し、現行 v2 を archive へ退避コピーしてから追記統合した。既存 v2 710,704点に PICK 11,130点・A_PICK 9,408点を重複なしで足し、統合後は731,242点。既存 v2 の行は1つも消えていない。Mac 側 ~/r2r2r-scripts/ と Unity 側 Assets/Data/ の fk_scan_motion_based_v2.csv は同一バイトに揃え、md5 dc339e2ef1ce388d874e644859e27854 で一致を確認した（片側だけ古いと lookup がずれる。LOG170 で97mm乖離の前例）。無印テーブルは 2cefbcbc12d03883bad179a9d27e1f6c で不変。スキャン出力2本は archive へ退避してから元を削除し、分裂は解消した。

ただし、現行 PICK にはまだ接続していない

この回で足したのは FK テーブル側の面であって、現行 PICK にはまだ接続していない。現行 PICK は j0_sweep_adopted の JSON を読むので、FK テーブルを面にしても、それだけでは挙動は変わらない。現行 PICK を面で動かすには、足す先を採用版 JSON 側に移し、複数 base の弧を持たせ、lookup に z 選択を1段加える必要がある。その lookup 変更は関節制御フローに効くので、進めるときは外部レビューを通す。

データを増やす前に、参照元を辿る。面を作る前に、いま読まれている弧を特定する。そうしないと、正しいデータを、間違った場所に足すことになる。読まれていないファイルをいくら拡充しても、実機は1mmも変わらない。

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