mechArm 270 Pi 動的追従の限界を探る Log#22 | 検証を積み上げる PLACE 動作の足場を組み直した記録

デザイナーがロボットを触ると、見えてくるものがある。LOG213で見えたのはこれ。

💡 Key Takeaway

動かなくなった 739 行の PLACE スクリプトを、検証する挙動が一つだけの単体スクリプトに分解し、6 本を 1 本ずつ実機で通した。設計と実装を別の担い手に分けたことで、グリッパーの幾何を取り違えた設計が実装の手前で止まり、過去に動作が中断した原因が「データの問題」ではなく「設定の問題」だったことも判明した。

動かないコードと、分解という方針

数日前のセッションで、PLACE 動作のスクリプトが動かなくなった。send_angles を送ってもアームが反応しない。4 回修正して、4 回とも失敗した。get_angles の空応答を疑い、fresh_mode を 4 通り書き換え、HOME 定数の誤読を直し——どれも当たらなかった。739 行のモノリシックなスクリプトは、沈黙したまま放置された。

その次のセッションで、方針が変わった。動かないコードを直すのではなく、解体する。739 行を、place_modules/ 配下の 6 つの単体スクリプトに分けた。「検証する挙動が一つだけ」のスクリプトを並べ、1 本ずつ実機で通していく。daemon 接続、send_angles 単発、カメラプレビュー、テンプレ撮影、白箱検出、テンプレ分類。ここまでが解体の完了地点だった。

このセッションは、その続きだ。解体した足場の上に、残りのスクリプトを積んでいく。PLACE 動作の全体像はこうなっている。

【1】daemon 起動
【2】プレビュー表示と白箱認識
【3】HOME 姿勢確認（Phase 0）
【4】APPROACH 移動（Phase 2）
【5】DESCEND（Phase 3）
【6】白箱に赤ブロックが入っているか確認
【7】RELEASE（Phase 4）
【8】RETREAT（Phase 5）

【1】【2】は解体時に完了済み。このセッションで【3】を完成させ、【4】の心臓部まで通す。動かす単位を一つに絞り、1 本ずつ実機で確かめる。それだけを繰り返す。

設計は Claude が担い、実装は Claude Code が担う。書く人と検証する人を分ける。レビューは別の担い手を通す。この役割分担が、このセッションで何度か効くことになる。

起動と HOME 姿勢 ── 08 と 09

最初のスクリプトは、HOME 復帰だ。

PLACE 動作はどのフェーズから始めるにしても、起点が確定していないと再現性がない。セッションをまたぐとアームの姿勢は不明になる。だから「現在の姿勢が何であっても HOME に戻す」スクリプトを最初に置く。

設計上、ひとつ判断があった。タイムアウトを設けるか設けないか。サーボが噛んで物理的に到達できないとき、待機ループは終わらない。普通なら上限時間で ABORT する。だが今回は検証フェーズなので、ABORT させず Ctrl+C を待つ方針にした。動かなければ人間が止める。

このスクリプトを見せる理由は、後続のすべての物理動作スクリプトが、ここで確立した daemon I/O のパターンを踏襲するからだ。08_home_from_anywhere.py の全文を示す。

"""
08_home_from_anywhere.py

検証する挙動: 現在姿勢に関わらず send_angles(HOME) で HOME に戻す。
Phase 0 の HOME 姿勢確認の前段として使う。
"""

import sys
import time
from pathlib import Path

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

from visual_servo_pick import (
    get_daemon_socket,
    close_daemon_socket,
    daemon_send_angles,
    daemon_get_angles,
    daemon_set_fresh_mode,
)

HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
TOL_REACH_DEG = 2.5
LOOP_INTERVAL_SEC = 0.5


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


def main():
    print("[INIT] daemon 接続中...")
    get_daemon_socket()
    daemon_set_fresh_mode(1)
    print("[INIT] fresh_mode=1")

    try:
        pre = daemon_get_angles()
        print(f"[PRE]  現在姿勢: {fmt(pre)}")

        t_start = time.time()
        daemon_send_angles(HOME, speed=SPEED, wait=0)
        print("[SEND] HOME へ移動指令")

        while True:
            time.sleep(LOOP_INTERVAL_SEC)

            measured = None
            for retry in range(3):
                m = daemon_get_angles()
                if m is not None and len(m) == 6:
                    measured = m
                    break
                print(f"[WARN] get_angles 失敗 (試行 {retry+1}/3)")
                time.sleep(0.2)

            if measured is None:
                continue

            elapsed = time.time() - t_start
            max_err = max(abs(measured[i] - HOME[i]) for i in range(6))
            print(f"[WAIT] t={elapsed:.1f}s  max_err={max_err:.2f}°")

            if max_err <= TOL_REACH_DEG:
                break

        print(f"[DONE] HOME 到達 max_err={max_err:.2f}°")

    except KeyboardInterrupt:
        print("[INTERRUPT] Ctrl+C 検出")
    except Exception as e:
        print(f"[ERROR] {e}")
        raise
    finally:
        try:
            post = daemon_get_angles()
            if post is not None:
                print(f"[POST] 最終姿勢: {fmt(post)}")
        except Exception:
            pass
        try:
            daemon_set_fresh_mode(0)
        except Exception:
            pass
        try:
            close_daemon_socket()
        except Exception:
            pass
        print("[EXIT] fresh_mode=0, socket close")


if __name__ == "__main__":
    main()

実機で動かした。アームは j1=92.46°、j4=-95.36° という、HOME から大きく離れた姿勢にあった。実機実行ログの全量を示す。

[INIT] daemon 接続中...
  [Pi] fresh_mode=1
[INIT] fresh_mode=1
[PRE]  現在姿勢: [0.00, 92.46, 0.96, -3.07, -95.36, -29.70]
[SEND] HOME へ移動指令
[WAIT] t=0.6s  max_err=81.12°
[WAIT] t=1.1s  max_err=66.00°
[WAIT] t=1.6s  max_err=51.24°
[WAIT] t=2.1s  max_err=35.94°
[WAIT] t=2.6s  max_err=20.65°
[WAIT] t=3.2s  max_err=5.36°
[WAIT] t=3.7s  max_err=1.23°
[DONE] HOME 到達 max_err=1.23°
[POST] 最終姿勢: [0.00, 1.23, 0.79, -0.52, 1.23, -29.70]
  [Pi] fresh_mode=0
[EXIT] fresh_mode=0, socket close

収束カーブが綺麗だ。81.12° から 1.23° まで、各ループで 15° 前後ずつ単調に減っている。サーボの脱調なし、get_angles の空応答リトライも一度も発火していない。[WARN] が一行も出ていない。動かなくなったあのスクリプトで 4 回失敗した「空応答」「send_angles が動かない」は、この単体スクリプトでは再現しなかった。

次は get_angles 単体の信頼性を確かめる。08 はアームが動いている最中の数回しか get_angles を叩いていない。09 は静止状態で連続 20 回叩いて、毎回有効な 6 要素が返るかを集計する。warm-up は入れず、1 回目から有効性を判定する。fresh_mode も触らない。get_angles の素の挙動だけを見るための最小構成にした。

なぜ最小構成かというと、過去に動作が止まった一因が「空応答」だったからだ。fresh_mode を絡めると、get_angles 単独の挙動が他の要素に汚染される。純度を保ちたい。09_get_angles_test.py の全文を示す。

"""
09_get_angles_test.py

検証する挙動: 静止アームに対し daemon_get_angles() を 20 回連続呼びし、
毎回有効な 6 要素の数値が返るかを集計する。
DAY210 で get_angles の空応答が失敗一因になったため、その挙動を単体で数値化する。
アームは動かさない（send 系は呼ばない）。fresh_mode も触らない。
"""

import sys
import time
from pathlib import Path

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

from visual_servo_pick import (
    get_daemon_socket,
    close_daemon_socket,
    daemon_get_angles,
)

HOME = [0, 0, 0, 0, 0, -30]
NUM_READS = 20
READ_INTERVAL_SEC = 0.3


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


def is_valid(m):
    if m is None:
        return False
    try:
        if len(m) != 6:
            return False
    except TypeError:
        return False
    for x in m:
        if not isinstance(x, (int, float)):
            return False
    return True


def print_summary(total, invalid_indices, max_errs):
    valid = total - len(invalid_indices)
    invalid = len(invalid_indices)
    rate = (valid / total * 100.0) if total > 0 else 0.0
    print("===== 集計 =====")
    print(f"有効: {valid}/{total}  無効: {invalid}/{total}  成功率: {rate:.1f}%")
    if max_errs:
        mn = min(max_errs)
        mx = max(max_errs)
        avg = sum(max_errs) / len(max_errs)
        print(f"max_err 最小: {mn:.2f}°  最大: {mx:.2f}°  平均: {avg:.2f}°")
    else:
        print("有効回なし、誤差計算スキップ")
    if invalid_indices:
        print(f"無効だった回: {invalid_indices}")


def main():
    print("[INIT] daemon 接続中...")
    get_daemon_socket()

    invalid_indices = []
    max_errs = []
    done = 0

    try:
        for n in range(1, NUM_READS + 1):
            m = daemon_get_angles()
            done = n
            if is_valid(m):
                max_err = max(abs(m[i] - HOME[i]) for i in range(6))
                max_errs.append(max_err)
                print(f"[{n:2d}/{NUM_READS}] valid   angles={fmt(m)} max_err={max_err:.2f}°")
            else:
                invalid_indices.append(n)
                print(f"[{n:2d}/{NUM_READS}] INVALID 戻り値={m}")
            time.sleep(READ_INTERVAL_SEC)

        print()
        print_summary(done, invalid_indices, max_errs)

    except KeyboardInterrupt:
        print("[INTERRUPT] Ctrl+C 検出")
        print()
        print_summary(done, invalid_indices, max_errs)
    except Exception as e:
        print(f"[ERROR] {e}")
        raise
    finally:
        try:
            close_daemon_socket()
        except Exception:
            pass
        print("[EXIT] socket close")


if __name__ == "__main__":
    main()

実機で動かした。アームは静止している。実機実行ログの全量を示す。

[INIT] daemon 接続中...
[ 1/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[ 2/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[ 3/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[ 4/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[ 5/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[ 6/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[ 7/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[ 8/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[ 9/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[10/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[11/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[12/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[13/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[14/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[15/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[16/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[17/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[18/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[19/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°
[20/20] valid   angles=[0.00, 1.23, 0.79, -0.26, 0.17, -29.70] max_err=1.23°

===== 集計 =====
有効: 20/20  無効: 0/20  成功率: 100.0%
max_err 最小: 1.23°  最大: 1.23°  平均: 1.23°

成功率 100%。20 回すべてが同じ値で、1 ピクセルもブレない。静止しているのだから、これは健全な結果だ。値が動いたら逆に問題になる。「キャッシュ値を返しているだけでは」という疑いは当然出るが、それは 08 が既に否定している。08 では HOME に向かう途中の get_angles が 81° → 66° → 51° → ... と毎ループ変化していた。動いているときは値が変わり、静止しているときは固定される。08 と 09 が揃って「daemon は実測値を返している」を確定させた。

そして 1 回目から valid だった。warm-up は要らない。今後のスクリプトで get_angles を使うとき、最初の数回を捨てる処理は不要になる。コードが一段シンプルになる。

カメラの足場 ── 10

【3】HOME 姿勢確認の最後のパーツは、カメラ画像の保存だ。

動かなくなったスクリプトには capture_both_cameras という関数があった。各フェーズで side / top の 2 カメラを 1 枚ずつ保存する。10 はその単体検証にあたる。ただし「1 枚ずつ」ではなく「各カメラ 5 枚ずつ」にした。連続 5 枚の取得成否が見えれば、保存できるかだけでなくカメラの安定性も同時にわかる。

カメラ起動直後の数フレームは、露出やホワイトバランスの自動調整が終わっておらず、内部バッファに古いフレームが残っていることもある。iPhone 連携カメラは特にそうなりやすい。だから 30 フレームを空読みして捨てる。10_capture_test.py の全文を示す。

"""
10_capture_test.py

検証する挙動: side / top 両カメラから各 5 枚ずつ画像を取得し、
~/r2r2r-scripts/dynamic_tracking_limits/logs に PNG 保存する。
連続 5 枚の取得成否でカメラの安定性も見る。
place_test.py の capture_both_cameras 相当の単体検証。
アームは動かさない。daemon にも接続しない。
"""

import os
from datetime import datetime

import cv2

TOP_CAMERA_INDEX = 2
SIDE_CAMERA_INDEX = 1
WARMUP_FRAMES = 30
NUM_SHOTS = 5
SAVE_DIR = os.path.expanduser("~/r2r2r-scripts/dynamic_tracking_limits/logs")

_results = {"top": 0, "side": 0}


def capture_camera(index, tag, timestamp):
    TAG = tag.upper()
    prefix = f"[{TAG}]".ljust(7)
    print(f"{prefix}opening camera (index={index}) ...")
    cap = cv2.VideoCapture(index)
    if not cap.isOpened():
        print(f"[ERROR] index={index} のカメラが開けません")
        return 0
    try:
        print(f"{prefix}opened, warming up ({WARMUP_FRAMES} reads) ...")
        for _ in range(WARMUP_FRAMES):
            cap.read()
        print(f"{prefix}warm-up done")
        for n in range(1, NUM_SHOTS + 1):
            ret, frame = cap.read()
            if ret and frame is not None:
                fname = f"place_capture_{timestamp}_{tag}_{n}.png"
                fpath = os.path.join(SAVE_DIR, fname)
                cv2.imwrite(fpath, frame)
                print(f"[SAVE] {fname}")
                _results[tag] += 1
            else:
                print(f"[FAIL] {tag} フレーム {n} 取得失敗")
    finally:
        cap.release()
    return _results[tag]


def print_summary():
    print(
        f"[SUMMARY] top 成功 {_results['top']}/{NUM_SHOTS}  "
        f"side 成功 {_results['side']}/{NUM_SHOTS}"
    )


def main():
    try:
        os.makedirs(SAVE_DIR, exist_ok=True)
    except Exception as e:
        print(f"[ERROR] 保存先ディレクトリ作成失敗: {e}")
        return
    print(f"[INIT] 保存先: {SAVE_DIR}")

    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
    print(f"[INIT] timestamp={timestamp}")

    try:
        capture_camera(TOP_CAMERA_INDEX, "top", timestamp)
        capture_camera(SIDE_CAMERA_INDEX, "side", timestamp)
        print_summary()
    except KeyboardInterrupt:
        print("[INTERRUPT] Ctrl+C 検出")
        print_summary()
    except Exception as e:
        print(f"[ERROR] {e}")
        raise
    finally:
        try:
            cv2.destroyAllWindows()
        except Exception:
            pass
        print("[DONE]")


if __name__ == "__main__":
    main()

実機で動かした。実機実行ログの全量を示す。

[INIT] 保存先: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs
[INIT] timestamp=20260521_150159
[TOP]  opening camera (index=2) ...
[TOP]  opened, warming up (30 reads) ...
[TOP]  warm-up done
[SAVE] place_capture_20260521_150159_top_1.png
[SAVE] place_capture_20260521_150159_top_2.png
[SAVE] place_capture_20260521_150159_top_3.png
[SAVE] place_capture_20260521_150159_top_4.png
[SAVE] place_capture_20260521_150159_top_5.png
[SIDE] opening camera (index=1) ...
[SIDE] opened, warming up (30 reads) ...
[SIDE] warm-up done
[SAVE] place_capture_20260521_150159_side_1.png
[SAVE] place_capture_20260521_150159_side_2.png
[SAVE] place_capture_20260521_150159_side_3.png
[SAVE] place_capture_20260521_150159_side_4.png
[SAVE] place_capture_20260521_150159_side_5.png
[SUMMARY] top 成功 5/5  side 成功 5/5
[DONE]

10 枚すべて保存成功。保存された画像を実際に開くと、top_ の 5 枚は真上からの俯瞰、side_ の 5 枚は真横からの視点だった。ラベルと中身が一致している。カメラ index は side=1 / top=2 で正しい。

この保存画像から、検証の主目的とは別の情報も得られた。横カメラの画像に白箱がはっきり写っている。マット右側に、輪郭くっきりで、アームの白い筐体とは画面上で離れた位置に。以前のセッションで「横カメラの白マスクがアームの白筐体と連結して白箱を抽出できない」という失敗があった。だが今回の配置を見る限り、白箱とアームは画面上で明確に分離している。横カメラを使う段階が来たとき、これは設計の材料になる。

ここまでで【3】HOME 姿勢確認の 3 パーツ ── HOME 復帰、get_angles 検証、画像保存 ── がすべて単体検証済みになった。

FK テーブルを読む ── 11

ここからが【4】APPROACH 移動だ。白箱の真上にアームを持っていくフェーズ。

APPROACH の核心は「白箱の位置 → 関節角度」の変換にある。これを担うのが FK テーブル、fk_scan_motion_based.csv。512,087 点の、関節角度とエンドエフェクタ座標の対応表だ。

FK lookup の前に、まずこのテーブルが実機環境で正しく読めることを確かめる。11 は最近傍検索をしない。読めるか、データが壊れていないか、それだけを検証する。本質的な役割は、統合段階で再利用できる読み込み手順を確立することにある。

ひとつ注意点があった。Claude Code が実装に入る前、プロジェクトの設定ファイルに残っていた古い記述を根拠に「テーブルは 134,456 点」と推測しようとした。これは古い情報だった。現在のテーブルは 512,087 点に統合されている。検証スクリプトの期待値を推測で埋めると、検証ロジックそのものが意味を失う。指示文の冒頭で「古い行数は使うな、512,087 を使え」と明示して、推測を止めた。

11_fk_load_test.py の全文を示す。

"""
11_fk_load_test.py

検証する挙動: fk_scan_motion_based.csv（512,087 点）を独自実装で読み込み、
データの健全性を検証する。lookup（最近傍検索）はしない。
「実機環境で読めるか・データが壊れていないか」だけを検証する。
place_integrated/ で再利用できる読み込み手順を確立するのが本質的な役割。
アームを動かさない。daemon に接続しない。CSV は read-only ("r" モードのみ)。
"""

import csv
import math
import os
import sys

EXPECTED_ROWS = 512087
EXPECTED_COLS = 9
EXPECTED_HEADER = ["ee_x", "ee_y", "ee_z", "j0", "j1", "j2", "j3", "j4", "j5"]
CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")


def scan_csv(path):
    stats = {
        "header": None,
        "data_rows": 0,
        "col_mismatch": 0,
        "non_numeric": 0,
        "empty_cells": 0,
        "nan_inf": 0,
        "duplicates": 0,
        "col_min": [math.inf] * EXPECTED_COLS,
        "col_max": [-math.inf] * EXPECTED_COLS,
        "valid_rows": 0,
    }
    seen = set()

    with open(path, "r", newline="") as f:
        reader = csv.reader(f)
        try:
            stats["header"] = next(reader)
        except StopIteration:
            return stats

        for row in reader:
            stats["data_rows"] += 1

            if len(row) != EXPECTED_COLS:
                stats["col_mismatch"] += 1
                continue

            row_tuple = tuple(row)
            if row_tuple in seen:
                stats["duplicates"] += 1
            else:
                seen.add(row_tuple)

            values = []
            row_ok = True
            for cell in row:
                if cell == "":
                    stats["empty_cells"] += 1
                    row_ok = False
                    values.append(None)
                    continue
                try:
                    v = float(cell)
                except ValueError:
                    stats["non_numeric"] += 1
                    row_ok = False
                    values.append(None)
                    continue
                if math.isnan(v) or math.isinf(v):
                    stats["nan_inf"] += 1
                    row_ok = False
                    values.append(None)
                    continue
                values.append(v)

            if row_ok:
                stats["valid_rows"] += 1
                for i, v in enumerate(values):
                    if v < stats["col_min"][i]:
                        stats["col_min"][i] = v
                    if v > stats["col_max"][i]:
                        stats["col_max"][i] = v

    return stats


def print_structure(stats):
    print()
    print("===== 構造 =====")
    header = stats["header"] or []
    print("ヘッダ: " + ", ".join(header))
    print(f"カラム数: {len(header)}")
    print(f"データ行数: {stats['data_rows']}  (期待値: {EXPECTED_ROWS})")


def print_health(stats):
    print()
    print("===== 健全性 =====")
    print(f"カラム数不一致の行: {stats['col_mismatch']} 件")
    print(f"float 変換不能セル: {stats['non_numeric']} 件")
    print(f"空セル: {stats['empty_cells']} 件")
    print(f"NaN / inf: {stats['nan_inf']} 件")
    print(f"完全重複行: {stats['duplicates']} 件")


def print_ranges(stats):
    sections = (("===== 座標範囲 (Unity m) =====", ("ee_x", "ee_y", "ee_z"), 0),
                ("===== 関節範囲 (度) =====", ("j0", "j1", "j2", "j3", "j4", "j5"), 3))
    for title, names, off in sections:
        print()
        print(title)
        for i, name in enumerate(names):
            lo, hi = stats["col_min"][off + i], stats["col_max"][off + i]
            if lo == math.inf:
                print(f"{name}: 有効データなし")
            else:
                print(f"{name}: min={lo:+.4f}  max={hi:+.4f}")


def determine_issues(stats):
    issues = []
    if stats["data_rows"] != EXPECTED_ROWS:
        issues.append(
            f"データ行数 {stats['data_rows']} が期待値 {EXPECTED_ROWS} と不一致"
        )
    if stats["header"] != EXPECTED_HEADER:
        issues.append(f"ヘッダが期待値と不一致: {stats['header']}")
    if stats["col_mismatch"] > 0:
        issues.append(f"カラム数不一致の行 {stats['col_mismatch']} 件")
    if stats["non_numeric"] > 0:
        issues.append(f"float 変換不能セル {stats['non_numeric']} 件")
    if stats["empty_cells"] > 0:
        issues.append(f"空セル {stats['empty_cells']} 件")
    if stats["nan_inf"] > 0:
        issues.append(f"NaN / inf {stats['nan_inf']} 件")
    return issues


def print_judgment(issues):
    print()
    print("===== 判定 =====")
    if not issues:
        print("[OK] CSV は健全。FK lookup の前提を満たしている")
    else:
        print("[WARN] 異常項目あり:")
        for msg in issues:
            print(f"  - {msg}")


def main():
    print(f"[INIT] CSV: {CSV_PATH}")
    if not os.path.exists(CSV_PATH):
        print(f"[ERROR] ファイルが見つかりません: {CSV_PATH}")
        sys.exit(1)

    print("[LOAD] 読み込み中...")
    try:
        stats = scan_csv(CSV_PATH)
    except KeyboardInterrupt:
        print("[INTERRUPT] Ctrl+C 検出")
        return
    except Exception as e:
        print(f"[ERROR] {e}")
        raise

    total = stats["data_rows"] + (1 if stats["header"] is not None else 0)
    print(f"[LOAD] 完了: {total} 行（ヘッダ 1 + データ {stats['data_rows']}）")

    print_structure(stats)
    print_health(stats)
    print_ranges(stats)
    print_judgment(determine_issues(stats))


if __name__ == "__main__":
    main()

実機で動かした。実機実行ログの全量を示す。

[INIT] CSV: /Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based.csv
[LOAD] 読み込み中...
[LOAD] 完了: 512088 行（ヘッダ 1 + データ 512087）

===== 構造 =====
ヘッダ: ee_x, ee_y, ee_z, j0, j1, j2, j3, j4, j5
カラム数: 9
データ行数: 512087  (期待値: 512087)

===== 健全性 =====
カラム数不一致の行: 0 件
float 変換不能セル: 0 件
空セル: 0 件
NaN / inf: 0 件
完全重複行: 0 件

===== 座標範囲 (Unity m) =====
ee_x: min=-0.3460  max=+0.3252
ee_y: min=-0.1820  max=+0.3239
ee_z: min=-0.1560  max=+0.3525

===== 関節範囲 (度) =====
j0: min=-65.0000  max=+76.0000
j1: min=+20.1700  max=+125.0000
j2: min=-65.0000  max=+51.6300
j3: min=-24.0800  max=+27.9000
j4: min=-95.5700  max=-34.2700
j5: min=-40.0000  max=+0.0000

512,087 行すべてが 9 個の有効な数値。破損ゼロ、欠損ゼロ、重複ゼロ。ここで関節範囲を見ておく。j1 の最大値は 125.00°。この数字が、後で効いてくる。

白箱の位置を実測する ── 12

FK lookup には入力が要る。白箱が「どこにあるか」だ。

最初は、テスト用の座標を想像で組もうとした。マット上に格子を張って、白箱が置かれそうな点を仮に決める。だが白箱は毎回違う場所に置かれる。想像の座標で検証しても、現実の白箱位置と合っている保証がない。

白箱の位置だけ取得するスクリプト作ったら？

この一言で方針が決まった。座標を想像するのではなく、白箱の実位置を取得する手段を 1 本作る。

ここで、既存の白箱検出スクリプトが何を出しているかを確認した。確認して分かったのは、検出スクリプトは白箱の pixel 座標までしか出していないことだった。画像上のどこにあるか、まで。ロボットが使う座標系には変換していない。

座標変換の実装がどこにあるかも確認した。visual_servo_pick.py に pixel_to_unity という関数がある。pixel を受け取り、Unity 座標を返す。戻り値は 2 要素 ── X と Z のみ。Y、つまり高さは含まれない。マットは 2 次元平面なので、画像から高さは決まらないからだ。

これは問題にならない。白箱の高さは 45mm で固定だと確定している。画像から測る必要がない。FK lookup に渡す高さは、APPROACH 用の固定値を別途与えればいい。白箱の位置取得スクリプトは X と Z だけ出せばいい。

12_box_position.py の全文を示す。白箱検出は既存スクリプトを importlib で動的に読み込んで再利用し、その pixel 結果に pixel_to_unity を適用する構造になっている。

"""
12_box_position.py

検証する挙動: 上カメラで白箱を検出し、visual_servo_pick.pixel_to_unity で
白箱の Unity 座標 (unity_x, unity_z) を取得・表示する。
白箱位置を実測する手段を確立するのが役割。後段の FK lookup の入力に使う
座標を、想像ではなく実測で得られるようにする。
アームを動かさない。daemon に接続しない。カメラと座標変換のみ。

06f は数字始まりファイル名のため通常 import 不可。
importlib.import_module で動的に読む。読み込み時の副作用として
06f 内の test_white_box_score への monkey patch
(TEMPLATES_DIR / ROTATION_ANGLES_DEG / SCALES) も適用されるので
12 側で patch を再現する必要はない。
"""

import importlib
import sys
from pathlib import Path

import cv2

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

from visual_servo_pick import setup_homography_and_mat_range, pixel_to_unity

TOP_CAMERA_INDEX = 2
WARMUP_FRAMES = 30


def main():
    print(f"[INIT] 上カメラ open (index={TOP_CAMERA_INDEX}) ...")
    cap = cv2.VideoCapture(TOP_CAMERA_INDEX)
    if not cap.isOpened():
        print(f"[ERROR] index={TOP_CAMERA_INDEX} のカメラが開けません")
        sys.exit(1)

    try:
        print(f"[INIT] warm-up {WARMUP_FRAMES} frames ...")
        for _ in range(WARMUP_FRAMES):
            cap.read()

        print("[INIT] 06f モジュールを動的 import ...")
        m06f = importlib.import_module("06f_top_white_box_detect_v7")

        print("[INIT] homography セットアップ中 ...")
        if not setup_homography_and_mat_range(cap):
            print("[ABORT] マット四隅の検出に失敗。homography をセットできません")
            sys.exit(1)
        print("[INIT] homography セット完了")

        print("[DETECT] 白箱検出中 ...")
        templates_by_group = m06f._load_white_box_templates()
        detection = m06f.detect_white_box_once(cap, templates_by_group)

        print()
        print("===== 白箱位置 =====")
        if detection is None:
            print("白箱未検出（検出スコアがしきい値未満、または白箱が視野にない）")
        else:
            cx, cy = detection["center"]
            score = detection["score"]
            angle = detection["angle"]
            unity_x, unity_z = pixel_to_unity(cx, cy)
            print(f"pixel 中心: ({cx}, {cy})")
            print(f"score: {score:.3f}")
            print(f"angle: {angle}°")
            print(
                f"Unity 座標: (unity_x={unity_x:+.3f}, unity_z={unity_z:+.3f})"
                f"  [単位 m]"
            )
        print()
        print("[DONE]")

    except KeyboardInterrupt:
        print("[INTERRUPT] Ctrl+C 検出")
    except Exception as e:
        print(f"[ERROR] {e}")
        raise
    finally:
        try:
            cap.release()
        except Exception:
            pass
        try:
            cv2.destroyAllWindows()
        except Exception:
            pass


if __name__ == "__main__":
    main()

白箱を 4 か所に動かして、それぞれ実機で動かした。実機実行ログの全量を示す。

[INIT] 上カメラ open (index=2) ...
[INIT] warm-up 30 frames ...
[INIT] 06f モジュールを動的 import ...
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[INIT] homography セットアップ中 ...
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[393, 1645] Y=[48, 957]
[INIT] homography セット完了
[DETECT] 白箱検出中 ...
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] DETECT: center=(1390,594) score=0.811 angle=0° scale=0.85 size=(161x158) template=white_box_20260520_163912_010.png (eval 4.98s)

===== 白箱位置 =====
pixel 中心: (1390, 594)
score: 0.811
angle: 0°
Unity 座標: (unity_x=-0.184, unity_z=+0.182)  [単位 m]

[DONE]

[INIT] 上カメラ open (index=2) ...
[INIT] warm-up 30 frames ...
[INIT] 06f モジュールを動的 import ...
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[INIT] homography セットアップ中 ...
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[393, 1645] Y=[48, 957]
[INIT] homography セット完了
[DETECT] 白箱検出中 ...
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] DETECT: center=(652,716) score=0.803 angle=60° scale=1.0 size=(195x191) template=white_box_20260520_163846_006.png (eval 4.96s)

===== 白箱位置 =====
pixel 中心: (652, 716)
score: 0.803
angle: 60°
Unity 座標: (unity_x=+0.183, unity_z=+0.244)  [単位 m]

[DONE]

[INIT] 上カメラ open (index=2) ...
[INIT] warm-up 30 frames ...
[INIT] 06f モジュールを動的 import ...
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[INIT] homography セットアップ中 ...
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[393, 1644] Y=[49, 958]
[INIT] homography セット完了
[DETECT] 白箱検出中 ...
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] DETECT: center=(584,385) score=0.800 angle=45° scale=1.0 size=(190x186) template=white_box_20260520_163912_010.png (eval 4.96s)

===== 白箱位置 =====
pixel 中心: (584, 385)
score: 0.800
angle: 45°
Unity 座標: (unity_x=+0.217, unity_z=+0.079)  [単位 m]

[DONE]

[INIT] 上カメラ open (index=2) ...
[INIT] warm-up 30 frames ...
[INIT] 06f モジュールを動的 import ...
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[INIT] homography セットアップ中 ...
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[393, 1645] Y=[50, 957]
[INIT] homography セット完了
[DETECT] 白箱検出中 ...
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] DETECT: center=(1383,355) score=0.795 angle=15° scale=1.0 size=(195x191) template=white_box_20260520_163846_006.png (eval 4.94s)

===== 白箱位置 =====
pixel 中心: (1383, 355)
score: 0.795
angle: 15°
Unity 座標: (unity_x=-0.182, unity_z=+0.062)  [単位 m]

[DONE]

4 か所すべてで検出と座標変換が通った。pixel が小さい（画面左寄り）と unity_x が正、pixel が大きい（画面右寄り）と unity_x が負。座標系の定義どおりに動いている。想像で組むはずだった 8 点が、実測 4 点に置き換わった。この 4 点が、次の FK lookup のテスト入力になる。

設計と実装を分けたから、誤りが止まった

ここで、このセッションのもう一つの出来事を書いておく。

DESCEND ── アームを白箱の高さまで降ろすフェーズ ── の設計を詰めていたとき、降下の終端高さを計算しようとした。赤ブロックは 24mm、白箱は 45mm。爪が赤ブロックを保持した状態で、ブロックの下端を白箱の上面から数 mm 上に置く。そのときのグリッパー先端の高さは何 mm か。

計算のために、グリッパーの幾何を図にした。爪が赤ブロックを上下から挟む図を描いた。

箱の中にグリッパー入れちゃダメだよ。

指摘を受けて、図を直した。爪は左右からブロックを挟むのだと描き直した。

なんで爪が上下なん？左右で赤ブロック掴むだろ。

また直した。爪先端と赤ブロック下端の関係を計算しようとした。

グリッパーは横向きだよ？なんで縦向き？

3 回、グリッパーの向きと幾何を取り違えた。実物を見ていない人間が、頭の中だけで幾何を組もうとして、3 回外した。

後でアーム動かして画像で確認してくれよ。

ここで止まった。

止まれたことが、この役割分担の成果だ。グリッパー幾何の取り違えは、設計の段階で起きた。もしこの誤った幾何のまま降下高さの数値を確定し、実装に渡していたら、実機のアームが間違った高さに降りる。爪が箱に当たるか、ブロックが意図しない位置に落ちる。

設計を担う者と、実装を担う者と、最終判断を下す者が分かれていた。だから誤った前提は、コードになる前に露見した。設計の中で閉じた議論なら、間違ったまま進んでいたかもしれない。実装に渡す手前に、別の目があった。

降下高さの確定は保留にした。実機でアームを動かし、画像でグリッパーと赤ブロックの位置関係を確認してから決める。頭の中の幾何ではなく、現物を見る。それまで DESCEND の設計は進めない。

【3】と【4】は、グリッパーの幾何に依存しない。だから先に進められる。間違えた場所だけを止めて、間違えていない場所は動かす。

FK lookup ── 過去の中断の正体 ── 13

【4】の心臓部、FK lookup に入る。

12 で白箱の実測座標が 4 点手に入った。13 はこの 4 点に APPROACH の目標高さ ee_y=0.120 を与え、512,087 点の FK テーブルから 3 次元距離の最近傍点を検索する。最近傍点の関節角度が、APPROACH の指令角度になる。

関節角度の安全制限 ── JOINT_LIMITS ── をどう設定するかが、ここでの判断だった。以前のセッションで、白箱位置の FK lookup が j1=110.74° を返し、JOINT_LIMITS の保守的な上限 100° に弾かれて動作が中断した。あのとき、原因は「テーブルにデータがない」と思われていた。

だが 11 で確認したとおり、FK テーブルの j1 範囲は 20.17° から 125.00° まである。テーブルは j1=110° のデータを持っている。lookup が返した 110° は、テーブル上、正当な値だった。弾いていたのは JOINT_LIMITS の設定のほうだった。

そこで JOINT_LIMITS の j1 上限を 130° に設定した。テーブルが 125° まで持つことを根拠に、その範囲を覆える上限にした。

13_fk_lookup_test.py の全文を示す。512,087 行を 4 回スキャンするのではなく、1 行読むごとに 4 ケース全部の距離を計算してそれぞれの最小を更新する。スキャンは 1 回で済む。

"""
13_fk_lookup_test.py

検証する挙動: 白箱の Unity 座標（12_box_position.py で実測した 4 点）に
APPROACH 高さ ee_y=0.120 を与えた目標座標について、FK テーブル
fk_scan_motion_based.csv から 3D 距離の最近傍点を検索し、最近傍距離・
関節角度・しきい値判定・JOINT_LIMITS 判定を出力する。
FK lookup（最近傍検索）が PLACE の文脈で正しく動くかを検証する。
アームを動かさない。daemon に接続しない。CSV は read-only ("r" モードのみ)。

実装方針: 512,087 行を 4 回スキャンせず、1 行読むごとに 4 ケース全部の
3D 距離を計算してそれぞれの最小を更新する。比較は squared 距離で行い、
最終出力時のみ sqrt して mm に変換する。同距離は最初に見つかった行を採用
（strict less-than でのみ更新）。
"""

import csv
import math
import os
import sys

CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")
EXPECTED_COLS = 9
EE_Y = 0.120
DIST_THRESHOLD_MM = 100.0

BOX_POSITIONS = [
    ("P1", -0.184, 0.182),
    ("P2", +0.183, 0.244),
    ("P3", +0.217, 0.079),
    ("P4", -0.182, 0.062),
]

JOINT_LIMITS = [
    (-160.0, 160.0),
    ( -90.0, 130.0),
    (-170.0,  40.0),
    (-150.0, 150.0),
    (-100.0, 100.0),
    (-180.0, 180.0),
]


def scan_csv_for_nearest(path):
    bests = [{"dist_sq": math.inf, "row": None} for _ in BOX_POSITIONS]
    data_rows = 0
    skipped = 0

    with open(path, "r", newline="") as f:
        reader = csv.reader(f)
        next(reader, None)

        for raw in reader:
            data_rows += 1
            if len(raw) != EXPECTED_COLS:
                skipped += 1
                continue
            vals = [float(c) for c in raw]
            ee_x, ee_y, ee_z = vals[0], vals[1], vals[2]
            for i, (_, tgt_x, tgt_z) in enumerate(BOX_POSITIONS):
                dx = ee_x - tgt_x
                dy = ee_y - EE_Y
                dz = ee_z - tgt_z
                d2 = dx * dx + dy * dy + dz * dz
                if d2 < bests[i]["dist_sq"]:
                    bests[i]["dist_sq"] = d2
                    bests[i]["row"] = vals

    return bests, data_rows, skipped


def check_joint_limits(joints):
    violations = []
    for i, (lo, hi) in enumerate(JOINT_LIMITS):
        v = joints[i]
        if v < lo or v > hi:
            violations.append((i, v, lo, hi))
    return violations


def print_case(label, tgt_x, tgt_z, best):
    print(f"[{label}] 目標 (ee_x={tgt_x:+.3f}, ee_y={EE_Y:+.3f}, ee_z={tgt_z:+.3f})")
    row = best["row"]
    if row is None:
        print("  最近傍: 該当なし")
        return False, False
    ee_x, ee_y, ee_z = row[0], row[1], row[2]
    joints = row[3:9]
    dist_mm = math.sqrt(best["dist_sq"]) * 1000.0
    print(
        f"  最近傍: ee=({ee_x:+.3f}, {ee_y:+.3f}, {ee_z:+.3f})  "
        f"3D距離={dist_mm:.1f}mm"
    )
    print("  関節角度: " + " ".join(f"j{i}={joints[i]:.2f}" for i in range(6)))

    thr_ok = dist_mm <= DIST_THRESHOLD_MM
    if thr_ok:
        print(f"  しきい値判定: OK ({dist_mm:.1f}mm <= {DIST_THRESHOLD_MM:.0f}mm)")
    else:
        print(
            f"  しきい値判定: NG ({dist_mm:.1f}mm > {DIST_THRESHOLD_MM:.0f}mm)"
            f" → 範囲外"
        )

    violations = check_joint_limits(joints)
    if not violations:
        print("  JOINT_LIMITS判定: OK")
    else:
        msgs = [
            f"j{i}={v:.2f} が範囲 [{lo}, {hi}] を超過"
            for i, v, lo, hi in violations
        ]
        print("  JOINT_LIMITS判定: NG (" + ", ".join(msgs) + ")")
    return thr_ok, not violations


def main():
    print(f"[INIT] CSV: {CSV_PATH}")
    if not os.path.exists(CSV_PATH):
        print(f"[ERROR] ファイルが見つかりません: {CSV_PATH}")
        sys.exit(1)

    print("[LOAD] 読み込み中...")
    try:
        bests, data_rows, skipped = scan_csv_for_nearest(CSV_PATH)
    except KeyboardInterrupt:
        print("[INTERRUPT] Ctrl+C 検出")
        return
    except Exception as e:
        print(f"[ERROR] {e}")
        raise

    print(f"[LOAD] 完了: {data_rows} 点")
    print()

    thr_count = 0
    jl_count = 0
    both_count = 0
    total = len(BOX_POSITIONS)
    for i, (label, tx, tz) in enumerate(BOX_POSITIONS):
        thr_ok, jl_ok = print_case(label, tx, tz, bests[i])
        if thr_ok:
            thr_count += 1
        if jl_ok:
            jl_count += 1
        if thr_ok and jl_ok:
            both_count += 1
        print()

    print("===== サマリ =====")
    print(f"しきい値内 (<={int(DIST_THRESHOLD_MM)}mm): {thr_count}/{total}")
    print(f"JOINT_LIMITS OK: {jl_count}/{total}")
    print(f"両方 OK: {both_count}/{total}")
    print(f"カラム数不一致でスキップした行: {skipped} 件")


if __name__ == "__main__":
    main()

実機で動かした。実機実行ログの全量を示す。

[INIT] CSV: /Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based.csv
[LOAD] 読み込み中...
[LOAD] 完了: 512087 点

[P1] 目標 (ee_x=-0.184, ee_y=+0.120, ee_z=+0.182)
  最近傍: ee=(-0.185, +0.080, +0.179)  3D距離=40.5mm
  関節角度: j0=50.00 j1=45.17 j2=31.63 j3=7.90 j4=-70.57 j5=0.00
  しきい値判定: OK (40.5mm <= 100mm)
  JOINT_LIMITS判定: OK

[P2] 目標 (ee_x=+0.183, ee_y=+0.120, ee_z=+0.244)
  最近傍: ee=(+0.184, +0.123, +0.242)  3D距離=4.1mm
  関節角度: j0=-45.00 j1=110.00 j2=-55.00 j3=-15.00 j4=-85.00 j5=-40.00
  しきい値判定: OK (4.1mm <= 100mm)
  JOINT_LIMITS判定: OK

[P3] 目標 (ee_x=+0.217, ee_y=+0.120, ee_z=+0.079)
  最近傍: ee=(+0.253, +0.135, +0.117)  3D距離=54.6mm
  関節角度: j0=26.00 j1=118.74 j2=-31.33 j3=7.60 j4=-76.11 j5=-30.00
  しきい値判定: OK (54.6mm <= 100mm)
  JOINT_LIMITS判定: OK

[P4] 目標 (ee_x=-0.182, ee_y=+0.120, ee_z=+0.062)
  最近傍: ee=(-0.195, +0.130, +0.029)  3D距離=36.5mm
  関節角度: j0=68.00 j1=118.74 j2=-39.33 j3=3.60 j4=-72.11 j5=-30.00
  しきい値判定: OK (36.5mm <= 100mm)
  JOINT_LIMITS判定: OK

===== サマリ =====
しきい値内 (<=100mm): 4/4
JOINT_LIMITS OK: 4/4
両方 OK: 4/4
カラム数不一致でスキップした行: 0 件

4 点すべて、しきい値内かつ JOINT_LIMITS 内。白箱を実測した 4 か所すべてで、APPROACH 姿勢が引ける。

P2・P3・P4 の j1 を見ると、110.00°、118.74°、118.74°。3 点が 100° を超えている。もし JOINT_LIMITS の j1 上限が 100° のままだったら、この 3 点は全部弾かれていた。以前の中断の正体は、これだった。テーブルにデータがなかったのではない。テーブルは持っていた。安全制限の数字が、テーブルの実力より小さく設定されていただけだった。

ひとつ、記録しておくべきことがある。最近傍の 3D 距離が、点によって大きく違う。P2 は 4.1mm でほぼ完璧だが、P1 は 40.5mm、P3 は 54.6mm。FK テーブルは 512,087 点あっても、座標空間で均一に密ではない。白箱の位置によって、引ける最近傍点の精度が変わる。APPROACH は白箱の十分上の高さに向かうフェーズなので、数十 mm の誤差は許容範囲だ。だが接触ギリギリまで降りる DESCEND では、この誤差は無視できない。DESCEND の設計に入るとき、この数字を思い出す必要がある。

何が確かになったか

このセッションで、6 本の単体スクリプトを新規に作り、すべて実機で通した。

【3】HOME 姿勢確認が完成し、【4】APPROACH 移動の心臓部 ── FK lookup ── が通った。動かなくなった 739 行のスクリプトは、まだ動かないまま放置されている。だが、その中にあった処理は、一つずつ別の場所で実機検証された足場に置き換わりつつある。

このセッションで動かした方法は、ひとつしかない。動かす単位を一つに絞り、1 本ずつ実機で確かめる。それを 6 回繰り返した。動かないコードに向き合うとき、いきなり直そうとすると、どこが悪いのか分からないまま修正を重ねることになる。4 回失敗したのは、そのやり方だった。分解して、検証できる単位にして、1 本ずつ通す。地味だが、足場はこうやって組む。

設計と実装と最終判断を、別の担い手に分けた。グリッパーの幾何を 3 回取り違えても、実装に渡る前に止まった。FK テーブルの行数を古い値で推測しかけても、止まった。書く人と検証する人が同じだと、同じ盲点を持つ。別の目を通す構造そのものが、誤りを止める仕組みになる。

次は、12 で実測した座標と 13 で引いた角度を、初めて実機のアームに送る。HOME から白箱の上空へ。APPROACH の実機移動だ。それは物理動作を伴うので、外部レビューを通してから実装に入る。

足場は組めた。ここから先は、アームが実際に動く。

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