LOG254R2R2R2026/6/25

mechArm 270 Pi 実機データを土台にデジタルツインを作る Log#05 | PLACEの姿勢面を、点から半周へ広げる

FK（順運動学）テーブルは、関節角を入れると爪先がどこに来るかを引く対応表だ。これを Unity のデジタルツイン内でロボットを実際に動かして作っている。一般の運動学計算では再現できない、グリッパー先端の Brep が実際に来る位置をツインから採るのがこのプロジェクトのやり方になる。

これまで PICK（掴む）と A_PICK（掴みに行く接近）の姿勢面は、マット全域・左右対称まで充填してあった。一方で PLACE（置く）と A_PLACE（置きに行く接近）は手薄だった。ビューア上で PLACE は 0 点、A_PLACE は 3,579 点。しかもその 3,579 点は、後で分かるが PLACE の姿勢ではなかった。この回は PLACE / A_PLACE を、PICK / A_PICK に近い厚みの面へ引き上げる作業の記録になる。最終的に埋まったのは奥半周で、手前は空いたまま残った。

実装そのものより、判断が何度も曲がった過程をそのまま残す。途中で何度も間違え、測って戻し、また間違えた。その訂正の連なりが、このテーブルが今の形になった理由だからだ。

始点：PLACE は本当に空なのか

最初の問いは単純だった。PLACE と A_PLACE の姿勢面を埋めたい。ならば、まず現状を数で確かめる。引き継ぎの指示は「いきなり格子を切らず、まず読み取りで現状を出す」だった。

採用版の姿勢 JSON から、place 系の base 姿勢を読み出した。3 種類あった。

place_noblock   base [j0,j1,j2,j3,j4,j5] = [42.45, 68.50, 7.47, 2.37, -81.73, -33.04]  sweep 20点 / detect_ok 0/20
place_withblock base [j0,j1,j2,j3,j4,j5] = [40.60, 59.15, 12.83, -1.40, -78.04, -29.53]  sweep 20点 / detect_ok 20/20
approach_place  base [j0,j1,j2,j3,j4,j5] = [38.40, 57.39, 17.49, 0.08, -89.20, -29.44]   sweep 20点 / detect_ok 20/20

この 3 つの base 姿勢を、現行 FK テーブル（当時 799,529 点）と関節角で照合した。j1〜j5 を ±0.5° から ±5° まで緩めても、3 系統すべて 0 件。place 系の base 姿勢の周辺は、テーブルに 1 点も無かった。

ところがビューアの A_PLACE ボタンには 3,579 点が表示される。0 と 3,579 が同じ場所について並んだ。どちらかが間違っているか、数え方が違う。ここで「薄いから構造的に無い」と即断するのは、前回の失敗の再演になる。前のシリーズで「右端は届かない」「構造的に不能」と二度書いて、二度とも実測で覆された。だから今回は、3,579 が本物か、どの姿勢の周りにあるのかまで割ってから進めることにした。

3,579 点の正体

3,579 点を現行テーブルで最近傍検索した。

最近傍距離: min=0.03 / median=0.47 / max=0.83 mm
3,579 全件が <1mm でテーブルに実在

点は実在した。関節照合が 0 だったのは、3,579 が ee 座標で抽出されたサブセットで、照合を関節角でやっていたからだ。定規が違っていた。

ただし中身を見ると、別の問題が出た。3,579 点の最近傍関節を出して、現行の approach_place base と並べる。

軸   3,579の最近傍(min/median/max)     approach_place base   差(median-base)
j0   -10.00 / +7.00 / +23.00            +38.40                -31.40
j1   +65.91 / +90.74 / +122.74          +57.39                +33.35
j2   -63.33 / -39.33 / -14.14           +17.49                -56.82
j3   -22.00 / +3.60  / +19.60           +0.08                 +3.52
j4   -80.11 / -66.27 / -34.27           -89.20                +22.93
j5   -32.87 / -30.00 / -29.44           -29.44                -0.56

j0/j1/j2 で 30〜57° ずれている。3,579 点の中心は、approach_place の姿勢ではなかった。j3=3.6・j5=−30 が A_PICK base と一致することから、これは A_PICK 系の点群のうち、たまたま箱の天面付近の高さに来たものだった。ビューアのボタンはラベルと中身がずれていた。「A_PLACE」と書いてあるが、中身は A_PICK 由来だった。

つまり place 系の姿勢としての面は、本当にゼロだった。3,579 は本物だが、別の姿勢の点。place の姿勢で箱に向かう面を作るには、新しくスキャンするしかない。ここまでで「スキャンが必要」が、印象でなく数で確定した。

どの関節が高さを動かすか

面を作る前に、どの関節を振れば何が動くかを知る必要がある。place の base 姿勢から関節を 1 軸ずつ動かしたときの ee 変化を、テーブル内の近傍差分で出そうとした。だが place base の周辺はテーブルに無いので、差分が取れない。「実スキャンでしか出ない」と返ってきた。

ならば測る。withblock base から、各軸を独立に振る十字型の探り格子を組んだ。j0 を 5 点、j1 を 8 点、j2 を 8 点、中心 1 点。合計 22 点。

ここで最初の躓きがあった。22 点をスキャンしても、出力ファイルが書き出されない。Unity のログは姿勢を回しているのに、最後の保存ステップに到達しない。何度 Play しても、毎回ちょうど同じ所で止まった。

原因はスキャナーのコードにあった。

private void OnRobotDone()
{
    _doneCount++;
    if (_doneCount >= RobotCount)        // RobotCount = 20
        StartCoroutine(WriteCSVAndQuit());
}

保存は「20 体のロボットすべてが完了したら」走る。22 点を 20 体に配ると、chunkSize = ceil(22/20) = 2。最初の 11 体に 2 点ずつで 22 点が尽き、残り 9 体は起動されない。完了カウントは 11 で止まり、20 に届かず、保存が永遠に呼ばれない。

点数が 20 の倍数でないと保存に届かない。これは闇雲に Play を繰り返しても分からず、コードを読んで初めて見えた。点数を 40（20 の倍数）に増やし、範囲は同じまま刻みを細かくして組み直した。今度は 20 体すべてが起動し、保存まで到達した。

40 点の ee を読むと、感度がきれいに割れた。

j0 列: ee_x が動く（横）。ee_z も連動（奥行き）。ee_y はほぼ不動。約 -3.4 mm/deg
j1 列: ee_y が動く（高さ）。x/z はほぼ不動。約 -4.9 mm/deg ← 高さ専任
j2 列: ee_y が動く（高さ）。x/z に弱く干渉。約 -3.4 mm/deg

高さを動かすのは j1。しかも横と奥行きをほとんど触らずに高さだけ動く。面を張るのに都合がいい。j0（横位置）× j1（高さ）の 2 軸で面を作る方針が、ここで決まった。

面を張る、ただし実需に絞りすぎた

place 系の base は 3 つあるが、place_noblock は採用版の sweep で detect_ok が 0/20（実機で一度も検出に成功していない）。FK 面としては作れるが、面生成の主対象は detect_ok 20/20 の place_withblock と approach_place の 2 つにした。

最初の面格子は、置き先の箱の位置（台帳の box_x が −208mm 付近に集中）に合わせて、j0 を狭い帯 [+41, +49] に切った。withblock と approach の 2 base で各 80 点。スキャンして統合した。

ここで方針の取り違えがあった。box の実需が −208 付近に集中しているからと、その帯だけに面を絞った。だが PICK / A_PICK は実需だけでなくマット全域・左右対称まで充填してある。PLACE だけ実需に絞るのは一貫していない。指摘を受けて、これは中途半端だと分かった。実需が片側でも、面としては全域を持っておくべきだった。

それでも、この 80 点 × 2 の作業から得たものはあった。統合の前に 6 軸キーの衝突を確認したところ、衝突は 0。place 160 点はすべて新規キーで、テーブルに既にある「同じ関節キーで ee が違う」種類の傷を増やさないことが確認できた。ee は既存と重なる（箱の同じ位置に PICK 系と PLACE 系の別姿勢が来るのは物理的に当然だ）が、関節キーは別物だった。

ここで何度も間違えた

面化に進む前に、ビューアへの反映で立て続けに間違えた。記録として残す。

ひとつめ。Unity の出力パスについて、「スキャナーが頭に Assets/Data/ を足すから、欄にはファイル名だけ書く」と説明した。これは誤りだった。実際のスキャナーは欄の文字列をそのまま使う。過去に成功したスキャンは出力欄に Assets/Data/... と書いていた。実機の画面が正しく、こちらの説明が間違っていた。前提を実物で確かめずに、推測を断定で語った。

ふたつめ。ビューアに place の面を反映するとき、新しいボタンを 2 つ作った。だが「ボタンを増やすな・既存ボタンに合算しろ」というルールがあった。違反だった。空いていた PLACE 枠に入れるべき点を、別ボタンに分けてしまった。撤去して、既存の PLACE 枠に入れ直した。

みっつめ。世代の違う点（古い設計の j5=0 の点群と、現行の j5≈−30 の点群）を見分けるために、片方をオレンジで色分けした。だがビューアはモノクロが基本だ。色を持ち込んだのは設計から外れていた。色をやめ、他のボタンと同じ単色の黒点に戻した。

よっつめ。PLACE は箱の天面（高さ約 43mm）に置くので、床近（ee_y < 40mm）の点は要らない。にもかかわらず、PLACE 枠に床近フィルタの特別扱いを入れて、しかもそれが想定通り動かず表示がちぐはぐになった。特別扱いを全部はがし、PLACE も他のボタンと同じ普通の挙動に戻した。

四つとも、共通しているのは「先回りして特別なことをした」結果だった。実物を確かめず推測で語り、ルールを忘れてボタンを増やし、基本を外して色を足し、要らないフィルタを足した。どれも、戻して普通にすることで直った。

奥半周は弧を描く

面化の範囲を決めるため、勘で大きな格子を切る前に、j0 を全振りする薄い列を測った。withblock と approach の各 base で、j1/j2 を固定したまま j0 だけ −90° から +90° まで 40 点。

結果は明快だった。place 姿勢で j0 を全振りすると、ee は半径約 280mm の円弧を描く。

withblock: 半径 約283mm。j0=-90 で ee=(+283,+7)、j0=0付近で (0,+283)、j0=+90 で (-283,-7)
approach:  半径 約273mm。ほぼ同じ弧
ee_y は終始一定（j0 は高さを変えない）。破綻・不連続なし。SDK 範囲内で全域到達

j0 を振ると爪先がマットの奥側〜横を半周なめる。手前（Z−）にはほぼ届かない。これは base の姿勢（爪が奥〜横を向いている）から来る構造で、j0 だけでは手前に回り込めない。だが奥半周は ±90° で素直に全部使える。ここで本格的な面格子の j0 範囲が ±90° に決まった。

厚く埋める

PICK / A_PICK は数万点の厚みで全面を持っている。PLACE / A_PLACE も同じ密度にする。j0 × j1 × j2 の 3 軸格子を組んだ。

j0: [-90, +90] を 120値（≈1.5°刻み・対称）
j1: base±20° を 17値（高さの段）
j2: base±15° を 13値
j3/j4/j5: base固定
120 × 17 × 13 = 26,520点（20の倍数・保存到達）

withblock と approach の 2 base で、各 26,520 点。1 ファイル約 30 分のスキャンを 2 回。出力を統計で確認した。

withblock 26,520点:
  ee_x: -300.4 / -9.0 / +300.4 mm
  ee_y: -22.1 / +96.4 / +252.4 mm
  ee_z: -11.0 / +191.4 / +300.4 mm
  天面帯(ee_y 43-160): 14,880点  箱貫通(ee_y<43): 6,240点
  象限: 左奥13,034 / 右奥13,237 / 左手前0 / 右手前249

approach 26,520点:
  ee_x: -287.4 / -3.8 / +287.4 mm
  ee_y: -13.2 / +120.1 / +267.1 mm
  ee_z: -5.8 / +185.5 / +287.4 mm
  天面帯(ee_y 43-160): 15,120点  箱貫通(ee_y<43): 3,480点
  象限: 左奥13,255 / 右奥13,044 / 左手前0 / 右手前221

奥側〜横は左右対称にびっしり充填された。ee_x は ±300 / ±287 まで届く。一方、手前（Z−）は両 base ともほぼゼロ。これは j0×j1×j2 を全振りした 26,520 点 × 2 で実測した結果で、この 2 つの base 姿勢では手前に出ないことが分かった。手前を埋めるには別の base 姿勢が要る。これは持ち越しになる。

統合とビューア

2 base の 53,040 点を、現行テーブルに統合した。統合前のゲートで 6 軸キーの衝突を確認する。

withblock ∩ テーブル: 0
approach  ∩ テーブル: 0
withblock ∩ approach: 0
ee不一致(同キー異ee の傷): 0
→ 全 53,040 点が新規キー

衝突ゼロ。既存の傷を増やさず、全点を新規追加できた。テーブルは退避（コピー）してから統合し、統合後に Mac と Unity の両側で md5 一致を確認した。

テーブル行数: 799,689 → 852,729（+53,040）
md5: 4a2f433db3910840011652c1c31fefc5（Mac=Unity一致）
無印512K: 2cefbcbc12d03883bad179a9d27e1f6c（不変）
退避: archive/fk_scan_motion_based_v2_pre_DAY254_place_full_merge.csv

この回を通したテーブルの遷移は次の通り。

段階	行数	md5
開始時	799,529	ec30e2fce34189f48130302a6cbf69c7
place面160点(face80)統合後	799,689	372e2b23113c9811a7c609c8a877f331
place全面53,040点統合後	852,729	4a2f433db3910840011652c1c31fefc5

ビューアには、新しいボタンを作らず、既存の PLACE / A_PLACE 枠に合算した。色は付けず、他のボタンと同じ単色の黒点にした。

ボタン	開始	最終
PLACE	0	42,782
A_PLACE	3,579	30,099

PLACE の 42,782 は、今回の全面スキャン分（withblock 26,520）だけでなく、途中で入れた face80 系の点と、表示抽出条件に入る既存点を合算した数になる。A_PLACE の 30,099 は、既存の 3,579（A_PICK 由来の旧データ）に approach 全面 26,520 を足したものだ。

開くと、奥半周の弧が左右対称に黒く埋まる。手前は空いたまま。これが今回の到達点になる。

残したもの

全面と言っても、まだ半周だ。手前（Z−）は今回の 2 base では埋まらなかった。全体として届かないと決めるのではなく、別の base 姿勢で確認する対象として残す（前のシリーズで「構造的に不能」と即断して二度覆された手前、ここは断定しない）。天面帯にも刻み由来の薄い穴が残る。place_noblock の base は、採用版の sweep で 1 点も検出に成功していない（FK 面としては問題ないが、実機で使えるかは別）。テーブルには以前から、同じ関節キーで ee が食い違う重複が 2,400 件ほど残っていて、これは今回増やさなかったが、出どころを特定して作り直すべき独立の課題として残っている。そして、この FK 面はあくまでツイン上の被覆であって、実際の通し動作（採用版 JSON を読む別系統）にこの面を繋ぐのは、まだ先になる。

今回やったのは、置く姿勢の地図を、点から弧へ、弧から奥半周へ広げたことだ。実機データを土台にしたツインの上で、爪先が実際に来る位置を採り続けている。地図は実物より先に決めない。薄い領域を見ても、構造のせいにする前に振って測る。間違えたら戻して普通にする。この回はその繰り返しだった。

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