PICK実機動作で3つの問題を分解し、青シール計測で19mm誤差を実測した | mechArm 270 × iPhone × Unity FKスキャン

PICK実機動作の目視確認だけのつもりだった。「爪が赤ブロックに届くか」それだけ。実際には、カメラ縦横切り替わり・補間低軌道横滑り・爪オフセットという3つの根本問題を順に分解することになった。最後は青シール計測インフラを構築して、目標との誤差を実測値19mmまで詰めた。

💡 Key Takeaway

「PICK実機が爪をブロックに届かせない」という現象を、独立した3レイヤーの問題に切り分け、最終的に青シール3点を使った実測インフラで、Unity FKスキャンEEと実機爪先中点の幾何学的整合性を定量評価できる状態にした。memoryに記録していた「爪オフセット約90mm」は誤りで、実測19mm程度だった。

当初の目的：「爪が赤ブロックに届くか」だけ

このセッションを開始する時点で、引き継ぎ書には1行だけ書いてあった。

PICK実機動作の確認。赤ブロックにアームの爪が届くかを目視する。それだけ。

前のセッションで、SSH疎通・カメラindex自動検出（find_iriun_camera() を camera_utils.py に共通モジュール化）・homography再キャリブレーション（最大誤差0.00mm）まで完了していた。

ここから本来は python3 r2r2r_loop.py を1回実行して、PICK動作の物理的な成否を目視で確認するだけの予定だった。

実際には、起動3〜5秒で予想していなかった現象が起きた。

第1の問題：起動するとiPhoneが通知音を鳴らす

スクリプトは走らせるとiPhoneのカメラの向きが横から縦に切り替わってしまう現象が出てしまうので、何が原因で起こっているか確認したい。

最初の症状はこれだった。机の上に固定されているiPhoneは物理的に動いていない。なのにIriun Webcamの表示だけが90°回転する。さらに、スクリプト起動から3〜5秒経つと、iPhoneから「ピコン」という通知音が鳴る。

ログを確認する。

[Camera] Detected Iriun at index=1 (brightness=131.8, shape=(1080, 1920, 3))
[Camera] WARNING: 複数の候補が見つかりました。index=1 を採用。他の候補: index=2(b=56.8)

find_iriun_camera() の実装は共通モジュール化されている：

#!/usr/bin/env python3
"""
camera_utils.py  DAY168
カメラindex自動検出の共通モジュール。
Iriun Webcam の cv2 index は macOS の都合で変動するため、
起動時に brightness + shape で動的に検出する。
"""

import cv2


def find_iriun_camera():
    """
    Iriun Webcam の cv2 index を自動検出する。

    判定条件（すべて満たす最小のindexを採用）:
      - cap.read() が成功 (ret=True)
      - shape == (1080, 1920, 3)
      - frame.mean() > 50
    """
    candidates = []
    for i in range(5):
        cap = cv2.VideoCapture(i)
        ret, frame = cap.read()
        if ret and frame.shape == (1080, 1920, 3):
            brightness = frame.mean()
            if brightness > 50:
                candidates.append((i, brightness))
        cap.release()

    if not candidates:
        raise RuntimeError(
            "Iriun Webcam が見つかりません。\n"
            "iPhone側で Iriun Webcam アプリを起動してから再実行してください。"
        )

    selected_index, selected_brightness = candidates[0]
    print(f"[Camera] Detected Iriun at index={selected_index} "
          f"(brightness={selected_brightness:.1f}, shape=(1080, 1920, 3))")

    if len(candidates) > 1:
        others = [f"index={c[0]}(b={c[1]:.1f})" for c in candidates[1:]]
        print(f"[Camera] WARNING: 複数の候補が見つかりました。"
              f"index={selected_index} を採用。他の候補: {', '.join(others)}")

    return selected_index

この実装は range(5) で index 0〜4 を順番に cv2.VideoCapture(i) で開いて、cap.read() してフレームの形状と明るさで Iriun を判定し、cap.release() する。Iriunに該当する候補（index 1 と 2）を両方とも一度開いて閉じている。そのあと r2r2r_loop.py の871行目付近で index=1 を再度開く。

仮説：iPhone側のIriunアプリは「USBカメラの新規接続イベント」を複数回受信していて、その都度通知音を鳴らし、向き状態をリセットしている可能性がある。

検証は単純な切り分けで済んだ。r2r2r_loop.py の871行目をコメントアウトして camera_index = 1 を直接指定する。

# 変更前:
camera_index = find_iriun_camera()

# 変更後（DAY168 一時調査）:
# camera_index = find_iriun_camera()
camera_index = 1

このバイパスでスクリプトを再実行：

• 通知音 ❌ 鳴らなかった

• Iriun Webcam の向き ✅ 横向きのまま、切り替わらず

切り分け完了。原因は find_iriun_camera() の複数 open/release 処理で確定。一時バイパスのまま、commitせずに次へ進んだ。

副次的にマット範囲検出の精度も上がった。前回は X=[169, 1919] Y=[0, 1079] と画面端まで張り付いていたものが、X=[364, 1658] Y=[29, 983] と適切な範囲に収まった。カメラ向きが本来の状態だったから。

第2の問題：lookup成功なのにアームが低軌道横滑り

カメラ問題を回避してPICK動作を実行。ログには成功が並ぶ。

[Mac] ✓ 赤ブロック検出: pixel=(549, 634)
[Mac] Unity座標: (0.2147, 0.1925)
[Mac] ✓ PICK lookup成功: dist=50.0mm
[Mac]   PICK angles=[-55.0, 100.0, -55.0, -2.0, -35.0, -20.0]
[Mac] Part1送信: APPROACH_PICK まで（3ステップ）
[Pi] HOME 完了
[Pi] GRIPPER_OPEN 完了
[Pi] INTERPOLATED_MOVE "TO_APPROACH_PICK" 開始

数値上は完璧だ。lookup成功 dist=50mm、PICK角度確定、Part1送信完了。

なのに目視では、アームが低い位置のまま横滑りでブロックを飛ばす。APPROACH_PICK の上空姿勢に到達していない。

ここで仮説検証の順序を間違えないように、まずPi側スクリプトの実装を確認した。Pi側 pick_and_place_fixed.py の run_lookup_mode() は、INTERPOLATED_MOVE を受け取ると execute_interpolated_motion(interp_list) を呼び、Mac側から渡された interp_list を順に send_angles() するだけ：

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

つまり、補間はMac側で計算されている。Pi側は受け取ったリストを再生するだけ。

次にMac側の補間構築を確認した。r2r2r_loop.py の make_approach_angles() (670行目) と build_pick_commands() (677行目)。構造は以下：

def make_approach_angles(pick_angles):
    """APPROACH_PICK の角度を構築"""
    # J0 = lookup PICK の J0（ブロック方向を向く）
    # J1〜J5 = APPROACH_PICK_BASE の固定値
    return [pick_angles[0]] + APPROACH_PICK_BASE[1:]

def build_pick_commands(pick_angles, current_angles):
    approach_angles = make_approach_angles(pick_angles)
    interp_to_approach = interpolate_angles(current_angles, approach_angles, n_steps=5)
    interp_to_pick     = interpolate_angles(approach_angles, pick_angles, n_steps=5)
    return [
        {"action": "HOME"},
        {"action": "GRIPPER_OPEN"},
        {"action": "INTERPOLATED_MOVE", "label": "TO_APPROACH_PICK", "interp_list": interp_to_approach},
        {"action": "INTERPOLATED_MOVE", "label": "TO_PICK",          "interp_list": interp_to_pick},
    ]

ここで決定的な事実が見えた。

current_angles は補間構築時点で Pi から取得した実機角度 だった。例：[13.97, 92.81, 2.37, 13.53, -96.15, -27.94]。これは前回PICKが終わった後の中途半端な姿勢。

一方、Pi側の実行順は：

1. HOME アクション → mc.send_angles(HOME, SPEED) で [0, 0, 0, 0, 0, -30] に直接移動

2. GRIPPER_OPEN

3. INTERPOLATED_MOVE "TO_APPROACH_PICK" → Mac側で計算済みの補間リストを順次再生

Mac側はステップ1の前に current_angles を取得し、その中途半端な姿勢を補間の起点として補間リストを作っている。Pi側はその補間リストの1点目から再生する。

つまり時系列は：

Pi: HOME姿勢 [0, 0, 0, 0, 0, -30] に到達
   ↓（補間リストの1点目にいきなりジャンプ）
Pi: 補間1点目 [-44.0, 96.6, -7.0, -3.5, -41.1, -29.6]
   ↓
Pi: 補間2点目 [-50.0, 92.8, -1.6, -3.6, -42.5, -29.8]
   ↓
...
Pi: APPROACH_PICK [-54.22, 81.91, -30.14, -8.08, -50.27, -29.44]

補間1点目はHOME姿勢から外れた、中途半端な姿勢の延長線上。HOMEに行ったあとそこへいきなりジャンプするから、低い高度の軌跡で横移動が起きる。アームの先端が低空を横滑りする。

切り分けは r2r2r_loop.py の755行目付近を一時バイパス：

# 変更前:
current_angles = get_current_angles_from_pi()

# 変更後（DAY168 一時調査）:
# current_angles = get_current_angles_from_pi()
current_angles = [0, 0, 0, 0, 0, -30]  # HOME固定

これで補間起点とPi到達姿勢が一致。再実行すると軌道は改善した。原因は current_angles の取得タイミングと、Pi側の HOME 到達とのズレ。これも一時バイパスで進行、commitせず。

第3の問題：それでも爪が赤ブロックに届かない

カメラと補間という2つの障害物を取り除いて、いよいよPICK実機の動作を見る。

アームは正しい姿勢で降下する。APPROACH_PICKに上空到達、PICKに降下。けれど、爪先は赤ブロックの隣を素通りした。目視で明らかに数センチずれている。

memoryには「爪オフセット問題：EE は wrist で定義されているが実際の jaw tip は約 90mm 先」と記録されていた。前のセッションでこれを根本問題として残していた。

しかしその記録に裏取りはあったか？ FK スキャナーの実装を見直すべきだった。

Claude Code に Unity 側 FK スキャナーの全4ファイル（FKScanner_Central.cs, FKScanner_XPos.cs, FKScanner_XNeg.cs, FKScanner_MotionBased.cs）を確認させた。EE 座標の計算箇所はすべて共通だった：

Vector3 midpoint = (cfg.leftTipBrep.position + cfg.rightTipBrep.position) * 0.5f;
Vector3 ee = midpoint - cfg.rootArticulationBody.transform.position;

つまり：

• EE = leftTipBrep と rightTipBrep の 中点

• link6_flange（手首）ではない

• グリッパ左右先端の中点を、ロボット root 位置からの相対座標（m単位）で記録

• これが fk_scan_motion_based.csv の ee_x, ee_y, ee_z カラム

memoryの「EE = 手首」は誤りだった。FKスキャナーが記録するEEは最初からグリッパ先端中点だった。

しかし、その事実がわかると今度は別の謎が残る。EE定義は正しそうなのに、なぜ実機で爪が届かないのか？

ここでさらに踏み込んで、Unity 側の leftTipBrep / rightTipBrep の Hierarchy を Claude Code に追わせた。

確定した事実は次のとおり：

1. leftTipBrep / rightTipBrep は CAD メッシュ由来の Brep（境界表現）オブジェクト
   Hierarchy: gripper_left1 → Instance-Def-210103_4302_101 → Brep
   URDFインポート時にCADデータから自動生成されたメッシュ。Inspectorのlocal positionはハードコードなし、CADデータ依存。

2. Unity側に GripperController.cs がある。これが gripper_left1/2/3 と gripper_right1/2/3 を Y軸回転で制御している。Brep は gripper_left1 の子孫なので、親の回転に連動して位置が変動する。

• グリッパが開いた状態 → leftTipBrep / rightTipBrep は離れた位置

• グリッパが閉じた状態 → 中央に寄る

• GripperController.cs の実装を読むと、命名と動作が逆転していた：

public float openAngle = 40f;
public float closeAngle = 20f;  // 未使用

public void Open()
{
    _targetAngle = 0f;          // ← 0°に設定（実は閉じた状態）
}

public void Close()
{
    _targetAngle = openAngle;   // ← 40°に設定（実は開いた状態）
}

void Start()
{
    _currentAngle = 0f;
    _targetAngle = 0f;          // ← デフォルトは閉じた状態
}

Open() を呼ぶと閉じ、Close() を呼ぶと開く。Start時のデフォルトは閉じた状態。

そして決定的に、FKスキャナー4ファイルのいずれも GripperController を呼び出していない。FKスキャン中のグリッパ状態は不定。ただしStart()のデフォルト＝angle=0で起動する設計だから、おそらく閉じた状態のまま FKスキャンが行われていた可能性が高い。

ここから論理的な帰結が一本道で導かれる：

FKスキャン時: グリッパは閉じた状態（angle=0）
   → leftTipBrep / rightTipBrep は中央に寄った位置
   → midpoint = グリッパ本体の中心軸付近
   → fk_scan_motion_based.csv に「閉じた爪の中点」として記録される

lookup時: 「閉じた爪の中点」が目標位置に来る角度が返される

実機PICK時: グリッパは開いた状態（GRIPPER_OPEN コマンドで開かれている、掴む直前なので）
   → 「閉じた爪の中点」想定で角度を出しても、実機の爪は外側に広がっている
   → 爪先は「想定された中点」より広く、ブロックの脇を素通り

memoryの「90mm」は当てずっぽうな数字だった。実際は何mmなのか、誰も実測していない。実測しないと前に進めない。

青シール3点で実機の爪先位置を計測する

ここまでの議論はすべて推論。実測値が一つもない。

提案：iPhone カメラで青シール3点（グリッパ本体上部1つ + 爪先2つ）を検出して、実機の現在位置を Unity 座標に変換して可視化する。これができれば、Unity 側の EE 座標と、実機の物理的な爪先中点を、同じ Unity 座標系で比較できるようになる。

新規スクリプトを4本作成した（commitせず、一時調査用）。

1. detect_blue_markers.py（スタンドアロン青シール検出）

#!/usr/bin/env python3
"""
detect_blue_markers.py  DAY168
カメラ画像から青シール3つを検出し、homographyでUnity座標に変換して表示する。
ESCキーで終了。
"""

import os
import numpy as np
import cv2

# ---- 設定 ----------------------------------------------------------------
HOMOGRAPHY_PATH = os.path.join(os.path.dirname(__file__), "homography.npy")

# 青色HSV閾値
BLUE_LOWER = np.array([100, 100, 50])
BLUE_UPPER = np.array([130, 255, 255])

# 輪郭面積フィルタ
MIN_CONTOUR_AREA = 80
MAX_CONTOUR_AREA = 8000


# ---- 座標変換 ------------------------------------------------------------

def pixel_to_unity(px, py, H):
    """pixel → homography → mat mm → Unity m"""
    pt = np.array([[[float(px), float(py)]]], dtype=np.float32)
    r = cv2.perspectiveTransform(pt, H)
    mat_x_mm = float(r[0][0][0])
    mat_z_mm = float(r[0][0][1])
    return mat_x_mm * 0.001, mat_z_mm * 0.001


# ---- 青シール検出 --------------------------------------------------------

def detect_blue_markers(frame):
    """
    青シールを検出し、重心pixel座標のリストを返す。
    Returns: [(cx, cy, area), ...] 面積降順
    """
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(hsv, BLUE_LOWER, BLUE_UPPER)

    kernel = np.ones((5, 5), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
    mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)

    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    markers = []
    for cnt in contours:
        area = cv2.contourArea(cnt)
        if area < MIN_CONTOUR_AREA or area > MAX_CONTOUR_AREA:
            continue
        M = cv2.moments(cnt)
        if M["m00"] == 0:
            continue
        cx = int(M["m10"] / M["m00"])
        cy = int(M["m01"] / M["m00"])
        markers.append((cx, cy, area))

    markers.sort(key=lambda m: m[2], reverse=True)
    return markers


# ---- メイン --------------------------------------------------------------

def main():
    # homography ロード
    if not os.path.exists(HOMOGRAPHY_PATH):
        print(f"[ERROR] {HOMOGRAPHY_PATH} が見つかりません")
        return
    H = np.load(HOMOGRAPHY_PATH)
    print(f"[INFO] homography.npy 読み込み完了")

    # カメラ起動(一時的に index=1 直接指定)
    camera_index = 1
    cap = cv2.VideoCapture(camera_index)
    if not cap.isOpened():
        print(f"[ERROR] カメラ index={camera_index} を開けません")
        return
    print(f"[INFO] カメラ起動 index={camera_index}")

    # 30フレーム読み捨て
    for _ in range(30):
        cap.read()

    print("[INFO] ESCキーで終了")

    while True:
        ret, frame = cap.read()
        if not ret:
            continue

        display = frame.copy()
        markers = detect_blue_markers(frame)

        # 各シールに緑の円 + 座標テキスト
        for i, (cx, cy, area) in enumerate(markers):
            ux, uz = pixel_to_unity(cx, cy, H)
            cv2.circle(display, (cx, cy), 12, (0, 255, 0), 2)
            cv2.drawMarker(display, (cx, cy), (0, 255, 0), cv2.MARKER_CROSS, 16, 1)
            label = f"#{i} px({cx},{cy}) U({ux:.3f},{uz:.3f}) a={area:.0f}"
            cv2.putText(display, label, (cx + 15, cy - 10),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 255, 0), 1, cv2.LINE_AA)

        # 3つ以上検出された場合、上位2つ(爪先想定)の中点を表示
        if len(markers) >= 3:
            # 面積上位3つのうち、下側2つ(Y座標が大きい=画像下方=爪先)を選ぶ
            top3 = markers[:3]
            sorted_by_y = sorted(top3, key=lambda m: m[1], reverse=True)
            tip1 = sorted_by_y[0]
            tip2 = sorted_by_y[1]
            mid_x = (tip1[0] + tip2[0]) // 2
            mid_y = (tip1[1] + tip2[1]) // 2
            ux_mid, uz_mid = pixel_to_unity(mid_x, mid_y, H)

            cv2.circle(display, (mid_x, mid_y), 15, (0, 165, 255), 3)
            cv2.drawMarker(display, (mid_x, mid_y), (0, 165, 255), cv2.MARKER_CROSS, 20, 2)
            mid_label = f"MID px({mid_x},{mid_y}) U({ux_mid:.3f},{uz_mid:.3f})"
            cv2.putText(display, mid_label, (mid_x + 20, mid_y + 20),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 165, 255), 2, cv2.LINE_AA)

        # ステータスバー
        cv2.putText(display, f"Blue markers: {len(markers)}  ESC to quit",
                    (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (200, 200, 200), 1, cv2.LINE_AA)

        cv2.imshow("Blue Marker Detect", display)
        if cv2.waitKey(1) & 0xFF == 27:  # ESC
            break

    cap.release()
    cv2.destroyAllWindows()
    print("[INFO] 終了")


if __name__ == "__main__":
    main()

爪先選出は2段階。まず面積降順で並べて面積上位3つを取り、その3つの中でY座標が大きい下側2つを爪先と見なす。これで画像上部に映り込む青色ノイズ（本体反射や机の小さな青点）を弾く構造にした。

2. move_to_pick.py（PICK位置で停止してライブ計測）

赤ブロックを検出してPICK位置までアームを動かし、そこで停止。停止後はカメラを開いたまま青シール検出を続けてリアルタイム表示する。macOSのIriunカメラデバイスは複数プロセスから同時に開けないため、detect_blue_markers.py のロジックをスクリプト内部に統合した。

#!/usr/bin/env python3
"""
move_to_pick.py  DAY168
赤ブロックを検出し、アームをHOME → APPROACH_PICK → PICKまで動かして停止する。
グリッパは開いたまま、PLACEは行わない。青シール計測用の静止状態を作る。
"""

import os
import sys
import csv
import json
import math
import numpy as np
import cv2
import paramiko

# ---- 定数 ----------------------------------------------------------------
HOMOGRAPHY_PATH = os.path.join(os.path.dirname(__file__), "homography.npy")
FK_CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")
SSH_HOST = "192.168.2.1"
SSH_USER = "ubuntu"
SSH_KEY_PATH = os.path.expanduser("~/.ssh/id_rsa_pi")
REMOTE_SCRIPT = "/home/ubuntu/pick_and_place_fixed.py"

HOME = [0, 0, 0, 0, 0, -30]
APPROACH_PICK_BASE = [None, 81.91, -30.14, -8.08, -50.27, -29.44]
PICK_EE_Y = 0.027
EE_Y_TOL = 0.020
DIST_THRESHOLD = 0.100
INTERP_STEPS = 5

# 赤色HSV閾値
RED_LOWER1 = np.array([0, 100, 100])
RED_UPPER1 = np.array([10, 255, 255])
RED_LOWER2 = np.array([160, 100, 100])
RED_UPPER2 = np.array([180, 255, 255])
MIN_CONTOUR_AREA = 500

# 青色HSV閾値
BLUE_LOWER = np.array([100, 100, 50])
BLUE_UPPER = np.array([130, 255, 255])
MIN_BLUE_AREA = 80
MAX_BLUE_AREA = 8000


# ---- 座標変換 ------------------------------------------------------------

def pixel_to_unity(px, py, H):
    """pixel → homography → mat mm → Unity m"""
    pt = np.array([[[float(px), float(py)]]], dtype=np.float32)
    r = cv2.perspectiveTransform(pt, H)
    return float(r[0][0][0]) * 0.001, float(r[0][0][1]) * 0.001


# ---- 赤ブロック検出 ------------------------------------------------------

def detect_red_block(frame):
    """赤ブロックの外接矩形中心pixel座標を返す。"""
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    m1 = cv2.inRange(hsv, RED_LOWER1, RED_UPPER1)
    m2 = cv2.inRange(hsv, RED_LOWER2, RED_UPPER2)
    mask = cv2.bitwise_or(m1, m2)
    kernel = np.ones((5, 5), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)

    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None

    candidates = []
    for cnt in contours:
        area = cv2.contourArea(cnt)
        if area < MIN_CONTOUR_AREA:
            continue
        x, y, w, h = cv2.boundingRect(cnt)
        cx = x + w // 2
        cy = y + h // 2
        candidates.append((area, cx, cy))

    if not candidates:
        return None
    _, cx, cy = max(candidates, key=lambda t: t[0])
    return (cx, cy)


# ---- 青シール検出 --------------------------------------------------------

def detect_blue_markers(frame):
    """青シールを検出し、重心pixel座標のリストを返す(面積降順)。"""
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(hsv, BLUE_LOWER, BLUE_UPPER)
    kernel = np.ones((5, 5), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
    mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)

    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    markers = []
    for cnt in contours:
        area = cv2.contourArea(cnt)
        if area < MIN_BLUE_AREA or area > MAX_BLUE_AREA:
            continue
        M = cv2.moments(cnt)
        if M["m00"] == 0:
            continue
        cx = int(M["m10"] / M["m00"])
        cy = int(M["m01"] / M["m00"])
        markers.append((cx, cy, area))
    markers.sort(key=lambda m: m[2], reverse=True)
    return markers


# ---- FK lookup -----------------------------------------------------------

def load_fk_filtered():
    """FK scan CSVを読み込み、ee_yでフィルタした行を返す。"""
    rows = []
    with open(FK_CSV_PATH, "r") as f:
        reader = csv.reader(f)
        next(reader)
        for row in reader:
            ee_x, ee_y, ee_z = float(row[0]), float(row[1]), float(row[2])
            joints = [float(row[j + 3]) for j in range(6)]
            rows.append((ee_x, ee_y, ee_z, joints))

    ee_y_min = PICK_EE_Y - EE_Y_TOL
    ee_y_max = PICK_EE_Y + EE_Y_TOL
    filtered = [(x, y, z, j) for x, y, z, j in rows if ee_y_min <= y <= ee_y_max]
    print(f"[INFO] FK CSV: {len(rows)}点ロード, ee_yフィルタ後: {len(filtered)}点")
    return filtered


def lookup_nearest(fk_filtered, unity_x, unity_z):
    """2D距離でnearest-neighbor検索。"""
    best = None
    best_dist = float("inf")
    for ee_x, ee_y, ee_z, joints in fk_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:
        return None, best_dist
    return best[3], best_dist


# ---- 補間 ----------------------------------------------------------------

def interpolate_angles(start, end, steps=5):
    """イージング付き補間角度リストを生成。"""
    result = []
    for s in range(1, steps + 1):
        t = s / steps
        t = t * t * (3 - 2 * t)  # smoothstep easing
        angles = [round(a + (b - a) * t, 2) for a, b in zip(start, end)]
        result.append(angles)
    return result


def make_approach_angles(pick_angles):
    """PICK角度のJ0 + APPROACH_PICK_BASEのJ1-J5。"""
    return [
        pick_angles[0], APPROACH_PICK_BASE[1], APPROACH_PICK_BASE[2],
        APPROACH_PICK_BASE[3], APPROACH_PICK_BASE[4], APPROACH_PICK_BASE[5],
    ]


# ---- SSH送信 -------------------------------------------------------------

def ssh_connect():
    client = paramiko.SSHClient()
    client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    client.connect(SSH_HOST, username=SSH_USER, key_filename=SSH_KEY_PATH, timeout=30)
    return client


def execute_commands(commands):
    """Pi側にスクリプト転送 + JSONコマンド送信。"""
    local_script = os.path.join(os.path.dirname(__file__), "pick_and_place_fixed.py")

    # SCP転送
    client = ssh_connect()
    sftp = client.open_sftp()
    sftp.put(local_script, REMOTE_SCRIPT)
    sftp.close()
    client.close()

    # コマンド送信
    json_data = json.dumps(commands)
    client = ssh_connect()
    stdin, stdout, stderr = client.exec_command(f"python3 {REMOTE_SCRIPT} --lookup")
    stdin.write(json_data)
    stdin.channel.shutdown_write()

    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 main():
    # 1. homography ロード
    if not os.path.exists(HOMOGRAPHY_PATH):
        print(f"[ERROR] {HOMOGRAPHY_PATH} が見つかりません")
        sys.exit(1)
    H = np.load(HOMOGRAPHY_PATH)
    print("[INFO] homography.npy 読み込み完了")

    # 2. FK scan CSV ロード
    fk_filtered = load_fk_filtered()

    # 3. カメラ起動 + 赤ブロック検出
    camera_index = 1
    cap = cv2.VideoCapture(camera_index)
    if not cap.isOpened():
        print(f"[ERROR] カメラ index={camera_index} を開けません")
        sys.exit(1)
    for _ in range(30):
        cap.read()

    ret, frame = cap.read()
    if not ret:
        print("[ERROR] フレーム取得失敗")
        cap.release()
        sys.exit(1)

    result = detect_red_block(frame)
    if result is None:
        print("[ERROR] 赤ブロック検出失敗")
        sys.exit(1)
    cx, cy = result
    print(f"[INFO] 赤ブロック検出: pixel=({cx}, {cy})")

    # 4. pixel → Unity座標
    unity_x, unity_z = pixel_to_unity(cx, cy, H)
    print(f"[INFO] Unity座標: ({unity_x:.4f}, {unity_z:.4f})")

    # 5. PICK lookup
    pick_angles, pick_dist = lookup_nearest(fk_filtered, unity_x, unity_z)
    if pick_angles is None:
        print(f"[ERROR] PICK lookup失敗: dist={pick_dist*1000:.1f}mm > {DIST_THRESHOLD*1000:.0f}mm")
        sys.exit(1)
    print(f"[INFO] PICK lookup成功: dist={pick_dist*1000:.1f}mm")
    print(f"[INFO] PICK angles={[round(j, 1) for j in pick_angles]}")

    # 6. コマンド構築
    current_angles = HOME
    approach_angles = make_approach_angles(pick_angles)
    interp_to_approach = interpolate_angles(current_angles, approach_angles, INTERP_STEPS)
    interp_to_pick = interpolate_angles(approach_angles, pick_angles, INTERP_STEPS)

    commands = [
        {"action": "HOME"},
        {"action": "GRIPPER_OPEN"},
        {"action": "INTERPOLATED_MOVE", "label": "TO_APPROACH_PICK",
         "interp_list": interp_to_approach},
        {"action": "INTERPOLATED_MOVE", "label": "TO_PICK",
         "interp_list": interp_to_pick},
    ]

    print(f"\n[INFO] approach_angles={[round(a, 1) for a in approach_angles]}")
    print(f"[INFO] pick_angles={[round(a, 1) for a in pick_angles]}")
    print(f"[INFO] コマンド数: {len(commands)}")

    # 7. SSH送信
    print("\n[INFO] Pi側に送信中...")
    success, output, error = execute_commands(commands)
    if output:
        print(f"[Pi出力]:\n{output}")
    if error:
        print(f"[Piエラー]:\n{error}")

    if not success:
        print("[ERROR] Pi側実行失敗")
        sys.exit(1)

    # 8. 青シール検出ライブ表示(ESCで終了)
    print("\n==============================")
    print("PICK位置到達。アームを保持中。")
    print("青シール検出をライブ表示中。ESCキーで終了。")
    print("(HOMEには戻りません)")
    print("==============================")

    while True:
        ret, frame = cap.read()
        if not ret:
            continue

        display = frame.copy()
        markers = detect_blue_markers(frame)

        # 各青シールに緑の円 + 座標テキスト
        for i, (mx, my, area) in enumerate(markers):
            ux, uz = pixel_to_unity(mx, my, H)
            cv2.circle(display, (mx, my), 12, (0, 255, 0), 2)
            cv2.drawMarker(display, (mx, my), (0, 255, 0), cv2.MARKER_CROSS, 16, 1)
            label = f"#{i} px({mx},{my}) U({ux:.3f},{uz:.3f}) a={area:.0f}"
            cv2.putText(display, label, (mx + 15, my - 10),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 255, 0), 1, cv2.LINE_AA)

        # 3つ以上検出: 下側2つ(爪先)の中点をオレンジで表示
        if len(markers) >= 3:
            top3 = markers[:3]
            sorted_by_y = sorted(top3, key=lambda m: m[1], reverse=True)
            tip1, tip2 = sorted_by_y[0], sorted_by_y[1]
            mid_x = (tip1[0] + tip2[0]) // 2
            mid_y = (tip1[1] + tip2[1]) // 2
            ux_mid, uz_mid = pixel_to_unity(mid_x, mid_y, H)

            cv2.circle(display, (mid_x, mid_y), 15, (0, 165, 255), 3)
            cv2.drawMarker(display, (mid_x, mid_y), (0, 165, 255), cv2.MARKER_CROSS, 20, 2)
            mid_label = f"MID px({mid_x},{mid_y}) U({ux_mid:.3f},{uz_mid:.3f})"
            cv2.putText(display, mid_label, (mid_x + 20, mid_y + 20),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 165, 255), 2, cv2.LINE_AA)

        cv2.putText(display, f"Blue markers: {len(markers)}  ESC to quit",
                    (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (200, 200, 200), 1, cv2.LINE_AA)

        cv2.imshow("PICK + Blue Markers", display)
        if cv2.waitKey(1) & 0xFF == 27:
            break

    cap.release()
    cv2.destroyAllWindows()
    print("[INFO] 終了")


if __name__ == "__main__":
    main()

補間は smoothstep easing（t = t * t * (3 - 2 * t)）を使って、始点と終点で速度が滑らかになるようにしている。SSH通信は paramiko で SFTP による pick_and_place_fixed.py のSCP転送 + exec_command で stdin に JSON を流す構造。

3. move_to_home.py（HOME復帰のみ）

#!/usr/bin/env python3
"""
move_to_home.py  DAY168
アームをHOME姿勢 [0, 0, 0, 0, 0, -30] に戻す。
"""

import os
import json
import paramiko

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


def main():
    local_script = os.path.join(os.path.dirname(__file__), "pick_and_place_fixed.py")

    # SCP転送
    print("[INFO] Pi側にスクリプト転送中...")
    client = paramiko.SSHClient()
    client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    client.connect(SSH_HOST, username=SSH_USER, key_filename=SSH_KEY_PATH, timeout=30)
    sftp = client.open_sftp()
    sftp.put(local_script, REMOTE_SCRIPT)
    sftp.close()
    client.close()

    # HOMEコマンド送信
    print("[INFO] HOME送信中...")
    commands = [{"action": "HOME"}]
    json_data = json.dumps(commands)

    client = paramiko.SSHClient()
    client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    client.connect(SSH_HOST, username=SSH_USER, key_filename=SSH_KEY_PATH, timeout=30)
    stdin, stdout, stderr = client.exec_command(f"python3 {REMOTE_SCRIPT} --lookup")
    stdin.write(json_data)
    stdin.channel.shutdown_write()

    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()

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

    if exit_code == 0:
        print("[INFO] HOME復帰完了")
    else:
        print(f"[ERROR] 失敗 (exit_code={exit_code})")


if __name__ == "__main__":
    main()

4. close_gripper.py（GRIPPER_CLOSE のみ）

#!/usr/bin/env python3
"""
close_gripper.py  DAY168
現在のアーム姿勢を維持したまま、グリッパを閉じる。
"""

import os
import json
import paramiko

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


def main():
    local_script = os.path.join(os.path.dirname(__file__), "pick_and_place_fixed.py")

    # SCP転送
    print("[INFO] Pi側にスクリプト転送中...")
    client = paramiko.SSHClient()
    client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    client.connect(SSH_HOST, username=SSH_USER, key_filename=SSH_KEY_PATH, timeout=30)
    sftp = client.open_sftp()
    sftp.put(local_script, REMOTE_SCRIPT)
    sftp.close()
    client.close()

    # GRIPPER_CLOSEコマンド送信
    print("[INFO] GRIPPER_CLOSE送信中...")
    commands = [{"action": "GRIPPER_CLOSE"}]
    json_data = json.dumps(commands)

    client = paramiko.SSHClient()
    client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    client.connect(SSH_HOST, username=SSH_USER, key_filename=SSH_KEY_PATH, timeout=30)
    stdin, stdout, stderr = client.exec_command(f"python3 {REMOTE_SCRIPT} --lookup")
    stdin.write(json_data)
    stdin.channel.shutdown_write()

    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()

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

    if exit_code == 0:
        print("[INFO] グリッパ閉じ完了")
    else:
        print(f"[ERROR] 失敗 (exit_code={exit_code})")


if __name__ == "__main__":
    main()

Pi側 pick_and_place_fixed.py の該当部分（参考）

HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
INTERP_SLEEP = 0.05


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


def run_lookup_mode():
    """Mac側からJSON形式で動作指示を受信して実行"""
    raw = sys.stdin.read()
    commands = json.loads(raw)
    print(f"受信コマンド数: {len(commands)}")

    for cmd in commands:
        action = cmd.get("action", "")
        angles = cmd.get("angles", [])
        interp_list = cmd.get("interp_list", [])

        if action == "HOME":
            print(f"  [{action}] ホーム位置へ移動")
            mc.send_angles(HOME, SPEED)
            time.sleep(3)
            log_angles("HOME")

        elif action == "GRIPPER_OPEN":
            print(f"  [{action}] グリッパーを開く")
            mc.set_gripper_state(0, 100)
            time.sleep(2)
            log_angles("GRIPPER_OPEN")

        elif action == "GRIPPER_CLOSE":
            print(f"  [{action}] グリッパーを閉じる")
            mc.set_gripper_state(1, 70)
            time.sleep(2)
            log_angles("GRIPPER_CLOSE")

        elif action == "INTERPOLATED_MOVE":
            label = cmd.get("label", "INTERP")
            print(f"  [{action}] {label} ({len(interp_list)}ステップ)")
            execute_interpolated_motion(interp_list, label)

        # ... GET_ANGLES / MOVE など他アクションの分岐

    # ログ保存
    log_path = f"/home/ubuntu/motion_log_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
    save_log(log_path)
    print(f"  LOG_FILE={log_path}")

mc.set_gripper_state(0, 100) が GRIPPER_OPEN、mc.set_gripper_state(1, 70) が GRIPPER_CLOSE。各アクション後に log_angles() で実機角度を CSV ロギングするのがこの実装の特徴。

物理側の対応：本体上部の青シールを剥がす

最初の detect_blue_markers.py 実行時、3つの想定（本体1+爪先2）に対して、グリッパ手首付近の青色光沢が拾われたり、本体上部のシールが画面で見える位置にあって、4つ目の候補が出ることがあった。

グリッパ本体上部に貼ってあった旧青シールを物理的に剥がした。これで検出される青シールは2-3個に整理。スクリプト側でも、detect_blue_markers() で面積降順にソートし、上位3つのうち Y座標が大きい下2つを爪先と見なすロジックで、画像上部のマーカー（本体側）を自然に弾く設計にした。

実測値：開いた状態と閉じた状態のMID

move_to_home.py でHOMEに戻し、move_to_pick.py を実行。PICK位置に到達したアームの開いた爪先MIDを実測：

[INFO] homography.npy 読み込み完了
[INFO] FK CSV: 134456点ロード, ee_yフィルタ後: 77747点
[INFO] 赤ブロック検出: pixel=(511, 660)
[INFO] Unity座標: (0.2325, 0.2048)
[INFO] PICK lookup成功: dist=71.1mm
[INFO] PICK angles=[-50.0, 100.0, -55.0, -2.0, -35.0, -20.0]

[INFO] approach_angles=[-50.0, 81.9, -30.1, -8.1, -50.3, -29.4]
[INFO] pick_angles=[-50.0, 100.0, -55.0, -2.0, -35.0, -20.0]
[INFO] コマンド数: 4

[INFO] Pi側に送信中...
[Pi出力]:
受信コマンド数: 4
  [HOME] ホーム位置へ移動
  [GRIPPER_OPEN] グリッパーを開く
  [INTERPOLATED_MOVE] TO_APPROACH_PICK (5ステップ)
  [INTERPOLATED_MOVE] TO_PICK (5ステップ)

==============================
PICK位置到達。アームを保持中。
青シール検出をライブ表示中。ESCキーで終了。
(HOMEには戻りません)
==============================

PICK + Blue Markers ウィンドウに表示された値：

赤ブロック位置（目標）と開いた爪先MIDの差：

目標 (赤ブロック): Unity = (0.2325, 0.2048)
実機 MID (開):    Unity = (0.250,  0.197)

X方向のズレ: 0.250  - 0.2325 = +0.0175 m = +17.5 mm  (爪先は X+ 方向 = 画面左寄り)
Z方向のズレ: 0.197  - 0.2048 = -0.0078 m =  -7.8 mm  (爪先は Z- 方向 = 画面奥寄り)

合計ズレ：約19mm

memoryに「90mm」と書いていた数字は、実測の約4-5倍も過大だった。この時点で close_gripper.py を実行し、グリッパを閉じて detect_blue_markers.py で再計測：

開閉差：

開いた MID: Unity = (0.250, 0.197)
閉じた MID: Unity = (0.260, 0.207)

X差: +0.010 m = +10 mm
Z差: +0.010 m = +10 mm

合成距離: √(10² + 10²) ≈ 14 mm

グリッパ開閉で爪先中点が約14mm動く。これが Unity 側の leftTipBrep/rightTipBrep も同じ量だけ動くはずなので、FKスキャン時のグリッパ状態（おそらく閉）と実機PICK時のグリッパ状態（開＝GRIPPER_OPEN後）のミスマッチが、19mm誤差の主要因の一部を説明する。

Unity側EE座標と実機爪先中点の整合性

ここで新しい謎が一つ残った。同じセッションの lookup ログから：

ee_x = 0.183, ee_y = 0.042, ee_z = 0.154 （Unity FKスキャン側のEE）

実機の爪先MID（開状態）は Unity = (0.250, 0.197)。

Unity側 EE と実機側爪先MIDがX方向で約67mm、Z方向で約43mm 異なる。これは「グリッパ開閉差14mm」だけでは説明できない大きさ。

可能性：

• Unity側 leftTipBrep/rightTipBrep の位置が、実機グリッパ先端と幾何学的に一致していない

• URDF インポート時の座標系変換（軸の入れ替えや単位）に問題が残っている

• カメラ検出の系統誤差

これは次のセッションで Unity Editor 上の Inspector 値を読み、実機グリッパの寸法と直接照合する必要がある。今日のセッションのスコープを越える。

今のパイプライン状態（ハンドオフ）

次セッションで議論すべきこと

19mm誤差をどう詰めるか

• A案：FKスキャン時にグリッパを「開」状態に固定して再スキャン（134,456点の再生成、Unity側のみ変更）

• B案：lookup時に開閉によるオフセットを補正係数で打ち消す（姿勢依存補正）

• 構造的にはA案が素直

補間起点の根本対応

• current_angles 取得タイミングを HOME 到達後にする

• または Mac 側で current_angles = HOME 固定にしてしまう（現在の一時バイパスを正式化）

カメラindex検出の根本対応

• find_iriun_camera() を「全indexを試す」から「キャッシュ→失敗時のみ走査」に変更

• またはIriun起動の安定index（macOSの環境では1が常に Iriun になる経験則）を設定ファイル化

• Unity 側 leftTipBrep/rightTipBrep の位置検証

• Unity Editor の Inspector で Transform を読み、実機グリッパの寸法と照合

• URDFインポート時の座標変換ログを再確認

「目視確認だけ」で終わらない日

このセッションは引き継ぎ書では「PICK動作の目視確認、それだけ」だった。実際には3つの独立した障害物に当たり、それぞれを切り分けで原因特定し、最後は計測インフラまで構築することになった。

特に第3の問題（爪オフセット）では、memoryに記録していた「~90mm」という数字を疑わなかった過去の自分を上書きする実測値（19mm）が出た。前のセッションの結論を、次のセッションが裏取りなしに引き継ぐと、推論が雪だるま式に広がる。今日の作業の本当の価値は、その雪だるまを19mmという実測値に縮めたこと。

P002「ハンドオフ文書は各レイヤーの状態をバイナリ✅/❌で明示する」が今日も効いた。lookup成功を「PICK動作成功」と読み替えていた過去のハンドオフが、目視と実測で分解された。

Pixels to Motorsは、iPhoneとUnityと小型ロボットアームだけで、ピッキング動作が再現できるかを毎日記録するプロジェクトです。robot開発はエンジニアだけのものじゃない、というところから始めています。

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