mechArm 270 Pi 動的追従の限界を探る Log#18 | mechArm 把持初成立。視覚サーボだけでブロックを掴むまでの 21 ループ

LOG208。視覚サーボだけで mechArm 270 Pi がブロックを掴むことに成功した。動的追従シリーズでの初把持成立。

💡 Key Takeaway

視覚サーボだけで mechArm 270 Pi がブロックを掴むことに成功した。動的追従シリーズでの初把持成立──FK 直接指令や open-loop アプローチでは過去に把持できていたが、視覚フィードバックでアームをループ補正しながら掴むのは初めて。pymycobot のキューイング問題、軸選択ロジックの優先順位の罠、no_motion 自動 ABORT の過保護、Z 閾値の固さ──6 時間で 4 つの壁を順番に外し、最後は閾値を 8mm から 12mm に緩めた 1 行の変更で爪が閉じた。「精度より体験の成立を優先する。精度は後から金で解決できる」という設計哲学が、コード行ではなく動作として証明された夜。

22 時 13 分に爪が閉じた

mechArm 270 Pi が、視覚サーボだけで赤いブロックを掴んだ。

iPhone をカメラに使い、Unity で座標系を作り、Mac から TCP socket で Raspberry Pi に指令を送る。手元のハードは mechArm 270 Pi（実効ペイロード 150g）と iPhone 14 Pro Max と iPhone 11 だけ。深度センサーなし、特殊なロボティクスハードなし。市販のスマートフォン 2 台と 10 万円のロボットアームだけで、視覚情報から関節角度を逆算してブロックを掴む動作が成立した。

成功時のターミナルログを残す。

[LOOP 20] diff_x=+2.78mm OK  diff_z=+11.34mm OK  diff_y=+6pix OK
  [GRIP_CHECK] x_ok=True(2.78<=6.0)  z_ok=True(11.34<=12.0)  y_ok=True(6<=12)  => True
[GRIP] 掴める近傍に到達。厳密収束を待たずグリッパーを閉じて保持します。
  [Pi] GRIPPER: CLOSE result= None

21 ループ。爪先がブロックに到達するまで 21 回の補正を要した。掴むまでに要した 21 回の補正、6 時間で外した 4 つの壁、8mm から 12mm への閾値緩和。順に振り返る。

壁 1：sync_send_angles が動かなかった（15:55 〜 19:00）

最初の壁は daemon 側にあった。Pi で常駐している mecharm_daemon.py の send_angles ハンドラは、pymycobot の sync_send_angles を呼んでいた。これは「指令角に到達するまで待つ」ブロッキング関数で、timeout=3 秒で抜ける仕様だ。

実機の daemon ログを全量見る。

[DAEMON 15:53:57] MyCobot 初期化中 (/dev/ttyAMA0, baud=1000000)...
Note: This class is no longer maintained since v3.6.0, please refer to the project documentation: https://github.com/elephantrobotics/pymycobot/blob/main/README.md
[DAEMON 15:53:59] ✓ MyCobot 初期化完了
[DAEMON 15:53:59] ポート 5555 で待ち受け中...
[DAEMON 15:54:47] 接続: ('192.168.2.2', 65100)
[DAEMON 15:54:47] sync_send_angles 呼び出し前: angles=[0.0, 0.0, 0.0, 0.0, 0.0, -30.0], speed=20
[DAEMON 15:54:50] sync_send_angles 呼び出し後: result=0
[DAEMON 15:54:50] sync_send_angles 後の現在角度: [-0.35, 1.49, 0.7, -0.08, 55.72, -29.79]
[DAEMON 15:55:04] sync_send_angles 呼び出し前: angles=[0.0, 87.74, -20.0, 3.6, -90.0, -30.0], speed=20
[DAEMON 15:55:07] sync_send_angles 呼び出し後: result=0
[DAEMON 15:55:07] sync_send_angles 後の現在角度: [-0.35, 88.85, -18.63, 2.81, -86.57, -29.79]
[DAEMON 15:55:10] sync_send_angles 呼び出し前: angles=[-3.35, 91.0, -15.629999999999999, 3.07, -84.89, -29.79], speed=50
[DAEMON 15:55:13] sync_send_angles 呼び出し後: result=0
[DAEMON 15:55:13] sync_send_angles 後の現在角度: [-2.02, 90.79, -16.69, 3.77, -88.06, -29.79]
[DAEMON 15:55:15] sync_send_angles 呼び出し前: angles=[-5.02, 90.79, -13.690000000000001, 3.77, -84.71, -29.79], speed=50
[DAEMON 15:55:18] sync_send_angles 呼び出し後: result=0
[DAEMON 15:55:18] sync_send_angles 後の現在角度: [-2.02, 90.79, -16.69, 3.77, -87.27, -29.79]
[DAEMON 15:55:20] sync_send_angles 呼び出し前: angles=[-5.02, 90.79, -13.690000000000001, 3.77, -84.18, -29.79], speed=50
[DAEMON 15:55:23] sync_send_angles 呼び出し後: result=0
[DAEMON 15:55:23] sync_send_angles 後の現在角度: [-2.02, 90.79, -16.69, 3.69, -87.09, -29.79]
[DAEMON 15:55:25] sync_send_angles 呼び出し前: angles=[-5.02, 90.79, -13.690000000000001, 3.69, -84.09, -29.79], speed=50
[DAEMON 15:55:28] sync_send_angles 呼び出し後: result=0
[DAEMON 15:55:28] sync_send_angles 後の現在角度: [-2.02, 90.79, -16.69, 3.6, -87.09, -29.79]
[DAEMON 15:55:30] sync_send_angles 呼び出し前: angles=[-5.02, 90.79, -13.690000000000001, 3.6, -84.09, -29.79], speed=50
[DAEMON 15:55:33] sync_send_angles 呼び出し後: result=0
[DAEMON 15:55:33] sync_send_angles 後の現在角度: [-2.02, 90.79, -16.69, 3.6, -87.09, -29.79]
[DAEMON 15:55:35] sync_send_angles 呼び出し前: angles=[0.0, 0.0, 0.0, 0.0, 0.0, -30.0], speed=20
[DAEMON 15:55:37] sync_send_angles 呼び出し後: result=0
[DAEMON 15:55:37] sync_send_angles 後の現在角度: [0.08, 1.66, 0.52, 0.35, 0.17, -29.88]
[DAEMON 15:55:43] 切断: ('192.168.2.2', 65100)

すべてのループで result=0 が返っている。pymycobot の sync_send_angles の戻り値仕様で、これは「timeout 内に指令角に到達できずに抜けた」を意味する。さらに悪いことに、result=0 で抜けた後、daemon の get_angles で取得する measured 値は target に追従していない。LOOP 0 で target [-3.35, 91.0, -15.63, 3.07, -84.89, -29.79] を送ったのに、measured は [-2.02, 90.79, -16.69, 3.77, -88.06, -29.79]。LOOP 1 で target [-5.02, 90.79, -13.69, 3.77, -84.71, -29.79] を送っても、measured は同じく [-2.02, 90.79, -16.69, 3.77, -87.27, -29.79]。

target は変わるのに measured が固定されている。アームは目視でもまったく動いていなかった。

実機 CSV（19:57:25 セッション）を全量見る。

loop,diff_x_mm,diff_z_mm,diff_y_pix,selected_axis,delta_j0,delta_j1,delta_j2,delta_j4_for_Z,delta_j4_for_Y,delta_j4_total,actual_step_deg,new_angles,measured,reach_ok,converged
0,131.7588609457016,-50.28160667419434,151,X,-3.0,2.1486000000000002,3.0,0.0,3.0,3.0,,"[-3.17, 91.0, -15.8, 2.81, -84.18, -29.79]","[-2.02, 90.79, -16.69, 3.77, -88.06, -29.79]",False,False
1,121.18075346946716,-39.62066650390625,83,X,-3.0,0.21,3.0,0.0,3.0,3.0,3.0,"[-4.84, 90.79, -13.87, 3.51, -84.27, -29.79]","[-2.02, 90.79, -16.69, 3.77, -86.83, -29.79]",True,False
2,120.93281030654907,-39.87332153320312,80,X,-3.0,0.21,3.0,0.0,3.0,3.0,0.0900000000000034,"[-4.84, 90.79, -13.87, 3.51, -83.83, -29.79]","[-2.02, 90.79, -16.69, 3.77, -86.74, -29.79]",True,False
3,120.94081830978394,-39.622680664062436,78,X,-3.0,0.21,3.0,0.0,3.0,3.0,0.3500000000000085,"[-4.84, 90.79, -13.87, 3.51, -83.74, -29.79]","[-2.02, 90.79, -16.69, 3.77, -86.39, -29.79]",True,False
4,121.18034362792969,-39.62274169921875,74,X,-3.0,0.21,3.0,0.0,3.0,3.0,0.0,"[-4.84, 90.79, -13.87, 3.51, -83.39, -29.79]","[-2.02, 90.79, -16.69, 3.77, -86.39, -29.79]",True,False

LOOP 1 以降、measured の j0/j1/j2 が完全に同じ値（-2.02, 90.79, -16.69）で固定されている。j4 だけは微妙に動いているが、本体は固まっている。

ターミナル側のログも同じ症状。

[LOOP 4] diff_x=+120.42mm NG  diff_z=-39.62mm NG  diff_y=+82pix NG
  [CLAMP] j1 Δ +3.00 -> +0.21 (current=90.79, soft_max=91.00)
  [SEND] Δ=[j0=-3.00, j1=+0.00, j2=+3.00, j4_Z=+0.00, j4_Y=+3.00, j4_total=+3.00]
         new_angles=[-5.02, 90.79, -13.69, 3.6, -84.09, -29.79]
  [WARN] j0 到達誤差 3.00° > 2.00° (target=-5.02, measured=-2.02)
  [WARN] j2 到達誤差 3.00° > 2.00° (target=-13.69, measured=-16.69)
  [WARN] j4 到達誤差 3.00° > 2.00° (target=-84.09, measured=-87.09)
  [WARN] 実測変化が小さすぎます: max_step=0.00° (3/3)
[ABORT] 指令を出しても実測角度が変化しません。daemon/電源/関節上限を確認してください。

target と measured の差が 全関節とも 3.00° で固定。3° の指令を出しても measured が 1.75° 程度しか変わらないという LOG204 の知見（サーボの不感帯）を踏まえても、3° ぴったり固定はおかしい。これは「指令はサーボへ届いているが、実際の動作キューが詰まって新指令が反映されていない」ことを示していた。

複数回の実行と仮説の絞り込みを経て、原因は pymycobot のキューイング仕様にあると分かった。デフォルトでは send_angles で送った指令が未完了のうちに次の指令を送ると、新指令がキューに積まれて順番待ちになる。視覚サーボのように 1.5 秒ごとに新 target を投げ続ける使い方では、キューが詰まって最新指令が反映されない。

LOG204 引き継ぎ書の P3 タスクに「set_fresh_mode 代替の挙動確認」が候補として書かれていた。pymycobot には set_fresh_mode(1) という関数があり、これを有効化すると「新しい send_angles が来たときに、未完了の動作キューを破棄して新指令を即実行する」モードに切り替わる。デフォルトは 0（キューイング）。

この修正案は外部レビュー（codex）が LOG204 引き継ぎ書から拾ってきた。私は完全に見落としていた。

修正後の mecharm_daemon.py フルコードを以下に掲載する。

ここで本質は「sync_send_angles を send_angles に変えた」ことではない。pymycobot のキューを「未来予約型」から「現在優先型」に切り替えたこと。新指令が来るたびに過去の動作キューが破棄される。視覚サーボの 1.5 秒周期と pymycobot の挙動を、ようやく同じ時間軸に乗せた。

#!/usr/bin/env python3
"""
mecharm_daemon.py
Pi 側で常駐する MyCobot 制御デーモン。
TCP socket で Mac からの JSON コマンドを受け取り、永続化された MyCobot 経由で実機に転送する。
"""

import json
import socket
import sys
import time
import signal
import traceback
from pymycobot.mycobot import MyCobot

DAEMON_HOST = "0.0.0.0"
DAEMON_PORT = 5555
SERIAL_PORT = "/dev/ttyAMA0"
BAUDRATE = 1000000

_mc = None
_server_sock = None
_running = True
_fresh_mode_state = 0


def log(msg):
    print(f"[DAEMON {time.strftime('%H:%M:%S')}] {msg}", flush=True)


def init_mycobot():
    global _mc
    log(f"MyCobot 初期化中 ({SERIAL_PORT}, baud={BAUDRATE})...")
    _mc = MyCobot(SERIAL_PORT, BAUDRATE)
    log("✓ MyCobot 初期化完了")


def handle_command(cmd_obj):
    """コマンドを処理してレスポンス dict を返す。"""
    global _fresh_mode_state
    cmd = cmd_obj.get("cmd")

    if cmd == "ping":
        return {"ok": True, "daemon": "mecharm_daemon", "fresh_mode": _fresh_mode_state}

    elif cmd == "send_angles":
        try:
            angles = cmd_obj["angles"]
            speed = int(cmd_obj.get("speed", 20))
            wait = float(cmd_obj.get("wait", 0))
            if len(angles) != 6:
                return {"ok": False, "error": f"angles must be length 6, got {len(angles)}"}

            log(f"send_angles 呼び出し前: angles={angles}, speed={speed}, wait={wait}")
            t0 = time.perf_counter()
            result = _mc.send_angles(angles, speed)
            elapsed_ms = (time.perf_counter() - t0) * 1000.0
            log(f"send_angles 呼び出し後: result={result}, elapsed_ms={elapsed_ms:.1f}")

            if wait > 0:
                time.sleep(wait)
            current_angles = _mc.get_angles()
            log(f"send_angles 後の現在角度: {current_angles}")

            return {"ok": True, "result": result, "elapsed_ms": elapsed_ms}
        except Exception as e:
            log(f"send_angles 例外: {type(e).__name__}: {e}")
            log(traceback.format_exc().rstrip())
            return {"ok": False, "error": f"{type(e).__name__}: {e}"}

    elif cmd == "send_angle":
        try:
            joint_id = int(cmd_obj["joint_id"])
            angle = float(cmd_obj["angle"])
            speed = int(cmd_obj.get("speed", 20))
            wait = float(cmd_obj.get("wait", 0))
            if not (1 <= joint_id <= 6):
                return {"ok": False, "error": f"joint_id must be 1..6, got {joint_id}"}

            log(f"send_angle 呼び出し前: joint_id={joint_id}, angle={angle}, speed={speed}, wait={wait}")
            t0 = time.perf_counter()
            result = _mc.send_angle(joint_id, angle, speed)
            elapsed_ms = (time.perf_counter() - t0) * 1000.0
            log(f"send_angle 呼び出し後: result={result}, elapsed_ms={elapsed_ms:.1f}")

            if wait > 0:
                time.sleep(wait)
            current_angles = _mc.get_angles()
            log(f"send_angle 後の現在角度: {current_angles}")

            return {"ok": True, "result": result, "elapsed_ms": elapsed_ms}
        except Exception as e:
            log(f"send_angle 例外: {type(e).__name__}: {e}")
            log(traceback.format_exc().rstrip())
            return {"ok": False, "error": f"{type(e).__name__}: {e}"}

    elif cmd == "get_angles":
        # DAY190: mc.get_angles() は稀に [] や None を返す pymycobot 既知挙動。100ms × 最大3回リトライ。
        a = None
        for attempt in range(3):
            a = _mc.get_angles()
            if a and len(a) == 6:
                break
            time.sleep(0.1)
        if not a or len(a) != 6:
            return {"ok": False, "error": "get_angles returned empty/invalid after 3 retries"}
        return {"ok": True, "angles": a}

    elif cmd == "set_fresh_mode":
        value = int(cmd_obj["value"])
        if value not in (0, 1):
            return {"ok": False, "error": f"value must be 0 or 1, got {value}"}
        try:
            _mc.set_fresh_mode(value)
        except AttributeError:
            return {"ok": False, "error": "set_fresh_mode is not available in this pymycobot environment"}
        _fresh_mode_state = value
        log(f"set_fresh_mode({value})")
        return {"ok": True, "fresh_mode": _fresh_mode_state}

    elif cmd == "set_gripper":
        state = int(cmd_obj["state"])
        speed = int(cmd_obj.get("speed", 100))
        wait = float(cmd_obj.get("wait", 0))
        # DAY175 対策: 連続送信時の動作不具合回避のため事前 sleep
        time.sleep(2)
        result = _mc.set_gripper_state(state, speed)
        if wait > 0:
            time.sleep(wait)
        return {"ok": True, "result": result}

    else:
        return {"ok": False, "error": f"unknown cmd: {cmd}"}


def serve_client(client_sock, addr):
    log(f"接続: {addr}")
    buf = b""
    try:
        while _running:
            data = client_sock.recv(4096)
            if not data:
                break
            buf += data
            while b"\n" in buf:
                line, buf = buf.split(b"\n", 1)
                line = line.strip()
                if not line:
                    continue
                try:
                    cmd_obj = json.loads(line.decode("utf-8"))
                    response = handle_command(cmd_obj)
                except Exception as e:
                    response = {"ok": False, "error": f"{type(e).__name__}: {e}"}
                    log(f"コマンド処理エラー: {e}")
                resp_line = (json.dumps(response) + "\n").encode("utf-8")
                client_sock.sendall(resp_line)
    except ConnectionResetError:
        log(f"接続リセット: {addr}")
    except Exception as e:
        log(f"クライアントエラー: {e}")
    finally:
        client_sock.close()
        log(f"切断: {addr}")


def shutdown_handler(signum, frame):
    global _running
    log(f"シグナル受信 ({signum}), シャットダウン開始")
    _running = False
    if _mc is not None:
        try:
            _mc.set_fresh_mode(0)
            log("set_fresh_mode(0)")
        except Exception:
            pass
    if _server_sock:
        try:
            _server_sock.close()
        except Exception:
            pass
    sys.exit(0)


def main():
    global _server_sock

    signal.signal(signal.SIGTERM, shutdown_handler)
    signal.signal(signal.SIGINT, shutdown_handler)

    init_mycobot()

    _server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    _server_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    _server_sock.bind((DAEMON_HOST, DAEMON_PORT))
    _server_sock.listen(1)
    log(f"ポート {DAEMON_PORT} で待ち受け中...")

    try:
        while _running:
            try:
                client_sock, addr = _server_sock.accept()
            except OSError:
                break
            serve_client(client_sock, addr)
    finally:
        if _mc is not None:
            try:
                _mc.set_fresh_mode(0)
                log("set_fresh_mode(0)")
            except Exception:
                pass
        _server_sock.close()
        log("シャットダウン完了")


if __name__ == "__main__":
    main()

変更したのは 4 箇所。sync_send_angles(timeout=3) を send_angles のノンブロッキング呼び出しに置き換える。set_fresh_mode コマンドハンドラを追加。状態を _fresh_mode_state グローバル変数で保持して ping レスポンスでも返す。シャットダウン時と例外時に set_fresh_mode(0) を呼び戻す──daemon 終了後に他プロセスがデフォルト挙動で動けるように。

xyz_track_test.py の起動時に daemon_set_fresh_mode(1) を呼ぶようにした。

# (4) daemon 接続
print("[4] daemon 接続...")
get_daemon_socket()
daemon_set_fresh_mode(1)

result = "aborted"
try:
    # (5) HOME → OPEN → APPROACH_BASE → XYZ_TRACK
    result = run_xyz_track_test(cap_top, cap_side)
except KeyboardInterrupt:
    print("\n[INTERRUPT] Ctrl+C 検出。HOME 復帰します。")
finally:
    try:
        daemon_set_fresh_mode(0)
        # ...

これでアームが動くようになった。最初の壁を抜けるのに 3 時間かかった。

壁 2：Y 補正が呼ばれない（21:13 〜 21:55）

アームが動き出した後、新しい問題が見えた。複数回の実機実行（21:12 〜 21:38 の 14 セッション）の CSV を順番に解析した結果を表にする。GRIP_READY 閾値は当時 X≤6, Z≤8, Y_pix≤12。

21:24:47 の実行で LOOP 16, 17, 18 で GRIP_READY 条件が 3 回連続成立していた。しかし爪は閉じなかった。

21:24:47 の CSV を全量見る（19 ループ分）。

loop,diff_x_mm,diff_z_mm,diff_y_pix,selected_axis,delta_j0,delta_j1,delta_j2,delta_j4_for_Z,delta_j4_for_Y,delta_j4_total,actual_step_deg,new_angles,measured,reach_ok,converged
0,41.44572401046753,-19.24188232421875,170,X,-6.251240423901589,0.0,0.0,0.0,0.0,0.0,,"[-6.421240423901589, 88.59, -18.72, 3.07, -87.36, -30.14]",,False,False
1,41.447264671325684,-18.994537353515568,185,X,-6.25147280110493,0.0,0.0,0.0,0.0,0.0,6.24,"[-6.42147280110493, 88.59, -18.72, 3.07, -87.36, -30.14]","[-6.41, 90.08, -18.36, 3.77, -88.76, -30.23]",True,False
2,-5.033132553100586,-9.880111694335938,163,X,0.7591451814631351,0.0,0.0,0.0,0.0,0.0,1.0499999999999998,"[-5.650854818536865, 90.08, -18.36, 3.77, -88.76, -30.23]","[-5.36, 90.08, -18.36, 3.77, -88.85, -30.23]",True,False
3,0.6837224960327148,-11.14323425292963,163,Z,0.0,0.0,1.8996307966126205,0.0,0.0,0.0,2.369999999999999,"[-5.36, 90.08, -16.46036920338738, 3.77, -88.85, -30.23]","[-5.36, 90.08, -15.99, 3.77, -88.85, -30.23]",True,False
4,1.7054042816162038,-6.19775390625,136,Z,0.0,0.0,1.0565553880412548,0.0,0.0,0.0,0.0,"[-5.36, 90.08, -14.933444611958745, 3.77, -88.85, -30.23]","[-5.36, 90.08, -15.99, 3.77, -88.85, -30.23]",True,False
5,1.7054042816162038,-6.19775390625,136,Z,0.0,0.0,1.0565553880412548,0.0,0.0,0.0,1.4900000000000002,"[-5.36, 90.08, -14.933444611958745, 3.77, -88.85, -30.23]","[-5.36, 90.08, -14.5, 3.69, -88.85, -30.23]",True,False
6,2.470661163330078,-2.73468017578125,120,Y,0.0,0.0,0.6666666666666666,0.0,2.0,2.0,1.0600000000000005,"[-5.36, 90.08, -13.833333333333334, 3.69, -86.85, -30.23]","[-5.36, 90.87, -13.44, 3.69, -88.85, -30.23]",True,False
7,2.989924430847168,1.2261352539061932,98,Y,0.0,0.0,0.0,0.0,2.0,2.0,0.6999999999999886,"[-5.36, 90.87, -13.44, 3.69, -86.85, -30.23]","[-5.36, 90.87, -13.44, 3.69, -88.15, -30.23]",True,False
8,2.9916439056396484,1.4737701416015625,91,Y,0.0,0.0,0.0,0.0,2.0,2.0,0.44000000000001194,"[-5.36, 90.87, -13.44, 3.69, -86.15, -30.23]","[-5.36, 90.87, -13.44, 3.69, -87.71, -30.23]",True,False
9,2.9916439056396484,1.4737701416015625,86,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.5799999999999983,"[-5.36, 90.87, -13.44, 3.69, -85.71, -30.23]","[-5.36, 90.87, -13.44, 3.69, -86.13, -30.23]",True,False
10,3.2416324615478445,1.720611572265625,68,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.6700000000000017,"[-5.36, 90.87, -13.44, 3.69, -84.13, -30.23]","[-5.36, 90.87, -13.44, 3.69, -84.46, -30.23]",True,False
11,3.2469377517700195,2.7115631103515625,53,Y,0.0,0.0,0.0,0.0,2.0,2.0,0.0,"[-5.36, 90.87, -13.44, 3.69, -82.46, -30.23]","[-5.36, 90.87, -13.44, 3.69, -84.46, -30.23]",True,False
12,3.4949121475219727,2.4626007080078125,54,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.5799999999999983,"[-5.36, 90.87, -13.44, 3.69, -82.46, -30.23]","[-5.36, 90.87, -13.44, 3.69, -82.88, -30.23]",True,False
13,3.4977006912231516,2.9582977294921875,36,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.6700000000000017,"[-5.36, 90.87, -13.44, 3.69, -80.88, -30.23]","[-5.36, 90.87, -13.44, 3.69, -81.21, -30.23]",True,False
14,3.7537002563476562,4.1959381103515625,21,Y,0.0,0.0,0.0,0.0,2.0,2.0,0.0,"[-5.36, 90.87, -13.44, 3.69, -79.21, -30.23]","[-5.36, 90.87, -13.44, 3.69, -81.21, -30.23]",True,False
15,3.7537002563476562,4.1959381103515625,23,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.6699999999999875,"[-5.36, 90.87, -13.44, 3.69, -79.21, -30.23]","[-5.36, 90.87, -13.44, 3.69, -79.54, -30.23]",True,False
16,4.008090972900391,5.186126708984432,5,X,-0.6045386082806019,0.0,0.0,0.0,0.0,0.0,0.0,"[-5.964538608280602, 90.87, -13.44, 3.69, -79.54, -30.23]","[-5.36, 90.87, -13.44, 3.69, -79.54, -30.23]",True,False
17,4.0054779052734375,4.690704345703182,5,X,-0.6041444804333993,0.0,0.0,0.0,0.0,0.0,0.0,"[-5.9641444804334, 90.87, -13.44, 3.69, -79.54, -30.23]","[-5.36, 90.87, -13.44, 3.69, -79.54, -30.23]",True,False
18,4.0054779052734375,4.690704345703182,6,X,-0.6041444804333993,0.0,0.0,0.0,0.0,0.0,0.0,"[-5.9641444804334, 90.87, -13.44, 3.69, -79.54, -30.23]","[-5.36, 90.87, -13.44, 3.69, -79.54, -30.23]",True,False

LOOP 16-18 で diff_x=+4.01, diff_z=+5.19→+4.69, diff_y_pix=+5→+5→+6。全部 GRIP_READY 閾値（X≤6, Z≤8, Y≤12）以内。なのに selected_axis は X で記録されており、L420 のグリッパー閉じ判定が発火していない。

最初の仮説は is_grip_ready() 関数の中の問題だったが、コードを読み直してインライン式に置き換えるテストでも発火しないケースが続いた。次の実行（21:52 以降）で本当の原因が見えた。

[LOOP 5] diff_x=+4.99mm NG  diff_z=+7.12mm NG  diff_y=+226pix NG
  [GRIP_CHECK] x_ok=True(4.99<=6.0)  z_ok=True(7.12<=8.0)  y_ok=False(226<=12)  => False
  [MODE] single-axis debug: X only (joint=j0, candidates=[('X', 2.5), ('Z', 3.6)], y_allowed=False)

X≤6 で OK、Z≤8 で OK、Y_pix=+226 で全然合っていない。にもかかわらず y_allowed=False で Y 補正が候補に入っていない。

xyz_track_test.py の軸選択ロジック（修正前）はこうなっていた。

y_correction_allowed = (
    abs(diff_x_mm) <= Y_CORRECTION_START_X_MM   # 4.0
    and abs(diff_z_mm) <= Y_CORRECTION_START_Z_MM  # 6.0
)

X が 4mm 以下 AND Z が 6mm 以下にならないと Y 補正が候補に入らない条件になっていた。LOOP 5 は X=4.99 で X 条件を超え、Z=7.12 で Z 条件も超えている。だから y_allowed=False。

さらに、軸選択の優先順位は X→Z→Y の固定順だった。

if abs(diff_x_mm) > Y_CORRECTION_START_X_MM and "X" in candidates_by_axis:
    selected_axis, _, selected_joint_index = candidates_by_axis["X"]
elif abs(diff_z_mm) > Y_CORRECTION_START_Z_MM and "Z" in candidates_by_axis:
    selected_axis, _, selected_joint_index = candidates_by_axis["Z"]
else:
    selected_axis, _, selected_joint_index = max(
        candidates, key=lambda item: item[1]
    )

X が閾値超え → X 優先。Z が閾値超え → Z 優先。X が縮みきらないと Z には進めない。Z が縮みきらないと Y には進めない。Z は j2 が動作限界に達して縮まないことが多い。

判断を変えた。X が GRIP_READY 領域に入っているなら、Z がまだ揺れていても Y 補正に進ませる。Z は副次的に揃う。実機ログの観察から、Y 軸下降中に j4 が動くと Z が ±5mm くらい揺れることが分かっていた。Z をきっちり 6mm 以下に追い込んでも、Y 下降中にまた 8mm まで広がってしまう。

修正後のロジック：

y_correction_allowed = (
    abs(diff_x_mm) <= Y_CORRECTION_START_X_MM
)

if abs(diff_x_mm) > Y_CORRECTION_START_X_MM and "X" in candidates_by_axis:
    selected_axis, _, selected_joint_index = candidates_by_axis["X"]
else:
    selected_axis, _, selected_joint_index = max(
        candidates, key=lambda item: item[1]
    )

Z 条件削除 + Z 優先分岐削除。X が GRIP_READY 領域（X≤6）に入っていれば、Z の状況に関わらず Y 補正が候補に入り、候補内のスコア（diff/tol の比率）が最大のものが選ばれる。Y_pix が大きければ Y、Z が大きければ Z、というスコアベース選択になる。

合わせて Y_CORRECTION_START 閾値そのものも GRIP_READY に揃えた。

GRIP_READY_X_MM = 6.0
GRIP_READY_Z_MM = 12.0     # ← この値はあとで変更（壁 4）
GRIP_READY_Y_PIX = 12
Y_CORRECTION_START_X_MM = 6.0   # 旧 4.0 → 6.0
Y_CORRECTION_START_Z_MM = 8.0   # 旧 6.0 → 8.0

この修正の後、実機で Y 補正が走り始めた。Y_pix が +200 → +50 → +20 と縮んでいった。

この時点で 22:00。最初の壁を抜けてから 6 時間経っていた。

壁 3：「実測変化が小さすぎます」の過保護（22:00 〜 22:05）

Y 補正が走り始めたあと、別のセーフティに引っかかった。

実機ログの該当部分：

[LOOP 3] diff_x=+1.54mm OK  diff_z=+9.85mm NG  diff_y=+179pix NG
  [GRIP_CHECK] x_ok=True(1.54<=6.0)  z_ok=False(9.85<=8.0)  y_ok=False(179<=12)  => False
  [MODE] single-axis debug: Z only (joint=j2, candidates=[('Z', 4.9)], y_allowed=False)
  [SEND] Δ=[j0=+0.00, j1=+0.00, j2=-1.68, j4_Z=+0.00, j2_Y=+0.00, j4_Y=+0.00, j4_total=+0.00]
         new_angles=[-9.84, 90.0, -20.84, 3.86, -86.83, -29.79]
  [SEND_JOINT] j2 -> -20.84 speed=50

[LOOP 4] diff_x=+1.28mm OK  diff_z=+8.86mm NG  diff_y=+185pix NG
  [GRIP_CHECK] x_ok=True(1.28<=6.0)  z_ok=False(8.86<=8.0)  y_ok=False(185<=12)  => False
  [SEND] Δ=[j0=+0.00, j1=+0.00, j2=-1.51, ...]
         new_angles=[-9.84, 90.0, -21.1, 3.86, -86.83, -29.79]
  [SEND_JOINT] j2 -> -21.10 speed=50
  [WARN] 実測変化が小さすぎます: max_step=0.00° (1/3)

[LOOP 5] diff_x=+1.28mm OK  diff_z=+8.62mm NG  diff_y=+185pix NG
  [SEND_JOINT] j2 -> -21.06 speed=50
  [WARN] 実測変化が小さすぎます: max_step=0.00° (2/3)

[LOOP 6] diff_x=+1.53mm OK  diff_z=+8.37mm NG  diff_y=+186pix NG
  [SEND_JOINT] j2 -> -21.02 speed=50
  [WARN] 実測変化が小さすぎます: max_step=0.00° (3/3)
[ABORT] 指令を出しても実測角度が変化しません。daemon/電源/関節上限を確認してください。
[CLEANUP] HOME 復帰...

LOOP 3 で j2 を -1.68° 動かす指令を出して、実機が動いた。LOOP 4, 5, 6 でさらに j2 を動かそうとしたが、max_step=0.00° で実測がほぼ動かない（MIN_EFFECTIVE_MOTION_DEG=0.35 未満）。3 回連続でこれが続くと自動 ABORT → HOME 復帰、という仕様だった。

この自動 ABORT には正当な理由があった。「daemon が指令を送ったのに実機が反応していない（電源不足・関節上限・通信断）」を検知するためのセーフティ。だが pymycobot のキューイング特性で、新指令が処理待ちで一時的に止まったり、関節がソフトリミット近傍で動きにくくなったりすることがある。「動かないと思ったら、次の瞬間に動き出す」というケースがあり、3 回でいきなり HOME に戻すのは過保護だった。

指示：

「固まって動かないと思ったらおれが止めるので、勝手にホームに戻るのをやめさせて」

判断は明確だった。自動 ABORT を完全に削除する。GRIP_READY 成立時のグリッパー閉じ動作だけは残し、それ以外は警告だけ出してループ継続。停止判断は Ctrl+C / ESC で手動で行う

修正前の該当箇所：

if no_motion_count >= NO_MOTION_LIMIT:
    if GRIP_ON_NEAR_TARGET_LOST and is_grip_ready(*last_good_diff):
        print("[GRIP] 近傍到達後に実測変化が止まりました。"
              "ABORTせずグリッパーを閉じて保持します。")
        daemon_set_gripper(1, speed=100, wait=0)
        time.sleep(2.5)
        abort_due_to_no_motion = "gripped_hold"
    elif selected_axis == "Y" and y_correction_allowed:
        print("[HOLD] 高さ合わせ中に実測変化が止まりました。"
              "HOMEへ戻さず現在姿勢で保持します。")
        abort_due_to_no_motion = "hold_no_home"
    else:
        print("[ABORT] 指令を出しても実測角度が変化しません。daemon/電源/関節上限を確認してください。")
        abort_due_to_no_motion = "aborted"

修正後：

if no_motion_count >= NO_MOTION_LIMIT:
    if GRIP_ON_NEAR_TARGET_LOST and is_grip_ready(*last_good_diff):
        print(
            "[GRIP] 近傍到達後に実測変化が止まりました。"
            "ABORTせずグリッパーを閉じて保持します。"
        )
        daemon_set_gripper(1, speed=100, wait=0)
        time.sleep(2.5)
        abort_due_to_no_motion = "gripped_hold"
    else:
        # DAY205: no_motion で自動 ABORT しない。
        # ユーザーが Ctrl+C / ESC で止めるまでループを継続する。
        print(
            f"  [WARN] 実測変化なし {no_motion_count} 回連続。"
            "ABORTせずループを継続します（停止するには Ctrl+C または ESC）。"
        )
        no_motion_count = 0

"hold_no_home" 分岐と "aborted" 分岐を削除。GRIP_READY 成立時のみ閉じる。それ以外は WARN だけ出して no_motion_count をリセットしてループ継続。abort_due_to_no_motion は False のままなので、後段 L773 の if abort_due_to_no_motion: return を通らない。

実機ログでこの変更後の挙動が見える。

[LOOP 6] diff_x=+1.53mm OK  diff_z=+8.37mm NG  diff_y=+186pix NG
  [WARN] 実測変化が小さすぎます: max_step=0.00° (3/3)
  [WARN] 実測変化なし 3 回連続。ABORTせずループを継続します（停止するには Ctrl+C または ESC）。

[LOOP 7] diff_x=+5.49mm NG  diff_z=+9.08mm NG  diff_y=+194pix NG

ABORT せずに LOOP 7, 8, 9 とループが続いた。これが効いた。

壁 4：Z 閾値 8mm が物理現実より厳しすぎた（22:10 〜 22:13）

Y 補正まで通って、ようやく PICK 位置の至近に来た。22:00 台の実機ログから：

[LOOP 19] diff_x=+2.5mm  diff_z=+10.34mm  diff_y=+0pix
side cam: TIP(667,861)  BLOCK(660,861)

X≤6 OK。Y=0pix で完璧。Z=+10.34mm で詰まっている。j2 を回しても max_step=0.00° で動かない。サイドカメラ画像を見ると、TIP（爪先）と BLOCK（赤丸）はピクセル座標がほぼ完全に一致。爪はブロックを左右から挟む位置にいる。

ここで二つの確認をした。

ひとつは座標系の確認。diff_z は上カメラのホモグラフィ変換後の値で、Unity 座標系における「奥行き」方向の差分。Y 軸（高さ）はサイドカメラの diff_y_pix として独立に取得している。Pi カメラ上からの見下ろし視点で Unity Z+ = 画面下（手前）、Z- = 画面上（奥）。サイドカメラで TIP と BLOCK の Y 座標が一致している（=同じ高さにいる）状態で、上カメラの diff_z=+10.34mm は爪先がブロックより 10mm 手前 or 奥にズレているという意味になる。

もうひとつは物理的な確認。サイドカメラの画像を見ると、TIP(667, 861) と BLOCK(660, 861) のピクセル座標が 7 ピクセル差。横方向 7 ピクセル、縦方向は完全に一致。アダプティブグリッパーの開口幅は ±15mm、ブロックは 30mm 角。中心から ±10mm のズレなら挟める。10mm の奥行きズレは、グリッパーが「真上からブロックを挟む」位置にいる限り、把持成立に対しては許容範囲。

つまり数値としての diff_z=+10.34mm は閾値（8.0）を超えているが、物理的にはブロックを掴める範囲にいた。閾値の方が実際の把持可能範囲より厳しすぎた。

そこで指示を出した：

z閾値を緩める（8mm → 12mmとか）→ ループ抜けさせる→閉じる
でやってみる？

修正は 1 行だけだった。

GRIP_READY_X_MM = 6.0
GRIP_READY_Z_MM = 12.0   # 旧 8.0 → 12.0
GRIP_READY_Y_PIX = 12

xyz_track_test.py を起動するとバナー出力で確認できる：

============================================================
XYZ_TRACK 単体検証スクリプト (DAY205)
[VERSION] XZ-first + blended Y descent: j0_step=8.0 speed=80, Y starts at X<=6.0mm Z<=8.0mm, j4_step=2.0 j2Y_step=1.0, grip X<=6.0mm Z<=12.0mm Y<=12px
============================================================

grip X<=6.0mm Z<=12.0mm Y<=12px で新閾値が反映されている。

実機を回した。

21 ループで爪が閉じた

成功した実機実行（22:13:13 セッション）の全 21 ループの CSV を全量見る。

loop,diff_x_mm,diff_z_mm,diff_y_pix,selected_axis,delta_j0,delta_j1,delta_j2,delta_j4_for_Z,delta_j4_for_Y,delta_j4_total,actual_step_deg,new_angles,measured,reach_ok,converged
0,54.504112243652344,-2.2642211914061363,200,X,-8.0,0.0,0.0,0.0,0.0,0.0,0.0,"[-8.61, 88.94, -18.8, 2.9, -87.36, -29.97]","[-0.61, 88.94, -18.8, 2.98, -87.36, -29.97]",False,False
1,4.984058380126939,6.6349945068359375,212,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.1400000000000006,"[-0.61, 88.94, -18.8, 2.98, -85.36, -29.97]","[-8.87, 88.94, -18.8, 3.86, -88.5, -29.97]",False,False
2,5.231845855712891,6.633697509765625,197,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.4899999999999949,"[-8.87, 88.94, -18.8, 3.86, -86.5, -29.97]","[-8.87, 88.94, -18.8, 3.86, -87.01, -29.97]",True,False
3,5.231845855712891,6.633697509765625,196,Y,0.0,0.0,0.0,0.0,2.0,2.0,0.9699999999999989,"[-8.87, 88.94, -18.8, 3.86, -85.01, -29.97]","[-8.87, 88.94, -18.8, 3.86, -86.04, -29.97]",True,False
4,5.233509063720689,6.88031005859375,186,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.3200000000000074,"[-8.87, 88.94, -18.8, 3.86, -84.04, -29.97]","[-8.87, 88.94, -18.8, 3.86, -84.72, -29.97]",True,False
5,5.481296539306641,6.8790130615234375,171,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.5799999999999983,"[-8.87, 88.94, -18.8, 3.86, -82.72, -29.97]","[-8.87, 88.94, -18.8, 3.86, -83.14, -29.97]",True,False
6,5.729084014892578,6.877716064453125,155,Y,0.0,0.0,0.0,0.0,2.0,2.0,0.0,"[-8.87, 88.94, -18.8, 3.86, -81.14, -29.97]","[-8.87, 88.94, -18.8, 3.86, -83.14, -29.97]",True,False
7,5.233676910400391,6.88037109375,155,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.6700000000000017,"[-8.87, 88.94, -18.8, 3.86, -81.14, -29.97]","[-8.87, 88.94, -18.8, 3.86, -81.47, -29.97]",True,False
8,5.4840850830078125,7.372589111328125,136,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.6700000000000017,"[-8.87, 88.94, -18.8, 3.86, -79.47, -29.97]","[-8.87, 88.94, -18.8, 3.86, -79.8, -29.97]",True,False
9,6.229913711547852,7.8621826171875,118,X,-0.9396551601127981,0.0,0.0,0.0,0.0,0.0,1.1400000000000006,"[-9.809655160112797, 88.94, -18.8, 3.86, -79.8, -29.97]","[-10.01, 88.94, -18.8, 3.77, -79.8, -29.97]",True,False
10,0.5371494293212891,9.373275756835938,120,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.6700000000000017,"[-10.01, 88.94, -18.8, 3.77, -77.8, -29.97]","[-10.01, 88.94, -18.8, 3.77, -78.13, -29.97]",True,False
11,0.7874565124511719,9.8656005859375,102,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.759999999999991,"[-10.01, 88.94, -18.8, 3.77, -76.13, -29.97]","[-10.01, 88.94, -18.8, 3.77, -76.37, -29.97]",True,False
12,1.2872123718261577,10.603317260742188,84,Y,0.0,0.0,0.0,0.0,2.0,2.0,0.0,"[-10.01, 88.94, -18.8, 3.77, -74.37, -29.97]","[-10.01, 88.94, -18.8, 3.77, -76.37, -29.97]",True,False
13,1.2872123718261577,10.603317260742188,84,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.7600000000000051,"[-10.01, 88.94, -18.8, 3.77, -74.37, -29.97]","[-10.01, 88.94, -18.8, 3.77, -74.61, -29.97]",True,False
14,1.5400733947753764,11.589370727539062,66,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.8400000000000034,"[-10.01, 88.94, -18.8, 3.77, -72.61, -29.97]","[-10.01, 88.94, -18.8, 3.77, -72.77, -29.97]",True,False
15,2.288665771484375,12.57293701171875,49,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.759999999999991,"[-10.01, 88.94, -18.8, 3.77, -70.77, -29.97]","[-10.01, 88.94, -18.8, 3.77, -71.01, -29.97]",True,False
16,2.5432052612304688,13.80596923828125,23,Z,0.0,0.0,-2.0,0.0,0.0,0.0,0.9699999999999989,"[-10.01, 88.94, -20.8, 3.77, -71.01, -29.97]","[-10.01, 88.94, -19.77, 3.77, -71.01, -29.97]",True,False
17,2.0332183837890625,11.093109130859432,39,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.8500000000000085,"[-10.01, 88.94, -19.77, 3.77, -69.01, -29.97]","[-10.01, 88.94, -19.77, 3.77, -69.16, -29.97]",True,False
18,3.278839111328139,12.320724487304744,16,Z,0.0,0.0,-2.0,0.0,0.0,0.0,0.879999999999999,"[-10.01, 88.94, -21.77, 3.77, -69.16, -29.97]","[-10.01, 88.94, -20.65, 3.77, -69.16, -29.97]",True,False
19,2.522644042968764,9.856216430664062,32,Y,0.0,0.0,0.0,0.0,2.0,2.0,1.8400000000000034,"[-10.01, 88.94, -20.65, 3.77, -67.16, -29.97]","[-10.01, 88.94, -20.65, 3.77, -67.32, -29.97]",True,False
20,2.778339385986314,11.335922241210994,6,GRIP_READY,0.0,0.0,0.0,0.0,0.0,0.0,0.0,"[-10.01, 88.94, -20.65, 3.77, -67.32, -29.97]","[-10.01, 88.94, -20.65, 3.77, -67.32, -29.97]",True,False

最後の行、LOOP 20 で selected_axis=GRIP_READY が記録されている。これがグリッパー閉じが発火した瞬間のログだ。3 軸の値は diff_x=+2.78, diff_z=+11.34, diff_y=+6。新閾値 (6, 12, 12) の中に全部入っている。

21 ループの内訳を整理する。

21 ループの軌跡を解析して、見えてきた事実がいくつかある。

まず LOOP 0 の異常値（diff_x=54.5mm）。これは APPROACH_BASE 直後の初期誤差で、j0 を -8° 動かす一発の指令で 5mm まで詰まっている。粗合わせは速い。

次に Z 軸。selected_axis=Z で能動的に動かしたのは LOOP 16 と LOOP 18 の 2 回だけ。残りは Y 軸の j4 動作の副作用で Z が揺れているだけだ。LOG207 で確定した「j4 単独で Y 補正可能、j2 との連動は不要」という設計判断と整合する。j4 が動くと姿勢の幾何学的な構造から Z 方向にも影響が出るが、その量は j2 の単独動作の 5〜13 倍小さい。

決め手は diff_y_pix=6 だった。LOOP 19 で 32pix、LOOP 20 で一気に 6pix に下がって発火。最後の 1 ピースは Y だった。

Z 閾値 12mm の妥当性についてデータを見ると、diff_z は 6〜14mm の範囲で振動している。真値は 8〜11mm 付近に集中。8mm 閾値はこのレンジを取り切れない。12mm はギリギリ妥当な値だった。これは「Y 下降中の Z 副作用変動」を吸収する閾値であって、本来 Z が揃っている訳ではない。

CLOSE 発火時のターミナルログ：

[LOOP 20] diff_x=+2.78mm OK  diff_z=+11.34mm OK  diff_y=+6pix OK
  [GRIP_CHECK] x_ok=True(2.78<=6.0)  z_ok=True(11.34<=12.0)  y_ok=True(6<=12)  => True
[GRIP] 掴める近傍に到達。厳密収束を待たずグリッパーを閉じて保持します。
  [Pi] GRIPPER: CLOSE result= None

[Pi] GRIPPER: CLOSE result= None は daemon の _mc.set_gripper_state(1, 100) の戻り値が None になっただけで、グリッパー動作自体は成功した（実機の爪は閉じてブロックを保持）。pymycobot の set_gripper_state は環境によって戻り値が None / 0 / 1 のいずれかになる仕様で、None でも実行は成立する。

修正後の xyz_track_test.py フルコード

最終的に動作した xyz_track_test.py のフルコードを掲載する。GRIP_READY_Z_MM=12.0、Y_CORRECTION_START_X_MM=6.0、Y_CORRECTION_START_Z_MM=8.0、no_motion 自動 ABORT 廃止、軸選択ロジックは X 優先のみ、というすべての修正を含めた版。

このコードのコアは「軸を 1 つずつ動かして、X と Y のどちらを優先するかをスコアで決める」ロジックにある。Z は副次。閾値は数字で書いてあるが、決定したのは設計判断のほうだ。

#!/usr/bin/env python3
"""
xyz_track_test.py (DAY205)

mechArm 270 Pi の XYZ_TRACK フェーズ単体検証スクリプト。

フロー:
    HOME → OPEN → (APPROACH_BASE) → XYZ_TRACK ループ → HOME 復帰

GRIP / LIFT は接続しない。XYZ_TRACK ループが 3 軸収束まで到達できるかを
単体で検証する。visual_servo_pick.py には統合せず、関数のみ import する。

恒久ルール:
    - send_coords() 使用禁止 → daemon_send_angles() のみ
    - daemon の wait 引数は機能しないため Mac 側で time.sleep() を明示
    - homography.npy / fk_scan_motion_based.csv を変更しない
"""

import os
import csv
import time
from datetime import datetime

import cv2

# ---- 既存モジュールから関数/定数を import -----------------------------
from visual_servo_pick import (
    load_fk_table,
    setup_homography_and_mat_range,
    detect_red_block_unity,
    detect_tip_from_frame,
    daemon_send_angles,
    daemon_send_angle,
    daemon_get_angles,
    daemon_set_gripper,
    daemon_set_fresh_mode,
    get_daemon_socket,
    close_daemon_socket,
    send_home,
    SIDE_CAMERA_INDEX,
    APPROACH_PICK_BASE_DAY185,
    HOME,
    MM_PER_DEG_J0_X,
    MM_PER_DEG_ELBOW_Z_DAY189,
    SPEED,
    detect_red_block,
    draw_tracking_overlay,
)
from side_camera_utils import detect_tip_side, detect_red_block_side
from auto_detect_cameras import auto_detect_cameras


# ============================================================
# 設計定数
# ============================================================

# ---- 基準姿勢 ----------------------------------------------------------
APPROACH_BASE = [0.0, 87.74, -20.0, 3.6, -90.0, -30.0]   # APPROACH_PICK_BASE_DAY185 相当
HOME_ANGLES = [0.0, 0.0, 0.0, 0.0, 0.0, -30.0]

# ---- ループ周期・待機 --------------------------------------------------
LOOP_INTERVAL_SEC = 1.5       # 1500ms (DAY205実機知見: 0.2sではアームが到達前に次指令)
WAIT_SEC = 2.0                # 通常動作後の待機
APPROACH_WAIT_SEC = 12.0      # APPROACH_BASE 到達待ち。短いと移動中に追従が始まり衝突する
HOME_WAIT_SEC = 6.0           # HOME 復帰待機（j1 大移動 87.74° に対応）
TRACK_SPEED = 50              # XYZ_TRACK中は静止摩擦・重力負荷対策で高めに送る
X_TRACK_SPEED = 80            # APPROACH後の水平合わせはブロック接触リスクが低いため速める
Y_TRACK_SPEED = 50            # j4=-90°付近はspeed20/1°だと静止摩擦で動かないことがある

# ---- 収束許容値 --------------------------------------------------------
TOL_X_MM = 2.0                # X 収束許容（±mm）
TOL_Z_MM = 2.0                # Z 収束許容（±mm）
TOL_Y_PIX = 5                 # Y 収束許容（±pixel）

# ---- 安全機構 ----------------------------------------------------------
MAX_DELTA_DEG_PER_STEP = 5.0  # 1ループあたりの最大移動量（全関節共通）
MAX_DELTA_J0_PER_STEP = 8.0   # X方向の粗合わせは遅すぎたため j0 だけ大きめに許可
MAX_DELTA_Z_DEG_PER_STEP = 2.0
MAX_DELTA_J4_PER_STEP = 2.0
MAX_DELTA_J2_Y_PER_STEP = 1.0 # 高さ合わせ時、j4にj2を小さく協調させる
MAX_LOOPS = 100               # 暴走防止
DETECT_FAIL_LIMIT = 3         # 検出失敗連続 N 回で ABORT
MIN_COMMAND_DEG = 1.0         # |Δ| < 1° は不感帯で動かさない（DAY204 知見）
MIN_COMMAND_J0_DEG = 0.4      # X最終合わせ用。1°だと約6.6mm未満を詰められない
MIN_EFFECTIVE_MOTION_DEG = 0.35  # これ未満なら目視/実測とも「動いていない」扱い
NO_MOTION_LIMIT = 3           # 実測変化なしが連続したら ABORT
SINGLE_AXIS_DEBUG = True      # 1ループ1軸だけ動かし、詰まる関節を切り分ける
GRIP_ON_NEAR_TARGET_LOST = True
GRIP_READY_X_MM = 6.0       # 4.01mmで掴めず微修正に入り続けたため、検出ノイズ込みで許容
GRIP_READY_Z_MM = 12.0      # DAY208: 8.0 → 12.0 (Y下降中のZ副作用変動を吸収)
GRIP_READY_Y_PIX = 12
Y_CORRECTION_START_X_MM = 6.0  # DAY208: 4.0 → 6.0 (GRIP_READY と揃える)
Y_CORRECTION_START_Z_MM = 8.0  # DAY208: 6.0 → 8.0 (GRIP_READY と揃える)

# ---- 関節別 safety limit (DAY205) -------------------------------------
# 公式 docs と実機実績（DAY204 APPROACH_BASE 動作実績）の両方を踏まえた保守的値。
# - j1: 87.74° 実機動作実績、+12° マージン
# - j4: -90° 実機動作実績、±10° マージン
# - j2: DAY204 範囲 -20°〜-2° は範囲内
JOINT_LIMITS = [
    (-160.0, 160.0),    # j0
    ( -90.0, 100.0),    # j1
    (-170.0,  40.0),    # j2
    (-150.0, 150.0),    # j3
    (-100.0, 100.0),    # j4
    (-180.0, 180.0),    # j5
]

# ---- 到達確認 ----------------------------------------------------------
TOL_REACH_DEG_ACTIVE = 2.0    # 動かす関節の到達許容（DAY205: 3°指令で実測誤差 1.25°）
TOL_REACH_DEG_STATIC = 2.5    # 動かさない関節の到達許容（j1 重力ドリフト +1° 許容）
TOL_APPROACH_DEG = 5.0        # APPROACH待ちの許容。実機ドリフト込みで広めに見る

# ---- XYZ_TRACK中の動的ソフト上限 ----------------------------------------
# 2026-05-18実機ログで j1 は 90.79° 付近から上に動かず、93.79° 指令を
# 出すと他関節も止まった。XYZ_TRACK中は j1 を押し上げ続けず、Z補正は
# j2 側に逃がす。
J1_TRACK_SOFT_MIN = -90.0
J1_TRACK_SOFT_MAX = 91.0


# ============================================================
# 補正係数（Y: j4 単独 / 線形補間）
# ============================================================

def dy_per_degree_j4(j2):
    """j4 を 1° 動かしたときの横カメラ Y 方向 pixel 変化。

    j2 値（姿勢）で線形補間する。範囲外は [-20.0, -2.0] にクリップ。
    DAY204 の線形補間モデル。
    """
    j2_clipped = max(-20.0, min(-2.0, j2))
    return 6.0 - 0.222 * (j2_clipped + 2)


def clamp_delta_to_range(current, delta, lo, hi):
    """現在角度 + delta が範囲内に収まるよう delta だけをクリップする。"""
    target = max(lo, min(hi, current + delta))
    return target - current


def is_grip_ready(diff_x_mm, diff_z_mm, diff_y_pix):
    """検出が消えても掴みに行ってよい近傍状態かを判定する。"""
    if diff_x_mm is None or diff_z_mm is None or diff_y_pix is None:
        return False
    return (
        abs(diff_x_mm) <= GRIP_READY_X_MM
        and abs(diff_z_mm) <= GRIP_READY_Z_MM
        and abs(diff_y_pix) <= GRIP_READY_Y_PIX
    )


def wait_for_pose(target_angles, label, timeout_sec, tolerance_deg):
    """目標姿勢に到達するまで待つ。未到達なら追跡へ入らず止めるための安全待ち。"""
    deadline = time.time() + timeout_sec
    last_measured = None

    while time.time() < deadline:
        measured = daemon_get_angles()
        if measured is not None and len(measured) == 6:
            last_measured = list(measured)
            max_err = max(
                abs(last_measured[i] - target_angles[i]) for i in range(6)
            )
            print(
                f"  [WAIT] {label}: max_err={max_err:.2f}° "
                f"measured={[round(a, 2) for a in last_measured]}"
            )
            if max_err <= tolerance_deg:
                return last_measured, True
        else:
            print(f"  [WAIT] {label}: daemon_get_angles 失敗")
        time.sleep(0.5)

    return last_measured, False


# ============================================================
# CSV 保存
# ============================================================

def save_loop_log(loop_log):
    """ループログを CSV に保存する。"""
    ts = datetime.now().strftime("%Y%m%d_%H%M%S")
    csv_path = os.path.expanduser(
        f"~/r2r2r-scripts/logs/xyz_track_test_{ts}.csv"
    )
    os.makedirs(os.path.dirname(csv_path), exist_ok=True)

    fields = [
        "loop", "diff_x_mm", "diff_z_mm", "diff_y_pix",
        "selected_axis",
        "delta_j0", "delta_j1", "delta_j2",
        "delta_j4_for_Z", "delta_j4_for_Y", "delta_j4_total",
        "actual_step_deg", "new_angles", "measured", "reach_ok", "converged",
    ]
    with open(csv_path, "w", newline="") as f:
        w = csv.DictWriter(f, fieldnames=fields, extrasaction="ignore")
        w.writeheader()
        for row in loop_log:
            row_out = dict(row)
            if row_out.get("new_angles") is not None:
                row_out["new_angles"] = str(row_out["new_angles"])
            if row_out.get("measured") is not None:
                row_out["measured"] = str(row_out["measured"])
            w.writerow(row_out)
    print(f"\n[CSV保存] {csv_path} ({len(loop_log)} ループ)")


# ============================================================
# XYZ_TRACK 単体検証フロー
# ============================================================

def run_xyz_track_test(cap_top, cap_side):
    """1 試行の XYZ_TRACK 単体検証フロー。

    HOME へは戻さない（呼び出し元の finally で実施）。
    """

    # ---- (a) HOME へ ----
    print("\n[ARM] HOME へ移動")
    daemon_send_angles(HOME_ANGLES, speed=SPEED, wait=0)
    time.sleep(HOME_WAIT_SEC)

    # ---- (b) グリッパー OPEN ----
    print("[ARM] グリッパー OPEN")
    daemon_set_gripper(0, speed=100, wait=0)
    time.sleep(2.5)

    # ---- (c) ユーザーに赤ブロック設置を促す ----
    input("\n>>> 赤ブロックをマット上に置いて Enter を押してください: ")

    # ---- (d) APPROACH_BASE へ移動 ----
    print(f"\n[ARM] APPROACH_BASE へ移動: {APPROACH_BASE}")
    daemon_send_angles(APPROACH_BASE, speed=SPEED, wait=0)

    # ---- (e) 初期状態のログ出力 ----
    measured, approach_reached = wait_for_pose(
        APPROACH_BASE,
        "APPROACH_BASE",
        APPROACH_WAIT_SEC,
        TOL_APPROACH_DEG,
    )
    print(f"[ARM] 実関節角度: {measured}")
    if measured is None or len(measured) != 6:
        print("[ABORT] 初期姿勢の関節角度取得失敗")
        return "aborted"
    if not approach_reached:
        print(
            "[ABORT] APPROACH_BASE に到達していません。"
            "このまま追跡すると低い姿勢でブロックへ当たるため中止します。"
        )
        return "aborted"
    current_angles = list(measured)

    # ---- (f) XYZ_TRACK ループ開始 ----
    print("\n" + "=" * 60)
    print("XYZ_TRACK ループ開始")
    print("=" * 60)

    detect_fail_count = 0
    no_motion_count = 0
    last_good_diff = None
    loop_log = []

    for loop_idx in range(MAX_LOOPS):
        # ---- (f-1) 4 つの観測値を取得 ----
        # 上カメラ TIP
        for _ in range(2):
            cap_top.read()
        ret_t, frame_t = cap_top.read()
        if not ret_t:
            print(f"[LOOP {loop_idx}] 上カメラ read 失敗")
            detect_fail_count += 1
            if detect_fail_count >= DETECT_FAIL_LIMIT:
                print(f"[ABORT] 検出失敗 {DETECT_FAIL_LIMIT} 回連続")
                save_loop_log(loop_log)
                return "aborted"
            time.sleep(LOOP_INTERVAL_SEC)
            continue

        # detect_tip_from_frame: blue_markers も保持する（プレビュー描画で使用）
        tip_pixel_top, blue_markers_top, tip_unity_top = detect_tip_from_frame(frame_t)
        # detect_red_block: 同じ frame_t から pixel 座標で取得（プレビュー描画用）
        red_pixel_top = detect_red_block(frame_t)
        # detect_red_block_unity: Unity 座標で取得（既存ロジック互換のため残す）
        red_unity = detect_red_block_unity(cap_top)

        # 横カメラ TIP / BLOCK
        for _ in range(2):
            cap_side.read()
        ret_s, frame_s = cap_side.read()
        if not ret_s:
            print(f"[LOOP {loop_idx}] 横カメラ read 失敗")
            detect_fail_count += 1
            if detect_fail_count >= DETECT_FAIL_LIMIT:
                print(f"[ABORT] 検出失敗 {DETECT_FAIL_LIMIT} 回連続")
                save_loop_log(loop_log)
                return "aborted"
            time.sleep(LOOP_INTERVAL_SEC)
            continue

        tip_pixel_side, _, _ = detect_tip_side(frame_s)
        block_side = detect_red_block_side(frame_s)

        # ---- (f-2) 検出失敗チェック ----
        if (tip_unity_top is None or red_unity is None
                or tip_pixel_side is None or block_side is None):
            print(
                f"[LOOP {loop_idx}] 検出失敗: "
                f"tip_top={tip_unity_top is not None} "
                f"red_top={red_unity is not None} "
                f"tip_side={tip_pixel_side is not None} "
                f"block_side={block_side is not None}"
            )
            if (GRIP_ON_NEAR_TARGET_LOST and last_good_diff is not None
                    and tip_unity_top is not None and red_unity is not None
                    and block_side is not None
                    and is_grip_ready(*last_good_diff)):
                print(
                    "[GRIP] 近傍到達後に横TIPを見失いました。"
                    "ABORTせずグリッパーを閉じて保持します。"
                )
                daemon_set_gripper(1, speed=100, wait=0)
                time.sleep(2.5)
                save_loop_log(loop_log)
                return "gripped_hold"
            detect_fail_count += 1
            if detect_fail_count >= DETECT_FAIL_LIMIT:
                print(f"[ABORT] 検出失敗 {DETECT_FAIL_LIMIT} 回連続")
                save_loop_log(loop_log)
                return "aborted"
            time.sleep(LOOP_INTERVAL_SEC)
            continue
        detect_fail_count = 0

        # ---- (f-3) 差分計算 ----
        tip_x_mm = tip_unity_top[0] * 1000
        tip_z_mm = tip_unity_top[1] * 1000
        red_x_mm = red_unity[0] * 1000
        red_z_mm = red_unity[1] * 1000
        tip_y_pix = tip_pixel_side[1]
        block_y_pix = block_side["center_y"]

        diff_x_mm = red_x_mm - tip_x_mm
        diff_z_mm = red_z_mm - tip_z_mm
        diff_y_pix = block_y_pix - tip_y_pix
        last_good_diff = (diff_x_mm, diff_z_mm, diff_y_pix)

        # ---- (f-3.5) プレビュー描画（目視確認用） ----
        status_text = (
            f"LOOP {loop_idx}\n"
            f"diff_x={diff_x_mm:+.1f}mm  diff_z={diff_z_mm:+.1f}mm\n"
            f"diff_y={diff_y_pix:+d}pix"
        )
        overlay_top = draw_tracking_overlay(
            frame_t,
            red_pixel=red_pixel_top,
            blue_markers=blue_markers_top,
            tip_pixel=tip_pixel_top,
            red_xz_mm=(red_x_mm, red_z_mm),
            tip_xz_mm=(tip_x_mm, tip_z_mm),
            status_text=status_text,
        )
        cv2.imshow("R2R2R Tracking", overlay_top)

        # 横カメラ: TIP（緑塗りつぶし円 + 緑ラベル）
        if tip_pixel_side is not None:
            cv2.circle(frame_s, (int(tip_pixel_side[0]), int(tip_pixel_side[1])),
                       8, (0, 255, 0), -1)
            cv2.putText(frame_s,
                        f"TIP ({int(tip_pixel_side[0])}, {int(tip_pixel_side[1])})",
                        (int(tip_pixel_side[0]) + 12, int(tip_pixel_side[1])),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)

        # 横カメラ: BLOCK（赤円 + 赤ラベル）
        if block_side is not None:
            bx, by = block_side["center_x"], block_side["center_y"]
            cv2.circle(frame_s, (bx, by), 15, (0, 0, 255), 2)
            cv2.putText(
                frame_s,
                f"BLOCK ({bx}, {by})",
                (bx + 20, by - 15),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.7,
                (0, 0, 255),
                2,
            )

        # 横カメラ: 状態テキスト（左上）
        cv2.putText(frame_s,
                    f"LOOP {loop_idx} | diff_y={diff_y_pix:+d}pix",
                    (15, 30),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2, cv2.LINE_AA)

        cv2.imshow("R2R2R Side Tracking", frame_s)

        # waitKey(1) で描画を反映、ESC で中断
        key = cv2.waitKey(1) & 0xFF
        if key == 27:  # ESC
            print("[ESC] プレビューウィンドウから ESC 検出。HOME 復帰します。")
            raise KeyboardInterrupt

        # ---- (f-4) 収束判定 ----
        converged_x = abs(diff_x_mm) <= TOL_X_MM
        converged_z = abs(diff_z_mm) <= TOL_Z_MM
        converged_y = abs(diff_y_pix) <= TOL_Y_PIX

        print(
            f"\n[LOOP {loop_idx}] "
            f"diff_x={diff_x_mm:+.2f}mm {'OK' if converged_x else 'NG'}  "
            f"diff_z={diff_z_mm:+.2f}mm {'OK' if converged_z else 'NG'}  "
            f"diff_y={diff_y_pix:+d}pix {'OK' if converged_y else 'NG'}"
        )

        # GRIP_READY 判定（インライン式）
        _gr_x_ok = abs(diff_x_mm) <= GRIP_READY_X_MM
        _gr_z_ok = abs(diff_z_mm) <= GRIP_READY_Z_MM
        _gr_y_ok = abs(diff_y_pix) <= GRIP_READY_Y_PIX
        _grip_ready_inline = _gr_x_ok and _gr_z_ok and _gr_y_ok
        print(
            f"  [GRIP_CHECK] x_ok={_gr_x_ok}({abs(diff_x_mm):.2f}<={GRIP_READY_X_MM})  "
            f"z_ok={_gr_z_ok}({abs(diff_z_mm):.2f}<={GRIP_READY_Z_MM})  "
            f"y_ok={_gr_y_ok}({abs(diff_y_pix)}<={GRIP_READY_Y_PIX})  "
            f"=> {_grip_ready_inline}"
        )
        if GRIP_ON_NEAR_TARGET_LOST and _grip_ready_inline:
            print("[GRIP] 掴める近傍に到達。厳密収束を待たずグリッパーを閉じて保持します。")
            daemon_set_gripper(1, speed=100, wait=0)
            time.sleep(2.5)
            loop_log.append({
                "loop": loop_idx,
                "diff_x_mm": diff_x_mm,
                "diff_z_mm": diff_z_mm,
                "diff_y_pix": diff_y_pix,
                "selected_axis": "GRIP_READY",
                "delta_j0": 0.0,
                "delta_j1": 0.0,
                "delta_j2": 0.0,
                "delta_j4_for_Z": 0.0,
                "delta_j4_for_Y": 0.0,
                "delta_j4_total": 0.0,
                "actual_step_deg": 0.0,
                "new_angles": current_angles,
                "measured": current_angles,
                "reach_ok": True,
                "converged": False,
            })
            save_loop_log(loop_log)
            return "gripped_hold"

        if converged_x and converged_z and converged_y:
            print(f"\n[CONVERGED] 3軸収束達成 (loop={loop_idx})")
            print("[GRIP] 3軸収束。グリッパーを閉じて保持します。")
            daemon_set_gripper(1, speed=100, wait=0)
            time.sleep(2.5)
            loop_log.append({
                "loop": loop_idx,
                "diff_x_mm": diff_x_mm,
                "diff_z_mm": diff_z_mm,
                "diff_y_pix": diff_y_pix,
                "selected_axis": "DONE",
                "delta_j0": 0.0,
                "delta_j1": 0.0,
                "delta_j2": 0.0,
                "delta_j4_for_Z": 0.0,
                "delta_j4_for_Y": 0.0,
                "delta_j4_total": 0.0,
                "actual_step_deg": 0.0,
                "new_angles": None,
                "measured": current_angles,
                "reach_ok": None,
                "converged": True,
            })
            save_loop_log(loop_log)
            return "gripped_hold"

        # ---- (f-5) 補正量計算 ----
        # X: j0 単独 (MM_PER_DEG_J0_X = -6.63)
        delta_j0 = 0.0
        if not converged_x:
            delta_j0 = diff_x_mm / MM_PER_DEG_J0_X
            delta_j0 = max(-MAX_DELTA_J0_PER_STEP,
                           min(MAX_DELTA_J0_PER_STEP, delta_j0))

        # Z: j1 + j2 連動
        delta_j1 = 0.0
        delta_j2 = 0.0
        delta_j4_for_Z = 0.0
        if not converged_z:
            elbow_delta = diff_z_mm / MM_PER_DEG_ELBOW_Z_DAY189
            elbow_delta = max(-MAX_DELTA_Z_DEG_PER_STEP,
                              min(MAX_DELTA_Z_DEG_PER_STEP, elbow_delta))
            delta_j1 = -elbow_delta
            delta_j2 = -elbow_delta

        # Y: j4 単独（j2 値で線形補間）
        delta_j4_for_Y = 0.0
        delta_j2_for_Y = 0.0
        if not converged_y:
            current_j2 = current_angles[2]
            sensitivity = dy_per_degree_j4(current_j2)
            delta_j4_for_Y = diff_y_pix / sensitivity
            delta_j4_for_Y = max(-MAX_DELTA_J4_PER_STEP,
                                 min(MAX_DELTA_J4_PER_STEP, delta_j4_for_Y))
            if diff_y_pix > GRIP_READY_Y_PIX and diff_z_mm < 0:
                delta_j2_for_Y = min(
                    MAX_DELTA_J2_Y_PER_STEP,
                    max(0.5, abs(diff_y_pix) / 180.0),
                )

        # j4 合算
        delta_j4_total = delta_j4_for_Z + delta_j4_for_Y
        delta_j4_total = max(-MAX_DELTA_J4_PER_STEP,
                             min(MAX_DELTA_J4_PER_STEP, delta_j4_total))

        # j1 ソフト上限クランプ
        requested_delta_j1 = delta_j1
        delta_j1 = clamp_delta_to_range(
            current_angles[1], delta_j1, J1_TRACK_SOFT_MIN, J1_TRACK_SOFT_MAX
        )
        if abs(delta_j1 - requested_delta_j1) > 1e-6:
            print(
                f"  [CLAMP] j1 Δ {requested_delta_j1:+.2f} -> {delta_j1:+.2f} "
                f"(current={current_angles[1]:.2f}, soft_max={J1_TRACK_SOFT_MAX:.2f})"
            )

        # ---- (f-6) 不感帯処理 ----
        if abs(delta_j0) < MIN_COMMAND_J0_DEG:
            delta_j0 = 0.0
        if abs(delta_j1) < MIN_COMMAND_DEG:
            delta_j1 = 0.0
        if abs(delta_j2) < MIN_COMMAND_DEG:
            delta_j2 = 0.0
        if abs(delta_j4_total) < MIN_COMMAND_DEG:
            delta_j4_total = 0.0

        # ---- 軸選択 (DAY208 修正版) ----
        selected_axis = "XYZ"
        selected_joint_index = None
        if SINGLE_AXIS_DEBUG:
            candidates = []
            if delta_j0 != 0.0 and not converged_x:
                candidates.append(("X", abs(diff_x_mm) / TOL_X_MM, 0))
            if (delta_j1 != 0.0 or delta_j2 != 0.0) and not converged_z:
                candidates.append(("Z", abs(diff_z_mm) / TOL_Z_MM, 2))
            # DAY208: Y 補正開始条件から Z を外す。X だけ揃えば Y を走らせる。
            y_correction_allowed = (
                abs(diff_x_mm) <= Y_CORRECTION_START_X_MM
            )
            if ((delta_j4_total != 0.0 or delta_j2_for_Y != 0.0)
                    and not converged_y and y_correction_allowed):
                candidates.append(("Y", abs(diff_y_pix) / TOL_Y_PIX, 4))

            if candidates:
                candidates_by_axis = {item[0]: item for item in candidates}
                # DAY208: X 優先のみ。Z 優先分岐削除。残りはスコア最大選択
                if abs(diff_x_mm) > Y_CORRECTION_START_X_MM and "X" in candidates_by_axis:
                    selected_axis, _, selected_joint_index = candidates_by_axis["X"]
                else:
                    selected_axis, _, selected_joint_index = max(
                        candidates, key=lambda item: item[1]
                    )
                if selected_axis != "X":
                    delta_j0 = 0.0
                if selected_axis != "Z":
                    delta_j1 = 0.0
                    delta_j2 = 0.0
                    delta_j4_for_Z = 0.0
                else:
                    delta_j1 = 0.0
                if selected_axis != "Y":
                    delta_j4_for_Y = 0.0
                    delta_j4_total = 0.0
                    delta_j2_for_Y = 0.0
                else:
                    delta_j1 = 0.0
                    delta_j2 = delta_j2_for_Y
                print(
                    f"  [MODE] single-axis debug: {selected_axis} only "
                    f"(joint=j{selected_joint_index}, "
                    f"candidates={[(axis, round(score, 1)) for axis, score, _ in candidates]}, "
                    f"y_allowed={y_correction_allowed})"
                )

        # ---- (f-7) 全て 0 なら待機して次ループ ----
        if (delta_j0 == 0.0 and delta_j1 == 0.0
                and delta_j2 == 0.0 and delta_j4_total == 0.0):
            print("  [DEADBAND] 全Δが不感帯。待機して次ループへ")
            loop_log.append({
                "loop": loop_idx,
                "diff_x_mm": diff_x_mm,
                "diff_z_mm": diff_z_mm,
                "diff_y_pix": diff_y_pix,
                "selected_axis": selected_axis,
                "delta_j0": 0.0,
                "delta_j1": 0.0,
                "delta_j2": 0.0,
                "delta_j4_for_Z": delta_j4_for_Z,
                "delta_j4_for_Y": delta_j4_for_Y,
                "delta_j4_total": 0.0,
                "actual_step_deg": 0.0,
                "new_angles": None,
                "measured": current_angles,
                "reach_ok": True,
                "converged": False,
            })
            time.sleep(LOOP_INTERVAL_SEC)
            continue

        # ---- (f-8) 新指令角度を組み立て ----
        new_angles = [
            current_angles[0] + delta_j0,
            current_angles[1] + delta_j1,
            current_angles[2] + delta_j2,
            current_angles[3],
            current_angles[4] + delta_j4_total,
            current_angles[5],
        ]

        # ---- (f-9) 関節別 safety limit チェック ----
        safety_ok = True
        for i, a in enumerate(new_angles):
            lo, hi = JOINT_LIMITS[i]
            if not (lo <= a <= hi):
                print(f"[ABORT] j{i} 安全範囲超過: {a:.2f}° not in [{lo}, {hi}]")
                safety_ok = False
                break
        if not safety_ok:
            print(f"[ABORT] new_angles={new_angles}")
            save_loop_log(loop_log)
            return "aborted"

        # ---- (f-10) 送信 ----
        angles_before_send = list(current_angles)
        print(
            f"  [SEND] Δ=[j0={delta_j0:+.2f}, j1={delta_j1:+.2f}, "
            f"j2={delta_j2:+.2f}, j4_Z={delta_j4_for_Z:+.2f}, "
            f"j2_Y={delta_j2_for_Y:+.2f}, "
            f"j4_Y={delta_j4_for_Y:+.2f}, j4_total={delta_j4_total:+.2f}]"
        )
        print(f"         new_angles={[round(a, 2) for a in new_angles]}")
        if selected_axis == "Y" and (delta_j2 != 0.0 and delta_j4_total != 0.0):
            print(
                f"  [SEND_BLEND] Y descend j2 -> {new_angles[2]:+.2f}, "
                f"j4 -> {new_angles[4]:+.2f} speed={Y_TRACK_SPEED}"
            )
            daemon_send_angles(new_angles, speed=Y_TRACK_SPEED, wait=0)
        elif SINGLE_AXIS_DEBUG and selected_joint_index is not None:
            if selected_joint_index == 0:
                joint_speed = X_TRACK_SPEED
            elif selected_joint_index == 4:
                joint_speed = Y_TRACK_SPEED
            else:
                joint_speed = TRACK_SPEED
            print(
                f"  [SEND_JOINT] j{selected_joint_index} -> "
                f"{new_angles[selected_joint_index]:+.2f} speed={joint_speed}"
            )
            daemon_send_angle(
                selected_joint_index,
                new_angles[selected_joint_index],
                speed=joint_speed,
                wait=0,
            )
        else:
            daemon_send_angles(new_angles, speed=TRACK_SPEED, wait=0)
        time.sleep(LOOP_INTERVAL_SEC)

        # ---- (f-11) 到達確認 ----
        measured = daemon_get_angles()
        if measured is None or len(measured) != 6:
            print("  [WARN] daemon_get_angles 失敗。current_angles を更新せず継続")
            loop_log.append({
                "loop": loop_idx,
                "diff_x_mm": diff_x_mm,
                "diff_z_mm": diff_z_mm,
                "diff_y_pix": diff_y_pix,
                "selected_axis": selected_axis,
                "delta_j0": delta_j0,
                "delta_j1": delta_j1,
                "delta_j2": delta_j2,
                "delta_j4_for_Z": delta_j4_for_Z,
                "delta_j4_for_Y": delta_j4_for_Y,
                "delta_j4_total": delta_j4_total,
                "actual_step_deg": None,
                "new_angles": new_angles,
                "measured": None,
                "reach_ok": False,
                "converged": False,
            })
            time.sleep(LOOP_INTERVAL_SEC)
            continue

        active_indices = []
        if delta_j0 != 0.0:
            active_indices.append(0)
        if delta_j1 != 0.0:
            active_indices.append(1)
        if delta_j2 != 0.0:
            active_indices.append(2)
        if delta_j4_total != 0.0:
            active_indices.append(4)

        reach_ok = True
        for i in range(6):
            err = abs(measured[i] - new_angles[i])
            tol = (TOL_REACH_DEG_ACTIVE if i in active_indices
                   else TOL_REACH_DEG_STATIC)
            if err > tol:
                print(
                    f"  [WARN] j{i} 到達誤差 {err:.2f}° > {tol:.2f}° "
                    f"(target={new_angles[i]:.2f}, measured={measured[i]:.2f})"
                )
                reach_ok = False

        actual_step = 0.0
        if active_indices:
            actual_step = max(
                abs(measured[i] - angles_before_send[i]) for i in active_indices
            )

        # ---- DAY208: no_motion 自動 ABORT 廃止 ----
        abort_due_to_no_motion = False
        if active_indices and actual_step < MIN_EFFECTIVE_MOTION_DEG:
            no_motion_count += 1
            print(
                f"  [WARN] 実測変化が小さすぎます: max_step={actual_step:.2f}° "
                f"({no_motion_count}/{NO_MOTION_LIMIT})"
            )
            if no_motion_count >= NO_MOTION_LIMIT:
                if GRIP_ON_NEAR_TARGET_LOST and is_grip_ready(*last_good_diff):
                    print(
                        "[GRIP] 近傍到達後に実測変化が止まりました。"
                        "ABORTせずグリッパーを閉じて保持します。"
                    )
                    daemon_set_gripper(1, speed=100, wait=0)
                    time.sleep(2.5)
                    abort_due_to_no_motion = "gripped_hold"
                else:
                    print(
                        f"  [WARN] 実測変化なし {no_motion_count} 回連続。"
                        "ABORTせずループを継続します（停止するには Ctrl+C または ESC）。"
                    )
                    no_motion_count = 0
        else:
            no_motion_count = 0

        current_angles = list(measured)

        loop_log.append({
            "loop": loop_idx,
            "diff_x_mm": diff_x_mm,
            "diff_z_mm": diff_z_mm,
            "diff_y_pix": diff_y_pix,
            "selected_axis": selected_axis,
            "delta_j0": delta_j0,
            "delta_j1": delta_j1,
            "delta_j2": delta_j2,
            "delta_j4_for_Z": delta_j4_for_Z,
            "delta_j4_for_Y": delta_j4_for_Y,
            "delta_j4_total": delta_j4_total,
            "actual_step_deg": actual_step,
            "new_angles": new_angles,
            "measured": measured,
            "reach_ok": reach_ok,
            "converged": False,
        })
        if abort_due_to_no_motion:
            save_loop_log(loop_log)
            return abort_due_to_no_motion
    else:
        print(f"\n[TIMEOUT] MAX_LOOPS={MAX_LOOPS} 到達。収束せず終了")
        loop_log.append({
            "loop": MAX_LOOPS,
            "diff_x_mm": None,
            "diff_z_mm": None,
            "diff_y_pix": None,
            "delta_j0": None,
            "delta_j1": None,
            "delta_j2": None,
            "delta_j4_for_Z": None,
            "delta_j4_for_Y": None,
            "delta_j4_total": None,
            "new_angles": None,
            "measured": None,
            "reach_ok": None,
            "converged": False,
        })

    save_loop_log(loop_log)
    return "completed"


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

def main():
    print("=" * 60)
    print("XYZ_TRACK 単体検証スクリプト (DAY205)")
    print(
        "[VERSION] XZ-first + blended Y descent: "
        f"j0_step={MAX_DELTA_J0_PER_STEP:.1f} speed={X_TRACK_SPEED}, "
        f"Y starts at X<={Y_CORRECTION_START_X_MM:.1f}mm "
        f"Z<={Y_CORRECTION_START_Z_MM:.1f}mm, "
        f"j4_step={MAX_DELTA_J4_PER_STEP:.1f} "
        f"j2Y_step={MAX_DELTA_J2_Y_PER_STEP:.1f}, "
        f"grip X<={GRIP_READY_X_MM:.1f}mm "
        f"Z<={GRIP_READY_Z_MM:.1f}mm Y<={GRIP_READY_Y_PIX}px"
    )
    print("=" * 60)

    # (1) カメラ index ハードコード
    print("[1] カメラ index 設定 (ハードコード)...")
    top_index = 2     # iPhone 14 Pro Max (Continuity Camera)
    side_index = 1    # iPhone 11 (Iriun Webcam)
    print(f"   上カメラ index: {top_index}")
    print(f"   横カメラ index: {side_index}")

    # (2) FK テーブル + Homography ロード
    print("[2] FKテーブル + Homography ロード...")
    load_fk_table()

    cap_top = cv2.VideoCapture(top_index)
    if not cap_top.isOpened():
        print(f"[ERROR] 上カメラ open 失敗 (index={top_index})")
        return
    for _ in range(30):
        cap_top.read()
    if not setup_homography_and_mat_range(cap_top):
        print("[ERROR] homography セットアップ失敗")
        cap_top.release()
        return

    # (3) 横カメラ open
    print("[3] 横カメラ open...")
    cap_side = cv2.VideoCapture(side_index)
    if not cap_side.isOpened():
        print(f"[ERROR] 横カメラ open 失敗 (index={side_index})")
        cap_top.release()
        return
    for _ in range(30):
        cap_side.read()

    # (4) daemon 接続
    print("[4] daemon 接続...")
    get_daemon_socket()
    daemon_set_fresh_mode(1)

    result = "aborted"
    try:
        # (5) HOME → OPEN → APPROACH_BASE → XYZ_TRACK
        result = run_xyz_track_test(cap_top, cap_side)
    except KeyboardInterrupt:
        print("\n[INTERRUPT] Ctrl+C 検出。HOME 復帰します。")
    finally:
        try:
            daemon_set_fresh_mode(0)
            if result == "gripped_hold":
                print("[HOLD] グリッパーを閉じた状態で保持します。HOME復帰は自動実行しません。")
            elif result == "hold_no_home":
                print("[HOLD] 現在姿勢で保持します。HOME復帰は自動実行しません。")
            else:
                print("[CLEANUP] HOME 復帰...")
                daemon_send_angles(HOME_ANGLES, speed=SPEED, wait=0)
                time.sleep(HOME_WAIT_SEC)
        except Exception as e:
            print(f"[CLEANUP] HOME 復帰中にエラー: {e}")
        cap_top.release()
        cap_side.release()
        close_daemon_socket()
        cv2.destroyAllWindows()
        print("[DONE]")


if __name__ == "__main__":
    main()

なぜ閾値を緩めたのが正解だったのか

ここがこの記事の核心だ。

工学の発想で言えば、Z 閾値 8mm を守るために Z 補正のアルゴリズムを改善する道があった。j2 が動かなくなった理由を調べて、別関節（j3）で Z を制御する。Z 補正と Y 補正を並列化する。ホモグラフィのキャリブレーションを精密に取り直す。複数フレーム平均化で検出ノイズを抑える。どれも「精度を上げる」方向の解決策だ。

選んだのは逆方向だった。閾値の方を物理現実に合わせた。

180 日言い続けてきた「体験の先行」を、ここで具体的なコードの選択に落とし込んだ。

視覚サーボが収束する真の精度は、今のホモグラフィと検出アルゴリズムでは ±10mm 程度が限界らしい。これを ±2mm まで上げるには、深度センサーを足すか、もっと丁寧なキャリブレーションをするか、複数フレームの平均化を入れるか。どれもコストがかかる。

一方、把持成立に必要な精度は、グリッパーの開口幅とブロックのサイズで決まる。アダプティブグリッパーの開口幅は ±15mm 程度。ブロックは 30mm 角。中心から ±10mm のズレなら掴める。

システムの限界精度と、把持に必要な精度が、たまたま同じ ±10mm のオーダーだった。

だから 8mm という閾値は、システムの限界より厳しすぎた。12mm に緩めることは、精度を諦めることではなく、システムの実力に閾値を合わせることだった。

精度を 8mm から 12mm に緩めても、物理的にブロックは掴めた。逆に 8mm を守るために 1 週間アルゴリズムを書き換えていたら、今夜爪は閉じなかった。

どちらでも「いつか」体験は成立する。だが「いつ」が違う。今夜成立させるためには、閾値を緩める道しかなかった。

設計判断が動作になった

体験の成立を先に取った。動的追従シリーズでの初把持成立。FK 直接指令では以前から掴めていたが、視覚フィードバックでループ補正しながら掴むのは今夜が初めて。「iPhone と Unity だけで、ロボットが物を掴む動作を個人が作れる」。コアメッセージの「動的追従版」が、22:13 に動作として証明された。

これは技術的な勝利ではなく、設計判断の勝利だ。アルゴリズムの泥沼に嵌まる前に、12mm へ緩めて今夜中に爪を閉じる。その判断の速度こそが欲しかったものだ。

精度はあとで上がる。深度センサーを足せばいい。複数フレーム平均化を入れればいい。学習済みモデルを使えばいい。それらは金と時間で解決できる。一方、「体験が一度も成立していない状態」はマイルストーンとして圧倒的に重い。180 日のログが、たった 1 ループで「掴んだ」という事実に切り替わる。

サイドカメラの pixel→Unity 変換式すらまだ未取得（diff_y は pix のまま）の状態で掴めた。これも大きい。完璧な座標系を作ってから動かすのではなく、不完全な座標系のまま体験を成立させる。

残課題

• Z 閾値 12mm の再現性。今回の 1 回成功で「常に 12mm で抜けられる」とは限らない。複数回の再現テストが要る。

• PLACE 動作との接続。今は爪が閉じて HOLD した状態。LIFT して別の場所に置くフローはまだ繋がっていない。

• APPROACH_BASE がブロックに向いていない問題。j0=0° 固定で開始しているので、ブロックが左寄りだと最初の diff_x が +50mm にもなる。HOME 姿勢でブロックを検出して j0 を逆算する設計に変えるべき。これは LOG209 以降の課題。

• Y 補正と Z 干渉の関係の精密化。j4 単独で Y を補正する設計は LOG207 で確定したが、実機ループでの Z の揺れがどの程度予測可能か、まだデータが少ない。

• サイドカメラ pixel → Unity 変換式の取得。現状は diff_y を pixel 単位で扱っているが、距離換算ができれば 3 軸全部 mm 単位の収束判定に統一できる。

22 時 13 分の意味

今夜の 6 時間で 4 つの壁を順番に外した。

最後の壁を抜けたのは、複雑なアルゴリズム改修ではなく、数字を一つ変える判断だった。設計哲学が、技術ではなく判断として効いた。

これが動的追従シリーズでの初把持成立記念日になる。あとで深度センサーを足すかもしれない。複数フレーム平均化を入れるかもしれない。学習済みモデルを繋ぐかもしれない。だが今夜、iPhone 2 台と Unity と 10 万円のロボットアームだけで、視覚フィードバックによる追従制御だけで物を掴む動作が成立した。

LOOP 20 が終わって、ターミナルに [Pi] GRIPPER: CLOSE result= None が出た直後、サーボの音が一段下がって、爪がブロックを挟んだまま止まった。一瞬、自分も止まった。21 回の補正を経て、爪先と赤いブロックの距離がゼロになった瞬間。

精度より体験の成立を優先する。精度は後から金で解決できる。 22 時 13 分、爪が閉じた。

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