mechArm 270 Pi 実機データを土台にデジタルツインを作る Log#04 | Unity で作る FK テーブルを、マット全面・左右対称まで広げる

前回、現行の PICK が FK テーブルを読んでいないことを確かめ、FK 側には被覆ビューア用の面を足した。ただしそれは旧来の狭い枠（ee_x 95〜260 / ee_z 185〜330 mm）の中の面で、マットの全面ではなかった。この回は、その面をマット全面へ広げる。

途中で、自分の出した地図が間違っていることに気づかされた。「マットの右端はアームが届かない」「片側は構造的に埋まらない」と二度書いた。どちらも実機の可動とは関係のない、こちらの読み違いだった。指摘で止まり、振り直して、データに正された。この回は面を広げた手順と、その読み違いと訂正を、数字とやりとりのまま残す。

まず、マットのどこまで届くかを数える

面を広げると言っても、当てずっぽうで関節を振っても意味がない。マットの端に届く関節角を、勘ではなく既存のテーブルから逆引きして決める。物理マットは 620mm（X）× 450mm（Z）、アームベースはその左右中央・上端から 89mm の位置にある。

実機の可動範囲も勘で切らない。アームを動かしている当の SDK（pymycobot 4.0.5）から、関節リミットの定数を読み出した。実機・daemon は使わず、ライブラリの定義を参照しただけだ。

[Mac] python3 - <<'EOF'
import pymycobot, os
from pymycobot.robot_info import RobotLimit
lim = RobotLimit.robot_limit["MechArm270"]
print("pymycobot:", pymycobot.__version__)
print("angles_min:", lim["angles_min"])
print("angles_max:", lim["angles_max"])
print("coords_min:", lim["coords_min"])
print("coords_max:", lim["coords_max"])
EOF

出力は以下。出所は pymycobot/robot_info.py の RobotLimit.robot_limit["MechArm270"]。旧 MechArm（165/90/180/165/115/175）とは別値で、MechArm270 専用の定義だ。ここでの ee_x はマット左右方向、ee_z は奥行き方向として読む。

座標リミットも同じ辞書にあった。ee の xyz は ±272mm、ee_z は -86〜408.9mm。懸念が一つ浮く。マットの左右端は ±310mm、対して SDK の座標リミットは ±272mm。端の 38mm は座標指令では届かない。ただし関節角を直接送る経路（send_angles）なら突き抜けるかもしれない。この時点では保留とした。

この関節リミットを上限として、PICK 系・APPROACH_PICK 系それぞれの base 姿勢から、マットの各端に届く j0 / j1 / j2 を、統合済みの FK テーブル（この時点で 783,945 点）から逆引きした。j3 / j4 / j5 は base 固定のまま、j0 / j1 / j2 だけを振る。逆引きは関節角→爪先位置の対応がすでにテーブルにあるので、目標の ee に最も近い実在点を引くだけで済む。

「右端（X-）は届かない」と書いた

逆引きの結果を、こう読んだ。

PICK 系は、マット右端（ee_x のマイナス側）に向けて j0 を +60 まで振ると ee_x -292mm まで届いた。マット奥（ee_z のマイナス側）も -121mm まで届いた。左端（ee_x プラス側）は +316mm。左右と奥は埋まる。

APPROACH_PICK 系は様子が違った。逆引きでは、右端に届く姿勢が j1=122.74°、つまり SDK 上限 +120° を超える値に頼っていた。これを +120° でクランプすると、右端には届かなくなる。奥（ee_z=-89mm）も同様に j1>120° が必要で、SDK 内では届かない。

ここで、こう書いた。マット端の到達地図として残すつもりだった。

そして「埋まらない領域は grid の問題ではなく、SDK リミットと固定姿勢の構造的な帰結だ。grid を広げても蹴られる」と書いた。届かないこと自体が今回の成果物だ、とまで書いた。

これが間違いだった。

「左右同じ可動範囲がある」で止められた

スキャンを終えてビューアで被覆を見ると、APPROACH_PICK の点群は右側（X-）が薄く、-150mm あたりで切れているように見えた。それを見て「やはり右は届いていない」と報告した。地図のとおりだ、と。

ここで止められた。

右端が物理的に届かないは嘘だね。

返す言葉がなかった。同じマットの右端に、PICK 系は -292mm まで届いている。同じアーム、同じ作業半径で右が埋まっているのに、APPROACH_PICK だけ「右は物理的に届かない」と書いたのは、PICK の結果と矛盾している。作業半径を持ち出した説明そのものが破綻していた。撤回した。

だが撤回しきれていなかった。撤回した直後の文章で、まだ「APPROACH_PICK 右端は j1=120 クランプで構造的に不能」と書いていた。可動軸の話を、こちらが取り違えたまま続けていた。もう一度、はっきり止められた。

いやそんなことはない！ちゃんと調査した？少なくとも左右同じ可動範囲がアームにはあるよ。

これで腑に落ちた。j0 は ±160° で左右対称な軸だ。左（X+）に届くなら、j0 を反対側へ同じだけ振れば右（X-）にも同じだけ届く。APPROACH_PICK が右で切れていたのは、可動域でも SDK でもない。こちらが生成した grid の j0 範囲を、APPROACH_PICK だけ [-75, +30] と片側に寄せて切っていたからだった。PICK は [-92, +60] と右へ伸ばしたから届いた。APPROACH_PICK は私が右へ伸ばさなかっただけだ。

逆引きが「右端は j1=122.74° 頼み」と出したのも、既存テーブルにその姿勢しか入っていなかったからで、「その姿勢でしか届かない」とは別の話だった。j0 を右へ振った姿勢を grid に入れてスキャンすれば、普通に埋まるはずだった。「届かない地図」と呼んで構造のせいにしたのは、自分の grid 設計の不足を物理のせいにしただけだった。

数で確かめる

判断ではなく数字で確かめる、という原則（P008）に戻る。本当に右に実需があり、SDK 内で届くのかを、台帳と既存テーブルから逆引きし直した。

台帳（pick_place_ledger_v3、100 件）の block_pick_x（95 件記録）の分布。

min (右端 / X- 側) = -110.1 mm   ← 1 件のみ（外れ値）
2nd                = +44.9 mm
max (左端)         = +274.2 mm

ヒストグラム (50 mm bucket)
[-150,-100)   1   ← -110.1 の 1 件
[  +0, +50)   1
[ +50,+100)   8
[+100,+150)   5
[+150,+200)  24
[+200,+250)  54
[+250,+300)   2

現行テーブルの床近（ee_y<40）で、右端がどこまで届いているか。

PICK base   床近 20,899 行   ee_x min = -292.4 mm
APICK base  床近 22,024 行   ee_x min = -250.1 mm

事実が2つ浮いた。APPROACH_PICK の右端はビューアで「-150 で切れている」ように見えたが、実データでは -250.1mm まで届いていた。点が薄くてまばらに見えていただけで、未到達ではなかった。そして右端目標（-110.1mm）に届く近傍のうち、PICK は 92%、APPROACH_PICK は 90% が j1≤118°、つまり SDK 余裕内に存在した。SDK を厳守しても届く。「構造的に不能」は、ここでも数字に否定された。

地図に書いた「未到達」は、薄さを未到達と読み違えたものだった。整理するとこうなる。

左右対称に振り直す

やることは単純になった。左で埋まっている j0 範囲を、符号を反転して右にも同じだけ作る。j0 は左右対称な軸なので、左で届く範囲は右にも届く。j1 / j2 / j3 / j4 / j5 はそのまま、j0 だけを対称化する。

grid のレンジはこうした。SDK は厳守する（j1≤+120°、122.74° は入れない）。

PICK   : j0 [-92, +92]   1.5°刻み  /  j1 [83, 120] 2.5°  /  j2 [-45, -5] 2.5°
         j3=1.4  j4=-70.48  j5=-32.87  (base 固定)
APICK  : j0 [-75, +75]   1.5°刻み  /  j1 [82.74, 120] 2.5°  /  j2 [-65, -19.5] 2.5°
         j3=3.6  j4=-70.11  j5=-30.0  (base 固定)

行数は PICK 33,728 点、APPROACH_PICK 32,320 点、合計 66,048 点。左半分は既存テーブルと重複するので、統合時に重複除去で落ち、実際に増えるのは右側の拡張分だけになる。

Mac 側（~/r2r2r-scripts/）と Unity 側（Assets/Data/）の両方に同じ入力 CSV を置き、md5 一致を確認した（片側だけ古いと lookup がずれる前例があるため）。Unity の FKScanner_MotionBased を MotionLog モードで実行し、各姿勢の爪先位置をスキャンして出力させた。

スキャン出力の床近（ee_y<40）で、左右の対称性を測る。

PICK v5   全 33,728 / 床近 25,011
  ee_x  左端(max, X+) = +316.2 mm   右端(min, X-) = -316.2 mm   差 = -0.0 mm
  X+ 側 床近 12,622 点  /  X- 側 床近 12,389 点   比 1.02
  j0 範囲(床近) -92.0 〜 +92.0

APICK v5  全 32,320 / 床近 15,425
  ee_x  左端(max, X+) = +319.3 mm   右端(min, X-) = -316.8 mm   差 = +2.5 mm
  X+ 側 床近 7,775 点  /  X- 側 床近 7,650 点   比 1.02
  j0 範囲(床近) -75.0 〜 +75.0

PICK は左端 +316.2 / 右端 -316.2、差 0.0mm。完全に対称になった。点数も X+ 12,622 / X- 12,389 でほぼ同数。APPROACH_PICK も左 +319.3 / 右 -316.8 で、差 2.5mm は j0 刻みの端の丸めにすぎない。右が左と同じ -316mm まで、同じ密度で埋まった。「構造的に届かない」と書いた右端は、普通に届いた。

テーブルへ統合する

スキャン出力を FK テーブル v2 に統合する。今回は箱内抽出をせず、全行を投入し、重複は 6 軸関節角キー（j0..j5 を小数 3 桁で照合）で除去する。既存行は 1 行も削除・改変しない。統合前のテーブルは archive へコピー退避してから行う（コピーのみ、上書き・移動・削除はしない）。

統合の数字が以下になる。

統合前 v2 行数         783,945
PICK v5 入力            33,728
APICK v5 入力           32,320
PICK 被り (∩ v2)        27,744   (左半分は前回 grid と完全被り)
PICK 新規                5,984   (右側 [+61.5, +92] 拡張分)
APICK 被り (∩ v2)       22,720
APICK 新規               9,600   (右側 [+31.5, +75] 拡張分)
合計新規追記            15,584
統合後 v2 行数         799,529   (= 783,945 + 15,584)

全行を投入しても、実際に増えたのは右側拡張分の 15,584 点だけだった。左半分は重複として落ちている。md5 は次のとおり。

統合後 v2 (Mac)    ec30e2fce34189f48130302a6cbf69c7
統合後 v2 (Unity)  ec30e2fce34189f48130302a6cbf69c7   ← Mac=Unity 一致
size               58,552,549 bytes (header + 799,529 行)

無印 fk_scan_motion_based.csv  2cefbcbc12d03883bad179a9d27e1f6c  不変
pre-merge 退避 v2               0b93233221a1f20392d400fc2e031a97  (= 統合前 v2)

被覆ビューア（FK_density_4poses.html）にも、新規分だけを既存の PICK / APPROACH_PICK ボタンへ合算した（ボタンは増やさない）。母数 before/after はこうなる。

新規分の床近を ee_x で見ると、PICK は -316mm、APPROACH_PICK は -317mm まで、すべて右側（X-）に集中していた。左の +316 / +319 と鏡写しになっている。ビューアを開くと、これまで左に偏っていた U 字の帯が、右にも同じだけ伸びて、左右対称に埋まっていた。

途中で出た傷と罠

作業の過程で、記録に残すべきものが二つ出た。どちらも今回の主目的とは別だが、消さずに残す。

一つは、統合後のテーブルに 6 軸キーの重複が 2,442 件あり、うち 156 件は同じ関節角なのに ee が一致しない（最大 32.6mm のばらつき）ことだった。FK は決定的なので、同じ関節角なら ee も同じはずだ。ばらつくのは、URDF やスケールや原点の異なる古いスキャン版が混ざっている疑いがある。これは今回の統合が作ったものではなく、統合前のテーブルにすでに存在していた。新規追加分はすべて既存に無いキーなので、重複を増やしてはいない。今は実害がない（FK は現行ライブが読んでいない）ので温存し、出どころの特定は独立した作業に回す。

もう一つは、Unity のスキャン出力で、Inspector の出力パスに Assets/Data/... と先頭から書くと、Unity が Application.dataPath（= Assets）と二重に結合して Assets/Data/Assets/Data/ という入れ子に書き出すことだった。出力データ自体は退避済みで無事だが、空の入れ子ディレクトリが残った。次回からは出力欄の先頭の Assets/Data/ を省く。

この回でやったこと

FK テーブルの被覆を、旧来の狭い枠からマット全面へ広げ、さらに j0 を左右対称に振って、右（X-）を左（X+）と同じ -316mm まで同密度で埋めた。テーブルは 783,945 点から 799,529 点になり、Mac と Unity で md5 が一致している。

数字以上に残ったのは、自分の出した地図が間違っていたという記録だ。「右は届かない」「構造的に不能」と二度書いた。どちらも実機の可動とは関係なく、点群が薄いのを未到達と読み違え、それを物理や SDK の構造のせいにしただけだった。可動軸は左右対称で、片側に届くなら反対側にも届く。指摘で止まり、台帳と既存テーブルを数え直して、ようやく誤りが見えた。

未充填の領域を見たとき、「構造的に届かない」と即断してはいけない。base や j0 / j1 / j2 を実際に振ってスキャンし、数で確かめてから言う。手前（ee_z+361）と奥（ee_z-89）の未充填も、まだ「届かない」とは確定していない。次に振って確かめる対象として残っている。

点が薄いことと、届かないことは違う。未充填は、未到達ではない。地図を疑う前に、まず振って、数える。

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