mechArm 270 Pi 動的追従の限界を探る Log#55 | Pick & Place 実行の常駐化と中断時の自動退避。モックで固めて、実機で分かったこと
ロボットアームに物を掴んで置かせる——その一連の動作を、これまでは一回ごとにプログラムを起動し直して走らせていた。起動のたびに数秒待たされるので、一度立ち上げたら続けて何度でも試せる「点けっぱなし」の形に変えることにした。あわせて、動作の途中で緊急停止（Ctrl-C）をかけたとき、アームが自分で定位置（HOME）へ戻る仕組みを入れた。掴んだまま止まると物を落とすからだ。設計から実装、外部レビュー4周、コミットまで進め、最後に実機で回した。その一部始終と、実機で最初に見えた一点を残す。
立ち上がりのどこが重いのかを測る
常駐化の動機は単純で、毎回の試行が起動から始まると、そのぶん待たされることだった。ただし「立ち上げが重い」は印象であって、まず何が重いのかを測ることにした。推論より先に測る。
インポートにかかる時間を、実機にもカメラにも触れない dry-run で切り出す。なぜこれを見せるかというと、常駐化で「消える一回コスト」と「毎試行残るコスト」を分けるための一次データだからだ。
import time: 16237 | 100800 | numpy
import time: 664 | 104500 | fk_lookup
import time: 447427 | 449591 | cv2
import time: 661 | 13266 | visual_servo_pick
import time: 324 | 210411 | cv2_enumerate_cameras.macos_backend
import time: 430 | 211192 | cv2_enumerate_cameras
import time: 222 | 211413 | camera_resolvercumulative（2列目・マイクロ秒）で見ると、cv2 が約0.45秒、camera_resolver が約0.21秒（実体は macOS のカメラデバイス列挙で、import した瞬間に走る）、numpy が約0.10秒。独立な大物を足すとインポート合計は0.8秒前後。
ただしこの数字に FK テーブルの実ロードは入っていない。テーブルは初回の lookup 呼び出し時に読まれるからだ。そこを別に測った。なぜこれを見せるかというと、事前の見立て（170万点のロードは一桁重いはず）が当たっているかを確かめるためだ。
[FK] import : 0.109 s
[FK] first load : 0.793 s <- 常駐化で消える一回コスト（本命）
[FK] cached load : 0.00000 s <- 2試行目以降の実コスト見立ては外れた。170万点の np.loadtxt は0.79秒で、数秒ではなかった。実測が正しい。そして2回目のロードはキャッシュが完全に効いて実質ゼロ。
一回コストを積むと、インポート約0.8秒＋FK first 0.79秒で約1.6秒。しかもその全部が「プロセスを生かし続ければ消える」種類のものだった。インポートキャッシュも FK のキャッシュも、モジュールの寿命とともに生きる。プロセスが1試行で死ぬから毎回捨てられていただけだ。
run() を割らずに包む
改修の入り方には二通りあった。試行本体の関数 run() を初期化部と試行部に外科的に割るか、run() はそのままに外側からループで包むか。現物のコードを読むと、run() は既に同一プロセスで複数回呼ばれることを想定して書かれていた。冒頭に観測状態をリセットする処理があり、試行識別子は毎回採番し、daemon 接続は毎回張って finally で閉じる。
つまり1試行に必要な状態リセットは、プロセスの再起動ではなく run() の内部で既に完結していた。立ち上げが重いのは run() の構造のせいではなく、プロセスが死んでモジュールキャッシュが捨てられるからだ。それならプロセスを生かし続けるだけでよい。run() を割っても追加で消せるコストはなく、割ればリセットの網羅を人手で保証し直すリスクだけが増える。
持ち越す状態とリセットする状態を、現物のコードに紐付けて整理した。
状態 | 扱い | 根拠 |
|---|---|---|
FKテーブルキャッシュ | 持ち越し | モジュールグローバル・読み取り専用。プロセス寿命で持続 |
白箱テンプレ | 持ち越し | 初回ロードのキャッシュ。プロセス寿命で持続 |
補正記憶（corr/height） | 持ち越し | ファイル背面。試行またぎで学習が続くのが正 |
観測状態 | リセット | run() 冒頭で全キーをリセット |
試行識別子・ログ | リセット | 毎回採番・新規辞書 |
daemon ソケット | 毎試行フレッシュ | run() が open→finally で close |
アーム姿勢 | 毎試行 HOME 開始 | run() の stage1 |
方式は「包む」で確定した。外側にキー入力ループ（n＝次試行／h＝HOME 復帰／q＝終了）を足し、run() 本体には触れない。得られる高速化はプロセス寿命だけで手に入り、各試行の物理挙動は従来と変わらない。
中断で落とさない ―― auto-HOME の安全フレーム
常駐で連続試行を回すなら、途中で止めたい場面が必ず出る。止め方をどうするかで一つ判断があった。試行の途中で Ctrl-C が入ったとき、アームをその場で止めるか、HOME へ戻してから止めるか。
HOME へ戻す方を選んだ。ただし無条件ではない。掴んだまま止まればブロックを落とすし、掴んでいない位置からの HOME は安全でも、掴んでいる最中の HOME は経路次第で危ない。そこで中断パスには制約を置いた。
グリッパーには触らない。掴んでいたら抱えたまま HOME まで運ぶ。開けば落とす。
送角は既存の単発送角プリミティブを再利用する。座標送りは使わない。HOME は固定定数なので姿勢の安全は保証される。
daemon が応答しなければ送角せず、停止して報告する。ハングしない。
HOME 送出中の2度目の Ctrl-C を握って、送出を完走させる。
中断でも記録（ログと台帳）を必ず残す。
従来の「自動 HOME 復帰の禁止」原則は残し、中断時の一度きりの HOME だけを意図的な例外とする。失敗リトライの HOME ではない。
この最後の一点は、過去に「失敗しても自動で HOME に戻さず、目視で判断して手で戻す」と決めた原則に、意図的な例外を一つ開けることを意味する。だからコードのコメントにも、次の引き継ぎにも、その例外の性格（Ctrl-C 中断時のみ・失敗リトライではない）を明記して、後から原則違反と誤解されない形にした。
レビューが削り出したもの
物理挙動に触れる変更なので、実装のたびに外部レビューへ出した。同じ書き手で書いて確認すると同じ盲点を持つ。レビューは4周し、指摘は周を追うごとに性質が変わっていった。
一周目は設計の穴だった。daemon 接続中に Ctrl-C が入ると安全処理を迂回するのに、表示だけ「退避済み」と出る経路。そしてもう一つ、常駐化で初めて顕在化した状態漏れ。前試行の箱位置が観測状態に残り、次試行が途中で終わると誤った箱ズレ量を台帳へ書き得る、という指摘だった。これは「新しいモジュールグローバルは足していない」と報告した側の見落としで、キーを追加したときにリセット列へ転記し忘れる、という文書側でも何度か踏んでいる型と同じだった。外の目が拾った。
二周目は方式の欠陥だった。中断パスのタイムアウトを signal（SIGALRM）で作っていたのを、レビューが「recv を外から中断するので、既存のソケット後始末を通らない。割込みとも競合する」と指摘した。方式ごと転換することにした。daemon クライアント側のファイルは触らない制約があったので、クライアントのタイムアウト値とリトライ回数が呼び出し時にモジュール属性から読まれることを利用して、中断パスの間だけ一時的に短い値へ差し替え、finally で戻すコンテキストマネージャにした。タイムアウト時に既存の後始末（ソケットを閉じてバッファを捨てる）を正しく通る。この関数は後掲の全コードに載せる。
三周目と四周目は、割込みが特定の一行に刺さった場合のエッジだった。属性を差し替えた直後・復元している最中・記録を代入している最中に2度目の Ctrl-C が来ると、値が戻らなかったり記録が既定値に化けたりする、という指摘が並んだ。対処は代入の再捕捉で、記録の確定はこうなった。なぜこれを見せるかというと、中断処理のどこで2度目の割込みが来ても記録を残す、という保証の実体だからだ。
# setdefault が DAY265 既定値を書くのを防ぐ.
try:
log_record["home_recovery"] = _hr
except KeyboardInterrupt:
log_record["home_recovery"] = _hr
raise四周目の最後の一点は、割込みが「3度目とは限らず2度目になり得る」箇所だった。1度目で中断し、退避が追加の割込みなしで正常終了し、最後の後始末の RPC（5秒待つ）の最中に2度目が来る、という自然な経路がある。ここで抜けると記録も台帳も残らず、daemon 側の状態が戻らないまま残る。ここも握った。
そして、これ以上は硬化させない境界も同時に引いた。純粋な Python では、任意のバイトコード位置に入る割込みに対する完全な原子性は作れない。だから合格の基準を「指定した9つの注入点でモックが全部通ること」に置き、その線をコードのコメントにも残した。無限に硬く締め続けないための境界だ。
9点で固める
割込みを9つの位置に注入するモックで、各点について三つを確認した。タイムアウトとリトライが元の値に復元されるか、退避の記録が既定値でなく退避を試みた値になるか、後始末（クローズ・台帳・JSON書き出し）が各1回完走するか。
① before_pre : 35.0/12 | DAY283_interrupt_auto_home | 1/1/1
② during_pre : 35.0/12 | DAY283_interrupt_auto_home | 1/1/1
③ during_move : 35.0/12 | DAY283_interrupt_auto_home | 1/1/1
④ after_move : 35.0/12 | DAY283_interrupt_auto_home | 1/1/1
⑤ after_CM_attr1 : 35.0/12 | DAY283_interrupt_auto_home | 1/1/1
⑥ after_CM_attr2 : 35.0/12 | DAY283_interrupt_auto_home | 1/1/1
⑦ before_CM_restore1 : 35.0/12 | DAY283_interrupt_auto_home | 1/1/1
⑧ before_HR_assign : 35.0/12 | DAY283_interrupt_auto_home | 1/1/1
⑨ finally fresh_mode : 35.0/12 | DAY283_interrupt_auto_home | 1/1/19点すべてで復元・記録・後始末が揃った。この版を、変更前後を確認し、他ファイルの混入がないことを確認したうえで commit した。
[main cbfb7b3] DAY283: run_pick_place 常駐化(wrap・キー入力ループ)＋Ctrl-C auto-HOME＋STAGE1条件付きスキップ
1 file changed, 376 insertions(+), 39 deletions(-)実機で分かったこと
ここまでは机上とモックの話だ。最後に実機で回した。掴んだ状態で Ctrl-C を入れ、アームがブロックを落とさず HOME まで運ぶか。これが今回の物理的な検証目標だった。なぜこの出力を見せるかというと、モックで固めた安全弁が実機で最初に見せた挙動そのものだからだ。
[INTERRUPT] Ctrl-C 検出。auto-HOME 試行 → ログ保存 → セッション終了
[INTERRUPT] auto-HOME 例外 (握って停止・報告): TimeoutError: timed out
[LOG ] /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260713_195532/run_0001_20260713_195532.json
[LOOP] 試行中 Ctrl-C 受領. auto-HOME/台帳の実施状況は log の 'home_recovery' セクションで確認のこと (daemon 接続前 or SSD 未マウント等の早期中断では未実施).
[LOOP] メニューに戻らずセッション終了 (§4 セマンティクス).
[LOOP] 常駐モード終了. 実行試行数 = 1記録された退避の中身はこうだった。
"home_recovery": {
"attempted": true,
"note": "DAY283_interrupt_auto_home",
"success": false,
"abort_reason": "auto_home_exception:TimeoutError",
"pre_home_pose": null
},
"ended_at": "2026-07-13T19:56:02.316900",
"result": "aborted"pre_home_pose が null であることが原因を絞る。退避パスには入った（attempted が true）が、HOME 送角の前段にある生存確認の姿勢読み取りで姿勢が取れていない。取れていれば pre_home_pose に6軸が入る。その読み取りが5秒の予算でタイムアウトし、例外を握って停止・報告に落ちた。HOME 送角には到達していない。
つまり、退避の安全フレーム（割込みを握り、アームを暴走させず、記録を残してセッションを終える）は設計通りに動いた。だが「掴んだまま HOME まで運ぶ」という本体は、この一回では実行に至っていない。安全側に落ちることは実証できたが、運ぶ方はまだ実証できていない。
考えられる筋の本命は、生存確認の5秒予算が実 daemon の応答に対して短すぎたことだ。Ctrl-C の瞬間に daemon が送角処理の最中で、その RPC が返るまで次の姿勢読み取りが返らず、5秒を超えた。応答しない偽 daemon で作ったモックでは、この「生きている daemon が処理中で詰まっている」状態は原理的に出ない。実機で初めて出た。
予算を緩めれば「ハングしない」という別の要件と綱引きになる。だから値は勘で動かさず、生きている daemon が Ctrl-C 相当のタイミングで姿勢読み取りをどれくらいで返すかを実測してから決める。土台（安全側に落ちる中断パス）はできた。次は、その土台の上で実際に HOME まで運ぶ予算を、実測で決める番になる。
変更後の全コード
ここからは、この回で変更・追加した関数を全量で載せる。掲載は commit cbfb7b3（md5 9e973d4ef0fb41a9c62764495f902956）の実ファイルから直接抜き出したもので、抜粋も省略もしていない。後から本人や別の人間・AI が見返して使える記録として、変更後の姿をそのまま残す。
最初に、起動時の HOME 復帰とグリッパー開を担う stage1_home_and_open。実測ゲート付きの STAGE1 スキップ（既に HOME にいれば HOME 送角を飛ばす・グリッパー開は常に実行）がここに入る。
def stage1_home_and_open(log_record, dry_run):
"""段1: HOME 復帰 + グリッパー開.
DAY265: 失敗完走化. どちらの sub-step が失敗しても記録だけ残して True を返し、
通しを止めない. arrived 情報は _do_move が log_record.moves に記録済み.
DAY283 FIX-6 (常駐化スキップ・実測ゲート):
HOME 送角の直前に実測角を 1 回読み, 全軸 |Δ| ≤ TOL_HOME なら「既に HOME」と
判定して送角のみスキップ. 判定は「試行回数」ではなく実測に依る (前試行が
途中失敗して空中/把持中なら実測が弾き, 従来の HOME 送角に落ちる. 推論より測定).
- グリッパー開は常に実行 (スキップ対象外・DAY175 read 不信).
- dry_run は従来通り (実測せず planned のみ).
- 判定閾値は既存 TOL_HOME 流用 (新定数を作らない).
"""
import time as _time
print("[STAGE 1] HOME 復帰 + グリッパー開")
log_record["stages"].append("stage1")
# DAY283 FIX-6: 実測ゲート (実機のみ). HOME 送角スキップ判定.
skipped = False
skip_meta = {"home_move_skipped": False, "measured_max_delta_deg": None,
"measured_angles": None, "reason": None}
if not dry_run:
_cur = _daemon_get_angles_filtered() # 通常経路の filtered 版を使用 (信頼性優先)
_delta = _max_delta(HOME, _cur)
if _delta is not None and _delta <= TOL_HOME:
skip_meta = {"home_move_skipped": True,
"measured_max_delta_deg": round(float(_delta), 3),
"measured_angles": list(_cur),
"reason": "already_at_home"}
print("[STAGE 1] 実測が HOME 一致 (max|Δ|={:.2f}° ≤ TOL_HOME={:.2f}°) → "
"HOME 送角スキップ".format(_delta, TOL_HOME))
skipped = True
# DAY283 FIX-R2-5: スキップ時の観測行補完. stage2 の非移動 emit と同形式.
# 「1 STAGE = 1 行」の記録仕様とダッシュボード STAGE HISTORY の stage1 行
# を維持する. _STAGE_RECORD_SEEN が重複を防ぐため 1 回発火で良い.
_OBSERVE_STATE["last_angles"] = list(_cur) # 実測角を残す (None 回避)
_observe_emit_now()
_observe_stage_record(kind="stage")
else:
skip_meta = {"home_move_skipped": False,
"measured_max_delta_deg":
round(float(_delta), 3) if _delta is not None else None,
"measured_angles": list(_cur) if _cur is not None else None,
"reason": ("delta_over_tol" if _delta is not None
else "measured_read_failed")}
log_record["stage1_skip"] = skip_meta
# HOME 送角 (skip 判定でスキップ・実測時間も記録)
_t_home_start = _time.monotonic()
if not skipped:
ok, _ = _do_move(HOME, "stage1_to_home", TOL_HOME, log_record, dry_run=dry_run)
if not ok:
log_record.setdefault("stage_failures", []).append(
{"stage": 1, "label": "stage1_to_home", "kind": "not_reached"})
print("[STAGE 1] HOME 未到達. 記録のみ残して継続")
_t_home_elapsed = _time.monotonic() - _t_home_start
# グリッパー開 (常に実行)
_t_grip_start = _time.monotonic()
if not _do_gripper("open", "stage1_grip_open", log_record, dry_run):
log_record.setdefault("stage_failures", []).append(
{"stage": 1, "label": "stage1_grip_open", "kind": "gripper_open_fail"})
print("[STAGE 1] グリッパー開失敗. 記録のみ残して継続")
_t_grip_elapsed = _time.monotonic() - _t_grip_start
# 内訳時間 (スキップ導入後の残時間の説明用)
log_record["stage1_timing"] = {
"home_move_sec": round(_t_home_elapsed, 3),
"gripper_open_sec": round(_t_grip_elapsed, 3),
"home_move_skipped": skipped,
}
print("[STAGE 1] 内訳時間: home_move={:.2f}s (skipped={}) / gripper_open={:.2f}s".format(
_t_home_elapsed, skipped, _t_grip_elapsed))
return True次に、試行本体の run()。1試行の起動から通し動作、中断時の auto-HOME、記録までを担う。この回で変わったのは冒頭の観測状態リセット（box_xz_last の追加）と setup を中断処理の内側へ移した点、そして末尾の auto-HOME の except ブロックと finally の後始末ゲートだ。stage2〜10 のパイプラインは前の回から変えていないが、変更箇所だけを切り出すと文脈が読めないため、関数全量をそのまま載せる。
def run(dry_run=False, dummy_block_xz=None, dummy_box_xz=None,
dummy_pick_verdict=None, dummy_place_verdict=None,
human_judge=False):
# KIOXIA 未マウント時に Mac 内蔵ディスクへ誤書き込みするのを防ぐ.
# dry_run は撮影/保存しないので素通し. 実機 run のみガード.
if not dry_run and not _ensure_ssd_mounted():
print("[ABORT ] KIOXIA SSD 未マウント。run データの出力先が無いため中断します。"
"（{} を確認）".format(LEDGER_DIR))
return False, None
log_path = _next_log_path()
run_stem = log_path.stem
# 画像も JSON と同じ run フォルダ直下に置く（_frames サブ階層は廃止）
frames_dir = log_path.parent
# 画面のファイル情報表示用に run フォルダパスを観測状態へ
_OBSERVE_STATE["run_dir"] = str(log_path.parent)
# 観測 emit 用の状態を初期化（このプロセスで複数 run() を回す場合の前回残置を消す）
_OBSERVE_STATE["phase"] = None
_OBSERVE_STATE["phase_t0_ms"] = None
_OBSERVE_STATE["gripper"] = None
_OBSERVE_STATE["verdict"] = {"pick": None, "place": None, "side_place": None}
_OBSERVE_STATE["block_xz"] = None
_OBSERVE_STATE["box_xz"] = None
_OBSERVE_STATE["block_place_xz"] = None
_OBSERVE_STATE["tip_pick"] = None
_OBSERVE_STATE["cap_tip"] = None
_OBSERVE_STATE["cap_pick"] = None
_OBSERVE_STATE["cap_place"] = None
# DAY283 FIX-3: 常駐化で 2 試行目が stage9b 前に終了すると, 前試行の box_xz_last と
# 今回の box_xz を組み合わせ誤った box_shift_* を台帳へ書き得たため, 試行冒頭で
# 明示リセット. リセット列漏れは box_xz_last 1 件のみ (他 全キー突合済み・報告参照).
_OBSERVE_STATE["box_xz_last"] = None
_OBSERVE_STATE["last_angles"] = None
_OBSERVE_STATE["lookup"] = None
_OBSERVE_STATE["corr"] = None
_OBSERVE_STATE["captures"] = None
_OBSERVE_STATE["run_stem"] = run_stem
log_record = {
"started_at": datetime.now().isoformat(),
"dry_run": bool(dry_run),
"frames_dir": str(frames_dir),
"constants": {
"HOME": list(HOME), "SPEED": SPEED,
"BOX_XZ_FALLBACK": list(BOX_XZ),
"X_RESID_THRESH": X_RESID_THRESH,
"BOX_SCORE_THRESH": BOX_SCORE_THRESH,
"GRIPPER_SLEEP": GRIPPER_SLEEP,
"TOL_APPROACH": TOL_APPROACH,
"TOL_PICK_PLACE": TOL_PICK_PLACE,
"TOL_HOME": TOL_HOME,
"SEND_WAIT_SEC": SEND_WAIT_SEC,
"DETECT_REPEAT": DETECT_REPEAT,
"SWEPT_J0_FOR_PLACE_JUDGE": SWEPT_J0_FOR_PLACE_JUDGE,
"MAX_PICK_ATTEMPTS": MAX_PICK_ATTEMPTS,
"SWEPT_J0_FOR_PICK_RETREAT": SWEPT_J0_FOR_PICK_RETREAT,
},
"stages": [],
"detect": [],
"lookups": [],
"moves": [],
"gripper": [],
"vlm_captures": [],
"judgments": {},
"pick_attempts": [],
}
print("=" * 70)
print("[RUN] run_pick_place.py dry_run={} log={}".format(
dry_run, log_path.name))
print("=" * 70)
success = False
user_interrupted = False
# DAY283 FIX-R2-4: daemon 接続状態を追跡. finally の fresh_mode(0) / 台帳追記を
# ゲートする. 未接続時に finally が再接続を試み 35s 待つ問題 (再レビュー必須5) と,
# 接続失敗行を台帳に載せる回帰 (エイイチ確定: 従来互換=接続失敗は台帳なし) を防ぐ.
daemon_connected = False
# DAY283 FIX-2: try 開始を daemon 接続の前に移動. daemon 接続中の Ctrl-C を
# except KeyboardInterrupt に拾わせ, auto-HOME → finally (台帳/JSON 追記) を通す.
# daemon 未接続時の生存確認は _interrupt_daemon_budget 下の daemon_get_angles が
# 例外/None を返し自然に "daemon_unresponsive_home_skipped" に落ちる (§4-3).
try:
# 実機モードのみ daemon 接続 + fresh_mode=1
if not dry_run:
try:
get_daemon_socket()
daemon_connected = True
except Exception as e:
# FIX-R2-3: 明示 _atomic_write_json 削除 (finally の write と二重書き
# =1回目は未完成 JSON になる). finally の1回に統一.
# FIX-R2-4: daemon_connected=False のまま return → finally が
# fresh_mode(0) 送出せず, 台帳追記もスキップ (従来互換).
_abort(log_record, 0, "daemon ping 失敗: {}".format(e))
return False, log_path
try:
daemon_set_fresh_mode(1)
print("[INIT ] fresh_mode=1")
except Exception as e:
print("[WARN ] fresh_mode=1 失敗: {}".format(e))
# 段1
_observe_phase("stage1")
if not stage1_home_and_open(log_record, dry_run):
return False, log_path
# stage1 で gripper open まで完了
_observe_gripper(0)
# 段2
_observe_phase("stage2")
block_xz, box_xz = stage2_detect_block_and_box(
log_record, dry_run, dummy_block_xz, dummy_box_xz)
if block_xz is None or box_xz is None:
return False, log_path
# 観測状態を更新（メートル/Unity 系のまま、検出値を素通し）
_OBSERVE_STATE["block_xz"] = [block_xz[0], block_xz[1]]
_OBSERVE_STATE["box_xz"] = [box_xz[0], box_xz[1]]
# 非移動 stage の通知 emit（移動なし→画面に stage2 行を出す）
_observe_emit_now()
_observe_stage_record(kind="stage")
# 段3〜判定①: 「pick 判定 verdict=no なら掴み直し」ループ（最大 MAX_PICK_ATTEMPTS 回）
# 初回ラベル接尾辞は ""（既存ログ解析と互換）、2 回目以降は "_a{attempt}"
cur_block_xz = block_xz
pick_ok = False
final_pick_judgment = None
for attempt in range(1, MAX_PICK_ATTEMPTS + 1):
suf = "" if attempt == 1 else "_a{}".format(attempt)
# 試行間補正の適用 (lookup 呼び出し直前)。
# USE_CORRECTION / KIOXIA / 記憶 が揃っているときだけ block_xz を
# 補正する。素通り時は cur_block_xz と完全一致 (回帰確認可)。
target_xz_pick, corr_meta = _apply_correction("pick", cur_block_xz)
log_record.setdefault("correction", {})["last"] = corr_meta
# 観測 corr の書き込みは stage3 phase 遷移の直後に 1 回だけ行う (下方).
# 「補正記憶を参照した stage」だけを corr に載せる方針. stage4/5/6 は
# phase 遷移で corr がクリアされたまま (実際には参照していないため).
# 段3 APPROACH_PICK
_observe_phase("stage3_approach_pick" + suf)
# _apply_correction("pick") の結果をこの stage の corr に 1 回だけ記録する.
# updated/update_reason は apply 時点では未確定 → False / None. update 決定は
# PICK 側では観測へは書き戻さない (post-stage6 の判定は phase 変わっていないため
# emit にも載らず、次 stage で phase clear されるため).
_OBSERVE_STATE["corr"] = {
"phase": "pick",
"memory_file": str(CORR_MEM_PATH),
"cell_key": corr_meta.get("cell_key"),
"applied_dx_mm": corr_meta.get("dx_mm"),
"applied_dz_mm": corr_meta.get("dz_mm"),
"target_before": list(corr_meta.get("target_before_mm") or []),
"target_after": list(corr_meta.get("target_after_mm") or []),
"apply_reason": corr_meta.get("reason") or "",
"updated": False,
"update_reason": None,
}
ok, r_app_pick = stage_lookup_and_move(
"approach_pick", target_xz_pick, "stage3_approach_pick" + suf,
TOL_APPROACH, log_record, dry_run, 3)
if not ok:
return False, log_path
approach_pick_target = list(r_app_pick["target_angles"])
# 段4 PICK descend.
# DAY277: approach で確定した j0 を descend へ渡し j0 lock
# (真下降下). approach_pick_target[0] は stage3 の fk_lookup 結果.
_observe_phase("stage4_pick" + suf)
ok, r_pick = stage_lookup_and_move(
"pick", target_xz_pick, "stage4_pick" + suf,
TOL_PICK_PLACE, log_record, dry_run, 4,
pick_j0_ref=float(approach_pick_target[0]))
if not ok:
return False, log_path
# 台帳の pick_pose_id / pick_used_j0..j5 用に lookup の pose_id と
# 直近 _do_move の post_actual を log_record へ寄せる（新規 I/O ゼロ・
# 既存 moves[-1].post_actual を再利用）。attempt loop なので最後の試行が残る。
pose_used = log_record.setdefault("pose_used", {})
pose_used["pick_pose_id"] = (r_pick.get("pose_id")
if r_pick else None)
moves = log_record.get("moves") or []
if moves:
pa = moves[-1].get("post_actual")
if isinstance(pa, (list, tuple)) and len(pa) == 6:
pose_used["pick_used"] = list(pa)
# DAY277: stage4_5a_align (PICK 視覚サーボ) は本線から切り離し.
# 経緯: ran 112705 で block_x_mm=None のまま「収束」を偽装し j0 を
# -22→-34 に悪化させた事例あり. 検出 block_x が None のときの
# 収束判定に穴があり, 害になる. approach fk_lookup + descend j0
# lock (真下降下) で開ループの精度を上げる方針に移行.
# 関数定義 (_detect_tip_from_frame/_visual_servo_loop_pick/
# _run_pick_visual_servo) と定数はコミット履歴保存のため残置.
# 段4.5 爪先(青シール) vs block 差分測定（GRIP前・測定専用・動作非干渉）
# 関節は動かさず pick到達姿勢のまま上カメラ1枚を撮り、同フレームで
# 爪先中点と赤blockを検出。差分は台帳へ。失敗は None 記録で素通し。
tip_img = _capture_for_vlm(
"stage4_pick_tip" + suf, frames_dir, log_record,
dry_run, run_stem=run_stem)
_observe_capture_push(log_record)
_OBSERVE_STATE["cap_tip"] = (str(tip_img) if tip_img else None)
tip_diff = _measure_tip_vs_block(tip_img)
_OBSERVE_STATE["tip_pick"] = tip_diff
log_record.setdefault("tip_measurements", []).append(
{"attempt": attempt, "image": (str(tip_img) if tip_img else None),
"diff": tip_diff})
if tip_diff is not None:
print("[TIP ] attempt={} dist={:.1f}mm "
"(dx={:.1f} dz={:.1f})".format(
attempt, tip_diff["dist_mm"],
tip_diff["dx_mm"], tip_diff["dz_mm"]))
# 補正記憶の更新は、VLM 判定 + tip_dist override 後の
# final_verdict=="yes" 時のみに移動した (この下の判定ブロック後で実行)。
else:
print("[TIP ] attempt={} 測定スキップ（検出不足/前提破綻）"
.format(attempt))
# 段5 グリッパー閉
_observe_phase("stage5" + suf)
log_record["stages"].append("stage5" + suf)
grip5_ok = _do_gripper(
"close", "stage5_grip_close" + suf, log_record, dry_run)
if not grip5_ok:
# DAY265: グリッパー閉失敗しても継続 (掴めなかった事実として記録).
log_record.setdefault("stage_failures", []).append(
{"stage": 5, "label": "stage5_grip_close" + suf,
"kind": "gripper_close_fail", "attempt": attempt})
print("[STAGE 5] グリッパー閉失敗. 記録のみで次 stage へ")
_observe_gripper(1)
# 非移動 stage の通知 emit（移動なし→画面に stage5 行を出す）
_observe_emit_now()
_observe_stage_record(kind="stage")
# 段6 LIFT（その試行の approach_pick target_angles へ戻る）
_observe_phase("stage6" + suf)
log_record["stages"].append("stage6" + suf)
# C1 (DAY268 案R): stage4_pick (DESCEND) が未到達なら stage6_lift をガード.
# moves から stage4_pick の arrived を引く (stage_lookup_and_move は
# record-and-continue で ok=True を返すため ok では判定できない).
# reversed で直近の試行の move を拾う (regrasp attempt ループの正しい試行).
_pick_not_reached = False
for _m in reversed(log_record.get("moves") or []):
if str(_m.get("label", "")).startswith("stage4_pick"):
_pick_not_reached = (_m.get("arrived") is False)
break
ok, _ = _do_move(
approach_pick_target, "stage6_lift" + suf, TOL_APPROACH,
log_record, dry_run=dry_run,
abort_if_prev_not_reached=_pick_not_reached)
if not ok:
# DAY265: LIFT 到達失敗しても継続 (掴めているかは VLM が判定).
log_record.setdefault("stage_failures", []).append(
{"stage": 6, "label": "stage6_lift" + suf,
"kind": "not_reached", "attempt": attempt})
print("[STAGE 6] LIFT 未到達. 記録のみで判定へ")
# 判定①: stage4_pick_tip (リフト前) と stage6_lift 後の現フレームを撮り、
# 2 枚比較で「赤ブロックが浮いて持ち上がったか」を判定する (DAY257 後段)。
# その後 tip_dist 二重チェックで VLM yes でも dist > PICK_DIST_MAX_MM なら
# no に倒す。最終 verdict が yes のときだけ補正記憶を更新する。
vlm_label = "pick" if attempt == 1 else "pick_a{}".format(attempt)
pick_img = _capture_for_vlm(
vlm_label, frames_dir, log_record, dry_run, run_stem=run_stem)
_observe_capture_push(log_record)
_OBSERVE_STATE["cap_pick"] = (str(pick_img) if pick_img else None)
# tip_img は stage4 直後に撮影済み (L1351 付近・_OBSERVE_STATE["cap_tip"])。
# 両画像が揃っていれば 2 枚渡し、欠けていれば従来通り pick_img だけ。
pick_imgs_for_vlm = (
[tip_img, pick_img] if (tip_img is not None and pick_img is not None)
else pick_img)
pj = _judge_image(
pick_imgs_for_vlm, "pick", dry_run, dummy_pick_verdict)
vlm_verdict = pj["verdict"]
human_verdict = None
if human_judge and not dry_run:
human_verdict = _ask_human_verdict(pick_img, pj, kind="pick")
# DAY257 後段 + DAY273 案A: tip_dist 二重チェック (3 分岐).
# dist <= PICK_DIST_MAX_MM (30) → override せず VLM を信じる
# 30 < dist <= PICK_DIST_BREAKDOWN_MM (150) → override 発火 no (掴めていない)
# dist > PICK_DIST_BREAKDOWN_MM (150) → override せず VLM を信じる
# (測定破綻・DAY273 案A)
# tip 測定失敗 (tip_diff is None) のときは override 判定保留 (False).
# dist_override_suppressed は破綻値で override を無効にした事実を残す
# (台帳は 67 列ヘッダ不変・pick_attempts の JSON 側にのみ記録).
dist_override = False
dist_override_suppressed = False
final_verdict = vlm_verdict
if (vlm_verdict == "yes" and tip_diff is not None
and tip_diff.get("dist_mm") is not None):
_dist_mm = float(tip_diff["dist_mm"])
if _dist_mm > PICK_DIST_BREAKDOWN_MM:
# 破綻値: override 発火させず VLM を信じる
dist_override_suppressed = True
print("[OVERRIDE] tip_dist={:.1f}mm > {}mm 測定破綻とみなし "
"override 無効・VLM 判定 ({}) を採用".format(
_dist_mm, PICK_DIST_BREAKDOWN_MM, vlm_verdict))
elif _dist_mm > PICK_DIST_MAX_MM:
# 従来どおり掴み不良: no に倒す
final_verdict = "no"
dist_override = True
print("[OVERRIDE] vlm=yes だが tip_dist={:.1f}mm > {}mm "
"→ pick_verdict=no に倒す".format(
_dist_mm, PICK_DIST_MAX_MM))
adopted = final_verdict # 補正記憶の判定はこの final を見る
_observe_verdict("pick", adopted, pj["confidence"], pj["reason"])
log_record["pick_attempts"].append({
"attempt": attempt,
"verdict": adopted,
"vlm_verdict": vlm_verdict,
"human_verdict": human_verdict,
"dist_override": dist_override,
# DAY273 案A: 破綻値で override を無効化した事実.
# 台帳 67 列ヘッダは触らず JSON にのみ残す (後段で診断可能).
"dist_override_suppressed": dist_override_suppressed,
"confidence": pj["confidence"],
"reason": pj["reason"],
"block_xz_m": list(cur_block_xz),
"x_residual_m": r_pick["x_residual_m"],
"z_residual_m": r_pick["z_residual_m"],
})
print("[JUDGE ] pick attempt={} adopted={} (vlm={} human={} "
"dist_override={}) confidence={} reason={}".format(
attempt, adopted, vlm_verdict, human_verdict,
dist_override, pj["confidence"], pj["reason"]))
# judgments["pick"] は最終試行の dict を保存（verdict は adopted・VLM/人を併記）
final_pick_judgment = dict(pj)
final_pick_judgment["verdict"] = adopted
final_pick_judgment["vlm_verdict"] = vlm_verdict
final_pick_judgment["human_verdict"] = human_verdict
final_pick_judgment["dist_override"] = dist_override
# 補正記憶の更新: 最終 yes (掴み成功確定) かつ tip 測定成功時のみ。
# override で no に倒した試行 / VLM no / tip 測定失敗 のΔは
# 記憶に入れない (掴めていない試行で学習を汚さない)。
# DAY274 修正1 (codex ❌): dist_override_suppressed (tip_dist > 150mm の
# 測定破綻で override 無効化した試行) は adopted=yes でも tip_diff は破綻値.
# そのΔで記憶を更新すると PICK 補正が汚染される (前回 codex B1 未解消分).
# 破綻無効化した試行では PICK 補正記憶を更新しない.
# KIOXIA 未マウントや USE_CORRECTION=False 時は _corr_save がスキップ。
if (USE_CORRECTION and adopted == "yes"
and tip_diff is not None
and tip_diff.get("dx_mm") is not None
and tip_diff.get("dz_mm") is not None
and not dist_override_suppressed):
key = _corr_key("pick", cur_block_xz[0], cur_block_xz[1])
mem = _corr_load()
e_new = _corr_update(mem, key,
tip_diff["dx_mm"], tip_diff["dz_mm"])
if _corr_save(mem):
print("[CORR ] mem key={} dx={:+.2f} dz={:+.2f} n={} "
"(EMA α={}・最終 yes のみ更新)".format(
key, e_new["dx_mm"], e_new["dz_mm"],
e_new["n"], EMA_ALPHA))
if isinstance(_OBSERVE_STATE.get("corr"), dict):
_OBSERVE_STATE["corr"]["updated"] = True
_OBSERVE_STATE["corr"]["update_reason"] = None
elif (USE_CORRECTION and adopted == "yes"
and dist_override_suppressed):
# 破綻無効化で adopted=yes になった試行 (VLM yes だが tip_dist > 150mm).
# 記憶更新を skip した事実をログに残す (見えなくなる事態を防ぐ).
print("[CORR ] PICK update skip: reason=dist_override_suppressed "
"(tip_dist 測定破綻・記憶を汚染しないため学習しない)")
if isinstance(_OBSERVE_STATE.get("corr"), dict):
_OBSERVE_STATE["corr"]["updated"] = False
_OBSERVE_STATE["corr"]["update_reason"] = "dist_override_suppressed"
else:
# PICK update スキップの残り経路 (disabled / not_yes / tip_missing).
# 画面で「なぜ学習していないか」を明示するため update_reason を必ず入れる.
if isinstance(_OBSERVE_STATE.get("corr"), dict):
if not USE_CORRECTION:
_reason = "disabled"
elif adopted != "yes":
_reason = "adopted_not_yes"
else:
_reason = "tip_diff_missing"
_OBSERVE_STATE["corr"]["updated"] = False
_OBSERVE_STATE["corr"]["update_reason"] = _reason
if adopted == "yes":
pick_ok = True
break
if adopted != "no":
# unknown 等（VLM/撮影/API 失敗 or 人が unknown 入力）は「掴めていない証拠」ではない。
# ここで open すると掴めていたブロックを落とす機序があるため regrasp しない。
# ループを抜けて pick_failed 経路（HOME 復帰）へ流す。
print("[REGRASP] verdict={} は no ではないため掴み直さず終了"
.format(adopted))
break
if attempt == MAX_PICK_ATTEMPTS:
# 最終試行で yes でなければ break（外側で pick 失敗処理）
break
# ---- 掴み直し準備: open → 退避 → 再検出 ----
if not _do_gripper(
"open", "regrasp_open_a{}".format(attempt),
log_record, dry_run):
# DAY265: 掴み直しの open 失敗. 台帳に記録し掴み直し中止して外側へ抜ける.
log_record.setdefault("stage_failures", []).append(
{"stage": "regrasp_open", "attempt": attempt,
"kind": "gripper_open_fail"})
print("[REGRASP] グリッパー open 失敗. 掴み直し中止・PLACE 以降へ")
break
_observe_gripper(0)
# 退避（approach_pick base の j0 を ±85 に振る・place 退避と同形）
retreat = ([SWEPT_J0_FOR_PICK_RETREAT]
+ list(r_app_pick["base_angles"][1:]))
ok, _ = _do_move(
retreat, "regrasp_retreat_a{}".format(attempt),
TOL_APPROACH, log_record, dry_run=dry_run)
if not ok:
# 退避失敗 → アームがカメラ前に残り安全に再検出不可。break して外側 HOME 復帰へ
print("[REGRASP] 退避失敗。掴み直し中止")
break
# block のみ再検出（box は段2 初回の値を保持）
new_block_xz, _ = stage2_detect_block_and_box(
log_record, dry_run, dummy_block_xz, dummy_box_xz,
require_box=False)
if new_block_xz is None:
print("[REGRASP] 再検出でブロック無し。掴み直し中止")
break
cur_block_xz = new_block_xz
_OBSERVE_STATE["block_xz"] = [new_block_xz[0], new_block_xz[1]]
_observe_phase("stage2_redetect" + suf) # regrasp 中の再検出を履歴に出す
_observe_emit_now()
_observe_stage_record(kind="stage")
# ループ次イテレーション
# 既存キー互換: judgments["pick"] は最終試行の判定で埋める
log_record["judgments"]["pick"] = final_pick_judgment
# DAY265: pick 失敗でも PLACE 以降まで進める. 自動 HOME 復帰は無効化.
# 失敗は log_record.pick_failed / stage_failures / pick_attempts に残る.
# PLACE は空グリッパーで走り、VLM が実態 (no 等) を返す. 台帳は下段の finally
# で必ず 1 行追記される.
if not pick_ok:
log_record["pick_failed"] = {
"attempts": len(log_record.get("pick_attempts", [])),
"final_verdict": (final_pick_judgment["verdict"]
if final_pick_judgment else None),
}
log_record.setdefault("stage_failures", []).append(
{"stage": "pick_overall", "kind": "pick_failed",
"final_verdict": (final_pick_judgment.get("verdict")
if final_pick_judgment else None),
"attempts": log_record["pick_failed"]["attempts"]})
print("[RESULT] pick 失敗（{}回試行）. PLACE 以降を空グリッパーで完走"
.format(log_record["pick_failed"]["attempts"]))
# NOTE: 自動 HOME 送信は削除. エイイチが手動で HOME に戻す方針 (DAY265).
# 段7 APPROACH_PLACE（段2 で実検出した box_xz を使う。BOX_XZ 固定値は使わない）
_observe_phase("stage7_approach_place")
ok, r_app_place = stage_lookup_and_move(
"approach_place", box_xz, "stage7_approach_place",
TOL_APPROACH_PLACE, log_record, dry_run, 7)
if not ok:
return False, log_path
approach_place_target = list(r_app_place["target_angles"])
# DAY265: stage7 到達後に横カメラ (Iriun / index=1) で 1 枚撮影 (判定は行わない・
# 記録のみ). 上カメラ撮影は無し (approach_place の上視点は既存フローで撮ってない).
ap_place_side_img = _capture_for_vlm(
"ap_place_side", frames_dir, log_record, dry_run,
run_stem=run_stem, camera_role="side")
_observe_capture_push(log_record)
# 段8 PLACE降下（place_withblock。実検出 box_xz を使う）.
# DAY262: place 補正記憶を適用. pick の "last" は上書きせず "place_last" に格納.
# approach_place (段7) は raw box_xz のまま (approach は円弧概略位置決めなので補正不要).
target_xz_place, place_corr_meta = _apply_correction("place", box_xz)
# 観測 corr の書き込みは stage8 phase 遷移の直後に 1 回だけ行う (下方).
# updated/update_reason は stage9b の update 決定サイトで別途 1 回書く.
# DAY265: PLACE の FK 点選択に姿勢連続性を入れる.
# approach_place 6 軸から ±PLACE_JOINT_TOL_DEG 以内の点のみ候補にする
# (j3/j5 個別帯を全軸帯に統合. 曲げ軸 j1/j4 に歪みが逃げるのを防ぐ).
# approach_place_target 全 6 要素が float 化できるか呼び出し側で確認し、
# 1 つでも欠損なら None を渡す (fk_lookup 側型ガードでも二重に守る).
_ref_joints_place = None
try:
_rj = [float(v) for v in approach_place_target]
if len(_rj) == 6:
_ref_joints_place = _rj
except (TypeError, ValueError, IndexError):
_ref_joints_place = None
# DAY274 層2: PLACE 高さ補正記憶を引く. XZ 補正とは別記憶・独立.
# box_xz からセルキーを組み, dee_y_mm を取得. base_ee_y は fk_lookup が
# 実際に選ぶ点の ee_y を後で参照する (現行 target_ee_y なしで暫定 base 選択→
# base_ee_y + dee_y を狙う二段構成). 実装簡略化のため base_ee_y は
# (PLACE_EE_Y_MIN + PLACE_EE_Y_MAX) / 2 = 100mm を使う (帯中央・保守的).
# 帯全体は 85-115mm なので, dee_y の効果は 100mm 中央から上に稼ぐことに相当.
# 実機で「箱に当たらないラチェット学習」の期待挙動: dee_y=0 → 100mm 中央,
# dee_y=15mm → 115mm 上端付近, dee_y=+30mm → 130mm (帯外・fk は帯拡張候補も評価).
_place_hgt_mem = _height_load() if USE_CORRECTION else {}
_hgt_key = _height_key(float(box_xz[0]), float(box_xz[1]))
_dee_y_mm = _height_get_dee_y_mm(_place_hgt_mem, _hgt_key)
_base_ee_y_m = 0.5 * (PLACE_EE_Y_MIN + PLACE_EE_Y_MAX) # 100mm 帯中央
# DAY281 (レビュー P1 ❌): 高さ記憶ゼロ (通常状態) でも層3を常時有効にする.
# 旧: dee_y=0 で None → 層3 (fk_lookup.py L552) スキップ → ±15mm tol と
# DAY281 新重みが本線で無効化されていた. プローブは常に target_ee_y_m=0.100 で
# 測っていたためこの経路差が診断で漏れていた (レビューが指摘・修正案提示).
# 常に base(=0.100m) 中心で層3を通し, 記憶 (dee_y>0) があれば加算.
_target_ee_y_m = _base_ee_y_m + _dee_y_mm / 1000.0
if _dee_y_mm > 0.0:
print("[HGT ] PLACE apply: cell={} dee_y={:.1f}mm "
"→ target_ee_y={:.4f}m".format(
_hgt_key, _dee_y_mm, _target_ee_y_m))
# DAY274: 補正が適用された場合, 補正後 target で fk_lookup を事前検査し
# joint_ok=True かつ j4 が [J4_SAFE_MIN, J4_SAFE_MAX] 内にあることを確認.
# 満たさなければ補正を破棄し raw box_xz で降下 (servo が深い j4 領域=
# swept_retreat スタール帯へ補正で押し込まれるのを防ぐ). 破綻ガードとは別レイヤー.
if place_corr_meta.get("applied"):
_corr_safety_ok = False
_corr_safety_note = ""
# DAY281 (レビュー P2 ⚠️): この補正 pre-check は意図的に
# place_ee_y_tol_m を渡さない (fk_lookup デフォ ±5mm のまま).
# stage8 本経路は PLACE_EE_Y_TOL_M=±15mm へ緩和したが, pre-check は
# 「補正済み target が旧条件でも成立するか」を厳しく問う位置に留める.
# 不一致 → 補正破棄 (安全側) で run は継続. 双方揃える議論は別タスク.
print("[place_precheck] ee_y_tol=default(±5mm) intentional "
"(stage8 本経路は PLACE_EE_Y_TOL_M=±{:.0f}mm へ緩和済み・pre-check は旧条件のまま)"
.format(PLACE_EE_Y_TOL_M * 1000.0))
try:
_r_safety = fk_lookup(
"place_withblock", target_xz_place,
ee_y_min=PLACE_EE_Y_MIN, ee_y_max=PLACE_EE_Y_MAX,
ref_joints=_ref_joints_place,
joint_tol_deg=PLACE_JOINT_TOL_DEG,
place_ee_y_floor=PLACE_EE_Y_FLOOR,
place_j1_min_deg=PLACE_J1_MIN_DEG,
target_ee_y_m=_target_ee_y_m)
_tgt_angles = _r_safety.get("target_angles") or []
if len(_tgt_angles) == 6:
_tgt_j4 = float(_tgt_angles[4])
_joint_ok = bool(_r_safety.get("joint_limit_ok"))
if not _joint_ok:
_corr_safety_note = "joint_limit_ok=False"
elif not (PLACE_CORR_J4_SAFE_MIN <= _tgt_j4
<= PLACE_CORR_J4_SAFE_MAX):
_corr_safety_note = ("j4={:+.2f} out of [{}, {}]"
.format(_tgt_j4,
PLACE_CORR_J4_SAFE_MIN,
PLACE_CORR_J4_SAFE_MAX))
else:
_corr_safety_ok = True
else:
_corr_safety_note = "target_angles 不正 (len!=6)"
except Exception as e:
# DAY274 修正3 (codex ⚠️): fk_lookup 事前検査の失敗は全て安全側
# (補正破棄・raw box_xz で進む) に倒す. Exception は BaseException
# 直系の KeyboardInterrupt / SystemExit を除外するので, Ctrl+C と
# 正常終了は生きる (握りつぶさない). 想定外例外での中途停止を防ぐ.
_corr_safety_note = "fk_precheck_exc:" + type(e).__name__
if not _corr_safety_ok:
# rollback: 素の box target に戻し, meta もリセット + フラグ残す.
# DAY274 原則1: cell_key は rollback でも残す (どのセルの試行かは
# 補正の成否と無関係な事実). box_xz からセルキー算出し記録.
print("[CORR ] PLACE 補正後 target が不安全 ({}). 補正を破棄し "
"raw box_xz で降下.".format(_corr_safety_note))
_bx_raw = float(box_xz[0])
_bz_raw = float(box_xz[1])
target_xz_place = (_bx_raw, _bz_raw)
place_corr_meta = {
"phase": "place",
"target_before_mm": (_bx_raw * 1000.0, _bz_raw * 1000.0),
"target_after_mm": (_bx_raw * 1000.0, _bz_raw * 1000.0),
"cell_key": _corr_key("place", _bx_raw, _bz_raw),
"dx_mm": None, "dz_mm": None, "n": 0,
"applied": False,
"safety_rollback": True,
"safety_reason": _corr_safety_note,
"reason": "safety_rollback",
}
log_record.setdefault("correction", {})["place_last"] = place_corr_meta
# DAY274 層2 meta を log_record に格納 (台帳・ラチェット更新側で使う).
log_record.setdefault("correction", {})["place_height_last"] = {
"cell_key": _hgt_key,
"dee_y_mm": _dee_y_mm,
"target_ee_y_m": _target_ee_y_m,
}
# DAY278: place_withblock (descend_place) に approach_place の j0 を
# place_j0_ref として渡し, 「真上→真下」を成立させる (A_PICK と同型).
# approach_place_target は stage7 の fk_lookup 結果.
_observe_phase("stage8_place")
# _apply_correction("place") の結果をこの stage の corr に 1 回だけ記録する.
# place_corr_meta は上流で rollback された場合もその値を反映済み (safety fail 時は
# target_before == target_after で reason に "rollback_*" 相当が入る).
# updated/update_reason は stage9b の update 決定サイトで別途書く.
_OBSERVE_STATE["corr"] = {
"phase": "place",
"memory_file": str(CORR_MEM_PATH),
"cell_key": place_corr_meta.get("cell_key"),
"applied_dx_mm": place_corr_meta.get("dx_mm"),
"applied_dz_mm": place_corr_meta.get("dz_mm"),
"target_before": list(place_corr_meta.get("target_before_mm") or []),
"target_after": list(place_corr_meta.get("target_after_mm") or []),
"apply_reason": place_corr_meta.get("reason") or "",
"updated": False,
"update_reason": None,
}
ok, r_place = stage_lookup_and_move(
"place_withblock", target_xz_place, "stage8_place",
TOL_PICK_PLACE, log_record, dry_run, 8,
ref_joints=_ref_joints_place,
joint_tol_deg=PLACE_JOINT_TOL_DEG,
target_ee_y_m=_target_ee_y_m,
place_j0_ref=float(approach_place_target[0]))
# DAY274 層2: stage8 の fk_lookup が実際に選んだ j4 を記録 (台帳
# place_height_j4_selected 用). r_place=None (失敗) は j4=None のまま.
if isinstance(r_place, dict):
_j4_sel = r_place.get("j4_selected_deg")
log_record["correction"]["place_height_last"]["j4_selected_deg"] = (
float(_j4_sel) if isinstance(_j4_sel, (int, float)) else None)
if not ok:
return False, log_path
# ========== stage 8.5 (DAY272 案T): PLACE 再照準 =====================
# stage8 到達後・stage9 リリース前に上カメラで保持ブロック X と箱中心 X
# のズレを j0 だけで締める. Z は触らない (DAY200 地雷回避).
# PICK 視覚サーボの複製移植 (visual_servo_pick.visual_servo_loop 相当).
# PICK 本線関数は 1 バイトも改変しない. 補正記憶とは独立の実時間制御なので
# 計測モード PLACE_CORR_MEASURE_MODE=True でも動く. 失敗時は制御停止せず
# stage9 リリースへ進む (fire-and-forget 相当). place_pre 撮影より前で
# 実施するので, 補正学習の入力は「再照準後の実際の落下位置」になる.
_observe_phase("stage8_5_replace_align")
log_record["stages"].append("stage8_5_replace_align")
# Z サーボは stage8 の place_withblock lookup と同一の引数構成を継承する
# (ee_y 帯・ref_joints・target_ee_y_m・place_j0_ref). 独自 lookup を書かない.
# current_target_xz は stage8 が実際に使った狙い (target_xz_place).
_run_place_visual_servo(
box_xz, log_record, dry_run,
current_target_xz=target_xz_place,
ref_joints=_ref_joints_place,
target_ee_y_m=_target_ee_y_m,
place_j0_ref=float(approach_place_target[0]))
# DAY265: stage8 到達後に横カメラで 1 枚撮影し PLACE 状態を VLM 判定.
# 上カメラの既存 place 判定 (下段の stage9b で撮影/判定) は無変更・並列に追加.
# 姿勢は stage8 到達姿 (箱内降下位置) のまま. 送角/グリッパー変更なし (観測のみ).
place_side_img = _capture_for_vlm(
"place_side", frames_dir, log_record, dry_run,
run_stem=run_stem, camera_role="side")
_observe_capture_push(log_record)
if place_side_img is not None and not dry_run:
side_pj = _judge_image(
place_side_img, "place_side", dry_run, None)
log_record.setdefault("judgments", {})["place_side"] = side_pj
print("[JUDGE ] place_side: verdict={} confidence={} reason={}".format(
side_pj.get("verdict"), side_pj.get("confidence"),
side_pj.get("reason")))
_observe_verdict("side_place", side_pj.get("verdict"),
side_pj.get("confidence"), side_pj.get("reason"))
else:
# 撮影失敗 / dry_run のとき judge は呼ばない. 台帳側は空欄で入る.
log_record.setdefault("judgments", {})["place_side"] = {
"verdict": "unknown", "confidence": 0.0,
"reason": ("dry_run" if dry_run else "capture_failed"),
"model": None, "image": None, "raw": None}
# 台帳の place_pose_id / place_used_j0..j5 用に lookup の pose_id と
# 直近 _do_move の post_actual を寄せる（離す直前の関節角。新規 I/O ゼロ）。
pose_used = log_record.setdefault("pose_used", {})
pose_used["place_pose_id"] = (r_place.get("pose_id")
if r_place else None)
moves = log_record.get("moves") or []
if moves:
pa = moves[-1].get("post_actual")
if isinstance(pa, (list, tuple)) and len(pa) == 6:
pose_used["place_used"] = list(pa)
# DAY262 (仕様1): 補正記憶の学習入力を「離す前」に測る (置きズレ純度を上げる).
# stage9 open より前・stage8 到達姿勢のまま上カメラで1枚撮影しブロック位置を
# 検出し block_place_pre_xz へ. dry_run 時は _capture_for_vlm が None を返し、
# _detect_block_in_place_img も None を返すので自動で学習 skip.
# 送角/グリッパー/sleep は追加しない (fire-and-forget の観測処理).
# 保存先: frames_dir / "{run_stem}_place_pre.jpg".
# VLM 成否判定 (stage9b) は退避後 place.jpg のまま・触らない.
place_pre_img = _capture_for_vlm(
"place_pre", frames_dir, log_record, dry_run, run_stem=run_stem)
_observe_capture_push(log_record)
block_place_pre_xz = _detect_block_in_place_img(place_pre_img)
if block_place_pre_xz is not None:
print("[PLACE_PRE] block xz_m=({:+.4f},{:+.4f})".format(
block_place_pre_xz[0], block_place_pre_xz[1]))
else:
print("[PLACE_PRE] block 検出失敗 (補正学習は skip・制御は継続)")
# 段9 グリッパー開（離す）
_observe_phase("stage9")
log_record["stages"].append("stage9")
stage9_ok = _do_stage9_release_gripper(log_record, dry_run)
if not stage9_ok:
# DAY265: 離す動作失敗しても継続. 台帳/JSON に記録.
log_record.setdefault("stage_failures", []).append(
{"stage": 9, "label": "stage9_release", "kind": "gripper_open_fail"})
print("[STAGE 9] グリッパー離す失敗. 記録のみで判定へ")
_observe_gripper(0)
# 非移動 stage の通知 emit（移動なし→画面に stage9 行を出す）
_observe_emit_now()
_observe_stage_record(kind="stage")
# 判定②: 段9 と段10 の間に挟む。退避（j0=+85, approach_place base）→ 撮影 → 判定。
# 退避移動が沈黙/ABORT → place 判定 skip・ログ記録のみ。stage 10 (HOME 復帰) は続行。
_observe_phase("stage9b_judge_place")
log_record["stages"].append("stage9b_judge_place")
swept_target = [SWEPT_J0_FOR_PLACE_JUDGE] + list(r_app_place["base_angles"][1:])
ok_swept, _ = _do_move(
swept_target, "stage9b_swept_retreat", TOL_SWEPT_RETREAT,
log_record, dry_run=dry_run)
if not ok_swept:
log_record["judgments"]["place"] = {
"verdict": "unknown", "confidence": 0.0,
"reason": "swept_retreat_failed", "model": None,
"image": None, "raw": None,
}
_observe_verdict("place", "unknown", 0.0, "swept_retreat_failed")
# 非移動 stage の通知 emit（stage9b 失敗ブランチ→画面に verdict=unknown 行）
_observe_emit_now()
_observe_stage_record(kind="stage")
print("[JUDGE ] place: swept_retreat 失敗のため判定スキップ。"
"HOME 復帰へ続行")
else:
place_img = _capture_for_vlm(
"place", frames_dir, log_record, dry_run, run_stem=run_stem)
_observe_capture_push(log_record)
_OBSERVE_STATE["cap_place"] = (str(place_img) if place_img else None)
# 観測: 同じ画像に赤ブロック HSV 検出を脇でかける（既存判定/分岐は不変）
_OBSERVE_STATE["block_place_xz"] = _detect_block_in_place_img(place_img)
# DAY274: 同じ画像に白箱検出も脇でかける (stage2 と同一検出器 06f).
# 撮影・送角・グリッパー・sleep は追加しない (画像は既に手元にある).
# 検出失敗 (None) は _append_ledger 側で box_shift 3 列を空欄にする.
_OBSERVE_STATE["box_xz_last"] = _detect_white_box_in_place_img(place_img)
# DAY274 層2: box_shift_dist を計算し, 高さ補正記憶をラチェット更新.
# box_xz (stage2 検出) と box_xz_last (退避後) 両方揃った時のみ.
# 検出失敗 (None) や box_xz 欠損時は更新スキップ (現状維持).
# PLACE 補正 (XZ) と独立記憶 (別 json). USE_CORRECTION=False で完全停止.
_bxl = _OBSERVE_STATE.get("box_xz_last")
_box_shift_dist_mm = None
if (USE_CORRECTION
and isinstance(box_xz, (list, tuple)) and len(box_xz) == 2
and isinstance(box_xz[0], (int, float))
and isinstance(box_xz[1], (int, float))
and isinstance(_bxl, (list, tuple)) and len(_bxl) == 2
and isinstance(_bxl[0], (int, float))
and isinstance(_bxl[1], (int, float))):
_dx = (float(_bxl[0]) - float(box_xz[0])) * 1000.0
_dz = (float(_bxl[1]) - float(box_xz[1])) * 1000.0
_box_shift_dist_mm = (_dx * _dx + _dz * _dz) ** 0.5
try:
_hmem = _height_load()
_hkey = _height_key(float(box_xz[0]), float(box_xz[1]))
_new_entry = _height_update_ratchet(
_hmem, _hkey, _box_shift_dist_mm)
if _height_save(_hmem):
print("[HGT ] update {} dist={:.1f}mm → dee_y={:.1f}mm "
"(n={})".format(
_hkey, _box_shift_dist_mm,
_new_entry.get("dee_y_mm", 0.0),
_new_entry.get("n", 0)))
except Exception as e:
print("[HGT ] [WARN] ratchet update 例外: {}: {}".format(
type(e).__name__, e))
place_vlm = _judge_image(
place_img, "place", dry_run, dummy_place_verdict)
place_vlm_verdict = place_vlm["verdict"]
place_human_verdict = None
if human_judge and not dry_run:
place_human_verdict = _ask_human_verdict(
place_img, place_vlm, kind="place")
place_adopted = place_vlm_verdict # 分岐は常に VLM。記録のみ人
_observe_verdict("place", place_adopted, place_vlm["confidence"], place_vlm["reason"])
# 非移動 stage の通知 emit（stage9b 成功ブランチ→画面に verdict 行）
_observe_emit_now()
_observe_stage_record(kind="stage")
place_judgment = dict(place_vlm)
place_judgment["verdict"] = place_adopted
place_judgment["vlm_verdict"] = place_vlm_verdict
place_judgment["human_verdict"] = place_human_verdict
log_record["judgments"]["place"] = place_judgment
print("[JUDGE ] place: adopted={} (vlm={} human={}) "
"confidence={} reason={}".format(
place_adopted, place_vlm_verdict, place_human_verdict,
place_vlm["confidence"], place_vlm["reason"]))
# DAY274 案: PLACE 補正記憶の起動 (PICK と対称移植).
# 更新条件: 最終 pick_verdict == "yes" の全試行 (place verdict 非依存).
# 掴めていれば「置きズレ」は有効な学習情報で、外した試行こそズレ方向を持つ.
# place=yes 限定にすると学習が始まらない (swept_retreat 抜けで判定 skip も
# 起きるため).
# 破綻ガード 3 種 (どれかに当たったら update skip・apply は通常どおり動く):
# 1. block_place_pre_xz is None (検出失敗)
# 2. sign(block_place_x) != sign(box_x) かつ abs(block_place_x - box_x)
# > PLACE_CORR_SIGN_FLIP_GAP_MM (150mm) → PICK 残像/別物拾い
# 3. dist(block_place_xz, box_xz) > PLACE_CORR_MAX_DIST_MM (120mm) → 外れ値
# Δ = (離す前 - 箱) = 「実測 - 目標」・PICK と符号整合.
# EMA α=0.5 + 1 更新 ±15mm クランプ は _corr_update 側で PICK と共通に効く.
# KIOXIA 未マウント時は _corr_save がスキップし自動素通り.
# DAY268 手0 (案S) の measure mode 分岐は互換のため残置 (通常 False 運用).
# DAY274 原則2: update 各経路で reason を place_last meta に必ず埋める.
# 判定ロジックは無改変. reason を log_record.correction.place_last に追記するだけ.
_place_update_reason = None
if PLACE_CORR_MEASURE_MODE:
print("[CORR ] PLACE measure mode: apply/update skipped")
_place_update_reason = "measure_mode"
elif not USE_CORRECTION:
_place_update_reason = "disabled" # correction 完全無効時は素通り
else:
# 更新条件: 最終 pick_verdict == "yes" (override 後の adopted を採用).
_final_pick_verdict = (
final_pick_judgment.get("verdict")
if isinstance(final_pick_judgment, dict) else None)
if _final_pick_verdict != "yes":
print("[CORR ] PLACE update skip: reason=pick_not_yes "
"(final_verdict={})".format(_final_pick_verdict))
_place_update_reason = "pick_not_yes"
elif (block_place_pre_xz is None
or not isinstance(block_place_pre_xz, (list, tuple))
or len(block_place_pre_xz) != 2
or block_place_pre_xz[0] is None
or block_place_pre_xz[1] is None):
# 破綻ガード 1: 検出失敗.
print("[CORR ] PLACE update skip: reason=detect_none")
_place_update_reason = "detect_none"
elif (not isinstance(box_xz, (list, tuple))
or len(box_xz) != 2
or box_xz[0] is None or box_xz[1] is None):
print("[CORR ] PLACE update skip: reason=box_xz_none")
_place_update_reason = "box_xz_none"
else:
_bp_x_mm = float(block_place_pre_xz[0]) * 1000.0
_bp_z_mm = float(block_place_pre_xz[1]) * 1000.0
_bx_mm = float(box_xz[0]) * 1000.0
_bz_mm = float(box_xz[1]) * 1000.0
_dx_mm_place = _bp_x_mm - _bx_mm
_dz_mm_place = _bp_z_mm - _bz_mm
_dist_mm = math.hypot(_dx_mm_place, _dz_mm_place)
# 破綻ガード 2: sign flip かつ 大ギャップ (>150mm) → PICK 残像判定.
_sign_flip = (
((_bp_x_mm >= 0.0) != (_bx_mm >= 0.0))
and abs(_bp_x_mm - _bx_mm) > PLACE_CORR_SIGN_FLIP_GAP_MM)
# 破綻ガード 3: 距離が閾値超 → 外れ値.
_dist_over = (_dist_mm > PLACE_CORR_MAX_DIST_MM)
if _sign_flip:
print("[CORR ] PLACE update skip: reason=sign_flip "
"block_x={:+.1f} box_x={:+.1f} gap={:.1f}mm > {:.0f}mm"
.format(_bp_x_mm, _bx_mm,
abs(_bp_x_mm - _bx_mm),
PLACE_CORR_SIGN_FLIP_GAP_MM))
_place_update_reason = "sign_flip"
elif _dist_over:
print("[CORR ] PLACE update skip: reason=dist_over "
"dist={:.1f}mm > {:.0f}mm"
.format(_dist_mm, PLACE_CORR_MAX_DIST_MM))
_place_update_reason = "dist_over"
else:
key_place = _corr_key("place", box_xz[0], box_xz[1])
mem_place = _corr_load()
e_new_place = _corr_update(
mem_place, key_place, _dx_mm_place, _dz_mm_place)
if _corr_save(mem_place):
print("[CORR ] mem key={} dx={:+.2f} dz={:+.2f} n={} "
"(EMA α={}・pick=yes 全試行・dist={:.1f}mm)".format(
key_place, e_new_place["dx_mm"],
e_new_place["dz_mm"], e_new_place["n"],
EMA_ALPHA, _dist_mm))
_place_update_reason = "updated"
# place_last.update_reason に記録 (判定はしない・記録だけ). ledger で参照.
_pl = log_record.setdefault("correction", {}).get("place_last")
if isinstance(_pl, dict) and _place_update_reason is not None:
_pl["update_reason"] = _place_update_reason
# 観測: _corr_update / update skip の判定が確定した箇所で corr を 1 回だけ書く.
# updated / update_reason を含めた完全な dict を書き直す (mutation ではなく再構築).
# apply 情報は place_corr_meta から復元. この時点で phase は stage9b_judge_place の
# まま (stage10 遷移前), 直近の emit は判定分岐内で完了済み. ここで書いた corr は
# 次 emit を伴わないが「実際に _corr_update / skip を判断した stage」を示す情報として
# _OBSERVE_STATE に残る.
if isinstance(place_corr_meta, dict):
_OBSERVE_STATE["corr"] = {
"phase": "place",
"memory_file": str(CORR_MEM_PATH),
"cell_key": place_corr_meta.get("cell_key"),
"applied_dx_mm": place_corr_meta.get("dx_mm"),
"applied_dz_mm": place_corr_meta.get("dz_mm"),
"target_before": list(place_corr_meta.get("target_before_mm") or []),
"target_after": list(place_corr_meta.get("target_after_mm") or []),
"apply_reason": place_corr_meta.get("reason") or "",
"updated": (_place_update_reason == "updated"),
"update_reason": (None if _place_update_reason == "updated"
else _place_update_reason),
}
# ---- 修正3 (2026-07-12): 学習オフセット (place_release_offset.json) の更新 ----
# correction_memory とは独立. PLACE_CORR_MEASURE_MODE=True (今の運用) でも
# ここは動く. servo_place に after_mm / update_reason を残す (grep 可能).
#
# 修正B (2026-07-12・R3 本丸): 学習入力を「release 後の最終検出」に切替.
# 旧: block_place_pre_xz (爪で保持中・Z サーボが 5mm まで詰めた値 → 系統誤差
# +32mm を学習できない)
# 新: _OBSERVE_STATE["block_place_xz"] (stage9b 退避後の最終 place 検出.
# 台帳の in_box_dist / place Δz と同じ測定).
# 修正C (2026-07-12・R3 追加): Z サーボが異常終了した run では学習 skip.
# 正常系 (z_converged / z_same_pose_skip / z_observe_once) 以外は skip.
# 異常系 (z_guard_stop / lookup_error / z_move_failed / z_detect_fail /
# x_not_converged:* / loop_exception / disabled) で拾うと最終位置が「正しく
# 置けなかった位置」なのでゴミを食う.
_sp_rec = log_record.setdefault("servo_place", {})
_release_after_mm = float(
_sp_rec.get("release_offset_applied_mm", 0.0) or 0.0)
_release_update_reason = None
# 修正C: Z サーボの exit 正常性チェック.
_NORMAL_Z_EXITS = ("z_converged", "z_same_pose_skip", "z_observe_once")
_z_exit_str = str(_sp_rec.get("z_exit_reason") or "")
_top_exit_str = str(_sp_rec.get("exit_reason") or "")
_z_exit_normal = (
_z_exit_str in _NORMAL_Z_EXITS
and _top_exit_str != "loop_exception")
if not PLACE_RELEASE_OFFSET_ENABLE:
_release_update_reason = "disabled"
else:
_final_pick_verdict_off = (
final_pick_judgment.get("verdict")
if isinstance(final_pick_judgment, dict) else None)
# 修正B: release 後の最終検出値を使う.
_final_block_xz = _OBSERVE_STATE.get("block_place_xz")
if _final_pick_verdict_off != "yes":
_release_update_reason = "pick_not_yes"
elif not _z_exit_normal:
# 修正C: 異常終了 run は学習 skip.
_release_update_reason = "z_exit_abnormal:{}".format(
_z_exit_str or _top_exit_str or "empty")
elif (_final_block_xz is None
or not isinstance(_final_block_xz, (list, tuple))
or len(_final_block_xz) != 2
or _final_block_xz[0] is None
or _final_block_xz[1] is None):
# release 後に検出できていない → 学習しない.
_release_update_reason = "detect_none_after_release"
elif (not isinstance(box_xz, (list, tuple))
or len(box_xz) != 2
or box_xz[0] is None or box_xz[1] is None):
_release_update_reason = "box_xz_none"
else:
_bp_z_mm_off = float(_final_block_xz[1]) * 1000.0
_bx_z_mm_off = float(box_xz[1]) * 1000.0
_final_dz_mm = _bp_z_mm_off - _bx_z_mm_off
_rel_mem_upd = _release_offset_load()
_rel_key_upd = _release_offset_key(
float(box_xz[0]), float(box_xz[1]))
_release_after_mm, _release_update_reason = (
_release_offset_update(
_rel_mem_upd, _rel_key_upd, _final_dz_mm))
if _release_update_reason == "updated":
if _release_offset_save(_rel_mem_upd):
print("[OFFSET] key={} final_dz={:+.2f}mm "
"(release 後の検出 - box_z) applied_before={:+.2f}mm "
"after={:+.2f}mm "
"(α={} step_clamp=±{} total_clamp=±{})".format(
_rel_key_upd, _final_dz_mm,
float(_sp_rec.get(
"release_offset_applied_mm", 0.0)
or 0.0),
_release_after_mm,
PLACE_RELEASE_OFFSET_ALPHA,
PLACE_RELEASE_OFFSET_STEP_MAX,
PLACE_RELEASE_OFFSET_TOTAL_MAX))
else:
_release_update_reason = "save_failed"
_sp_rec["release_offset_after_mm"] = float(_release_after_mm)
_sp_rec["release_offset_update_reason"] = _release_update_reason
# 段10 lift_back → HOME. DAY265 で自動 HOME 復帰を全撤廃した後、
# 正常完走フロー末尾の HOME 復帰まで失われていたため復活 (2026-07-03 追記).
# 区別: 「失敗した瞬間に勝手に HOME」(abort_recover_home / pickfail_to_home) は
# 撤廃したまま. 「通しを走り切った最後に HOME で締める」は残す.
# 失敗完走 (stage_failures 混じり) でも stage10 まで到達したらここで HOME に戻る.
_observe_phase("stage10")
log_record["stages"].append("stage10")
# C1 (DAY268 案R): stage8_place (PLACE 降下) が未到達なら stage10_lift_back を
# ガード. moves から stage8_place の arrived を引く (stage_lookup_and_move は
# record-and-continue で ok=True を返すため ok では判定できない).
_place_not_reached = False
for _m in reversed(log_record.get("moves") or []):
if str(_m.get("label", "")).startswith("stage8_place"):
_place_not_reached = (_m.get("arrived") is False)
break
ok, _ = _do_move(
approach_place_target, "stage10_lift_back", TOL_LIFT_BACK,
log_record, dry_run=dry_run,
abort_if_prev_not_reached=_place_not_reached)
if not ok:
log_record.setdefault("stage_failures", []).append(
{"stage": 10, "label": "stage10_lift_back", "kind": "not_reached"})
print("[STAGE 10] lift_back 未到達. 記録のみで HOME 締めへ")
# 通し末尾の HOME 締め. HOME=[0,0,0,0,0,-30] (SPEED=20 は _do_move / daemon 側).
# arrived=False でも記録のみで完走判定へ進める (record-and-continue).
ok, _ = _do_move(
HOME, "stage10_to_home", TOL_HOME,
log_record, dry_run=dry_run)
if not ok:
log_record.setdefault("stage_failures", []).append(
{"stage": 10, "label": "stage10_to_home", "kind": "not_reached"})
print("[STAGE 10] HOME 未到達. 記録のみで完走判定へ")
# 通し完走. 途中で stage_failures があれば result にその旨を残す.
stage_failures = log_record.get("stage_failures") or []
if stage_failures:
log_record["result"] = "completed_with_failures"
print("[RESULT] 完走 (失敗 {} 件を含む)".format(len(stage_failures)))
else:
log_record["result"] = "completed"
print("[RESULT] 完走")
success = True
return True, log_path
except KeyboardInterrupt:
user_interrupted = True
_abort(log_record, log_record["stages"][-1] if log_record["stages"] else 0,
"KeyboardInterrupt")
print("[INTERRUPT] Ctrl-C 検出。auto-HOME 試行 → ログ保存 → セッション終了")
# DAY283 §4 + FIX-R2-1/2: 試行中 Ctrl-C の auto-HOME 中断パス.
# ① gripper 不変 (掴んだまま HOME まで運ぶ・ブロックを落とさない・
# stage1_home_and_open は流用しない)
# ② 送角は既存 _do_move プリミティブ再利用 (send_coords 禁止・
# HOME は固定定数 [0,0,0,0,0,-30] を送る・lookup ガード不要)
# ③ daemon 無応答は _interrupt_daemon_budget で 5s 予算に短縮 →
# visual_servo_pick._daemon_request の既存クリーンアップ経由で
# RuntimeError → daemon_get_angles が None を返し諦める
# ④ 2 度目 Ctrl-C は「中断処理全体」を囲む except KeyboardInterrupt で
# 捕捉 (どの段階で来ても _hr を確定・inner finally で必ず代入)
# ⑤ auto-HOME の例外は握って finally の台帳追記を必ず走らせる
# ⑥ 抜けは raise (return しない). finally → main へ KeyboardInterrupt 伝播.
_hr = {"attempted": True, "note": "DAY283_interrupt_auto_home",
"success": None, "abort_reason": None,
"pre_home_pose": None} # OBS-1: auto-HOME 前姿勢を必ず記録
# FIX-R2-2: 2度目 Ctrl-C の catch を中断処理全体へ拡大.
# `log_record["home_recovery"] = _hr` を inner try の finally に置き,
# 生存確認中/送角中/どこで2度目が来ても home_recovery と pre_home_pose
# (取得済みなら) が残る. outer bare raise は最初の KeyboardInterrupt を再送出.
try:
try:
if dry_run:
# dry_run: _do_move が dry_run=True で planned のみ返す.
_do_move(list(HOME), "interrupt_home", TOL_HOME,
log_record, dry_run=True)
_hr["success"] = True # planned OK の意味
_hr["note"] = "DAY283_interrupt_auto_home_dry_run"
else:
# FIX-R2-1: 中断/手動 HOME パス全体 (生存確認〜_do_move 完了) を
# 予算 CM で包む. visual_servo_pick / j0_sweep_scan は無編集で,
# モジュール属性の一時差し替え + finally 復元により全 RPC が 5s 化.
with _interrupt_daemon_budget(timeout_sec=5.0, get_retry=2):
# 生存確認: filtered ではなく単発 daemon_get_angles を使用.
# 予算内で最悪 5s. None/例外 → 諦めて停止・報告.
_pre = daemon_get_angles()
_hr["pre_home_pose"] = (list(_pre)
if _pre and len(_pre) == 6 else None)
if not _pre or len(_pre) != 6:
_hr["success"] = False
_hr["abort_reason"] = "daemon_unresponsive_home_skipped"
print("[INTERRUPT] daemon 無応答 (budget=5s) → auto-HOME 諦めて停止・報告のみ")
else:
# arrived を必ず読み silent drop (DAY189/268) を防ぐ.
_arrived, _post = _do_move(
list(HOME), "interrupt_home", TOL_HOME,
log_record, dry_run=False)
_hr["success"] = bool(_arrived)
if _arrived:
print("[INTERRUPT] auto-HOME 完了 (arrived=True)")
else:
_hr["abort_reason"] = "home_not_reached"
print("[INTERRUPT] auto-HOME 未到達 (arrived=False). "
"現在姿勢を目視確認のこと")
except KeyboardInterrupt:
# 2度目 Ctrl-C: 生存確認中/送角中/到達待ち中のどこでも捕捉.
# daemon 読みは追加しない (プロトコル割込み中で不確実).
# 取得済み _pre は上で pre_home_pose に既記録.
_hr["success"] = False
_hr["abort_reason"] = "interrupted_during_home_second_ctrl_c"
print("[INTERRUPT] 2 度目 Ctrl-C: 中断処理を打ち切り. HOME 指令は "
"送信済みなら継続中の可能性あり. 目視確認のこと")
except Exception as _e_home:
_hr["success"] = False
_hr["abort_reason"] = "auto_home_exception:{}".format(
type(_e_home).__name__)
print("[INTERRUPT] auto-HOME 例外 (握って停止・報告): {}: {}".format(
type(_e_home).__name__, _e_home))
finally:
# FIX-R2-2 + FIX-R3-4: どの経路 (正常/2度目Ctrl-C/例外) でも _hr を必ず
# log_record へ. 代入自体に KeyboardInterrupt が刺さっても再捕捉して
# 再代入 (dict STORE は失敗しない) → raise で伝播. run finally の
# setdefault が DAY265 既定値を書くのを防ぐ.
try:
log_record["home_recovery"] = _hr
except KeyboardInterrupt:
log_record["home_recovery"] = _hr
raise
# § 4-5/6: raise. return しない. finally 経由で台帳追記 + main へ伝播.
raise
finally:
# DAY265 → DAY283: 「自動 HOME 復帰の禁止」原則は現存（失敗リトライ HOME や
# completed_with_failures 経路での自動送角は依然として書かない）.
# DAY283 で意図的な例外を追加: Ctrl-C 中断時のみ except 内で auto-HOME を試行し
# log_record["home_recovery"] を上書き済み. その値を尊重するため setdefault で
# 未設定 (通常経路) のときだけ従来の "auto_home_recover_disabled_DAY265" を書く.
# NOTE (レビュー・handoff 用): 「auto_home_recover_disabled_DAY265」の禁止は
# 【失敗リトライ HOME】に対する原則であり, Ctrl-C 中断パスの一度きり HOME は
# 意図的な例外. ドリフトで再無効化しないこと (次セッションの誤修正防止).
log_record.setdefault("home_recovery", {
"attempted": False, "note": "auto_home_recover_disabled_DAY265"})
log_record["ended_at"] = datetime.now().isoformat()
if not success and "result" not in log_record:
log_record["result"] = "aborted"
if not dry_run:
# FIX-R2-4: fresh_mode(0) は daemon_connected のときだけ. 未接続時に
# finally が再接続を試みて DAEMON_TIMEOUT_SEC (最悪 35s) 待つのを防ぐ.
# FIX-R3-3: 中断終了時 (user_interrupted=True) は socket の応答境界が
# 不確かなため予算 CM (5s) 下で送る. 通常本線 (user_interrupted=False)
# は従来通り 35s のまま (本線耐性を維持).
if daemon_connected:
if user_interrupted:
try:
with _interrupt_daemon_budget(timeout_sec=5.0, get_retry=2):
try:
daemon_set_fresh_mode(0)
except (Exception, KeyboardInterrupt):
pass
except (Exception, KeyboardInterrupt):
# FIX-R4-1: 中断 run の fresh_mode cleanup 中の追加 Ctrl-C は握って
# 記録・close を優先する (以後 close / atomic write / ledger へ進む).
# 設計境界: この cleanup RPC 中の追加割込みは握るが, 以後の連打
# (atomic write / ledger の任意バイトコード位置) までは非保証.
# 純 Python で任意位置の完全原子性は作れないため, ここを保証境界とする.
pass
else:
try:
daemon_set_fresh_mode(0)
except Exception:
pass
# close_daemon_socket は内部 None チェック済みなので常に呼んで良い.
try:
close_daemon_socket()
except Exception:
pass
try:
_atomic_write_json(log_path, log_record)
except Exception as e:
print("[WARN ] ログ書き出し失敗: {}".format(e))
print("[LOG ] {}".format(log_path))
# 試行台帳 CSV を KIOXIA SSD に追記（fire-and-forget・未マウントなら skip）.
# 既存 JSON 出力は維持・CSV は別物.
# dry_run の試行は実機データではないため台帳に残さない（CSV を清浄に保つ）.
# FIX-R2-4: daemon_connected でもゲート (エイイチ確定: 接続失敗行は台帳に
# 含めない. 下流解析の「行数=送角試行数」の前提を守る. 旧挙動=try 移動前は
# 接続失敗が return False で台帳追記まで届かなかった. try 移動後も等価再現).
if not dry_run and daemon_connected:
_append_ledger(log_record, run_stem)これがレビュー二周目で SIGALRM から置き換えた予算コンテキストマネージャ。中断・手動 HOME の間だけ daemon クライアントのタイムアウトとリトライ回数を短い値へ差し替え、finally で必ず戻す。クライアント側のファイルには一切触っていない。
@contextlib.contextmanager
def _interrupt_daemon_budget(timeout_sec=5.0, get_retry=2):
"""DAY283 FIX-R2-1: 中断/手動 HOME パス専用の daemon RPC 予算コンテキスト.
- visual_servo_pick.DAEMON_TIMEOUT_SEC と j0_sweep_scan.GET_ANGLES_RETRY を
一時的に短縮する (visual_servo_pick / j0_sweep_scan は **ファイル無編集**).
両者はモジュールグローバルで呼び出し時参照される (現物 L541/L154) ため
モジュール属性の一時差し替えで全 RPC に自動反映される.
- timeout 経路は visual_servo_pick._daemon_request の既存クリーンアップ
(sock.close / _daemon_sock=None / buffer クリア / RuntimeError raise) を
そのまま通る. 前版 SIGALRM の問題群 (ハンドラ復元・SIGINT 競合・
クリーンアップ迂回) は根ごと消える.
- 復元は finally. 2度目 Ctrl-C (KeyboardInterrupt) でも必ず復元される.
- **本線経路では絶対に使わないこと** (中断/手動 HOME 専用. 本線は
_daemon_get_angles_filtered の従来 35s×12 の耐性を維持する).
設計判断 (仕様書に記録): 中断 HOME 経路全体の最悪は厳密 timeout_sec 秒では
ない. 生存確認は単発上限, _do_move 内部 (filtered read×2 + 送角 + 到達待ち)
は各 RPC timeout_sec × 有限回で最悪 30-60秒級 (従来 420秒級から短縮).
「必ず有限時間で安全側に落ちる」を満たす.
"""
import visual_servo_pick as _vsp
import j0_sweep_scan as _jss
# 読み取り (失敗しない・状態を汚さない) は try 前で可.
_saved_timeout = _vsp.DAEMON_TIMEOUT_SEC
_saved_retry = _jss.GET_ANGLES_RETRY
# FIX-R3-1: 属性変更を try 内へ. 1個目/2個目の間 or 直後の KeyboardInterrupt でも
# finally 復元が確実に走る (旧: try 外だと片方 or 両方が新値のまま残る).
try:
_vsp.DAEMON_TIMEOUT_SEC = float(timeout_sec)
_jss.GET_ANGLES_RETRY = int(get_retry)
yield
finally:
# FIX-R3-2: 復元中の KeyboardInterrupt を再捕捉で保護. 1個目復元前/途中で
# 割込みが来ると片方または両方が短縮値のまま残り本線に波及するため, KbdInt
# を握って再代入 (属性代入は単一 STORE で失敗しない) → raise で伝播.
try:
_vsp.DAEMON_TIMEOUT_SEC = _saved_timeout
_jss.GET_ANGLES_RETRY = _saved_retry
except KeyboardInterrupt:
_vsp.DAEMON_TIMEOUT_SEC = _saved_timeout
_jss.GET_ANGLES_RETRY = _saved_retry
raiseh キーで呼ぶ手動 HOME 復帰。試行の外で使う短命接続で、グリッパーには触らず、台帳にも追記しない。到達結果を見て「完了」か「未到達」かを正しく表示する。
def _manual_home(dry_run=False):
"""DAY283 §5: `h` コマンドで HOME 復帰して待機する (試行外・台帳追記なし).
- 短命ソケット: get_daemon_socket → HOME 送角 → close_daemon_socket
- gripper は触らない (open しない・前の中断で掴んでいた場合に落とさない)
- 送角は _send_and_wait_arrival(HOME, "manual_home", TOL_HOME) 単発
- daemon 無応答時は停止・報告 (ハングしない・§4-3 同型)
- 例外は握って戻る (常駐ループを止めない).
"""
if dry_run:
print("[HOME ] --dry-run のため送角スキップ (planned: HOME={})".format(HOME))
return
print("[HOME ] 手動 HOME 復帰開始 (試行外・台帳追記なし)")
_opened = False
try:
# FIX-R2-1: connect+ping 含め全 RPC を 5s 予算に短縮.
# get_daemon_socket 内の ping (visual_servo_pick L513) も予算 CM 下で
# DAEMON_TIMEOUT_SEC=5 で走る → 再レビュー指摘の「manual_home connect 35s
# 問題」も同時解消.
with _interrupt_daemon_budget(timeout_sec=5.0, get_retry=2):
try:
get_daemon_socket()
_opened = True
except Exception as e:
print("[HOME ] daemon 接続失敗 (budget=5s): {}: {}. 停止・報告のみ".format(
type(e).__name__, e))
return
# 生存確認: filtered ではなく単発 daemon_get_angles.
_pre = daemon_get_angles()
if not _pre or len(_pre) != 6:
print("[HOME ] daemon 無応答 (budget=5s). HOME 諦めて停止・報告")
return
try:
# arrived を必ず読み silent drop を防ぐ.
_arrived, _retried = _send_and_wait_arrival(
list(HOME), "manual_home", TOL_HOME,
send_wait_sec=SEND_WAIT_SEC)
if _arrived:
print("[HOME ] 手動 HOME 完了 (arrived=True)")
else:
print("[HOME ] 手動 HOME 未到達 (arrived=False). "
"現在姿勢を目視確認のこと")
except KeyboardInterrupt:
# 送角途中の Ctrl-C は握って報告のみ. Pi 側へ送信済み HOME 指令は
# 物理的に継続中の可能性あり. 常駐ループを終わらせない.
print("[HOME ] 手動 HOME 中に Ctrl-C: 到達待ちを中断. HOME 指令は "
"送信済みのため継続中の可能性あり. 目視確認のこと")
except Exception as e:
print("[HOME ] HOME 送角例外: {}: {}. 停止・報告".format(
type(e).__name__, e))
finally:
if _opened:
try:
close_daemon_socket()
except Exception:
pass最後に、常駐ループ本体の main()。一度の起動でキー入力（n／h／q）を受けて試行を回し続ける。試行中の Ctrl-C を受けたらメニューに戻らずセッションを終える。
def main():
ap = argparse.ArgumentParser(
description="通し Pick & Place（j0 対応表方式）")
ap.add_argument(
"--dry-run", action="store_true",
help="送信せず、各段の lookup 結果とガード判定だけを出力",
)
ap.add_argument(
"--dummy-block-x", type=float, default=None,
help="dry_run 用 block_xz の x（指定時は --dummy-block-z とセット）",
)
ap.add_argument(
"--dummy-block-z", type=float, default=None,
help="dry_run 用 block_xz の z（指定時は --dummy-block-x とセット）",
)
ap.add_argument(
"--dummy-box-x", type=float, default=None,
help="dry_run 用 box_xz の x（指定時は --dummy-box-z とセット）。"
"未指定なら BOX_XZ={} をデフォルト".format(BOX_XZ),
)
ap.add_argument(
"--dummy-box-z", type=float, default=None,
help="dry_run 用 box_xz の z（指定時は --dummy-box-x とセット）",
)
ap.add_argument(
"--dummy-pick-verdict", choices=["yes", "no", "unknown"], default=None,
help="dry_run 用 pick 判定の verdict 注入（デフォルト unknown）",
)
ap.add_argument(
"--dummy-place-verdict", choices=["yes", "no", "unknown"], default=None,
help="dry_run 用 place 判定の verdict 注入（デフォルト unknown）",
)
ap.add_argument(
"--human-judge", action="store_true",
help="pick/place 判定で人による目視判定を入力させる（dry_run 時は無効）。"
"校正フェーズ向け。VLM 判定は記録のみで分岐は人の verdict を採用",
)
args = ap.parse_args()
dummy_block = None
dummy_box = None
if args.dry_run:
if args.dummy_block_x is not None and args.dummy_block_z is not None:
dummy_block = (args.dummy_block_x, args.dummy_block_z)
else:
dummy_block = (0.05, 0.30)
print("[INIT ] dry_run dummy block_xz default = {}".format(dummy_block))
if args.dummy_box_x is not None and args.dummy_box_z is not None:
dummy_box = (args.dummy_box_x, args.dummy_box_z)
else:
dummy_box = tuple(BOX_XZ)
print("[INIT ] dry_run dummy box_xz default (BOX_XZ) = {}".format(dummy_box))
# DAY283 §3-A: 常駐ループ. 1 プロセスで複数試行を回す (FK cache / import cost を
# 2 試行目以降ゼロにするため wrap 方式). run() は毎回そのまま呼ぶ (init_once を
# 新設しない・モジュールキャッシュがプロセス寿命で持続する・§6 表参照).
# DAY283 FIX-5: プロンプト案内は断定表現を避け, 目視/h コマンドを促す (C4 対応).
# 前試行が把持中に失敗した場合, 次試行 stage1 の gripper open が空中で
# ブロックを落とし得る (stage1 のロジックは不変・従来挙動維持). そのため
# プロンプト先頭で「掴んだまま失敗した場合は h で HOME 確認」を明示する.
print("[LOOP] 常駐モード開始. コマンド: n または Enter=次試行 / h=HOME 復帰 / q=終了")
print("[LOOP] 注意: 前試行が把持中に失敗 (HOME 未到達で握ったまま等) した場合,"
" 次試行 stage1 の gripper open で空中落下し得る."
" その場合は h で HOME を試行してから n で次試行へ進むこと.")
_trial_counter = 0
try:
while True:
try:
_cmd = input(
"\n[LOOP] 次の操作 [n=次試行 (Enter可) / h=HOME / q=終了]: "
).strip().lower()
except EOFError:
# 標準入力 EOF (pipe 終端等) は正常終了扱い.
print("[LOOP] EOF 検出 → 常駐モード終了")
break
if _cmd in ("q", "quit", "exit"):
print("[LOOP] q 受領 → 常駐モード終了")
break
if _cmd == "h":
_manual_home(dry_run=args.dry_run)
continue
if _cmd in ("", "n", "next"):
_trial_counter += 1
print("[LOOP] === 試行 #{} 開始 ===".format(_trial_counter))
try:
success, log_path = run(
dry_run=args.dry_run,
dummy_block_xz=dummy_block,
dummy_box_xz=dummy_box,
dummy_pick_verdict=args.dummy_pick_verdict,
dummy_place_verdict=args.dummy_place_verdict,
human_judge=args.human_judge,
)
except KeyboardInterrupt:
# FIX-5-3: 断定形の「試行済み・追記済み」は虚偽になる経路がある
# (setup 前・SSD 未マウント等の早期 return では auto-HOME も台帳も
# 走らない). ここでは事実だけ伝え, 詳細は home_recovery / JSON を
# 参照するよう促す.
print("[LOOP] 試行中 Ctrl-C 受領. auto-HOME/台帳の実施状況は"
" log の 'home_recovery' セクションで確認のこと"
" (daemon 接続前 or SSD 未マウント等の早期中断では未実施).")
print("[LOOP] メニューに戻らずセッション終了 (§4 セマンティクス).")
break
print("[LOOP] === 試行 #{} 終了: success={} log={} ===".format(
_trial_counter, success, log_path))
continue
print("[LOOP] 未知のコマンド: {!r}. n / Enter / h / q のいずれか".format(_cmd))
except KeyboardInterrupt:
# FIX-5-3: 断定「HOME 位置のはず」をやめ, 目視/h コマンドを促す.
# 前試行が未完走 or HOME 未到達の場合, アームは中途姿勢 or 把持中の可能性あり.
print("\n[LOOP] プロンプト中 Ctrl-C 受領.")
print("[LOOP] 前試行が未完走または HOME 未到達の場合, 現在姿勢と把持状態を"
" 目視確認のこと. 必要なら h で HOME を試行してから次試行へ.")
print("[LOOP] 常駐モード終了. 実行試行数 = {}".format(_trial_counter))
if __name__ == "__main__":
main()
