mechArm 270 Pi 動的追従の限界を探る Log#4 | 動的追従の本体が回り始めた日 j0視覚サーボから2D収束判定へ
mechArm 270 Pi の物理応答速度を測ってきた前2本のあとで、ついに視覚サーボの本体を実装に持ち込んだ。「ブロックを指でゆっくり押して、止まるまで追う」という設計から始めて、j0だけのX方向追従、TIPと赤ブロックの差分検出、静止判定、2D距離による収束判定までを一気に通した。最終的に2D距離2.5mmで重なる瞬間に到達した。把持はまだ届かないが、追従の本体はようやく回り始めた。
💡 Key Takeaway
動的追従の設計は「停止して観察→動作」というステップ駆動を捨て、「動いている最中も観察と動作を並行する」連続制御に切り替えることから始まる。基準点は TIP に置く。赤ブロックと TIP を毎フレーム両方検出し、その差分で j0 を更新し続ける。静止判定（直近フレームの位置変動の標準偏差）を加えることで「動かしている最中は追い続け、止まった瞬間に把持へ移行する」という体験が成立する。
発生した問題 | 現場判断 | 結果 |
|---|---|---|
視覚サーボの最初の実装で j0 の更新方向が逆になり、追従どころか反対側に逃げた |
| 符号を直した直後から追従が成立 |
X方向は1mm以下まで詰まるのに、Z方向に95mmズレたまま把持に行ってしまう | 収束判定をX方向の差分から「X+Z両方の2D距離」に変更 | 2D距離2.5mm まで詰まった瞬間に STABLE+CONVERGED を出せるようになった |
APPROACH姿勢で爪が前傾して床に当たる、または爪先がブロックの上方に逃げる | 姿勢設計を「直立気味で爪先が真下を向く」方向に倒し、APPROACH と DESCEND を別の角度で持つよう分離 | 直立姿勢に近づいたが、DESCEND後の爪先位置はまだブロックの真横に届かない（次セッションへ） |
なぜステップ駆動を捨てたか
このシリーズの Log#1 と Log#2 で見えていたのは、mechArm の物理応答が振幅10°で約500ms（2Hz）かかるという数字だった。一方で set_fresh_mode(1) を有効にすればソフトウェアは91Hzで命令を送れる。「送れる速さ」と「動ける速さ」は45倍の差がある。
この前提のもとで動的追従を設計するとき、最初に捨てるべきは「停止して観察 → 計算 → 動作 → 停止して観察」というステップ駆動だ。1サイクル3秒かかる構造では、「指でゆっくり動いている対象」にすら追いつけない。観察した位置にアームが到達した頃には、対象はもうそこにいない。
代わりに「動いている最中も観察を続け、毎フレーム最新の目標値で命令を上書きする」連続制御に切り替える。set_fresh_mode(1) はそのためにある機能だ。前回の動作完了を待たずに次の指令で上書きできる。
本セッションで実装したのは、この構造変更の最初の形だ。ループ周期を 0.3 秒に縮め、毎ループで赤ブロックと TIP の両方を検出し、差分で j0 を即座に更新する。ループ周期 0.3 秒、SPEED=20 でアームが動き続ける時間と命令送信が並行する。
「ゆっくり止まる」という設計の選択
設計の方針を決める前に、対象の動き方を定義する必要があった。動的追従と一口に言っても、対象がどう動くかで制御方式が変わる。
ベルトコンベア的に X方向だけに流れる
手で自由に動かす（XZ両方向）
ゆっくり押されて、止まるまで追う
3つ目を選んだ。これが選ばれた瞬間に、設計の核がはっきりする。
「ゆっくり止まる」対象なら、既存の j0 視覚サーボに 静止判定 を足せば成立する。把持タイミングを「収束したら」から「静止して、かつ収束していたら」に変えるだけだ。
追従ループ（無限）:
① ブロック位置を検出（履歴N=5フレーム保持）
② TIP位置を検出 → diff計算
③ 静止判定: 直近5フレームの位置変動 ≤ 1.0mm
if 静止 and |diff| ≤ 10mm:
→ 把持に移行（break）
if |diff| > 2mm: # デッドバンド
→ j0更新（追従継続）
sleepこの構造が成立すると、体験はこうなる。人がブロックを動かしている間、アームは静かに j0 を更新して追従し続ける。指を離してブロックが静止した瞬間にアームが把持しに行く。
検出ノイズと静止判定の整合
静止判定の閾値はノイズに対して十分なマージンを取る必要がある。mechArm のカメラ検出ノイズは前々セッションで実測してあった。
項目 | stdev |
|---|---|
red_x_mm | 0.00mm |
tip_x_mm | 0.08mm |
diff_x（合成） | 約0.08mm |
静止判定閾値 STABLE_THRESHOLD_MM = 1.0mm は、検出ノイズの約12倍のマージン。誤判定リスクなし。「指でゆっくり押す」動きは1秒に数mmは動くため、動と静の分離は明確に可能。最終的にこの閾値は 1.0mm から 2.0mm に緩めた（後述）。
基準点はTIP、毎フレーム再検出
LOG178で確立していたのは「カメラで実機の爪先と赤ブロックの両方を測って差分を直接フィードバックする」という閉ループ思想だった。FK や Unity シミュレーションの誤差は全部無視できる。実機の測定値だけで完結する。
動的追従でこの思想を維持するには、TIPも毎フレーム再検出する必要がある。なぜなら追従中は j0 が動くたびに TIP の絶対座標が変わるからだ。「初回TIPを基準にred_xを追う」のではなく、毎ループで「TIP位置」と「ブロック位置」を両方カメラで取り直し、その差分で j0 を更新する。
LOG178の検出パイプラインは画像のような精度を出していた：
赤ブロック: pixel=(649, 818) → 位置: (233.3mm, 259.2mm)
青#0: pixel=(727, 613) → 位置: (193.1mm, 156.6mm)
青#1: pixel=(576, 665) → 位置: (269.6mm, 181.9mm)
青#2: pixel=(654, 756) → 位置: (230.5mm, 228.2mm)
TIP (中点): pixel=(615, 710) → 位置: (250.0mm, 204.8mm)青3点から TIP を計算するロジックは、Y座標で並べて画面下側の2点（=爪先2点）の中点を取る方式だった。後にLOG184で「最短辺判定」（3点の組み合わせのうち距離が最短のペアを爪先2点とする）に切り替え、TIP の stdev が 19.42mm → 0.08mm まで改善されている。本セッションの動的追従でもこの最短辺方式を使う。
最初の罠：j0の更新方向が逆だった
実装の初版では、視覚サーボループの delta_deg 計算をこう書いた。
delta_deg = -diff_mm / MM_PER_DEG_J0_Xこれが致命的なバグだった。MM_PER_DEG_J0_X = -6.63 は「j0を+1°→TIPはX方向に-6.63mm動く」という係数。すでに「動く方向の符号」を含んでいる。
diff_mm = +20mm（赤ブロックがTIPより20mm右）
TIPを右（+X）へ動かしたい → j0は負方向へ動かす必要がある（係数が-6.63のため）
正しい式: delta_deg = +20 / -6.63 = -3.02° ✅（j0を負方向＝正解）
バグ: delta_deg = -(+20) / -6.63 = +3.02° ❌（j0を正方向＝逆走）頭の - が二重否定で逆走させていた。Log#3 で確定済みの正しい式は delta_deg = diff_x_mm / MM_PER_DEG_J0_X。1行修正して直った。
直した直後の trial で、TIPは赤ブロックに向かって正しい方向に動き始めた。X方向の追従は問題なく成立する。
第2の罠：X方向だけ収束しても把持に行ってはいけない
修正後、ログは確かに「X方向は1mm以下まで詰まる」状態になった。しかしウィンドウを目視すると、TIPの十字マーカーは赤ブロックの横にあって、丸の中には入っていなかった。
[Track] t=71.7s diff=-0.4mm（X方向はほぼ完璧）
[STABLE+CONVERGED] 把持に移行ターミナルログでは確かに収束しているが、画面では：
TIP (50, 274)mm
RED (62, 300)mm
X方向は12mmズレ、Z方向は26mmズレ
X方向だけで収束判定していたから、Z方向のズレが残ったまま把持に行ってしまっていた。
設計を変更した。diff_mm を diff_x_mm にrenameし、diff_z_mm と diff_2d_mm = sqrt(dx² + dz²) を追加。把持条件を 2D距離 ≤ CONVERGE_THRESHOLD_MM (5mm) に変更した。
ただし j0 の更新（追従動作）は X方向だけでよい。j0 を回しても Z方向にはほとんど効かない（LOG180 の実測で確認済み）から、Z方向の差分を使って j0 を動かしても収束しない。j0 更新は diff_x_mm だけで判定し、収束判定だけ 2D距離を使う、という非対称な設計になる。
第3の罠：APPROACH姿勢が爪先を床に押し付ける
姿勢設計でも段階的に詰まった。
最初の APPROACH姿勢は [None, 87.74, -38.33, 3.6, -70.11, -30.0]（LOG180の確定値）。これでは爪先が床に近すぎて、追従中にブロックや床に当たる。
j2 を浅くして肘を曲げる方向に振れば爪先が上がるはずだと考えて、-38.33 → -55.0 に変えた。今度は爪先が高すぎて、DESCEND時に PICK_J1_FIXED=90.87 で降下しても床に届かない。「爪が浮いたまま挟もうとする」状態。
両極端を経て、-45.0 で「爪先が床から数cm浮いた高さ」に落ち着いた。X方向の追従は機能し、収束も確認できた。
ところが画像で見ると、アームが斜めに前傾している。爪先を真下に向けつつアームを直立させるには、j2 だけでなく j4（手首の折り角）も同時に変える必要があった。j2=-20.0、j4=-90.0 に振ると、アームが直立気味になり、爪先が真下を向く理想形に近づいた。
そしてここで、DESCEND が新しい問題を生んだ。直立姿勢から「j1だけを 87.74 → 93.0 に5°深くする」と、爪先は下ではなく斜め前方に動く。ブロックの上方に逃げてしまう。
DESCEND専用の角度を新設して、j1ではなく j2 を動かす方針に変更：
APPROACH姿勢: j1=87.74, j2=-20.0 ← 爪先が床から数cm浮いた直立姿勢
DESCEND姿勢: j1=87.74, j2=-10.0 ← j2だけ10°浅くして、肘を伸ばし爪先を下にj1 は変えない。j2 だけ伸ばすことで爪先を下方向に降ろす。これで DESCEND は「肘を伸ばして爪先を真下に降ろす純粋降下」になる。
連続制御で発見した波及問題
実装を進める中で、姿勢以外にも調整が連鎖した。
APPROACH 姿勢の j2-j5 を lookup値ではなく固定値にする。当初の実装では APPROACH の j1 だけを APPROACH_PICK_BASE_DAY185[1] の固定値とし、j2-j5 は lookup_angles_lookup[2:] を使っていた。これだと爪先の高さがブロック位置依存になり、定数で設定した j2 が機能しない。
# 修正前
approach_angles = [initial_j0, APPROACH_PICK_BASE_DAY185[1]] + list(approach_angles_lookup[2:])
# 修正後
approach_angles = [initial_j0] + list(APPROACH_PICK_BASE_DAY185[1:])これで APPROACH/DESCEND/LIFT すべてが「j0だけブロック位置依存、j1-j5は固定」という設計に統一された。
TIP検出の青シール検出を全画面に変更。当初は青シールも _mat_px_range（マット範囲）でフィルタしていたが、APPROACH姿勢が変わると爪がマット枠の外にはみ出して、青シールが除外されて 3点揃わなくなった。引き継ぎ書には「stdev=0.00mm（全画面検出モードで確認）」と書かれていた通り、青シールは全画面検出が正解だった。
macOSスレッドの罠
ウィンドウ表示の実装でも一度ハマった。
最初は「Trial開始のEnter待ち中もウィンドウのプレビューを更新したい」という要件から、バックグラウンドスレッドでカメラを読んで描画する設計を試した。
def preview_loop():
"""別スレッドで動くプレビュー描画ループ"""
while True:
if not _preview_active:
time.sleep(0.05)
continue
# frame取得→検出→imshow
cv2.imshow("R2R2R Tracking", overlay)
cv2.waitKey(1)これは macOS で破綻した。cv2.imshow はメインスレッドからしか呼べない（macOS の Cocoa GUI 制約）。サブスレッドで imshow を呼ぶとウィンドウが出ない、または出てもフリーズする。
設計を反転させた。入力（input()）の方をサブスレッドで読み、imshow はメインスレッドが回し続ける。
_input_buffer = []
_input_lock = threading.Lock()
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()待ちの間、メインスレッドでカメラフレームを描画し続ける"""
_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:
# 検出と描画
...
cv2.imshow("R2R2R Tracking", overlay)
cv2.waitKey(1)
else:
time.sleep(0.05)
return user_inputinput() は GUI を触らないのでサブスレッドでも問題ない。imshow は常にメインスレッドで呼ばれる。これで macOS でも安定して動く。
連続制御ループの実装
設計が固まった後の dynamic_tracking_loop 関数。X方向のみで j0 を更新するが、収束判定は X+Z の 2D距離で行う非対称な設計。
def dynamic_tracking_loop(cap, approach_angles_base, initial_j0):
"""
赤ブロックとTIPを毎ループ検出し、j0を連続更新して追従する。
赤ブロックが静止し、かつ2D距離が収束したら把持に移行する。
"""
cv2.namedWindow("R2R2R Tracking", cv2.WINDOW_NORMAL)
j0_current = initial_j0
red_x_history = []
history = []
start_time = time.time()
last_red_z_mm = None
loop_count = 0
while True:
loop_count += 1
elapsed = time.time() - start_time
# タイムアウト
if elapsed > MAX_TRACKING_SEC:
print(f"[TIMEOUT] {MAX_TRACKING_SEC}秒超過。追従中断。")
return j0_current, None, last_red_z_mm, False, history
# 単一フレーム取得
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, rz = pixel_to_unity(red_pixel[0], red_pixel[1])
red_x_mm = rx * 1000
red_z_mm = rz * 1000
red_xz_mm = (red_x_mm, red_z_mm)
last_red_z_mm = red_z_mm
# TIP検出
tip_pixel, blue_markers, tip_unity = detect_tip_from_frame(frame)
tip_xz_mm = None
tip_x_mm = None
tip_z_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)
# 検出失敗時は履歴リセットして次ループ
if red_x_mm is None or tip_x_mm is None:
red_x_history = []
time.sleep(LOOP_INTERVAL_SEC)
continue
# diff計算（X方向、Z方向、2D距離）
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)
# ジャンプ検出（30mm以上のジャンプは履歴リセット）
if len(red_x_history) > 0:
last_red_x = red_x_history[-1]
if abs(red_x_mm - last_red_x) > 30.0:
print(f"[JUMP] red_x={red_x_mm:.1f} (前={last_red_x:.1f}) ジャンプ検出、履歴リセット")
red_x_history = []
# 履歴更新
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
# ログ
print(f"[Track] t={elapsed:.1f}s diff_x={diff_x_mm:+.1f}mm "
f"diff_z={diff_z_mm:+.1f}mm 2d={diff_2d_mm:.1f}mm "
f"j0={j0_current:+.2f} red_x={red_x_mm:+.1f} tip_x={tip_x_mm:+.1f} "
f"stable={stable} stdev={stdev:.2f}")
history.append({
"loop": loop_count,
"elapsed_sec": round(elapsed, 1),
"red_x_mm": round(red_x_mm, 1),
"tip_x_mm": round(tip_x_mm, 1),
"diff_x_mm": round(diff_x_mm, 1),
"diff_z_mm": round(diff_z_mm, 1),
"diff_2d_mm": round(diff_2d_mm, 1),
"j0": round(j0_current, 3),
"stable": stable,
"stdev": round(stdev, 2),
})
# 把持条件: 2D距離が収束 + 静止
if diff_2d_mm > CONVERGE_THRESHOLD_MM:
stable_for_grip = False
else:
stable_for_grip = stable
if stable_for_grip:
print(f"[STABLE+CONVERGED] 把持に移行 (2D dist={diff_2d_mm:.1f}mm)")
return j0_current, red_x_mm, red_z_mm, True, history
# j0更新（X方向のdiffだけで判定）
if 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
# 範囲制限（initial_j0基準で±60°）
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
new_angles = [j0_new] + list(approach_angles_base[1:])
ssh_send_angles(new_angles, wait=0)
j0_current = j0_new
# オーバーレイ描画
status = (f"t={elapsed:.1f}s\n"
f"diff_x={diff_x_mm:+.1f} diff_z={diff_z_mm:+.1f} 2d={diff_2d_mm:.1f}\n"
f"j0={j0_current:+.2f}\n"
f"stable={stable} stdev={stdev:.2f}")
overlay = draw_tracking_overlay(frame, red_pixel, blue_markers,
tip_pixel, red_xz_mm, tip_xz_mm, status)
cv2.imshow("R2R2R Tracking", overlay)
cv2.waitKey(1)
time.sleep(LOOP_INTERVAL_SEC)実行ログ：2D距離2.5mmまで詰まった瞬間
最終的に動いた trial のログから、収束に至るループの後半を抜粋する。
=== 動的追従ループ開始 ===
[Track] t=0.0s diff_x=-0.0mm diff_z=+31.9mm 2d=31.9mm j0=-32.00 red_x=+184.5 tip_x=+184.5 stable=False stdev=inf
[Track] t=0.5s diff_x=-0.0mm diff_z=+31.9mm 2d=31.9mm j0=-32.00 red_x=+184.5 tip_x=+184.5 stable=False stdev=inf
[Track] t=1.1s diff_x=+0.5mm diff_z=+31.9mm 2d=31.9mm j0=-32.00 red_x=+185.0 tip_x=+184.5 stable=False stdev=inf
[Track] t=1.5s diff_x=-0.0mm diff_z=+31.9mm 2d=31.9mm j0=-32.00 red_x=+184.5 tip_x=+184.5 stable=False stdev=inf
[Track] t=2.1s diff_x=-0.3mm diff_z=+31.9mm 2d=31.9mm j0=-32.00 red_x=+184.5 tip_x=+184.8 stable=True stdev=0.20
（ブロックを動かしている期間）
[Track] t=4.4s diff_x=+73.0mm diff_z=+76.8mm 2d=105.9mm j0=-32.00 red_x=+257.5 tip_x=+184.6 stable=False stdev=29.09
[Track] t=13.6s diff_x=-10.1mm diff_z=+51.3mm 2d=52.3mm j0=-37.00 red_x=+198.2 tip_x=+208.3 stable=False stdev=28.20
[Track] t=22.3s diff_x=-2.7mm diff_z=+45.3mm 2d=45.3mm j0=-35.47 red_x=+198.2 tip_x=+200.9 stable=False stdev=27.08
[Track] t=31.0s diff_x=-2.5mm diff_z=+45.0mm 2d=45.1mm j0=-35.06 red_x=+198.2 tip_x=+200.6 stable=False stdev=25.64
[Track] t=39.7s diff_x=-2.2mm diff_z=+45.0mm 2d=45.1mm j0=-34.69 red_x=+198.2 tip_x=+200.4 stable=False stdev=23.73
[Track] t=48.4s diff_x=+3.5mm diff_z=+40.7mm 2d=40.8mm j0=-34.36 red_x=+198.2 tip_x=+194.7 stable=True stdev=0.00
（さらに押した）
[Track] t=57.0s diff_x=+41.1mm diff_z=+58.7mm 2d=71.6mm j0=-34.89 red_x=+238.5 tip_x=+197.4 stable=False stdev=16.11
[Track] t=65.8s diff_x=+18.2mm diff_z=+78.0mm 2d=80.1mm j0=-39.89 red_x=+238.5 tip_x=+220.3 stable=False stdev=19.73
[Track] t=74.5s diff_x=+6.5mm diff_z=+89.9mm 2d=90.2mm j0=-42.63 red_x=+238.5 tip_x=+232.0 stable=False stdev=19.73注目すべきは、X方向の diff_x が 1mm 以下まで詰まっているのに、Z方向の diff_z は 95mm のまま残り続けるところ。j0 は X方向にしか効かないので、Z方向のズレは構造的に詰まらない。
そして別のtrialで、ブロックを TIP の真下に置いた条件で再度回したとき：
[Track] t=36.4s diff_x=+0.6mm diff_z=+1.7mm 2d=1.8mm j0=-34.55 red_x=+234.0 tip_x=+233.5 stable=False stdev=11.11
[Track] t=36.8s diff_x=+0.1mm diff_z=+0.2mm 2d=0.3mm j0=-34.55 red_x=+233.5 tip_x=+233.5 stable=True stdev=0.40
[STABLE+CONVERGED] 把持に移行そしてもう1つの記録に値する trial：
[Track] t=62.8s diff_x=+5.7mm diff_z=+0.3mm 2d=5.7mm j0=-32.00 red_x=+62.1 tip_x=+56.4 stable=True stdev=0.73
[STABLE+CONVERGED] 把持に移行 (2D dist=5.7mm)そして本セッションの最良結果：
[STABLE+CONVERGED] 把持に移行 (2D dist=2.5mm)2D距離 2.5mm。TIPと赤ブロックが画面上でほぼ完全に重なる位置。ブロックを置く位置が「TIPの真下に近い」状態で、人がそれをわずかに動かして止めた瞬間に成立する条件。動的追従の本体はここで初めて回り始めた。
把持の最後の壁
ただし、把持自体はまだ届かなかった。STABLE+CONVERGED で DESCEND に移行するが、降下後の TIP がブロックの上方や横にズレてしまう。
[STABLE+CONVERGED] 把持に移行 (2D dist=2.5mm)
[ARM] DESCEND（純粋降下、j2-j5=固定値）...
ANGLES: [-12.21, 90.26, -19.16, 3.16, -91.75, -30.67]
[評価] 最終TIP_X=+75.5mm 目標との差=+6.4mmDESCEND後に X方向で +6.4mm のズレが残った。降下中に j2 が変わることでアームの肩が動き、爪先が斜めに進むためだ。爪はブロックの真上に届かず、横にずれた位置で閉じてしまう。
これは姿勢設計の問題で、DESCEND の角度をさらに詰める必要がある。直立から肘を伸ばす方向に j2 を変える設計までは入れたが、変位量と方向の最適化が足りていない。次のセッションで詰める。
このセッションが残したもの
このセッションで動いたのは、追従の本体だけだ。把持はまだ詰まっていない。それでも、設計の枠は完全に変わった。
ステップ駆動から連続制御へ。FK 計算から実機計測の閉ループへ。X方向だけの追従から 2D 距離での収束判定へ。j0 の符号バグから直立姿勢への試行錯誤まで含めて、設計の各レイヤーが「動的に動く対象を捕まえる」という1つの目的に向かって整理された。
「ロボットの動きはデザインできる」という主張の中に、この日付けで「ゆっくり動かして止めるまで追って、止まった瞬間に取りに行く」という体験が含まれた。これが小さなデモにすぎないとしても、設計レイヤーで通しで作ったのは初めてだ。
次のセッションでは把持の最後の数mmを詰める。DESCEND の姿勢、グリッパー閉じのタイミング、LIFT 後の角度復帰。動的追従の体験を「掴めた」で締めくくれるかどうか、ここから試す。
視覚サーボピックのフルコード
#!/usr/bin/env python3
"""
visual_servo_pick.py
DAY185改：動的追従モード。
APPROACH_PICK姿勢を維持したまま、赤ブロックとTIPを毎ループ検出し、
j0を連続更新して追従する。赤ブロックが静止し、かつ2D距離が収束したら把持に移行する。
"""
import os
import sys
import math
import csv
import time
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 = -10.0
PICK_J1_FIXED = 93.0
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 = 5.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 = 2.0
TRACK_DEADBAND_MM = 2.0
MAX_TRACKING_SEC = 600.0
LOOP_INTERVAL_SEC = 0.3
J0_RELATIVE_LIMIT_DAY185 = 60.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 input_thread_func(prompt):
"""別スレッドでinput()を実行し、結果を_input_bufferに入れる。"""
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()待ちの間、カメラフレームを描画し続ける。戻り値: ユーザー入力文字列"""
_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
# ---- FK テーブル -----------------------------------------------------------
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)
# ---- Homography + マット範囲 -----------------------------------------------
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)
mat_x_mm = float(r[0][0][0])
mat_z_mm = float(r[0][0][1])
unity_x = mat_x_mm * 0.001
unity_z = mat_z_mm * 0.001
return unity_x, unity_z
# ---- 赤ブロック検出 ---------------------------------------------------------
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)
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if not contours:
return []
candidates = []
for cnt in contours:
area = cv2.contourArea(cnt)
if area < 50:
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((cx, cy, area))
candidates.sort(key=lambda c: c[2], reverse=True)
return candidates[:3]
def detect_tip_from_frame(frame):
"""frame（既に取得済み）からTIPを検出する。
最短辺判定で爪先2点を選び、その中点をTIPとする。"""
blue_markers = detect_blue_markers(frame, None)
if not blue_markers or len(blue_markers) < 3:
return None, blue_markers, None
top3 = blue_markers[:3]
pairs = [(0, 1), (1, 2), (0, 2)]
min_dist = float("inf")
tip_pair = None
for i, j in pairs:
d = ((top3[i][0] - top3[j][0])**2 + (top3[i][1] - top3[j][1])**2) ** 0.5
if d < min_dist:
min_dist = d
tip_pair = (top3[i], top3[j])
tip1, tip2 = tip_pair
mid_x = (tip1[0] + tip2[0]) // 2
mid_y = (tip1[1] + tip2[1]) // 2
tip_pixel = (mid_x, mid_y)
tx_unity, tz_unity = pixel_to_unity(mid_x, mid_y)
return tip_pixel, blue_markers, (tx_unity, tz_unity)
# ---- オーバーレイ描画 ------------------------------------------------------
def draw_tracking_overlay(frame, red_pixel, blue_markers, tip_pixel,
red_xz_mm, tip_xz_mm, status_text):
"""検出結果をframeに描画して返す"""
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, (mx, my, _) in enumerate(blue_markers[:3]):
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
# ---- SSH ------------------------------------------------------------------
def ssh_send_angles(angles, speed=SPEED, wait=5):
"""SSH経由でPi上のアームにsend_anglesを実行"""
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_set_gripper(state, speed=100, wait=2):
"""SSH経由でグリッパーを開閉する。state=0:開く, state=1:閉じる"""
cmd = (
f"python3 -c \""
f"from pymycobot.mycobot import MyCobot; import time; "
f"mc = MyCobot('/dev/ttyAMA0', 1000000); time.sleep(2); "
f"r = mc.set_gripper_state({state}, {speed}); time.sleep({wait}); "
f"print('GRIPPER: {'CLOSE' if state == 1 else 'OPEN'} result=', r)\""
)
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 send_home():
"""HOMEへ移動"""
print("[ARM] HOME へ移動...")
ssh_send_angles(HOME, wait=4)
print("[ARM] HOME 完了")
# ---- 動的追従ループ（DAY185） ----------------------------------------------
def dynamic_tracking_loop(cap, approach_angles_base, initial_j0):
"""
赤ブロックとTIPを毎ループ検出し、j0を連続更新して追従する。
赤ブロックが静止し、かつ2D距離が収束したら把持に移行する。
引数:
cap: cv2.VideoCapture
approach_angles_base: j1〜j5の基準姿勢リスト（j0は後で上書き）
initial_j0: 初期j0（lookup値）
戻り値:
(final_j0, final_red_x_mm, final_red_z_mm, success, history)
"""
cv2.namedWindow("R2R2R Tracking", cv2.WINDOW_NORMAL)
j0_current = initial_j0
red_x_history = []
history = []
start_time = time.time()
last_red_z_mm = None
loop_count = 0
while True:
loop_count += 1
elapsed = time.time() - start_time
if elapsed > MAX_TRACKING_SEC:
print(f"[TIMEOUT] {MAX_TRACKING_SEC}秒超過。追従中断。")
return j0_current, None, last_red_z_mm, False, history
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, rz = pixel_to_unity(red_pixel[0], red_pixel[1])
red_x_mm = rx * 1000
red_z_mm = rz * 1000
red_xz_mm = (red_x_mm, red_z_mm)
last_red_z_mm = red_z_mm
tip_pixel, blue_markers, tip_unity = detect_tip_from_frame(frame)
tip_xz_mm = None
tip_x_mm = None
tip_z_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)
if red_x_mm is None or tip_x_mm is None:
red_x_history = []
status_fail = (f"t={elapsed:.1f}s\n"
f"RED:{'OK' if red_x_mm is not None else 'NG'} "
f"TIP:{'OK' if tip_x_mm is not None else 'NG'}\n"
f"j0={j0_current:+.2f}")
overlay = draw_tracking_overlay(frame, red_pixel, blue_markers,
tip_pixel, red_xz_mm, tip_xz_mm, status_fail)
cv2.imshow("R2R2R Tracking", overlay)
cv2.waitKey(1)
print(f"[Track] t={elapsed:.1f}s "
f"RED:{'OK' if red_x_mm is not None else 'NG'} "
f"TIP:{'OK' if tip_x_mm is not None else 'NG'} "
f"j0={j0_current:+.2f}")
time.sleep(LOOP_INTERVAL_SEC)
continue
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)
if len(red_x_history) > 0:
last_red_x = red_x_history[-1]
if abs(red_x_mm - last_red_x) > 30.0:
print(f"[JUMP] red_x={red_x_mm:.1f} (前={last_red_x:.1f}) "
f"ジャンプ検出、履歴リセット")
red_x_history = []
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
print(f"[Track] t={elapsed:.1f}s diff_x={diff_x_mm:+.1f}mm "
f"diff_z={diff_z_mm:+.1f}mm 2d={diff_2d_mm:.1f}mm "
f"j0={j0_current:+.2f} red_x={red_x_mm:+.1f} tip_x={tip_x_mm:+.1f} "
f"stable={stable} stdev={stdev:.2f}")
history.append({
"loop": loop_count,
"elapsed_sec": round(elapsed, 1),
"red_x_mm": round(red_x_mm, 1),
"tip_x_mm": round(tip_x_mm, 1),
"diff_x_mm": round(diff_x_mm, 1),
"diff_z_mm": round(diff_z_mm, 1),
"diff_2d_mm": round(diff_2d_mm, 1),
"j0": round(j0_current, 3),
"stable": stable,
"stdev": round(stdev, 2),
})
if diff_2d_mm > CONVERGE_THRESHOLD_MM:
stable_for_grip = False
else:
stable_for_grip = stable
if stable_for_grip:
print(f"[STABLE+CONVERGED] 把持に移行 (2D dist={diff_2d_mm:.1f}mm)")
return j0_current, red_x_mm, red_z_mm, True, history
if 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:
print(f"[RANGE_WARN] j0={j0_new:.2f} 制限超過、クランプして継続")
sign = 1 if (j0_new - initial_j0) > 0 else -1
j0_new = initial_j0 + sign * J0_RELATIVE_LIMIT_DAY185
new_angles = [j0_new] + list(approach_angles_base[1:])
ssh_send_angles(new_angles, wait=0)
j0_current = j0_new
status = (f"t={elapsed:.1f}s\n"
f"diff_x={diff_x_mm:+.1f} diff_z={diff_z_mm:+.1f} 2d={diff_2d_mm:.1f}\n"
f"j0={j0_current:+.2f}\n"
f"stable={stable} stdev={stdev:.2f}")
overlay = draw_tracking_overlay(frame, red_pixel, blue_markers,
tip_pixel, red_xz_mm, tip_xz_mm, status)
cv2.imshow("R2R2R Tracking", overlay)
cv2.waitKey(1)
time.sleep(LOOP_INTERVAL_SEC)
# ---- CSV保存 ---------------------------------------------------------------
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", "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)}点)")
# ---- メイン ----------------------------------------------------------------
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}")
prompt = f"[Trial {trial}] 赤ブロックをマット上に置いてEnterを押してください（qで終了）: "
status = f"WAITING for Trial {trial}"
resp = wait_for_input_with_preview(cap, prompt, status)
if resp == "q":
print("[INFO] q押下、終了します")
break
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})")
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]} "
f"initial_j0={initial_j0:+.2f}")
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)
print(f"\n === 動的追従ループ開始 ===")
approach_base_for_servo = list(APPROACH_PICK_BASE_DAY185)
final_j0, final_red_x_mm, final_red_z_mm, success, history = dynamic_tracking_loop(
cap, approach_base_for_servo, initial_j0)
print(f" === 追従終了 (success={success}) ===")
if not success:
send_home()
continue
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],
]
print(f"[ARM] DESCEND（純粋降下、j2-j5=固定値）...")
ssh_send_angles(descend_angles, wait=3)
tip_unity_final, markers_final, _ = detect_tip_from_frame(cap.read()[1])
if tip_unity_final is not None:
tip_x_final = tip_unity_final[0] * 1000
final_diff = (final_red_x_mm or 0) - tip_x_final
print(f" [評価] 最終TIP_X={tip_x_final:+.1f}mm 目標との差={final_diff:+.1f}mm")
print(f"[ARM] グリッパー閉じる（GRIP）...")
ssh_set_gripper(state=1, speed=100, wait=2)
lift_angles = [final_j0] + list(APPROACH_PICK_BASE_DAY185[1:])
print(f"[ARM] LIFT（APPROACH姿勢へ上昇、j1-j5=固定値）...")
ssh_send_angles(lift_angles, wait=4)
send_home()
prompt2 = "次のtrialへ進む場合はEnter / 終了する場合は 'q' + Enter: "
resp2 = wait_for_input_with_preview(cap, prompt2, "Trial完了 / 次を待機中")
if resp2 == "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()
