LOG146R2R2R2026/4/12

send_coords()のIKが信頼できない。だからデジタルツインシミュレーションが必要だ

homographyの二重変換バグを修正し、マット四隅の実測キャリブレーションでアームがブロック方向に向かうことを確認した。しかしsend_coords()のIKが不安定で正確な到達ができない。解決策はUnityシミュレーションによる関節角度データの事前収集だ。

💡 Key Takeaway

カメラ→homography→Robot XYの変換パイプラインは正しく機能している。問題はsend_coords()に渡すRXYZが位置によって変化するのに固定値を使っていること。IKが不安定な解を出し続ける。「任意の位置に対応する関節角度データ」はUnityシミュレーションで大量生成するのが正道だ。

やったこと	起きたこと	判断
dryrun_check.pyの二重変換バグを修正	Robot座標が-220.7→60.6mmに正常化	homographyはpixel→Robot XYとして正常動作
PICK_RXYZを実測値に修正	アームがブロック方向に向かうようになった	前進。ただし正確な到達はできていない
マット四隅で黄色点キャリブレーション	homographyがマット全域で正常動作	キャリブレーションは完了
send_coords()でIK不安定を確認	送った座標と実測座標が大きくズレる	send_coords()は信頼できないと確定
次の方針をシミュレーション活用に設定	Sim-to-Realのコア用途が明確になった	Unityで関節角度データを収集する

二重変換バグを発見した

最初の確認は homography.npy の実態だった。

python3 -c "import numpy as np; print(np.load('/Users/shimazueiichi/r2r2r-scripts/homography.npy'))"

出力された行列：

[[-1.34255941e-02  3.10998878e-01 -1.17426799e+02]
 [ 3.06614876e-01  1.25341245e-02 -2.77933724e+02]
 [ 2.71896427e-05  1.15733839e-05  1.00000000e+00]]

Robot座標4点でキャリブレーション済みなので、この行列のpixel→変換出力はRobot座標のはずだ。

次に dryrun_check.py を実行した。

[DRY RUN] pixel: (1347, 640)
[DRY RUN] World: (60.8 mm, 137.1 mm)
[DRY RUN] Robot: (-220.7 mm, -165.3 mm)

問題が見えた。dryrun_check.py の中でこういうコードが動いていた。

# World座標変換
world_xy = pixel_to_world((pixel_x, pixel_y), homography_matrix)
world_x, world_y = world_xy[0], world_xy[1]

# Robot座標計算 ← これが余計
robot_x = -world_x + COORD_OFFSET_X
robot_y = -world_y + COORD_OFFSET_Y

homographyの出力はすでにRobot座標なのに、そこからさらにオフセット変換をかけていた。二重変換だった。

修正はシンプルだった。

# pixel → Robot座標（homographyが直接Robot座標を出力）
robot_xy = pixel_to_world((pixel_x, pixel_y), homography_matrix)
robot_x = robot_xy[0]
robot_y = robot_xy[1]
print(f"[DRY RUN] Robot: ({robot_x:.1f} mm, {robot_y:.1f} mm)")

修正後の出力：

[DRY RUN] pixel: (1347, 639)
[DRY RUN] Robot: (60.6 mm, 137.1 mm)

キャリブレーション範囲（X: -111.5〜+141.9 / Y: -202.4〜+193.8）に収まった。

PICK_RXYZの固定値が間違っていた

フルループを実行すると、アームが全く別の場所に向かっていた。ログとカメラ映像を照合して問題の構造を確認した。

フリームードでアームをブロックの真上に手で持っていき、実測値を取った。

# ABOVE位置（Z=60.6mm）
angles: [29.88, 63.98, 26.01, -53.78, -88.76, -26.89]
coords: [92.8, 105.8, 60.6, -92.71, 63.07, -8.75]

# APPROACH位置（Z=46.4mm）
angles: [33.22, 71.63, 25.83, -48.86, -92.28, -35.41]
coords: [84.9, 106.4, 46.4, -95.28, 60.1, -12.47]

# PICK位置（地面スレスレ、Z=21.1mm）
angles: [33.57, 86.22, 13.97, -48.95, -93.51, -38.75]
coords: [84.5, 107.1, 21.1, -96.16, 58.77, -12.76]

旧来のPICK_RXYZは (-97.14, 55.44, -143.21) だったが、実測値は (-92.71, 63.07, -8.75) だった。特にRZが-143から-8.75へ、全く違う値だった。

camera_pick_pi.py を修正した。

# 修正前
PICK_RX = -97.14
PICK_RY = 55.44
PICK_RZ = -143.21

# 修正後（実測値）
PICK_RX = -92.71
PICK_RY = 63.07
PICK_RZ = -8.75

# Z値も実測値に統一
ABOVE_Z = 60.6
APPROACH_PICK_Z = 46.4
PICK_Z = 21.1

send_angles()で固定座標ピックを試すと成功した。アームがブロックを掴んでホームに戻った。

homographyをマット四隅で取り直した

カメラの検出座標と実際のアーム位置のズレを切り分けるため、homographyをマット四隅の黄色い点で取り直した。

手順：

Mac側で camera_check.py を実行してpixel座標をリアルタイム確認
Pi側でフリームードにしてアームをブロック真上に手動移動
get_coords() でRobot座標を実測

取得した4点：

左上: pixel(216,  82)  → robot(-89.0,  -202.6)
右上: pixel(1547, 56)  → robot(-112.5,  204.6)
左下: pixel(330,  873) → robot(138.1,  -172.9)
右下: pixel(1459, 847) → robot(128.6,   182.9)

Macで findHomography() を実行して保存した。

import cv2
import numpy as np
import os

src_points = np.array([
    [216,  82],
    [1547, 56],
    [330,  873],
    [1459, 847]
], dtype=np.float32)

dst_points = np.array([
    [-89.0,  -202.6],
    [-112.5,  204.6],
    [138.1,  -172.9],
    [128.6,   182.9]
], dtype=np.float32)

H, _ = cv2.findHomography(src_points, dst_points)
np.save(os.path.expanduser('~/r2r2r-scripts/homography.npy'), H)

生成された行列：

[[-7.14191679e-03  2.79045771e-01 -1.09249542e+02]
 [ 2.93409928e-01  1.77930738e-03 -2.63642181e+02]
 [-4.37488018e-05 -3.40545320e-05  1.00000000e+00]]

dryrun確認：

[DRY RUN] pixel: (1461, 849)
[DRY RUN] Robot: (129.2 mm, 183.6 mm)

キャリブ点の右下 pixel(1459, 847) → robot(128.6, 182.9) と誤差1mm以下で一致。homographyは正常動作している。

send_coords()のIKが信頼できないと確定した

フルループを実行した結果が証拠だ。

[Pi] 真上に移動: [70.9, 178.6, 60.6, -81.36, 61.94, 11.17]
[Pi] ABOVE実測座標: [71.5, 175.5, 54.9, -86.32, 63.03, 5.68]

XYのズレは0.6mm、3.1mm。send_coords()がRobot XYには正しく到達できている。

しかし別の実行では：

[Pi] 真上に移動: [-0.6, -167.7, 60.6, -92.71, 63.07, -8.75]
[Pi] ABOVE実測座標: [155.3, 0.4, 218.1, -94.04, 59.96, -93.32]

XYが完全に別の場所になった。RXYZが変わると、IKが全く違う解を出す。

根本問題：RXYZは位置によって変化するのに固定値を渡し続けているため、IKが不安定な解を出す。

今日取った複数点のRXYZを見ると：

左上位置: RX=-77.95, RY=55.72, RZ=167.57
右上位置: RX=-86.78, RY=60.48, RZ=37.35
左下位置: RX=-94.37, RY=56.79, RZ=-140.32
右下位置: RX=-95.93, RY=64.77, RZ=-43.79

RZが167から-140まで位置によって大きく変化する。これを固定値で渡すとIKが別解を選ぶ。

シミュレーションが必要な理由が明確になった

「任意のブロック位置に対応する関節角度データ」を実機で一点ずつ取るのは非効率だ。

Unityシミュレーションでマット全域にブロックをグリッド配置し、各位置でのPICK動作を実行して関節角度を記録する。これがSim-to-Realの本来の用途だ。

シミュレーション上の処理：
1. ブロックをマット全域にグリッド配置（例: 5cm間隔）
2. 各位置でIKを解いてPICK動作を実行
3. pixel座標 → angles の対応テーブルを出力

実機での処理：
1. カメラでブロックのpixel座標を取得
2. テーブルから最近傍のanglesを取得
3. send_angles()で確実に動作

homographyのパイプラインは正しく機能している。次のフェーズはUnityでこのデータを生成することだ。

現在のスクリプト構成

~/r2r2r-scripts/（Mac側）
  camera_pick_mac.py      カメラ追従ピック・Mac側（RXYZ最近傍選択実装済み）
  camera_pick_pi.py       カメラ追従ピック・Pi側（RXYZ実測値、Z実測値）
  dryrun_check.py         座標変換ドライラン確認（二重変換バグ修正済み）
  camera_check.py         カメラプレビュー（赤ブロック検出表示付き）
  detect_block_position.py 赤ブロック検出
  calibrate_homography.py  ホモグラフィキャリブレーション
  homography.npy           マット四隅実測値で再キャリブレーション済み

確定済みパラメータ：

# Z座標（実測値）
ABOVE_Z = 60.6
APPROACH_PICK_Z = 46.4
PICK_Z = 21.1

# SSH接続
SSH_HOST = "192.168.2.1"
SSH_KEY_PATH = "~/.ssh/id_rsa_pi"

# homographyキャリブレーション点（黄色い点4隅）
左上: pixel(216,  82)  → robot(-89.0,  -202.6)
右上: pixel(1547, 56)  → robot(-112.5,  204.6)
左下: pixel(330,  873) → robot(138.1,  -172.9)
右下: pixel(1459, 847) → robot(128.6,   182.9)