mechArm 270 Pi 動的追従の限界を探る Log#21 | place_test.py を 6 つの単体スクリプトに解体した記録

Key Takeaway

Log#20 で 4 回修正しても動かなかった place_test.py 739 行を、6 つの単体スクリプトに解体した。コードを書き直すのではなく、コードを分ける作業に切り替えたとたん、AI の推測は測定に置き換わった。誤検出の真因はテンプレの経年劣化。角度のズレは撮影手順の揺れ。描画の狂いは座標規約の反転。高速化の余地はグレースケール化に隠されていた。すべてを一度に動かそうとしていた手を止め、コードを切り離した瞬間に、これらが一つずつ独立して観測可能なデータになった。

直すより解体する

Log#20 は place_test.py を 4 回修正して、4 回とも外した日だった。fresh_mode のタイミングを 3 通り書き換え、HOME 定数を一度疑い、wait=0 への切り替えも試した。アームは動かなかった。xyz_track_test.py を単独で叩くとアームは動く。place_test.py だけが沈黙した。

差分の在処は掴めないまま日が暮れた。

翌セッション、再開する前に「もう一度この 739 行を直す」のは間違いだと判断した。直すべきはコードではなかった。全部を一度に動かそうとする設計そのものが、AI の判断粒度を超えていた。

place_test.pyが上手く動作しないため（place_test.pyだけではなく、今までの作り方が雑すぎたと感じています） コードの生成の方法を切り替えてみます。 切り替え方法として、コード内に必要な項目を先に定義して各項目ごとに別々にスクリプトを作成した後に、最終的にコードを統合します。

設計判断は明示的だった。

place_test.py を構成要素ごとに切り出す。01: daemon 接続。02: send_angles。03: カメラプレビュー。04: 白箱検出。05: テンプレ撮影。06: 検出統合。07: テンプレ分類。各スクリプトは単独実行可能で、検証する挙動が一つだけある。

これは一般に「単一責任の原則」や「リファクタリング」と呼ばれる。だが今回の動機はコードの美化ではない。AI が文脈を正確に把握し、判断できる粒度まで物理的に引き剥がすこと。その意図の差は、結果の解像度として現れた。

フォルダ構造を物理的に分離する

r2r2r-scripts/ 直下は既に PICK 用のスクリプト群（xyz_track_test.py, visual_servo_pick.py, mecharm_daemon.py ほか）が存在する。PLACE 用の解体作業を始めるにあたり、PICK 側を一切壊さない物理的な分離が必要だった。

~/r2r2r-scripts/
├── visual_servo_pick.py        ← PICK 側、触らない
├── xyz_track_test.py           ← PICK 側、触らない
├── mecharm_daemon.py           ← Pi 側 daemon、触らない
├── test_white_box_score.py     ← 既存検出ロジック、触らない
├── templates/                  ← 既存テンプレ、触らない
└── dynamic_tracking_limits/
    └── place/
        ├── place_modules/      ← 動作単位ごとの単体スクリプト
        ├── place_integrated/   ← 統合済みコード（最終的にここ）
        ├── templates/          ← PLACE 用テンプレ（新規撮影）
        └── templates_skewed/   ← 撮影中に混入した斜めテンプレを退避

mkdir -p で一気に作る。

mkdir -p ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules
mkdir -p ~/r2r2r-scripts/dynamic_tracking_limits/place/place_integrated
mkdir -p ~/r2r2r-scripts/dynamic_tracking_limits/place/templates

PICK 側に影響しない。退避や追加が必要になった時も mv で戻せる。git clean も使わない。物理的な隔離が、AI の作業範囲を限定する装置として機能する。

daemon を常駐させる

これまでは SSH でターミナルを 1 枚開き、Pi で python3 mecharm_daemon.py をフォアグラウンド起動し、SSH を切らないようにしていた。

セッションを切るたびに daemon が止まると、検証の最小単位を Mac 側で叩くたびに「Pi の状態」が変わる可能性を排除できない。daemon 通信を白として扱うには、daemon が常駐している前提を物理的に保証する必要がある。

nohup で起動する。

ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi
# Pi に入った状態で
ps aux | grep mecharm_daemon | grep -v grep   # 既存プロセスを確認
pkill -f mecharm_daemon                        # 古いプロセスを停止
nohup python3 ~/r2r2r-scripts/mecharm_daemon.py > ~/mecharm_daemon.log 2>&1 &
sleep 2 && tail -10 ~/mecharm_daemon.log       # 起動確認
exit                                            # SSH を切る

起動ログ:

[DAEMON 20:35:41] MyCobot 初期化中 (/dev/ttyAMA0, baud=1000000)...
[DAEMON 20:35:41] ✓ MyCobot 初期化完了
[DAEMON 20:35:41] ポート 5555 で待ち受け中...

SSH を切っても daemon は走り続ける。アームの電源を切るまで稼働。Mac 側のスクリプトは起動するたびに get_daemon_socket() で接続するだけ。

これで「Pi 側 daemon が常駐」「Mac 側スクリプトは毎回接続」という運用が確定した。daemon の挙動を毎回確認する手間が消える。

daemon 接続だけを検証する

place_modules/01_daemon_minimal.py。検証する挙動はひとつ。daemon に接続して fresh_mode=1 を立て、明示的に close するだけのスクリプトがエラーなく完走するか。送信もカメラも何もしない。

"""
01_daemon_minimal.py

検証する挙動:
  daemon に接続 → fresh_mode=1 → 0.5s 待機 → fresh_mode=0 → close
  これだけがエラーなく完走するか。

含めない処理:
  - send_angles / get_angles / set_gripper
  - カメラ
  - FK lookup
"""

import sys
import time
from pathlib import Path

# place_modules/ から見て r2r2r-scripts/ は 3 階層上
SCRIPTS_ROOT = Path(__file__).resolve().parents[3]
sys.path.insert(0, str(SCRIPTS_ROOT))

import visual_servo_pick as vsp


def main():
    print("[01] start")

    print("[01] connecting to daemon ...")
    vsp.get_daemon_socket()
    print("[01] connected")

    print("[01] fresh_mode=1 ...")
    vsp.daemon_set_fresh_mode(1)
    print("[01] fresh_mode=1 done")

    time.sleep(0.5)

    print("[01] fresh_mode=0 ...")
    vsp.daemon_set_fresh_mode(0)
    print("[01] fresh_mode=0 done")

    print("[01] closing daemon socket ...")
    vsp.close_daemon_socket()
    print("[01] closed")

    print("[01] all done, no error")


if __name__ == "__main__":
    main()

実行ログ。

[01] start
[01] connecting to daemon ...
[01] connected
[01] fresh_mode=1 ...
  [Pi] fresh_mode=1
[01] fresh_mode=1 done
[01] fresh_mode=0 ...
  [Pi] fresh_mode=0
[01] fresh_mode=0 done
[01] closing daemon socket ...
[01] closed
[01] all done, no error

daemon は白と確定した。Pi 側のエコー [Pi] fresh_mode=1 / [Pi] fresh_mode=0 が返っている。

send_angles 単発を検証する

place_modules/02a_send_angles_small.py。HOME → 小さい動き → HOME の最小シーケンス。j1 だけを -10° 動かす。

"""
02a_send_angles_small.py

検証する挙動:
  daemon 経由で send_angles を 1 回だけ送って、アームが実際に動くか。
  HOME → 小さい動き → HOME の最小シーケンス。

含めない処理:
  - カメラ
  - FK lookup
  - 複数姿勢の連続送信
  - tolerance チェック
"""

import sys
import time
from pathlib import Path

SCRIPTS_ROOT = Path(__file__).resolve().parents[3]
sys.path.insert(0, str(SCRIPTS_ROOT))

import visual_servo_pick as vsp

HOME = [0, 0, 0, 0, 0, -30]
SMALL_MOVE = [0, -10, 0, 0, 0, -30]  # j1 だけ -10°
SPEED = 20


def fmt(angles):
    if angles is None:
        return "None"
    return "[" + ", ".join(f"{a:.2f}" for a in angles) + "]"


def main():
    print("[02a] start")

    print("[02a] connecting to daemon ...")
    vsp.get_daemon_socket()
    print("[02a] connected")

    print("[02a] fresh_mode=1 ...")
    vsp.daemon_set_fresh_mode(1)
    time.sleep(0.5)
    print("[02a] fresh_mode=1 done")

    # Step 1: HOME を確実に取る
    print(f"[02a] sending HOME {HOME} ...")
    vsp.daemon_send_angles(HOME, speed=SPEED, wait=0)
    time.sleep(3.0)
    measured = vsp.daemon_get_angles()
    print(f"[02a] after HOME: measured = {fmt(measured)}")

    # Step 2: 小さい動き
    print(f"[02a] sending SMALL_MOVE {SMALL_MOVE} ...")
    vsp.daemon_send_angles(SMALL_MOVE, speed=SPEED, wait=0)
    time.sleep(3.0)
    measured = vsp.daemon_get_angles()
    print(f"[02a] after SMALL_MOVE: measured = {fmt(measured)}")

    if measured is not None:
        diff_j1 = abs(measured[1] - SMALL_MOVE[1])
        print(f"[02a] j1 diff from target: {diff_j1:.2f}°")

    # Step 3: HOME 復帰
    print(f"[02a] returning to HOME {HOME} ...")
    vsp.daemon_send_angles(HOME, speed=SPEED, wait=0)
    time.sleep(3.0)
    measured = vsp.daemon_get_angles()
    print(f"[02a] after return HOME: measured = {fmt(measured)}")

    # 後始末
    print("[02a] fresh_mode=0 ...")
    vsp.daemon_set_fresh_mode(0)
    print("[02a] fresh_mode=0 done")

    print("[02a] closing daemon socket ...")
    vsp.close_daemon_socket()
    print("[02a] closed")

    print("[02a] all done")


if __name__ == "__main__":
    main()

実機ログ。

[02a] start
[02a] connecting to daemon ...
[02a] connected
[02a] fresh_mode=1 ...
  [Pi] fresh_mode=1
[02a] fresh_mode=1 done
[02a] sending HOME [0, 0, 0, 0, 0, -30] ...
[02a] after HOME: measured = [-0.26, 1.49, 0.61, 0.26, -0.26, -29.79]
[02a] sending SMALL_MOVE [0, -10, 0, 0, 0, -30] ...
[02a] after SMALL_MOVE: measured = [-0.26, -8.96, 0.61, 0.26, -0.17, -29.79]
[02a] j1 diff from target: 1.04°
[02a] returning to HOME [0, 0, 0, 0, 0, -30] ...
[02a] after return HOME: measured = [-0.26, 0.87, 0.61, 0.26, -0.17, -29.79]
[02a] fresh_mode=0 ...
  [Pi] fresh_mode=0
[02a] fresh_mode=0 done
[02a] closing daemon socket ...
[02a] closed
[02a] all done

j1=-8.96°、目標 -10° に対し誤差 1.04°。サーボの整定精度として正常値。send_angles の最小経路はシロ。

前セッションであれほど沈黙していた原因は、daemon にも低レイヤーの通信にも存在しなかった。バグはもっと上の、人間の書いたロジック層に潜んでいる。

カメラプレビューを常駐させる

PLACE 動作はカメラ起点。だから起動シーケンスのいちばん最初にカメラの確認をしたい。アーム起動より前にカメラが動作しているべきだ、というのは設計判断として明示された:

まずは上カメラから白箱の位置と向きを正確に取得して、赤い線で描画させる

place_modules/03_camera_preview.py。side（iPhone 11 / Iriun）と top（iPhone 14 Pro Max / Continuity Camera）の両方を開き、無限ループで cv2.imshow を回す。途中で消したり付けたりしない。

"""
03_camera_preview.py

検証する挙動:
  side / top の 2 つのカメラを open し、両方のプレビューを無限ループで
  表示し続ける。q キー押下で終了するまで切れない。

含めない処理:
  - daemon 接続 / send_angles / get_angles
  - FK lookup
  - テンプレマッチ
  - 検出オーバーレイ
"""

import time

import cv2

# place_test.py と同じカメラ index
SIDE_CAMERA_INDEX = 1
TOP_CAMERA_INDEX = 2

WARMUP_FRAMES = 30


def open_camera(index, label):
    print(f"[03] opening {label} camera (index={index}) ...")
    cap = cv2.VideoCapture(index)
    if not cap.isOpened():
        print(f"[03] ERROR: {label} camera open failed (index={index})")
        return None
    print(f"[03] {label} opened, warming up ({WARMUP_FRAMES} reads) ...")
    for _ in range(WARMUP_FRAMES):
        cap.read()
    print(f"[03] {label} warm-up done")
    return cap


def main():
    print("[03] start")

    side_cap = open_camera(SIDE_CAMERA_INDEX, "side")
    top_cap = open_camera(TOP_CAMERA_INDEX, "top")

    if side_cap is None or top_cap is None:
        print("[03] ABORT: camera open failed")
        if side_cap is not None:
            side_cap.release()
        if top_cap is not None:
            top_cap.release()
        return

    print("[03] both cameras opened, entering preview loop")
    print("[03] press 'q' on a preview window to quit")

    fps_t0 = time.time()
    frame_count = 0
    side_fail = 0
    top_fail = 0
    fps = 0.0

    try:
        while True:
            ret_s, frame_s = side_cap.read()
            ret_t, frame_t = top_cap.read()

            if not ret_s or frame_s is None:
                side_fail += 1
            if not ret_t or frame_t is None:
                top_fail += 1

            frame_count += 1
            elapsed = time.time() - fps_t0
            if elapsed >= 1.0:
                fps = frame_count / elapsed
                fps_t0 = time.time()
                frame_count = 0

            label = f"FPS={fps:.1f}  side_fail={side_fail}  top_fail={top_fail}  q=quit"

            if ret_s and frame_s is not None:
                disp_s = frame_s.copy()
                cv2.putText(disp_s, label, (20, 40),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
                cv2.putText(disp_s, "SIDE camera", (20, 80),
                            cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
                cv2.imshow("side", disp_s)

            if ret_t and frame_t is not None:
                disp_t = frame_t.copy()
                cv2.putText(disp_t, label, (20, 40),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
                cv2.putText(disp_t, "TOP camera", (20, 80),
                            cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
                cv2.imshow("top", disp_t)

            key = cv2.waitKey(1) & 0xFF
            if key == ord("q"):
                print("[03] 'q' pressed, exiting")
                break
    except KeyboardInterrupt:
        print("[03] Ctrl+C detected")
    finally:
        side_cap.release()
        top_cap.release()
        cv2.destroyAllWindows()
        print(f"[03] released. final side_fail={side_fail} top_fail={top_fail}")
        print("[03] done")


if __name__ == "__main__":
    main()

実行すると 2 枚のウィンドウが開き、FPS 9〜10 で安定。side_fail と top_fail はゼロのまま増えない。カメラは白と確定した。

アームの link を白箱として誤検出する

検出を試す。place_modules/04_top_white_box_detect.py は既存の test_white_box_score.evaluate_frame を呼び、上カメラから 1 フレーム取得して白箱を検出する。

旧テンプレ（~/r2r2r-scripts/templates/ に 12 枚）を読み込む実装。

[04] DETECT: center=(1059,353) score=0.735 angle=0° scale=0.7

スコアは 0.735 と高い。しかし検出位置 (1059, 353) は アームの link 上端 だった。マット ROI = (380, 1701, 31, 979) の中に link が映る座標が含まれており、テンプレ群が「白っぽい四角」として link をマッチさせていた。

LOG165 でも同じ構造の問題が出ていた。「マットの上に白いアームがある」状況では、テンプレマッチが形状の類似だけでアーム筐体を拾う。

旧テンプレは 2026 年 4 月撮影。当時の照明条件・カメラ位置・homography 設定で撮ったもので、現在の環境とは合わない。直すべきはコードではなく、テンプレだった。

白色マスクで自動トリムするテンプレ撮影

cv2.selectROI でマウスドラッグして矩形選択する素朴な実装（05）を最初に書いたが、人間の手元精度ではぴったり囲めない。余白が混入する。

05b_capture_templates_v2.py で改善する。粗選択 → HSV 白色マスクで白箱本体の外接矩形に自動トリム → 保存。

"""
05b_capture_templates_v2.py

検証する挙動:
  cv2.selectROI で粗く囲った後、HSV 白色マスクで白箱本体の輪郭を自動検出し、
  白箱ぴったりに再トリムして保存する。マウス操作の精度に依存せず、テンプレ
  サイズが常に白箱本体の外接矩形に揃う。

含めない処理:
  - アーム制御 / daemon 接続
  - 検出ロジック呼出
"""

import os
import sys
import time
from datetime import datetime
from pathlib import Path

import cv2
import numpy as np

SCRIPTS_ROOT = Path(__file__).resolve().parents[3]
sys.path.insert(0, str(SCRIPTS_ROOT))

import visual_servo_pick as vsp

TOP_CAMERA_INDEX = 2
WARMUP_FRAMES = 30

TEMPLATES_DIR = Path(__file__).resolve().parents[1] / "templates"

# 粗選択時の最小サイズ
MIN_W_ROUGH, MIN_H_ROUGH = 30, 30

# 自動トリム時の白色マスク閾値
# 白箱は明るい白、マットは暗い黒〜茶
WHITE_S_MAX = 60      # 彩度がこれ以下を白とみなす
WHITE_V_MIN = 160     # 明度がこれ以上を白とみなす
WHITE_MIN_AREA_RATIO = 0.10  # ROI 全体の何 % 以上の白色輪郭を白箱とみなすか


def auto_trim_to_white_box(crop_bgr):
    """切り出した crop から白箱本体だけを外接矩形でトリムして返す。
    成功時 trimmed BGR、失敗時 None。
    """
    if crop_bgr is None or crop_bgr.size == 0:
        return None
    h, w = crop_bgr.shape[:2]
    total_area = h * w
    if total_area == 0:
        return None

    hsv = cv2.cvtColor(crop_bgr, cv2.COLOR_BGR2HSV)
    s = hsv[:, :, 1]
    v = hsv[:, :, 2]
    mask = ((s <= WHITE_S_MAX) & (v >= WHITE_V_MIN)).astype(np.uint8) * 255

    # 細かいノイズを除去
    k = np.ones((5, 5), 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)
    area = cv2.contourArea(largest)
    if area < total_area * WHITE_MIN_AREA_RATIO:
        return None

    x, y, bw, bh = cv2.boundingRect(largest)
    if bw < MIN_W_ROUGH or bh < MIN_H_ROUGH:
        return None

    trimmed = crop_bgr[y:y + bh, x:x + bw]
    return trimmed


def main():
    print("[05b] start")
    print(f"[05b] templates dir: {TEMPLATES_DIR}")
    TEMPLATES_DIR.mkdir(parents=True, exist_ok=True)

    print(f"[05b] opening top camera (index={TOP_CAMERA_INDEX}) ...")
    cap = cv2.VideoCapture(TOP_CAMERA_INDEX)
    if not cap.isOpened():
        print(f"[05b] ERROR: camera open failed")
        return
    print(f"[05b] warming up ({WARMUP_FRAMES} reads) ...")
    for _ in range(WARMUP_FRAMES):
        cap.read()
    print("[05b] warm-up done")

    print("[05b] detecting mat range ...")
    try:
        vsp.setup_homography_and_mat_range(cap)
        mat_px_range = vsp._mat_px_range
        print(f"[05b] mat_px_range = {mat_px_range}")
    except Exception as e:
        print(f"[05b] WARN: mat range detection failed: {e}")
        mat_px_range = None

    capture_count = 0
    trim_success = 0
    trim_fallback = 0
    print("[05b] entering preview loop")
    print("[05b] SPACE: capture (rough select, auto trim) / Q: quit")

    try:
        while True:
            ret, frame = cap.read()
            if not ret or frame is None:
                continue

            disp = frame.copy()

            if mat_px_range is not None:
                min_x, max_x, min_y, max_y = mat_px_range
                cv2.rectangle(disp,
                              (min_x, min_y), (max_x, max_y),
                              (0, 255, 255), 2)

            cv2.putText(disp,
                        f"Saved: {capture_count}  trim_ok: {trim_success}  trim_fail: {trim_fallback}",
                        (20, 40),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
            cv2.putText(disp,
                        "TOP camera | SPACE: capture (rough OK)  Q: quit",
                        (20, 80),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)

            cv2.imshow("capture", disp)
            key = cv2.waitKey(1) & 0xFF

            if key in (ord("q"), ord("Q")):
                print("[05b] 'q' pressed, exiting")
                break

            if key == ord(" "):
                print("[05b] selectROI: rough drag around the box, then Enter")
                roi = cv2.selectROI("Select Region (rough)", frame,
                                    showCrosshair=True, fromCenter=False)
                cv2.destroyWindow("Select Region (rough)")
                x, y, w, h = roi
                if w < MIN_W_ROUGH or h < MIN_H_ROUGH:
                    print(f"[05b] WARN: too small ({w}x{h}), skip")
                    continue

                rough_crop = frame[y:y + h, x:x + w]
                trimmed = auto_trim_to_white_box(rough_crop)

                ts = datetime.now().strftime("%Y%m%d_%H%M%S")
                count_str = f"{capture_count + 1:03d}"

                if trimmed is not None:
                    th, tw = trimmed.shape[:2]
                    filename = f"white_box_{ts}_{count_str}.png"
                    path = TEMPLATES_DIR / filename
                    cv2.imwrite(str(path), trimmed)
                    capture_count += 1
                    trim_success += 1
                    print(f"[05b] TRIM OK: {path} rough=({w}x{h}) -> trim=({tw}x{th})")
                else:
                    # 自動トリム失敗 → 粗選択をそのまま保存（fallback）
                    filename = f"white_box_{ts}_{count_str}_rough.png"
                    path = TEMPLATES_DIR / filename
                    cv2.imwrite(str(path), rough_crop)
                    capture_count += 1
                    trim_fallback += 1
                    print(f"[05b] TRIM FAILED, saved rough: {path} ({w}x{h})")
    except KeyboardInterrupt:
        print("[05b] Ctrl+C detected")
    finally:
        cap.release()
        cv2.destroyAllWindows()
        print(f"[05b] total saved: {capture_count} "
              f"(trim_ok={trim_success}, trim_fallback={trim_fallback})")
        print("[05b] done")


if __name__ == "__main__":
    main()

19 枚撮った結果。すべて trim_ok。rough サイズ 214〜324px に対し trim 後は 187〜259px。平均で約 50px ぶんの余白が削られた。

[05b] total saved: 19 (trim_ok=19, trim_fallback=0)

テンプレ角度を自動分類する

19 枚で 06b（検出版）を回したところ、白箱が斜めに置かれているのに angle=0°、score=0.998 と返ってきた。選ばれたテンプレは 19 枚目の最後に撮ったもので、それ自体が斜め向きで撮影されていた。

撮影中に白箱を回転させたつもりはなかったが、見直すと 19 枚の中に 0° と 30° が混在 していた。テンプレを全部 0° で揃えるという前提が、撮影者の手の揺れで崩れていた。

人間の目で 19 枚を仕分けると判断がブレる。cv2.minAreaRect で角度を測定して自動分類する。

"""
07_classify_templates.py

検証する挙動:
  ~/r2r2r-scripts/dynamic_tracking_limits/place/templates/ の各 PNG に対し、
  HSV 白色マスク → 最大輪郭 → minAreaRect で角度を取得し、
  「0° に近い」と「斜め」に分類して一覧出力する。

ファイル移動はしない。出力を見て手動で判断する。
"""

from pathlib import Path

import cv2
import numpy as np

TEMPLATES_DIR = Path(__file__).resolve().parents[1] / "templates"

# 白色マスク閾値（05b と合わせる）
WHITE_S_MAX = 60
WHITE_V_MIN = 160

# 「0° に近い」とみなす角度の絶対値（±この度数以内なら平行扱い）
ANGLE_TOLERANCE_DEG = 8.0


def measure_angle(path):
    """画像から白箱の minAreaRect 角度を取得。0〜45° に正規化して返す。"""
    img = cv2.imread(str(path))
    if img is None:
        return None

    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    s = hsv[:, :, 1]
    v = hsv[:, :, 2]
    mask = ((s <= WHITE_S_MAX) & (v >= WHITE_V_MIN)).astype(np.uint8) * 255

    k = np.ones((5, 5), 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) < 1000:
        return None

    rect = cv2.minAreaRect(largest)
    angle = rect[2]
    # cv2.minAreaRect は -90〜0° を返す。正方形なので 0〜45° に折り畳む
    angle = abs(angle)
    if angle > 45:
        angle = 90 - angle
    return angle


def main():
    print(f"[07] scanning {TEMPLATES_DIR}")
    paths = sorted(TEMPLATES_DIR.glob("*.png"))
    if not paths:
        print("[07] no templates found")
        return

    parallel = []   # 0° に近い
    skewed = []     # 斜め
    failed = []

    print(f"\n{'file':<45} {'angle':>8} {'group'}")
    print("-" * 70)
    for p in paths:
        angle = measure_angle(p)
        if angle is None:
            failed.append(p)
            print(f"{p.name:<45} {'N/A':>8} FAILED")
            continue
        is_parallel = angle <= ANGLE_TOLERANCE_DEG
        group = "parallel" if is_parallel else "skewed"
        print(f"{p.name:<45} {angle:>7.2f}° {group}")
        if is_parallel:
            parallel.append(p)
        else:
            skewed.append(p)

    print("-" * 70)
    print(f"[07] parallel ({len(parallel)}枚, 角度 ≤ {ANGLE_TOLERANCE_DEG}°):")
    for p in parallel:
        print(f"  {p.name}")
    print(f"\n[07] skewed ({len(skewed)}枚, 角度 > {ANGLE_TOLERANCE_DEG}°):")
    for p in skewed:
        print(f"  {p.name}")
    if failed:
        print(f"\n[07] failed ({len(failed)}枚):")
        for p in failed:
            print(f"  {p.name}")
    print(f"\n[07] total: {len(paths)} = parallel {len(parallel)} + skewed {len(skewed)} + failed {len(failed)}")


if __name__ == "__main__":
    main()

実行結果。

file                                             angle group
----------------------------------------------------------------------
white_box_20260520_163735_001.png               26.72° skewed
white_box_20260520_163803_002.png               33.69° skewed
white_box_20260520_163815_003.png               18.88° skewed
white_box_20260520_163829_004.png                0.00° parallel
white_box_20260520_163838_005.png                0.00° parallel
white_box_20260520_163846_006.png                3.42° parallel
white_box_20260520_163853_007.png               33.42° skewed
white_box_20260520_163858_008.png               36.37° skewed
white_box_20260520_163905_009.png                0.00° parallel
white_box_20260520_163912_010.png                0.00° parallel
white_box_20260520_163918_011.png                1.68° parallel
white_box_20260520_163923_012.png                0.00° parallel
white_box_20260520_163929_013.png                4.14° parallel
white_box_20260520_163938_014.png               24.06° skewed
white_box_20260520_163945_015.png               27.76° skewed
white_box_20260520_163951_016.png               33.69° skewed
white_box_20260520_163957_017.png               24.50° skewed
white_box_20260520_164003_018.png               25.68° skewed
white_box_20260520_164010_019.png               31.37° skewed

[07] total: 19 = parallel 8 + skewed 11 + failed 0

parallel 8 枚、skewed 11 枚。skewed の角度分布は 18.88°〜36.37°、中央値 27° 前後。撮影時に「ダイヤ向き（45°のつもり）」が実際は 30° 前後で固定されていた。

正方形の対称性により、テンプレは 0°（画面に対して平行）の 1 種類だけ揃えれば、内部の回転テンプレマッチで 30° / 60° / 90° が生成される。0° と 30° の両方を持つ必要はない。0° 群を残し、skewed 11 枚を templates_skewed/ に物理退避 した。

mv white_box_20260520_163735_001.png ... white_box_20260520_164010_019.png ../templates_skewed/

06b → 06c → 06d → 06e → 06f の段階改善

検出スクリプト 06 系を段階的に改善した。各バージョンの差分が一つだけになるように切る。

06b: 描画サイズを scaled_w / scaled_h に切り替える

04 の初版では描画矩形を base_size = 200 固定で描いていた。検出結果の scaled_w scaled_h を使わないと、矩形サイズが実物と合わない。

overall_best 辞書のキーを grep で確認した:

352:        sw = overall_best["scaled_w"]
353:        sh = overall_best["scaled_h"]

実際にマッチに使われたテンプレの実サイズ（スケール反映済み）が入っている。これを取り出して描画に渡す。

06c: angle 符号を反転する

検出 angle=30° で描画したら、矩形の傾きが白箱と逆方向になった。test_white_box_score.py の L366 を見ると本体描画でも -angle を使っていた。OpenCV の cv2.boxPoints は時計回り正、検出側は反時計回り正。

# 画像座標は Y 軸が下向き。matchTemplate でテンプレを angle 回した結果が
# 一致したということは、frame 上の白箱はテンプレに対して -angle ぶん
# 回って見えている。boxPoints の angle 符号は実機で要調整。
rect = ((cx, cy), (sw, sh), -angle)

ソースに既にコメントで明記されていた。-angle で描画するように修正。

06d: 角度刻みを 0/15/30/45/60/75 に細かくする

test_white_box_score.ROTATION_ANGLES_DEG = [0, 30, 60] がデフォルト。45° 配置の白箱は 30° か 60° に丸められ、矩形がズレる。

ただし test_white_box_score.py 本体は触らない（PICK 側に影響する可能性）。Python のモンキーパッチで上書きする。

import test_white_box_score as twbs
twbs.ROTATION_ANGLES_DEG = [0, 15, 30, 45, 60, 75]

load_templates() を呼ぶ前にこの代入を済ませる。load_templates の内部は for ang in ROTATION_ANGLES_DEG: でこの定数を参照する。

実機テストで 45° 配置の白箱を angle=45° で検出できた。30° も 60° もそれぞれ正しく返る。

06e: スケール削減

SCALES = [0.70, 0.85, 1.00, 1.15, 1.30] を [0.85, 1.00, 1.15] に。これまでの検出履歴で scale=1.0 / 0.85 しか選ばれておらず、0.70 と 1.30 は使われていない。同じくモンキーパッチで twbs.SCALES = [0.85, 1.0, 1.15]。

計算量: 8 (テンプレ) × 6 (角度) × 5 (スケール) = 240 通り → 144 通り（1.67 倍速）。

実機で evaluate_frame が 14.48s に短縮された。ただし副作用として score が 0.770 → 0.636 へ低下、scale が 1.0 → 0.85 に変わった試行があった。

06f: グレースケール化

ボトルネックは evaluate_frame の 14.48s。cv2.matchTemplate の計算量はチャネル数に比例する。BGR (3 チャネル) → GRAY (1 チャネル) で約 3 倍速。

test_white_box_score.py L124 にコメントが残っていた:

# 本番 r2r2r_loop.py:497 はカラー(BGR)のまま matchTemplate を呼ぶ。

L248 で確認:

res = cv2.matchTemplate(roi_bgr, scaled, cv2.TM_CCOEFF_NORMED)

BGR で matchTemplate を呼んでいる。グレースケール化していない。白箱は白〜灰の無彩色で、色情報を捨てても情報損失が実質ゼロ。グレースケールにする余地がある。

evaluate_frame は関数なので定数の上書きでは対応できない。06f 内に同じインターフェイスの evaluate_frame_gray を実装し、roi とテンプレを cv2.cvtColor(..., COLOR_BGR2GRAY) で 1 チャネルに変換してから matchTemplate を呼ぶ。

test_white_box_score.py 本体は触らない（PICK 側に影響しない）。

06f の全コード

"""
06f_top_white_box_detect_v7.py

06e との差分:
  evaluate_frame を 06f 内で自前実装し、テンプレートマッチを
  BGR (3 チャネル) → グレースケール (1 チャネル) で実行する。
  cv2.matchTemplate の計算量がチャネル数に比例するため、約 3 倍速を期待。

  test_white_box_score.py 本体は触らない (PICK 側に影響しない)。
  自前 evaluate_frame_gray は元実装と同じインターフェイス。

  ROI = マット全体 (変更なし)
"""

import sys
import time
from pathlib import Path

import cv2

SCRIPTS_ROOT = Path(__file__).resolve().parents[3]
sys.path.insert(0, str(SCRIPTS_ROOT))

import visual_servo_pick as vsp
import test_white_box_score as twbs

# テンプレ参照先と角度・スケールを上書き
NEW_TEMPLATES_DIR = Path(__file__).resolve().parents[1] / "templates"
twbs.TEMPLATES_DIR = str(NEW_TEMPLATES_DIR)
twbs.ROTATION_ANGLES_DEG = [0, 15, 30, 45, 60, 75]
twbs.SCALES = [0.85, 1.0, 1.15]
print(f"[06f] TEMPLATES_DIR overridden to: {twbs.TEMPLATES_DIR}")
print(f"[06f] ROTATION_ANGLES_DEG overridden to: {twbs.ROTATION_ANGLES_DEG}")
print(f"[06f] SCALES overridden to: {twbs.SCALES}")

from test_white_box_score import load_templates as _load_white_box_templates

SIDE_CAMERA_INDEX = 1
TOP_CAMERA_INDEX = 2
WARMUP_FRAMES = 30
TEMPLATE_SCORE_THRESHOLD = 0.5
DETECT_AFTER_SEC = 0.5


def evaluate_frame_gray(frame, mat_bbox, templates_by_group):
    """グレースケール版 evaluate_frame。

    元の test_white_box_score.evaluate_frame と同じインターフェイス。
    内部で roi と template を BGR → GRAY に変換してから cv2.matchTemplate を呼ぶ。
    cv2.matchTemplate の計算量がチャネル数に比例するため、3 チャネル → 1 チャネルで
    約 3 倍速になる想定。
    """
    min_x, max_x, min_y, max_y = mat_bbox
    roi = frame[min_y:max_y, min_x:max_x]
    if roi.size == 0:
        print("[06f] ERROR: ROI が空です")
        return {"day165": [], "auto": [], "overall_best": None}

    # ★ ここでグレースケール化 ★
    roi_gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
    roi_h, roi_w = roi_gray.shape[:2]

    results = {"day165": [], "auto": []}
    overall_best = None

    for group, tmpls in templates_by_group.items():
        for t in tmpls:
            best_score = -1.0
            best_scale = None
            best_angle = None
            best_size = (0, 0)
            best_cxcy = (0, 0)

            for rot in t["rotations"]:
                angle = rot["angle"]
                template_bgr = rot["bgr"]
                # ★ テンプレもグレースケール化 ★
                template_gray = cv2.cvtColor(template_bgr, cv2.COLOR_BGR2GRAY)
                for scale in twbs.SCALES:
                    sw = int(template_gray.shape[1] * scale)
                    sh = int(template_gray.shape[0] * scale)
                    if sw < 30 or sh < 30:
                        continue
                    if sw >= roi_w or sh >= roi_h:
                        continue
                    scaled = cv2.resize(template_gray, (sw, sh))
                    res = cv2.matchTemplate(roi_gray, scaled, cv2.TM_CCOEFF_NORMED)
                    _, max_val, _, max_loc = cv2.minMaxLoc(res)
                    if max_val > best_score:
                        best_score = max_val
                        best_scale = scale
                        best_angle = angle
                        best_size = (sw, sh)
                        mx, my = max_loc
                        cx = mx + min_x + sw // 2
                        cy = my + min_y + sh // 2
                        best_cxcy = (cx, cy)

            row = {
                "name": t["name"],
                "best_score": best_score,
                "best_scale": best_scale,
                "best_angle": best_angle,
                "scaled_w": best_size[0],
                "scaled_h": best_size[1],
                "cx": best_cxcy[0],
                "cy": best_cxcy[1],
            }
            results[group].append(row)

            if best_score > -1.0:
                cand = dict(row)
                cand["group"] = group
                if overall_best is None or cand["best_score"] > overall_best["best_score"]:
                    overall_best = cand

    results["overall_best"] = overall_best
    return results


def open_camera(index, label):
    print(f"[06f] opening {label} camera (index={index}) ...")
    cap = cv2.VideoCapture(index)
    if not cap.isOpened():
        print(f"[06f] ERROR: {label} camera open failed (index={index})")
        return None
    print(f"[06f] {label} opened, warming up ({WARMUP_FRAMES} reads) ...")
    for _ in range(WARMUP_FRAMES):
        cap.read()
    print(f"[06f] {label} warm-up done")
    return cap


def detect_white_box_once(top_cap, templates_by_group):
    if vsp._mat_px_range is None:
        print("[06f] ERROR: マット bbox 未初期化")
        return None

    for _ in range(2):
        top_cap.read()
    ret, frame = top_cap.read()
    if not ret or frame is None:
        print("[06f] ERROR: top camera read 失敗")
        return None

    mat_bbox = vsp._mat_px_range
    t0 = time.time()
    results = evaluate_frame_gray(frame, mat_bbox, templates_by_group)
    elapsed_eval = time.time() - t0

    best = results.get("overall_best")
    if best is None or best.get("best_score") is None:
        print(f"[06f] DETECT: overall_best=None (eval {elapsed_eval:.2f}s)")
        return None

    score = best["best_score"]
    cx, cy = best["cx"], best["cy"]
    angle = best.get("best_angle") or 0
    scale = best.get("best_scale") or 1.0
    scaled_w = best.get("scaled_w") or 200
    scaled_h = best.get("scaled_h") or 200
    name = best.get("name", "?")

    print(f"[06f] DETECT: center=({cx},{cy}) score={score:.3f} "
          f"angle={angle}° scale={scale} size=({scaled_w}x{scaled_h}) "
          f"template={name} (eval {elapsed_eval:.2f}s)")

    below = score < TEMPLATE_SCORE_THRESHOLD
    if below:
        print(f"[06f] WARN: score < threshold ({TEMPLATE_SCORE_THRESHOLD})")

    return {
        "center": (cx, cy),
        "score": score,
        "angle": angle,
        "scale": scale,
        "scaled_w": scaled_w,
        "scaled_h": scaled_h,
        "name": name,
        "below_threshold": below,
    }


def draw_overlay(frame, detection):
    if detection is None:
        return
    cx, cy = detection["center"]
    score = detection["score"]
    angle = detection["angle"]
    scale = detection["scale"]
    sw = detection["scaled_w"]
    sh = detection["scaled_h"]
    below = detection.get("below_threshold", False)

    draw_angle = -float(angle)
    rect = ((cx, cy), (float(sw), float(sh)), draw_angle)
    box_pts = cv2.boxPoints(rect).astype(int)

    color = (0, 0, 255)
    cv2.polylines(frame, [box_pts], isClosed=True, color=color, thickness=3)
    cv2.circle(frame, (cx, cy), 10, color, -1)

    tag = " [BELOW THRESHOLD]" if below else ""
    cv2.putText(frame,
                f"DETECTED: ({cx},{cy}) angle={angle} (draw={draw_angle:.0f}) "
                f"scale={scale} size=({sw}x{sh}) score={score:.3f}{tag}",
                (20, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2)


def main():
    print("[06f] start")
    t_total = time.time()

    side_cap = open_camera(SIDE_CAMERA_INDEX, "side")
    top_cap = open_camera(TOP_CAMERA_INDEX, "top")

    if side_cap is None or top_cap is None:
        print("[06f] ABORT: camera open failed")
        if side_cap is not None:
            side_cap.release()
        if top_cap is not None:
            top_cap.release()
        return

    print("[06f] setting up homography and mat range ...")
    try:
        vsp.setup_homography_and_mat_range(top_cap)
        print(f"[06f] mat_px_range = {vsp._mat_px_range}")
    except Exception as e:
        print(f"[06f] ERROR: homography セットアップ失敗: {e}")
        side_cap.release()
        top_cap.release()
        return

    print(f"[06f] loading white box templates from {twbs.TEMPLATES_DIR} ...")
    print(f"[06f] (angles {twbs.ROTATION_ANGLES_DEG}, scales {twbs.SCALES})")
    try:
        templates_by_group = _load_white_box_templates()
    except Exception as e:
        print(f"[06f] ERROR: テンプレ読込失敗: {e}")
        side_cap.release()
        top_cap.release()
        return

    print(f"[06f] ready (setup {time.time() - t_total:.2f}s)")
    print(f"[06f] entering preview loop, detection at +{DETECT_AFTER_SEC}s")
    print("[06f] press 'q' to quit")

    t_start = time.time()
    detection = None
    detected = False

    try:
        while True:
            ret_s, frame_s = side_cap.read()
            ret_t, frame_t = top_cap.read()
            elapsed = time.time() - t_start

            if not detected and elapsed >= DETECT_AFTER_SEC:
                detected = True
                print(f"[06f] running detection at t={elapsed:.2f}s")
                detection = detect_white_box_once(top_cap, templates_by_group)
                t_done = time.time() - t_start
                print(f"[06f] detection done at t={t_done:.2f}s")

            label = (f"t={elapsed:.1f}s  "
                     f"detected={'YES' if detected else 'pending'}  q=quit")

            if ret_s and frame_s is not None:
                disp_s = frame_s.copy()
                cv2.putText(disp_s, label, (20, 40),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
                cv2.putText(disp_s, "SIDE camera", (20, 80),
                            cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
                cv2.imshow("side", disp_s)

            if ret_t and frame_t is not None:
                disp_t = frame_t.copy()
                cv2.putText(disp_t, label, (20, 40),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
                cv2.putText(disp_t, "TOP camera", (20, 80),
                            cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
                draw_overlay(disp_t, detection)
                cv2.imshow("top", disp_t)

            key = cv2.waitKey(1) & 0xFF
            if key == ord("q"):
                print("[06f] 'q' pressed, exiting")
                break
    except KeyboardInterrupt:
        print("[06f] Ctrl+C detected")
    finally:
        side_cap.release()
        top_cap.release()
        cv2.destroyAllWindows()
        print("[06f] done")


if __name__ == "__main__":
    main()

速度比較

06d、06e、06f の実機ログから抽出した時間。

[06f] start
[06f] opening side camera (index=1) ...
[06f] side opened, warming up (30 reads) ...
[06f] side warm-up done
[06f] opening top camera (index=2) ...
[06f] top opened, warming up (30 reads) ...
[06f] top warm-up done
[06f] setting up homography and mat range ...
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[376, 1696] Y=[33, 981]
[06f] mat_px_range = (376, 1696, 33, 981)
[06f] loading white box templates from /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates ...
[06f] (angles [0, 15, 30, 45, 60, 75], scales [0.85, 1.0, 1.15])
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] ready (setup 5.68s)
[06f] entering preview loop, detection at +0.5s
[06f] press 'q' to quit
[06f] running detection at t=0.51s
[06f] DETECT: center=(1471,568) score=0.647 angle=60° scale=1.0 size=(212x197) template=white_box_20260520_163929_013.png (eval 5.09s)
[06f] detection done at t=5.72s

evaluate_frame 単体で 14.48s → 5.09s（2.84 倍速）。起動から検出完了まで 29s → 11s（2.6 倍速）。理論値の 3 倍速にほぼ一致。

精度の変化:

scale が 1.0 に戻り、size=212×197 が実物に密着する。グレースケール化は速度だけでなく描画精度にも寄与した。白箱の白さとマットの暗色のコントラストが、形状照合の精度に直接効いている。

何が起きていたのか

Log#20 で 4 回の修正を試みた。fresh_mode のタイミングを 3 通り書き換え、HOME 定数を疑い、wait=0 への切り替えを試した。アームは動かなかった。

理由は単純だ。739 行のモノリシック構造の中で、どこが effect を出しているかを切り分けられなかった。仮説の検証粒度が、コードの構造粒度を超えていた。

Log#21 で起きたことを単位ごとに見ると解像度が違う。

• 01 daemon 接続だけ単独で動かす → 即動いた → daemon はシロ

• 02a send_angles だけ単独で動かす → 即動いた → 制御もシロ

• 03 カメラだけ単独で動かす → 即動いた → カメラもシロ

• 04 検出だけ単独で動かす → 一発で誤検出が見えた → アーム link を拾っていた

• 07 テンプレを単独で分析 → 撮影中に角度が混入していたと判明

解体したスクリプトはどれも 50〜200 行。人間も AI も、一瞥で全体を把握できるサイズ。grep で内部仕様を読み取り、モンキーパッチで定数を上書きする 5 段階の改善は、各ステップが「1 箇所だけの差分」を持つ。前セッションの 4 回の書き換えがただの当て推量だったとすれば、今回の 5 ステップは完全な測定だ。構造を切り分けるだけで、コードの挙動はすべて定量的なデータに変わった。

どう呼ぶべき仕事か

この作業を「リファクタリング」と呼ぶこともできる。古典的なソフトウェアエンジニアリングの語彙では、ソフトウェアアーキテクトや DevEx エンジニア、リサーチエンジニア、テクニカルリードの仕事と隣接している。

これらが扱ってきたのは「人間が扱いやすいコード」だった。

ただし今回の動機は別の場所にあった。AI が判断できる粒度に構造を切る。

Log#20 で AI が 4 回外したのは、AI の判断能力が低かったからではない。AI が把握できる単位より大きな単位で問題が発生していたから、AI は推測するしかなかった。Log#21 で各単体スクリプトに切ったあと、AI は推測しなくてよくなった。grep で読める、time.time() で計測できる、print で観測できる。判断は根拠ベースになる。

これを名指す言葉はまだ世界に存在しない。「AI Workflow Design」か、「Context-Aware Architecture」か。あるいは Pixels to Motors の言葉で言えば、これは Motion UX の一部 だ。アームの物理軌跡を設計するのと同じ手つきで、AI の思考の文脈を設計する。コードの構造そのものが、AI と、人と、機械が破綻なく協働するための「動作の場」になる。

まだ職種名はない。でも、やっている仕事の輪郭だけは見え始めている。AI が推測ではなく測定で動ける構造を、人が意識的に切り分けて作る仕事。

確定した単体スクリプト一覧

Log#21 終了時点で place_modules/ に並んだスクリプト群。

これらが揃ったうえで、次回以降:

• 横カメラからの白箱高さ取得（接触回避用）

• pixel → robot 座標変換の単独検証

• FK lookup の単独検証

• これら全部を place_integrated/ に統合した PLACE 動作スクリプト

そのつど、検証粒度を超えない範囲で進める。

739 行を壊した先で、AI は初めて測定を始めた。問題を解いたわけではない。問題を観測できる場所まで、構造を引き剥がしただけだ。

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