mechArm 270 Pi 動的追従の限界を探る Log#19 | PLACE 動作の入口 設計と現場の往復で見えた構造的な詰まり

設計の前提が物理的に成立しないとき、書き直すのはコードではなく設計だ。

💡 Key Takeaway

PLACE 動作の設計を実装へ移す。ドラフトを 2 回書き直し、自己レビュー 9 件、外部レビュー 4 件を叩き込んだ。最初の実機テストで PLACE_APPROACH は一度だけ成立した。ただし、そこに至るまでに HOME 姿勢のズレ、カメラ index の入れ替わり、JOINT_LIMITS の弾きといった、コードの外側に潜む構造的な詰まりが順番に表面化した。設計と現場は別の論理で動いている。両者の往復の摩擦でしか埋まらない溝が、最初の APPROACH 1 回成立の手前に積み重なっていた。

何を作る一日だったか

LOG208 で動的追従の PICK が初成立した。次の段階は PLACE だ。

PICK と PLACE は構造的に似ている。視覚で捉え、上空へ回り、降りて、掴むか離すか。違うのは対象の高さと、グリッパーの開閉の向きだけだ。

このセッションは PLACE の入口を作る一日だった。設計ドラフトを書き、外部レビューを通し、Phase 0〜2(初期化・白箱検出・APPROACH 移動)だけを実装して実機で動かす。Phase 3 以降の DESCEND・OPEN・LIFT・HOME 復帰は、物理計測値が確定していないため別セッションに送る。

横カメラの役割を決め直す

設計に入る前に、横カメラから何を見るかを決める必要があった。

最初のたたき台は「横カメラから白箱と赤ブロックの両方を検出する」案だった。横カメラの画像内で、白箱の上端 y と赤ブロックの下端 y を毎フレーム比較すれば、赤ブロックが箱の中まで降りたかを判定できる。この案で検証スクリプトを書いた。

def detect_white_box(frame_bgr, mat_roi):
    """白箱を HSV マスクで検出。マット ROI 内に限定する。"""
    x_min, x_max, y_min, y_max = mat_roi
    roi = frame_bgr[y_min:y_max, x_min:x_max]
    hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
    s = hsv[:, :, 1]
    v = hsv[:, :, 2]
    mask = ((s <= WHITE_S_MAX) & (v >= WHITE_V_MIN)).astype(np.uint8) * 255

5 枚のサンプル画像で試した。結果は揃って NG だった。

=== place_sample_05_correct_position.png ===
  red:    (1164, 1000)
  white:  (1519, 959)
  x_diff: 355
  y_diff: 308
  verdict: APPROACH(x方向ずれ)

5 枚すべてで赤検出は正確に動いていた。問題は白箱検出だ。マット ROI 内に限定しても、白マスクは爪のグリッパー本体、アーム筐体、画面端の白い物体まで一つの連結成分として拾ってしまう。最大面積で 1 つを選ぶと、それは常に「爪+アームの巨大領域」になり、本物の白箱は無視された。

debug 画像を見ると一目瞭然だった。水色の bbox(白箱検出結果)が、写真ごとに白箱の位置を変えてもほぼ同じ場所に描かれている。検出していたのはアームの白筐体だった。

箱の位置自体は上カメラで決めて、横カメラは爪と赤の y を見るだけにする

この一言で設計が確定した。

横カメラに白箱を探させる必要はなくなった。設計骨格が固まる。上カメラで一度だけ白箱の (X, Z) を確定して上空へ跳び、横カメラは赤ブロックの降下 y だけを追う。

設計ドラフトを 2 回書く

設計ドラフト v1 を書いた。フェーズ構成、定数、関数構成、ログ仕様、物理リスクと対策、未確定事項——256 行。書き終わった直後に、自分で読み直して 9 件の修正点を出した。

これに加えて Claude Code を経由した codex レビューから 4 件の指摘が来た。

Phase 3 の判定軸が Step 2 のオフライン検証スクリプトとズレている MM_PER_PIX_SIDE が実は不要に見える Phase 5 LIFT の目標高さ表記が「現在地から +LIFT_EXTRA_MM」なのか「絶対 115mm」なのか曖昧 「未確定事項 #3」j2 単独 vs j3 併用は LOG210 着手前に手動 jog で確認しておくと実装スコープが固まる

合計 13 件のうち 1 件は重複していたので 12 件を反映して v2 を書いた。379 行。書きながら気づいたのは、「自分で書いて自分で読み直す」だけでは見つからない種類の指摘が、外部レビューには 4 件入っていたことだ。

特に重要だったのは「MM_PER_PIX_SIDE が不要」という指摘だった。v1 では「横カメラの mm/pixel 換算」として定数に入れていたが、よく考えると DESCEND は固定 STEP(mm 単位)で降下し、終了判定は pixel 単位で行うので、両者の間に変換が登場しない。可変ステップ最適化を入れるなら必要になるが、今の設計では使う場所がない。v2 から削除した。

自分が「使うつもりで書いた定数」の死蔵は、書いた本人には見えない。構造的な無駄は、他人の目を通した瞬間にあぶり出される。

Claude Code が書いた実装、codex が読み返す

v2 を実装指示書に変換して Claude Code に渡した。504 行のスクリプト place_test.py が返ってきた。

実装は概ね指示通りだった。visual_servo_pick.py の関数を import で再利用し、daemon 通信は既存パターンを踏襲し、JOINT_LIMITS の安全チェックも入っていた。

このコードを codex に再レビューさせた。5 件の指摘が返ってきた。

1. HOME 前提が崩れている。実コードでは HOME へ移動していない。実行時の現在姿勢から直接 APPROACH へ飛ぶため、HOME 前提が崩れると想定外の軌道になる

2. phase_0_init の赤検出が「爪に保持されている」確認として弱い。背景に赤い物があっても通る

3. ROTATION_ANGLES_DEG / SCALES が死蔵定数になっている

4. detect_white_box_top_camera が evaluate_frame を流用しており、本番系の検出と完全一致しない場合がある

5. phase_2_approach で到達後の角度誤差が大きくても True を返す

特に 1 と 5 は安全性に直結する。1 を放置すると初動から事故、5 を放置すると Phase 3 で「白箱の上空にいないつもりで降下開始」という事故になる。

5 件全部を反映した修正指示書を Claude Code に渡し、578 行に膨らんだ修正版が返ってきた。HOME 近傍チェックを Phase 0 冒頭に追加し、detect_red_block を引数で ROI と bbox サイズフィルタを切替可能にし、死蔵定数を削除し、APPROACH 到達後の角度誤差で ABORT するロジックを入れた。

「HOME 姿勢」前提が現実と衝突する

仕上がった修正版を実機にかけた。最初の実行で、修正 1 の HOME チェックが正しく動いた。

[2.84s] Phase 0 ABORT: HOME 姿勢から外れている  
        max_err=90.17° (tolerance=5.0°)
        measured=[-0.08, 89.64, -19.33, 3.6, -90.17, -29.97]
        expected=[0, 0, 0, 0, 0, -30]

j1 が +89.64°、j4 が -90.17°。HOME [0, 0, 0, 0, 0, -30] から 90° 以上ズレている姿勢でテストを開始したため、安全装置が正しく止めた。

ただし、これは想定外の発見だった。アームは「PICK 完了直後のような前傾姿勢」のままになっていた。理由はすぐ分かった。

今は最初からブロックが保持されてる状態だよ。

爪に赤ブロックを挟むためにアームを作業しやすい角度に倒し、その姿勢のままスクリプトを起動していた。設計指示書には「開始時、爪は赤ブロックを CLOSE で保持済み、HOME 姿勢に居る」と書いていたが、この 2 つの前提は両立しない。HOME 姿勢では爪先が上を向くため、物理的にブロックを挟むのが難しい。

修正 1(HOME チェック)は安全性に直結するので残す。ただし修正 2(赤ブロック保持確認)は Phase 0 から外す。Phase 0〜2 のテストではブロック保持の確認は不要で、Phase 3 DESCEND 実装時に追加すればよい。

設計上の前提が現実の運用と衝突した時、書き直すのは現実ではなく設計だ。

カメラ index が入れ替わる

修正版で HOME に戻して再実行すると、別の問題が出た。

OpenCV: out device of bound (0-1): 2
[ERROR] 上カメラ open 失敗 (index=2)

上カメラ(Continuity Camera)が認識されない。前回の実行では index=2 で見えていた iPhone 14 Pro Max が、消えていた。

カメラ index を確認するコードを書いて走らせた結果、index 2 は NOT opened。iPhone を起こして Continuity Camera を再認識させてから再度確認すると、index 2 が復活した。

index 0: opened, frame=(1080, 1920, 3)
index 1: opened, frame=(1080, 1920, 3)
index 2: opened, frame=(1080, 1920, 3)

3 つの index に対してそれぞれフレームを保存して目視で確認した。index 0 は真っ黒、index 1 は横カメラ(Iriun)、index 2 が上カメラだった。

物理的に接続されているかどうかと、OS から見える状態は別物だ。机の上で iPhone が点灯していても、AVFoundation 側で「カメラとして公開されている」状態でなければ OpenCV からは見えない。設計とは無関係な、運用の落とし穴だった。

j1 安全範囲を超えるという発見

カメラを揃えて再実行すると、今度は別の場所で止まった。

[FK] target=(X=-0.1724, Y=0.0350, Z=0.2615) dist=0.1mm 
     angles=[29.0, 110.74, -51.33, -12.4, -68.11, -30.0]
[ABORT] j1 安全範囲超過: 110.74° not in [-90.0, 100.0]

FK lookup 自体は成立していた。止めたのは制御ではなく、安全設計だった。

JOINT_LIMITS の j1 上限は 100°。FK が返した j1=110.74° は、この上限を 10° 超えていた。xyz_track_test.py(PICK で実績のある同等スクリプト)を grep すると、PICK 側でも同じ値だった。つまり、PICK 動作中は j1 が 100° を超える姿勢に行っていない。今回の白箱位置 X=-172mm, Z=261mm は、PICK で慣れている範囲(マット中央付近)から外れた極端な位置にあり、そこへ届くために大きな関節変位が必要だった。

選択肢は二つ。白箱の配置を PICK の実績範囲(マット中央)へ戻すか、JOINT_LIMITS の j1 上限を緩めるか。前者は計測の P008 原則「測れる範囲で先に動く」に沿った安全策であり、後者は実機の物理限界を攻めるリスクを伴う。

最終的に両方やった。白箱をマット中央付近に置き直し、その上で念のため j1 上限を一時的に 130° へ緩めた。

1 回目の APPROACH 成立、そして接触

修正後に再実行した。

[ 2.78s] Phase 0 START: HOME 姿勢確認
[ 2.81s] Phase 0 OK: HOME 近傍確認 max_err=1.58°
[ 2.81s] Phase 1 START: 上カメラで白箱検出
[20.39s] Phase 1 DETECT: 白箱 center=(1390,796) score=0.724 angle=0° scale=0.85
[20.39s] Phase 1 OK: robot 座標 X=-172.4mm Z=261.5mm
[20.39s] Phase 2 START: APPROACH target=(X=-172.4mm, Y=75.0mm, Z=261.5mm)
[21.16s] Phase 2 SEND: send_angles [37.0, 77.91, -18.14, 7.92, -54.27, -29.44]
[29.21s] Phase 2 REACH: measured=[36.03, 76.99, -18.63, 8.17, -57.48, -29.97] max_err=3.21°
[29.21s] Phase 2 ABORT: APPROACH 角度誤差超過 max_err=3.21° (tolerance=3.0°)

アームが白箱の上空 Y=75mm 付近へ移動した。

ただし、横カメラから見ると 爪がすでに白箱に接触している状態だった。

座標は正しかった。構造が間違っていた。

設計上は箱上端 45mm + 余裕 30mm = Y=75mm で爪が箱の上空 30mm の位置に来るはずだった。実際にぶつかっていたのは、爪先より手前のリンクだった。FK が指す位置に「爪先」は来ていたが、アームの形状がその上空に余分なボリュームを持ち込んでいた。

到達後の角度誤差は 3.21°。tolerance 3.0° をわずかに超えて ABORT 扱いになったが、移動自体は成立している。FK lookup が出した目標角度に対して、実機は 3.21° 以内で追従できていた。

今日のテストはここで止めた。Phase 0〜2 が一通り動作し、APPROACH が一度成立した。

完成した place_test.py

最終的に手元に残ったスクリプトの全量を載せる。578 行。Phase 0〜2 までを実装し、Phase 3 以降は別セッションへ送る形にした。設計指示書とのズレを 5 件直し、HOME チェック・JOINT_LIMITS チェック・角度誤差 ABORT がすべて入っている状態だ。

#!/usr/bin/env python3
"""
place_test.py (DAY209)

PLACE 動作スクリプト (Phase 0〜2 のみ)。
赤ブロックを爪に CLOSE 保持した状態で HOME から開始し、白箱上空 Y=75mm まで
移動して停止する。Phase 3 (DESCEND) 以降は DAY210 以降で段階追加する予定。

恒久ルール:
    - send_coords() 禁止
    - release_all_servos() 禁止
    - fk_scan_motion_based.csv は読込のみ
    - homography.npy は変更しない
"""

import csv as csv_module
import os
import sys
import time
from datetime import datetime

import cv2
import numpy as np

import visual_servo_pick as vsp
from visual_servo_pick import (
    PICK_EE_Y,
    close_daemon_socket,
    daemon_get_angles,
    daemon_send_angles,
    daemon_set_fresh_mode,
    daemon_set_gripper,
    get_daemon_socket,
    load_fk_table,
    lookup_nearest_angles_with_ee,
    pixel_to_unity,
    setup_homography_and_mat_range,
)
from test_white_box_score import (
    detect_mat_range as _detect_top_mat_range,
    evaluate_frame as _evaluate_white_box,
    load_templates as _load_white_box_templates,
)


# ============================================================
# 定数
# ============================================================

# ---- 物理計測 ----------------------------------------------------------
BOX_HEIGHT_MM = 45.0
APPROACH_OFFSET_MM = 30.0
LIFT_EXTRA_MM = 40.0
Y_APPROACH_MM = BOX_HEIGHT_MM + APPROACH_OFFSET_MM            # 75
Y_LIFT_ABS_MM = BOX_HEIGHT_MM + APPROACH_OFFSET_MM + LIFT_EXTRA_MM  # 115 (Phase 5 用、未使用)

# ---- 横カメラ・赤検出(test_place_detection_offline.py と同じ HSV)----
SIDE_CAMERA_INDEX = 1
RED_H_MIN_1, RED_H_MAX_1 = 0, 10
RED_H_MIN_2, RED_H_MAX_2 = 170, 180
RED_S_MIN, RED_V_MIN = 100, 80
RED_MIN_AREA = 100
RED_MORPH_KERNEL = 5

# Phase 0 の赤検出 ROI(画像比率、HOME 姿勢で爪が映る中央付近)
RED_ROI_X_MIN_RATIO = 0.30
RED_ROI_X_MAX_RATIO = 0.70
RED_ROI_Y_MIN_RATIO = 0.30
RED_ROI_Y_MAX_RATIO = 0.85

# bbox サイズの妥当性範囲(赤ブロックは画角内で概ね 60〜200px 角)
RED_BBOX_MIN_SIDE = 40
RED_BBOX_MAX_SIDE = 300

# ---- 上カメラ・白箱検出 -------------------------------------------------
# 注: 回転角度 / スケールの実値は test_white_box_score.evaluate_frame の内部定数で
# 駆動される。ここで定義しても挙動に反映されないため定数は持たない。
TOP_CAMERA_INDEX = 2
TEMPLATE_DIR = os.path.expanduser("~/r2r2r-scripts/templates")
TEMPLATE_SCORE_THRESHOLD = 0.5

# ---- グリッパー -------------------------------------------------------
GRIPPER_CLOSE = 1     # visual_servo_pick.daemon_set_gripper の規約に合わせる: 1=閉
GRIPPER_OPEN = 0      # 0=開(Phase 4 で使用、Phase 0〜2 では未使用)

# ---- 姿勢 -------------------------------------------------------------
HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
APPROACH_WAIT_SEC = 8.0    # APPROACH 到達待ち。j1 の大移動を含む可能性あり
HOME_ANGLE_TOLERANCE_DEG = 5.0    # HOME 近傍判定の許容誤差
MAX_APPROACH_ERROR_DEG = 3.0      # APPROACH 到達後の角度誤差許容値

# ---- 関節制限(xyz_track_test と同等)-----------------------------------
JOINT_LIMITS = [
    (-160.0, 160.0),    # j0
    ( -90.0, 130.0),    # j1
    (-170.0,  40.0),    # j2
    (-150.0, 150.0),    # j3
    (-100.0, 100.0),    # j4
    (-180.0, 180.0),    # j5
]

# ---- パス -------------------------------------------------------------
HOMOGRAPHY_PATH = os.path.expanduser("~/r2r2r-scripts/homography.npy")
FK_TABLE_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")
LOGS_DIR = os.path.expanduser("~/r2r2r-scripts/logs")


# ============================================================
# ログ
# ============================================================

def setup_logging(timestamp):
    """テキストログと CSV ログのファイルハンドルを作成。

    戻り値: (txt_handle, csv_handle, csv_writer)
    CSV ヘッダ: loop, time, phase, event, detail
    """
    os.makedirs(LOGS_DIR, exist_ok=True)
    txt_path = os.path.join(LOGS_DIR, f"place_test_{timestamp}.txt")
    csv_path = os.path.join(LOGS_DIR, f"place_test_{timestamp}.csv")
    txt_handle = open(txt_path, "w")
    csv_handle = open(csv_path, "w", newline="")
    csv_writer = csv_module.writer(csv_handle)
    csv_writer.writerow(["loop", "time", "phase", "event", "detail"])
    csv_handle.flush()
    print(f"[LOG] txt: {txt_path}")
    print(f"[LOG] csv: {csv_path}")
    return txt_handle, csv_handle, csv_writer


_log_start_time = None


def log_phase(txt, csv_writer, phase, event, detail=""):
    """両方のログに 1 行書く。stdout にも echo する。"""
    global _log_start_time
    now = time.time()
    if _log_start_time is None:
        _log_start_time = now
    elapsed = now - _log_start_time
    line = f"[{elapsed:6.2f}s] Phase {phase} {event}: {detail}"
    print(line)
    if txt is not None:
        txt.write(line + "\n")
        txt.flush()
    if csv_writer is not None:
        csv_writer.writerow(["", f"{elapsed:.3f}", phase, event, detail])


# ============================================================
# 初期化系
# ============================================================

def open_cameras():
    """横カメラと上カメラを開く。失敗時 (None, None)。"""
    side_cap = cv2.VideoCapture(SIDE_CAMERA_INDEX)
    if not side_cap.isOpened():
        print(f"[ERROR] 横カメラ open 失敗 (index={SIDE_CAMERA_INDEX})")
        return None, None
    for _ in range(30):
        side_cap.read()

    top_cap = cv2.VideoCapture(TOP_CAMERA_INDEX)
    if not top_cap.isOpened():
        print(f"[ERROR] 上カメラ open 失敗 (index={TOP_CAMERA_INDEX})")
        side_cap.release()
        return None, None
    for _ in range(30):
        top_cap.read()
    return side_cap, top_cap


def load_homography_via_top_cap(top_cap):
    """上カメラからマット四隅を検出して動的 homography を構築。

    visual_servo_pick.setup_homography_and_mat_range を流用。
    成功時 True / 失敗時 False。
    """
    return setup_homography_and_mat_range(top_cap)


# ============================================================
# 検出系
# ============================================================

def detect_red_block(frame_bgr, use_roi=False, validate_bbox_size=False):
    """横カメラの 1 フレームから赤ブロックの bbox を取得。

    HSV 2 区間マスク(0-10, 170-180)。

    Args:
        frame_bgr: 入力フレーム
        use_roi: True なら画像中央の ROI に限定して検出(Phase 0 用)
        validate_bbox_size: True なら bbox サイズの妥当性をチェック(Phase 0 用)

    戻り値: {"bbox": (x, y, w, h), "bottom_y": int} または None。
    """
    h, w = frame_bgr.shape[:2]

    if use_roi:
        rx_min = int(w * RED_ROI_X_MIN_RATIO)
        rx_max = int(w * RED_ROI_X_MAX_RATIO)
        ry_min = int(h * RED_ROI_Y_MIN_RATIO)
        ry_max = int(h * RED_ROI_Y_MAX_RATIO)
        roi = frame_bgr[ry_min:ry_max, rx_min:rx_max]
    else:
        rx_min, ry_min = 0, 0
        roi = frame_bgr

    hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
    mask1 = cv2.inRange(
        hsv,
        (RED_H_MIN_1, RED_S_MIN, RED_V_MIN),
        (RED_H_MAX_1, 255, 255),
    )
    mask2 = cv2.inRange(
        hsv,
        (RED_H_MIN_2, RED_S_MIN, RED_V_MIN),
        (RED_H_MAX_2, 255, 255),
    )
    mask = mask1 | mask2

    k = np.ones((RED_MORPH_KERNEL, RED_MORPH_KERNEL), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, k)
    mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, k)

    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None
    largest = max(contours, key=cv2.contourArea)
    if cv2.contourArea(largest) < RED_MIN_AREA:
        return None
    x, y, bw, bh = cv2.boundingRect(largest)

    if validate_bbox_size:
        if bw < RED_BBOX_MIN_SIDE or bw > RED_BBOX_MAX_SIDE:
            return None
        if bh < RED_BBOX_MIN_SIDE or bh > RED_BBOX_MAX_SIDE:
            return None

    # ROI 座標系から frame 座標系へ戻す
    abs_x = x + rx_min
    abs_y = y + ry_min
    return {"bbox": (abs_x, abs_y, bw, bh), "bottom_y": abs_y + bh}


def detect_white_box_top_camera(top_cap, templates_by_group):
    """上カメラから 1 フレーム取得し、テンプレマッチングで白箱中心を取得。

    test_white_box_score.evaluate_frame の overall_best を流用。
    マット bbox は visual_servo_pick._mat_px_range(setup_homography 直後に確定)を再利用。

    注: evaluate_frame は本来スコア評価用の関数。本番系(place_test)が
    そのまま流用しているため、検出ロジックの差がそのまま PLACE 移動先の差になる。
    将来は白箱検出関数を test_white_box_score から独立させ、両者で共有する設計が望ましい。

    戻り値: {"center": (cx, cy), "score": float, "angle": int, "scale": float}
            または None(スコア < 閾値の場合も None)。
    """
    if vsp._mat_px_range is None:
        return None

    for _ in range(2):
        top_cap.read()
    ret, frame = top_cap.read()
    if not ret or frame is None:
        return None

    mat_bbox = vsp._mat_px_range
    results = _evaluate_white_box(frame, mat_bbox, templates_by_group)
    best = results.get("overall_best")
    if best is None or best.get("best_score") is None:
        return None
    if best["best_score"] < TEMPLATE_SCORE_THRESHOLD:
        return {
            "center": (best["cx"], best["cy"]),
            "score": best["best_score"],
            "angle": best.get("best_angle"),
            "scale": best.get("best_scale"),
            "below_threshold": True,
        }
    return {
        "center": (best["cx"], best["cy"]),
        "score": best["best_score"],
        "angle": best.get("best_angle"),
        "scale": best.get("best_scale"),
        "below_threshold": False,
    }


def pixel_to_robot_xz(cx, cy):
    """pixel 座標 → robot Unity 座標 (X, Z) [m]。

    visual_servo_pick.pixel_to_unity をそのまま使う。
    homography が未ロードならエラーになる前提(呼び出し前に setup 済み想定)。
    """
    unity_x, unity_z = pixel_to_unity(cx, cy)
    return unity_x, unity_z


# ============================================================
# 動作系
# ============================================================

def lookup_fk(X, Y, Z):
    """FK lookup table から (X, Y, Z) [m] に対応する角度を取得。

    X = unity_x (= mat 横方向), Z = unity_z (= mat 奥行), Y = ee_y (= 高さ)。
    visual_servo_pick.lookup_nearest_angles_with_ee を呼ぶ。失敗時 None。
    """
    angles, dist, ee = lookup_nearest_angles_with_ee(X, Z, Y, label="PLACE_APPROACH")
    if angles is None:
        print(f"  [WARN] lookup_fk 失敗 dist={dist*1000:.1f}mm")
        return None
    print(f"  [FK] target=(X={X:.4f}, Y={Y:.4f}, Z={Z:.4f}) "
          f"matched ee={ee} dist={dist*1000:.1f}mm "
          f"angles={[round(a, 2) for a in angles]}")
    return angles


def safety_check_angles(angles):
    """各関節が JOINT_LIMITS 内か確認する。"""
    for i, a in enumerate(angles):
        lo, hi = JOINT_LIMITS[i]
        if not (lo <= a <= hi):
            print(f"[ABORT] j{i} 安全範囲超過: {a:.2f}° not in [{lo}, {hi}]")
            return False
    return True


# ============================================================
# Phase 系
# ============================================================

def phase_0_init(side_cap, txt, csv_writer):
    """初期化チェック。現在角度を取得して HOME 近傍か確認、外れていれば ABORT。

    Phase 0〜2 のテストではブロック保持確認は不要のため、赤検出は行わない。
    Phase 3 DESCEND 実装時にブロック保持確認を別途追加する。
    """
    log_phase(txt, csv_writer, 0, "START", "HOME 姿勢確認")

    measured = daemon_get_angles()
    if measured is None or len(measured) != 6:
        log_phase(txt, csv_writer, 0, "ABORT", "現在角度取得失敗")
        return False
    max_home_err = max(abs(measured[i] - HOME[i]) for i in range(6))
    if max_home_err > HOME_ANGLE_TOLERANCE_DEG:
        log_phase(txt, csv_writer, 0, "ABORT",
                  f"HOME 姿勢から外れている max_err={max_home_err:.2f}° "
                  f"(tolerance={HOME_ANGLE_TOLERANCE_DEG}°) "
                  f"measured={[round(a, 2) for a in measured]} "
                  f"expected={HOME}")
        return False
    log_phase(txt, csv_writer, 0, "OK",
              f"HOME 近傍確認 max_err={max_home_err:.2f}°")
    return True


def phase_1_detect_box(top_cap, templates_by_group, txt, csv_writer):
    """上カメラで白箱検出 → robot 座標 (X_box, Z_box) [m] に変換。"""
    log_phase(txt, csv_writer, 1, "START", "上カメラで白箱検出")

    # 上カメラのマット bbox を初回検出(visual_servo_pick._mat_px_range が既にあれば再利用)
    if vsp._mat_px_range is None:
        log_phase(txt, csv_writer, 1, "ERROR",
                  "マット bbox 未初期化(setup_homography_and_mat_range が失敗?)")
        return None

    result = detect_white_box_top_camera(top_cap, templates_by_group)
    if result is None:
        log_phase(txt, csv_writer, 1, "ABORT",
                  "白箱検出失敗(テンプレマッチング結果なし)")
        return None
    if result.get("below_threshold"):
        log_phase(txt, csv_writer, 1, "ABORT",
                  f"白箱スコア閾値未達 score={result['score']:.3f} "
                  f"< {TEMPLATE_SCORE_THRESHOLD}")
        return None

    cx, cy = result["center"]
    log_phase(txt, csv_writer, 1, "DETECT",
              f"白箱 center=({cx},{cy}) score={result['score']:.3f} "
              f"angle={result['angle']}° scale={result['scale']}")

    X_box, Z_box = pixel_to_robot_xz(cx, cy)
    log_phase(txt, csv_writer, 1, "OK",
              f"robot 座標 X={X_box*1000:.1f}mm Z={Z_box*1000:.1f}mm")
    return X_box, Z_box


def phase_2_approach(X_box, Z_box, txt, csv_writer):
    """白箱上空(Y=Y_APPROACH_MM)へ移動。lookup_fk + send_angles。

    target_ee_y は「マット表面 ≒ PICK_EE_Y」を基準に Y_APPROACH_MM 上方へ。
    PICK_EE_Y(-0.040) + 0.075 = 0.035 [m]。
    """
    target_ee_y = PICK_EE_Y + Y_APPROACH_MM * 0.001
    log_phase(txt, csv_writer, 2, "START",
              f"APPROACH target=(X={X_box*1000:.1f}mm, "
              f"Y={Y_APPROACH_MM:.1f}mm above PICK_EE_Y, "
              f"Z={Z_box*1000:.1f}mm) -> target_ee_y={target_ee_y:.4f}m")

    angles = lookup_fk(X_box, target_ee_y, Z_box)
    if angles is None:
        log_phase(txt, csv_writer, 2, "ABORT", "lookup_fk 失敗")
        return False

    if not safety_check_angles(angles):
        log_phase(txt, csv_writer, 2, "ABORT",
                  f"safety_check 失敗 angles={[round(a, 2) for a in angles]}")
        return False

    log_phase(txt, csv_writer, 2, "SEND",
              f"send_angles {[round(a, 2) for a in angles]} speed={SPEED}")
    try:
        daemon_send_angles(angles, speed=SPEED, wait=0)
    except Exception as e:
        log_phase(txt, csv_writer, 2, "ERROR", f"daemon_send_angles 例外: {e}")
        return False
    time.sleep(APPROACH_WAIT_SEC)

    measured = daemon_get_angles()
    if measured is None or len(measured) != 6:
        log_phase(txt, csv_writer, 2, "ABORT", "到達後の角度取得失敗")
        return False

    max_err = max(abs(measured[i] - angles[i]) for i in range(6))
    log_phase(txt, csv_writer, 2, "REACH",
              f"measured={[round(a, 2) for a in measured]} "
              f"max_err={max_err:.2f}°")

    if max_err > MAX_APPROACH_ERROR_DEG:
        log_phase(txt, csv_writer, 2, "ABORT",
                  f"APPROACH 角度誤差超過 max_err={max_err:.2f}° "
                  f"(tolerance={MAX_APPROACH_ERROR_DEG}°)")
        return False

    return True


# ============================================================
# main
# ============================================================

def main():
    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
    print("=" * 60)
    print(f"place_test.py (DAY209) Phase 0〜2 only  ts={timestamp}")
    print("=" * 60)

    txt, csv_handle, csv_writer = setup_logging(timestamp)

    side_cap, top_cap = open_cameras()
    if side_cap is None or top_cap is None:
        log_phase(txt, csv_writer, -1, "ABORT", "カメラ open 失敗")
        if txt:
            txt.close()
        if csv_handle:
            csv_handle.close()
        return

    log_phase(txt, csv_writer, -1, "INFO",
              f"side_idx={SIDE_CAMERA_INDEX} top_idx={TOP_CAMERA_INDEX}")

    # FK テーブル
    try:
        load_fk_table()
    except Exception as e:
        log_phase(txt, csv_writer, -1, "ABORT", f"FK テーブル読込失敗: {e}")
        side_cap.release()
        top_cap.release()
        txt.close()
        csv_handle.close()
        return

    # Homography(上カメラ)
    if not load_homography_via_top_cap(top_cap):
        log_phase(txt, csv_writer, -1, "ABORT", "homography セットアップ失敗")
        side_cap.release()
        top_cap.release()
        txt.close()
        csv_handle.close()
        return

    # 白箱テンプレ
    try:
        templates_by_group = _load_white_box_templates()
    except SystemExit:
        log_phase(txt, csv_writer, -1, "ABORT", "テンプレ読込失敗(SystemExit)")
        side_cap.release()
        top_cap.release()
        txt.close()
        csv_handle.close()
        return

    # daemon 接続
    try:
        get_daemon_socket()
        daemon_set_fresh_mode(1)
    except Exception as e:
        log_phase(txt, csv_writer, -1, "ABORT", f"daemon 接続失敗: {e}")
        side_cap.release()
        top_cap.release()
        txt.close()
        csv_handle.close()
        return

    try:
        # Phase 0
        if not phase_0_init(side_cap, txt, csv_writer):
            log_phase(txt, csv_writer, 0, "ABORT", "ブロック保持されてない")
            return

        # Phase 1
        result = phase_1_detect_box(top_cap, templates_by_group, txt, csv_writer)
        if result is None:
            log_phase(txt, csv_writer, 1, "ABORT", "白箱検出失敗")
            return
        X_box, Z_box = result

        # Phase 2
        if not phase_2_approach(X_box, Z_box, txt, csv_writer):
            log_phase(txt, csv_writer, 2, "ABORT", "APPROACH 失敗")
            return

        log_phase(txt, csv_writer, 2, "DONE",
                  "Phase 2 完了。手動確認後 Ctrl+C で終了")
        print("\n[STOPPED] Phase 2 完了。爪が白箱上空 Y=75mm にあるはず。"
              "目視確認してから Ctrl+C で終了。")

        try:
            while True:
                time.sleep(1)
        except KeyboardInterrupt:
            print("\n[EXIT] 手動終了")
    except KeyboardInterrupt:
        print("\n[INTERRUPT] Ctrl+C 検出")
        log_phase(txt, csv_writer, -1, "INTERRUPT", "Ctrl+C")
    finally:
        try:
            daemon_set_fresh_mode(0)
        except Exception:
            pass
        try:
            close_daemon_socket()
        except Exception:
            pass
        side_cap.release()
        top_cap.release()
        cv2.destroyAllWindows()
        if txt:
            txt.close()
        if csv_handle:
            csv_handle.close()
        print("[DONE]")


if __name__ == "__main__":
    main()

確定したこと、していないこと

確定したこと:

• 横カメラの役割を「絶対位置の検出」から「relative 比較」に再定義

• 上カメラで白箱を検出し homography で robot 座標へ変換するパイプラインが HOME 姿勢で成立する(score=0.724〜0.735)

• FK lookup の精度は dist=0.1mm(512,087 点テーブル)

• HOME 近傍チェック・JOINT_LIMITS チェック・角度誤差 ABORT がすべて意図通り動作する

• PLACE_APPROACH(Y=75mm 上空)の到達が一度成立した

未解決:

• 爪が白箱に物理的に接触する。APPROACH_OFFSET_MM を 30 → 50 へ調整、もしくはアームの構造的な前傾の解析が必要

• 角度誤差 tolerance 3° は厳しすぎる可能性。5° まで緩める判断は次セッションで

• JOINT_LIMITS の j1 上限を 100° → 130° に緩めたままになっている。実機の本当の限界を再確認するか、白箱の配置範囲をマット中央付近に固定する運用にするかの判断

• Y_RED_TARGET_PIX は物理計測未完。Phase 3 DESCEND 実装の前提

• PICK 側の保持力問題(爪が CLOSE してもブロックを安定保持できない現象)は今日触っていない

今回の感想

設計を 2 回書いて、コードを 2 回書いて、実機を 4 回動かして、最初の APPROACH が 1 回成立した。

設計の往復で発見できたのは「横カメラの役割再定義」と「Phase 0 の保持確認は不要」の 2 つ。実機を動かして初めて発見できたのは「HOME 姿勢で爪にブロックを挟めない」「上カメラ index が状態によって入れ替わる」「JOINT_LIMITS が極端な位置の FK 出力を弾く」の 3 つ。

設計だけでは見つからない問題が、現場には必ず 3 つは潜んでいる。設計の精度不足ではない。設計の論理と現場の物理は別物であり、その往復の摩擦でしか埋まらない溝がある。

最初に成立するのは動作ではない。設計と現場の往復そのものだ。

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