mechArm 270 Pi 動的追従の限界を探る Log#5 | Z方向へのリーチ制御とSSHオーバーヘッドという壁

DESCEND後の爪先が目標に届かない問題を解消するため、Z方向の追従制御を追加した。最初はj1+j2の逆方向連動（肘伸縮）を試み、それが効かないと判明してからj1+j2+j4の3関節同方向連動（リーチ伸ばし）に設計を変えた。追従ループはより精密になったはずだった。しかし「なぜアームが動かないのか」を調べると、設計とは別のところに壁があった。SSH接続のオーバーヘッドが1ループあたり6〜8秒を生んでいた。

💡 Key Takeaway

Z方向追従のためにj1+j2+j4の3関節同方向連動を実装したが、SSH経由でPiにAngles指令を送るアーキテクチャが1指令あたり約3秒の遅延を持っており、1ループあたり6〜8秒の処理時間になっていた。追従設計の問題より先に、通信アーキテクチャの問題が露呈した。

DESCENDで爪先がズレる問題の観測

前セッション（Log#4）で動的追従ループは2D距離2.5mmまで収束した。しかし収束後にDESCENDを実行すると、爪先が目標位置から+6.4mmズレてグリッパーが空振りした。これはDESCEND時にj2角度が変わることでアームの幾何が変化し、爪先がX方向に流れる問題だった。

今セッションでまず実施したのは、このズレを定量化する観測パッチの追加だ。DESCEND前後でそれぞれssh_get_angles()を呼び、「送信した角度」と「実際に到達した角度」の差分をログ出力・CSV保存できるようにした。

# [OBS-DAY186] DESCEND前のangle取得
pre_descend_angles = ssh_get_angles()
if pre_descend_angles is not None:
    print(f"[OBS] DESCEND前 angle: {[round(a, 2) for a in pre_descend_angles]}")
else:
    print(f"[OBS] DESCEND前 angle: 取得失敗")
print(f"[OBS] DESCEND送信 angle: {[round(a, 2) for a in descend_angles]}")

print(f"[ARM] DESCEND（j2={DESCEND_J2_DAY185}で肘伸ばし降下）...")
ssh_send_angles(descend_angles, wait=3)

# [OBS-DAY186] DESCEND後のangle取得 + 送/着ズレ計算
post_descend_angles = ssh_get_angles()
descend_delta = None
if post_descend_angles is not None:
    print(f"[OBS] DESCEND後 angle: {[round(a, 2) for a in post_descend_angles]}")
    descend_delta = [post_descend_angles[i] - descend_angles[i] for i in range(6)]
    print(f"[OBS] 送/着ズレ (post - sent): {[round(d, 2) for d in descend_delta]}")
else:
    print(f"[OBS] DESCEND後 angle: 取得失敗")

実機で取得した観測ログ：

[OBS] DESCEND前 angle: [-22.32, 80.33, -6.15, 3.42, -89.73, -29.79]
[OBS] DESCEND送信 angle: [-22.55, 87.74, -5.0, 3.6, -90.0, -30.0]
[ARM] DESCEND（j2=-5.0で肘伸ばし降下）...
[OBS] DESCEND後 angle: [-22.32, 88.85, -4.83, 3.42, -89.73, -29.79]
[OBS] 送/着ズレ (post - sent): [0.23, 1.11, 0.17, -0.18, 0.27, 0.21]
  [評価] 最終TIP_X=+128.4mm  目標との差=+17.8mm

全関節で送/着ズレが1度以内に収まっていた。DESCENDの姿勢自体はほぼ正確に到達できている。最大ズレはj1の+1.11度。問題はズレではなく、DESCENDで姿勢が変わること自体だ。j1が80.33→88.85と+8.5度動いた結果、爪先がX方向に17.8mm流れた。

CSV記録にDESCEND前後の全関節角度を追加した。Trial 7のDESCEND観測データ：

descend_sent:  [-22.55, 87.74, -5.0, 3.6, -90.0, -30.0]
descend_post:  [-22.32, 88.85, -4.83, 3.42, -89.73, -29.79]
descend_delta: [0.23, 1.11, 0.17, -0.18, 0.27, 0.21]

追従ループのZ方向問題

それまでのdynamic_tracking_loop()はj0（ベース回転軸）だけでX方向を追っていた。j0はベース回転のため、Z方向（奥行き）への移動能力を持たない。

実機ログで確認していたZ方向のズレの実態。Log#4（LOG185）の終盤：

[Track] t=42.7s diff_x=-0.1mm diff_z=+37.2mm 2d=37.2mm j0=+31.35 stable=True stdev=0.00
[Track] t=43.2s diff_x=-0.1mm diff_z=+37.2mm 2d=37.2mm j0=+31.35 stable=True stdev=0.00
[Track] t=43.7s diff_x=-0.1mm diff_z=+37.2mm 2d=37.2mm j0=+31.35 stable=True stdev=0.00
[Track] t=44.2s diff_x=-0.1mm diff_z=+37.2mm 2d=37.2mm j0=+31.35 stable=True stdev=0.00
...（以降60ループ以上、全く同じ値が続く）
[Track] t=194.0s diff_x=+1.7mm diff_z=-2.4mm 2d=2.9mm j0=-22.55 stable=True stdev=0.00
[STABLE+CONVERGED] 把持に移行 (2D dist=2.9mm)

X方向は-0.1mmとほぼ一致しているのに、Z方向が+37.2mmズレたまま60ループ以上動かなかった。最終的にdiff_2dが3.0mm以下になったのはZ方向が詰まったのではなく、X方向の微細な変動でたまたま2D距離が閾値を下回った瞬間だった。

肘伸縮設計とその限界

最初に試したのはj1+j2の逆方向連動（肘伸縮）だ。

# 当初の設計: 肘伸縮（逆方向連動）
elbow_delta_deg = diff_z_mm / MM_PER_DEG_ELBOW_Z_DAY186  # 係数5.0mm/度
j1_new = j1_current + elbow_delta_deg   # 肩を+方向
j2_new = j2_current - elbow_delta_deg   # 肘を-方向（逆方向）
new_angles = [j0_current, j1_new, j2_new] + list(approach_angles_base[3:])
ssh_send_angles(new_angles, wait=0)

実機で走らせたログ：

[Track] t=0.0s  diff_x=-6.5mm diff_z=+37.3mm j0=+53.00 j1=+87.74 j2=-20.00 j4=-90.00
ANGLES: [53.78, 88.85, -18.8, 3.51, -89.91, -30.05]
[Z-TRACK] diff_z=+37.3mm elbow_Δ=+2.00° j1=+89.74 j2=-22.00
[Track] t=16.2s diff_x=-1.3mm diff_z=+42.9mm 2d=42.9mm j0=+44.05 j1=+89.74 j2=-22.00
ANGLES: [44.03, 90.87, -17.4, 3.86, -89.91, -30.05]
[Z-TRACK] diff_z=+42.9mm elbow_Δ=+2.00° j1=+91.74 j2=-24.00
[Track] t=32.7s diff_x=-1.8mm diff_z=+42.7mm 2d=42.7mm j0=+44.05 j1=+91.74 j2=-24.00
[Z-TRACK] diff_z=+42.7mm elbow_Δ=+2.00° j1=+93.74 j2=-26.00
[Track] t=40.6s diff_x=-1.5mm diff_z=+43.4mm 2d=43.4mm j0=+53.98 j1=+95.74 j2=-28.00
[Track] t=48.7s diff_x=-1.5mm diff_z=+43.1mm 2d=43.2mm j0=+53.98 j1=+97.74 j2=-30.00
[RANGE_WARN] j1制限超過、クランプ: j1=97.74
[Track] t=56.9s diff_x=-2.0mm diff_z=+43.1mm 2d=43.2mm j0=+53.98 j1=+97.74 j2=-32.00
[RANGE_WARN] j1制限超過、クランプ: j1=97.74
[RANGE_WARN] j2制限超過、クランプ: j2=-35.00

数値の推移：

j1とj2を18度動かしてもdiff_zが37→43mmに増えている。アームが前方に伸びた姿勢では、j1とj2を逆方向に動かしても爪先は前後に動かず上下方向にしか動かない。また制限値（j1=±10°、j2=±15°）に数ループで到達し、それ以降は制限に張り付いた。

3関節同方向連動への設計変更

設計を変えた。j1+j2+j4の3関節を同方向に動かす「リーチ伸ばし」に切り替えた。

# [DAY186] Z方向追従（j1+j2+j4 同方向3関節連動「リーチ伸ばし」制御）
# diff_z>0: tipが手前 → 3関節同時に動かしてリーチを前に伸ばす
# j1+, j2+, j4-（j4は手首を立てる方向で爪先を前に出す）
if diff_z_mm is not None and abs(diff_z_mm) > TRACK_DEADBAND_Z_MM_DAY186:
    reach_delta_deg = diff_z_mm / MM_PER_DEG_ELBOW_Z_DAY186
    reach_delta_deg = max(-MAX_DELTA_ELBOW_DEG_DAY186,
                           min(MAX_DELTA_ELBOW_DEG_DAY186, reach_delta_deg))

    # 3関節を同方向（リーチ伸ばし方向）に動かす
    j1_new = j1_current + reach_delta_deg   # 肩を倒す → 爪先が前に出る
    j2_new = j2_current + reach_delta_deg   # 肘も同方向 → 前腕が前に進む
    j4_new = j4_current - reach_delta_deg   # 手首を立てて爪先を前へ

    new_angles = [j0_current, j1_new, j2_new, approach_angles_base[3], j4_new, approach_angles_base[5]]
    ssh_send_angles(new_angles, wait=0)
    j1_current = j1_new
    j2_current = j2_new
    j4_current = j4_new
    print(f"[Z-TRACK] diff_z={diff_z_mm:+.1f}mm reach_Δ={reach_delta_deg:+.2f}° "
          f"j1={j1_current:+.2f} j2={j2_current:+.2f} j4={j4_current:+.2f}")

j1（肩の上下）とj2（肘の角度）を同方向に動かしながら、j4（手首ピッチ）を反対方向に動かして爪先の向きを補正する。3関節協調でZ方向のリーチを最大化する設計だ。

制限値も変更した。実機で関節に余裕があることを確認し、±180°（実質無効化）に変更。

# [DAY186] j1/j2の相対制限を実質無効化（refine_mode問わず±180°）
j1_limit = 180.0
j2_limit = 180.0

3関節連動での実機ログ（制限無効化後）：

[Track] t=0.0s  diff_x=-6.5mm diff_z=+37.3mm j0=+53.00 j1=+87.74 j2=-20.00 j4=-90.00
ANGLES: [53.78, 88.85, -18.8, 3.51, -89.91, -30.05]
ANGLES: [53.78, 90.79, -21.09, 3.51, -91.05, -30.05]
[Z-TRACK] diff_z=+37.3mm reach_Δ=+2.00° j1=+89.74 j2=-18.00 j4=-92.00
[Track] t=16.1s diff_x=-1.3mm diff_z=+42.9mm 2d=42.9mm j0=+44.05 j1=+89.74 j2=-18.00 j4=-92.00
ANGLES: [44.03, 90.87, -17.4, 3.86, -89.91, -30.05]
ANGLES: [44.03, 90.79, -21.09, 3.86, -91.05, -30.05]
[Z-TRACK] diff_z=+42.9mm reach_Δ=+2.00° j1=+91.74 j2=-16.00 j4=-94.00
[Track] t=32.8s diff_x=-7.9mm diff_z=+34.2mm 2d=35.1mm j0=+54.80 j1=+91.74 j2=-16.00 j4=-94.00
[Z-TRACK] diff_z=+34.2mm reach_Δ=+2.00° j1=+93.74 j2=-14.00 j4=-96.00
...
[Track] t=97.4s diff_x=-7.6mm diff_z=+33.7mm 2d=34.6mm j0=+60.60 j1=+101.74 j2=-6.00 j4=-104.00
[Z-TRACK] diff_z=+33.7mm reach_Δ=+2.00° j1=+103.74 j2=-4.00 j4=-106.00
[Track] t=115.1s diff_x=-7.6mm diff_z=+33.7mm 2d=34.6mm j0=+60.60 j1=+103.74 j2=-4.00 j4=-106.00

j2が-20→-4まで動いているのに、diff_zは+37→+34mmと3mmしか詰まっていない。設計は変わったが、依然としてZ方向への実効的な追従が成立していない。この時点でも1ループあたり約16秒かかっていた。

この原因を掘り下げたのが次のセクションだ。

DESCEND後微調整ループの追加

並行して、DESCENDで生まれた爪先ズレをカメラ観測で詰めるためにDESCEND後に追従ループを再呼び出しする設計も追加した。

# [DAY186] DESCEND後の微調整: 追従ループを再起動して最後の数mmを詰める
print(f"\n=== DESCEND後 微調整ループ開始 ===")
descend_angles_base = list(descend_angles)  # DESCEND姿勢を新たな基準姿勢として渡す
final_j0_refined, final_red_x_refined, refined_success, refined_history = \
    dynamic_tracking_loop(cap, descend_angles_base, descend_angles[0], refine_mode=True)
print(f"=== DESCEND後 微調整ループ終了 (success={refined_success}) ===\n")

DESCEND後の姿勢（descend_angles）を新しいapproach_angles_baseとして渡す。refine_mode=Trueを渡すことでj1/j2の制限が±180°になる。

実機での微調整ループログ：

=== DESCEND後 微調整ループ開始 ===
[REFINE] 制限無効化: j1=±180° j2=±180°
[Track] t=0.0s diff_x=-25.4mm diff_z=+27.5mm 2d=37.5mm j0=+48.92 j1=+87.74 j2=-5.00 j4=-90.00
ANGLES: [52.73, 88.85, -4.92, 3.25, -89.73, -29.79]
ANGLES: [52.73, 90.79, -6.06, 3.25, -89.73, -29.79]
[Z-TRACK] diff_z=+10.6mm reach_Δ=+2.00° j1=+89.74 j2=-7.00 j4=-92.00
[Track] t=16.4s diff_x=-13.6mm diff_z=+27.2mm 2d=30.4mm j0=+52.74 j1=+89.74 j2=-7.00 j4=-92.00
[Z-TRACK] diff_z=+27.2mm reach_Δ=+2.00° j1=+91.74 j2=-9.00 j4=-94.00
[Track] t=32.8s diff_x=-7.9mm diff_z=+34.2mm 2d=35.1mm j0=+54.80 j1=+91.74 j2=-9.00 j4=-94.00
[Z-TRACK] diff_z=+34.2mm reach_Δ=+2.00° j1=+93.74 j2=-11.00 j4=-96.00
[Track] t=49.1s diff_x=-7.6mm diff_z=+34.0mm 2d=34.8mm j0=+55.99 j1=+93.74 j2=-11.00 j4=-96.00 stable=True stdev=0.00
[Z-TRACK] diff_z=+34.0mm reach_Δ=+2.00° j1=+95.74 j2=-13.00 j4=-98.00
...
[Track] t=115.1s diff_x=-7.6mm diff_z=+33.7mm 2d=34.6mm j0=+60.60 j1=+101.74 j2=-19.00 j4=-104.00 stable=True stdev=0.00
[RANGE_WARN] j1制限超過、クランプ: j1=107.74
[Track] t=164.0s diff_x=-10.3mm diff_z=+34.0mm 2d=35.5mm j0=+64.83 j1=+107.74 j2=-25.00 j4=-110.00 stable=True
[RANGE_WARN] j1制限超過、クランプ: j1=107.74
[RANGE_WARN] j2制限超過、クランプ: j2=-35.00

±180°にしたのにクランプが出た。通常追従ループ（refine_mode=False）の時点でAPPROACH基準点からすでに大きく動いており、refine_modeになったときapproach_angles_baseがDESCEND姿勢に変わっても、j1_currentの累積値がすでに制限を超えていた。

アームが動かない本当の原因：SSH処理時間の解剖

3関節連動に変えて走らせると、カメラ映像がコマ送りになり、アームがほとんど動いていないように見えた。ログのタイムスタンプ：

[Track] t=0.0s   diff_z=+37.3mm
[Track] t=16.2s  diff_z=+42.9mm   ← 16秒経過
[Track] t=32.7s  diff_z=+42.7mm   ← さらに16秒
[Track] t=40.6s  diff_z=+43.4mm
[Track] t=48.7s  diff_z=+43.1mm

LOOP_INTERVAL_SEC = 0.1（100ms）に設定しているのに、1ループあたり約16秒かかっていた。ssh_send_angles()の実装を確認した。

def ssh_send_angles(angles, speed=SPEED, wait=5):
    angles_str = str(angles)
    cmd = (
        f"python3 -c \""
        f"from pymycobot.mycobot import MyCobot; import time; "
        f"mc = MyCobot('/dev/ttyAMA0', 1000000); time.sleep(2); "  # ← 毎回2秒待機
        f"mc.send_angles({angles_str}, {speed}); time.sleep({wait}); "
        f"print('ANGLES:', mc.get_angles())\""
    )
    client = paramiko.SSHClient()
    client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    client.connect(SSH_HOST, username=SSH_USER, key_filename=SSH_KEY_PATH)  # ← 毎回接続
    stdin, stdout, stderr = client.exec_command(cmd)
    output = stdout.read().decode("utf-8", errors="ignore").strip()
    error = stderr.read().decode("utf-8", errors="ignore").strip()
    client.close()  # ← 毎回切断

毎回SSHを接続・切断し、毎回Pythonプロセスを起動し、毎回pymycobotをimportし、毎回time.sleep(2)を実行していた。

1ループの所要時間の完全な内訳：

# SSH送信1回（j0更新）の内訳
SSH接続                        0.3〜0.5秒
Python起動 + pymycobot import  0.3〜0.5秒
time.sleep(2)                  2.0秒  ← 支配的
mc.send_angles()               数ms
mc.get_angles() + print        0.2〜0.3秒  ← wait=0でも実行される
SSH切断                        即時
小計: 約2.8〜3.3秒 / 1回

# 1ループあたりの合計
j0更新（SSH送信1回目）         2.8〜3.3秒
Z追従（SSH送信2回目）          2.8〜3.3秒
cap.read() × 3                 〜100ms（30fps時）
detect_red_block()             30〜50ms
detect_tip_from_frame()        30〜50ms
cv2.imshow() + waitKey         数ms
time.sleep(0.1)                100ms

合計: 約6,000〜7,000ms/ループ

Z追従を追加したことで1ループのSSH送信が1回→2回に倍増し、これが16秒ループの正体だった。

これはLOG182で測定した「ソフトウェア律速91Hz」とは全く別の話だ。あのときはPi側常駐デーモンへのstdin/stdout通信で計測した。現在のコードは毎回新規プロセスを立ち上げる構造になっており、同じ話ではない。

wait=0でssh_send_angles()を呼んでも、Piで実行されるコマンドの末尾にprint('ANGLES:', mc.get_angles())が固定で入っているため、wait引数とは別にget_angles()の分だけ処理時間が増えていた。追従ループではPi側のANGLES確認は不要で（カメラでTIP位置を取っているため）、この出力も削除対象になる。

軽量化の7段階ロードマップ

処理を重くしている要因を分解し、対処コストと削減効果を整理した。

各レバーの詳細：

レバー1：time.sleep(2) 削除（最優先） MyCobot('/dev/ttyAMA0', 1000000)インスタンス生成後にシリアル通信が安定するまで待つためのスリープ。1行変更で4秒/ループ削減。削除後にsend_anglesが無視されるケースが生じるかどうかは実機で確認が必要。これだけで3倍速になる。

レバー2：SSH送信を1回に統合 現状、j0更新とZ追従更新がそれぞれ独立したSSH送信になっている（1ループで最大2回）。j0・j1・j2・j4の全更新値をまとめて1回のssh_send_angles([j0, j1, j2, j3, j4, j5])で送ることで、1ループのSSH送信を1回に削減できる。レバー1と組み合わせて0.5〜1秒になる。

レバー3：cap.read()の捨て削減 毎ループ2枚の古いフレームを捨てた後に本命フレームを取得している（計3回のcap.read）。30fpsのIriun Webcamなら2枚捨ては67ms。1枚に減らすことで33ms削減できる。

レバー4：get_angles出力を削除 ssh_send_angles()の末尾で毎回Pi側がmc.get_angles()を呼んでログ出力している。追従ループではカメラでTIP位置を検出しているため不要。wait=0の呼び出しでも実行されるため、この出力を削除するだけで数百ms削減できる。

レバー5+6：デーモン化 + set_fresh_mode(1) LOG182で実装・実証済みのPi側常駐デーモン方式。SSH接続を1本張りっぱなしにし、stdin/stdoutでJSONコマンドをやり取りする。Python起動・import・MyCobot初期化のオーバーヘッドをゼロにできる。LOG182実測では76Hz（1指令13ms）。set_fresh_mode(1)と組み合わせれば91Hzまで出る。

レバー7：HSV検出の解像度を下げる フレームをHSV変換する前に1/2にダウンサンプルしてからdetect_red_block()とdetect_tip_from_frame()を行う。処理量は1/4になるため30〜50msの削減が見込める。青シールが小さく映っている場合に検出失敗のリスクがある。

今セッションで完成したvisual_servo_pick.pyのフルコード

#!/usr/bin/env python3
"""
visual_servo_pick.py
DAY186改: Z方向追従（j1+j2+j4同方向連動）追加、DESCEND前後観測パッチ追加、
DESCEND後微調整ループ追加、LOOP_INTERVAL_SEC 0.3→0.1、cv2.imwrite一時無効化。
"""

import os
import sys
import math
import csv
import time
import glob
import threading
import numpy as np
import cv2
import paramiko
from datetime import datetime
from mat_corner_detection import capture_frames, detect_corners, remove_outliers_and_average

# ---- 設定 ----------------------------------------------------------------
SSH_HOST = "192.168.2.1"
SSH_USER = "ubuntu"
SSH_KEY_PATH = os.path.expanduser("~/.ssh/id_rsa_pi")
FK_CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")
CAMERA_INDEX = 1
SPEED = 20

PICK_EE_Y = -0.040
EE_Y_RANGE = 0.015
DIST_THRESHOLD = 0.100

TARGET_J2 = -33.33
TARGET_J3 = 3.6
TARGET_J4 = -45.11
TARGET_J5 = -30.0
TOL_J2 = 15.0
TOL_J3 = 15.0
TOL_J4 = 15.0
TOL_J5 = 10.0

APPROACH_PICK_BASE = [None, 87.74, -38.33, 3.6, -70.11, -30.0]
APPROACH_PICK_BASE_DAY185 = [None, 87.74, -20.0, 3.6, -90.0, -30.0]
DESCEND_J1_DAY185 = 87.74
DESCEND_J2_DAY185 = -5.0   # DAY186: -10.0→-5.0（爪先を床に近づける）
PICK_J1_FIXED = 90.87
HOME = [0, 0, 0, 0, 0, -30]

OUT_W = 1240
OUT_H = 900
MAT_CORNER_NUM_FRAMES = 10
MAT_CORNER_WARMUP_FRAMES = 3
MAT_CORNER_FRAME_INTERVAL = 0.1

# 視覚サーボ設定（DAY180）
MM_PER_DEG_J0_X = -6.63
CONVERGE_THRESHOLD_MM = 3.0
MAX_DELTA_DEG = 5.0
MAX_LOOP_COUNT = 20
DETECT_FAIL_LIMIT = 3
J0_RELATIVE_LIMIT = 15.0
LOOP_WAIT_SEC = 0.5

# 動的追従設定（DAY185）
STABLE_FRAMES        = 5
STABLE_THRESHOLD_MM  = 1.0
TRACK_DEADBAND_MM    = 2.0
MAX_TRACKING_SEC     = 600.0
J0_RELATIVE_LIMIT_DAY185 = 60.0
LOOP_INTERVAL_SEC    = 0.1     # DAY186: 0.3→0.1

# Z方向追従設定（DAY186追加）
MM_PER_DEG_ELBOW_Z_DAY186 = 5.0
TRACK_DEADBAND_Z_MM_DAY186 = 5.0
MAX_DELTA_ELBOW_DEG_DAY186 = 2.0
J1_RELATIVE_LIMIT_DAY186 = 10.0
J2_RELATIVE_LIMIT_DAY186 = 15.0

DEBUG_DIR = os.path.expanduser("~/r2r2r-scripts/debug_visual_servo")

_fk_table = []
_homography = None
_mat_px_range = None
_input_buffer = []
_input_lock = threading.Lock()


def draw_tracking_overlay(frame, red_pixel, blue_markers, tip_pixel,
                          red_xz_mm, tip_xz_mm, status_text):
    img = frame.copy()
    if _mat_px_range is not None:
        x1, x2, y1, y2 = _mat_px_range
        cv2.rectangle(img, (x1, y1), (x2, y2), (255, 255, 255), 2)
    if red_pixel is not None and red_xz_mm is not None:
        rx, ry = red_pixel
        rx_mm, rz_mm = red_xz_mm
        cv2.circle(img, (rx, ry), 22, (0, 0, 255), 3)
        cv2.putText(img, f"RED ({rx_mm:.0f}, {rz_mm:.0f})mm",
                    (rx + 28, ry + 8), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA)
    if blue_markers:
        for i, pt in enumerate(blue_markers[:3]):
            mx, my = pt[0], pt[1]
            cv2.circle(img, (mx, my), 18, (0, 255, 0), 2)
            cv2.putText(img, f"BLUE#{i}", (mx + 22, my + 5), cv2.FONT_HERSHEY_SIMPLEX,
                        0.6, (0, 255, 0), 2, cv2.LINE_AA)
    if tip_pixel is not None and tip_xz_mm is not None:
        tx, ty = tip_pixel
        tx_mm, tz_mm = tip_xz_mm
        cv2.drawMarker(img, (tx, ty), (0, 165, 255), cv2.MARKER_CROSS, 32, 3)
        cv2.putText(img, f"TIP ({tx_mm:.0f}, {tz_mm:.0f})mm",
                    (tx + 35, ty + 8), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 165, 255), 2, cv2.LINE_AA)
    y = 30
    for line in status_text.split("\n"):
        cv2.putText(img, line, (15, y), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2, cv2.LINE_AA)
        y += 30
    return img


def input_thread_func(prompt):
    try:
        val = input(prompt)
    except EOFError:
        val = "q"
    with _input_lock:
        _input_buffer.append(val.strip().lower())


def wait_for_input_with_preview(cap, prompt, status_text):
    _input_buffer.clear()
    t = threading.Thread(target=input_thread_func, args=(prompt,), daemon=True)
    t.start()
    user_input = None
    while user_input is None:
        with _input_lock:
            if _input_buffer:
                user_input = _input_buffer.pop(0)
                break
        ret, frame = cap.read()
        if ret:
            red_pixel_pv = detect_red_block(frame)
            red_xz_pv = None
            if red_pixel_pv is not None:
                rx, rz = pixel_to_unity(red_pixel_pv[0], red_pixel_pv[1])
                red_xz_pv = (rx * 1000, rz * 1000)
            tip_pixel_pv, blue_markers_pv, tip_unity_pv = detect_tip_from_frame(frame)
            tip_xz_pv = None
            if tip_unity_pv is not None:
                tip_xz_pv = (tip_unity_pv[0] * 1000, tip_unity_pv[1] * 1000)
            overlay = draw_tracking_overlay(frame, red_pixel_pv, blue_markers_pv,
                                            tip_pixel_pv, red_xz_pv, tip_xz_pv, status_text)
            cv2.imshow("R2R2R Tracking", overlay)
            cv2.waitKey(1)
        else:
            time.sleep(0.05)
    return user_input


def load_fk_table():
    global _fk_table
    print("[Mac] FK scan CSV読み込み中...")
    rows = []
    with open(FK_CSV_PATH, "r") as f:
        reader = csv.reader(f)
        next(reader)
        for row in reader:
            ee_x = float(row[0])
            ee_y = float(row[1])
            ee_z = float(row[2])
            joints = [float(row[j + 3]) for j in range(6)]
            rows.append((ee_x, ee_y, ee_z, joints))
    _fk_table = rows
    print(f"[Mac] ✓ {len(_fk_table)}点ロード")


def lookup_nearest_angles_with_ee(unity_x, unity_z, target_ee_y, label=""):
    pose_filtered = [
        (ee_x, ee_y, ee_z, joints) for ee_x, ee_y, ee_z, joints in _fk_table
        if abs(ee_y - target_ee_y) <= EE_Y_RANGE
        and abs(joints[2] - TARGET_J2) <= TOL_J2
        and abs(joints[3] - TARGET_J3) <= TOL_J3
        and abs(joints[4] - TARGET_J4) <= TOL_J4
        and abs(joints[5] - TARGET_J5) <= TOL_J5
    ]
    print(f"[Mac] {label} 姿勢制約後候補数: {len(pose_filtered)}点")
    if not pose_filtered:
        return None, float("inf"), None
    best = None
    best_dist = float("inf")
    for ee_x, ee_y, ee_z, joints in pose_filtered:
        dist = math.sqrt((ee_x - unity_x)**2 + (ee_z - unity_z)**2)
        if dist < best_dist:
            best_dist = dist
            best = (ee_x, ee_y, ee_z, joints)
    if best is None or best_dist > DIST_THRESHOLD:
        return None, best_dist, None
    ee_x, ee_y, ee_z, joints = best
    return joints, best_dist, (ee_x, ee_y, ee_z)


def make_approach_angles(lookup_angles, approach_base):
    return [lookup_angles[0], approach_base[1], approach_base[2],
            approach_base[3], approach_base[4], approach_base[5]]


def setup_homography_and_mat_range(cap):
    global _homography, _mat_px_range
    print(f"[Mac] マット四隅検出中... ({MAT_CORNER_NUM_FRAMES}フレーム)")
    frames = capture_frames(cap, MAT_CORNER_NUM_FRAMES, MAT_CORNER_WARMUP_FRAMES, MAT_CORNER_FRAME_INTERVAL)
    all_corners = [c for c in (detect_corners(f) for f in frames) if c is not None]
    if not all_corners:
        print("[Mac] ✗ マット四隅検出失敗")
        return False
    corners = remove_outliers_and_average(all_corners)
    print(f"[Mac] ✓ マット四隅検出: {len(all_corners)}/{MAT_CORNER_NUM_FRAMES} フレーム成功")
    src = np.array(corners, dtype=np.float32)
    dst_mm = np.array([[+310, -89], [-310, -89], [-310, +361], [+310, +361]], dtype=np.float32)
    _homography = cv2.getPerspectiveTransform(src, dst_mm)
    print(f"[Mac] ✓ 動的homography計算完了")
    min_x = int(min(corners[0][0], corners[3][0]))
    max_x = int(max(corners[1][0], corners[2][0]))
    min_y = int(min(corners[0][1], corners[1][1]))
    max_y = int(max(corners[2][1], corners[3][1]))
    _mat_px_range = (min_x, max_x, min_y, max_y)
    print(f"[Mac] ✓ マット範囲: X=[{min_x}, {max_x}] Y=[{min_y}, {max_y}]")
    return True


def pixel_to_unity(px, py):
    pt = np.array([[[float(px), float(py)]]], dtype=np.float32)
    r = cv2.perspectiveTransform(pt, _homography)
    return float(r[0][0][0]) * 0.001, float(r[0][0][1]) * 0.001


def detect_red_block(frame):
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    lower_red1 = np.array([0, 100, 100])
    upper_red1 = np.array([10, 255, 255])
    lower_red2 = np.array([160, 100, 100])
    upper_red2 = np.array([180, 255, 255])
    m1 = cv2.inRange(hsv, lower_red1, upper_red1)
    m2 = cv2.inRange(hsv, lower_red2, upper_red2)
    mask = cv2.bitwise_or(m1, m2)
    kernel = np.ones((5, 5), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None
    MIN_AREA = 500
    candidates = []
    for cnt in contours:
        area = cv2.contourArea(cnt)
        if area < MIN_AREA:
            continue
        x, y, w, h = cv2.boundingRect(cnt)
        cx = x + w // 2
        cy = y + h // 2
        if _mat_px_range is not None:
            min_x, max_x, min_y, max_y = _mat_px_range
            if not (min_x <= cx <= max_x and min_y <= cy <= max_y):
                continue
        candidates.append((area, cx, cy))
    if not candidates:
        return None
    _, cx, cy = max(candidates, key=lambda t: t[0])
    return (cx, cy)


def detect_red_block_unity(cap):
    for _ in range(5):
        cap.read()
    ret, frame = cap.read()
    if not ret:
        return None
    pixel = detect_red_block(frame)
    if pixel is None:
        return None
    cx, cy = pixel
    unity_x, unity_z = pixel_to_unity(cx, cy)
    print(f"  赤ブロック pixel=({cx}, {cy}) → Unity=({unity_x:.4f}, {unity_z:.4f})")
    return (unity_x, unity_z)


def detect_blue_markers(frame, mat_px_range, debug_pose_id=None):
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    lower_blue = np.array([100, 100, 50])
    upper_blue = np.array([130, 255, 255])
    mask = cv2.inRange(hsv, lower_blue, upper_blue)
    kernel = np.ones((5, 5), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
    mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
    if debug_pose_id is not None:
        os.makedirs(DEBUG_DIR, exist_ok=True)
        mask_bgr = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
        masked = cv2.bitwise_and(frame, frame, mask=mask)
        combined = np.hstack([frame, masked, mask_bgr])
        cv2.imwrite(f"{DEBUG_DIR}/blue_mask_{debug_pose_id}.png", combined)
    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    MIN_AREA = 80
    MAX_AREA = 8000
    candidates = []
    for cnt in contours:
        area = cv2.contourArea(cnt)
        if area < MIN_AREA or area > MAX_AREA:
            continue
        M = cv2.moments(cnt)
        if M["m00"] == 0:
            continue
        cx = int(M["m10"] / M["m00"])
        cy = int(M["m01"] / M["m00"])
        if mat_px_range is not None:
            min_x, max_x, min_y, max_y = mat_px_range
            if not (min_x <= cx <= max_x and min_y <= cy <= max_y):
                continue
        candidates.append((area, cx, cy))
    if len(candidates) < 3:
        return None
    candidates.sort(key=lambda t: -t[0])
    top3 = [(cx, cy) for _, cx, cy in candidates[:3]]
    return top3


def identify_blue_markers(blue_points, homography):
    """
    3点を wrist/claw_a/claw_b に割り当てる（DAY184改: 最短辺判定）。
    3点間の最短辺の両端を claw_a/claw_b、残りを wrist とする。
    """
    p0, p1, p2 = blue_points
    d01 = math.hypot(p0[0] - p1[0], p0[1] - p1[1])
    d02 = math.hypot(p0[0] - p2[0], p0[1] - p2[1])
    d12 = math.hypot(p1[0] - p2[0], p1[1] - p2[1])
    if d01 <= d02 and d01 <= d12:
        wrist_px = p2
        claw_pts = [p0, p1]
    elif d02 <= d01 and d02 <= d12:
        wrist_px = p1
        claw_pts = [p0, p2]
    else:
        wrist_px = p0
        claw_pts = [p1, p2]
    claw_pts.sort(key=lambda p: p[0])
    claw_a_px = claw_pts[0]
    claw_b_px = claw_pts[1]

    def to_unity(px, py):
        pt = np.array([[[float(px), float(py)]]], dtype=np.float32)
        r = cv2.perspectiveTransform(pt, homography)
        return float(r[0][0][0]) * 0.001, float(r[0][0][1]) * 0.001

    wrist_ux, wrist_uz = to_unity(*wrist_px)
    claw_a_ux, claw_a_uz = to_unity(*claw_a_px)
    claw_b_ux, claw_b_uz = to_unity(*claw_b_px)
    return {
        "wrist": (wrist_px[0], wrist_px[1], wrist_ux, wrist_uz),
        "claw_a": (claw_a_px[0], claw_a_px[1], claw_a_ux, claw_a_uz),
        "claw_b": (claw_b_px[0], claw_b_px[1], claw_b_ux, claw_b_uz),
    }


def get_gripper_tip_unity(markers):
    a_x = markers["claw_a"][2]
    a_z = markers["claw_a"][3]
    b_x = markers["claw_b"][2]
    b_z = markers["claw_b"][3]
    return ((a_x + b_x) / 2, (a_z + b_z) / 2)


def detect_gripper_tip_auto(cap, trial=None):
    for _ in range(5):
        cap.read()
    ret, frame = cap.read()
    if not ret:
        return None, None, None
    blue_points = detect_blue_markers(frame, _mat_px_range, debug_pose_id=trial)
    if blue_points is None:
        return None, None, frame
    markers = identify_blue_markers(blue_points, _homography)
    tip_unity = get_gripper_tip_unity(markers)
    return tip_unity, markers, frame


def detect_tip_from_frame(frame):
    blue_markers = detect_blue_markers(frame, None)
    if not blue_markers or len(blue_markers) < 3:
        return None, blue_markers, None
    markers = identify_blue_markers(blue_markers, _homography)
    tip_unity = get_gripper_tip_unity(markers)
    ca = markers["claw_a"]
    cb = markers["claw_b"]
    tip_pixel = ((ca[0] + cb[0]) // 2, (ca[1] + cb[1]) // 2)
    return tip_pixel, blue_markers, tip_unity


def ssh_send_angles(angles, speed=SPEED, wait=5):
    angles_str = str(angles)
    cmd = (
        f"python3 -c \""
        f"from pymycobot.mycobot import MyCobot; import time; "
        f"mc = MyCobot('/dev/ttyAMA0', 1000000); time.sleep(2); "
        f"mc.send_angles({angles_str}, {speed}); time.sleep({wait}); "
        f"print('ANGLES:', mc.get_angles())\""
    )
    client = paramiko.SSHClient()
    client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    client.connect(SSH_HOST, username=SSH_USER, key_filename=SSH_KEY_PATH)
    stdin, stdout, stderr = client.exec_command(cmd)
    output = stdout.read().decode("utf-8", errors="ignore").strip()
    error = stderr.read().decode("utf-8", errors="ignore").strip()
    client.close()
    if output:
        print(f"  [Pi] {output}")
    if error and "no longer maintained" not in error:
        print(f"  [Pi stderr] {error}")


def ssh_get_angles():
    cmd = (
        f"python3 -c \""
        f"from pymycobot.mycobot import MyCobot; import time; "
        f"mc = MyCobot('/dev/ttyAMA0', 1000000); time.sleep(2); "
        f"print('ANGLES_GET:', mc.get_angles())\""
    )
    client = paramiko.SSHClient()
    client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    client.connect(SSH_HOST, username=SSH_USER, key_filename=SSH_KEY_PATH)
    stdin, stdout, stderr = client.exec_command(cmd)
    output = stdout.read().decode("utf-8", errors="ignore").strip()
    client.close()
    for line in output.split("\n"):
        if line.startswith("ANGLES_GET:"):
            try:
                return eval(line.replace("ANGLES_GET:", "").strip())
            except Exception:
                pass
    return None


def send_home():
    print("[ARM] HOME へ移動...")
    ssh_send_angles(HOME, wait=4)
    print("[ARM] HOME 完了")


def ssh_set_gripper(state, speed=100, wait=2):
    cmd = (
        f"python3 -c \""
        f"from pymycobot.mycobot import MyCobot; import time; "
        f"mc = MyCobot('/dev/ttyAMA0', 1000000); time.sleep(2); "
        f"result = mc.set_gripper_state({state}, {speed}); "
        f"time.sleep({wait}); "
        f"print('GRIPPER:', 'OPEN' if {state} == 0 else 'CLOSE', 'result=', result)\""
    )
    client = paramiko.SSHClient()
    client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    client.connect(SSH_HOST, username=SSH_USER, key_filename=SSH_KEY_PATH)
    stdin, stdout, stderr = client.exec_command(cmd)
    output = stdout.read().decode("utf-8", errors="ignore").strip()
    error = stderr.read().decode("utf-8", errors="ignore").strip()
    client.close()
    if output:
        print(f"  [Pi] {output}")
    if error and "no longer maintained" not in error:
        print(f"  [Pi stderr] {error}")


def dynamic_tracking_loop(cap, approach_angles_base, initial_j0, refine_mode=False):
    """
    赤ブロックとTIPを毎ループ検出し、j0を連続更新して追従する。
    DAY186: Z方向追従（j1+j2+j4 同方向3関節連動）を追加。
    refine_mode=True: DESCEND後の微調整ループ用（j1/j2制限を±180°に拡大）
    戻り値: (final_j0, final_red_x_mm, success, history)
    """
    # [DAY186] j1/j2の相対制限を実質無効化（refine_mode問わず±180°）
    j1_limit = 180.0
    j2_limit = 180.0
    if refine_mode:
        print(f"[REFINE] 制限無効化: j1=±{j1_limit:.0f}° j2=±{j2_limit:.0f}°")

    j0_current = initial_j0
    j1_current = approach_angles_base[1]
    j2_current = approach_angles_base[2]
    j4_current = approach_angles_base[4]  # [DAY186] j4も動かすため保持
    red_x_history = []
    history = []
    start_time = time.time()

    cv2.namedWindow("R2R2R Tracking", cv2.WINDOW_NORMAL)
    cv2.resizeWindow("R2R2R Tracking", 1280, 720)

    loop_count = 0
    while True:
        loop_count += 1
        elapsed = time.time() - start_time

        if elapsed > MAX_TRACKING_SEC:
            print(f"[TIMEOUT] {MAX_TRACKING_SEC}秒超過。追従中断。")
            cv2.destroyWindow("R2R2R Tracking")
            return j0_current, None, False, history

        # 単一フレーム取得（2フレーム捨て + 1フレーム取得）
        for _ in range(2):
            cap.read()
        ret, frame = cap.read()
        if not ret:
            time.sleep(LOOP_INTERVAL_SEC)
            continue

        # 赤ブロック検出（同一フレームから）
        red_pixel = detect_red_block(frame)
        red_xz_mm = None
        red_x_mm = None
        red_z_mm = None
        if red_pixel is not None:
            rx_unity, rz_unity = pixel_to_unity(red_pixel[0], red_pixel[1])
            red_x_mm = rx_unity * 1000
            red_z_mm = rz_unity * 1000
            red_xz_mm = (red_x_mm, red_z_mm)

        # 青シール検出 → TIP計算（同一フレームから、幾何学ベース）
        tip_pixel, blue_markers, tip_unity = detect_tip_from_frame(frame)
        tip_xz_mm = None
        tip_x_mm = None
        if tip_unity is not None:
            tip_x_mm = tip_unity[0] * 1000
            tip_z_mm = tip_unity[1] * 1000
            tip_xz_mm = (tip_x_mm, tip_z_mm)

        # diff計算（X + Z + 2D距離）
        diff_x_mm = None
        diff_z_mm = None
        diff_2d_mm = None
        if red_x_mm is not None and tip_x_mm is not None:
            diff_x_mm = red_x_mm - tip_x_mm
            diff_z_mm = red_z_mm - tip_z_mm
            diff_2d_mm = math.sqrt(diff_x_mm**2 + diff_z_mm**2)

        # red_x履歴に追加
        if red_x_mm is not None:
            red_x_history.append(red_x_mm)
            if len(red_x_history) > STABLE_FRAMES:
                red_x_history.pop(0)

        # 静止判定
        if len(red_x_history) >= STABLE_FRAMES:
            stdev = float(np.std(red_x_history))
            stable = stdev <= STABLE_THRESHOLD_MM
        else:
            stdev = float('inf')
            stable = False

        if diff_x_mm is not None:
            print(f"[Track] t={elapsed:.1f}s diff_x={diff_x_mm:+.1f}mm diff_z={diff_z_mm:+.1f}mm 2d={diff_2d_mm:.1f}mm "
                  f"j0={j0_current:+.2f} j1={j1_current:+.2f} j2={j2_current:+.2f} j4={j4_current:+.2f} "
                  f"red_x={red_x_mm:+.1f} tip_x={tip_x_mm:+.1f} "
                  f"stable={stable} stdev={stdev:.2f}")
        else:
            det_status = []
            if red_x_mm is None:
                det_status.append("RED:NG")
            if tip_x_mm is None:
                det_status.append("TIP:NG")
            print(f"[Track] t={elapsed:.1f}s {' '.join(det_status)} j0={j0_current:+.2f}")

        history.append({
            "loop": loop_count,
            "elapsed_sec": round(elapsed, 1),
            "red_x_mm": round(red_x_mm, 1) if red_x_mm is not None else None,
            "tip_x_mm": round(tip_x_mm, 1) if tip_x_mm is not None else None,
            "diff_x_mm": round(diff_x_mm, 1) if diff_x_mm is not None else None,
            "diff_z_mm": round(diff_z_mm, 1) if diff_z_mm is not None else None,
            "diff_2d_mm": round(diff_2d_mm, 1) if diff_2d_mm is not None else None,
            "j0": round(j0_current, 3),
            "j1": round(j1_current, 3),
            "j2": round(j2_current, 3),
            "j4": round(j4_current, 3),
            "stable": stable,
            "stdev": round(stdev, 2) if stdev != float('inf') else None,
        })

        # 把持条件チェック（X+Z両方の2D距離で収束判定）
        if diff_2d_mm is not None and diff_2d_mm > CONVERGE_THRESHOLD_MM:
            stable_for_grip = False
        else:
            stable_for_grip = stable

        if diff_2d_mm is not None and stable_for_grip:
            print(f"[STABLE+CONVERGED] 把持に移行 (2D dist={diff_2d_mm:.1f}mm)")
            cv2.destroyWindow("R2R2R Tracking")
            return j0_current, red_x_mm, True, history

        # j0更新（追従、X方向のdiffのみで判定）
        if diff_x_mm is not None and abs(diff_x_mm) > TRACK_DEADBAND_MM:
            delta_deg = diff_x_mm / MM_PER_DEG_J0_X
            delta_deg = max(-MAX_DELTA_DEG, min(MAX_DELTA_DEG, delta_deg))
            j0_new = j0_current + delta_deg
            if abs(j0_new - initial_j0) > J0_RELATIVE_LIMIT_DAY185:
                sign = 1 if (j0_new - initial_j0) > 0 else -1
                j0_new = initial_j0 + sign * J0_RELATIVE_LIMIT_DAY185
                print(f"[RANGE_WARN] j0制限超過、クランプ: j0={j0_new:.2f}")
            new_angles = [j0_new, j1_current, j2_current, approach_angles_base[3], j4_current, approach_angles_base[5]]
            ssh_send_angles(new_angles, wait=0)
            j0_current = j0_new

        # [DAY186] Z方向追従（j1+j2+j4 同方向3関節連動「リーチ伸ばし」制御）
        if diff_z_mm is not None and abs(diff_z_mm) > TRACK_DEADBAND_Z_MM_DAY186:
            reach_delta_deg = diff_z_mm / MM_PER_DEG_ELBOW_Z_DAY186
            reach_delta_deg = max(-MAX_DELTA_ELBOW_DEG_DAY186,
                                   min(MAX_DELTA_ELBOW_DEG_DAY186, reach_delta_deg))
            j1_new = j1_current + reach_delta_deg
            j2_new = j2_current + reach_delta_deg
            j4_new = j4_current - reach_delta_deg
            new_angles = [j0_current, j1_new, j2_new, approach_angles_base[3], j4_new, approach_angles_base[5]]
            ssh_send_angles(new_angles, wait=0)
            j1_current = j1_new
            j2_current = j2_new
            j4_current = j4_new
            print(f"[Z-TRACK] diff_z={diff_z_mm:+.1f}mm reach_Δ={reach_delta_deg:+.2f}° "
                  f"j1={j1_current:+.2f} j2={j2_current:+.2f} j4={j4_current:+.2f}")

        # 状態テキスト構築
        lines = [f"t={elapsed:.1f}s"]
        if diff_2d_mm is not None:
            lines.append(f"diff_x={diff_x_mm:+.1f} diff_z={diff_z_mm:+.1f} 2d={diff_2d_mm:.1f}mm")
        else:
            lines.append("diff=N/A")
        lines.append(f"j0={j0_current:+.2f}")
        lines.append(f"stable={stable} stdev={stdev:.2f}" if stdev != float('inf') else f"stable={stable}")
        if red_x_mm is None:
            lines.append("RED: NOT DETECTED")
        if tip_x_mm is None:
            lines.append("TIP: NOT DETECTED")
        status_text = "\n".join(lines)

        overlay = draw_tracking_overlay(frame, red_pixel, blue_markers,
                                        tip_pixel, red_xz_mm, tip_xz_mm, status_text)
        cv2.imshow("R2R2R Tracking", overlay)
        # [DAY186] カメラ負荷軽減でimwrite一時無効化
        # if loop_count % 2 == 0:
        #     save_path = os.path.join(DEBUG_DIR, f"track_t{int(elapsed*10):04d}.png")
        #     cv2.imwrite(save_path, overlay)
        key = cv2.waitKey(1) & 0xFF
        if key == 27:
            print("[ESC] ユーザー中断")
            cv2.destroyWindow("R2R2R Tracking")
            return j0_current, red_x_mm, False, history

        time.sleep(LOOP_INTERVAL_SEC)


def save_csv(results):
    ts = datetime.now().strftime("%Y%m%d_%H%M%S")
    csv_path = os.path.expanduser(f"~/r2r2r-scripts/visual_servo_{ts}.csv")
    fields = [
        "trial", "pick_x_mm", "pick_z_mm",
        "initial_j0", "final_j0", "converged", "loop_count",
        "final_tip_x_mm", "final_diff_x_mm",
        # [OBS-DAY186] DESCEND観測項目
        "descend_sent_j0", "descend_sent_j1", "descend_sent_j2",
        "descend_sent_j3", "descend_sent_j4", "descend_sent_j5",
        "descend_post_j0", "descend_post_j1", "descend_post_j2",
        "descend_post_j3", "descend_post_j4", "descend_post_j5",
        "descend_delta_j0", "descend_delta_j1", "descend_delta_j2",
        "descend_delta_j3", "descend_delta_j4", "descend_delta_j5",
        "history",
    ]
    with open(csv_path, "w", newline="") as f:
        w = csv.DictWriter(f, fieldnames=fields)
        w.writeheader()
        w.writerows(results)
    print(f"\n[CSV保存] {csv_path} ({len(results)}点)")
    converged_count = sum(1 for r in results if r["converged"])
    diffs = [r["final_diff_x_mm"] for r in results
             if r["final_diff_x_mm"] != "" and r["final_diff_x_mm"] is not None]
    loops = [r["loop_count"] for r in results]
    print(f"  収束率: {converged_count}/{len(results)}")
    if loops:
        print(f"  平均ループ数: {sum(loops)/len(loops):.1f}")
    if diffs:
        print(f"  最終差分 avg={sum(abs(d) for d in diffs)/len(diffs):.1f}mm  max={max(abs(d) for d in diffs):.1f}mm")


def main():
    print("=" * 60)
    print("Visual Servo PICK — Dynamic Tracking (DAY185)")
    print("=" * 60)

    load_fk_table()

    cap = cv2.VideoCapture(CAMERA_INDEX)
    if not cap.isOpened():
        print(f"[ERROR] カメラ index={CAMERA_INDEX} を開けません")
        return
    for _ in range(30):
        cap.read()

    if not setup_homography_and_mat_range(cap):
        cap.release()
        return

    os.makedirs(DEBUG_DIR, exist_ok=True)
    results = []
    trial = 0

    cv2.namedWindow("R2R2R Tracking", cv2.WINDOW_NORMAL)
    cv2.resizeWindow("R2R2R Tracking", 1280, 720)
    print("[INFO] R2R2R Tracking ウィンドウ起動")

    try:
        send_home()

        while True:
            trial += 1
            print(f"\n{'='*60}")
            print(f"[Trial {trial}]")
            print(f"{'='*60}")

            resp = wait_for_input_with_preview(
                cap,
                f"[Trial {trial}] 赤ブロックをマット上に置いてEnterを押してください（qで終了）: ",
                f"WAITING for Trial {trial}")
            if resp == "q":
                break

            # 1. 赤ブロック初期検出
            red_init = detect_red_block_unity(cap)
            if red_init is None:
                print("[SKIP] 赤ブロック検出失敗")
                continue
            red_x_init, red_z_init = red_init
            print(f"[CAM] 赤ブロック Unity=({red_x_init:.4f}, {red_z_init:.4f})")

            # 2. 初回lookup
            approach_angles_lookup, pick_dist, pick_ee = lookup_nearest_angles_with_ee(
                red_x_init, red_z_init, PICK_EE_Y, label="PICK_INIT")
            if approach_angles_lookup is None:
                print("[SKIP] lookup失敗")
                continue
            initial_j0 = approach_angles_lookup[0]
            print(f"[LOOKUP] 角度: {[round(a,2) for a in approach_angles_lookup]}  initial_j0={initial_j0:+.2f}")

            # 3. グリッパー開 → APPROACH_PICKへ移動
            print(f"[ARM] グリッパー開く（PICK準備）...")
            ssh_set_gripper(state=0, speed=100, wait=2)
            approach_angles = [initial_j0] + list(APPROACH_PICK_BASE_DAY185[1:])
            print(f"[ARM] APPROACH_PICK へ移動（j1-j5=固定値）...")
            ssh_send_angles(approach_angles, wait=3)

            # 4. 動的追従ループ
            for f in glob.glob(os.path.join(DEBUG_DIR, "track_*.png")):
                os.remove(f)
            print(f"\n  === 動的追従ループ開始 ===")
            approach_base_for_servo = list(APPROACH_PICK_BASE_DAY185)
            final_j0, final_red_x_mm, success, history = dynamic_tracking_loop(
                cap, approach_base_for_servo, initial_j0)
            print(f"  === 追従終了 (success={success}) ===")

            if not success:
                print("[SKIP] 追従失敗、HOME復帰")
                send_home()
                results.append({
                    "trial": trial,
                    "pick_x_mm": round(red_x_init * 1000, 2),
                    "pick_z_mm": round(red_z_init * 1000, 2),
                    "initial_j0": round(initial_j0, 3),
                    "final_j0": round(final_j0, 3),
                    "converged": False,
                    "loop_count": len(history),
                    "final_tip_x_mm": "",
                    "final_diff_x_mm": "",
                    "descend_sent_j0": "", "descend_sent_j1": "", "descend_sent_j2": "",
                    "descend_sent_j3": "", "descend_sent_j4": "", "descend_sent_j5": "",
                    "descend_post_j0": "", "descend_post_j1": "", "descend_post_j2": "",
                    "descend_post_j3": "", "descend_post_j4": "", "descend_post_j5": "",
                    "descend_delta_j0": "", "descend_delta_j1": "", "descend_delta_j2": "",
                    "descend_delta_j3": "", "descend_delta_j4": "", "descend_delta_j5": "",
                    "history": str(history),
                })
                resp = wait_for_input_with_preview(
                    cap, "次のtrialへ進む場合はEnter / 終了する場合は 'q' + Enter: ",
                    "Trial 完了 / 次のTrialを待機中")
                if resp == 'q':
                    break
                continue

            # 5. DESCEND
            descend_angles = [
                final_j0, DESCEND_J1_DAY185, DESCEND_J2_DAY185,
                APPROACH_PICK_BASE_DAY185[3], APPROACH_PICK_BASE_DAY185[4], APPROACH_PICK_BASE_DAY185[5],
            ]

            # [OBS-DAY186] DESCEND前のangle取得
            pre_descend_angles = ssh_get_angles()
            if pre_descend_angles is not None:
                print(f"[OBS] DESCEND前 angle: {[round(a, 2) for a in pre_descend_angles]}")
            else:
                print(f"[OBS] DESCEND前 angle: 取得失敗")
            print(f"[OBS] DESCEND送信 angle: {[round(a, 2) for a in descend_angles]}")

            print(f"[ARM] DESCEND（j2={DESCEND_J2_DAY185}で肘伸ばし降下）...")
            ssh_send_angles(descend_angles, wait=3)

            # [OBS-DAY186] DESCEND後のangle取得 + 送/着ズレ計算
            post_descend_angles = ssh_get_angles()
            descend_delta = None
            if post_descend_angles is not None:
                print(f"[OBS] DESCEND後 angle: {[round(a, 2) for a in post_descend_angles]}")
                descend_delta = [post_descend_angles[i] - descend_angles[i] for i in range(6)]
                print(f"[OBS] 送/着ズレ (post - sent): {[round(d, 2) for d in descend_delta]}")
            else:
                print(f"[OBS] DESCEND後 angle: 取得失敗")

            # [DAY186] DESCEND後の微調整ループ
            print(f"\n=== DESCEND後 微調整ループ開始 ===")
            descend_angles_base = list(descend_angles)
            final_j0_refined, final_red_x_refined, refined_success, refined_history = \
                dynamic_tracking_loop(cap, descend_angles_base, descend_angles[0], refine_mode=True)
            print(f"=== DESCEND後 微調整ループ終了 (success={refined_success}) ===\n")

            # 6. 最終TIP測定（評価用、GRIP前）
            tip_unity_final, markers_final, _ = detect_gripper_tip_auto(cap, trial=f"trial_{trial:03d}")
            final_red_x = final_red_x_mm / 1000.0 if final_red_x_mm is not None else red_x_init
            if tip_unity_final is not None:
                tip_x_final = tip_unity_final[0] * 1000
                target_x_mm = final_red_x * 1000
                final_diff = target_x_mm - tip_x_final
                print(f"  [評価] 最終TIP_X={tip_x_final:+.1f}mm  目標との差={final_diff:+.1f}mm")
            else:
                tip_x_final = None
                final_diff = None
                markers_final = None
                target_x_mm = final_red_x * 1000

            # 7. GRIP
            print(f"[ARM] グリッパー閉じる（GRIP）...")
            ssh_set_gripper(state=1, speed=100, wait=2)

            # 8. LIFT
            lift_angles = [final_j0] + list(APPROACH_PICK_BASE_DAY185[1:])
            print(f"[ARM] LIFT（APPROACH姿勢へ上昇、j1-j5=固定値）...")
            ssh_send_angles(lift_angles, wait=4)

            # 9. 結果記録
            results.append({
                "trial": trial,
                "pick_x_mm": round(final_red_x * 1000, 2),
                "pick_z_mm": round(red_z_init * 1000, 2),
                "initial_j0": round(initial_j0, 3),
                "final_j0": round(final_j0, 3),
                "converged": True,
                "loop_count": len(history),
                "final_tip_x_mm": round(tip_x_final, 2) if tip_x_final is not None else "",
                "final_diff_x_mm": round(final_diff, 2) if final_diff is not None else "",
                "descend_sent_j0": round(descend_angles[0], 2),
                "descend_sent_j1": round(descend_angles[1], 2),
                "descend_sent_j2": round(descend_angles[2], 2),
                "descend_sent_j3": round(descend_angles[3], 2),
                "descend_sent_j4": round(descend_angles[4], 2),
                "descend_sent_j5": round(descend_angles[5], 2),
                "descend_post_j0": round(post_descend_angles[0], 2) if post_descend_angles else "",
                "descend_post_j1": round(post_descend_angles[1], 2) if post_descend_angles else "",
                "descend_post_j2": round(post_descend_angles[2], 2) if post_descend_angles else "",
                "descend_post_j3": round(post_descend_angles[3], 2) if post_descend_angles else "",
                "descend_post_j4": round(post_descend_angles[4], 2) if post_descend_angles else "",
                "descend_post_j5": round(post_descend_angles[5], 2) if post_descend_angles else "",
                "descend_delta_j0": round(descend_delta[0], 2) if descend_delta else "",
                "descend_delta_j1": round(descend_delta[1], 2) if descend_delta else "",
                "descend_delta_j2": round(descend_delta[2], 2) if descend_delta else "",
                "descend_delta_j3": round(descend_delta[3], 2) if descend_delta else "",
                "descend_delta_j4": round(descend_delta[4], 2) if descend_delta else "",
                "descend_delta_j5": round(descend_delta[5], 2) if descend_delta else "",
                "history": str(history),
            })

            # 10. HOME 復帰
            send_home()

            resp = wait_for_input_with_preview(
                cap, "次のtrialへ進む場合はEnter / 終了する場合は 'q' + Enter: ",
                "Trial 完了 / 次のTrialを待機中")
            if resp == 'q':
                break

    except KeyboardInterrupt:
        print("\n[Ctrl+C検出] HOME 復帰...")
        try:
            send_home()
        except Exception:
            pass
    except Exception as e:
        print(f"\n[ERROR] {e}")
        try:
            send_home()
        except Exception:
            pass
    finally:
        cap.release()
        cv2.destroyAllWindows()

    if results:
        save_csv(results)


if __name__ == "__main__":
    main()

このセッションが示したこと

Z方向の追従制御は設計上正しい方向に進んだ。j1+j2の逆方向連動から始まり、それが姿勢依存で効かないと判明し、j1+j2+j4の3関節同方向連動に進化した。DESCEND前後の角度観測パッチも追加し、送/着ズレが全関節1度以内に収まることを実測で確認した。

しかし「追従がうまくいかない」最大の原因は制御設計ではなく、通信アーキテクチャにあった。ssh_send_angles()が毎回SSH接続・Python起動・pymycobot import・time.sleep(2)を行う構造になっており、1ループ6〜8秒という数字が出た。

Z追従を追加したことでSSH送信が1ループ2回になり、これが問題を可視化した。逆に言えば、Z追従を追加したことでアーキテクチャの問題が露呈した。j0だけの追従なら1ループ3秒で、人間の感覚では「遅いが動いている」と見えていた。Z追従を入れた瞬間に「アームが動かない」と認識できる速度になった。

追従設計の精度を上げる前に、指令が十分な速度で届く状態を作ることが先だ。次のセッションでは軽量化レバー1（time.sleep(2)削除）から順番に入れ、1ループの処理時間を測定しながら段階的に進める。

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