mechArm 270 Pi 動的追従シリーズ Log#32 | j0を振って実測する。工程姿勢の到達範囲をカメラで地図にした日

工程姿勢のj0を右端から左端まで振って、爪先がマット上のどこに来るかをカメラで実測したい。位置への汎化（play-B)に進む前に、j0とマット座標の対応表を自分の手で持っておきたかった。

ただ、計測ツールの前に棚卸しをやった。これが結果的に、このツールの土台になった。

先に棚卸し ― 「動くものリスト」を作る

直前のセッションで、実績ある作業に対して前例のない方式を選び、実機テストが8回に膨らんだ（Log#31）。根本は「動く前例の一覧がないから、毎回『新しく作る』に流れる」という構造で、その対策として、新規実装ゼロ・削る方向だけの整理から入った。

まずPi側のdrag_teaching/とMac側のplace_modules/を3観点で分類した。各スクリプトに実機で動いた実績があるか。確認のたびに増えたファイルは資産か使い捨てか。安全装置（読み耐性・ゴミ読みフィルタ・再送・ABORT）を毎回書き直していないか。

結果、Mac側のplace_modules/は31ファイル中13本をアーカイブへ移した（検出器の旧版6本、立ち上げ期の単発確認5本ほか）。Pi側も試行版のteach JSON群と切り分け用の最小テスト、v1のdrag_teach.pyを退役させた。v1はv2が完全上位互換で、しかもMac側のv1は実機修正前の版（存在しないクラス名を参照していて、そもそも実行不能）だった。削除ではなく_archive/への移動方式にしたので、判断を誤っても戻せる。

整理の成果物としてWORKING_ASSETS.md（動くものリスト）を作った。ファイル・役割・実機実績のDAY・依存関係の一覧で、冒頭にルールを3行書いた。新規タスク開始時、まずこのリストを見る。ここにある資産で組めるなら新規スクリプトを書かない。実機実績の無いものは資産ではない。

もうひとつ大きい発見があった。git statusを流したら、リポジトリのほぼ全資産が未追跡だった。daemon本体、現行FKテーブル、homography、drag_teaching/とplace_modules/の現役スクリプト全部。さらにリポジトリ自体にリモートが無く、コミットはローカルメモでしかなかった。ディスク障害が起きたら「git管理しているつもりの資産」が履歴ごと消える状態だった。現役資産だけを個別指定でコミットし（45ファイル）、GitHubにprivateリポジトリ（PixelstoMotors/r2r2r-scripts）を作ってpushした。動くものリストを作っても、動くもの自体が保護されていなければ意味が半減する。

そしてこの棚卸しの直後に、今回の計測ツールを設計した。必要な部品が全部リストの中にあった。daemon経由の送信と到達確認はPLACE系モジュールの実績パターン、カメラ解決とhomographyと赤ブロック検出はdetect_positions_once、工程姿勢の抽出とJOINT_LIMITSと読み耐性はwaypoint_replayのimport流用。新しく書いたのは「j0を差し替えてループする」ロジックだけだ。リストを作った当日に「流用が初手」が一度機能した。

爪先の検出には、視覚サーボ時代（LOG184）に作った青シールの幾何検出をそのまま使う。掴んでいない工程（アプローチPICK）は爪先の青シール、ブロックを咥える工程（PICK以降）は赤ブロック検出。検出に使ったフレームは成功・失敗を問わず全点画像保存する。検出値がおかしかったとき、画像が残っていなければ原因を後から切り分けられないからだ。

設計上の注意がひとつ。上カメラのhomographyはマット平面（床面）基準で組んでいる。アプローチ姿勢の爪は床から約111mm浮いているので、床面基準の変換を通すと視差で外側に膨らんで見える。ずれは系統的なので工程内では一貫したデータになるが、工程をまたいで位置データを混ぜると事故る。出力JSONに高さ注記を必須で入れ、1工程=1ファイルの構造で混用を防いだ。

今回作ったもの

j0_sweep_scan.py。Mac上で実行し、アームはdaemon（TCP 192.168.2.1:5555）経由で動かす。daemonの既存6コマンドのみ使用で、daemon側の変更はない。構成は、工程姿勢へ移動 → j0をステップ送り → 各停止点でget_angles実測＋カメラ検出 → 1点ずつ即時保存、の単純なループ。安全側は、全スイープ点の送信前JOINT_LIMITS一括検査、移動量からの動的タイムアウト、到達失敗時のABORT（未到達のまま次の点へ進む経路を持たない）。

レビューと実機の往復で3回修正が入った（base_anglesのJOINT_LIMITS事前検査、フレーム取得失敗時のplaceholder画像、無進捗再送、--base-anglesによる基準姿勢の直接指定、--no-moveによる送信なし測定、ストリーミング送信）。最終形のフルコード（797行）は記事末尾に全量掲載する。リポジトリは PixelstoMotors/r2r2r-scripts。

実機1回目 ― teachから抽出した姿勢が中腰だった

最初の実機で、アームはアプローチPICKとは違う姿勢で止まった。マットに近づいたのでCtrl-Cで停止し、08_home_from_anywhere.py（棚卸しで現役棚に残した復旧ツール）でHOMEに戻した。

原因は抽出元データにあった。基準姿勢はteach JSONからargmax方式（phase区間内でPICKから最も遠い姿勢）で抽出していたが、ログにこう出ていた。

[INIT] 抽出根拠: phase APPROACH_PICK argmax_idx=108 (start=108 end=194, gap=33.48°)
[INIT] phase=approach_pick 基準姿勢: [ -36.29,  +58.18,   +2.28,   +2.81,  -61.17,  -31.81]

argmaxが区間の開始点ぴったり（idx=108）を選んでいる。このteachではAPPROACH_PICKの記録が始まった瞬間、アームはまだHOMEからアプローチへ移動している途中だった。j1が58°（アプローチの定義は約88°）という値がそれを物語っている。抽出ロジックの不具合ではなく、teach時の印の打ち方とのミスマッチだった。

対処は抽出をやめることにした。アプローチPICKの正しい姿勢はarm_kinematics.mdに確定値がある。teach抽出に固執する理由がないので、--base-anglesで6軸を直接指定できるようにした。

python j0_sweep_scan.py --phase approach_pick --j0-min -10 --j0-max 10 --step 5 --base-angles "0,87.74,-38.33,3.6,-70.11,-30"

検出ゼロの原因はケーブルだった

正しい姿勢で5点のスイープが通った。だが検出は5点とも失敗。「青マーカー 0 点」。保存された画像を見ると、青シールは爪先にはっきり写っている。

全点画像保存の仕組みがさっそく生きた。アームを動かさず、保存されたフレームに対してオフラインで既存のHSV閾値を当て直してみる。結果、爪先の2点はきれいに抜けていた。検出できないのは3点目——グリッパー本体上のwristシールだった。

j0_-010_0.jpg → 全輪郭(area>=10): [(264, 929, 990), (261, 795, 956), (62, 920, 783)]
j0__000_0.jpg → 全輪郭(area>=10): [(259, 921, 988), (257, 1059, 997), (57, 1012, 795), (33, 994, 560)]
j0__005_0.jpg → 全輪郭(area>=10): [(264, 985, 994), (255, 1122, 992), (67, 1058, 795)]

爪先2点は面積250〜265px。3点目は54〜67pxで、検出関数のMIN_AREA=80を下回っていた。原因はグリッパーに沿って走る黒いケーブルがwristシールの上にかかり、青の見かけ面積を削っていたこと。検出ロジックは3点（wrist＋爪先2点）の三角形から最短辺で爪先を識別する設計なので、3点目が消えると全体が不成立になる。

ケーブルの位置を直しただけで、コードは1行も変えずに5点全部が通った。

xはj0に対してきれいに直線を描き、傾きは約-6.4mm/degに収まった。LOG180で視覚サーボの補正係数として実測したMM_PER_DEG_J0_X = -6.63とほぼ同じ値だ。別のアプローチで取った実測値が同じところに収束し、座標の確からしさが裏付けられた形になった。

全域スイープ ― 実動域は±95°

j0の実測リミット全域（-156°〜+161°）を21点でスイープした。途中、7点目でsend_anglesが実機に無視されてABORTする事象が起きた（LOG228で見た散発沈黙と同類。後述の件とは別で、こちらは再実行で回復した）。無進捗時のみ同じ姿勢を最大2回再送する機構を足し、2回に分けて20点を取得した。

実行ログ（第1区間・ABORTまで）。

[INIT] スイープ点 (21 点): -156.00, -140.15, -124.30, -108.45, -92.60, -76.75, -60.90, -45.05, -29.20, -13.35, +2.50, +18.35, +34.20, +50.05, +65.90, +81.75, +97.60, +113.45, +129.30, +145.15, +161.00
[INIT] JOINT_LIMITS OK (base + 21 スイープ点)
[MOVE] 基準姿勢（教示時 j0=+0.00°）へ移動 ...
[SEND] to base: send_angles [  +0.00,  +87.74,  -38.33,   +3.60,  -70.11,  -30.00]  speed=20  (pre max|Δ|=123.66° → timeout=20.5s)
[SEND] arrival OK: max|Δ|=1.99° (tol 2.0°)
--- [ 1/21] j0_cmd=-156.00° ---
[SEND] arrival OK: max|Δ|=1.99°
[ 1/21] detect FAILED: tip 未検出（青マーカー 0 点）
--- [ 2/21] j0_cmd=-140.15° ---
[ 2/21] detect FAILED: tip 未検出（青マーカー 0 点）
--- [ 3/21] j0_cmd=-124.30° ---
[ 3/21] detect FAILED: tip 未検出（青マーカー 0 点）
--- [ 4/21] j0_cmd=-108.45° ---
[ 4/21] detect FAILED: tip 未検出（青マーカー 0 点）
--- [ 5/21] j0_cmd=-92.60° ---
[ 5/21] detect OK: xz=(+0.346, -0.058) (tip pixel=(315, 191))
--- [ 6/21] j0_cmd=-76.75° ---
[ 6/21] detect OK: xz=(+0.345, +0.040) (tip pixel=(323, 389))
--- [ 7/21] j0_cmd=-60.90° ---
[SEND] [ 7/21] j0=-60.90: send_angles  speed=20  (pre max|Δ|=16.26° → timeout=8.0s)
[SEND] [TIMEOUT] 8.0s 内に未到達: max|Δ|=16.26° actual=[ -77.16,  +89.73,  -37.44,   +3.33,  -70.40,  -29.70]
[ABORT] [ 7/21] 到達確認タイムアウト。これまでの記録を保存して終了

検出失敗の4点は爪がカメラ視野（マット範囲）の外。これは失敗ではなく「カメラで見える実動域の端」の測定になる。1点ごとの即時保存のおかげで、ABORTしても6点は無駄にならず、残り区間だけ再実行すれば済んだ。

20点（+161°は実動域外につき省略）を合わせた対応表。検出OKは13点。

全域を叩いて、見えた事実が3つある。

まず、カメラ観測下のj0実動域は約-93°〜+98°。左右ほぼ対称で、ベースがマット上端寄りに置かれている配置と整合する。

13点は半径約0.36mのきれいな円弧に乗る。視差込みの見かけ半径がほぼ一定で、homographyと関節の再現性が信用できることを示している。マット端（物理+0.310m）を超えるx=+0.346という値は、アプローチ高度111mmの視差による系統的な膨らみで、概算（カメラ高さ約1m・横350mm・高さ111mm）とも整合する。

マット中央のx範囲（±0.31m）はj0±約60°でカバーされる。PICK可能域の見当がこれで付いた。

j0_cmd=0付近の検出xは+0.001m。原点がほぼぴったり原点に写っている。

グリッパーの後、アームが黙る

次はPICK姿勢（床ギリギリ・赤ブロックを咥える）のスイープに進んだ。手順は、gripper.pyで爪を開く、ブロックを挟む、閉じる、スイープ実行。ここで止まった。

[MOVE] 基準姿勢（教示時 j0=+0.00°）へ移動 ...
[SEND] to base: send_angles [  +0.00,  +90.87,  -37.00,   +3.00,  -67.00,  -30.00]  speed=20  (pre max|Δ|=89.38° → timeout=15.4s, progress check @ 5.1s)
[SEND] [WARN] no-progress at 5.2s: |pre→cur|=0.09° < 2.0° (attempt 1/3, 再送します)
[SEND] [RETRY] attempt 2/3: 無進捗検知のため同じ target を再送
[SEND] [WARN] no-progress at 5.5s: |pre→cur|=1.66° < 2.0° (attempt 2/3, 再送します)
[SEND] [RETRY] attempt 3/3: 無進捗検知のため同じ target を再送
[SEND] [WARN] no-progress at 5.4s but retry 上限到達 (attempt 3/3, このまま timeout 待ち)
[SEND] [TIMEOUT] 15.4s 内に未到達: max|Δ|=89.38° actual=[  -0.26,   +1.49,   +0.87,   -0.26,   -0.26,  -30.23]
[ABORT] 基準姿勢への到達確認タイムアウト。スイープを開始しません

LOG228で見た「グリッパー直後の単発send_angles無視」と同じ症状だが、今回は再送3回が全部食われた。時間を置いた再実行も同じ。daemonを再起動しても同じ。daemon側のログが決定的だった。

[DAEMON 01:46:14] cmd received: send_angles
[DAEMON 01:46:14] send_angles 呼び出し前: angles=[0.0, 90.87, -37.0, 3.0, -67.0, -30.0], speed=20, wait=0.0
[DAEMON 01:46:14] send_angles 呼び出し後: result=None, elapsed_ms=9.3
[DAEMON 01:46:14] send_angles 後の現在角度: [-0.26, 1.49, 0.87, -0.26, -0.26, -30.23]

daemonはコマンドを受け取り、pymycobotのsend_anglesを正常に呼び、即座に返ってきている。Mac→Pi→pymycobotまで全部通っていて、その先が黙っている。get_anglesは正常に読める。daemon再起動でシリアル接続もMyCobotオブジェクトも作り直されているのに直らない。

沈黙していたのはMacでもPiでもない。アーム側のコントローラだった。LOG228では「送信文脈を変えて回避」までだった。今回は原因の場所まで分かった。

アームの電源を物理的に入れ直したら、一発で直った。88°の大移動が通り、検出も成功した。

そのままブロックを咥え直してPICKスイープを再実行したら、また沈黙した。2回中2回。確定した事実をまとめる。

daemon経由のグリッパーclose実行後、send_anglesがコントローラレベルで全沈黙する。再送・時間経過・daemon再起動では解除されない。アームの電源リセットでのみ解除される。

PICKへ ― 残り2°で止まった

対処は2案あった。手順の工夫（移動を全部closeの前に済ませ、咥えたら送信なしで測定だけ行う--no-moveモード）と、LOG228で実証済みのストリーミング送信（0.1s周期で補間姿勢を流す方式はグリッパー後でも通る）への置き換え。実装には両方が入った。

電源リセット後、空の爪でPICK姿勢（j0=-5°）への移動を実行した。

[MOVE] 基準姿勢（教示時 j0=-5.00°）へ移動 ...
[SEND] to base: streaming send_angles  speed=20  (pre max|Δ|=89.29° → 45 ticks @ 0.1s = 4.5s, post-stream timeout 15.4s)
[SEND] [TIMEOUT] 15.4s 内に未到達: max|Δ|=2.02° actual=[  -4.92,  +90.26,  -34.98,   +2.90,  -65.21,  -29.97]
[ABORT] 基準姿勢への到達確認タイムアウト。スイープを開始しません

アームはPICK姿勢の直前まで来て止まった。j2の実測-34.98°に対し目標-37.0°、差2.02°。到達判定の許容2.0°を0.02°超えてABORTした。床ギリギリの姿勢では負荷でサーボが最後の2°を詰め切れていない。j2の2°は爪先の高さに数cm効くので、目視でも「降り切っていない」のが分かる状態だった。

PICK姿勢のスイープは、この到達許容値の扱い（実測に合わせて緩めるか、姿勢側を調整するか）を決めてからになる。ここで作業を止めた。

j0と位置の対応表が1工程ぶん手に入り、グリッパー沈黙の条件も確定した。床ギリギリの2°という次の一手も見えている。地図は1枚できた。残りの工程も、このプロセスをなぞるだけだ。

j0_sweep_scan.py（フルコード）

Mac上で実行し、アームはdaemon（TCP 5555）経由で動かす。daemonの既存6コマンドのみ使用で、daemon側の変更はない。レビューと実機の往復で3回修正が入った最終形を掲載する（base_anglesのJOINT_LIMITS事前検査、フレーム取得失敗時のplaceholder画像、移動量からの動的タイムアウト、無進捗再送、--base-anglesによる基準姿勢の直接指定、--no-moveによる送信なし測定、ストリーミング送信）。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""j0_sweep_scan.py — j0 単軸スイープ自動計測（工程姿勢ベース）

工程姿勢の j0 だけを段階的に振りながら、各停止点で「関節実測角度」と
「上カメラによる位置検出」をペアで記録する。Mac 上で実行し、アームは
daemon 経由（TCP 192.168.2.1:5555）で動かす。

工程と検出対象（人が事前に爪の状態を準備すること。set_gripper は呼ばない）:
  approach_pick : 爪が空                 → 爪先（青シール）/ visual_servo_pick.detect_tip_from_frame
  pick          : 爪が赤ブロックを咥える → 赤ブロック     / visual_servo_pick.detect_red_block
  approach_place: 爪が赤ブロックを咥える → 赤ブロック     / 同上
  place         : 爪が赤ブロックを咥える → 赤ブロック     / 同上

絶対ルール:
- send_coords() / release_all_servos() を呼ばない（daemon 経由なので構造的に呼べない）
- set_gripper を呼ばない（このスクリプトはグリッパー操作の責任外）
- daemon にコマンドを追加しない
- 既存の fk_scan_motion_based*.csv に書き込まない
- 全送信前に JOINT_LIMITS（waypoint_replay の実測値）一括検査
- 1点ごとに JSON を atomic write（途中 ABORT でもデータが残る）

CLI:
    python3 j0_sweep_scan.py --phase pick --j0-min -20 --j0-max 20 --step 2
    python3 j0_sweep_scan.py --phase approach_pick --j0-min -20 --j0-max 20 --step 2

出力:
    ~/r2r2r-scripts/logs/j0_sweep/j0_sweep_{phase}_{NN}.json
    ~/r2r2r-scripts/logs/j0_sweep/j0_sweep_{phase}_{NN}_frames/j0_{j0_cmd:+06.1f}.jpg
"""

from __future__ import print_function

import argparse
import importlib
import json
import math
import os
import sys
import time
import types
from pathlib import Path


# ---- パス解決 --------------------------------------------------------------
_THIS = Path(__file__).resolve()
_PLACE_MODULES_DIR = _THIS.parent
_SCRIPTS_ROOT = _THIS.parents[3]  # ~/r2r2r-scripts
_DRAG_TEACHING_DIR = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "drag_teaching"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES_DIR))
sys.path.insert(0, str(_DRAG_TEACHING_DIR))


# ---- pymycobot スタブ ------------------------------------------------------
# waypoint_replay.py は import 時に `from pymycobot.mycobot import MyCobot` を
# 実行するが、このスクリプトはアーム制御を全て daemon TCP 経由で行うため、
# MyCobot を一切インスタンス化しない。Mac に pymycobot が無い前提で、
# import エラーを起こさないようスタブだけ差し込む（waypoint_replay 自体は
# 変更しない）。スタブを呼び出した瞬間に RuntimeError を投げ、誤って実体化
# しようとしたら直ちに分かるようにする。
if "pymycobot" not in sys.modules:
    _pkg = types.ModuleType("pymycobot")
    _mod = types.ModuleType("pymycobot.mycobot")

    class _MyCobotStub(object):
        def __init__(self, *args, **kwargs):
            raise RuntimeError(
                "j0_sweep_scan.py: MyCobot stub is not callable. "
                "Arm control must go through daemon TCP, not direct serial.")

    _mod.MyCobot = _MyCobotStub
    sys.modules["pymycobot"] = _pkg
    sys.modules["pymycobot.mycobot"] = _mod


# ---- 流用 import ----------------------------------------------------------
import cv2  # noqa: E402
import numpy as np  # noqa: E402

import waypoint_replay as wr  # noqa: E402
import waypoint_streaming_replay as wsr  # noqa: E402  # _interpolate / STREAM_INTERVAL_SEC / STEP_DEG_PER_TICK 流用
from visual_servo_pick import (  # noqa: E402
    get_daemon_socket,
    close_daemon_socket,
    daemon_send_angles,
    daemon_get_angles,
    daemon_set_fresh_mode,
    setup_homography_and_mat_range,
    pixel_to_unity,
    detect_red_block,
    detect_tip_from_frame,
)


# ---- 定数 -----------------------------------------------------------------
DEFAULT_TEACH_JSON = _DRAG_TEACHING_DIR / "drag_v2_20221013_152840_with_pos.json"
DEFAULT_OUTPUT_ROOT = Path.home() / "r2r2r-scripts" / "dynamic_tracking_limits" / "logs" / "j0_sweep"

SPEED = 20                       # 全送信共通（task 仕様）
TOL_REACH_DEG = 2.0               # 到達判定（task 仕様）
# 到達確認 timeout は wr._compute_arrival_timeout(max_delta) で送信ごとに算出する
# （DAY228実績の式：BASE + max_delta/SPEED * FACTOR、MIN/MAX で clamp）。
# REACH_TIMEOUT_SEC はフォールバック下限：
#   - 現在姿勢が取れず max_delta を算出できないときの固定値
#   - 算出値が REACH_TIMEOUT_SEC 未満になるケースの最終下限
REACH_TIMEOUT_SEC = 8.0
POLL_SLEEP = 0.3
GET_ANGLES_RETRY = 12             # 読みリトライ（waypoint_replay と同値）
GET_ANGLES_RETRY_SLEEP = 0.2
WARMUP_FRAMES = 30
FIRST_MOVE_WARN_DEG = 30.0
DEFAULT_SETTLE_SEC = 1.0
# フレーム取得失敗時の placeholder 画像サイズ（成功フレームを 1 枚も得ていない場合のフォールバック）
PLACEHOLDER_W = 1280
PLACEHOLDER_H = 720

VALID_PHASES = ("approach_pick", "pick", "approach_place", "place")
PHASE_TO_WP_KEY = {
    "approach_pick": "APPROACH_PICK",
    "pick": "PICK",
    "approach_place": "APPROACH_PLACE",
    "place": "PLACE",
}
PHASE_DETECT_TARGET = {
    "approach_pick": "tip",
    "pick": "red_block",
    "approach_place": "red_block",
    "place": "red_block",
}


# ---- ヘルパ ---------------------------------------------------------------
def _fmt_angles(a):
    if a is None:
        return "None"
    return "[" + ", ".join("{:+7.2f}".format(v) for v in a) + "]"


def _daemon_get_angles_filtered():
    """daemon_get_angles を最大 GET_ANGLES_RETRY 回試し、waypoint_replay の
    _is_valid_angles（JOINT_LIMITS ± READ_SANITY_MARGIN_DEG）を通った値だけ返す。
    全失敗で None。
    """
    for _ in range(GET_ANGLES_RETRY):
        a = None
        try:
            a = daemon_get_angles()
        except Exception:
            a = None
        if wr._is_valid_angles(a):
            return [float(x) for x in a]
        time.sleep(GET_ANGLES_RETRY_SLEEP)
    return None


def _send_and_wait_arrival(target, label):
    """target を補間ストリーミングで送信し、tol 内に入るまで到達確認する。

    DAY228 take2-4 で「daemon 経由の set_gripper(close) 直後、単発 send_angles が
    コントローラレベルで全沈黙する（再送・daemon 再起動とも無効、電源リセットのみ解除）」
    事象が実証されたため、移動方式を waypoint_streaming_replay の補間ストリーミングに
    置き換えている（DAY228 で実証済みの方式）。

    手順:
      1. 送信前姿勢を取得し、現在姿勢→target を線形補間
         （wsr._interpolate, STEP_DEG_PER_TICK=2.0°/tick）
      2. wsr.STREAM_INTERVAL_SEC=0.1s 周期で daemon_send_angles を流す
      3. ストリーム完了後、wr._compute_arrival_timeout で算出した timeout 窓で
         get_angles 実測ポーリング、tol TOL_REACH_DEG (=2.0°) で到達判定
      4. 送信前姿勢が取れない場合は補間なしの単発送信に degrade（従来 fallback）

    タイムアウトで False。ABORT 経路は変更しない。
    """
    # 1. 送信前姿勢 → timeout 算出
    pre_actual = _daemon_get_angles_filtered()
    if pre_actual is not None:
        pre_max, _ = wr._max_axis_diff(target, pre_actual)
        timeout_sec = max(REACH_TIMEOUT_SEC, wr._compute_arrival_timeout(pre_max))
    else:
        pre_max = None
        timeout_sec = REACH_TIMEOUT_SEC

    # 2. ストリーミング送信（送信前姿勢が取れない場合は単発に degrade）
    if pre_actual is None:
        print("[SEND] {}: send_angles {}  speed={}  "
              "(現在姿勢取得失敗 → 補間なし単発, post-stream timeout {:.1f}s)".format(
                  label, _fmt_angles(target), SPEED, timeout_sec))
        try:
            daemon_send_angles(target, speed=SPEED, wait=0)
        except Exception as e:
            print("[SEND] [ERROR] daemon_send_angles 例外: {}".format(e))
            return False
    else:
        n_steps = max(1, int(math.ceil(pre_max / wsr.STEP_DEG_PER_TICK)))
        poses = wsr._interpolate(pre_actual, target, n_steps)
        stream_duration = n_steps * wsr.STREAM_INTERVAL_SEC
        print("[SEND] {}: streaming send_angles  speed={}  "
              "(pre max|Δ|={:.2f}° → {} ticks @ {}s = {:.1f}s, "
              "post-stream timeout {:.1f}s)".format(
                  label, SPEED, pre_max, n_steps, wsr.STREAM_INTERVAL_SEC,
                  stream_duration, timeout_sec))
        k = 0
        try:
            for k, pose in enumerate(poses):
                daemon_send_angles(pose, speed=SPEED, wait=0)
                time.sleep(wsr.STREAM_INTERVAL_SEC)
        except Exception as e:
            print("[SEND] [ERROR] streaming send_angles 例外 tick={}/{}: {}".format(
                k + 1, n_steps, e))
            return False

    # 3. ストリーム完了後の到達確認ポーリング
    t0 = time.time()
    last_err = None
    last_actual = None
    while time.time() - t0 < timeout_sec:
        actual = _daemon_get_angles_filtered()
        if actual is not None:
            max_d, _ = wr._max_axis_diff(actual, target)
            last_err = max_d
            last_actual = actual
            if max_d <= TOL_REACH_DEG:
                print("[SEND] arrival OK: max|Δ|={:.2f}° (tol {:.1f}°)".format(
                    max_d, TOL_REACH_DEG))
                return True
        time.sleep(POLL_SLEEP)
    le = "{:.2f}".format(last_err) if last_err is not None else "None"
    la = _fmt_angles(last_actual) if last_actual is not None else "None"
    print("[SEND] [TIMEOUT] {:.1f}s 内に未到達: max|Δ|={}° actual={}".format(
        timeout_sec, le, la))
    return False


def _next_output_paths(out_root, phase):
    """j0_sweep_{phase}_{NN}.json と j0_sweep_{phase}_{NN}_frames/ の次の連番を返す。
    既存ファイル/ディレクトリは上書きしない。"""
    out_root.mkdir(parents=True, exist_ok=True)
    for n in range(1, 1000):
        seq = "{:02d}".format(n)
        json_path = out_root / "j0_sweep_{}_{}.json".format(phase, seq)
        frames_dir = out_root / "j0_sweep_{}_{}_frames".format(phase, seq)
        if not json_path.exists() and not frames_dir.exists():
            frames_dir.mkdir(parents=True, exist_ok=False)
            return json_path, frames_dir
    raise RuntimeError("出力ファイル連番が 999 を超えました")


def _atomic_write_json(path, data):
    """tmp ファイルに書き出してから rename。途中 ABORT でも path が壊れない。"""
    tmp = path.with_suffix(path.suffix + ".tmp")
    with open(str(tmp), "w") as f:
        json.dump(data, f, ensure_ascii=False, indent=2)
    os.replace(str(tmp), str(path))


def _capture_frame(cap):
    """cap から 1 frame 取得（数枚捨ててバッファをクリア）。"""
    for _ in range(3):
        cap.read()
    ret, frame = cap.read()
    if not ret or frame is None:
        return None
    return frame


def _detect_for_phase(frame, phase):
    """phase に応じた検出を実行。
    戻り値: {"ok", "xz_m", "score", "pixel", "info"}
        - tip / red_block とも score は提供されないため None で記録する
        - pixel は描画用（成功時のみ）
    """
    target = PHASE_DETECT_TARGET[phase]
    if target == "tip":
        try:
            tip_pixel, blue_markers, tip_unity = detect_tip_from_frame(frame)
        except Exception as e:
            return {"ok": False, "xz_m": None, "score": None, "pixel": None,
                    "info": "tip 検出例外: {}".format(e)}
        if tip_pixel is None or tip_unity is None:
            n_markers = 0 if blue_markers is None else len(blue_markers)
            return {"ok": False, "xz_m": None, "score": None, "pixel": None,
                    "info": "tip 未検出（青マーカー {} 点）".format(n_markers)}
        ux, uz = tip_unity
        return {"ok": True, "xz_m": [float(ux), float(uz)], "score": None,
                "pixel": tip_pixel,
                "info": "tip pixel=({}, {})".format(int(tip_pixel[0]), int(tip_pixel[1]))}
    elif target == "red_block":
        try:
            pixel = detect_red_block(frame)
        except Exception as e:
            return {"ok": False, "xz_m": None, "score": None, "pixel": None,
                    "info": "red_block 検出例外: {}".format(e)}
        if pixel is None:
            return {"ok": False, "xz_m": None, "score": None, "pixel": None,
                    "info": "red_block 未検出（HSV しきい値内の領域なし）"}
        cx, cy = pixel
        try:
            ux, uz = pixel_to_unity(cx, cy)
        except Exception as e:
            return {"ok": False, "xz_m": None, "score": None, "pixel": pixel,
                    "info": "pixel_to_unity 例外: {}".format(e)}
        return {"ok": True, "xz_m": [float(ux), float(uz)], "score": None,
                "pixel": pixel,
                "info": "red_block pixel=({}, {})".format(int(cx), int(cy))}
    return {"ok": False, "xz_m": None, "score": None, "pixel": None,
            "info": "unknown detect target: {}".format(target)}


def _annotate_and_save(frame, det, j0_cmd, phase, dst_path):
    """frame に検出結果を描画して dst_path に保存する（成功・失敗とも保存）。"""
    out = frame.copy()
    cv2.putText(out, "phase={} j0_cmd={:+.1f}".format(phase, j0_cmd),
                (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
    if det.get("pixel") is not None:
        try:
            ipx = int(det["pixel"][0])
            ipy = int(det["pixel"][1])
            cv2.circle(out, (ipx, ipy), 12, (0, 0, 255), 2)
            cv2.drawMarker(out, (ipx, ipy), (0, 0, 255), cv2.MARKER_CROSS, 22, 2)
        except Exception:
            pass
    if det["ok"]:
        ux, uz = det["xz_m"]
        score = det["score"]
        s_str = "{:.3f}".format(score) if isinstance(score, (int, float)) else "None"
        cv2.putText(out,
                    "detect: x={:+.3f}m z={:+.3f}m score={}".format(ux, uz, s_str),
                    (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
    else:
        cv2.putText(out, "detect: FAILED ({})".format(det.get("info", "")),
                    (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
    ok_write = cv2.imwrite(str(dst_path), out)
    if not ok_write:
        print("[WARN] cv2.imwrite が False を返しました: {}".format(dst_path))


def _resolve_base_angles(teach_json_path, phase):
    """waypoint_replay の抽出ロジックを流用し、指定工程の 6 軸基準姿勢を返す。
    抽出ロジック自体は触らない（印優先・phase 範囲フィルタ・APPROACH argmax）。"""
    payload, angles, phases, gripper_events, count = wr._load_teach(str(teach_json_path))
    waypoints, sources = wr._extract_waypoints(angles, phases, gripper_events, count)
    name = PHASE_TO_WP_KEY[phase]
    base = list(waypoints[name])
    src = sources[name]
    return base, src


def _parse_base_angles_arg(s):
    """--base-angles の "j0,j1,j2,j3,j4,j5" をパースして長さ 6 の float リストを返す。
    フォーマット違反は ValueError。"""
    parts = [p.strip() for p in s.split(",")]
    if len(parts) != 6:
        raise ValueError(
            "--base-angles は 6 値（カンマ区切り）が必要。got {} 値: {!r}".format(
                len(parts), s))
    vals = []
    for i, p in enumerate(parts):
        try:
            vals.append(float(p))
        except ValueError:
            raise ValueError(
                "--base-angles の {} 番目が数値でない: {!r}（全体: {!r}）".format(
                    i + 1, p, s))
    return vals


def _generate_sweep_points(j0_min, j0_max, step):
    """j0_min から step 刻みで j0_max まで（境界含む）の点列を返す。"""
    if step <= 0:
        raise ValueError("step must be > 0")
    if j0_max < j0_min:
        raise ValueError("j0_max < j0_min")
    pts = []
    n = int(math.floor((j0_max - j0_min) / step + 1e-9)) + 1
    for k in range(n):
        v = j0_min + step * k
        if v > j0_max + 1e-6:
            break
        pts.append(round(v, 6))
    return pts


# ---- メイン --------------------------------------------------------------
def main():
    p = argparse.ArgumentParser(description="j0 単軸スイープ + 上カメラ位置検出（工程別）")
    p.add_argument("--phase", required=True, choices=VALID_PHASES,
                   help="計測する工程")
    p.add_argument("--j0-min", type=float, default=None,
                   help="j0 最小値 (度)。--no-move 時は不要")
    p.add_argument("--j0-max", type=float, default=None,
                   help="j0 最大値 (度)。--no-move 時は不要")
    p.add_argument("--step", type=float, default=None,
                   help="j0 ステップ幅 (度, >0)。--no-move 時は不要")
    p.add_argument("--no-move", action="store_true",
                   help=("アームへの送信を一切行わず、現在姿勢で 1 点だけ測定する。"
                         "事前に人手で目標姿勢へ移動済みのケース用。"
                         "--j0-min/--j0-max/--step は無視され、j0_cmd には実測 j0 が入る"))
    p.add_argument("--settle", type=float, default=DEFAULT_SETTLE_SEC,
                   help="各停止点での安定待ち秒 (デフォルト {:.1f}s)".format(DEFAULT_SETTLE_SEC))
    p.add_argument("--teach-json", type=str, default=str(DEFAULT_TEACH_JSON),
                   help="teach JSON のパス（--base-angles 指定時は使用しない）")
    p.add_argument("--base-angles", type=str, default=None,
                   help=('基準姿勢を直接指定（"j0,j1,j2,j3,j4,j5" カンマ区切り）。'
                         '指定時は teach JSON 抽出をスキップし、この値を base_angles として使う。'
                         'JOINT_LIMITS 検査は従来どおり適用'))
    p.add_argument("--out-dir", type=str, default=str(DEFAULT_OUTPUT_ROOT),
                   help="出力ディレクトリ")
    p.add_argument("--top", type=int, default=None,
                   help="上カメラ index（未指定なら camera_resolver で自動）")
    args = p.parse_args()

    if args.settle < 0:
        print("[ABORT] --settle は 0 以上を指定してください")
        sys.exit(1)
    if not args.no_move:
        if args.j0_min is None or args.j0_max is None or args.step is None:
            print("[ABORT] 通常モードでは --j0-min / --j0-max / --step が必須です "
                  "（--no-move 指定時は不要）")
            sys.exit(1)
        if args.step <= 0:
            print("[ABORT] --step は正の値を指定してください")
            sys.exit(1)
        if args.j0_max < args.j0_min:
            print("[ABORT] --j0-max < --j0-min は不正")
            sys.exit(1)

    phase = args.phase

    # 1. 基準姿勢の解決：--base-angles が指定されていればそれを使い、
    #    無ければ teach JSON から既存の抽出ロジックで取り出す。
    teach_json = None  # cli_direct 時は None のまま（出力 JSON の source_teach_json で参照）
    if args.base_angles is not None:
        try:
            base_angles = _parse_base_angles_arg(args.base_angles)
        except ValueError as e:
            print("[ABORT] {}".format(e))
            sys.exit(1)
        base_source = "cli_direct: {}".format(args.base_angles)
        print("[INIT] base_angles を CLI で直接指定（teach JSON 抽出はスキップ）")
    else:
        teach_json = Path(args.teach_json).expanduser()
        if not teach_json.exists():
            print("[ABORT] teach JSON が見つかりません: {}".format(teach_json))
            sys.exit(1)
        print("[INIT] teach JSON: {}".format(teach_json))
        try:
            base_angles, base_source = _resolve_base_angles(teach_json, phase)
        except SystemExit:
            raise
        except Exception as e:
            print("[ABORT] 基準姿勢の抽出で例外: {}".format(e))
            sys.exit(1)
    print("[INIT] phase={} 基準姿勢: {}".format(phase, _fmt_angles(base_angles)))
    print("[INIT] 抽出根拠: {}".format(base_source))
    j0_teach = base_angles[0]
    print("[INIT] 教示時 j0 = {:+.2f}°".format(j0_teach))

    # 2. スイープ点を生成（--no-move 時は生成しない）
    if args.no_move:
        pts = None
        print("[INIT] --no-move 指定: 現在姿勢で 1 点のみ測定（送信なし）")
    else:
        pts = _generate_sweep_points(args.j0_min, args.j0_max, args.step)
        print("[INIT] スイープ点 ({} 点): {}".format(
            len(pts), ", ".join("{:+.2f}".format(v) for v in pts)))
        if len(pts) == 0:
            print("[ABORT] スイープ点がゼロです")
            sys.exit(1)

    # 3. JOINT_LIMITS 一括検査（送信前）
    # base_angles 自体も検査する。基準姿勢は最初に send_angles される（STEP 8）ので、
    # スイープ点の検査より前に守る必要がある。
    # --no-move 時は送信しないが、base_angles は metadata として記録されるので検査は維持。
    print("[INIT] JOINT_LIMITS 一括検査 ...")
    base_viol = wr._check_joint_limits(base_angles)
    if base_viol:
        print("[ABORT] JOINT_LIMITS 違反: base_angles が範囲外（基準姿勢が不正）")
        for axis, val, lo, hi in base_viol:
            print("  base  j{}={:+.3f} not in [{:.1f}, {:.1f}]".format(
                axis + 1, val, lo, hi))
        sys.exit(1)
    if pts is not None:
        bad = []
        for j0v in pts:
            target = [j0v] + list(base_angles[1:])
            viol = wr._check_joint_limits(target)
            if viol:
                bad.append((j0v, viol))
        if bad:
            print("[ABORT] JOINT_LIMITS 違反 ({} 点):".format(len(bad)))
            for j0v, viol in bad:
                for axis, val, lo, hi in viol:
                    print("  j0_cmd={:+.2f}°  j{}={:+.3f} not in [{:.1f}, {:.1f}]".format(
                        j0v, axis + 1, val, lo, hi))
            sys.exit(1)
        print("[INIT] JOINT_LIMITS OK (base + {} スイープ点)".format(len(pts)))
    else:
        print("[INIT] JOINT_LIMITS OK (base のみ。スイープ点は --no-move のため対象外)")

    # 4. daemon ping（get_daemon_socket 内部で ping を発行）
    print("[INIT] daemon 接続中 ...")
    try:
        get_daemon_socket()
    except Exception as e:
        print("[ABORT] daemon ping 失敗: {}".format(e))
        sys.exit(1)
    print("[INIT] daemon ping OK")

    cap = None
    output_data = None
    json_path = None
    frames_dir = None
    exit_code = 0
    try:
        if not daemon_set_fresh_mode(1):
            print("[ABORT] fresh_mode=1 設定に失敗")
            sys.exit(1)
        print("[INIT] fresh_mode=1")

        # 現在姿勢 → base_angles 差分チェック
        print("[INIT] 現在姿勢を取得（最大 {} 回・{:.1f}s 間隔・ゴミ読みフィルタ）".format(
            GET_ANGLES_RETRY, GET_ANGLES_RETRY_SLEEP))
        current = _daemon_get_angles_filtered()
        if current is None:
            print("[ABORT] 現在姿勢が取得できません")
            sys.exit(1)
        print("[INIT] 現在姿勢 = {}".format(_fmt_angles(current)))
        if args.no_move:
            print("[INIT] --no-move のため、基準姿勢への移動・30° 急加速チェックはスキップ")
        else:
            first_max, _ = wr._max_axis_diff(base_angles, current)
            print("[INIT] 現在 → 基準姿勢 max|Δ| = {:.2f}°".format(first_max))
            if first_max > FIRST_MOVE_WARN_DEG:
                print("[WARN] 先頭移動の max|Δ| {:.2f}° > {:.1f}°。急加速の可能性があります".format(
                    first_max, FIRST_MOVE_WARN_DEG))
                print("[INIT] 続行するには 'yes' と入力（それ以外は中止）")
                try:
                    ans = input().strip()
                except EOFError:
                    print("[ABORT] stdin EOF")
                    sys.exit(1)
                if ans != "yes":
                    print("[INIT] 中止しました")
                    sys.exit(1)

        # 5. カメラ解決＋セットアップ
        top_index = args.top
        if top_index is None:
            try:
                cr = importlib.import_module("camera_resolver")
                cams = cr.resolve_cameras(require_top=True, require_side=False)
                top_index = cams.get("top") if cams else None
            except Exception as e:
                print("[ABORT] camera_resolver 失敗: {}".format(e))
                sys.exit(1)
            if top_index is None:
                print("[ABORT] 上カメラ未解決")
                sys.exit(1)
        print("[CAM] 上カメラ index = {}".format(top_index))
        cap = cv2.VideoCapture(top_index)
        if not cap.isOpened():
            print("[ABORT] カメラ open 失敗 (index={})".format(top_index))
            sys.exit(1)
        for _ in range(WARMUP_FRAMES):
            cap.read()
        if not setup_homography_and_mat_range(cap):
            print("[ABORT] homography セットアップ失敗")
            sys.exit(1)
        print("[CAM] homography OK")

        # 6. 爪状態確認
        if phase == "approach_pick":
            prompt = ("[CONFIRM] phase={} → 検出対象=爪先（青シール）。"
                      "爪は空（ブロックを咥えていない）状態にしてください。OK なら Enter".format(phase))
        else:
            prompt = ("[CONFIRM] phase={} → 検出対象=赤ブロック。"
                      "爪に赤ブロックを咥えさせた状態にしてください。OK なら Enter".format(phase))
        print(prompt)
        try:
            input()
        except EOFError:
            print("[ABORT] stdin EOF")
            sys.exit(1)

        # 7. 出力パス確保（連番。既存があればインクリメント。上書き禁止）
        out_root = Path(args.out_dir).expanduser()
        json_path, frames_dir = _next_output_paths(out_root, phase)
        print("[OUT] JSON  : {}".format(json_path))
        print("[OUT] frames: {}/".format(frames_dir))

        output_data = {
            "phase": phase,
            "detect_target": PHASE_DETECT_TARGET[phase],
            "base_angles": list(base_angles),
            "source_teach_json": teach_json.name if teach_json is not None else None,
            "base_source": base_source,
            "height_note": "床面homography基準。工程をまたぐ位置データの混用禁止",
            "frames_dir": frames_dir.name + "/",
            "sweep_params": {
                "j0_min": args.j0_min,
                "j0_max": args.j0_max,
                "step": args.step,
                "settle_sec": args.settle,
                "n_points": 1 if args.no_move else len(pts),
                "speed": SPEED,
                "tol_reach_deg": TOL_REACH_DEG,
                "no_move": bool(args.no_move),
            },
            "sweep": [],
        }
        _atomic_write_json(json_path, output_data)

        # 8〜10: --no-move 時は送信を一切行わず、現在姿勢で 1 点だけ測定する
        if args.no_move:
            print("[MEASURE] --no-move: 単一点測定モード（送信なし）")
            print("[MEASURE] settle {:.1f}s".format(args.settle))
            time.sleep(args.settle)

            actual = _daemon_get_angles_filtered()
            if actual is None:
                print("[ABORT] 測定時の get_angles 全失敗。記録なしで終了")
                sys.exit(1)
            j0_cmd = actual[0]  # 実測 j0 を j0_cmd として記録
            print("[MEASURE] actual = {}  (j0_cmd = j0_actual = {:+.2f}°)".format(
                _fmt_angles(actual), j0_cmd))

            frame = _capture_frame(cap)
            if frame is None:
                print("[WARN] フレーム取得失敗。placeholder 画像で記録")
                det = {"ok": False, "xz_m": None, "score": None, "pixel": None,
                       "info": "frame capture failed"}
                frame_for_save = np.zeros((PLACEHOLDER_H, PLACEHOLDER_W, 3),
                                          dtype=np.uint8)
            else:
                det = _detect_for_phase(frame, phase)
                frame_for_save = frame

            frame_filename = "j0_{:+06.1f}.jpg".format(j0_cmd)
            frame_path = frames_dir / frame_filename
            try:
                _annotate_and_save(frame_for_save, det, j0_cmd, phase, frame_path)
            except Exception as e:
                print("[WARN] フレーム保存例外: {}".format(e))

            if det["ok"]:
                ux, uz = det["xz_m"]
                print("[MEASURE] detect OK: xz=({:+.3f}, {:+.3f}) ({})".format(
                    ux, uz, det["info"]))
            else:
                print("[MEASURE] detect FAILED: {}".format(det["info"]))

            point = {
                "j0_cmd": j0_cmd,
                "angles_actual": actual,
                "detect_xz_m": det["xz_m"],
                "detect_score": det["score"],
                "detect_ok": bool(det["ok"]),
                "frame_file": frame_filename,
            }
            if not det["ok"]:
                point["detect_info"] = det["info"]
            output_data["sweep"].append(point)
            _atomic_write_json(json_path, output_data)
            print("[MEASURE] 完了（1 点記録）")
        else:
            # 通常モード：基準姿勢へ移動 → スイープループ → 基準姿勢へ復帰
            # 8. 基準姿勢へ移動
            print("[MOVE] 基準姿勢（教示時 j0={:+.2f}°）へ移動 ...".format(j0_teach))
            if not _send_and_wait_arrival(base_angles, "to base"):
                print("[ABORT] 基準姿勢への到達確認タイムアウト。スイープを開始しません")
                sys.exit(1)

            # 9. スイープループ
            n_ok = 0
            n_fail = 0
            # 直前の成功フレームのサイズを記録（取得失敗時の placeholder サイズに使う）
            last_frame_shape = None
            for i, j0v in enumerate(pts):
                tag = "[{:2d}/{:2d}]".format(i + 1, len(pts))
                target = [j0v] + list(base_angles[1:])
                print("--- {} j0_cmd={:+.2f}° target={} ---".format(
                    tag, j0v, _fmt_angles(target)))

                if not _send_and_wait_arrival(target, "{} j0={:+.2f}".format(tag, j0v)):
                    print("[ABORT] {} 到達確認タイムアウト。これまでの記録を保存して終了".format(tag))
                    _atomic_write_json(json_path, output_data)
                    sys.exit(1)

                print("{} settle {:.1f}s".format(tag, args.settle))
                time.sleep(args.settle)

                actual = _daemon_get_angles_filtered()
                if actual is None:
                    print("[ABORT] {} get_angles 全失敗。これまでの記録を保存して終了".format(tag))
                    _atomic_write_json(json_path, output_data)
                    sys.exit(1)
                print("{} actual = {}".format(tag, _fmt_angles(actual)))

                frame = _capture_frame(cap)
                if frame is None:
                    print("[WARN] {} フレーム取得失敗。検出スキップ・placeholder 画像で記録".format(tag))
                    det = {"ok": False, "xz_m": None, "score": None, "pixel": None,
                           "info": "frame capture failed"}
                    if last_frame_shape is not None:
                        ph_h, ph_w = last_frame_shape[0], last_frame_shape[1]
                        ph_c = last_frame_shape[2] if len(last_frame_shape) >= 3 else 3
                    else:
                        ph_h, ph_w, ph_c = PLACEHOLDER_H, PLACEHOLDER_W, 3
                    frame_for_save = np.zeros((ph_h, ph_w, ph_c), dtype=np.uint8)
                else:
                    det = _detect_for_phase(frame, phase)
                    frame_for_save = frame
                    last_frame_shape = frame.shape

                frame_filename = "j0_{:+06.1f}.jpg".format(j0v)
                frame_path = frames_dir / frame_filename
                try:
                    _annotate_and_save(frame_for_save, det, j0v, phase, frame_path)
                except Exception as e:
                    print("[WARN] {} フレーム保存例外: {}".format(tag, e))

                if det["ok"]:
                    ux, uz = det["xz_m"]
                    print("{} detect OK: xz=({:+.3f}, {:+.3f}) ({})".format(
                        tag, ux, uz, det["info"]))
                    n_ok += 1
                else:
                    print("{} detect FAILED: {}".format(tag, det["info"]))
                    n_fail += 1

                point = {
                    "j0_cmd": j0v,
                    "angles_actual": actual,
                    "detect_xz_m": det["xz_m"],
                    "detect_score": det["score"],
                    "detect_ok": bool(det["ok"]),
                    "frame_file": frame_filename,
                }
                if not det["ok"]:
                    point["detect_info"] = det["info"]
                output_data["sweep"].append(point)
                _atomic_write_json(json_path, output_data)

            print("[SWEEP] 完了: 検出成功 {}/{}, 失敗 {}".format(n_ok, len(pts), n_fail))

            # 10. 基準姿勢へ復帰
            print("[MOVE] 基準姿勢へ復帰 ...")
            if not _send_and_wait_arrival(base_angles, "back to base"):
                print("[WARN] 復帰の到達確認タイムアウト。送信は停止します（姿勢はその場維持）")

    except KeyboardInterrupt:
        print("\n[INTERRUPT] Ctrl-C 検出。新しい送信を停止し、これまでの記録を保存します")
        if output_data is not None and json_path is not None:
            try:
                _atomic_write_json(json_path, output_data)
            except Exception as e:
                print("[WARN] 中断時の保存に失敗: {}".format(e))
        exit_code = 130
    finally:
        try:
            if cap is not None:
                cap.release()
        except Exception:
            pass
        try:
            cv2.destroyAllWindows()
        except Exception:
            pass
        try:
            daemon_set_fresh_mode(0)
        except Exception as e:
            print("[WARN] fresh_mode=0 失敗: {}".format(e))
        try:
            close_daemon_socket()
        except Exception:
            pass
        # 最終保存（KeyboardInterrupt 経路も含めもう一度ベストエフォートで）
        if output_data is not None and json_path is not None:
            try:
                _atomic_write_json(json_path, output_data)
            except Exception:
                pass
        if json_path is not None:
            print("[EXIT] JSON   : {}".format(json_path))
        if frames_dir is not None:
            try:
                n_files = sum(1 for p in frames_dir.iterdir() if p.is_file())
            except Exception:
                n_files = -1
            print("[EXIT] frames : {} ({} files)".format(frames_dir, n_files))

    sys.exit(exit_code)


if __name__ == "__main__":
    main()

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