mechArm 270 Pi 動的追従の限界を探る Log#20 | アームが動かない 4 回の修正と4 回の失敗
誤診を潰していくと、最後に本当の原因だけが残る。
Key Takeaway
LOG209 の PLACE_APPROACH 1 回成立をベースラインに、爪の接触と j5 回転を潰すはずだった。だが実機を立ち上げた瞬間から物理の挙動が変わる。daemon_get_angles の空応答、叩いても動かない send_angles、HOME 定数の誤読、fresh_mode を巡る暗中模索。直すたびに、別の「動かない理由」が目の前に露出した。xyz_track_test.py は動く。place_test.py だけが沈黙する。差分の在処は最後まで掴めなかった。
発生した問題 | 現場判断 | 結果 |
|---|---|---|
daemon の | Pi 上で | Phase 0 通過。get_angles の不安定性は構造的に解消。恒久修正として残った |
Phase 2 で | fresh_mode を 4 段階で書き換え。送信前後で切替、wait=1→0、send_angles 直後に再投入、xyz_track_test.py の LOG208 流儀へ全面書き換え | アームは動かない。 |
一時的に HOME 定数 | 直後のテストで measured の値が変わり、誤判断と判明。即座に戻した | 「目視の HOME」と「コード上の HOME」の不一致は誤認。過去に投げた |
何を確かめる一日だったか
LOG209 で PLACE_APPROACH の 1 回成立まで漕ぎ着けた。次は再現性。爪が箱に接触する問題と、APPROACH で j5 が回転する問題を順に潰す。Phase 2 が安定して通れば、物理計測フェーズに入って Phase 3 DESCEND 設計の前提を固める。
実機を立ち上げる。前日まで動いていた place_test.py が、Phase 0 の HOME 姿勢確認で必ず ABORT する。修正を重ねても、別の場所で別の理由で止まる。
再現性を確認するはずの一日は、再現すらしない一日に変わった。
起動チェック: カメラ index と daemon
セッション開始の手順は決まっている。Continuity Camera が AVFoundation に乗っているか、Iriun が起きているか、index が前回と同じか。
import cv2
for i in range(4):
cap = cv2.VideoCapture(i)
if cap.isOpened():
ret, frame = cap.read()
if ret:
cv2.imwrite(f"/tmp/cam_check_{i}.png", frame)
print(f"index {i}: opened, frame={frame.shape}")
else:
print(f"index {i}: opened but no frame")
else:
print(f"index {i}: NOT opened")
cap.release()結果。
index 0: opened, frame=(1080, 1920, 3)
index 1: opened, frame=(1080, 1920, 3)
index 2: opened, frame=(1080, 1920, 3)目視で 1 が横カメラ、2 が上カメラ。LOG209 と同じ。
Pi 側で daemon を起動する。
ubuntu@ubuntu:~/r2r2r-scripts$ nohup python3 mecharm_daemon.py > ~/mecharm_daemon.log 2>&1 &
[1] 13519
ubuntu@ubuntu:~/r2r2r-scripts$ tail -20 ~/mecharm_daemon.log
[DAEMON 15:03:12] MyCobot 初期化中 (/dev/ttyAMA0, baud=1000000)...
[DAEMON 15:03:12] ✓ MyCobot 初期化完了
[DAEMON 15:03:12] ポート 5555 で待ち受け中...ここまでは正常。準備完了、のはずだった。
get_angles が空応答を返す
place_test.py を起動する。Phase 0 で必ず ABORT。
[ 2.76s] Phase 0 START: HOME 姿勢確認
[WARN] daemon_get_angles failed: daemon error: get_angles returned empty/invalid after 3 retries
[ 3.39s] Phase 0 ABORT: 現在角度取得失敗
[ 3.39s] Phase 0 ABORT: ブロック保持されてない最後の「ブロック保持されてない」は古い誤メッセージで、本質は 1 行上。get_angles returned empty/invalid after 3 retries。
daemon を通さず、Pi 上で直接 pymycobot を叩く。
python3 -c "
from pymycobot import MyCobot
import time
mc = MyCobot('/dev/ttyAMA0', 1000000)
for i in range(10):
a = mc.get_angles()
print(f'try {i}: {a}')
time.sleep(0.2)
"
try 0: []
try 1: []
try 2: []
try 3: []
try 4: []
try 5: []
try 6: [0.0, 0.7, 1.58, 0.26, 0.26, -29.7]
try 7: [0.0, 0.7, 1.58, 0.26, 0.26, -29.7]
try 8: [0.0, 0.7, 1.58, 0.26, 0.26, -29.7]
try 9: [0.0, 0.7, 1.58, 0.26, 0.26, -29.7]最初の 6 回が空。7 回目から安定する。pymycobot はシリアル初期化完了まで [] を返す仕様だった。3 回 × 100ms = 0.3 秒のリトライでは絶対に届かない。約 1.2 秒の暖機が必要だった。
mecharm_daemon.py を以下の方針で codex に書き直してもらった。
リトライを 3 回 × 100ms から 12 回 × 200ms に拡張
daemon 起動時に warm-up 処理を追加（
MyCobot()初期化直後に get_angles を最大 15 回試行し、有効値が返るまで待ってから「ポート 5555 で待ち受け中...」を表示）全コマンドに
cmd received: ...ログ追加send_angles 後の角度取得も同じ 12 回リトライ化
再起動後の daemon log。
[DAEMON 15:09:58] MyCobot 初期化中 (/dev/ttyAMA0, baud=1000000)...
[DAEMON 15:09:58] ✓ MyCobot 初期化完了
[DAEMON 15:09:58] warm-up中...
[DAEMON 15:09:58] warm-up OK (try 7): [0.0, 0.7, 1.58, 0.26, 0.26, -29.7]
[DAEMON 15:09:58] ポート 5555 で待ち受け中...try 7 で warm-up 完了。Phase 0 は安定して通るようになった。
send_angles を送ってもアームが動かない
Phase 0 通過、Phase 1 で白箱検出に成功する。
[ 3.16s] Phase 0 OK: HOME 近傍確認 max_err=1.40°
[ 3.77s] Phase 1 START: 上カメラで白箱検出
[ 21.61s] Phase 1 DETECT: 白箱 center=(1302,852) score=0.731 angle=0° scale=1.15
[ 21.61s] Phase 1 OK: robot 座標 X=-159.7mm Z=302.0mm
[ 21.79s] Phase 2 START: APPROACH target=(X=-159.7mm, Y=95.0mm above PICK_EE_Y, Z=302.0mm) -> target_ee_y=0.0550m
[Mac] PLACE_APPROACH 姿勢制約後候補数: 76213点
[FK] target=(X=-0.1597, Y=0.0550, Z=0.3020) matched ee=(-0.160142, 0.057245, 0.302147) dist=0.5mm angles=[23.0, 69.91, -22.14, 3.92, -38.27, 0.0]
[ 21.97s] Phase 2 SEND: send_angles [23.0, 69.91, -22.14, 3.92, -38.27, 0.0] speed=20
[Pi] fresh_mode=1
[Pi] fresh_mode=0
[ 31.09s] Phase 2 REACH: measured=[-0.08, 1.49, 0.79, -0.08, -0.26, -30.23] max_err=70.31°
[ 31.09s] Phase 2 ABORT: APPROACH 角度誤差超過 max_err=70.31° (tolerance=3.0°)送信値 j1=69.91° に対し measured j1=1.49°。max_err=70.31°。アームは動いていない。
別ターミナルから直接 Python ワンライナーで daemon_send_angles を叩く。
python3 -c "
import visual_servo_pick as vsp
import time
vsp.get_daemon_socket()
m = vsp.daemon_get_angles()
print(f'before: {[round(a,2) for a in m] if m else None}')
target = [0, -10, 0, 0, 0, -30]
print(f'sending: {target}')
vsp.daemon_send_angles(target, speed=20, wait=1)
time.sleep(4)
m = vsp.daemon_get_angles()
print(f'after: {[round(a,2) for a in m] if m else None}')
vsp.close_daemon_socket()
"
socket OK
before: [0.26, 0.79, 0.61, 0.0, -0.26, -29.7]
sending: [0, -10, 0, 0, 0, -30]
after: [0.26, -8.96, 0.61, 0.0, -0.26, -29.7]j1 が 0.79° から -8.96° へ動いた。daemon・pymycobot・シリアル通信は正常。違いは呼び出し側のコードだけだった。
j5 が APPROACH で 0° に飛ぶ
修正の途中、Phase 2 SEND まで通る試行が一度あった。
[ 21.85s] Phase 2 SEND: send_angles [50.0, 65.17, 16.63, 2.9, -80.57, 0.0] speed=20
[ 29.92s] Phase 2 REACH: measured=[18.36, 77.87, -25.57, -7.03, -47.98, -29.17] max_err=70.31°送信 angles の j5=0.0。HOME の j5=-30°。HOME→APPROACH で j5 が 30° 回転している。爪が箱に近づきながら回転する。安全リスクが高い。
lookup_fk は (X, Y, Z) を満たす最近傍の FK 点を返す。j5 はエンドエフェクタの回転だけなので、最近傍の j5 が偶然 0° だっただけ。
最小の対応として、lookup_fk の戻り値の j5 を HOME[5] = -30 に書き換える FIX_J5 を入れた。
angles = lookup_fk(X_box, target_ee_y, Z_box)
if angles is None:
log_phase(txt, csv_writer, 2, "ABORT", "lookup_fk 失敗")
return False
# j5 を HOME と同じ値に固定（爪の回転を防ぐ）
angles = list(angles)
angles[5] = HOME[5]
log_phase(txt, csv_writer, 2, "FIX_J5",
f"j5 を HOME 値 {HOME[5]} に固定 angles={[round(a, 2) for a in angles]}")以降のログに Phase 2 FIX_J5: j5 を HOME 値 -30 に固定 が出るようになった。アームが回転せずに動くベースラインができた、はずだった。
HOME 定数を疑う
途中、HOME の定数 [0, 0, 0, 0, 0, -30] が実機の物理 HOME とずれていると疑った瞬間があった。
[ 2.77s] Phase 0 ABORT: HOME 姿勢から外れている
max_err=60.73°
measured=[0.26, -60.73, 0.52, 0.17, -0.26, -29.7]
expected=[0, 0, 0, 0, 0, -30]目視ではアームは HOME 姿勢に見える。measured の j1=-60.73° が「目視 HOME」の真の値ではないか。定数を [0, -60, 0, 0, 0, -30] に書き換えた。
直後のテスト。
[ 2.80s] Phase 0 ABORT: HOME 姿勢から外れている
max_err=60.79°
measured=[0.26, 0.79, 0.61, 0.0, 0.43, -29.7]
expected=[0, -60, 0, 0, 0, -30]measured が変わった。書き換えた定数とまた合わない。
ここで気づいた。アームは動いている。1 回目のテストで CLEANUP の HOME 復帰が実行され、j1=-60 から j1=0 へ動いた結果が 2 回目に反映されただけだった。
「測定が推論に先立つ」という原則を、自分の手で破っていた。定数を即座に元に戻す。ただ、コードと物理の HOME が本当に一致しているかを徹底的に疑う視点だけは、この失敗で手元に残った。
fresh_mode を疑った 4 回の書き換え
ここから fresh_mode との戦いが始まる。pymycobot の set_fresh_mode(1) は「新指令で前指令をキャンセル即実行」を有効にするモード。LOG208 で動的追従 PICK を成立させた鍵だった。これの使い方を間違えているのではないか。
書き換え 1: 「動作中だけ ON にすべきだ」
仮説。fresh_mode は常時 ON だと get_angles が不安定になる。動かす瞬間だけ立てて、すぐ落とせばいい。
try:
daemon_set_fresh_mode(1)
except Exception as e:
log_phase(txt, csv_writer, 2, "WARN", f"fresh_mode=1 切替失敗: {e}")
try:
daemon_send_angles(angles, speed=SPEED, wait=1)
except Exception as e:
log_phase(txt, csv_writer, 2, "ERROR", f"daemon_send_angles 例外: {e}")
return False
time.sleep(APPROACH_WAIT_SEC)
try:
daemon_set_fresh_mode(0)
except Exception as e:
log_phase(txt, csv_writer, 2, "WARN", f"fresh_mode=0 切替失敗: {e}")動かない。
書き換え 2: 「動いていない」のではなく「命令が捨てられている」
daemon ログを見ると [Pi] fresh_mode=0 が送信直後に出ている。daemon 側の send_angles が内部で fresh_mode を 0 に戻している疑い。それなら Mac 側でもう一度立て直せばいい。
try:
daemon_set_fresh_mode(1)
except Exception:
...
try:
daemon_send_angles(angles, speed=SPEED, wait=1)
except Exception:
...
try:
daemon_set_fresh_mode(1) # 再投入
except Exception:
...
time.sleep(APPROACH_WAIT_SEC)
try:
daemon_set_fresh_mode(0)
except Exception:
...動かない。
書き換え 3: 「fresh_mode=0 が動作開始前に切られている」
仮説の方向を変える。fresh_mode=0 のタイミングが APPROACH_WAIT_SEC の前にあって、動作開始前に切られている。sleep の後ろに動かす。
実機ログ。
[ 22.02s] Phase 2 SEND: send_angles [46.0, 110.74, -43.33, 3.6, -68.11, -30.0] speed=20
[WARN] daemon_get_angles failed: daemon error: get_angles returned empty/invalid after 3 retries
[ 30.75s] Phase 2 WARN: 到達後の角度取得失敗。目視確認へ進む8.7 秒の sleep を挟んでも measured が空。これも違う。
書き換え 4: 「fresh_mode=1 が伝わる前に send_angles が来ている」
Pi 側 daemon log を精読する。
[DAEMON 17:01:08] set_fresh_mode(1)
[DAEMON 17:01:08] send_angles 呼び出し前: angles=[33.0, 97.91, -42.14, -12.08, -62.27, -30], speed=20, wait=1.0
[DAEMON 17:01:08] send_angles 呼び出し後: result=None, elapsed_ms=11.8
[DAEMON 17:01:09] send_angles 後の現在角度: [-0.08, 1.49, 0.79, -0.08, -0.26, -30.23]
[DAEMON 17:01:17] set_fresh_mode(0)set_fresh_mode(1) と send_angles 呼び出し前 が 同秒内 (ミリ秒間隔)。pymycobot は result=None, elapsed_ms=11.8 で即座に戻る。実機は動かない。
MyCobot 本体に fresh_mode=1 が伝わる前に send_angles が来ているのではないか。間に 0.3 秒の sleep を挟む。
try:
daemon_set_fresh_mode(1)
except Exception:
...
time.sleep(0.3) # MyCobot 側に fresh_mode=1 が伝わるのを待つ
try:
daemon_send_angles(angles, speed=SPEED, wait=1)
except Exception:
...動かない。
xyz_track_test.py を読み返す
ここで一度立ち止まり、LOG208 で動的追従 PICK を成立させた xyz_track_test.py を読み直した。冒頭のコメント。
# - send_coords() 使用禁止 → daemon_send_angles() のみ
# - daemon の wait 引数は機能しないため Mac 側で time.sleep() を明示そして main の構造。
get_daemon_socket()
daemon_set_fresh_mode(1) # main で 1 回だけ立てる
try:
daemon_send_angles(HOME_ANGLES, speed=SPEED, wait=0)
time.sleep(HOME_WAIT_SEC)
daemon_set_gripper(0, speed=100, wait=0)
time.sleep(2.5)
daemon_send_angles(APPROACH_BASE, speed=SPEED, wait=0)
# ...送信を何度でも繰り返す...
finally:
daemon_set_fresh_mode(0) # 最後に 0 に戻す
daemon_send_angles(HOME_ANGLES, speed=SPEED, wait=0)
time.sleep(HOME_WAIT_SEC)3 つの違いに気づいた。
fresh_mode=1は main の最初に 1 回だけ立てるdaemon_send_anglesの wait は 0。wait=1は効かない動作完了は Mac 側
time.sleep()で明示的に待つ
place_test.py は全部逆をやっていた。phase_0_init でも phase_2_approach でも fresh_mode を毎回切り替え、send_angles は wait=1 で投げ、その後 sleep。
書き直す。
xyz_track_test.py 流儀への全面書き換え
claude code に全面書き換えを指示した。
phase_0_initの fresh_mode 切替を削除phase_2_approachの fresh_mode 切替を削除、wait=1をwait=0にmain()の daemon 接続直後にdaemon_set_fresh_mode(1)を 1 回だけfinallyの最後にdaemon_set_fresh_mode(0)を 1 回だけfinallyの HOME 復帰もwait=0+time.sleep(5)
書き換え後の Phase 2 部分。
log_phase(txt, csv_writer, 2, "SEND",
f"send_angles {[round(a, 2) for a in angles]} speed={SPEED}")
try:
daemon_send_angles(angles, speed=SPEED, wait=0)
except Exception as e:
log_phase(txt, csv_writer, 2, "ERROR", f"daemon_send_angles 例外: {e}")
return False
# 動作完了を Mac 側 time.sleep で待つ。
# プレビュー更新を回しながら APPROACH_WAIT_SEC 経過するまで待機。
t_start = time.time()
while time.time() - t_start < APPROACH_WAIT_SEC:
update_preview(side_cap, top_cap, "Phase 2 APPROACH")
time.sleep(0.1)実機テスト。
[ 24.36s] Phase 2 START: APPROACH target=(X=-197.1mm, Y=95.0mm above PICK_EE_Y, Z=253.6mm) -> target_ee_y=0.0550m
[Mac] PLACE_APPROACH 姿勢制約後候補数: 76213点
[FK] target=(X=-0.1971, Y=0.0550, Z=0.2536) matched ee=(-0.197309, 0.045802, 0.253467) dist=0.2mm angles=[33.0, 97.91, -42.14, -12.08, -62.27, -29.44]
[ 24.52s] Phase 2 FIX_J5: j5 を HOME 値 -30 に固定 angles=[33.0, 97.91, -42.14, -12.08, -62.27, -30]
[ 24.52s] Phase 2 SEND: send_angles [33.0, 97.91, -42.14, -12.08, -62.27, -30] speed=20
[ 33.78s] Phase 2 REACH: measured=[-0.08, 1.49, 0.79, -0.08, -0.26, -30.23] max_err=96.42°動かない。
念のため xyz_track_test.py を単独で起動した。アームは動いた。
ハードは正常だった。崩れていたのは、仮説の方だった。
どこに差分があるのか
ここで日が尽きた。
xyz_track_test.py と place_test.py の差分は、fresh_mode の使い方では説明できない場所にある。明日の検証対象は以下。
xyz_track_test.pyはmain冒頭でcap.read()を 30 回空回ししてカメラを温める。place_test.pyにもあるが、デバイスオープンとスレッド起動の順序が狂っている疑いxyz_track_test.pyはmake_approach_anglesで姿勢を再構築する。place_test.pyは FK lookup の生値の j5 だけを書き換えて送る。MyCobot 側の関節制限の排他条件に引っかかっている疑いdaemon 接続から最初の send_angles までの時間間隔
send_angles の前に
daemon_set_gripperを呼んでいるかどうか送る angles の値そのものに MyCobot 側で拒否される条件が含まれている可能性
1 行ずつ照合する。
残ったもの
動かなかった一日の中で、確実に残った成果がある。
カメラプレビューのリアルタイム表示
cv2.imshow で side / top の両ウィンドウを常時開き、各フェーズに合わせてラベルを書き換える。APPROACH_WAIT_SEC の sleep ループ内でも 0.1 秒刻みでフレーム更新を回す。
def update_preview(side_cap, top_cap, label=""):
global _current_detection_overlay
try:
ret_s, frame_s = side_cap.read()
ret_t, frame_t = top_cap.read()
if ret_s and frame_s is not None:
disp_s = frame_s.copy()
if label:
cv2.putText(disp_s, label, (20, 40),
cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
cv2.imshow("side", disp_s)
if ret_t and frame_t is not None:
disp_t = frame_t.copy()
if _current_detection_overlay is not None:
d = _current_detection_overlay
cx, cy = d["center"]
score = d["score"]
angle = d.get("angle", 0) or 0
scale = d.get("scale", 1.0) or 1.0
base_size = 200
half = (base_size * scale) / 2.0
rect = ((cx, cy), (2 * half, 2 * half), float(angle))
box_pts = cv2.boxPoints(rect)
box_pts = box_pts.astype(int)
cv2.polylines(disp_t, [box_pts], isClosed=True,
color=(0, 255, 0), thickness=3)
cv2.circle(disp_t, (cx, cy), 10, (0, 0, 255), -1)
cv2.putText(disp_t,
f"DETECTED: ({cx},{cy}) score={score:.3f} angle={angle} scale={scale}",
(20, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
if label:
cv2.putText(disp_t, label, (20, 40),
cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
cv2.imshow("top", disp_t)
cv2.waitKey(1)
except Exception as e:
print(f"[PREVIEW] 更新例外: {e}")検出結果の回転矩形オーバーレイ
Phase 1 で白箱検出に成功した瞬間に _current_detection_overlay をセットし、以降の update_preview 呼び出しで bbox と center を上カメラに重畳する。cv2.boxPoints((center, size, angle)) で回転矩形を計算、cv2.polylines で描画。
実機ログ。
[ 24.16s] Phase 1 DETECT: 白箱 center=(1466,749) score=0.775 angle=30° scale=1.15
[ 24.16s] Phase 1 OK: robot 座標 X=-197.2mm Z=253.7mm
[PHASE 1 PREVIEW] 検出オーバーレイを以降のプレビューで持続表示DETECTED: (1466,749) score=0.775 angle=30 scale=1.15 の文字情報と一緒に、白箱の輪郭にぴったり合う緑の回転矩形が描かれる。検出ロジックが正しく動いていることが視覚的に確認できる装置になった。
終了時の自動 HOME 復帰
Ctrl+C / Phase ABORT / 正常終了のいずれも、finally で HOME へ戻す。
finally:
try:
print("[CLEANUP] HOME 復帰中...")
daemon_send_angles(HOME, speed=SPEED, wait=0)
time.sleep(5)
measured = daemon_get_angles()
if measured is not None and len(measured) == 6:
err = max(abs(measured[i] - HOME[i]) for i in range(6))
print(f"[CLEANUP] HOME 復帰完了 max_err={err:.2f}°")
log_phase(txt, csv_writer, -1, "CLEANUP",
f"HOME 復帰完了 max_err={err:.2f}° "
f"measured={[round(a, 2) for a in measured]}")
else:
print("[CLEANUP] HOME 復帰後の angles 取得失敗")
log_phase(txt, csv_writer, -1, "CLEANUP",
"HOME 復帰後の angles 取得失敗")
except Exception as e:
print(f"[CLEANUP] HOME 復帰例外: {e}")次回起動時に「アームが ABORT 姿勢のまま放置されて Phase 0 で ABORT」というループが切れる。
各フェーズで上下カメラの自動キャプチャ
Phase 0 OK 後、Phase 2 完了後、CLEANUP 完了後の 3 か所で画像を自動保存する。
def capture_both_cameras(side_cap, top_cap, timestamp, phase_label, txt=None, csv_writer=None):
saved = []
for cap, tag in [(side_cap, "side"), (top_cap, "top")]:
try:
ret, frame = cap.read()
if not ret or frame is None:
continue
path = os.path.join(
LOGS_DIR,
f"place_test_{timestamp}_{phase_label}_{tag}.png"
)
cv2.imwrite(path, frame)
saved.append(path)
except Exception as e:
print(f"[CAPTURE] {tag} 保存例外: {e}")
return saveddaemon の安定性強化
mecharm_daemon.py の get_angles は 12 回 × 200ms + 起動時 warm-up に強化された。今後の DAY でも恩恵が続く。
失敗の構造
4 回の修正のうち、本当に問題を解いたのは 1 回だけだ。残りの 3 回は、問題が存在しない場所をこじ開けようとしていた。診断のベクトルが間違っている。
xyz_track_test.py は今も動き、place_test.py だけが沈黙している。fresh_mode は犯人ではなかった。真因は明日、引きずり出す。
本当の原因は、修正しても消えない。
place_test.py 全文 (LOG210 セッション終了時点)
xyz_track_test.py 流儀への書き換え後の状態で、py_compile は通っているがアームを動かさないコード。明日の出発点。
#!/usr/bin/env python3
"""
place_test.py (DAY209)
PLACE 動作スクリプト (Phase 0〜2 のみ)。
赤ブロックを爪に CLOSE 保持した状態で HOME から開始し、白箱上空 Y=75mm まで
移動して停止する。Phase 3 (DESCEND) 以降は DAY210 以降で段階追加する予定。
恒久ルール:
- send_coords() 禁止
- release_all_servos() 禁止
- fk_scan_motion_based.csv は読込のみ
- homography.npy は変更しない
"""
import csv as csv_module
import os
import sys
import time
from datetime import datetime
import cv2
import numpy as np
import visual_servo_pick as vsp
from visual_servo_pick import (
PICK_EE_Y,
close_daemon_socket,
daemon_get_angles,
daemon_send_angles,
daemon_set_fresh_mode,
daemon_set_gripper,
get_daemon_socket,
load_fk_table,
lookup_nearest_angles_with_ee,
pixel_to_unity,
setup_homography_and_mat_range,
)
from test_white_box_score import (
detect_mat_range as _detect_top_mat_range,
evaluate_frame as _evaluate_white_box,
load_templates as _load_white_box_templates,
)
# ============================================================
# 定数
# ============================================================
# ---- 物理計測 ----------------------------------------------------------
BOX_HEIGHT_MM = 45.0
APPROACH_OFFSET_MM = 50.0
LIFT_EXTRA_MM = 40.0
Y_APPROACH_MM = BOX_HEIGHT_MM + APPROACH_OFFSET_MM # 75
Y_LIFT_ABS_MM = BOX_HEIGHT_MM + APPROACH_OFFSET_MM + LIFT_EXTRA_MM # 115 (Phase 5 用、未使用)
# ---- 横カメラ・赤検出（test_place_detection_offline.py と同じ HSV）----
SIDE_CAMERA_INDEX = 1
RED_H_MIN_1, RED_H_MAX_1 = 0, 10
RED_H_MIN_2, RED_H_MAX_2 = 170, 180
RED_S_MIN, RED_V_MIN = 100, 80
RED_MIN_AREA = 100
RED_MORPH_KERNEL = 5
# Phase 0 の赤検出 ROI（画像比率、HOME 姿勢で爪が映る中央付近）
RED_ROI_X_MIN_RATIO = 0.30
RED_ROI_X_MAX_RATIO = 0.70
RED_ROI_Y_MIN_RATIO = 0.30
RED_ROI_Y_MAX_RATIO = 0.85
# bbox サイズの妥当性範囲（赤ブロックは画角内で概ね 60〜200px 角）
RED_BBOX_MIN_SIDE = 40
RED_BBOX_MAX_SIDE = 300
# ---- 上カメラ・白箱検出 -------------------------------------------------
# 注: 回転角度 / スケールの実値は test_white_box_score.evaluate_frame の内部定数で
# 駆動される。ここで定義しても挙動に反映されないため定数は持たない。
TOP_CAMERA_INDEX = 2
TEMPLATE_DIR = os.path.expanduser("~/r2r2r-scripts/templates")
TEMPLATE_SCORE_THRESHOLD = 0.5
# ---- グリッパー -------------------------------------------------------
GRIPPER_CLOSE = 1 # visual_servo_pick.daemon_set_gripper の規約に合わせる: 1=閉
GRIPPER_OPEN = 0 # 0=開（Phase 4 で使用、Phase 0〜2 では未使用）
# ---- 姿勢 -------------------------------------------------------------
HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
APPROACH_WAIT_SEC = 8.0 # APPROACH 到達待ち。j1 の大移動を含む可能性あり
HOME_ANGLE_TOLERANCE_DEG = 5.0 # HOME 近傍判定の許容誤差
MAX_APPROACH_ERROR_DEG = 3.0 # APPROACH 到達後の角度誤差許容値
ALLOW_HOME_CHECK_BYPASS_ON_READ_FAIL = True # get_angles 不調時は手動 HOME 確認済み前提で続行
ALLOW_REACH_CHECK_BYPASS_ON_READ_FAIL = True # get_angles 不調時でも送信後は目視確認へ進む
# ---- 関節制限（xyz_track_test と同等）-----------------------------------
JOINT_LIMITS = [
(-160.0, 160.0), # j0
( -90.0, 130.0), # j1
(-170.0, 40.0), # j2
(-150.0, 150.0), # j3
(-100.0, 100.0), # j4
(-180.0, 180.0), # j5
]
# ---- パス -------------------------------------------------------------
HOMOGRAPHY_PATH = os.path.expanduser("~/r2r2r-scripts/homography.npy")
FK_TABLE_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")
LOGS_DIR = os.path.expanduser("~/r2r2r-scripts/logs")
# ============================================================
# ログ
# ============================================================
def setup_logging(timestamp):
"""テキストログと CSV ログのファイルハンドルを作成。
戻り値: (txt_handle, csv_handle, csv_writer)
CSV ヘッダ: loop, time, phase, event, detail
"""
os.makedirs(LOGS_DIR, exist_ok=True)
txt_path = os.path.join(LOGS_DIR, f"place_test_{timestamp}.txt")
csv_path = os.path.join(LOGS_DIR, f"place_test_{timestamp}.csv")
txt_handle = open(txt_path, "w")
csv_handle = open(csv_path, "w", newline="")
csv_writer = csv_module.writer(csv_handle)
csv_writer.writerow(["loop", "time", "phase", "event", "detail"])
csv_handle.flush()
print(f"[LOG] txt: {txt_path}")
print(f"[LOG] csv: {csv_path}")
return txt_handle, csv_handle, csv_writer
_log_start_time = None
# 検出オーバーレイ情報 (Phase 1 で確定したら以降ずっと描画する)
_current_detection_overlay = None # dict or None
def log_phase(txt, csv_writer, phase, event, detail=""):
"""両方のログに 1 行書く。stdout にも echo する。"""
global _log_start_time
now = time.time()
if _log_start_time is None:
_log_start_time = now
elapsed = now - _log_start_time
line = f"[{elapsed:6.2f}s] Phase {phase} {event}: {detail}"
print(line)
if txt is not None:
txt.write(line + "\n")
txt.flush()
if csv_writer is not None:
csv_writer.writerow(["", f"{elapsed:.3f}", phase, event, detail])
# ============================================================
# 初期化系
# ============================================================
def open_cameras():
"""横カメラと上カメラを開く。失敗時 (None, None)。"""
side_cap = cv2.VideoCapture(SIDE_CAMERA_INDEX)
if not side_cap.isOpened():
print(f"[ERROR] 横カメラ open 失敗 (index={SIDE_CAMERA_INDEX})")
return None, None
for _ in range(30):
side_cap.read()
top_cap = cv2.VideoCapture(TOP_CAMERA_INDEX)
if not top_cap.isOpened():
print(f"[ERROR] 上カメラ open 失敗 (index={TOP_CAMERA_INDEX})")
side_cap.release()
return None, None
for _ in range(30):
top_cap.read()
return side_cap, top_cap
def capture_both_cameras(side_cap, top_cap, timestamp, phase_label, txt=None, csv_writer=None):
"""横カメラと上カメラから1枚ずつ画像を保存する。
保存先:
logs/place_test_{timestamp}_{phase_label}_side.png
logs/place_test_{timestamp}_{phase_label}_top.png
phase_label: "phase0", "phase2_approach", "cleanup" など短い識別子。
"""
saved = []
for cap, tag in [(side_cap, "side"), (top_cap, "top")]:
try:
ret, frame = cap.read()
if not ret or frame is None:
msg = f"[CAPTURE] {tag} カメラ read 失敗"
print(msg)
if txt is not None and csv_writer is not None:
log_phase(txt, csv_writer, -1, "CAPTURE_WARN",
f"{tag} read 失敗 phase={phase_label}")
continue
path = os.path.join(
LOGS_DIR,
f"place_test_{timestamp}_{phase_label}_{tag}.png"
)
cv2.imwrite(path, frame)
saved.append(path)
print(f"[CAPTURE] {tag}: {path}")
except Exception as e:
print(f"[CAPTURE] {tag} 保存例外: {e}")
if txt is not None and csv_writer is not None and saved:
log_phase(txt, csv_writer, -1, "CAPTURE",
f"phase={phase_label} saved={saved}")
return saved
def update_preview(side_cap, top_cap, label=""):
"""両カメラのフレームを1枚ずつ取得し cv2.imshow に流す。
_current_detection_overlay が設定されていれば、上カメラに bbox と center を描画する。
"""
global _current_detection_overlay
try:
ret_s, frame_s = side_cap.read()
ret_t, frame_t = top_cap.read()
if ret_s and frame_s is not None:
disp_s = frame_s.copy()
if label:
cv2.putText(disp_s, label, (20, 40),
cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
cv2.imshow("side", disp_s)
if ret_t and frame_t is not None:
disp_t = frame_t.copy()
# 検出オーバーレイを描画
if _current_detection_overlay is not None:
d = _current_detection_overlay
cx, cy = d["center"]
score = d["score"]
angle = d.get("angle", 0) or 0
scale = d.get("scale", 1.0) or 1.0
# テンプレの基底サイズ (px) - 大体 200x200 px と仮定
base_size = 200
half = (base_size * scale) / 2.0
rect = ((cx, cy), (2 * half, 2 * half), float(angle))
box_pts = cv2.boxPoints(rect)
box_pts = box_pts.astype(int)
cv2.polylines(disp_t, [box_pts], isClosed=True,
color=(0, 255, 0), thickness=3)
cv2.circle(disp_t, (cx, cy), 10, (0, 0, 255), -1)
cv2.putText(disp_t,
f"DETECTED: ({cx},{cy}) score={score:.3f} angle={angle} scale={scale}",
(20, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
if label:
cv2.putText(disp_t, label, (20, 40),
cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
cv2.imshow("top", disp_t)
cv2.waitKey(1)
except Exception as e:
print(f"[PREVIEW] 更新例外: {e}")
def load_homography_via_top_cap(top_cap):
"""上カメラからマット四隅を検出して動的 homography を構築。
visual_servo_pick.setup_homography_and_mat_range を流用。
成功時 True / 失敗時 False。
"""
return setup_homography_and_mat_range(top_cap)
# ============================================================
# 検出系
# ============================================================
def detect_red_block(frame_bgr, use_roi=False, validate_bbox_size=False):
"""横カメラの 1 フレームから赤ブロックの bbox を取得。
HSV 2 区間マスク（0-10, 170-180）。
Args:
frame_bgr: 入力フレーム
use_roi: True なら画像中央の ROI に限定して検出（Phase 0 用）
validate_bbox_size: True なら bbox サイズの妥当性をチェック（Phase 0 用）
戻り値: {"bbox": (x, y, w, h), "bottom_y": int} または None。
"""
h, w = frame_bgr.shape[:2]
if use_roi:
rx_min = int(w * RED_ROI_X_MIN_RATIO)
rx_max = int(w * RED_ROI_X_MAX_RATIO)
ry_min = int(h * RED_ROI_Y_MIN_RATIO)
ry_max = int(h * RED_ROI_Y_MAX_RATIO)
roi = frame_bgr[ry_min:ry_max, rx_min:rx_max]
else:
rx_min, ry_min = 0, 0
roi = frame_bgr
hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
mask1 = cv2.inRange(
hsv,
(RED_H_MIN_1, RED_S_MIN, RED_V_MIN),
(RED_H_MAX_1, 255, 255),
)
mask2 = cv2.inRange(
hsv,
(RED_H_MIN_2, RED_S_MIN, RED_V_MIN),
(RED_H_MAX_2, 255, 255),
)
mask = mask1 | mask2
k = np.ones((RED_MORPH_KERNEL, RED_MORPH_KERNEL), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, k)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, k)
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if not contours:
return None
largest = max(contours, key=cv2.contourArea)
if cv2.contourArea(largest) < RED_MIN_AREA:
return None
x, y, bw, bh = cv2.boundingRect(largest)
if validate_bbox_size:
if bw < RED_BBOX_MIN_SIDE or bw > RED_BBOX_MAX_SIDE:
return None
if bh < RED_BBOX_MIN_SIDE or bh > RED_BBOX_MAX_SIDE:
return None
# ROI 座標系から frame 座標系へ戻す
abs_x = x + rx_min
abs_y = y + ry_min
return {"bbox": (abs_x, abs_y, bw, bh), "bottom_y": abs_y + bh}
def detect_white_box_top_camera(top_cap, templates_by_group):
"""上カメラから 1 フレーム取得し、テンプレマッチングで白箱中心を取得。
test_white_box_score.evaluate_frame の overall_best を流用。
マット bbox は visual_servo_pick._mat_px_range（setup_homography 直後に確定）を再利用。
注: evaluate_frame は本来スコア評価用の関数。本番系（place_test）が
そのまま流用しているため、検出ロジックの差がそのまま PLACE 移動先の差になる。
将来は白箱検出関数を test_white_box_score から独立させ、両者で共有する設計が望ましい。
戻り値: {"center": (cx, cy), "score": float, "angle": int, "scale": float}
または None（スコア < 閾値の場合も None）。
"""
if vsp._mat_px_range is None:
return None
for _ in range(2):
top_cap.read()
ret, frame = top_cap.read()
if not ret or frame is None:
return None
mat_bbox = vsp._mat_px_range
results = _evaluate_white_box(frame, mat_bbox, templates_by_group)
best = results.get("overall_best")
if best is None or best.get("best_score") is None:
return None
if best["best_score"] < TEMPLATE_SCORE_THRESHOLD:
return {
"center": (best["cx"], best["cy"]),
"score": best["best_score"],
"angle": best.get("best_angle"),
"scale": best.get("best_scale"),
"frame": frame,
"below_threshold": True,
}
return {
"center": (best["cx"], best["cy"]),
"score": best["best_score"],
"angle": best.get("best_angle"),
"scale": best.get("best_scale"),
"frame": frame,
"below_threshold": False,
}
def pixel_to_robot_xz(cx, cy):
"""pixel 座標 → robot Unity 座標 (X, Z) [m]。
visual_servo_pick.pixel_to_unity をそのまま使う。
homography が未ロードならエラーになる前提（呼び出し前に setup 済み想定）。
"""
unity_x, unity_z = pixel_to_unity(cx, cy)
return unity_x, unity_z
# ============================================================
# 動作系
# ============================================================
def lookup_fk(X, Y, Z):
"""FK lookup table から (X, Y, Z) [m] に対応する角度を取得。
X = unity_x (= mat 横方向), Z = unity_z (= mat 奥行), Y = ee_y (= 高さ)。
visual_servo_pick.lookup_nearest_angles_with_ee を呼ぶ。失敗時 None。
"""
angles, dist, ee = lookup_nearest_angles_with_ee(X, Z, Y, label="PLACE_APPROACH")
if angles is None:
print(f" [WARN] lookup_fk 失敗 dist={dist*1000:.1f}mm")
return None
print(f" [FK] target=(X={X:.4f}, Y={Y:.4f}, Z={Z:.4f}) "
f"matched ee={ee} dist={dist*1000:.1f}mm "
f"angles={[round(a, 2) for a in angles]}")
return angles
def safety_check_angles(angles):
"""各関節が JOINT_LIMITS 内か確認する。"""
for i, a in enumerate(angles):
lo, hi = JOINT_LIMITS[i]
if not (lo <= a <= hi):
print(f"[ABORT] j{i} 安全範囲超過: {a:.2f}° not in [{lo}, {hi}]")
return False
return True
# ============================================================
# Phase 系
# ============================================================
def phase_0_init(side_cap, txt, csv_writer):
"""初期化チェック。現在角度を取得して HOME 近傍か確認、外れていれば ABORT。
get_angles が取得失敗した場合は、実機切り分け用に手動 HOME 確認済み前提で続行する。
Phase 0〜2 のテストではブロック保持確認は不要のため、赤検出は行わない。
Phase 3 DESCEND 実装時にブロック保持確認を別途追加する。
"""
log_phase(txt, csv_writer, 0, "START", "HOME 姿勢確認")
measured = daemon_get_angles()
if measured is None or len(measured) != 6:
if ALLOW_HOME_CHECK_BYPASS_ON_READ_FAIL:
log_phase(txt, csv_writer, 0, "WARN",
"現在角度取得失敗。手動 HOME 確認済み前提で続行")
return True
else:
log_phase(txt, csv_writer, 0, "ABORT", "現在角度取得失敗")
return False
max_home_err = max(abs(measured[i] - HOME[i]) for i in range(6))
if max_home_err > HOME_ANGLE_TOLERANCE_DEG:
log_phase(txt, csv_writer, 0, "ABORT",
f"HOME 姿勢から外れている max_err={max_home_err:.2f}° "
f"(tolerance={HOME_ANGLE_TOLERANCE_DEG}°) "
f"measured={[round(a, 2) for a in measured]} "
f"expected={HOME}")
return False
log_phase(txt, csv_writer, 0, "OK",
f"HOME 近傍確認 max_err={max_home_err:.2f}°")
return True
def phase_1_detect_box(top_cap, templates_by_group, txt, csv_writer):
"""上カメラで白箱検出 → robot 座標 (X_box, Z_box) [m] に変換。"""
log_phase(txt, csv_writer, 1, "START", "上カメラで白箱検出")
# 上カメラのマット bbox を初回検出（visual_servo_pick._mat_px_range が既にあれば再利用）
if vsp._mat_px_range is None:
log_phase(txt, csv_writer, 1, "ERROR",
"マット bbox 未初期化（setup_homography_and_mat_range が失敗？）")
return None
result = detect_white_box_top_camera(top_cap, templates_by_group)
if result is None:
log_phase(txt, csv_writer, 1, "ABORT",
"白箱検出失敗（テンプレマッチング結果なし）")
return None
if result.get("below_threshold"):
log_phase(txt, csv_writer, 1, "ABORT",
f"白箱スコア閾値未達 score={result['score']:.3f} "
f"< {TEMPLATE_SCORE_THRESHOLD}")
return None
cx, cy = result["center"]
log_phase(txt, csv_writer, 1, "DETECT",
f"白箱 center=({cx},{cy}) score={result['score']:.3f} "
f"angle={result['angle']}° scale={result['scale']}")
X_box, Z_box = pixel_to_robot_xz(cx, cy)
log_phase(txt, csv_writer, 1, "OK",
f"robot 座標 X={X_box*1000:.1f}mm Z={Z_box*1000:.1f}mm")
detection_info = {
"center": (cx, cy),
"score": result["score"],
"angle": result.get("angle"),
"scale": result.get("scale"),
"frame": result.get("frame"),
}
return X_box, Z_box, detection_info
def phase_2_approach(X_box, Z_box, side_cap, top_cap, txt, csv_writer):
"""白箱上空（Y=Y_APPROACH_MM）へ移動。lookup_fk + send_angles。
target_ee_y は「マット表面 ≒ PICK_EE_Y」を基準に Y_APPROACH_MM 上方へ。
PICK_EE_Y(-0.040) + 0.075 = 0.035 [m]。
"""
target_ee_y = PICK_EE_Y + Y_APPROACH_MM * 0.001
log_phase(txt, csv_writer, 2, "START",
f"APPROACH target=(X={X_box*1000:.1f}mm, "
f"Y={Y_APPROACH_MM:.1f}mm above PICK_EE_Y, "
f"Z={Z_box*1000:.1f}mm) -> target_ee_y={target_ee_y:.4f}m")
angles = lookup_fk(X_box, target_ee_y, Z_box)
if angles is None:
log_phase(txt, csv_writer, 2, "ABORT", "lookup_fk 失敗")
return False
# j5 を HOME と同じ値に固定（爪の回転を防ぐ）
# FK テーブルの最近傍 j5 を使うとアームが回転しながら動くため
angles = list(angles)
angles[5] = HOME[5]
log_phase(txt, csv_writer, 2, "FIX_J5",
f"j5 を HOME 値 {HOME[5]} に固定 angles={[round(a, 2) for a in angles]}")
if not safety_check_angles(angles):
log_phase(txt, csv_writer, 2, "ABORT",
f"safety_check 失敗 angles={[round(a, 2) for a in angles]}")
return False
log_phase(txt, csv_writer, 2, "SEND",
f"send_angles {[round(a, 2) for a in angles]} speed={SPEED}")
try:
daemon_send_angles(angles, speed=SPEED, wait=0)
except Exception as e:
log_phase(txt, csv_writer, 2, "ERROR", f"daemon_send_angles 例外: {e}")
return False
# 動作完了を Mac 側 time.sleep で待つ (DAY208 流儀: daemon の wait は機能しない)。
# プレビュー更新を回しながら APPROACH_WAIT_SEC 経過するまで待機。
t_start = time.time()
while time.time() - t_start < APPROACH_WAIT_SEC:
update_preview(side_cap, top_cap, "Phase 2 APPROACH")
time.sleep(0.1)
measured = daemon_get_angles()
if measured is None or len(measured) != 6:
if ALLOW_REACH_CHECK_BYPASS_ON_READ_FAIL:
log_phase(txt, csv_writer, 2, "WARN",
"到達後の角度取得失敗。目視確認へ進む")
return True
else:
log_phase(txt, csv_writer, 2, "ABORT", "到達後の角度取得失敗")
return False
max_err = max(abs(measured[i] - angles[i]) for i in range(6))
log_phase(txt, csv_writer, 2, "REACH",
f"measured={[round(a, 2) for a in measured]} "
f"max_err={max_err:.2f}°")
if max_err > MAX_APPROACH_ERROR_DEG:
log_phase(txt, csv_writer, 2, "ABORT",
f"APPROACH 角度誤差超過 max_err={max_err:.2f}° "
f"(tolerance={MAX_APPROACH_ERROR_DEG}°)")
return False
return True
# ============================================================
# main
# ============================================================
def main():
global _current_detection_overlay
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
print("=" * 60)
print(f"place_test.py (DAY209) Phase 0〜2 only ts={timestamp}")
print("=" * 60)
txt, csv_handle, csv_writer = setup_logging(timestamp)
side_cap, top_cap = open_cameras()
if side_cap is None or top_cap is None:
log_phase(txt, csv_writer, -1, "ABORT", "カメラ open 失敗")
if txt:
txt.close()
if csv_handle:
csv_handle.close()
return
log_phase(txt, csv_writer, -1, "INFO",
f"side_idx={SIDE_CAMERA_INDEX} top_idx={TOP_CAMERA_INDEX}")
# FK テーブル
try:
load_fk_table()
except Exception as e:
log_phase(txt, csv_writer, -1, "ABORT", f"FK テーブル読込失敗: {e}")
side_cap.release()
top_cap.release()
txt.close()
csv_handle.close()
return
# Homography（上カメラ）
if not load_homography_via_top_cap(top_cap):
log_phase(txt, csv_writer, -1, "ABORT", "homography セットアップ失敗")
side_cap.release()
top_cap.release()
txt.close()
csv_handle.close()
return
# 白箱テンプレ
try:
templates_by_group = _load_white_box_templates()
except SystemExit:
log_phase(txt, csv_writer, -1, "ABORT", "テンプレ読込失敗（SystemExit）")
side_cap.release()
top_cap.release()
txt.close()
csv_handle.close()
return
# daemon 接続
try:
get_daemon_socket()
# DAY208 流儀: fresh_mode=1 を 1 回だけ立てる。以降は send_angles(wait=0) のみ。
daemon_set_fresh_mode(1)
log_phase(txt, csv_writer, -1, "INFO", "fresh_mode=1 (DAY208 流儀)")
except Exception as e:
log_phase(txt, csv_writer, -1, "ABORT", f"daemon 接続失敗: {e}")
side_cap.release()
top_cap.release()
txt.close()
csv_handle.close()
return
try:
# Phase 0
update_preview(side_cap, top_cap, "Phase 0 START")
if not phase_0_init(side_cap, txt, csv_writer):
log_phase(txt, csv_writer, 0, "ABORT", "HOME 姿勢確認失敗")
return
update_preview(side_cap, top_cap, "Phase 0 OK")
capture_both_cameras(side_cap, top_cap, timestamp, "phase0_home", txt, csv_writer)
# Phase 1
update_preview(side_cap, top_cap, "Phase 1 START")
# Phase 1 開始前、上カメラ映像を 2 秒間表示してカメラ確認
log_phase(txt, csv_writer, 1, "PREVIEW", "上カメラ確認中（2秒）")
print("[PHASE 1 PREVIEW] 上カメラ表示中... 2秒後に検出開始")
t_start = time.time()
while time.time() - t_start < 2.0:
update_preview(side_cap, top_cap, "Phase 1 PREVIEW (before detect)")
time.sleep(0.1)
update_preview(side_cap, top_cap, "Phase 1 DETECT (template match, blocking ~20s)")
result = phase_1_detect_box(top_cap, templates_by_group, txt, csv_writer)
if result is None:
log_phase(txt, csv_writer, 1, "ABORT", "白箱検出失敗")
return
X_box, Z_box, det = result
_current_detection_overlay = det # 以降のプレビューで描画される
print("[PHASE 1 PREVIEW] 検出オーバーレイを以降のプレビューで持続表示")
update_preview(side_cap, top_cap, "Phase 1 OK")
# Phase 2
update_preview(side_cap, top_cap, "Phase 2 START")
phase2_result = phase_2_approach(X_box, Z_box, side_cap, top_cap, txt, csv_writer)
update_preview(side_cap, top_cap, "Phase 2 DONE")
capture_both_cameras(side_cap, top_cap, timestamp, "phase2_approach", txt, csv_writer)
if not phase2_result:
log_phase(txt, csv_writer, 2, "ABORT", "APPROACH 失敗")
return
log_phase(txt, csv_writer, 2, "DONE",
"Phase 2 完了。手動確認後 Ctrl+C で終了")
print("\n[STOPPED] Phase 2 完了。爪が白箱上空 Y=75mm にあるはず。"
"目視確認してから Ctrl+C で終了。")
try:
while True:
update_preview(side_cap, top_cap, "STOPPED - press Ctrl+C to exit")
time.sleep(0.1)
except KeyboardInterrupt:
print("\n[EXIT] 手動終了")
except KeyboardInterrupt:
print("\n[INTERRUPT] Ctrl+C 検出")
log_phase(txt, csv_writer, -1, "INTERRUPT", "Ctrl+C")
finally:
# ★ DAY210 追加: 終了時 HOME 復帰 ★
# Ctrl+C / Phase ABORT / 正常終了のいずれでも、次回起動時の HOME 姿勢前提を守るため
# 必ず HOME へ戻す。socket がまだ生きている場合のみ実行。
try:
# fresh_mode は main で立てたまま (DAY208 流儀)
print("[CLEANUP] HOME 復帰中...")
daemon_send_angles(HOME, speed=SPEED, wait=0)
t_start = time.time()
while time.time() - t_start < 5:
update_preview(side_cap, top_cap, "CLEANUP HOME")
time.sleep(0.1)
measured = daemon_get_angles()
if measured is not None and len(measured) == 6:
err = max(abs(measured[i] - HOME[i]) for i in range(6))
print(f"[CLEANUP] HOME 復帰完了 max_err={err:.2f}°")
log_phase(txt, csv_writer, -1, "CLEANUP",
f"HOME 復帰完了 max_err={err:.2f}° "
f"measured={[round(a, 2) for a in measured]}")
else:
print("[CLEANUP] HOME 復帰後の angles 取得失敗")
log_phase(txt, csv_writer, -1, "CLEANUP",
"HOME 復帰後の angles 取得失敗")
except Exception as e:
print(f"[CLEANUP] HOME 復帰例外: {e}")
try:
log_phase(txt, csv_writer, -1, "CLEANUP",
f"HOME 復帰例外: {e}")
except Exception:
pass
try:
capture_both_cameras(side_cap, top_cap, timestamp, "cleanup_home", txt, csv_writer)
except Exception as e:
print(f"[CLEANUP] 撮影例外: {e}")
# ★ ここまで ★
# fresh_mode を 0 に戻す (main で立てた DAY208 流儀の終了処理)
try:
daemon_set_fresh_mode(0)
except Exception:
pass
try:
close_daemon_socket()
except Exception:
pass
side_cap.release()
top_cap.release()
cv2.destroyAllWindows()
if txt:
txt.close()
if csv_handle:
csv_handle.close()
_current_detection_overlay = None
print("[DONE]")
if __name__ == "__main__":
main()
