mechArm 270 Pi 動的追従の限界を探る Log#17 | 視覚サーボ Y 補正の最短解。2関節を比べて j4 単独に落ち着いた

XYZ_TRACK の最後の設計判断は、Y 補正にどの関節を使うかだった。机上では決めきれなかったので、3 姿勢で実測した。

💡 Key Takeaway

Y 補正を j2 と j4 のどちらに担わせるか。3 姿勢での実測の結果、j4 の Z 干渉は j2 の 1/5 以下と判明。j4 を「Y 専用関節」として単独起用し、当初予定の異符号連動設計を破棄。

XYZ_TRACK の最後の設計判断

視覚サーボピックの全体フローは HOME → OPEN → XYZ_TRACK → GRIP → LIFT → HOME の 6 段。中核の XYZ_TRACK は上カメラ（XZ 平面）と横カメラ（Y）の両方を同時に見て、爪先とブロックの 3 軸差分を 1 つのループで閉じるフェーズだ。

前セッションまでに、3 軸の補正方針は 2 軸分が決まっていた。X 補正は j0 単独。Z 補正は LOG189 で確立した j1 + j2 + j4 の連動。残る Y 補正だけが未確定だった。

事前の見立てはこうだ。「j2 と j4 はどちらも垂軸（ピッチ系）の関節で、両方とも Y に効く。ただし j2 は Z にも強く効くから、j2 と j4 を異符号で連動させて Z 干渉を打ち消しながら Y を動かす設計になる」。

机上では筋が通って見えた。しかしこの連動比率を決めるには、j2 と j4 が「1° あたり Y にどれだけ・Z にどれだけ動かすか」を実測する必要がある。机上の前提（j2 も j4 も同程度に Y に効く）が間違っていれば、ループ全体が動かない。

実測することにした。

y_calibration.py を作る

新規スクリプト y_calibration.py の仕様はシンプルだ。

mechArm を APPROACH 姿勢に戻す。j2 を ±5°, ±3° の 4 段階で単独動作させ、上カメラ TIP（X, Z）と横カメラ TIP（Y pixel）の動きを記録する。j4 を ±5°, ±3° で同じことをする。合計 8 試行を 1 セッションで取って CSV に保存する。

メインループの骨格はこうだ。

TRIALS = [
    (2, +5.0), (2, +3.0), (2, -3.0), (2, -5.0),
    (4, +5.0), (4, +3.0), (4, -3.0), (4, -5.0),
]

for trial_idx, (joint_idx, delta_deg) in enumerate(TRIALS, start=1):
    # (a) ベース姿勢に戻す
    daemon_send_angles(APPROACH_PICK_BASE, speed=SPEED, wait=WAIT_SEC)
    
    # (b) before 計測（上カメラXZ + 横カメラY）
    x_bef, z_bef, y_pix_bef = measure_both_with_retry(cap_top, cap_side, tag="before")
    
    # (c) Δ加算して目標姿勢に動かす
    target_angles = list(APPROACH_PICK_BASE)
    target_angles[joint_idx] += delta_deg
    daemon_send_angles(target_angles, speed=SPEED, wait=WAIT_SEC)
    
    # (d) after 計測
    x_aft, z_aft, y_pix_aft = measure_both_with_retry(cap_top, cap_side, tag="after")
    
    # (e) 実関節角度取得 + 到達確認
    measured = daemon_get_angles()
    # ... ACTIVE/STATIC TOLでチェック
    
    # (f) 差分計算してCSV行に追加
    dx_mm = x_aft - x_bef
    dz_mm = z_aft - z_bef
    dy_pix = y_pix_aft - y_pix_bef

既存の visual_servo_pick.py から daemon_send_angles、detect_tip_from_frame をインポートして再利用。横カメラ TIP は前セッションで作った side_camera_utils.detect_tip_side() を使う。新規実装は最小限にした。

このスクリプトに辿り着くまでに、外部レビュー（codex / gemini）で 3 ラウンドの指摘と修正があった。物理リスクのあるコードは必ずレビューを挟むという LOG200 で実証したルールを守った結果だ。

1 回目のレビュー：HOME 戻り・到達確認・ESC 検出

最初に書き上げた版に対して、レビューで以下 3 点が指摘された。

HOME 戻りの wait が短い。return_to_home() で WAIT_SEC=2 を使っていたが、APPROACH から HOME への移動量は大きい。既存の visual_servo_pick.send_home() は 4 秒待ちだった。HOME_WAIT_SEC=4 を別定数として定義し直した。

目標角度到達のチェックがない。daemon_get_angles() で実関節角度は取得していたが、目標との差を確認していなかった。daemon が遅延したり指令が届いていなかった場合でも、データが CSV に残ってしまう。目標角度との誤差が許容を超えたら中断するロジックを入れた。

ESC 検出が試行の先頭でしか働かない。1 試行は約 15 秒かかる。その間に押した ESC は次のループまで反映されない。base 送信後・before 検出後・target 送信後・after 検出後の 4 箇所で ESC チェックを追加した。

修正版で実機テストに入った。

2 回目のレビュー：daemon の wait 引数が機能していない

修正版を再レビューしたところ、より深い問題が判明した。daemon_send_angles(..., wait=N) の wait 引数を Pi 側 mecharm_daemon.py が無視していた。引数として受け取ってはいるが time.sleep(wait) を呼んでいない。

これに気づかず HOME 復帰の wait=4 を渡しても、実際には 0 秒で次の処理に進んでしまっていた。アームが動き終わる前に次の検出が走る可能性がある。

修正は Mac 側で time.sleep() を明示的に入れる形にした。daemon 側を変えると visual_servo_pick.py 等の既存スクリプトに副作用が出るリスクがある。y_calibration.py 単独で完結させた。

daemon_send_angles(HOME_ANGLES, speed=SPEED, wait=HOME_WAIT_SEC)
time.sleep(HOME_WAIT_SEC)  # daemon 側 wait 未実装のため Mac 側で明示待機

同じパターンを base 姿勢送信後と target 姿勢送信後にも入れた。target 後は到達確認ロジックがあるので必須ではないが、安全側に倒した。

daemon の wait 未実装は中期 TODO として残す。今は visual_servo_pick.py が動作中だから触らない。

1 回目の実機テスト：j1 の重力ドリフトで即 ABORT

修正版を実機で走らせた。1 試行目で即 ABORT。

--- Trial 1/8 j2 delta=+3.0° ---
  目標姿勢: [0.0, 87.74, -17.0, 3.6, -90.0, -30.0]
[ABORT] trial 1 目標角度未到達 max_err=1.110° > 0.5° 
  target=[0.0, 87.74, -17.0, 3.6, -90.0, -30.0] 
  measured=[-0.26, 88.85, -16.52, 3.25, -89.73, -29.79]

軸ごとの誤差を並べる。

j1 が 1.11° ズレている。j1 は 87.74° という大きな値を保持する関節で、重力負荷を強く受ける。サーボの保持力が重力に負けて、目標値より少し上にドリフトしていた。

肝心の j2 自体は 0.48° で TOL 内に収まっている。測りたい関節は止まっているが、測らない関節が止まれずに ABORT になったという構造だ。

到達許容差を関節ごとに分けることにした。動かす関節（active）は厳密に、動かさない関節（static）は重力ドリフトを許容して緩く。

TOL_REACH_DEG_ACTIVE = 0.5    # 動かす関節（j2 or j4）の到達許容差
TOL_REACH_DEG_STATIC = 2.0    # 動かさない関節（重力ドリフト許容）

ループ内のチェックも書き直した。動かす関節の誤差だけ ACTIVE TOL で見て、それ以外は STATIC TOL で見る。

2 回目の実機テスト：±1° で実機が反応しない

ACTIVE/STATIC 分離の修正後、再実行した。Trial 1, 2 は通った。Trial 3 (j2 -1°) で再び ABORT。

--- Trial 3/8 j2 delta=-1.0° ---
  目標姿勢: [0.0, 87.74, -21.0, 3.6, -90.0, -30.0]
[ABORT] trial 3 動かす関節 j2 未到達 err=1.230° > 0.5° 
  measured=[-0.26, 88.85, -19.77, 3.33, -90.43, -29.79]

j2 を -20° から -21° に動かそうとしたら、measured は -19.77° で止まっていた。指令と実測の差 1.23°。これは「動こうとして力が足りなかった」のではなく、「動く指令と認識しなかった」ように見える。

3 回再現した。±1° の指令では実機が動かない。パルスを送ってもサーボがピクリともしない。mechArm 270 Pi のサーボにある不感帯（dead band）の存在。数値をいじるだけの机上計算が弾かれた瞬間だった。

加えて、それまでに取れた数試行のデータを見直すと、同じ姿勢・同じ指令でも検出値がばらついていることに気づいた。j2 +3° の試行を 3 回繰り返したときの dy（横カメラ Y 方向の動き）を並べる。

j2 +3° で dy が 3 / 11 / 13 と 4 倍以上ばらついている。1 試行 1 回の検出では信頼性が足りない。横カメラの緑シールが姿勢で影に入ったり光が当たったりして、検出位置が ±10 pix ほどゆらいでいた。

修正は 2 つ入れた。第一に Δ角度を ±5° / ±3° の 2 点に変更（±1° は捨てる）。第二に、各検出を 3 回行って平均化する。ログには range（最大値 − 最小値）も吐き出し、ばらつきの大きい試行を後からスジの悪いデータとして弾けるようにした。

DETECT_AVERAGE_COUNT = 3
DETECT_AVERAGE_INTERVAL_SEC = 0.1

def measure_both_with_retry(cap_top, cap_side, tag=""):
    x_samples, z_samples, y_samples = [], [], []
    for sample_idx in range(DETECT_AVERAGE_COUNT):
        # ... 検出してサンプルを溜める ...
    
    avg_x = sum(x_samples) / len(x_samples)
    avg_z = sum(z_samples) / len(z_samples)
    avg_y = int(round(sum(y_samples) / len(y_samples)))
    
    print(f"  [AVG] {tag} samples={DETECT_AVERAGE_COUNT}: "
          f"x={avg_x:.2f}mm (range={max(x_samples)-min(x_samples):.2f}) "
          f"y={avg_y}pix (range={max(y_samples)-min(y_samples)})")
    return avg_x, avg_z, avg_y

TOL_REACH_DEG_ACTIVE も 0.5° → 1.0° に緩めた。±5° の動きでも未到達リスクがあるため。

3 回目の実機テスト：高姿勢で 7/8 完走

修正版で走らせた。Trial 1〜7 が通り、Trial 8 (j4 -5°) で ABORT。

[Trial 1/8] j2 +5.0deg | dx=-0.3mm dz=-8.1mm dy=+34pixel
[Trial 2/8] j2 +3.0deg | dx=-1.0mm dz=-4.8mm dy=+30pixel
[Trial 3/8] j2 -3.0deg | dx=+1.2mm dz=+5.3mm dy=-33pixel
[Trial 4/8] j2 -5.0deg | dx=+1.9mm dz=+7.8mm dy=-55pixel
[Trial 5/8] j4 +5.0deg | dx=-1.2mm dz=+0.7mm dy=+56pixel
[Trial 6/8] j4 +3.0deg | dx=-0.1mm dz=+0.2mm dy=+12pixel
[Trial 7/8] j4 -3.0deg | dx=+1.2mm dz=-1.7mm dy=-42pixel

--- Trial 8/8 j4 delta=-5.0° ---
[ABORT] trial 8 動かす関節 j4 未到達 err=1.050° > 1.0° 
  measured=[-0.26, 88.85, -19.42, 3.51, -93.95, -29.79]

Trial 8 は j4 を -90° から -95° に動かそうとして -93.95° で止まった。err 1.05°、TOL 1.0° を 0.05° だけ超過。実機の動作限界近くで起こる微小な未到達だ。TOL をさらに緩めても良かったが、7 試行で十分なデータが取れているので、ここで止めて分析に入った。

CSV ファイル: ~/r2r2r-scripts/logs/y_calibration_20260516_202140.csv

このデータから 1° あたりの動きベクトルを抽出する。j2 の 4 試行を平均すると：

ここで重要な発見があった。j2 も j4 も Y には同じくらい効く（約 +10 pix/°）が、Z への干渉が桁違いに違う。j2 は Z に -1.6 mm/° と強く効くのに対して、j4 は +0.3 mm/° しか動かない。約 5 倍の差だ。

これは事前の見立てを覆す結果だった。「j2 と j4 を異符号連動で Z 干渉を打ち消す」という設計の前提は、j2 と j4 の Z 干渉が同程度であることだった。実際は片方だけが Z に強く効く。だったら Z 干渉が小さい j4 単独で Y を補正すればいい。

ただし、ここで判断を確定させなかった。APPROACH 姿勢（j2=-20、爪先がマットから離れた高い位置）でのデータでしかない。実際の XYZ_TRACK ループは、爪先がブロックに近づくにつれて姿勢が変わる。姿勢が変わると感度も変わる可能性が高い。

姿勢依存性を確認するため、もう 2 つの姿勢でもキャリブを取ることにした。

低姿勢の決定：実機を目視で動かして探す

XYZ_TRACK の収束ゴールは「爪先がブロック直上にある」状態。GRIP に移行できる高さだ。この姿勢を机上ではなく実機で目視判定して決めることにした。

横カメラのプレビューを開きながら、daemon_send_angles() で j2 を 5° ずつ動かして爪先の高さを観察した。

4 ステップ目（j2 = -2°）を採用。これを新しい BASE 姿勢として APPROACH_PICK_BASE_LOW = [0.0, 87.74, -2.0, 3.6, -90.0, -30.0] と定義した。y_calibration.py の定数を書き換えて再実行。

ここで直感とコードが完全に逆を向く。j2 = elbow ピッチ のマイナス方向は、肘を伸ばす動き。数字の上では「-20° から -2° へ増えている」ように見えるのに、物理的には「肘を曲げて爪先が下がる」。実機を見ずにコードだけで組むと、高確率で逆方向に動かして激突させる罠だ。

低姿勢キャリブ：8/8 完走

低姿勢で y_calibration.py を実行した。全 8 試行が完走した。

[Trial 1/8] j2 +5.0deg | dx=+0.2mm dz=-6.7mm dy=-4pixel
[Trial 2/8] j2 +3.0deg | dx=-0.7mm dz=-6.5mm dy=+21pixel
[Trial 3/8] j2 -3.0deg | dx=+1.0mm dz=+7.2mm dy=-23pixel
[Trial 4/8] j2 -5.0deg | dx=+1.7mm dz=+10.0mm dy=-36pixel
[Trial 5/8] j4 +5.0deg | dx=-0.5mm dz=-0.9mm dy=+25pixel
[Trial 6/8] j4 +3.0deg | dx=-0.1mm dz=-0.5mm dy=+7pixel
[Trial 7/8] j4 -3.0deg | dx=+1.0mm dz=+1.4mm dy=-34pixel
[Trial 8/8] j4 -5.0deg | dx=+1.0mm dz=+0.9mm dy=-28pixel

CSV ファイル: ~/r2r2r-scripts/logs/y_calibration_low_20260516_203456.csv

Trial 1 (j2 +5°) で dy が -4 pix と他と方向が逆になっているのは検出ノイズ。range をログで確認すると、横カメラの緑シールが姿勢で影に入る瞬間があった試行で大きい値が出ている。3 回平均化が効いてはいるが、完全には消しきれない。

平均値を出す。

低姿勢でも j4 の Z 干渉は -0.25 mm/° と小さい。j2 の Z 干渉は -2.0 mm/° と高姿勢よりも強くなっている。さらに、j4 の Y 感度（dy/°）が高姿勢の +10 から低姿勢の +6 に下がっている。姿勢が低くなると j4 の Y 感度が落ちる——これは想定通りだが、定量的に確認できた。

中間姿勢でのキャリブ：線形性の確認

高姿勢 (j2=-20) と低姿勢 (j2=-2) の間に、中間姿勢 (j2=-11) を入れた。高姿勢と低姿勢の中点だ。線形補間が成立するかを見るためだ。

中間姿勢は Trial 6 (j4 +3°) で ABORT（err=1.59° > 1.5°）。j2 4 試行 + j4 1 試行が取れた。

[Trial 1/8] j2 +5.0deg | dx=-0.1mm dz=-8.6mm dy=+23pixel
[Trial 2/8] j2 +3.0deg | dx=-0.7mm dz=-5.3mm dy=+21pixel
[Trial 3/8] j2 -3.0deg | dx=+1.0mm dz=+6.2mm dy=-29pixel
[Trial 4/8] j2 -5.0deg | dx=+1.9mm dz=+9.2mm dy=-47pixel
[Trial 5/8] j4 +5.0deg | dx=-1.0mm dz=-1.0mm dy=+41pixel

CSV ファイル: ~/r2r2r-scripts/logs/y_calibration_mid_20260516_204525.csv

j4 は中間姿勢で 1 点しか取れなかったが、結果は dy=+8.2 pix/°。高姿勢 (+10) と低姿勢 (+6) の中点が +8 で、ほぼピッタリ。線形補間が成立しそうだと判断して、ここでキャリブを終わらせた。j4 の追加データは、実装に入ってから XYZ_TRACK の中で観察できる。

3 姿勢の比較

集まったデータを 1 つの表にまとめる。

ここから設計判断に行く。「Y 効率に対する Z 干渉比」を計算する。Z 干渉の絶対値を Y 効果（pix/°）で割って、mm/pix 単位にする。この値が小さいほど Y 専用関節として優秀だ。

j4 は j2 より 5〜13 倍も Y 専用に近い。

結論が出た。Y 補正は j4 単独で行う。j2 は使わない。当初想定していた「j2 と j4 を異符号連動で Z 干渉を打ち消す」設計は不要だった。設計が当初想定の半分くらいの複雑さで済むことになった。

面白かったのは、「どの関節が動くか」ではなく「どの関節を Y 専用関節として扱えるか」が問題だったことだ。関節は 6 軸ある。でも制御ループの中では、役割を固定できた瞬間にシステムが単純化する。「j4 = Y」「j0 = X」のように 1 軸 1 関節を割り当てられれば、ループは収束しやすくなる。j4 は元々 Z 補正の連動にも入っていて、Y も足すと役割の重複が起きるが、それは Δ の合算で吸収できる。

j4 感度の線形補間モデル

j4 の Y 感度 (dy/°) は姿勢で線形に変化する（+10 → +8.2 → +6）。XYZ_TRACK ループ中に j2 値が変わるたびに、j4 の感度も変える必要がある。固定値で計算すると、姿勢が低い領域では補正量が過剰になり、ループが振動するリスクがある。

線形補間式：

dy_per_degree_j4(j2) = 6.0 + (10.0 - 6.0) × (j2 - (-2)) / ((-20) - (-2))
                     = 6.0 - 0.222 × (j2 + 2)

検算：

実測との差は最大 0.2 pix/°。誤差 2.5% くらい。XYZ_TRACK の収束条件は ±5 pix なので、ここはもう十分だと思った。3 点で線形性が成立していれば、内挿は安全に使える。範囲外の j2 値（j2 < -20° や j2 > -2°）は実装でクリップする。

XYZ_TRACK の設計が決まった

今回のキャリブ結果から、XYZ_TRACK ループの設計は以下に確定した。

[XYZ_TRACK ループ]
    ↓
上カメラから現在 TIP (x, z) を取得
横カメラから現在 TIP y_pixel を取得
上カメラから BLOCK (x_target, z_target) を取得
横カメラから BLOCK y_pixel_target を取得
    ↓
diff_x, diff_z, diff_y を計算
    ↓
収束判定（全軸が許容内なら GRIP へ）
    ↓
Δj0 ← diff_x から（X補正、既存ロジック）
Δj1, Δj2, Δj4_for_Z ← diff_z から（Z補正、既存連動）
Δj4_for_Y ← diff_y / dy_per_degree_j4(現在のj2)   ← 今回確定
    ↓
Δj4_total = Δj4_for_Z + Δj4_for_Y
    ↓
new_angles = current_angles + [Δj0, Δj1, Δj2, 0, Δj4_total, 0]
clamp & safety check
daemon_send_angles(new_angles)
time.sleep(ループ周期)
    ↓
ループ継続

XYZ_TRACK の中で X / Y / Z の 3 軸補正を同時並行で進める。j4 だけが Y と Z の両方に関わるが、Z 補正分の j4_for_Z と Y 補正分の j4_for_Y を合算して 1 つの指令にまとめる。j4 の Y 効率は j2 値（現在の姿勢）で動的に補間する。

収束許容値の初期案は X が ±2mm、Z が ±2mm、Y が ±5pix。ループ周期は LOG182 で実測したソフト制御周波数 91Hz と物理追従 5〜10Hz から、200ms (5Hz) スタートが安全。

実機で測ってから分かったこと

机上では予想できなかった現象がいくつも出てきた。

mechArm 270 Pi のサーボには ±1° の不感帯がある。小さな指令には反応しない。これを知らずに「もっと細かく制御すれば精度が上がる」と思い込んで設計を組むと、ループが振動する。

j1 は重力負荷で +1° 程度ドリフトする。静止しても完全には止まらない。「動かさない関節」も完全に止まっていると仮定すると、到達確認のロジックが ABORT を量産する。

j2 と j4 の Y 感度は姿勢で 30〜40% 変化する。固定値では XYZ_TRACK が安定収束しない。線形補間で姿勢ごとに感度を変える必要がある。

j4 の Z 干渉は姿勢で符号が反転する。高姿勢では +0.3 mm/°、低姿勢では -0.25 mm/°。絶対値が小さいので Y 専用関節として使える結論は変わらないが、Z 補正側の連動係数を姿勢で変える検討は実装段階で必要になる。

机上の計算では、j2 も j4 も同じ Y 補正の候補だった。実測データが提示した答えは「j4 の単独起用」という最短解。j2 を連動させる複雑なロジックは、組む必要すらなかった。

23 試行（高 7 + 低 8 + 中 5、加えてテスト中の ABORT 3 回）の積み重ねが、当初想定より半分シンプルな設計を引き出した。

次のセッションでやること

設計は確定した。実装に移る。

1. xyz_track_test.py を新規作成（visual_servo_pick.py に統合する前の単体検証用）

2. HOME → OPEN → XYZ_TRACK で 3 軸収束まで（GRIP は接続しない）

3. 外部レビュー（codex / gemini）

4. 実機実行

5. 動作確認後、visual_servo_pick.py の REFINE フェーズを XYZ_TRACK で置き換え

6. GRIP / LIFT 接続

7. 動画撮影

Y 軸補正は、関節 1 つで十分だった。それを確かめるために、関節 2 つを 3 姿勢で 23 試行測ることが必要だった。

Physical AI は「複雑な制御を作る競争」に見える。でも実機を触ると逆で、実測を積むほど不要な複雑さを削る作業になっていく。実機は、設計を複雑にするためではなく、設計を削るために触る。

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