Unity FKスキャンは実機と一致しない。実測で確定した

動的lookupのパイプラインを通した。カメラで赤ブロックを検出し、homographyでUnity座標に変換し、姿勢制約付きnearest-neighbor検索で関節角度を引き、実機に送信する——全ステップが接続した。そして「下がらない」という目視確認で、根本問題がはっきりした。

💡 Key Takeaway

出発点

前回のセッションで test_full_sequence_day176.py による固定角度の通しシーケンスが完走した。今回のゴールは「カメラで赤ブロックを検出 → lookup → 動的PICKで把持」の接続だった。PLACEは固定角度のまま。

r2r2r_loop.py には2つの一時バイパスが残っていた。

# バイパス1（873〜875行目）
# camera_index = find_iriun_camera()
camera_index = 1  # 一時調査：index固定

# バイパス2（756〜757行目）
# current_angles = get_current_angles_from_pi()
current_angles = [0, 0, 0, 0, 0, -30]  # HOMEに固定

コードを読んで判断した。バイパス1（camera_index = 1）は以前のセッションで原因確定済み（find_iriun_camera() の複数open/close処理がIriunを混乱させる）。このまま触らない。バイパス2は build_pick_commands() の補間起点として使われているが、そもそも pick_cmds の最初のコマンドは {"action": "HOME"} でアームを必ずHOMEに戻す設計になっている。current_angles = HOME のハードコードは「バイパス」ではなく、すでに正解の実装だった。コードの修正は不要。問題はなかった。

Part1/Part2の分割送信

最初の実行で、APPROACH_PICKがブロックに当たった。

[Mac] Part1送信: APPROACH_PICKまで (3ステップ)
[Pi出力 Part1]:
受信コマンド数: 3
  [HOME] ホーム位置へ移動
  [GRIPPER_OPEN] グリッパーを開く
  [INTERPOLATED_MOVE] TO_APPROACH_PICK (5ステップ)
  ✓ ログ保存: /home/ubuntu/motion_log_20221013_231031.csv (3行)

[Mac] APPROACH_PICK到達。3秒待機...

[Mac] Part2送信: TO_PICK〜HOME (8ステップ)

Part1とPart2を別々のSSH接続で送信していた。この設計は「APPROACH_PICKで一度止まって青シール補正を行う」ための2分割だったが、補正機能はすでにコメントアウトされていた。不要な分割だった。

run_lookup_attempt 関数を修正して、全コマンドを1回の execute_lookup_commands() で送信するように統一した。

# 変更前（Part1/Part2分割）
success1, output1, error1 = execute_lookup_commands(part1)
time.sleep(3)
success2, output2, error2 = execute_lookup_commands(part2)

# 変更後（1回送信）
all_cmds = pick_cmds + place_cmds
print(f"[Mac] 全コマンド送信: {len(all_cmds)}ステップ")
success, output, error = execute_lookup_commands(all_cmds)

APPROACH_PICK_BASEの値ズレ

APPROACH_PICKの高さが改善されず、ブロックに当たる問題が残った。

APPROACH_PICK_BASE の j1〜j5 と確定値を比較した。

# 修正前（古い値が入っていた）
APPROACH_PICK_BASE = [None, 81.91, -30.14, -8.08, -50.27, -29.44]

# 修正後（確定APPROACH_PICK角度に合わせた）
APPROACH_PICK_BASE = [None, 87.74, -38.33, 3.6, -70.11, -30.0]

j1が 81.91 → 87.74、j2が -30.14 → -38.33 と大きくずれていた。この修正でAPPROACH_PICKの高さが改善された。

INTERP_SLEEP = 0.05

Pi側 pick_and_place_fixed.py の execute_interpolated_motion 関数：

INTERP_SLEEP = 0.05  # 50ms

def execute_interpolated_motion(angle_list, label="MOTION"):
    """補間済み角度リストを順に実行"""
    for i, angles in enumerate(angle_list):
        mc.send_angles(angles, SPEED)
        time.sleep(INTERP_SLEEP)  # 50msしか待っていない
    # 最終角度で安定待ち
    time.sleep(2)
    log_angles(label)

5ステップの補間を50ms間隔で連続送信していた。SPEED=20でアームが各ステップを動く時間は数秒かかるのに、次のコマンドが250ms後には全部届いてしまう。実質的に最後の補間角度だけに到達しようとして途中の動きがスキップされていた。

INTERP_SLEEP = 1.2 に変更したが、動きが遅くなって位置もずれる結果になった。根本原因の分析に戻り、最終的に 0.05 に戻した。

判明した真の原因は lookup が正しい角度を返していないことで、INTERP_SLEEP は別問題だった。

lookup が正しい姿勢を返さない

PICK_EE_Y フィルタの設定値が間違っていた。

# 修正前
PICK_EE_Y = 0.027   # 27mm
EE_Y_TOL  = 0.020   # ±20mm → フィルタ範囲 7〜47mm

CSV内でj0=-46.93、j1=87.74の確定PICK姿勢に最も近い行を検索すると：

ee_x: 0.208033
ee_y: 0.198647  ← 確定PICK姿勢の実際の高さ
ee_z: 0.233708
該当行: j0=-46.930, j1=87.740, j2=-58.330, j3=-11.400, j4=-75.110, j5=-30.000

ee_y = 0.1986m（198.6mm）。PICK_EE_Y=0.027のフィルタ範囲（7〜47mm）の完全に外だった。 lookupがそもそも正しい高さの点を探せていなかった。

PICK_EE_Y = 0.199、EE_Y_TOL = 0.030 に修正。

さらに、この状態でも lookup が j1=115° 台の望ましくない姿勢を返していた。

PICK angles=[-27.0, 115.0, -59.0, 3.5, -45.0, -30.5]

確定PICK姿勢の j1=87.74 と比べて27°も開きすぎ。6軸アームのIKには冗長解があり、「同じee座標に対して姿勢が全然違う複数の解」がCSVに含まれている。XZ距離だけで最近傍を選んでいた lookup 関数が、たまたま近いが姿勢が間違った点を返していた。

姿勢制約付きlookup の実装

lookup_nearest_angles 関数に姿勢制約フィルタを追加した。確定PICK姿勢の j2〜j5 に近い候補だけを残し、その中から XZ距離で最近傍を選ぶ設計。

さらに、PICKとPLACEで適切な ee_y が異なるため、target_ee_y を引数で受け取る設計に変更した。_fk_filtered（起動時に事前フィルタ済みの点集合）ではなく _fk_table（全点）を対象に、関数内で ee_y 範囲フィルタを行う。

PICK_EE_Y = -0.040    # PICK高さ（m）※キャリブレーション検証のため調整
PLACE_EE_Y = 0.090    # PLACE高さ（m）白箱の中

def lookup_nearest_angles(unity_x, unity_z, target_ee_y, label=""):
    """
    姿勢制約付きnearest-neighbor検索（target_ee_yで適切な高さに絞る）。
    """
    TARGET_J2 = -33.33
    TARGET_J3 = 3.6
    TARGET_J4 = -45.11
    TARGET_J5 = -30.0
    TOL_J2 = 15.0
    TOL_J3 = 15.0
    TOL_J4 = 15.0
    TOL_J5 = 10.0
    EE_Y_RANGE = 0.015  # ±15mm

    # 姿勢制約 + ee_y範囲フィルタ（全点を対象）
    pose_filtered = [
        (ee_x, ee_y, ee_z, joints) for ee_x, ee_y, ee_z, joints in _fk_table
        if abs(ee_y - target_ee_y) <= EE_Y_RANGE
        and abs(joints[2] - TARGET_J2) <= TOL_J2
        and abs(joints[3] - TARGET_J3) <= TOL_J3
        and abs(joints[4] - TARGET_J4) <= TOL_J4
        and abs(joints[5] - TARGET_J5) <= TOL_J5
    ]
    print(f"[Mac] {label} 姿勢制約後候補数: {len(pose_filtered)}点")

    if not pose_filtered:
        print(f"[Mac] ⚠ {label} 姿勢制約後に候補なし")
        return None, float("inf")

    # XZ平面2D距離でnearest-neighbor
    best = None
    best_dist = float("inf")
    for ee_x, ee_y, ee_z, joints in pose_filtered:
        dist = math.sqrt((ee_x - unity_x) ** 2 + (ee_z - unity_z) ** 2)
        if dist < best_dist:
            best_dist = dist
            best = (ee_x, ee_y, ee_z, joints)

    if best is None or best_dist > DIST_THRESHOLD:
        print(f"[Mac] ⚠ {label} lookup失敗: dist={best_dist*1000:.1f}mm > {DIST_THRESHOLD*1000:.0f}mm")
        return None, best_dist

    ee_x, ee_y, ee_z, joints = best
    print(f"[Mac] ✓ {label} lookup成功: ee=({ee_x:.4f}, {ee_y:.4f}, {ee_z:.4f}) dist={best_dist*1000:.1f}mm")
    print(f"[Mac]   {label} angles={[round(j, 1) for j in joints]}")
    return joints, best_dist

呼び出し側も変更。

# PICK lookup
pick_angles, pick_dist = lookup_nearest_angles(
    pick_unity_x, pick_unity_z, PICK_EE_Y, label="PICK")

# PLACE lookup
place_angles, place_dist = lookup_nearest_angles(
    place_unity_x, place_unity_z, PLACE_EE_Y, label="PLACE")

PICK_EE_Y の段階的検証ログ

ここから実測の話になる。

PICK_EE_Y を -0.040（マイナス40mm）まで下げても、実機グリッパーの高さは変わらなかった。

最終実行のログ（PICK_EE_Y = -0.040）：

[Mac] PICK Unity座標: (0.1962, 0.2384)
[Mac] PICK 姿勢制約後候補数: 172点
[Mac] ✓ PICK lookup成功: ee=(0.1830, -0.0547, 0.2019) dist=38.9mm
[Mac]   PICK angles=[-32.0, 120.0, -34.0, 13.5, -60.0, -30.5]
[Mac] PLACE Unity座標: (-0.2509, 0.1830)
[Mac] PLACE 姿勢制約後候補数: 10494点
[Mac] ✓ PLACE lookup成功: ee=(-0.2617, 0.0929, 0.1929) dist=14.3mm
[Mac]   PLACE angles=[57.0, 73.9, -22.1, 7.9, -58.3, -29.4]
[Mac] 現在角度: [0, 0, 0, 0, 0, -30]
[Mac] PLACE: 動的lookup角度を使用
[Mac] 全コマンド送信: 11ステップ

実測で分かったこと

Unity FKスキャンのee_y = -0.055m（マット下55mm）という角度を実機に送っても、グリッパーはマット表面まで降りなかった。

この結果が意味することは一つだ。Unity上でのFK計算（関節角度→グリッパー先端座標）と、実機での実際の動きが一致していない。

CSVの分析で確認した範囲：

全点 ee_y range: -0.1820 〜 0.3239m
姿勢制約後 ee_y range: -0.1820 〜 0.3239m
姿勢制約 + XZ50mm以内: 8912点
  ee_y最小: -0.0640m (XZ距離 46.8mm)
  ee_y最大: 0.1572m (XZ距離 36.9mm)
  うち ee_y < 0.020m: 3704点

CSVには低い高さの点が3704点ある。データのカバレッジ問題ではない。 角度を送っても実機が下がらない。

Unity FK ↔ 実機の不一致

R2R2Rパイプラインの構造を改めて見ると：

[Unity URDF] → [Unity FK計算] → [CSV] → [Mac lookup] → [Pi 実機]
      ↑                                                       ↑
  ここの寸法/Joint定義が                              同じ角度を受け取っても
  実機と一致していないと                              実際のグリッパー位置が違う
  どんなにlookupを頑張っても合わない

座標系定義は以前のセッションで恒久化した。しかしそれは「マット上の点とUnity原点の対応」だった。「joint角度→グリッパー先端座標の対応（FK）が実機と一致するか」は検証していなかった。

確定済みの固定角度（実機で手動調整した値）を直接送っていたフェーズは、Unity FKの精度問題は表に出なかった。動的lookupに切り替えた瞬間、「Unity FKを信じる」フェーズが始まり、初めてズレが露呈した。

可能性のある原因として3つが考えられる。Unity URDFのリンク長と実機の実寸法の不一致（設計値と製造誤差）、Joint軸の定義（向き・回転方向）がUnityと実機でズレていること、そしてUnity座標原点と実機ロボット座標原点のオフセットだ。

今回の収穫

「動的lookupのPICKが正しく動くまで」がゴールだった。完成しなかった。

しかし必要な作業を全部やり切った上での発見だった。カメラ→homography→Unity座標変換が完璧に動作し、lookup姿勢制約（j2〜j5フィルタ）を実装し、target_ee_y を引数化してPICK/PLACEで個別設定できるようにし、Part1/Part2分割を統合し、PICK_EE_Y を -0.040 まで段階的に調整しながら実測した。その末に「EE_Yを下げても実機は下がらない」という事実が確定した。

Unity FKスキャンが実機と一致しない——これはlookup側で解決できる問題ではない。デジタルツインの精度を上げることが、Phase 2の前提条件だ。

次の課題

まず実機でキャリブレーション計測を行う。HOME・確定APPROACH_PICK・確定PICKの3点でグリッパー先端位置をマット上で実測する（メジャー計測）。次にUnity URDFで同じ角度を入れてee座標を取得し、実測値と比較してズレの傾向を特定する。一律オフセットなら原点調整、リンク長ズレならURDF修正、軸定義ズレなら変換行列追加、という手順で修正する。修正後にFKスキャンを再生成して動的PICKを再検証する。

キャリブレーション点は最低3点あれば変換行列を推定できる。

※ プロジェクト名「R2R2R」は、UC Berkeley・Toyota Research Instituteの研究「Real-to-Render-to-Real (R2R2R)」にインスパイアされた個人実験プロジェクトです。当プロジェクトは同研究機関とは無関係です。