mechArm 270 Pi 実機データを土台にデジタルツインを作る Log#08 | 補正がZ方向に効かない、その原因はデジタルツインの姿勢の使い方にあった
試行のたびに爪先のズレを測り、その位置に紐づけて記憶し、次の試行で狙いを補正する。同じ場所を繰り返し掴むうちに、ズレが縮んでいく。そういう仕組みを実機に載せた。位置の記憶は EMA で更新し、次に同じ格子セルへ来たとき、狙いから記憶ぶんを引いて送る。
仕組みは動いた。ただ、効くのは X 方向だけだった。Z 方向のズレは、補正を重ねても縮まない。最初はパラメータの問題かと思ったが、そうではなかった。原因は、ブロックの位置から関節角度を引く lookup が、関節を一つしか動かしていなかったことにある。X は動く。Z は動かない。これは記憶の精度や補正係数の話ではなく、姿勢の引き方そのものの問題だった。
そしてその解決策は、これまで何日もかけてデジタルツイン側で積み上げてきたものの中に、最初からあった。
先に座標系を確認しておく。ここでいう Z は、マットを上から見たときの奥行き方向（手前と奥）である。アームのベースから遠い側が手前、近い側が奥にあたる。高さ方向は別の軸で、デジタルツイン上の先端座標では ee_y で見る。以下で「Z が動かない／動く」と書くのは、すべてこの奥行き方向の話で、高さの話ではない。
補正は効いている、ただし X だけ
補正記憶の仕組みは、ブロックの検出位置 (x, z) を 20mm 四方の格子セルに量子化し、セルごとに爪先のズレ Δ(x, z) を持つ。次に同じセルへ来たとき、狙いを block − Δ に補正してから lookup へ渡す。更新は指数移動平均で、係数は 0.5。
同じセルを繰り返した試行を、台帳から並べる。これは X=220mm・Z=200mm 付近のセル（x220_z200）の、補正導入直後の連続試行である。
stem 適用Δz tip_dz dist
213653 - 13.9 16.4
214122 13.88 25.0 25.0
214225 19.42 24.2 24.3
214332 21.81 23.6 23.7適用している Δz は 13.9 → 19.4 → 21.8 と回を追って増えている。記憶は更新され、補正は確かにかかっている。にもかかわらず、実際の爪先の Z 方向ズレ tip_dz は 25.0 → 24.2 → 23.6 と、ほとんど動かない。補正を強めても、爪先は Z 方向に寄ってこない。
一方で同じ試行群の X 方向（tip_dx）を見ると、こちらは縮んでいた。X は補正が効き、Z は効かない。同じ記憶、同じ EMA、同じ補正式で、軸によって結果が分かれている。
補正のコードを疑う前に、補正が渡した先 ── lookup が何を返しているかを見る必要があった。
Z が動かない理由は、j0 一軸だった
実機の pick は、ブロックの (x, z) を j0_lookup に渡し、関節角度を得て送っている。この j0_lookup は、採用済みの姿勢 JSON を土台に、j0（ベースの回転軸）だけを振って (x, z) に合わせる方式である。j0 を回すと、爪先は弧を描いて動く。その弧の上で、目標の x に最も近い点を選ぶ。
j0 はベースの回転だから、動かすと主に X 方向（と、弧に沿ったわずかな Z）が変わる。Z 方向を独立して動かすには、別の関節 ── 肘や手首にあたる j1, j2, j4 ── を動かして、爪先の前後位置を変えなければならない。j0 一軸では、Z は弧の上の副産物としてしか動かない。
つまり補正記憶が Z 方向の Δ を渡しても、j0_lookup はそれを j0 の回転に翻訳しようとし、弧の上の最寄り点を選び直すだけで終わる。Z の補正は弧の残差として記録されるが、爪先を Z 方向へ寄せる関節の動きには変換されない。X が効いて Z が効かないのは、補正の強さの問題ではなく、lookup が持つ自由度が一つしかないことの帰結だった。
Z を動かすには、6 関節すべてを引く lookup が要る。そして、そのための土台はすでにある。
デジタルツインで姿勢を増やしてきた理由
このシリーズで、デジタルツイン側の FK テーブル（fk_scan_motion_based_v2.csv）を何度も拡充してきた。Unity 上でアームの関節角度を動かし、グリッパー先端がどの座標に来るかをスキャンして、関節角と先端座標の対応表を作る。点数は 512,087 → 710,704 → 852,729 と増やしてきた。
このテーブルがあれば、(x, z) を与えて「その座標に先端が来る 6 関節の組み合わせ」を引ける。j0 一軸ではなく、6 関節すべてが決まる。Z 方向に先端を動かす姿勢も、テーブルの中に含まれている。デジタルツインで増やしていたのは、点の数ではなく、Z を動かすための姿勢の選択肢だった。
fk_lookup は、この FK テーブル全体から (x, z) 最近傍の行を引いて 6 関節を返す関数として、すでに用意してあった。Z を動かす手段は、新しく作るものではなく、これまで積んできたものの使い方の問題だった。
ただし、この fk_lookup を一度、誤った使い方をして実機を止めている。その失敗を踏まえないと、同じところに戻る。
一度目の失敗 ── approach を壊し、実機が黙った
最初に fk_lookup を pick 系へ導入したとき、approach（ブロックの手前上空へ行く中間姿勢）と descend（実際にブロックへ降りる動き）の両方に当てた。FK_LOOKUP_PHASES = ("approach_pick", "pick") という形である。
これで実機を回すと、アームは HOME から動かなかった。ログを引く。
[LOOKUP] target_xz=(+0.1479,+0.2641) → method=fk_nearest joint_ok=True
[SEND] stage3_approach_pick: send_angles [-29.00, +110.50, -45.00, +1.40, -70.48, -32.87]
[TIMEOUT] 18.3s 内に未到達: actual=[-0.26, +1.58, +1.93, -0.26, -0.26, -29.70]
[RETRY] 1回目送信が無視された（pre→last max|Δ| 0.00°）
[RETRY] 失敗。ABORTactual がほぼ全軸ゼロ ── アームは HOME のまま、一度も動いていない。送った角度は j1=110.5°。これは実機が黙って無視する角度だった。arm_kinematics.md に「j1 software angle limit ~90.7° で silent command rejection」と記録してある通り、j1 が一定を超えると send_angles が受け付けられず、エラーも返さずに無視される。
問題は二重だった。一つ、approach に fk_lookup を当てたことで、本来 j1=87.74° 固定で手前上空へ行くべき中間姿勢が消え、いきなり最終的な掴み姿勢を直接狙っていた。もう一つ、実機が動く範囲（j1≤88° 付近）を超える姿勢を、絞り込みなしに選んでいた。
このとき、外部レビュー（codex）にも切り分けを依頼した。返ってきた判定は、approach への適用が中間姿勢を壊していること、j1=110.5° が silent drop の主因として最も筋が通ること、そして j0_lookup の二段階へ戻すのが妥当な復旧であること。一度 j0_lookup に全面的に戻し、実機が掴める状態を取り戻した。
戻したことで pick は動いた。だが Z が縮まない問題は、j0_lookup のままでは解けない ── ここで冒頭に戻る。fk_lookup の発想は正しく、テーブルも正しく、間違っていたのは適用の箇所と絞り込みだけだった。
二度目 ── descend だけに、j1≤88° に絞って
正しい使い方は、approach は j0_lookup の中間姿勢のまま据え置き、descend（pick フェーズ）だけ fk_lookup を使う。そして fk_lookup は、実機が動く範囲 j1≤88° の行に絞ってから、最近傍を選ぶ。
FK_LOOKUP_PHASES を ("pick",) だけにする。これで approach は触らず、descend だけが 6 関節を引く。
FK_LOOKUP_PHASES = ("pick",) # descend のみ fk_lookup・approach は j0_lookup のまま
fk_lookup.py 側では、一度撤廃した j1≤88° の安全上限を復活させ、テーブルの絞り込み（safe_mask）に組み込む。実機は j1 が 90.7° 付近を超えると send_angles を黙って無視する。そこを直に上限にすると境界で取りこぼすので、余裕を見て 88° を運用上の上限にした。これにより、実機が黙って無視する角度の姿勢は最初から候補に入らない。
該当箇所を引く。テーブル読み込み時に hard limit に加えて j1≤88° で絞る部分である。
J1_SAFE_MAX = 88.0 # 実機が素直に動く上限。これを超えると send_angles が silent reject する
def _load_fk_table(path):
arr = np.loadtxt(path, delimiter=",", skiprows=1)
ee = arr[:, 0:3]
joints = arr[:, 3:9]
# hard limit（機械可動域）
safe = np.ones(len(arr), dtype=bool)
for i, (lo, hi) in enumerate(JOINT_LIMITS):
safe &= (joints[:, i] >= lo) & (joints[:, i] <= hi)
# j1 safety（実機が素直に動く範囲・descend 限定運用で復活）
safe &= (joints[:, 1] <= J1_SAFE_MAX)
return ee[safe], joints[safe]補正記憶は、この fk_lookup の (x, z) target にそのまま乗せる。_apply_correction は block − Δ を返すだけなので、lookup の方式が j0 一軸から 6 関節へ変わっても、接続はそのまま機能する。補正値の渡し方は変えず、受け手側の自由度だけを増やした形だ。j0_lookup では Z が弧の残差として死んでいたが、fk_lookup では ee_z（先端の Z 座標）の target に乗り、テーブルから「その Z に届く姿勢」が選ばれる。
補正適用の関数を全文で載せる。
def _apply_correction(block_xz):
"""ブロック検出位置に対し、記憶された Δ を引いて補正後の狙いを返す。
USE_CORRECTION=False のときは補正なしで素通し。KIOXIA 未マウント時も素通し。"""
bx_m, bz_m = block_xz
meta = {
"applied": False, "cell_key": _corr_key(bx_m * 1000, bz_m * 1000),
"dx_mm": 0.0, "dz_mm": 0.0,
"target_before_mm": (bx_m * 1000, bz_m * 1000),
"target_after_mm": (bx_m * 1000, bz_m * 1000),
}
if not USE_CORRECTION:
return (bx_m, bz_m), meta
if not _ssd_mounted():
return (bx_m, bz_m), meta
mem = _corr_load()
key = meta["cell_key"]
entry = mem.get(key)
if entry is None:
return (bx_m, bz_m), meta
if not isinstance(entry, dict):
# 壊れたエントリは補正せず素通し
print(f"[CORR ][WARN] mem[{key}] が dict 以外 ({type(entry).__name__}). 補正せず素通り")
return (bx_m, bz_m), meta
dx_mm = float(entry.get("dx_mm", 0.0))
dz_mm = float(entry.get("dz_mm", 0.0))
# クランプ（記憶破損でも狙いが飛ばない）
dx_mm = max(-MAX_CORRECTION_MM, min(MAX_CORRECTION_MM, dx_mm))
dz_mm = max(-MAX_CORRECTION_MM, min(MAX_CORRECTION_MM, dz_mm))
corrected_x_m = bx_m - dx_mm / 1000.0
corrected_z_m = bz_m - dz_mm / 1000.0
meta.update({
"applied": True, "dx_mm": dx_mm, "dz_mm": dz_mm,
"target_after_mm": (corrected_x_m * 1000, corrected_z_m * 1000),
})
return (corrected_x_m, corrected_z_m), metaデジタルツイン上で、Z が動くことを先に確かめる
実機に投入する前に、デジタルツインのテーブルだけで、Z 補正が姿勢選択に乗るかを確かめた。同じ X=220mm で、Z の target を 25mm 変えたとき、fk_lookup が返す先端の Z 座標がどれだけ動くか。
target ee_z j1
(220, 200) 補正なし +203.3 +85.50°
(220, 175) 補正 -25mm +175.8 +88.00°
→ ee_z 差 = -27.53mm（target_z -25mm に対し）target_z を 25mm 動かすと、返ってくる姿勢の ee_z が 27.53mm 動いた。テーブルが離散値で持つぶんの誤差を含むが、Z 補正は確かに姿勢選択に乗っている。j0_lookup では弧の残差として死んでいた Z が、fk_lookup では先端の Z 座標を動かす関節の組み合わせに翻訳される。
姿勢のバリエーションがテーブルに足りているかも、事前に確認した。j1≤88° を満たす行は全 852,729 行中 308,026 行。代表セル周辺の近傍には 13,000〜19,000 行あり、先端の高さ方向（ee_y）は 300mm 以上の幅で、2,500 段階以上のバリエーションを持つ。Z を動かす姿勢は、テーブルの中に十分にあった。
approach が壊れていないことも、ドライランで全件確認した。approach は j0_lookup の中間姿勢（j1=87.74° 固定）を返し、descend だけが fk_lookup で 6 関節を返す。descend の 8 サンプルはすべて j1≤88°、j1>88° は 0 件。一度目の失敗（j1=110.5° の silent drop）が起きない形になっている。
この変更も、姿勢を送る中核を変えるものなので、外部レビュー（codex）に通した。approach が壊れていないか、j1≤88° の絞り込みが効くか、Z 補正が ee_z に乗るか、テーブルの穴（カバレッジの薄い端）で送角を止めるか。6 点中 5 点が問題なし、1 点は端の領域で hard window を仕様化するかどうかの運用判断で、通常レンジの投入は可、という判定だった。
実機 ── Z が動いた
実機に投入した。まず補正なしで掴めることを確認し、次に補正を入れて、同じ位置を繰り返した。descend が fk_lookup の 6 関節で降りるようになった後の台帳を並べる。
stem cell 適用Δz tip_dx tip_dz dist pick
231631 x220_z200 22.71 0.5 -10.0 10.0 yes
232057 x220_z260 - 0.5 -10.0 10.0 yes
232215 x100_z320 - -0.2 -7.1 7.1 yes
232447 x240_z200 -4.71 7.5 6.2 9.7 -
232556 x220_z260 -9.97 -8.2 -0.4 8.2 yesx220_z200 を見る。j0_lookup の時代、このセルは補正を強めても tip_dz が 25 → 24 → 23 と縮まなかった場所である。descend を fk_lookup にした後、同じセルで tip_dz は -10.0 まで来た。Z 方向のズレが、25mm 級から 10mm 級へ下がっている。
232556（x220_z260）では、補正 Δz=-9.97 が乗った状態で tip_dz が -0.4 ── Z がほぼ合った。x100_z320 のような端寄りの位置でも tip_dz=-7.1 に収まっている。j0 一軸では動かなかった Z が、6 関節を引くことで動くようになった。
x240_z200（232447）の pick 欄が空なのは、掴み判定まで到達しなかった、または保持の途中で別の問題により中断した行として扱う。位置（tip dist=9.7mm）は届いているが、掴んだ結果が記録されていない。この行が、次の段落で触れる保持の問題にあたる。
実機を操作した感触としても、アーム自体の Z 方向の動きは良くなった。降りる深さと前後位置が、ブロックの Z に合わせて変わる。狙った座標へ、Z を含めて寄っていく。
これが、Z を動かすという課題への到達点だ。X だけ効いていた補正が、Z にも効くようになった。原因は補正の精度ではなく姿勢の引き方にあり、その解決は、デジタルツインで姿勢を増やしてきたことの上に乗っていた。
残った問題 ── 位置は合った、保持は別
descend が正しい位置へ降りるようになった一方で、別の問題が見えている。アームが動いている最中に、j4（手首付近の関節）が抜けたり、爪の握りが緩んだりして、掴んだブロックが落ちることがある。
台帳の 232447（x240_z200）は、tip dist=9.7mm と掴める距離まで降りているのに、pick の判定が空欄のまま終わっている。位置は合っているのに、掴んだ後に保持できていない。
これは、これまでとは別のレイヤーの問題になる。位置を合わせる（x, z を狙って降りる）ことは、descend の fk_lookup と補正記憶で形になった。掴んで保持する（落とさない）ことは、関節の保持力や爪の握りの問題で、姿勢選択とは別の軸にある。descend で j4 を含む 6 関節を動かすようになったことと、j4 の抜けが関係している可能性もある。
補正記憶は間違っていなかった。間違っていたのは、補正を渡す先だった。j0 一軸の弧では、Z は動かない。6 関節の姿勢を引いて初めて、Z 補正は爪先の位置に変わる。デジタルツインで増やしてきた姿勢が、ここで実機の Z 補正に効いた。
越えたのは、位置の山だ。保持の山は、次に残っている。
