mechArm 270 Pi 動的追従の限界を探る Log#24 | 「同じ接続が原因」という見立てが外れた ― 動かない理由を測って2つに分け、APPROACH が実機で通るまで

見立てを疑って測ると、動かない理由は1つではなく、2つに分かれた。

動かない理由は2つに分かれた

前回からの引き継ぎは「同じ接続を使い続けると複数関節の送信が通らない」という見立てだった。これを実機で測り直すと、見立ては外れていた。動かない理由は接続ではなく、2つの別物だった。1つは、FK の最近傍探索が実機では届かない肩の角度を含む姿勢を選んでいたこと。もう1つは、送信が低い確率ですり抜けること。両方を測って分け、それぞれに対処した結果、PLACE 動作【4】APPROACH が実機で到達まで通った。

引き継いだ見立てと、それを疑った理由

前回の引き継ぎでは、切り分けの結果として次の表が残っていた。

ここから「直叩きは毎回 MyCobot を作り直す／daemon は同じ接続を使い続ける。残った差はこの一点」という見立てになっていた。

だがこの切り分けには穴があった。直叩きで送っていた姿勢と、daemon で送っていた姿勢が同じだという保証がない。直叩きの確認では肩の低い姿勢を送り、daemon の確認では白箱の FK 姿勢を送っていた可能性がある。条件を1つに揃えていなかった。だから接続の差に見えていたものが、本当は目標値の差かもしれない。推測のまま直さず、目標値を固定して測り直すことにした。

接続ではなく、目標の関節角度だった

14 の送信部分だけを取り出し、カメラ・FK・確認プロンプトを外して、送る角度を HOME / STAGE1 / STAGE2 の固定値にした。同じ daemon 接続のまま、送信の条件だけを変えて、どの関節が動いたかを見た（diag_stage2_send.py、全文は末尾）。要は、STAGE2 の肩を 110° に置いたまま送ったとき何が起きるかを見た。

STAGE2 = [-15, 110, -65, 15, -85, -30]

結果ははっきり出た。送る目標の肩（2番目の関節）が 0° のときは動き、110° のときは動かない。

開始時の HOME 戻しは、肩を 92° から 0° へ、手首側を -94° から 0° へ動かす大きな複数関節の移動だった。これが同じ接続のまま動いた。「同じ接続では複数関節が通らない」という見立ては、この時点で崩れた。

決め手は C-0 だった。STAGE1 [-15, 0, 0, 0, 0, -30] と比べて違うのは肩だけ（0 → 110）。STAGE1 は動いて C-0 は動かない。変えた変数は接続ではなく、肩の目標値だった。TEST-A では、到達可能なはずの j0 すら動かなかった。範囲外の関節が1つでも含まれると、フレーム全体が無視されて、到達できる関節まで動かなかったということだ。

LOG173 のデータが裏付けている

この肩の上限は、過去に実機で測ってあった。LOG173 の実測関節可動域データに残っている。

110° は実機の到達上限（約 90.7°）を大きく超える。だから送っても届かず、フレームごと無視されていた。前回までの「動かない」は、ここに繋がっていた。FK の元になった Unity 側のスキャンは肩 125° まで点を持っているが、実機はそこへ行けない。テーブルと実機の間に、この差があった。

修正1：FK lookup に到達可能フィルタ

14 の FK 最近傍探索は距離が最も近い点を取るだけで、実機の関節範囲で絞っていなかった。だから実機で届かない肩の姿勢を拾って送っていた。あわせて JOINT_LIMITS の肩の上限が 130° で実測（90.7°）と合っておらず、送信前チェックも素通りしていた。

最近傍探索に、JOINT_LIMITS 範囲外の点を候補から外す処理を足した。

            vals = [float(c) for c in raw]
            j = vals[3:9]
            # DAY216: FKテーブルは実機で届かない姿勢を含む（特にindex-1=肩、実機上限+90.7°）。
            # JOINT_LIMITS 範囲外の点は候補にしない。
            reachable = True
            for k in range(6):
                lo, hi = JOINT_LIMITS[k]
                if j[k] < lo or j[k] > hi:
                    reachable = False
                    break
            if not reachable:
                skipped_unreach += 1
                continue

JOINT_LIMITS の肩は実測に合わせた。

    ( -90.0,  88.0),   # index-1(肩) 実機上限 DAY173実測+90.7°。マージンで88に制限

このフィルタが効いた量は大きい。CSV 512,087 点のうち、肩が 88° を超える行が約 74% を占める。これまで距離だけで最近傍を引いていたので、かなりの確率でこの届かない側を掴んでいた。フィルタ後は、残りの到達可能な側からだけ引く。

動いた。だが、到達可能な単関節すら止まった

FK 修正の後に 14 を実機で動かすと、最近傍は到達可能な姿勢 [33.00, 65.91, -14.14, -24.08, -66.27, -30.00]（肩 65.91、88 以下）が選ばれた。肩 110° を掴む問題は消えた。

ところが今度は STAGE1 [33, 0, 0, 0, 0, -30] ― 到達可能なはずの単関節（j0=33）― が動かず、max_err=33.08 で 19 秒張り付いた。送信が落ちている。標準診断では単関節の送信は動いていた。14 との違いを1つに絞ると、14 は socket を開いてから最初の送信までに、カメラ・homography・512k 点の FK スキャンで長い時間 daemon と無通信になり、かつ送信の直前に角度の読み取りを挟んでいなかった。「長い無通信のあと、読み取りなしで送る」状況が怪しい。これも推測のまま直さず、その状況だけを切り出して測った。

30秒の無通信は原因ではなかった

無通信の影響だけを切り出す追加診断を使った（probe_idle_send.py、全文は末尾）。送信直前に読み取りを入れると接続が温まって idle の影響を消すので、idle 後の送信前には読み取りを入れない設定にした。30秒の無通信のあと、読まずに送る（T2）が 14 と同じ条件になる。

idle は原因ではなかった。T2 は動いた。代わりに別の事実が出た。7回の送信のうち1回だけ落ちた（T2 の HOME 戻し）。pre_read 有りでも落ちている。再接続も必要なかった。落ちた直後の T3・T4 は全部通った。

送信が低確率ですり抜ける。戻り値が OK でも実機が動かないが、同じ目標を送り直すと通る。線を引いておくと、この追加診断は 14 の STAGE1 連続失敗を再現できなかった（T2 が動いてしまった）。だから「すり抜けの正確な仕組み」までは掴めていない。14 のほうが落ちる率が高い可能性（カメラや長い待ちの影響）は残る。全ログは末尾に掲載する。

修正2：到達を確認して、すり抜けたら送り直す

仕組みの完全な特定を待つ必要はない。分かった事実から、再現性に依存しない対処が打てる。送信が効いたかを到達（実測角度）で確認し、タイムアウトしたら同じ目標を送り直す。最大回数を送っても届かなければ、張り付かずに未到達を返す。すり抜けなら再送で回復し、本当に届かないなら未到達を出して止まる。最初に指摘していた「到達待ちにタイムアウトが無く、1回のすり抜けが 19 秒の張り付きになる」も、これで消える。送信の戻り値には頼らない。戻り値が OK でも実機が動かないことがあるためだ（LOG197 で確認済み）。判定は実測のみで行う。

def send_until_reach(target):
    """target を送信し到達を待つ。REACH_TIMEOUT_SEC 内に到達しなければ
    同じ target を再送する。MAX_SEND_ATTEMPTS 回送っても未到達なら False。
    送信すり抜け（戻り値OKでも実機が動かない）対策。判定は measured のみ。"""
    for attempt in range(1, MAX_SEND_ATTEMPTS + 1):
        print(f"[SEND] (試行 {attempt}/{MAX_SEND_ATTEMPTS}) send_angles {fmt(target)}")
        daemon_send_angles(target, speed=SPEED, wait=0)
        t_start = time.time()
        last_err = None
        while time.time() - t_start < REACH_TIMEOUT_SEC:
            time.sleep(LOOP_INTERVAL_SEC)
            measured = None
            for _ in range(3):
                m = daemon_get_angles()
                if m is not None and len(m) == 6:
                    measured = m
                    break
                time.sleep(0.2)
            if measured is None:
                continue
            last_err = max(abs(measured[i] - target[i]) for i in range(6))
            print(f"[WAIT] t={time.time()-t_start:.1f}s  max_err={last_err:.2f}°")
            if last_err <= TOL_REACH_DEG:
                return True
        le = f"{last_err:.2f}" if last_err is not None else "None"
        print(f"[WAIT] タイムアウト {REACH_TIMEOUT_SEC:.0f}s 未到達 max_err={le}°。再送する")
    print(f"[SEND] {MAX_SEND_ATTEMPTS} 回送っても未到達")
    return False

呼び出し側では、STAGE1・STAGE2 は未到達なら中断、HOME 復帰は未到達でも警告を出して続行する（中途姿勢で残っても、次回実行の開始時 HOME ガードが止める）。送信に関わる変更なので、codex と gemini に一度見てもらってから実機にかけた。

実機で APPROACH が通った

修正後の 14 を実機で動かした全出力。STAGE1・STAGE2 とも試行1回で到達し、再送は入らなかった。STAGE2 の max_err は 54 → 38 → 22 → 6.95 → 2.47 と収束し、到達判定 2.5° を切った。HOME 復帰も逆 Stage1・HOME とも1回で到達した。張り付きは無い。

[INIT] daemon 接続中...
  [Pi] fresh_mode=1
[INIT] fresh_mode=1
[PRE]  現在姿勢: [0.26, 1.23, 0.87, 0.00, -0.26, -29.97]
[PRE]  HOME ズレ max_err=1.23°  (許容 5.0°)
[DUMP] 上カメラ open (index=3) ...
[DUMP] warm-up 30 frames ...
[DUMP] 保存しました: /Users/shimazueiichi/Downloads/day215_top_check.png
[DUMP] この画像に白箱が真上から写っているか確認してください。
[DUMP] OK なら Enter、違うなら Ctrl+C で中断し --top で別 index を指定して再実行

[BOX] 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]
[BOX] homography セットアップ中 ...
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[231, 1598] Y=[23, 956]
[BOX] homography セット完了
[BOX] 白箱検出中 ...
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] DETECT: center=(1339,651) score=0.627 angle=0° scale=0.85 size=(158x158) template=white_box_20260520_163923_012.png (eval 5.23s)
[BOX] pixel 中心=(1339, 651)  score=0.627  angle=0°
[BOX] Unity 座標: (unity_x=-0.191, unity_z=+0.205) [単位 m]
[LOOKUP] 目標 (ee_x=-0.191, ee_y=+0.120, ee_z=+0.205)
[LOOKUP] CSV スキャン中: /Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based.csv
[LOOKUP] 完了: 512087 点  カラム不一致 0 件  到達不可で除外 382269 件
[LOOKUP] 最近傍: ee=(-0.216, +0.121, +0.227)  3D距離=33.3mm
[LOOKUP] 最近傍 関節角度: [33.00, 65.91, -14.14, -24.08, -66.27, -29.44]
[LOOKUP] しきい値判定: OK (33.3mm <= 100mm)
[LOOKUP] j5 を -30 に固定 → target_angles=[33.00, 65.91, -14.14, -24.08, -66.27, -30.00]
[LIMITS] OK
[CONFIRM] target_angles=[33.00, 65.91, -14.14, -24.08, -66.27, -30.00]
[CONFIRM] 3D距離=33.3mm  JOINT_LIMITS=OK
[CONFIRM] このターゲットへ移動します。送信するなら y、それ以外で中断:
y
[SEND] (試行 1/3) send_angles [33.00, 0.00, 0.00, 0.00, 0.00, -30.00]
[WAIT] t=0.5s  max_err=20.00°
[WAIT] t=1.1s  max_err=3.12°
[WAIT] t=1.6s  max_err=1.23°
[STAGE1] 到達
[SEND] (試行 1/3) send_angles [33.00, 65.91, -14.14, -24.08, -66.27, -30.00]
[WAIT] t=0.5s  max_err=54.23°
[WAIT] t=1.1s  max_err=38.41°
[WAIT] t=1.6s  max_err=22.51°
[WAIT] t=2.1s  max_err=6.95°
[WAIT] t=2.6s  max_err=2.47°
[STAGE2] 到達
[DONE] APPROACH 到達。グリッパーと白箱の位置関係を横から撮影してください。
[DONE] 撮り終えたら Enter で HOME に戻ります

[SEND] (試行 1/3) send_angles [33.00, 0.00, 0.00, 0.00, 0.00, -30.00]
[WAIT] t=0.5s  max_err=54.40°
[WAIT] t=1.1s  max_err=38.32°
[WAIT] t=1.6s  max_err=22.50°
[WAIT] t=2.1s  max_err=6.59°
[WAIT] t=2.6s  max_err=1.49°
[HOME] 逆Stage1 到達
[SEND] (試行 1/3) send_angles [0.00, 0.00, 0.00, 0.00, 0.00, -30.00]
[WAIT] t=0.5s  max_err=10.19°
[WAIT] t=1.0s  max_err=1.49°
[HOME] 到達
[POST] 最終姿勢: [-0.17, 1.49, 0.79, 0.52, 0.43, -29.97]
  [Pi] fresh_mode=0
[EXIT] fresh_mode=0, socket close

選ばれた最近傍は目標から 3D 距離 33.3mm、肩 65.91°（実機到達範囲内）。到達不可で除外された点は 382,269 件だった。APPROACH はターゲット姿勢へ到達し、HOME 復帰まで通った。検出した対象の実物確認（横からの撮影と、視野内に何があったか）は次回に回す。座標の到達自体は、このログがそのまま証拠になる。

締め

接続が原因だと思っていたが違った。原因は、実機で届かない姿勢を FK が選んでいたことと、低確率で送信がすり抜けることの2つだった。測って分けたことで、APPROACH は実機到達まで通った。次は【5】DESCEND を設計する。

付録：全文

証拠として、切り分け・追加診断・本体の全文と、追加診断の実機全ログを残す。

切り分けに使った diag_stage2_send.py。

"""
diag_stage2_send.py

DAY216 診断: 同じ daemon 接続のまま Stage2（複数関節を一度に大きく）を送ると
一部の関節が動かない問題の切り分け。

14_approach_move.py の【9】送信セクションだけを取り出す。
カメラ・FK・確認プロンプトを外し、送る角度は HOME / STAGE1 / STAGE2 の固定値だけ。
送信の条件だけを変えて、どの関節がどれだけ動いたかを直接観察する。

ルール:
- send_angles() のみ使用（send_coords は使わない）。
- 送る3姿勢はすべて JOINT_LIMITS 内。送信前に毎回チェックする。
- CSV は一切触らない。

3つのテスト:
  TEST-A  HOME から STAGE2 を一発（Stage1 を経由しない冷えた多関節送信）
  TEST-B  STAGE1 到達後に STAGE2（14 と同じ順。既知の失敗の再現）
  TEST-C  STAGE1 後に1関節ずつ STAGE2 へ寄せる（分割すれば通るか）
"""

import os
import sys
import time

sys.path.insert(0, os.path.expanduser("~/r2r2r-scripts"))

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]
STAGE1 = [-15, 0, 0, 0, 0, -30]
STAGE2 = [-15, 110, -65, 15, -85, -30]
SPEED = 20

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),
]

POLL_N = 8
POLL_INTERVAL = 0.4
HOME_TOL_DEG = 5.0


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


def limits_ok(angles):
    for i, (lo, hi) in enumerate(JOINT_LIMITS):
        if angles[i] < lo or angles[i] > hi:
            print(f"[LIMIT-NG] j{i}={angles[i]:.2f} が範囲 [{lo}, {hi}] 外")
            return False
    return True


def get_angles_retry():
    """get_angles は稀に空を返す既知挙動。3回までリトライ。"""
    for _ in range(3):
        m = daemon_get_angles()
        if m is not None and len(m) == 6:
            return m
        time.sleep(0.2)
    return None


def send_and_watch(label, target):
    """target を1回 send し、POLL_N 回角度を観察。始点と終点の差分を出す。"""
    if not limits_ok(target):
        print(f"[{label}] LIMIT 違反のため送信中止")
        return None
    pre = get_angles_retry()
    print(f"[{label}] PRE  = {fmt(pre)}")
    print(f"[{label}] SEND = {fmt(target)}  speed={SPEED}")
    daemon_send_angles(target, speed=SPEED, wait=0)
    last = pre
    for k in range(POLL_N):
        time.sleep(POLL_INTERVAL)
        m = get_angles_retry()
        if m is None:
            print(f"[{label}] poll {k}: get_angles 失敗(空)")
            continue
        max_err = max(abs(m[i] - target[i]) for i in range(6))
        print(f"[{label}] poll {k}: {fmt(m)}  max_err={max_err:.2f}")
        last = m
    if pre is not None and last is not None:
        delta = [last[i] - pre[i] for i in range(6)]
        print(f"[{label}] 移動量(終-始) = {fmt(delta)}")
    print("")
    return last


def ensure_home():
    m = get_angles_retry()
    if m is None:
        print("[HOME] 現在角度取得失敗。中断")
        return False
    err = max(abs(m[i] - HOME[i]) for i in range(6))
    print(f"[HOME] 現在={fmt(m)}  HOMEズレ max_err={err:.2f}")
    if err <= HOME_TOL_DEG:
        return True
    print("[HOME] HOME へ戻す")
    send_and_watch("HOME戻し", HOME)
    m = get_angles_retry()
    err = max(abs(m[i] - HOME[i]) for i in range(6)) if m else 999.0
    print(f"[HOME] 戻し後 max_err={err:.2f}")
    return err <= HOME_TOL_DEG


def pause(msg):
    print("\n" + "=" * 60)
    print(msg)
    print("アーム周囲に物がないこと・いつでも止められる状態を確認して Enter")
    print("=" * 60)
    input()


def main():
    print("[INIT] daemon 接続")
    get_daemon_socket()
    if not daemon_set_fresh_mode(1):
        print("[ABORT] fresh_mode=1 設定失敗")
        return
    print("[INIT] fresh_mode=1")

    try:
        if not ensure_home():
            print("[ABORT] HOME に揃わない。中断")
            return

        # ---- TEST-A: HOME から STAGE2 を直接（Stage1 を経由しない冷えた多関節送信）
        pause("TEST-A : HOME から STAGE2 を一発送信（Stage1 なし）")
        send_and_watch("A", STAGE2)
        send_and_watch("A-戻し", HOME)

        # ---- TEST-B: 14 と同じ順（STAGE1 到達後に STAGE2）= 既知の失敗の再現
        pause("TEST-B : STAGE1 → STAGE2（14 と同じ順。失敗が再現するはず）")
        send_and_watch("B-stage1", STAGE1)
        send_and_watch("B-stage2", STAGE2)
        send_and_watch("B-戻し", HOME)

        # ---- TEST-C: STAGE1 後に1関節ずつ STAGE2 へ寄せる（分割なら動くか）
        pause("TEST-C : STAGE1 後に1関節ずつ STAGE2 へ寄せる")
        send_and_watch("C-stage1", STAGE1)
        partials = [
            [-15, 110,   0,   0,   0, -30],
            [-15, 110, -65,   0,   0, -30],
            [-15, 110, -65,  15,   0, -30],
            [-15, 110, -65,  15, -85, -30],
        ]
        for idx, p in enumerate(partials):
            send_and_watch(f"C-{idx}", p)
        send_and_watch("C-戻し", HOME)

    except KeyboardInterrupt:
        print("[INTERRUPT] Ctrl+C 検出")
    finally:
        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()

追加診断の probe_idle_send.py。

"""
probe_idle_send.py  (DAY216 追加診断 / 改訂版)

14 で STAGE1（到達可能な単関節 j0=33）が動かなかった件。
14 は socket を開いてから STAGE1 送信までに、カメラ・homography・512k FKスキャンで
長時間 daemon と無通信になり、かつ送信の直前に get_angles を打っていない。
「無通信のあと、読み取りを挟まずに送る」状況だけを切り出して測る。

重要: 送信直前に get_angles を打つと接続が温まって idle の影響を消す。
よって idle 後の送信前には読み取りを入れない（pre_read=False）。

T1 WARM           : 直前に1回読む→すぐ送信（基準。動くはず）
T2 IDLE           : 30s 無通信→読まずに送信（14と同条件。ここが落ちるか）
T3 IDLE+READ      : 30s 無通信→get_angles 1回→送信（1回の読みで直るか）
T4 IDLE+RECONNECT : 30s 無通信→close/open/fresh_mode→送信（再接続で直るか）
"""
import os, sys, time
sys.path.insert(0, os.path.expanduser("~/r2r2r-scripts"))
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]
T_J0 = [33, 0, 0, 0, 0, -30]
SPEED = 20
IDLE_SEC = 30
POLL_N = 8
POLL_INTERVAL = 0.5
JOINT_LIMITS = [(-160, 160), (-90, 88), (-170, 40), (-150, 150), (-100, 100), (-180, 180)]


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


def limits_ok(a):
    for i, (lo, hi) in enumerate(JOINT_LIMITS):
        if a[i] < lo or a[i] > hi:
            print(f"[LIMIT-NG] j{i}={a[i]} 範囲外")
            return False
    return True


def get_retry():
    for _ in range(3):
        m = daemon_get_angles()
        if m and len(m) == 6:
            return m
        time.sleep(0.2)
    return None


def send_watch(label, target, pre_read):
    """pre_read=False のときは送信直前に get_angles を打たない（idleの影響を消さないため）。"""
    if not limits_ok(target):
        return None
    pre = get_retry() if pre_read else None
    print(f"[{label}] PRE  = {fmt(pre)} (pre_read={pre_read})")
    print(f"[{label}] SEND = {fmt(target)} speed={SPEED}")
    daemon_send_angles(target, speed=SPEED, wait=0)
    last = None
    for k in range(POLL_N):
        time.sleep(POLL_INTERVAL)
        m = get_retry()
        if m is None:
            print(f"[{label}] poll {k}: 空")
            continue
        me = max(abs(m[i] - target[i]) for i in range(6))
        print(f"[{label}] poll {k}: {fmt(m)} max_err={me:.2f}")
        last = m
    if last:
        print(f"[{label}] 終了姿勢 = {fmt(last)}  j0誤差={abs(last[0]-target[0]):.2f}")
    print("")
    return last


def goto_home():
    """次のテスト前に HOME に戻す。戻しは warm 状態で行う(pre_read=True)。"""
    send_watch("HOME戻し", HOME, pre_read=True)


def ensure_home_start():
    m = get_retry()
    if not m:
        print("[HOME] 取得失敗")
        return False
    if max(abs(m[i] - HOME[i]) for i in range(6)) <= 5.0:
        print(f"[HOME] OK {fmt(m)}")
        return True
    print("[HOME] 戻す")
    goto_home()
    m = get_retry()
    return bool(m) and max(abs(m[i] - HOME[i]) for i in range(6)) <= 5.0


def main():
    print("[INIT] 接続")
    get_daemon_socket()
    daemon_set_fresh_mode(1)
    try:
        if not ensure_home_start():
            print("[ABORT] HOME 揃わず")
            return

        print("\n=== T1 WARM: 直前に1回読む→すぐ送信（基準）===")
        get_retry()
        send_watch("T1", T_J0, pre_read=False)
        goto_home()

        print(f"\n=== T2 IDLE: {IDLE_SEC}s 無通信→読まずに送信（14と同条件）===")
        get_retry()
        print(f"[T2] {IDLE_SEC}s 無通信 ...")
        time.sleep(IDLE_SEC)
        send_watch("T2", T_J0, pre_read=False)
        goto_home()

        print(f"\n=== T3 IDLE+READ: {IDLE_SEC}s 無通信→get_angles 1回→送信 ===")
        get_retry()
        print(f"[T3] {IDLE_SEC}s 無通信 ...")
        time.sleep(IDLE_SEC)
        warm = get_retry()
        print(f"[T3] 無通信後の warm 読み = {fmt(warm)}")
        send_watch("T3", T_J0, pre_read=False)
        goto_home()

        print(f"\n=== T4 IDLE+RECONNECT: {IDLE_SEC}s 無通信→再接続→送信 ===")
        get_retry()
        print(f"[T4] {IDLE_SEC}s 無通信 ...")
        time.sleep(IDLE_SEC)
        print("[T4] 再接続 close→open→fresh_mode=1")
        try:
            close_daemon_socket()
            get_daemon_socket()
            daemon_set_fresh_mode(1)
        except Exception as e:
            print(f"[T4] 再接続でエラー: {e} （daemon が単一接続しか受けない可能性）")
        send_watch("T4", T_J0, pre_read=False)
        goto_home()
    finally:
        try:
            daemon_set_fresh_mode(0)
        except Exception:
            pass
        try:
            close_daemon_socket()
        except Exception:
            pass
        print("[EXIT] fresh_mode=0, close")


if __name__ == "__main__":
    main()

probe_idle_send.py の実機全出力。

[INIT] 接続
  [Pi] fresh_mode=1
[HOME] OK [-0.08, 1.23, 0.79, 0.00, -0.26, -29.97]

=== T1 WARM: 直前に1回読む→すぐ送信（基準）===
[T1] PRE  = None (pre_read=False)
[T1] SEND = [33.00, 0.00, 0.00, 0.00, 0.00, -30.00] speed=20
[T1] poll 0: [12.91, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=20.09
[T1] poll 1: [29.44, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=3.56
[T1] poll 2: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T1] poll 3: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T1] poll 4: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T1] poll 5: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T1] poll 6: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T1] poll 7: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T1] 終了姿勢 = [33.31, 1.23, 0.79, 0.00, -0.26, -29.97]  j0誤差=0.31

[HOME戻し] PRE  = [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] (pre_read=True)
[HOME戻し] SEND = [0.00, 0.00, 0.00, 0.00, 0.00, -30.00] speed=20
[HOME戻し] poll 0: [19.68, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=19.68
[HOME戻し] poll 1: [3.95, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=3.95
[HOME戻し] poll 2: [0.43, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 3: [0.35, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 4: [0.35, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 5: [0.35, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 6: [0.35, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 7: [0.35, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] 終了姿勢 = [0.35, 1.23, 0.79, 0.00, -0.26, -29.97]  j0誤差=0.35


=== T2 IDLE: 30s 無通信→読まずに送信（14と同条件）===
[T2] 30s 無通信 ...
[T2] PRE  = None (pre_read=False)
[T2] SEND = [33.00, 0.00, 0.00, 0.00, 0.00, -30.00] speed=20
[T2] poll 0: [23.20, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=9.80
[T2] poll 1: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T2] poll 2: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T2] poll 3: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T2] poll 4: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T2] poll 5: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T2] poll 6: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T2] poll 7: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T2] 終了姿勢 = [33.31, 1.23, 0.79, 0.00, -0.26, -29.97]  j0誤差=0.31

[HOME戻し] PRE  = [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] (pre_read=True)
[HOME戻し] SEND = [0.00, 0.00, 0.00, 0.00, 0.00, -30.00] speed=20
[HOME戻し] poll 0: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=33.31
[HOME戻し] poll 1: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=33.31
[HOME戻し] poll 2: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=33.31
[HOME戻し] poll 3: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=33.31
[HOME戻し] poll 4: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=33.31
[HOME戻し] poll 5: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=33.31
[HOME戻し] poll 6: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=33.31
[HOME戻し] poll 7: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=33.31
[HOME戻し] 終了姿勢 = [33.31, 1.23, 0.79, 0.00, -0.26, -29.97]  j0誤差=33.31


=== T3 IDLE+READ: 30s 無通信→get_angles 1回→送信 ===
[T3] 30s 無通信 ...
[T3] 無通信後の warm 読み = [33.31, 1.23, 0.79, 0.00, -0.26, -29.97]
[T3] PRE  = None (pre_read=False)
[T3] SEND = [33.00, 0.00, 0.00, 0.00, 0.00, -30.00] speed=20
[T3] poll 0: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T3] poll 1: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T3] poll 2: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T3] poll 3: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T3] poll 4: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T3] poll 5: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T3] poll 6: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T3] poll 7: [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T3] 終了姿勢 = [33.31, 1.23, 0.79, 0.00, -0.26, -29.97]  j0誤差=0.31

[HOME戻し] PRE  = [33.31, 1.23, 0.79, 0.00, -0.26, -29.97] (pre_read=True)
[HOME戻し] SEND = [0.00, 0.00, 0.00, 0.00, 0.00, -30.00] speed=20
[HOME戻し] poll 0: [19.07, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=19.07
[HOME戻し] poll 1: [2.72, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=2.72
[HOME戻し] poll 2: [0.35, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 3: [0.35, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 4: [0.26, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 5: [0.26, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 6: [0.26, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 7: [0.26, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] 終了姿勢 = [0.26, 1.23, 0.79, 0.00, -0.26, -29.97]  j0誤差=0.26


=== T4 IDLE+RECONNECT: 30s 無通信→再接続→送信 ===
[T4] 30s 無通信 ...
[T4] 再接続 close→open→fresh_mode=1
  [Pi] fresh_mode=1
[T4] PRE  = None (pre_read=False)
[T4] SEND = [33.00, 0.00, 0.00, 0.00, 0.00, -30.00] speed=20
[T4] poll 0: [13.09, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=19.91
[T4] poll 1: [29.17, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=3.83
[T4] poll 2: [33.22, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T4] poll 3: [33.22, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T4] poll 4: [33.22, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T4] poll 5: [33.22, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T4] poll 6: [33.22, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T4] poll 7: [33.22, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[T4] 終了姿勢 = [33.22, 1.23, 0.79, 0.00, -0.26, -29.97]  j0誤差=0.22

[HOME戻し] PRE  = [33.22, 1.23, 0.79, 0.00, -0.26, -29.97] (pre_read=True)
[HOME戻し] SEND = [0.00, 0.00, 0.00, 0.00, 0.00, -30.00] speed=20
[HOME戻し] poll 0: [19.42, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=19.42
[HOME戻し] poll 1: [3.86, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=3.86
[HOME戻し] poll 2: [0.43, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 3: [0.35, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 4: [0.35, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 5: [0.35, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 6: [0.35, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] poll 7: [0.35, 1.23, 0.79, 0.00, -0.26, -29.97] max_err=1.23
[HOME戻し] 終了姿勢 = [0.35, 1.23, 0.79, 0.00, -0.26, -29.97]  j0誤差=0.35

  [Pi] fresh_mode=0
[EXIT] fresh_mode=0, close

修正後の 14_approach_move.py 全文。

"""
14_approach_move.py

PLACE動作【4】APPROACH の実機移動。
HOME から白箱上空（APPROACH 高さ ee_y=0.120）まで実機のアームを動かす。
Mac から実行し、SSH 先の Pi daemon を叩く。

スコープ外（このスクリプトでやらない）:
- DESCEND（降下）
- グリッパー開閉
- 白箱内のブロック確認

14 は「白箱上空に到達して静止し、HOME に戻る」までで完結する。

絶対ルール:
- send_coords() 使用禁止。send_angles() のみ。
- fk_scan_motion_based.csv は read-only ("r" モードのみ)。
"""

import argparse
import csv
import importlib
import math
import os
import sys
import time
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 (
    get_daemon_socket,
    close_daemon_socket,
    daemon_send_angles,
    daemon_get_angles,
    daemon_set_fresh_mode,
    setup_homography_and_mat_range,
    pixel_to_unity,
)

HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
EE_Y = 0.120                 # APPROACH 目標高さ（Unity座標 m）
EE_Y_MIN_TOL = 0.030         # 最近傍点が目標高さから下に外れてよい上限（m）。下振れのみ制限
DIST_THRESHOLD_MM = 100.0    # FK lookup 3Dユークリッド距離しきい値
TOL_REACH_DEG = 2.5          # 到達判定（6軸 max_err、度）
LOOP_INTERVAL_SEC = 0.5      # 到達待ちループ間隔
REACH_TIMEOUT_SEC = 8.0      # 1送信あたりの到達待ち上限。超えたら同じ目標を再送
MAX_SEND_ATTEMPTS = 3        # 到達しなければ同じ目標を再送する最大回数（送信すり抜け対策）
J5_FIX = -30                 # j5 はHOME固定。lookup結果を上書き
HOME_PRE_TOL_DEG = 5.0       # 開始時HOMEズレ許容
CAMERA_INDEX_TOP = 3         # 上カメラ既定index。--top で上書き可
EXPECTED_COLS = 9            # CSVカラム数（13と同一）
WARMUP_FRAMES = 30
DUMP_PATH = os.path.expanduser("~/Downloads/day215_top_check.png")
CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")

JOINT_LIMITS = [
    (-160.0, 160.0),
    ( -90.0,  88.0),   # index-1(肩) 実機上限 DAY173実測+90.7°。マージンで88に制限
    (-170.0,  40.0),
    (-150.0, 150.0),
    (-100.0, 100.0),
    (-180.0, 180.0),
]


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


def scan_csv_for_nearest(path, tx, ty, tz):
    best_dist_sq = math.inf
    best_row = None
    data_rows = 0
    skipped = 0

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

        skipped_unreach = 0
        for raw in reader:
            data_rows += 1
            if len(raw) != EXPECTED_COLS:
                skipped += 1
                continue
            vals = [float(c) for c in raw]
            j = vals[3:9]
            # DAY216: FKテーブルは実機で届かない姿勢を含む（特にindex-1=肩、実機上限+90.7°）。
            # JOINT_LIMITS 範囲外の点は候補にしない。
            reachable = True
            for k in range(6):
                lo, hi = JOINT_LIMITS[k]
                if j[k] < lo or j[k] > hi:
                    reachable = False
                    break
            if not reachable:
                skipped_unreach += 1
                continue
            ee_x, ee_y, ee_z = vals[0], vals[1], vals[2]
            dx = ee_x - tx
            dy = ee_y - ty
            dz = ee_z - tz
            d2 = dx * dx + dy * dy + dz * dz
            if d2 < best_dist_sq:
                best_dist_sq = d2
                best_row = vals

    return best_row, best_dist_sq, data_rows, skipped, skipped_unreach


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 send_until_reach(target):
    """target を送信し到達を待つ。REACH_TIMEOUT_SEC 内に到達しなければ
    同じ target を再送する。MAX_SEND_ATTEMPTS 回送っても未到達なら False。
    送信すり抜け（戻り値OKでも実機が動かない）対策。判定は measured のみ。"""
    for attempt in range(1, MAX_SEND_ATTEMPTS + 1):
        print(f"[SEND] (試行 {attempt}/{MAX_SEND_ATTEMPTS}) send_angles {fmt(target)}")
        daemon_send_angles(target, speed=SPEED, wait=0)
        t_start = time.time()
        last_err = None
        while time.time() - t_start < REACH_TIMEOUT_SEC:
            time.sleep(LOOP_INTERVAL_SEC)
            measured = None
            for _ in range(3):
                m = daemon_get_angles()
                if m is not None and len(m) == 6:
                    measured = m
                    break
                time.sleep(0.2)
            if measured is None:
                continue
            last_err = max(abs(measured[i] - target[i]) for i in range(6))
            print(f"[WAIT] t={time.time()-t_start:.1f}s  max_err={last_err:.2f}°")
            if last_err <= TOL_REACH_DEG:
                return True
        le = f"{last_err:.2f}" if last_err is not None else "None"
        print(f"[WAIT] タイムアウト {REACH_TIMEOUT_SEC:.0f}s 未到達 max_err={le}°。再送する")
    print(f"[SEND] {MAX_SEND_ATTEMPTS} 回送っても未到達")
    return False


def main():
    parser = argparse.ArgumentParser(description="PLACE動作【4】APPROACH 実機移動")
    parser.add_argument(
        "--top",
        type=int,
        default=CAMERA_INDEX_TOP,
        help=f"上カメラ index（デフォルト {CAMERA_INDEX_TOP}）",
    )
    args = parser.parse_args()
    top_index = args.top

    cap = None
    try:
        print("[INIT] daemon 接続中...")
        get_daemon_socket()
        if not daemon_set_fresh_mode(1):
            print("[ABORT] fresh_mode の設定に失敗しました")
            return
        print("[INIT] fresh_mode=1")

        # 2. PRE / HOME ガード
        pre = daemon_get_angles()
        print(f"[PRE]  現在姿勢: {fmt(pre)}")
        if pre is None or len(pre) != 6:
            print("[ABORT] 現在姿勢の取得に失敗しました")
            return
        pre_max_err = max(abs(pre[i] - HOME[i]) for i in range(6))
        print(f"[PRE]  HOME ズレ max_err={pre_max_err:.2f}°  (許容 {HOME_PRE_TOL_DEG}°)")
        if pre_max_err > HOME_PRE_TOL_DEG:
            print(
                "[ABORT] HOME から離れすぎています。"
                "先に 08_home_from_anywhere.py を実行して HOME に戻してください"
            )
            return

        # 3. 上カメラ dump
        print(f"[DUMP] 上カメラ open (index={top_index}) ...")
        cap = cv2.VideoCapture(top_index)
        if not cap.isOpened():
            print(f"[ABORT] index={top_index} のカメラが開けません")
            return

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

        ret, frame = cap.read()
        if not ret or frame is None:
            print("[ABORT] 上カメラから 1 フレーム読めません")
            return

        cv2.imwrite(DUMP_PATH, frame)
        print(f"[DUMP] 保存しました: {DUMP_PATH}")
        print("[DUMP] この画像に白箱が真上から写っているか確認してください。")
        print(
            "[DUMP] OK なら Enter、違うなら Ctrl+C で中断し "
            "--top で別 index を指定して再実行"
        )
        input()

        # 4. 白箱検出（12 と同じ流儀: cap を使い回す）
        print("[BOX] 06f モジュールを動的 import ...")
        m06f = importlib.import_module("06f_top_white_box_detect_v7")

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

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

        if detection is None:
            print(
                "[ABORT] 白箱未検出"
                "（検出スコアがしきい値未満、または白箱が視野にない）"
            )
            return

        if detection.get("below_threshold"):
            thr = getattr(m06f, "TEMPLATE_SCORE_THRESHOLD", None)
            thr_str = f"{thr:.2f}" if isinstance(thr, (int, float)) else "?"
            print(
                f"[ABORT] 白箱検出スコアがしきい値未満"
                f"（score={detection.get('score'):.3f} < {thr_str}）。誤検出のため中断"
            )
            return

        cx, cy = detection["center"]
        score = detection["score"]
        angle = detection["angle"]
        try:
            unity_x, unity_z = pixel_to_unity(cx, cy)
        except Exception as e:
            print(f"[ABORT] pixel_to_unity に失敗: {e}")
            return
        print(f"[BOX] pixel 中心=({cx}, {cy})  score={score:.3f}  angle={angle}°")
        print(
            f"[BOX] Unity 座標: "
            f"(unity_x={unity_x:+.3f}, unity_z={unity_z:+.3f}) [単位 m]"
        )

        # 5. FK lookup
        print(
            f"[LOOKUP] 目標 (ee_x={unity_x:+.3f}, "
            f"ee_y={EE_Y:+.3f}, ee_z={unity_z:+.3f})"
        )
        if not os.path.exists(CSV_PATH):
            print(f"[ABORT] CSV が見つかりません: {CSV_PATH}")
            return

        print(f"[LOOKUP] CSV スキャン中: {CSV_PATH}")
        best_row, best_dist_sq, data_rows, skipped, skipped_unreach = scan_csv_for_nearest(
            CSV_PATH, unity_x, EE_Y, unity_z
        )
        print(
            f"[LOOKUP] 完了: {data_rows} 点  "
            f"カラム不一致 {skipped} 件  到達不可で除外 {skipped_unreach} 件"
        )

        if best_row is None:
            print(
                "[ABORT] 到達可能なFK姿勢が見つかりません"
                "（JOINT_LIMITS 内の有効姿勢が CSV に1件もありません）。白箱を別の位置へ動かして再実行してください"
            )
            return

        ee_x_n, ee_y_n, ee_z_n = best_row[0], best_row[1], best_row[2]
        joints = list(best_row[3:9])
        dist_mm = math.sqrt(best_dist_sq) * 1000.0
        print(
            f"[LOOKUP] 最近傍: ee=({ee_x_n:+.3f}, {ee_y_n:+.3f}, {ee_z_n:+.3f})  "
            f"3D距離={dist_mm:.1f}mm"
        )
        print(f"[LOOKUP] 最近傍 関節角度: {fmt(joints)}")

        if dist_mm > DIST_THRESHOLD_MM:
            print(
                f"[ABORT] しきい値超過: "
                f"{dist_mm:.1f}mm > {DIST_THRESHOLD_MM:.0f}mm"
            )
            return
        print(
            f"[LOOKUP] しきい値判定: OK "
            f"({dist_mm:.1f}mm <= {DIST_THRESHOLD_MM:.0f}mm)"
        )

        if ee_y_n < EE_Y - EE_Y_MIN_TOL:
            print(
                f"[ABORT] 最近傍の高さが低すぎます: "
                f"ee_y={ee_y_n:+.3f} < {EE_Y - EE_Y_MIN_TOL:+.3f} "
                f"(目標 {EE_Y:+.3f} の下振れ {EE_Y_MIN_TOL*1000:.0f}mm 超)"
            )
            return

        # 6. j5 固定
        target_angles = list(joints)
        target_angles[5] = J5_FIX
        print(f"[LOOKUP] j5 を {J5_FIX} に固定 → target_angles={fmt(target_angles)}")

        # 7. JOINT_LIMITS チェック
        violations = check_joint_limits(target_angles)
        if violations:
            for i, v, lo, hi in violations:
                print(f"[LIMITS] NG: j{i}={v:.2f} が範囲 [{lo}, {hi}] を超過")
            print("[ABORT] JOINT_LIMITS 違反のため送信しません")
            return
        print("[LIMITS] OK")

        # 8. 確認ポーズ
        print(f"[CONFIRM] target_angles={fmt(target_angles)}")
        print(f"[CONFIRM] 3D距離={dist_mm:.1f}mm  JOINT_LIMITS=OK")
        print("[CONFIRM] このターゲットへ移動します。送信するなら y、それ以外で中断:")
        ans = input().strip().lower()
        if ans != "y":
            print("[ABORT] ユーザーが y を入力しなかったため中断")
            return

        # 9. 移動（j0 先行2段階）。各送信は send_until_reach で到達確認＋再送
        stage1 = [target_angles[0], 0, 0, 0, 0, J5_FIX]
        if not send_until_reach(stage1):
            print("[ABORT] STAGE1 未到達")
            return
        print("[STAGE1] 到達")

        if not send_until_reach(target_angles):
            print("[ABORT] STAGE2 未到達")
            return
        print("[STAGE2] 到達")

        # 11. 到達後・静止
        print(
            "[DONE] APPROACH 到達。"
            "グリッパーと白箱の位置関係を横から撮影してください。"
        )
        print("[DONE] 撮り終えたら Enter で HOME に戻ります")
        input()

        # 12. HOME 復帰（逆順2段階）。失敗しても復帰は続行し WARN
        back_stage1 = [target_angles[0], 0, 0, 0, 0, J5_FIX]
        if send_until_reach(back_stage1):
            print("[HOME] 逆Stage1 到達")
        else:
            print("[WARN] 逆Stage1 未到達。HOME 復帰を続行")

        if send_until_reach(HOME):
            print("[HOME] 到達")
        else:
            print("[WARN] HOME 未到達")

    except KeyboardInterrupt:
        print("[INTERRUPT] Ctrl+C 検出")
    except Exception as e:
        print(f"[ERROR] {e}")
        raise
    finally:
        try:
            if cap is not None:
                cap.release()
        except Exception:
            pass
        try:
            cv2.destroyAllWindows()
        except Exception:
            pass
        try:
            post = daemon_get_angles()
            if post is not None:
                print(f"[POST] 最終姿勢: {fmt(post)}")
        except Exception:
            pass
        fresh0_ok = None
        try:
            fresh0_ok = daemon_set_fresh_mode(0)
        except Exception:
            fresh0_ok = None
        try:
            close_daemon_socket()
        except Exception:
            pass
        if fresh0_ok is False:
            print("[EXIT] WARN: fresh_mode=0 設定に失敗（attempted）, socket close")
        else:
            print("[EXIT] fresh_mode=0, socket close")


if __name__ == "__main__":
    main()

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