動かしてみて初めて見えた4つの罠 。lookup方式の実機初稼働で踏んだ落とし穴の連鎖

Pi側のnearest-neighbor lookup実装をr2r2r_loop.pyに統合し、カメラ→ホモグラフィ→lookup→補間→send_angles()の完全ループを初めて実機で走らせた。動き出すまでに4つの罠を順に踏んだ。禁止したはずのsend_coords()がClaude Codeの生成コードに復活していた。

座標系判定でカメラ視点を読み違えた。カメラのインデックスが過去から1つズレていた。最後にlookup tableのカバレッジ不足で、ブロック位置に動的追従できなかった。*それでもループは回った。*手でブロックをグリッパの位置に動かせば、PICK→PLACEまで完走した。残った課題はただ一つ、「lookupが動的に追従するか」だけになった。

💡 Key Takeaway
完璧を目指さず、まず動かしてループを回す。R2R2Rの設計思想は「精度より体験の成立を優先する。精度はあとからお金で解決できる」だが、これは「机上で詰めきれる」という意味ではなく「動かして見える問題を直す」という意味だった。事前検証で完璧なテーブルを作ろうとしても、実機を動かさないと見えない罠は必ずある。動かす→踏む→直す、を最小単位で回すことが、Sim-to-Realループの本体だ。

R2R2Rループの第1周回を走らせる準備が整っていた

—— 9点試し引きで7点FAIL、それでも本実装に進む判断。

シミュレーション補間アーキテクチャは一本通っていた。実機で記録した8姿勢（motion_log_block.csv）をMac側でEaseInOut補間して81点に拡張、同じCSVをUnityの20体バリエーション再生にもPi側のsend_angles()にも渡せる構造。Unity側はLOG152で動作確認済み、残るのはPi側のnearest-neighbor lookup実装だった。

LOG153は事前検証から始めた。fk_scan_motion_based.csv（134,456点）に対し、マット9点（4隅+4辺中点+中央）でnearest-neighbor検索を実機通信なしで走らせる。結果は厳しかった。

DIST_OK     : 1点（P4 LR corner = 0.12mm）
DIST_WARN   : 1点（P6 bottom mid = 16.97mm）
DIST_FAIL   : 7点（30〜273mm）

CSV内のj0範囲が[-65.0, -15.0]しかなく、マットの片側半分しかカバーされていない状態。最初は「テーブルを拡張してから本実装」と判断しかけた。が、エイイチの一言で方針が反転した。

「シミュレーションで取得したデータとリアルで取得したデータを補足しながら再度ギャップを埋めて動作を作る想定。そのためのシミュ・リアルループ」

つまり閉ループ運用が前提だった。完璧なテーブルを事前に作るのではなく、まず動かす、ズレを見る、ギャップを埋める、また動かす——R2R2Rの本来の意味どおりの設計だった。これを軍師側（Claude）が読み違えていた。「事前準備で完璧にしよう」モードに入っていた。プロジェクトの設計哲学「精度より体験の成立を優先する」を、机上で完璧を目指す方向に誤って解釈していた。

判断を切り替え、9点試し引きの7点FAILは「問題」ではなく「ループ第1周回のスタート地点」と位置づけ直した。本実装に進む。

罠①：禁止したはずのsend_coords()がコード生成で復活していた

—— Claude Codeに渡した最初のスクリプトに、LOG152で禁止確定したsend_coords()が含まれていた。

最初の検証で、Pi側に「j0をマイナス方向に動かしてカメラ画像のどっち側に動くか」を確認する小スクリプトをClaude Codeに作らせた。返ってきたコードを開いてみると、こうなっていた。

mc.send_coords([200, -100, 200, 0, 90, 0], 30)

明確に禁止したはずの関数。

send_coords() IK は RZ値が位置によって +167°〜−140°で暴れる。send_angles() + lookup table のみ使用。

Claude Codeが参照しているCLAUDE.mdに、この禁止ルールが書かれていなかった。「LOG152で確定した」と口頭で伝えても、コード生成のコンテキストには反映されない。プロジェクトの暗黙ルールは、ドキュメント化されていない限り再発する。

修正は2段階。スクリプト本体からsend_coords()を消してsend_angles()に置換する作業と、CLAUDE.md自体に「send_coords()使用禁止」を恒久ルールとして追記する作業。Claude Codeに両方やらせた。後者が本質で、これをやらないと同じミスがまた起きる。

軍師（Claude）の側にも問題があった。指示書を書く時に「物理憲法」として禁止事項を毎回コピペで含めるべきだった。それが抜けていた。

罠②：カメラ視点を読み違えて座標系判定を間違えかけた

—— アーム俯瞰画像を「前傾している」と判断、実際は水平に伸びていただけだった。

軸方向確認のテストで、j0を-30°に動かしてEEがカメラ画像のどっちに動くかを観察した。エイイチからスクショが届く。私（Claude）は俯瞰映像を見て、こう判断した。

アームが前方に倒れている姿勢。これは Step 1 のホーム姿勢として想定外。

エイイチが即座に訂正してくれた。

「明らかに前傾」じゃなくて、カメラは上からだから、アームは左に動いているよ。

カメラがマットの真上から下向きで撮影していることを、私は側面視点で見ていた。「前傾している」と見えたのは、実際は「アームが水平面でマットの方向に水平に伸びている」状態だった。ロボティクスの観察において、カメラ視点の取り違えは決定的なミスにつながる。座標系判定が完全に逆になり得る。

その後、エイイチが「右に動いて、真ん中に戻ってきたよ」と報告してきた。さっきの「左」と矛盾する。判定が真逆になる重大な情報なので、もう一度実行してもらってStep1・Step2・Step3の3枚スクショを並べて確定させた。結果、最初の「左」が正解だった。

カメラは真上、アームは水平。ロボティクスでは「視点」を最初に揃えないと、すべての判断が崩れる。

罠③：カメラのインデックスが過去から1つズレていた

—— detect_block_position.pyが動いていた頃のindex 1が、今は真っ黒画面を返していた。

lookup実装を完了して、いざ実機を起動する。検出が動かない。

[Mac] FK scan CSV読み込み中...
[Mac] ✓ 134456点ロード, ee_yフィルタ後: 77747点
[Mac] ✓ ホモグラフィ読み込み完了
[Mac] ✓ カメラ初期化完了（30フレーム読み捨て済み）

[Attempt 1/3] === lookup方式 ===
[Mac] × ブロックが検出できませんでした（10回リトライ失敗）

カメラには赤ブロックがハッキリ映っているのに、検出ゼロ。最初は赤検出ロジックを疑った。Claude Codeに過去の動作実績があるdetect_block_position.pyの実装を完全移植させ、HSV閾値も面積閾値も30フレーム読み捨ても全部揃えた。それでも同じ症状。

切り分けのため、検出失敗時にフレーム画像を保存させた。返ってきた画像はすべて真っ黒だった。1920×1080サイズは正しく取得しているのに、内容が黒。これは「カメラから何も読めていない」のサイン。

カメラインデックスを総当たりで確認する。

Camera 0: brightness=0.0   ← 真っ黒
Camera 1: brightness=0.0   ← 真っ黒  ← r2r2r_loop.pyが見ていたのはここ
Camera 2: brightness=115.2 ← 正常
Camera 3: brightness=117.2 ← 正常

detect_block_position.py時代に動いていたcv2.VideoCapture(1)が、今は別物を指していた。Mac環境（接続デバイス、Iriunのバージョン、macOSアップデート）が変わってインデックスが再割り当てされた結果。Iriun Webcamは現状index 2 か 3にいた。

修正は1行。cv2.VideoCapture(1) → cv2.VideoCapture(2)。これで検出が即座に成功した。

✓ ブロック検出: pixel=(388, 827) → robot=(124.4, -155.3)
Unity座標: (-0.1553, 0.1244)

過去のスクリプトに「動いていた」という事実は、未来の動作を保証しない。外部デバイスのインデックスは、コード本体とは独立に変動する。これも机上では絶対に見つからない罠だった。

罠④：lookup table のカバレッジ不足で動的追従ができなかった

—— 検出も変換もlookupロジックも完璧、ただテーブルが今のブロック位置をカバーしていなかった。

検出が成功して、いよいよlookup→アーム動作のフェーズへ。ターミナルが期待通り進んでいく。

✓ ブロック検出: pixel=(388, 827) → robot=(124.4, -155.3)
Unity座標: (-0.1553, 0.1244)
△ lookup失敗: dist=170.1mm > 100mm → fallback HOME
✗ lookup失敗 → 固定角度モードに fallback

距離閾値100mmを超えて、fallback（固定角度）に落ちる。これは設計通りの挙動だ。100mm超のlookup結果でアームを動かすと、想定と全然違う場所に降りる可能性があるから、fallbackで固定姿勢か（または）ホームに戻すように作ってあった。

ブロックを置き換えて再度試す。

✓ ブロック検出: pixel=(554, 862) → robot=(134.5, -105.2)
Unity座標: (-0.1052, 0.1345)
△ lookup失敗: dist=123.8mm > 100mm → fallback HOME
✗ lookup失敗 → 固定角度モードに fallback

これも同じ。lookupの最近傍距離が100mmを切らない。原因は事前検証で予測していた通りで、テーブルのj0範囲が[-65, -15]しかカバーしていないため、マット下半分（Y負側）の領域には届かない。9点試し引きで7点FAILだった結果が、実機でも忠実に再現されただけだった。

ここで重要な選択肢が見えた。lookupそのものが破綻しているわけではない。検出も座標変換も判定も全部動いている。問題は「テーブルが偏っているせいで、今のブロック位置に対して有効な姿勢が引けない」だけ。これはR2R2Rループの第2周回（テーブル拡張）で必ず解決する課題で、第1周回の到達点としては完成している。

それでもループは回った ── 手動アシストでPICK→PLACEまで完走

—— ブロックをグリッパの位置に手で動かしたら、最後まで動いた。

lookup失敗→固定角度fallback→APPROACH→PICK→グリッパ閉→APPROACH→PLACE→グリッパ開→ホーム、という設計したシーケンス全体が、固定角度モードで完走した。エイイチが「想定通りの動作」と確認。さらに途中でブロックを手でグリッパの位置に動かしてやったら、実際に物理的に掴んでPLACEまで運んだ。

これは大きな成果だ。整理するとこうなる。

「lookupの動的追従」だけが未実証で、それ以外の全要素が実機で動いた。lookup自体は明日、ブロックをテーブルカバレッジ内（マット右上、PLACEの小鉢の左隣あたり）に置き換えれば、必ずdist < 100mmの成功ケースが見られる。

4つの罠に共通していたパターン

—— 全部「机上では見つからない」種類の問題だった。

LOG153で踏んだ4つの罠を並べると、共通点が浮かび上がる。

①は防げた。②③は実機を動かして観察しなければ気づきようがない。④は予測できていたが、実機で動かしたことで「lookupそのものは動く、テーブルだけが問題」という切り分けが完了した。

事前準備でテーブルを完璧にしようとしても、①②③は同じように出る。だから「事前準備に時間をかけて完璧を目指す」より、「最小実装で動かして罠を踏んでから直す」のほうが、結果的に速い。これがR2R2Rループの本来の使い方だ。エイイチが言っていた「シミュ・リアルループ」とは、この罠を踏みに行く設計のことだった。

lookupの動的追従、ただ1つ

—— マット右上にブロックを置けば、距離100mm以内で成功するはず。

LOG154は最小タスクから入る。

[1] 赤ブロックを画面右上（Robot座標 X -30〜-90、Y +100〜+180 付近）に置く
    ※ 9点試し引きでP4=LR corner robot(128.6, 182.9)が dist=0.12mmだった領域
[2] r2r2r_loop.py 実行
[3] lookup成功（dist < 100mm）を確認
[4] アームがlookup結果通りにブロック真上に降りるか観察
[5] 成功 → lookup方式の動的追従の実機実証完了
   失敗 → 原因切り分け（座標変換 / EE位置オフセット / etc.）

ここが通れば、いよいよR2R2Rループの第2周回（B-1起点追加によるテーブル拡張）に進む。マット左側の4箇所×3高さの実機ティーチングを行い、再スキャンで20万点規模のテーブルを生成。これでマット全域での動的追従が可能になる。

LOG153の到達点を一行で言えばこうなる。

「lookup方式の物理パイプラインは実機で完成した。残るは動的追従の実証だけ」。

実装コード

以下、LOG153で書いた・修正した全コードを記録として残す。未来の自分がLOG153を再現・参照できるよう、ファイル名・パス・全文を併記する。

~/r2r2r-scripts/r2r2r_loop.py

カメラ→ホモグラフィ→lookup→補間→SSH送信→Pi側send_angles()の全パイプライン。

#!/usr/bin/env python3
"""
R2R2R ループスクリプト（lookup対応版）
カメラ検出 → ホモグラフィ → lookup → 補間 → SSH送信 → Pi側send_angles()
"""

import sys
import os
import time
import csv
import json
import math
import re
import numpy as np
import cv2
import paramiko
from datetime import datetime


# ===== 設定 =====
LOG_FILE = "r2r2r_log.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"
FK_CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")
HOMOGRAPHY_PATH = os.path.expanduser("~/r2r2r-scripts/homography.npy")

# 定数
HOME = [0, 0, 0, 0, 0, -30]
PICK_EE_Y = 0.027       # PICK高さ（m）
EE_Y_TOL = 0.020        # ee_yフィルタ閾値 ±20mm
DIST_THRESHOLD = 0.100   # 距離閾値 100mm（m単位）
INTERP_STEPS = 5
PLACE_ROI = (1571, 607, 249, 211)  # 変更禁止

# APPROACH_PICK ベース角度（J2〜J6は固定、J1だけlookup結果で置換）
APPROACH_PICK_BASE = [None, 81.91, -30.14, -8.08, -50.27, -29.44]

# 固定PLACE角度（PLACEはlookup tableカバレッジ外のためfallback）
APPROACH_PLACE = [59.85, 16.87, 27.94, 10.01, -45.17, -35.33]
PLACE_ANGLES = [54.49, 45.17, 26.63, 2.9, -70.57, -29.26]
PLACE_RELEASE = [59.85, 30.0, 27.94, 10.01, -45.17, -35.33]


# ===== lookup table =====
_fk_table = None
_fk_filtered = None


def load_fk_table():
    """FK scan CSVをメモリにロード（起動時に1回）"""
    global _fk_table, _fk_filtered
    print("[Mac] FK scan CSV読み込み中...")
    rows = []
    with open(FK_CSV_PATH, "r") as f:
        reader = csv.reader(f)
        next(reader)  # ヘッダスキップ
        for row in reader:
            ee_x = float(row[0])
            ee_y = float(row[1])
            ee_z = float(row[2])
            joints = [float(row[j + 3]) for j in range(6)]
            rows.append((ee_x, ee_y, ee_z, joints))
    _fk_table = rows

    # ee_yフィルタ（PICK高さ付近のみ）
    ee_y_min = PICK_EE_Y - EE_Y_TOL
    ee_y_max = PICK_EE_Y + EE_Y_TOL
    _fk_filtered = [(x, y, z, j) for x, y, z, j in _fk_table
                     if ee_y_min <= y <= ee_y_max]
    print(f"[Mac] ✓ {len(_fk_table)}点ロード, ee_yフィルタ後: {len(_fk_filtered)}点")


def robot_to_unity(robot_x, robot_y):
    """Robot XY (mm) → Unity EE座標 (m)"""
    unity_x = robot_y * 0.001
    unity_z = robot_x * 0.001
    return unity_x, unity_z


def lookup_nearest_angles(unity_x, unity_z):
    """
    (ee_x, ee_z)の2D距離でnearest-neighbor検索。
    距離 > DIST_THRESHOLD ならNoneを返す（fallback用）。
    """
    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:
        print(f"[Mac] ⚠ lookup失敗: dist={best_dist*1000:.1f}mm > {DIST_THRESHOLD*1000:.0f}mm → fallback HOME")
        return None, best_dist

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


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


# ===== カメラ（detect_block_position.py のロジックに完全一致） =====

DEBUG_FRAME_DIR = os.path.expanduser("~/r2r2r-scripts/logs/debug_frames")


def detect_red_block(frame, debug_log=False):
    """
    フレーム内の赤ブロックを検出して重心を計算。
    detect_block_position.py の detect_red_block() と同一ロジック。
    """
    hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

    lower_red1 = np.array([0, 100, 100])
    upper_red1 = np.array([10, 255, 255])
    lower_red2 = np.array([160, 100, 100])
    upper_red2 = np.array([180, 255, 255])

    mask1 = cv2.inRange(hsv_frame, lower_red1, upper_red1)
    mask2 = cv2.inRange(hsv_frame, lower_red2, upper_red2)
    red_mask = cv2.bitwise_or(mask1, mask2)

    if debug_log:
        nonzero_count = cv2.countNonZero(red_mask)
        print(f"  赤マスク反応ピクセル数: {nonzero_count}")

    contours, _ = cv2.findContours(red_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        if debug_log:
            print(f"  輪郭なし")
        return None

    max_contour = max(contours, key=cv2.contourArea)
    contour_area = cv2.contourArea(max_contour)
    if debug_log:
        print(f"  最大輪郭面積: {contour_area:.0f}px (閾値: 500)")
    if contour_area < 500:
        return None

    M = cv2.moments(max_contour)
    if M["m00"] == 0:
        return None

    cx = int(M["m10"] / M["m00"])
    cy = int(M["m01"] / M["m00"])
    return cx, cy


def pixel_to_world(pixel_point, homography_matrix):
    """ピクセル座標を実世界座標（mm）に変換"""
    pt = np.array([[pixel_point]], dtype=np.float32)
    world = cv2.perspectiveTransform(pt, homography_matrix)
    return world[0][0]


def detect_block_position(cap, homography_matrix):
    """カメラで赤ブロックを検出してRobot座標を返す"""
    os.makedirs(DEBUG_FRAME_DIR, exist_ok=True)

    centroid = None
    frame = None
    for i in range(10):
        ret, frame = cap.read()
        if not ret:
            print(f"  [リトライ {i}] cap.read() 失敗")
            continue
        print(f"  [リトライ {i}] フレーム取得 {frame.shape[1]}x{frame.shape[0]}")
        centroid = detect_red_block(frame, debug_log=True)
        if centroid is not None:
            break
        # 失敗フレームを保存
        timestamp = int(time.time() * 1000)
        debug_path = os.path.join(DEBUG_FRAME_DIR, f"fail_{timestamp}_attempt{i}.jpg")
        cv2.imwrite(debug_path, frame)
        print(f"  デバッグフレーム保存: {debug_path}")
        time.sleep(0.1)

    if centroid is None or frame is None:
        print("[Mac] ✗ ブロックが検出できませんでした（10回リトライ失敗）")
        return None, None

    pixel_x, pixel_y = centroid
    robot_xy = pixel_to_world((pixel_x, pixel_y), homography_matrix)
    robot_x, robot_y = float(robot_xy[0]), float(robot_xy[1])
    print(f"[Mac] ✓ ブロック検出: pixel=({pixel_x}, {pixel_y}) → robot=({robot_x:.1f}, {robot_y:.1f})")

    # デバッグ表示（緑丸 + Robot座標テキスト、2秒）
    display_frame = frame.copy()
    cv2.circle(display_frame, (int(pixel_x), int(pixel_y)), 10, (0, 255, 0), 2)
    text = f"Robot: ({robot_x:.1f} mm, {robot_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()

    return robot_x, robot_y


# ===== SSH通信 =====

def ssh_connect():
    """SSH接続を作成"""
    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 transfer_file_scp(local_path, remote_path):
    """scpでファイルを転送"""
    try:
        client = ssh_connect()
        sftp = client.open_sftp()
        sftp.put(local_path, remote_path)
        sftp.close()
        client.close()
        return True
    except Exception as e:
        print(f"  ✗ ファイル転送エラー: {e}")
        return False


def execute_lookup_commands(commands):
    """
    Pi側にコマンドリストをJSON送信して実行。
    pick_and_place_fixed.py --lookup モードで起動。
    """
    try:
        local_script = os.path.join(os.path.dirname(__file__), "pick_and_place_fixed.py")
        if not transfer_file_scp(local_script, REMOTE_SCRIPT):
            return False, "", "ファイル転送失敗"

        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

    except Exception as e:
        return False, "", str(e)


def get_current_angles_from_pi():
    """Pi側から現在の関節角度を取得"""
    commands = [{"action": "GET_ANGLES"}]
    success, output, error = execute_lookup_commands(commands)
    if success:
        match = re.search(r"CURRENT_ANGLES=(\[.*?\])", output)
        if match:
            return json.loads(match.group(1))
    return HOME  # フォールバック


# ===== メイン =====

def write_log(trial_number, result, fail_step, reason):
    """ログをCSVファイルに記録"""
    timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
    write_header = not os.path.exists(LOG_FILE)
    with open(LOG_FILE, "a", newline="") as f:
        writer = csv.writer(f)
        if write_header:
            writer.writerow(["timestamp", "trial_number", "result", "fail_step", "reason"])
        writer.writerow([timestamp, trial_number, result,
                         fail_step if fail_step else "", reason])
    print(f"  📝 ログ記録: trial={trial_number} result={result}")


def make_approach_angles(pick_angles):
    """lookup結果のJ1 + APPROACH_PICKのJ2〜J6でAPPROACH角度を生成"""
    return [
        pick_angles[0],          # J1: lookup結果（ベース回転を物体方向へ）
        APPROACH_PICK_BASE[1],   # J2: 81.91
        APPROACH_PICK_BASE[2],   # J3: -30.14
        APPROACH_PICK_BASE[3],   # J4: -8.08
        APPROACH_PICK_BASE[4],   # J5: -50.27
        APPROACH_PICK_BASE[5],   # J6: -29.44
    ]


def build_pick_commands(pick_angles, current_angles):
    """PICK動作のコマンドリストを生成（APPROACH段階あり）"""
    approach_angles = make_approach_angles(pick_angles)
    interp_to_approach = interpolate_angles(current_angles, approach_angles, steps=INTERP_STEPS, easing=True)
    interp_to_pick = interpolate_angles(approach_angles, pick_angles, steps=INTERP_STEPS, easing=True)
    interp_liftup = interpolate_angles(pick_angles, approach_angles, steps=INTERP_STEPS, easing=True)
    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},
        {"action": "GRIPPER_CLOSE"},
        {"action": "INTERPOLATED_MOVE", "label": "LIFTUP_TO_APPROACH",
         "interp_list": interp_liftup},
    ]


def build_place_commands(current_angles):
    """PLACE動作のコマンドリストを生成（固定角度）"""
    interp_to_approach = interpolate_angles(current_angles, APPROACH_PLACE, steps=INTERP_STEPS, easing=True)
    interp_to_place = interpolate_angles(APPROACH_PLACE, PLACE_ANGLES, steps=INTERP_STEPS, easing=True)
    return [
        {"action": "INTERPOLATED_MOVE", "label": "TO_APPROACH_PLACE",
         "interp_list": interp_to_approach},
        {"action": "INTERPOLATED_MOVE", "label": "TO_PLACE",
         "interp_list": interp_to_place},
        {"action": "MOVE", "label": "PLACE_RELEASE", "angles": PLACE_RELEASE},
        {"action": "GRIPPER_OPEN"},
        {"action": "MOVE", "label": "APPROACH_PLACE_RETURN", "angles": APPROACH_PLACE},
        {"action": "HOME"},
    ]


def run_lookup_attempt(attempt, cap, homography_matrix):
    """lookup方式で1回のPICK&PLACEを実行"""
    print(f"\n[Attempt {attempt}/3] === lookup方式 ===")

    # 1. カメラで赤ブロック検出
    robot_x, robot_y = detect_block_position(cap, homography_matrix)
    if robot_x is None:
        return False

    # 2. Robot → Unity変換
    unity_x, unity_z = robot_to_unity(robot_x, robot_y)
    print(f"[Mac] Unity座標: ({unity_x:.4f}, {unity_z:.4f})")

    # 3. nearest-neighbor lookup
    pick_angles, dist = lookup_nearest_angles(unity_x, unity_z)
    if pick_angles is None:
        print(f"[Mac] ✗ lookup失敗 → 固定角度モードにfallback")
        return run_fixed_attempt(attempt)

    # 4. 現在の角度を取得
    current_angles = get_current_angles_from_pi()
    print(f"[Mac] 現在角度: {current_angles}")

    # 5. PICK + PLACEコマンドを構築
    pick_cmds = build_pick_commands(pick_angles, current_angles)
    approach_after_pick = make_approach_angles(pick_angles)
    place_cmds = build_place_commands(approach_after_pick)
    all_commands = pick_cmds + place_cmds

    # 6. SSH送信・実行
    print(f"[Mac] コマンド送信中... ({len(all_commands)}ステップ)")
    success, output, error = execute_lookup_commands(all_commands)

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

    return success


def run_fixed_attempt(attempt):
    """固定角度モードのfallback"""
    print(f"[Attempt {attempt}/3] 固定角度モードで実行...")
    try:
        local_script = os.path.join(os.path.dirname(__file__), "pick_and_place_fixed.py")
        if not transfer_file_scp(local_script, REMOTE_SCRIPT):
            return False

        client = ssh_connect()
        stdin, stdout, stderr = client.exec_command(f"python3 {REMOTE_SCRIPT}")
        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}")

        return exit_code == 0

    except Exception as e:
        print(f"  ✗ SSH実行エラー: {e}")
        return False


def main():
    """メイン処理"""
    print("=== R2R2R Loop Start (lookup版) ===")

    # FK table ロード（起動時1回）
    load_fk_table()

    # ホモグラフィ読み込み（起動時1回）
    homography_matrix = np.load(HOMOGRAPHY_PATH)
    print(f"[Mac] ✓ ホモグラフィ読み込み完了")

    # カメラ初期化（起動時1回、30フレーム読み捨て）
    cap = cv2.VideoCapture(2)
    if not cap.isOpened():
        print("[Mac] ✗ カメラが見つかりません (index 1)")
        sys.exit(1)
    for _ in range(30):
        cap.read()
    print("[Mac] ✓ カメラ初期化完了（30フレーム読み捨て済み）")

    # 起動時サンプルフレーム保存
    os.makedirs(DEBUG_FRAME_DIR, exist_ok=True)
    ret, first_frame = cap.read()
    if ret:
        h, w, c = first_frame.shape
        print(f"[Mac] カメラフレーム取得: {w}x{h}, channels={c}")
        startup_path = os.path.join(DEBUG_FRAME_DIR, "startup.jpg")
        cv2.imwrite(startup_path, first_frame)
        print(f"[Mac] 起動時フレーム保存: {startup_path}")
    else:
        print("[Mac] ⚠ 起動時フレーム取得失敗")

    try:
        for attempt in range(1, 4):
            print(f"\n[Attempt {attempt}/3] ブロックをPICK位置に置いてください")

            # lookup方式で実行
            ssh_success = run_lookup_attempt(attempt, cap, homography_matrix)

            # 10秒待機
            print(f"[Attempt {attempt}/3] Waiting 10s for completion...")
            time.sleep(10)

            # カメラ判定（VLM無効化 — 赤検出のみで判定）
            print(f"[Attempt {attempt}/3] Checking camera...")
            ret, frame = cap.read()
            if not ret:
                print(f"[Attempt {attempt}/3] Failed to read camera")
                continue

            # PLACE ROI内で赤ブロックを検出
            rx, ry, rw, rh = PLACE_ROI
            roi_frame = frame[ry:ry+rh, rx:rx+rw]
            centroid = detect_red_block(roi_frame)
            is_detected = centroid is not None
            reason = f"ROI内赤ブロック検出={'あり' if is_detected else 'なし'}"

            if is_detected:
                print(f"[Attempt {attempt}/3] {reason} → SUCCESS ✓")
                write_log(attempt, "SUCCESS", None, reason)
                print("=== Loop Complete: SUCCESS ===")
                return
            else:
                print(f"[Attempt {attempt}/3] {reason} → FAIL ✗")
                fail_step_input = input("  失敗したステップを入力 (pick / place / none → Enter): ").strip()
                fail_step = fail_step_input if fail_step_input in ["pick", "place"] else None
                write_log(attempt, "FAIL", fail_step, reason)

        print("=== Loop Complete: FAILED ===")

    finally:
        cap.release()
        cv2.destroyAllWindows()


if __name__ == "__main__":
    main()

~/r2r2r-scripts/test_lookup_9points.py

本実装の前に走らせた、9点試し引き検証。実機通信なし、CSV検索のみで134,456点に対するnearest-neighborの妥当性を確認。

#!/usr/bin/env python3
"""
nearest-neighbor lookup 9点試し引き検証スクリプト
fk_scan_motion_based.csv から9点を検索し、距離・関節角度の妥当性を検証する。
実機通信なし、CSV検索のみ。
"""

import csv
import os
import math
from datetime import datetime

CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")
LOG_DIR = os.path.expanduser("~/r2r2r-scripts/logs")

# mechArm 270-Pi 可動域(公式スペック)
JOINT_LIMITS = [
    (-165.0, 165.0),  # j0
    (-165.0, 165.0),  # j1
    (-165.0, 165.0),  # j2
    (-165.0, 165.0),  # j3
    (-165.0, 165.0),  # j4
    (-175.0, 175.0),  # j5
]
ANGLE_WARN_MARGIN = 5.0  # 可動域端から±5°以内でWARN

# マット4隅(確定済み・変更禁止)
UL = (-89.0, -202.6)
UR = (-112.5, 204.6)
LL = (138.1, -172.9)
LR = (128.6, 182.9)

# 9点生成
def midpoint(*points):
    n = len(points)
    return (sum(p[0] for p in points) / n, sum(p[1] for p in points) / n)

POINTS = [
    ("P1 (UL corner)", UL),
    ("P2 (UR corner)", UR),
    ("P3 (LL corner)", LL),
    ("P4 (LR corner)", LR),
    ("P5 (top mid)", midpoint(UL, UR)),
    ("P6 (bottom mid)", midpoint(LL, LR)),
    ("P7 (left mid)", midpoint(UL, LL)),
    ("P8 (right mid)", midpoint(UR, LR)),
    ("P9 (center)", midpoint(UL, UR, LL, LR)),
]


def robot_to_unity(robot_x, robot_y):
    """Robot XY → Unity EE座標変換(確定済み)"""
    unity_x = robot_y * 0.001
    unity_z = robot_x * 0.001
    return unity_x, unity_z


def load_csv(path):
    """CSVを読み込んでリストで返す"""
    rows = []
    with open(path, "r") as f:
        reader = csv.reader(f)
        next(reader)  # ヘッダスキップ
        for row in reader:
            ee_x = float(row[0])
            ee_y = float(row[1])
            ee_z = float(row[2])
            joints = [float(row[j + 3]) for j in range(6)]
            rows.append((ee_x, ee_y, ee_z, joints))
    return rows


def nearest_neighbor(rows, query_x, query_z):
    """(ee_x, ee_z)の2次元ユークリッド距離で最近傍を検索"""
    best = None
    best_dist = float("inf")
    for ee_x, ee_y, ee_z, joints in rows:
        dist = math.sqrt((ee_x - query_x) ** 2 + (ee_z - query_z) ** 2)
        if dist < best_dist:
            best_dist = dist
            best = (ee_x, ee_y, ee_z, joints)
    return best, best_dist


def check_dist(dist_mm):
    if dist_mm <= 10:
        return "DIST_OK"
    elif dist_mm <= 20:
        return "DIST_WARN"
    else:
        return "DIST_FAIL"


def check_angles(joints):
    for j, angle in enumerate(joints):
        lo, hi = JOINT_LIMITS[j]
        if angle <= lo + ANGLE_WARN_MARGIN or angle >= hi - ANGLE_WARN_MARGIN:
            return "ANGLE_WARN"
    return "ANGLE_OK"


def main():
    print("=== nearest-neighbor lookup 9点検証 ===")
    print(f"CSV: {CSV_PATH}")

    rows = load_csv(CSV_PATH)
    print(f"読み込み完了: {len(rows)}点\n")

    timestamp = datetime.now().strftime("%Y%m%d_%H%M")
    log_path = os.path.join(LOG_DIR, f"lookup_test_{timestamp}.txt")

    lines = []
    dist_flags = {"DIST_OK": 0, "DIST_WARN": 0, "DIST_FAIL": 0}
    angle_flags = {"ANGLE_OK": 0, "ANGLE_WARN": 0}
    ee_ys = []

    for name, (robot_x, robot_y) in POINTS:
        unity_x, unity_z = robot_to_unity(robot_x, robot_y)
        match, dist = nearest_neighbor(rows, unity_x, unity_z)
        ee_x, ee_y, ee_z, joints = match
        dist_mm = dist * 1000  # m → mm

        df = check_dist(dist_mm)
        af = check_angles(joints)
        dist_flags[df] += 1
        angle_flags[af] += 1
        ee_ys.append(ee_y)

        block = []
        block.append(f"=== Point {name} ===")
        block.append(f"Query  : robot({robot_x}, {robot_y}) → unity({unity_x:.4f}, {unity_z:.4f})")
        block.append(f"Match  : ee=({ee_x:.4f}, {ee_y:.4f}, {ee_z:.4f})")
        block.append(f"         j0={joints[0]:.1f}, j1={joints[1]:.1f}, j2={joints[2]:.1f}, j3={joints[3]:.1f}, j4={joints[4]:.1f}, j5={joints[5]:.1f}")
        block.append(f"Dist   : {dist_mm:.2f} mm")
        block.append(f"ee_y   : {ee_y:.4f}")
        block.append(f"Flags  : {df}, {af}")

        # ANGLE_WARNの詳細
        if af == "ANGLE_WARN":
            for j, angle in enumerate(joints):
                lo, hi = JOINT_LIMITS[j]
                if angle <= lo + ANGLE_WARN_MARGIN:
                    block.append(f"  ⚠ j{j}={angle:.1f}° (下限{lo:.0f}°まで{angle - lo:.1f}°)")
                elif angle >= hi - ANGLE_WARN_MARGIN:
                    block.append(f"  ⚠ j{j}={angle:.1f}° (上限{hi:.0f}°まで{hi - angle:.1f}°)")

        block.append("")
        text = "\n".join(block)
        print(text)
        lines.append(text)

    # サマリ
    ee_y_min = min(ee_ys)
    ee_y_max = max(ee_ys)
    ee_y_mean = sum(ee_ys) / len(ee_ys)
    ee_y_stddev = math.sqrt(sum((y - ee_y_mean) ** 2 for y in ee_ys) / len(ee_ys))

    summary = []
    summary.append("=== Summary ===")
    summary.append(f"Total points : 9")
    summary.append(f"DIST_OK      : {dist_flags['DIST_OK']}")
    summary.append(f"DIST_WARN    : {dist_flags['DIST_WARN']}")
    summary.append(f"DIST_FAIL    : {dist_flags['DIST_FAIL']}")
    summary.append(f"ANGLE_OK     : {angle_flags['ANGLE_OK']}")
    summary.append(f"ANGLE_WARN   : {angle_flags['ANGLE_WARN']}")
    summary.append(f"ee_y range   : [{ee_y_min:.4f}, {ee_y_max:.4f}]")
    summary.append(f"ee_y stddev  : {ee_y_stddev:.4f}")
    summary.append("")

    summary_text = "\n".join(summary)
    print(summary_text)
    lines.append(summary_text)

    # ログ書き出し
    with open(log_path, "w") as f:
        f.write("\n".join(lines))
    print(f"ログ保存: {log_path}")


if __name__ == "__main__":
    main()

verify_y_direction.py

罠①の修正後の最終版。最初Claude Codeがsend_coords()を含むコードを書いてきたものを、send_angles()一本に書き直した。

#!/usr/bin/env python3
"""
Y方向確認スクリプト（Pi側）— send_angles版
ホーム → j0 負側 → ホーム の動作で、カメラ映像上の方向を確認する。
"""

from pymycobot.mycobot import MyCobot
import time

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

# Step 1: ホームへ
print("\n[Step 1] ホーム位置へ移動...")
mc.send_angles([0, 0, 0, 0, 0, -30], 20)
time.sleep(5)
print(f"  現在角度: {mc.get_angles()}")
input("  → カメラでホーム位置を確認してください。Enterで次へ...")

# Step 2: j0 を負側に -30° 振る（他の関節は維持）
print("\n[Step 2] j0 を -30° に動かす...")
mc.send_angles([-30, 0, 0, 0, 0, -30], 20)
time.sleep(5)
print(f"  現在角度: {mc.get_angles()}")
input("  → カメラでEEがどっち側に動いたか確認してください。Enterで次へ...")

# Step 3: ホームに戻す
print("\n[Step 3] ホームに戻す...")
mc.send_angles([0, 0, 0, 0, 0, -30], 20)
time.sleep(5)
print(f"  現在角度: {mc.get_angles()}")

print("\n=== 確認完了 ===")
print("画像「左」に動いた → 座標系OK（画像左 = Robot Y負側、j0 負 = Y 負側）")
print("画像「右」に動いた → 座標系の解釈を再検討")

~/r2r2r-scripts/CLAUDE.md

罠①を踏まえて、CLAUDE.md の末尾に「send_coords()使用禁止」を恒久ルール化した。今後Claude Codeに何を書かせても、このルールが効く。

## 物理憲法（追加ルール）

### send_coords() 使用禁止（DAY152で確定）
- mc.send_coords() の IK は RZ値が位置によって +167°〜−140° で暴れる
- ワークスペース全域で IK が不安定なため使用不可
- すべての動作は mc.send_angles() + lookup table 経由で実装する
- 例外なし。最適化や一時的な実験目的でも使用禁止
- 違反した場合、エイイチが即座に指摘・修正を要求

カメラindex特定の検証スニペット（罠③の決定打）

罠③で「カメラindex 1が真っ黒」を発見した時の検証コード。総当たりで全indexの輝度を測定し、Iriun Webcamの実際のindex（=2）を特定した。

import cv2
for i in range(5):
    cap = cv2.VideoCapture(i)
    if cap.isOpened():
        for _ in range(10):
            cap.read()
        ret, frame = cap.read()
        if ret:
            print(f'Camera {i}: OK, shape={frame.shape}, mean brightness={frame.mean():.1f}')
        else:
            print(f'Camera {i}: opened but no frame')
        cap.release()
    else:
        print(f'Camera {i}: not available')

実行結果：

Camera 0: OK, shape=(1080, 1920, 3), mean brightness=0.0
Camera 1: OK, shape=(1080, 1920, 3), mean brightness=0.0
Camera 2: OK, shape=(1080, 1920, 3), mean brightness=115.2
Camera 3: OK, shape=(1080, 1920, 3), mean brightness=117.2

これで「Iriunはindex 2か3にいる」と特定。各indexからフレーム画像を保存して目視確認し、index 2を採用。

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