毎フレーム両方を再検出
赤ブロック（HSV閾値）と TIP（青シール3点の最短辺中点）を、ループの毎回
カメラから取り直す。FK 計算には依存しない。
RED_xz = pixel_to_unity(red_pixel)
TIP_xz = pixel_to_unity(midpoint(blue_pair_min_dist))
検出ノイズ（実測）
| 項目 | stdev |
|---|
| red_x_mm | 0.00 mm |
| tip_x_mm | 0.08 mm |
非対称な差分計算
X方向と Z方向の差分を取り、2D距離として合成する。j0 の更新には X方向だけ
使うが、収束判定には 2D距離を使う。
diff_x = red_x - tip_x ← j0 更新に使う
diff_z = red_z - tip_z ← j0 では効かない
diff_2d = sqrt(diff_x^2 + diff_z^2)
← 把持判定に使う
なぜ非対称か
j0 を回しても Z方向にはほとんど効かない(実測値 -0.29 mm/deg)。
Z方向の差分を使って j0 を動かしても収束しない。
直近 N フレームの位置変動
赤ブロックの X座標を 5フレーム保持し、標準偏差が閾値以下かどうかで
「動いているか／止まっているか」を判定する。
history.append(red_x_mm)
if len > 5: history.pop(0)
stable = stdev(history) <= 2.0 mm
ノイズに対する余裕
検出ノイズの stdev は約 0.08 mm。閾値 2.0 mm はその約 25 倍。
誤判定は起きない。
把持条件
2D距離が収束(≤ 5 mm)かつ赤ブロックが静止していたら把持。
どちらか欠けたら追従を継続。
if diff_2d <= 5.0 and stable:
return GRIP ← 把持に移行
elif |diff_x| > 2.0:
j0 += diff_x / MM_PER_DEG_J0_X
send_angles(...) ← 追従継続
符号の罠
MM_PER_DEG_J0_X = -6.63 は既に「方向の符号」を含む。
delta_deg の頭に「-」を付けると二重否定で逆走する。