LOG144R2R2R2026/4/12

Unity × ROS2で座標変換の設計矛盾を根本から直した｜mechArm 270 R2R2R

カメラ追従ピックのフルループを初実行し、座標変換パイプラインの設計矛盾を発見した。解決策は「Mac側で全変換を完結させる」という一本化だった。

💡 Key Takeaway

send_angles() から send_coords() への統一でカメラ追従は正しく機能した。問題は座標変換の設計にあった。「カメラのWorld座標」と「ロボットのRobot座標」は別物で、Mac/Pi二段階変換がその違いを曖昧にしていた。設計方針を「Macで変換を完結させ、Piは受け取った値をそのまま使う」に統一した。

やったこと	起きたこと	判断
camera_pick_pi.pyのPICKをsend_coords()に統一	Pi単体テストで(138.8, -167.5)に正確到達	カメラ追従情報がPICKまで維持されることを確認
camera_pick_mac.py → SSH → camera_pick_pi.pyのフルループを実行	アームがブロックの位置に行かない	座標変換パイプラインに設計矛盾があると特定
calibrate_homography.pyで4点をRobot座標で入力してキャリブレーション	それでもアームがブロック位置に行かない	homography.npyが何を出力しているかを次の確認課題に設定
「Macで変換完結・Piは受け取るだけ」の設計方針を確定	設計の整合性が取れた	次回homography.npy実態確認後に実装する

send_angles()がカメラ追従情報を捨てていた

— コードは動いていた。でも意図通りには動いていなかった。

LOG143で発見した設計矛盾を修正するところから始まった。

camera_pick_pi.py のピック動作は、こういう流れになっていた。

mc.send_coords([x, y, 120.0, rx, ry, rz], 20, 0)  # 真上（カメラ追従）
mc.send_coords([x, y,  49.9, rx, ry, rz], 20, 0)  # APPROACH（カメラ追従）
mc.send_angles(PICK_ANGLES, SPEED)                  # ← ここで追従情報が捨てられる
mc.set_gripper_state(1, 70)

send_angles() は関節角度を直接指定するコマンドだ。ブロックがどこにあろうと、常に同じ固定位置に動く。カメラがブロックを検出して座標を計算しても、ピックの瞬間だけハードコードされた位置に上書きされる構造だった。

修正はシンプルだった。

# PICK位置に移動（send_coordsでカメラ追従座標を維持）
pick_coords = [robot_x, robot_y, PICK_Z, PICK_RX, PICK_RY, PICK_RZ]
print(f"[Pi] PICK位置に移動: {pick_coords}")
mc.send_coords(pick_coords, SPEED, MODE)
time.sleep(8)

PICKまで send_coords() で統一することで、カメラが検出した座標がPICKの瞬間まで維持される。

Pi単体テストで確認した。

python3 camera_pick_pi.py -258.5 204.8

[Pi] World座標受け取り: world_x=-258.5mm, world_y=204.8mm
[Pi] ✓ ロボット座標に変換: robot_x=138.8mm, robot_y=-167.5mm
[Pi] mechArm接続中...
[Pi] ✓ mechArm接続完了
[Pi] グリッパーを開く...
[Pi] 真上に移動: [138.8, -167.5, 120.0, -97.14, 55.44, -143.21]
[Pi] APPROACH_PICK高さに移動: [138.8, -167.5, 49.9, -97.14, 55.44, -143.21]
[Pi] PICK位置に移動: [138.8, -167.5, 35.1, -97.14, 55.44, -143.21]
[Pi] グリッパーを閉じる...
[Pi] APPROACH_PICK姿勢に戻る...
[Pi] ホームに戻る...
[Pi] ✓ Camera Pick完了

(138.8, -167.5) は実績のある確定済みPICK座標だ。全ステップが正しく動いた。

フルループを実行したら、アームがブロック位置に行かなかった

— 問題は「どこへ動くか」ではなく「どこへ動くよう指示しているか」だった。

camera_pick_mac.py からフルループを走らせた。

=== Camera Pick Start ===
[Mac] ブロック位置を検出中...
[Mac] ✓ ブロック検出: pixel_x=326, pixel_y=731
[Mac] ✓ World座標: world_x=-254.1mm, world_y=140.2mm
[Mac] Pi側スクリプトを転送中...
[Mac] Pi側スクリプトを実行中...
[Pi出力]:
[Pi] World座標受け取り: world_x=-254.1mm, world_y=140.2mm
[Pi] ✓ ロボット座標に変換: robot_x=134.4mm, robot_y=-102.9mm
[Pi] 真上に移動: [134.4, -102.9, 120.0, -97.14, 55.44, -143.21]
[Pi] APPROACH_PICK高さに移動: [134.4, -102.9, 49.9, -97.14, 55.44, -143.21]
[Pi] PICK位置に移動: [134.4, -102.9, 35.1, -97.14, 55.44, -143.21]
[Pi] ✓ Camera Pick完了
[Mac] ✓ Camera Pick完了

スクリプトはエラーなく完走した。でもアームはブロックの位置に行かなかった。

何が起きているかを調べるため、フリームードでアームを手動でブロックの真上に持っていき、get_coords() で実際の座標を読んだ。

mc.release_all_servos()
# → アームを手動でブロック位置へ移動
print(mc.get_coords())
# → [95.8, -153.2, 24.6, -70.29, 61.3, -138.18]

カメラのWorld座標:        (-267.7, 137.0)
ロボットの実際の座標:     (95.8, -153.2)

全く別の数値だ。

二つの座標系が混在していた

— 「カメラのWorld」と「ロボットのRobot」は別物だった。

問題を分解するとこうなっていた。

カメラ側:
  pixel → homography → World座標
  World座標の基準: ロボットベース原点、マット目盛り基準のmm

ロボット側:
  send_coords()が期待する値: mechArmのRobot座標
  Robot座標の基準: mechArmのファームウェアが定義する独自の座標系

この二つは原点も向きも単位も一致していない。線形変換式（robot_x = -world_x + offset）で繋ごうとしたが、1点のキャリブレーションから導出したオフセットは別の位置で破綻する。

実際に試した数値を整理するとこうなっている。

試行1:
  検出   World(-254.1, 140.2) → 計算 robot(134.4, -102.9)
  実際のブロック位置 robot(95.8, -153.2)
  → X方向に約+38mm、Y方向に約+50mmのずれ

試行2（別位置）:
  検出   World(-262.4, 143.4) → 計算 robot(142.7, -106.1)
  → アームがブロック位置に到達せず

試行3（フリームード実測→オフセット更新後）:
  World(-267.7, 137.0) → 実測 robot(95.8, -153.2)
  COORD_OFFSET_X = -171.9, COORD_OFFSET_Y = -16.2 に更新
  → 別の位置で再び破綻

dryrun_check.py を新規作成して座標の流れを可視化した。

[DRY RUN] pixel:    (1349, 635)
[DRY RUN] World:    (59.3 mm, 137.6 mm)     ← homographyの出力
[DRY RUN] Robot:    (-219.2 mm, -165.8 mm)  ← さらに変換した値

ところが実際にPiが動かした座標は (59.3, 137.6) だった。計算したRobot座標 (-219.2, -165.8) は渡っていなかった。

Mac/Pi分割で変換が二重になっていた。

Robot座標で4点キャリブレーションを実施した

— 根本から考え直した結果、calibrate_homography.pyに戻ってきた。

「homography.npy が pixel → Robot座標の変換行列になれば、変換式が不要になる」という方針で、4点のRobot座標を実測してキャリブレーションをやり直した。

フリームードでアームを4点に移動させ、各点で get_coords() を読んだ。

左上 pixel(238, 84)   → get_coords() → robot(-93.8,  -202.4)
右上 pixel(1563, 71)  → get_coords() → robot(-111.5,  193.8)
左下 pixel(353, 858)  → get_coords() → robot(141.9,  -155.9)
右下 pixel(1462, 846) → get_coords() → robot(120.1,   172.4)

同じ4点のWorld座標を detect_block_position.py で取得した。

左上: World(-300.8, -155.0)
右上: World( 308.3, -151.2)
左下: World(-255.6,  206.7)
右下: World( 257.5,  209.4)

calibrate_homography.py でRobot座標を入力してキャリブレーションした結果、新しい 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]]

これで homography.npy は「pixel → Robot座標」の変換行列になるはずだった。しかし実行してみると依然としてアームがブロック位置に行かなかった。

homography.npy が実際に何を出力しているかを直接確認する必要がある。これが次の確認課題だ。

設計方針を一本化した

— 複雑さはバグを生む。シンプルにする。

問題の構造を整理するとこうなる。

旧設計:
  Mac: pixel → homography → World座標 → Piへ送信
  Pi:  World座標 → 変換式 → Robot座標 → send_coords()

問題点:
  ・変換が2箇所に分散
  ・homography.npyはMacにしか存在せずPiで使えない
  ・「World座標」と「Robot座標」の区別が曖昧になる

新設計:
  Mac: pixel → homography(Robot座標直接出力) → Piへ送信
  Pi:  受け取った値をそのままsend_coords()に渡す

変換は1箇所に集約。Piは変換しない。

camera_pick_pi.py の座標変換部分はこうなる。

# 変換なし。受け取った値をそのままRobot座標として使用
robot_x = world_x
robot_y = world_y

camera_pick_mac.py 側でhomographyの出力をRobot座標としてそのままPiに渡す。

# Mac側で変換を完結させる
robot_xy = pixel_to_world((pixel_x, pixel_y), homography_matrix)
robot_x, robot_y = robot_xy[0], robot_xy[1]
print(f"[Mac] ✓ Robot座標: robot_x={robot_x:.1f}mm, robot_y={robot_y:.1f}mm")

# PiへはRobot座標をそのまま渡す
command = f"python3 {REMOTE_SCRIPT} {robot_x:.1f} {robot_y:.1f}"

次のステップ

Piが手元に戻ったら、まず以下を実行して homography.npy の実態を確認する。

python3 -c "import numpy as np; print(np.load('homography.npy'))"

この行列が「pixel → Robot座標」の変換として正しく機能していることを確認してから、camera_pick_mac.py と camera_pick_pi.py を新設計に沿って修正する。

変換は1箇所に集約する。Macが全変換を担い、Piは受け取った値をそのまま動作に使う。それだけだ。

コード全文

camera_pick_pi.py（send_coords統一版）

#!/usr/bin/env python3
"""
Camera Pick Pi側スクリプト
World座標を受け取り、座標変換してアームをPICK位置に動かす
"""

import sys
import time
from pymycobot.mycobot import MyCobot

COORD_OFFSET_X = -119.7
COORD_OFFSET_Y = 37.3
ABOVE_Z = 120.0
APPROACH_PICK_Z = 49.9
PICK_Z = 35.1

PICK_RX = -97.14
PICK_RY = 55.44
PICK_RZ = -143.21

# PICK_ANGLES = [-46.93, 102.74, -43.33, 3.6, -60.11, -31.9]
APPROACH_PICK_ANGLES = [-54.22, 81.91, -30.14, -8.08, -50.27, -29.44]

SPEED = 20
MODE = 0


def main():
    if len(sys.argv) != 3:
        print("[Pi] ✗ 使用方法: python3 camera_pick_pi.py <world_x> <world_y>")
        sys.exit(1)

    world_x = float(sys.argv[1])
    world_y = float(sys.argv[2])
    print(f"[Pi] World座標受け取り: world_x={world_x:.1f}mm, world_y={world_y:.1f}mm")

    robot_x = -world_x + COORD_OFFSET_X
    robot_y = -world_y + COORD_OFFSET_Y
    print(f"[Pi] ✓ ロボット座標に変換: robot_x={robot_x:.1f}mm, robot_y={robot_y:.1f}mm")

    mc = MyCobot('/dev/ttyAMA0', 1000000)
    time.sleep(3)
    print("[Pi] ✓ mechArm接続完了")

    print("[Pi] グリッパーを開く...")
    mc.set_gripper_state(0, 100)
    time.sleep(1)

    above_coords = [robot_x, robot_y, ABOVE_Z, PICK_RX, PICK_RY, PICK_RZ]
    print(f"[Pi] 真上に移動: {above_coords}")
    mc.send_coords(above_coords, SPEED, MODE)
    time.sleep(5)

    approach_coords = [robot_x, robot_y, APPROACH_PICK_Z, PICK_RX, PICK_RY, PICK_RZ]
    print(f"[Pi] APPROACH_PICK高さに移動: {approach_coords}")
    mc.send_coords(approach_coords, SPEED, MODE)
    time.sleep(5)

    pick_coords = [robot_x, robot_y, PICK_Z, PICK_RX, PICK_RY, PICK_RZ]
    print(f"[Pi] PICK位置に移動: {pick_coords}")
    mc.send_coords(pick_coords, SPEED, MODE)
    time.sleep(8)

    print("[Pi] グリッパーを閉じる...")
    mc.set_gripper_state(1, 70)
    time.sleep(2)

    print("[Pi] APPROACH_PICK姿勢に戻る...")
    mc.send_angles(APPROACH_PICK_ANGLES, SPEED)
    time.sleep(5)

    print("[Pi] ホームに戻る...")
    mc.send_angles([0, 0, 0, 0, 0, -30], 20)
    time.sleep(3)

    print("[Pi] ✓ Camera Pick完了")


if __name__ == "__main__":
    main()

camera_pick_mac.py（SSHタイムアウト修正版）

#!/usr/bin/env python3
"""
Camera Pick Mac側スクリプト
カメラでブロックのWorld XYを取得し、
SSH経由でPiのスクリプトを呼び出す
"""

import sys
import os
import time
import cv2
import numpy as np
import paramiko
from detect_block_position import detect_red_block, pixel_to_world

SSH_HOST = "192.168.2.1"
SSH_USER = "ubuntu"
SSH_KEY_PATH = os.path.expanduser("~/.ssh/id_rsa_pi")
REMOTE_SCRIPT = "/home/ubuntu/camera_pick_pi.py"


def execute_ssh_command(host, user, key_path, command):
    client = paramiko.SSHClient()
    client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    client.connect(host, username=user, key_filename=key_path, timeout=30)
    stdin, stdout, stderr = client.exec_command(command)
    exit_code = stdout.channel.recv_exit_status()
    output = stdout.read().decode('utf-8', errors='ignore')
    error = stderr.read().decode('utf-8', errors='ignore')
    client.close()
    return (exit_code == 0), output, error


def transfer_file_scp(host, user, key_path, local_path, remote_path):
    client = paramiko.SSHClient()
    client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    client.connect(host, username=user, key_filename=key_path, timeout=30)
    sftp = client.open_sftp()
    sftp.put(local_path, remote_path)
    sftp.close()
    client.close()
    return True


def main():
    print("=== Camera Pick Start ===")
    print("[Mac] ブロック位置を検出中...")

    homography_path = os.path.expanduser("~/r2r2r-scripts/homography.npy")
    homography_matrix = np.load(homography_path)

    cap = cv2.VideoCapture(1)
    if not cap.isOpened():
        print("[Mac] ✗ カメラが見つかりません")
        sys.exit(1)

    for _ in range(30):
        cap.read()

    centroid = None
    for i in range(10):
        ret, frame = cap.read()
        if not ret:
            continue
        centroid = detect_red_block(frame)
        if centroid is not None:
            break
        time.sleep(0.1)
    cap.release()

    if centroid is None:
        print("[Mac] ✗ ブロックが検出できませんでした")
        sys.exit(1)

    pixel_x, pixel_y = centroid
    print(f"[Mac] ✓ ブロック検出: pixel_x={pixel_x}, pixel_y={pixel_y}")

    world_xy = pixel_to_world((pixel_x, pixel_y), homography_matrix)
    world_x, world_y = world_xy[0], world_xy[1]
    print(f"[Mac] ✓ World座標: world_x={world_x:.1f}mm, world_y={world_y:.1f}mm")

    display_frame = frame.copy()
    cv2.circle(display_frame, (int(pixel_x), int(pixel_y)), 10, (0, 255, 0), 2)
    text = f"World: ({world_x:.1f} mm, {world_y:.1f} mm)"
    cv2.putText(display_frame, text, (int(pixel_x) + 15, int(pixel_y) - 10),
                cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
    cv2.imshow("Detected Block", display_frame)
    cv2.waitKey(2000)
    cv2.destroyAllWindows()

    print("[Mac] Pi側スクリプトを転送中...")
    local_script = os.path.join(os.path.dirname(__file__), "camera_pick_pi.py")
    if not transfer_file_scp(SSH_HOST, SSH_USER, SSH_KEY_PATH, local_script, REMOTE_SCRIPT):
        print("[Mac] ✗ ファイル転送失敗")
        sys.exit(1)

    print("[Mac] Pi側スクリプトを実行中...")
    command = f"python3 {REMOTE_SCRIPT} {world_x:.1f} {world_y:.1f}"
    success, output, error = execute_ssh_command(SSH_HOST, SSH_USER, SSH_KEY_PATH, command)

    if output:
        print(f"[Pi出力]:\n{output}")
    if error:
        print(f"[Piエラー]:\n{error}")

    if success:
        print("[Mac] ✓ Camera Pick完了")
    else:
        print("[Mac] ✗ Camera Pick失敗")
        sys.exit(1)


if __name__ == "__main__":
    main()

dryrun_check.py（新規作成）

#!/usr/bin/env python3
"""
Dry Run Check スクリプト
カメラでブロック位置を検出し、座標変換結果を表示（アームは動かさない）
"""

import os
import time
import cv2
import numpy as np
from detect_block_position import detect_red_block, pixel_to_world

COORD_OFFSET_X = -159.9
COORD_OFFSET_Y = -28.2
PICK_RX = -97.14
PICK_RY = 55.44
PICK_RZ = -143.21
ABOVE_Z = 120.0
APPROACH_PICK_Z = 49.9
PICK_Z = 35.1


def main():
    print("=== Dry Run Check Start ===")

    homography_path = os.path.expanduser("~/r2r2r-scripts/homography.npy")
    homography_matrix = np.load(homography_path)

    cap = cv2.VideoCapture(1)
    if not cap.isOpened():
        print("[DRY RUN] ✗ カメラが見つかりません")
        return

    for _ in range(30):
        cap.read()

    centroid = None
    for i in range(10):
        ret, frame = cap.read()
        if not ret:
            continue
        centroid = detect_red_block(frame)
        if centroid is not None:
            break
        time.sleep(0.1)
    cap.release()

    if centroid is None:
        print("[DRY RUN] ✗ ブロックが検出できませんでした")
        return

    pixel_x, pixel_y = centroid
    print(f"[DRY RUN] pixel: ({int(pixel_x)}, {int(pixel_y)})")

    world_xy = pixel_to_world((pixel_x, pixel_y), homography_matrix)
    world_x, world_y = world_xy[0], world_xy[1]
    print(f"[DRY RUN] World: ({world_x:.1f} mm, {world_y:.1f} mm)")

    robot_x = -world_x + COORD_OFFSET_X
    robot_y = -world_y + COORD_OFFSET_Y
    print(f"[DRY RUN] Robot: ({robot_x:.1f} mm, {robot_y:.1f} mm)")

    print(f"[DRY RUN] ABOVE:    [{robot_x}, {robot_y}, {ABOVE_Z}, {PICK_RX}, {PICK_RY}, {PICK_RZ}]")
    print(f"[DRY RUN] APPROACH: [{robot_x}, {robot_y}, {APPROACH_PICK_Z}, {PICK_RX}, {PICK_RY}, {PICK_RZ}]")
    print(f"[DRY RUN] PICK:     [{robot_x}, {robot_y}, {PICK_Z}, {PICK_RX}, {PICK_RY}, {PICK_RZ}]")
    print("[DRY RUN] ✓ Dry Run Check完了")


if __name__ == "__main__":
    main()

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