mechArm 270 Pi 動的追従の限界を探る Log#43 | PLACEの位置は合った。だがアームの手首が捻れ、姿勢が深すぎた
PLACE の置き位置がずれる。白い箱を狙っているのに、ブロックが箱の縁の外へ落ちる。この一点を潰しにいったら、原因が一つ解けるたびに次の原因が顔を出す一日になった。円弧拘束、腕の捻り（j3）、グリッパーの捻り（j5）、そして関節の深さ。順に測り、順に直し、そのたびに次が出てきた。記録として、出てきた順に残す。
PLACE のずれを測る
まず測る。狙った箱の中心と、実際にブロックが落ちた位置の差を、実機ログから出すところから始めた。
PICK 側はすでに FK テーブル（85万点の全 6 軸姿勢を持つ対応表）を引いて降下している。stage4 の残差は 1mm 台で安定している。一方 PLACE の降下は、まだ古い方式（j0 を一軸だけ振って半径固定の弧で位置を合わせる方式）のままだった。この差がそのまま出た。
過去の失敗回 2 本の lookup 残差を並べる。X（左右）と Z（前後）の残差を mm で出した。
run_0001_20260629_163852:
stage8_place target [-148.6, 227.7] interp [-148.6, 264.2] z_resid 36.4mm
run_0001_20260629_164021:
stage7_approach_place target [-187.6, 157.3] interp [-187.6, 234.7] z_resid 77.4
stage8_place target [-187.6, 157.3] interp [-187.6, 240.9] z_resid 83.6X 残差はゼロ。Z 残差だけが常にプラス（奥）方向に出て、しかも箱が手前にあるほど残差が膨らむ。1 本目は 36.4mm、2 本目は 83.6mm。
なぜ X は合うのに Z だけずれるのか。PLACE の姿勢素材（20 点の教示姿勢）の中身を見た。20 点すべてで j1〜j5 が固定されていて、j0 だけが動いていた。j0 を振ると爪先は半径一定の円弧を描く。X と Z が円弧の上で連動する。X を合わせにいった瞬間、Z は円弧が通る値に固定される。X と Z を独立に狙えない。
これが PLACE のずれの正体だった。爪先オフセットでも、カメラの歪みでも、素材のレンジ不足でもない。「置き場所を j0 一軸で決めていて、爪先が半径固定の円弧の上しか動けない」という構造。箱がその円弧から手前に離れているほど、奥へずれて落とす。
FK 方式へ切り替える
PICK の descend はすでに FK テーブルを引いて、x-z を独立に狙えている（残差 1mm 台）。PLACE も同じ方式に揃える。円弧を捨てて、FK テーブルから箱位置に届く姿勢を直接引く。
コードの切り替え点は一箇所に集約されていた。phase がどの集合に入るかで FK か円弧かを分岐している。
FK_LOOKUP_PHASES = ("pick",)ここに place_withblock を足せば PLACE も FK 方式になる。ただし一行では済まない。FK テーブルは「爪先の高さ帯（ee_y）」でフィルタしてから x-z 最近傍を取る。PICK 帯は床すれすれの 10〜40mm。PLACE は箱の中にブロックを置くので、床ではなく箱の高さ基準の帯が要る。箱の物理高さは 43mm で固定。
まず箱を置く位置の周辺に、FK テーブルがどんな高さの点を持っているか測った。補正後の狙い（x=−0.010, z=+0.108）の近傍 30mm の点を出す。
box近傍30mm以内の点数: 86
ee_y mm min/median/max: -108.9 / -72.0 / 323.9
ee_y分布(mm): [-109, -108, -108, ... （ほぼ全部マイナス）]この箱位置の近傍点は ee_y がほぼ床下（−108mm 前後）だった。物理的にありえない高さの姿勢が混じっている。この位置は FK テーブルの PLACE 有効点が薄い。帯を広げても拾えるのは使えない姿勢になる。
そこで FK テーブル全体で、箱高さ帯（ee_y 35〜50mm）かつ送れる姿勢（j1≤88）が PLACE 側にどれだけあるかを測った。
ee_y 35-50mm かつ j1<=88 の点数: 8981
X範囲mm: -328 .. 305
Z範囲mm: 21 .. 339
うちX<0(PLACE側)の点数: 8507
PLACE側 X範囲mm: -328 .. -0
PLACE側 Z範囲mm: 21 .. 339PLACE 領域全域に 8,507 点。テーブルは十分カバーしている。out_of_range になったのは「箱位置がテーブルの薄いゾーンの端だった」だけで、テーブル不足ではない。箱をテーブルの濃いゾーンに置けば FK で PLACE が成立する。
濃いゾーンの箱位置で fk_lookup 単体を叩いた。
$ python3 fk_lookup.py --phase place_withblock --x -0.22 --z 0.19 --ee-y-min 0.033 --ee-y-max 0.055
method: fk_nearest
x_residual_m: 0.00067 (0.67mm)
z_residual_m: 0.00002 (0.02mm)
ee_y: 40.9mm
joint_limit_ok: true円弧時代に Z 残差 36〜84mm だったものが、FK で Z 残差 0.02mm。x-z を独立にほぼ完全に狙えている。measure で立てた仮説（円弧拘束が原因、FK で解ける）が裏取りできた。
ブロックを持って降りると高さが足りない
実機で通した。stage4（PICK）は FK で正確に降りる。だが stage8（PLACE）で爪が箱の縁に当たった。
stage8_place method=fk_nearest x_resid=0.53mm z_resid=0.24mm
j1=85.00 ← 前傾で低い姿勢。ee_y が低い点を拾ったee_y 帯を 33〜55mm（箱高さ 43mm を中心に取った仮値）にしていた。だが PLACE では爪がブロックを掴んで降りる。ブロックは爪先の下にぶら下がっている。爪先が 40mm だと、ブロックの下端は箱の縁（43mm）より下に来て、箱の壁に激突する。
爪先基準ではなく「ブロック下端が箱の縁を越える高さ」で帯を取り直す必要があった。爪先の ee_y は 43mm ＋ ブロックの爪下寸法 ＋ クリアランス。仮に 70〜100mm 帯で fk_lookup を叩いた。
$ python3 fk_lookup.py --phase place_withblock --x -0.218 --z 0.208 --ee-y-min 0.070 --ee-y-max 0.100
method: fk_nearest
x_resid=0.09mm z_resid=0.07mm
ee_y: 84.9mm ← 箱の縁43mmの約42mm上。ブロック下端が縁を越える
j1: 82.0
joint_limit_ok: true70〜100mm 帯で成立。ee_y 84.9mm はブロックを縁の上から箱の中へ落とす高さ。この帯に直して通したら、ブロックが箱に入った。
[RESULT] 完走
[LEDGER] append OK: (in_box_geom=True dist=39.0)
stage8_place method=fk_nearest x_resid=0.53mm z_resid=0.24mm arrived=True円弧時代は同じ箱位置で in_box=False だった。FK 化と帯の取り直しで、in_box=True・dist 39.0mm。ブロックが箱に入った。
補正記憶が置きズレを縮める
FK 化と同時に、補正記憶を PLACE にも回した。PICK では稼働している仕組みで、20mm 格子のセルごとに「狙いに対して実際どれだけずれたか」を貯め、次回その狙いを先に補正してから lookup を引く。
ただし補正記憶のセルキーは座標だけで作られていて、PICK と PLACE の区別がなかった。そのまま回すと pick の補正（空爪の爪先ずれ）と place の補正（ブロック保持の落下点ずれ）が同じセルに混ざる。物理的な意味が違うので混ぜたら壊れる。セルキーに phase の接頭辞を足して名前空間を分けた。
[CORR] pick_x180_z280 dx=+0.43 dz=-7.13 n=1
[CORR] place_x-220_z220 dx=+26.92 dz=+28.17 n=1pick_ と place_ で別空間に入る。1 本目は学習だけ（n=1）。2 本目で同じ place セルに当たると、学習した補正が適用される。
置きズレの実測を並べる。
1本目: in_box dist 39.0mm
2本目: in_box dist 27.8mm （補正適用後）補正 1 回で 39→28mm。位置ずれは学習で縮んだ。「電源が入って動いている間、失敗を学んで動きが良くなっていくか」という評価軸に、数字で答えが出た局面だった。
腕が捻れる（j3）
2 本目は in_box dist が縮んだのに、箱に入らなかった。爪が箱に当たった。approach_place から place への姿勢を並べる。
approach_place: [+43.52, 57.39, 17.49, +0.08, -89.20, -29.44]
place(stage8): [+38.00, 85.00, -27.00, -32.00, -75.00, -30.00]
j0 j1 j2 j3(-32°) j4 j5j3（腕の捻り。前腕が軸周りに回る）が +0.08° → −32.0°。approach_place から place は「アームを下げるだけ」で腕を捻る必要はないのに、32° 捻れている。前腕が捻れると、その先のグリッパーの向きごと回る。
原因は fk_lookup が x-z 2D 最近傍だけで 6 関節を選び、j3 を考慮しないこと。FK テーブルには j3 を捻った姿勢も含まれ、x-z 最近傍がたまたま捻れた点だった。
補正では直らない。補正が動かすのは狙いの x-z だけで、関節を直接いじらない。狙いを動かしても、動いた先の最近傍がまた j3 捻れ点なら、やはり捻れる。実際、2 本のデータで j3 は -27→-32 とむしろ悪化していた。位置と j3 は独立している。
j3 を縛る必要があった。FK テーブルで、箱位置に対し j3 許容幅を変えて最近傍を測った。
j3許容±5° : 候補623点 最近傍dist=5.2mm そのj3=-2.0°
j3許容±10° : 候補1142点 最近傍dist=5.2mm そのj3=-2.0°
j3許容±15° : 候補1489点 最近傍dist=4.6mm そのj3=-12.0°
j3許容±999° : 候補2606点 最近傍dist=0.7mm そのj3=-32.0°制約なしだと dist 0.7mm の点を選ぶが、その代償が j3=−32° の捻り。j3±5° に絞れば、j3=−2°（ほぼ捻らない）で dist 5.2mm。位置精度はわずかに落ちるが、5.2mm は補正が吸収できる範囲。腕の捻りが消えて位置も合う。
fk_lookup に j3 帯フィルタを足した。ee_y 帯フィルタと同じ構造で、候補マスクに「approach の j3 から ±5° 以内」を AND する。
if j3_ref is not None:
try:
j3_ref_f = float(j3_ref); j3_tol_f = float(j3_tol_deg)
except (TypeError, ValueError):
j3_ref_f = None
if j3_ref_f is not None:
mask_j3 = (joints[:, 3] >= j3_ref_f - j3_tol_f) & (joints[:, 3] <= j3_ref_f + j3_tol_f)
mask = mask & mask_j3j3_ref=None のとき従来と完全に同じ挙動になる。PICK は j3_ref を渡さないので回帰ゼロ。place_withblock 呼び出しで approach_place の実 j3（≈0）を渡す。実装は Claude Code、外部レビューを codex に通してから実機へ。
実機で j3 は −2.4° に収まった。approach から place で j3 がほぼ動かない。「下げるだけ・腕を捻らない」が実現した。
グリッパーが捻れる（j5）
j3 を直したら、次は j5 が捻れた。j3 が前腕の捻りだったのに対し、j5 はグリッパー自体の捻り。軸が違うので、j3 を縛っても j5 は素通りだった。
approach_place: j5=-29.44
stage8 place: j5= 0.00 ← 29.4°の捻りj3 と同じ穴だった。fk_lookup は j5 も見ていない。j5=0 の点（グリッパーの捻りが 30° 違う姿勢）を x-z 最近傍で拾った。爪の開閉方向が箱に対して回り、縁に当たる。
同じように測った。j3±5° に加えて j5 許容幅を変える。
j3±5 ∩ j5±5: 545点 dist=6.5mm j3=3.0 j5=-30.0
j3±5 ∩ j5±10: 545点 dist=6.5mm j3=3.0 j5=-30.0
j3±5 ∩ j5±15: 545点 dist=6.5mm j3=3.0 j5=-30.0
j3±5 ∩ j5±999: 601点 dist=5.6mm j3=-2.1 j5=0.0j5±5° で候補 545 点・dist 6.5mm・j5=−30.0（捻らない）。制約なしだと dist 5.6mm だが j5=0（30° 捻り）。位置精度はほぼ同じで捻りが消える。
j3 帯フィルタと完全に同型の j5 帯フィルタを足した。approach_place の j5（≈−29.44）を基準に ±5° で絞る。これも実装 → codex レビュー → 実機。実機で j5 は −30 に揃った。
置きズレを測るタイミングが間違っていた
補正記憶が学習に使う置きズレ Δ の測り方に、根本的な間違いがあった。これまで Δ は、ブロックを離してアームが退避した後の画像から測っていた。
爪を開く前（ブロックを落とす前）に画像を取得して位置を出さないとズレ位置はわからないよ。
退避後に測ると、「置いたズレ」に「離してから箱の縁で転がったズレ」が混ざる。補正の教師データが濁る。正しくは、PLACE 姿勢に到達して爪を開く前、ブロックを保持したままで撮影し、その位置と箱中心の差を測る。離す前ならブロックは爪に固定されて転がらない。上から撮っているのでブロックは写る。
撮影のタイミングだけを変えた。stage8 到達後・stage9（グリッパー open）の直前に 1 枚撮り、その離す前ブロック位置を補正学習の入力にする。物理姿勢・送角・成否判定・台帳の列は一切触らない。
実機で離す前撮影が動いた。
[CAP-VLM] place_pre 保存: ...place_pre.jpg
[PLACE_PRE] block xz_m=(-0.2181,+0.2442)ブロックを掴んだまま撮影し、検出も成立。転がりを含まない純粋な置きズレを測る基盤ができた。
姿勢が深すぎて届かない
j3・j5 を縛り、離す前 Δ の撮影も入れた状態で通した。すると今度は stage8 が未到達で止まった。
[LOOKUP] stage8_place method=fk_nearest x_resid=0.00215m z_resid=0.00862m joint_ok=True
[SEND] stage8_place: send_angles [ +43.00, +87.00, -10.00, +3.00, -101.00, -30.00]
[SEND] [TIMEOUT] 8.0s 内に未到達: max|Δ|=27.84° actual=[ +41.57, +59.50, +17.84, -0.43, -75.32, -29.26]
[SEND] リトライしません: pre→last max|Δ| 13.53° >= 1.0° (部分移動/ストール疑い)
[MOVE ] stage8_place arrived=False retried=False post_max|Δ|=27.840°
[ABORT ] stage=8 reason=到達失敗j5 は −30 に揃った（狙い通り）。j3 も +3° に収まった。だが j4=−101°・j1=87° という極端に深い姿勢が選ばれ、アームがそこまで曲がりきれず 27° 未到達で物理的に止まった。ブロックを離す前に止まったので、箱に入れられなかった。
構造が見えてきた。fk_lookup は「ee_y 帯 ∩ j3 帯 ∩ j5 帯の中で x-z 最近傍」を取るだけで、選ばれた姿勢が approach から無理なく届くか（6 軸の移動量が小さいか）は見ていない。捻り（j3＝腕・j5＝グリッパー）を縛っても、残った曲げの自由度（j1＝肩・j2＝肘・j4＝手首ピッチ）が極端な深さに振れる点を選んでしまう。
一つ縛ると次が振れる。今日はこの連鎖をそのまま踏んだ。円弧拘束を FK で解いたら腕の捻りが出て、j3 を縛ったらグリッパーの捻り（j5）が出て、j5 を縛ったら曲げの深さ（j1・j4）が出た。捻りと曲げは別の軸で、1 軸ずつ帯フィルタを足す対症療法では追いつかない。FK 点の選択に「approach からの姿勢連続性（6 軸の移動量最小）」を入れる、という根本の設計が次に要る。PLACE の descend は、次のセッションで再度そこから修正する。
実装コード全量
この日の PLACE 改修（FK 化・補正 pick/place 分離・j3 帯フィルタ・j5 帯フィルタ・置きズレ測定の離す前撮影）が入った run_pick_place.py の全量。後から本人や AI が見返して再現できるよう、省略せず全て残す。fk_lookup.py（j3/j5 帯フィルタの本体側）は別途追記する。
run_pick_place.py（全量）
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""run_pick_place.py — 通し Pick & Place 本体（j0 対応表方式・実装単位2）
赤ブロックを上カメラで検出 → j0 対応表で各 phase の j0 を引く → 単発 send_angles で
基準姿勢へ移動 → グリッパー閉/開 → 箱（DAY232 確定固定値）へ運ぶ。
段ごとに lookup 結果と移動結果を構造化ログに全量記録する。
設計の要点（codex/gemini レビュー反映版）:
- 流用: j0_lookup / j0_sweep_scan._send_and_wait_arrival / visual_servo_pick の検出と daemon I/F
- 姿勢: lookup の target_angles をそのまま送る。J5 を -30 へ強制上書きしない
- 送信: 単発 send_angles のみ（_send_and_wait_arrival 経由）。ストリーミングしない
- ガード: joint_limit_ok / method=interp / x_residual<=THRESH の3条件を送信前に検査
- z_residual: 工程跨ぎ問い合わせでは構造的に大きい（pick弧 vs 箱z）。情報ログのみで ABORT しない
- グリッパー: daemon_set_gripper の戻り値は判定に使わない。各操作後に sleep>=2s（DAY175 対策）
- 工程跨ぎ位置データ禁止: block_xz は pick 系のみ、BOX_XZ は place 系のみ
絶対ルール:
- send_coords() / release_all_servos() を呼ばない
- Stiffness/Damping を触らない
- fk_scan_motion_based_v2.csv を書き換えない
CLI:
python3 run_pick_place.py # 一本通し実機実行
python3 run_pick_place.py --dry-run # 送信せず lookup+ガード判定だけ出力
python3 run_pick_place.py --dry-run --dummy-block-x 0.05 --dummy-block-z 0.30
"""
from __future__ import print_function
import argparse
import csv
import json
import math
import os
import sys
import time
import types
from datetime import datetime
from pathlib import Path
# ---- パス解決 ----------------------------------------------------------------
_THIS = Path(__file__).resolve()
_PLACE_MODULES_DIR = _THIS.parent
_SCRIPTS_ROOT = _THIS.parents[3]
_DRAG_TEACHING_DIR = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "drag_teaching"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES_DIR))
sys.path.insert(0, str(_DRAG_TEACHING_DIR))
# ---- pymycobot スタブ（waypoint_replay 経由の import を Mac 上で通すため） --
if "pymycobot" not in sys.modules:
_pkg = types.ModuleType("pymycobot")
_mod = types.ModuleType("pymycobot.mycobot")
class _MyCobotStub(object):
def __init__(self, *args, **kwargs):
raise RuntimeError(
"run_pick_place.py: MyCobot stub is not callable. "
"アーム制御は daemon TCP 経由のみ。")
_mod.MyCobot = _MyCobotStub
sys.modules["pymycobot"] = _pkg
sys.modules["pymycobot.mycobot"] = _mod
# ---- 流用 import ------------------------------------------------------------
# 段3/4/7/8 の j0 決定（B 案 = x のみマッチ）
from j0_lookup import j0_lookup # noqa: E402 (全 stage で利用 / DAY257 復旧)
from fk_lookup import fk_lookup # noqa: E402 (将来の実験用に温存。現状未呼出)
# DAY257 後段: descend (stage4_pick) のみ fk_lookup に切替。
# DAY262: place_withblock も fk_lookup 化 (円弧 lookup では x-z が円弧上のみで Z を狙えない.
# in_box=False 2 本 (Z 残差 +36.4/+83.6mm・両方奥) の解決策として X-Z 独立化)。
# approach_pick / approach_place は j0_lookup 維持 (中間姿勢を壊さない)。
# fk_lookup は j1<=88 safety 復活版で、descend で z を動かせるようにする目的。
# OFF にする場合は () に戻すか、"pick" / "place_withblock" を抜く。
FK_LOOKUP_PHASES = ("pick", "place_withblock")
# DAY262: place_withblock 用の ee_y 帯 (PICK 用 10-40mm 床近く帯とは別).
# 箱高さ 43mm を中心に ±10mm 強. 仮値 (measure-first: 初回実機で out_of_range 頻度と
# 落下点を見て調整). 帯外 → fk_lookup が method="out_of_range" を返して _check_guard が
# ABORT する (送角しない). PICK 側 (EE_Y_MIN/MAX_DEFAULT) は fk_lookup.py 側で保持.
PLACE_EE_Y_MIN = 0.070 # m (33mm)
PLACE_EE_Y_MAX = 0.100 # m (55mm)
# 段1/3/4/6/7/8/10 の送信・到達確認（単発 send_angles + 沈黙リトライ最大1回）
from j0_sweep_scan import ( # noqa: E402
_send_and_wait_arrival,
_daemon_get_angles_filtered,
)
# 段2 の block+box 同時検出（detect_positions が内部で camera_resolver →
# cv2.VideoCapture → warm-up → setup_homography_and_mat_range → detect_red_block と
# 06f detect_white_box_once を一括で叩く。中央値集約 repeat 対応）
from detect_positions_once import detect_positions # noqa: E402
from visual_servo_pick import ( # noqa: E402
get_daemon_socket,
close_daemon_socket,
daemon_set_gripper,
daemon_set_gripper_value, # DAY261: close の push 保持用 (open は state(0) のまま)
daemon_set_fresh_mode,
detect_red_block,
pixel_to_unity,
)
# VLM 判定用のフレーム撮影と Anthropic API 呼び出し
import cv2 # noqa: E402
import numpy as np # noqa: E402
from camera_resolver import resolve_cameras # noqa: E402
from vlm_judge import judge as vlm_judge_call # noqa: E402
# 観測 emit（脇 UDP・fire-and-forget・例外は emit 側で握る）
# 観測モジュールが理由で制御が止まらないよう、import を guard する
try:
from observe.emit import emit as _observe_emit # noqa: E402
except Exception:
def _observe_emit(*_args, **_kwargs): # 観測無効でも制御は動く
pass
# ---- 定数（仕様準拠） -------------------------------------------------------
HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
BOX_XZ = (-0.010, 0.108) # DAY232 実測値。dry-run のデフォルト + 緊急時固定値として残置
# 実機 run では段2 で実検出した box_xz を使う（DAY236〜）
X_RESID_THRESH = 0.02 # m。これ超で ABORT
BOX_SCORE_THRESH = 0.5 # 06f のテンプレ score。これ未満は箱位置不確実として段2 ABORT
# 根拠: DAY230 実績 score 0.618、06f 既定 TEMPLATE_SCORE_THRESHOLD=0.5
GRIPPER_SLEEP = 2.0 # s。DAY175 対策（>=2s）
GRIP_VALUE = 20 # close の push 保持目標値（DAY261 tour 実測の暫定値）。
# 閉じ切り state(1) は「位置で止まり握り直さず落ちる」(DAY259)。
# set_gripper_value(20) で閉じ切らず押し込み力を残す。
# gv は 開=97 / 把持厚み相当≈36 / これより小さいほど押し込み深い。
# 効かなければこの値のみ振る（descend 把持は tour と条件が違う点に留意）。
STAGE9_RELEASE_PRE_SLEEP = 2.0 # s。stage9 release は直前 PLACE 後に一呼吸置く
STAGE9_RELEASE_OPEN_ATTEMPTS = 2 # open→sleep→open で stage9 の離しを確実化
TOL_APPROACH = 2.0 # approach 系
TOL_APPROACH_PLACE = 3.0 # stage7 approach_place 専用（大移動で最後の数度が埋まらない対策）
TOL_SWEPT_RETREAT = 3.0 # stage9b swept_retreat 専用（残差2.55°で未到達になる対策・他stage非共有）
TOL_LIFT_BACK = 3.0 # stage10 lift_back 専用。実測残差 2.46〜2.55°（j2主因）を吸収。
# stage9b TOL_SWEPT_RETREAT=3.0 と同形。他stageと非共有
TOL_PICK_PLACE = 3.0 # pick / place 系
TOL_HOME = 2.5 # HOME 復帰
SEND_WAIT_SEC = 1.0 # daemon 側送信直後の get_angles ログ遅延
DETECT_REPEAT = 3 # 段2 検出の中央値集約サンプル数
SWEPT_J0_FOR_PLACE_JUDGE = 85.0 # 段9b の退避 j0（approach_place 採用版スイープ端＝採用済み値）
MAX_PICK_ATTEMPTS = 3 # pick=no 掴み直しの最大試行回数（初回含む）
SWEPT_J0_FOR_PICK_RETREAT = 85.0 # 掴み直し前のカメラ退避 j0（approach_pick base で振る・place の退避と同形）
VLM_CAPTURE_WARMUP_FRAMES = 30 # VLM 撮影前の warm-up
VLM_CAPTURE_DROP_FRAMES = 5 # 撮影直前のバッファクリア
# JSON・画像の出力先（KIOXIA・1 run = 1 フォルダで集約）。
# LEDGER_DIR と同じドライブ。未マウント時の誤書き込みは run() 冒頭ガードで防ぐ。
LOG_ROOT = Path("/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place")
# ---- 試行台帳 CSV（KIOXIA SSD 専用・ローカルへフォールバックしない）-------
LEDGER_DIR = Path("/Volumes/KIOXIA/r2r2r-data/runs")
LEDGER_CSV_PATH = LEDGER_DIR / "pick_place_ledger_v3.csv"
# in_box 判定の閾値（mm）: 白箱半幅 40mm + ブロック半幅 12mm ≈ 52mm 目安.
# 実機を見て調整可能な 1 定数.
IN_BOX_TOL_MM = 52.0
# ---- 試行間補正記憶 (DAY257) ---------------------------------------------
# tip 測定で得た Δ(dx,dz) をブロック (x,z) のセル別に蓄え、次回 lookup 直前に
# block_xz から Δ を引いて狙いを補正する。EMA で 1 セル 1 値に集約。
# 記憶 JSON は KIOXIA SSD の diag/ 配下。実機ローカルには書き込まない。
CELL_MM = 20.0 # セル幅 (mm)
EMA_ALPHA = 0.5 # EMA 重み (新 : 旧)
MAX_CORRECTION_MM = 40.0 # 補正量の絶対値クランプ (mm)
CORR_MEM_PATH = Path(
"/Volumes/KIOXIA/r2r2r-data/diag/correction_memory.json")
USE_CORRECTION = True # False で補正完全無効 (回帰確認用)
# DAY257 後段: pick 判定の二重チェック閾値。VLM yes でも tip 計測の dist がこれを
# 超えていれば「掴めていない」として no に倒す (over-pickの誤学習を防ぐ)。
# 単位 mm。実機データを見て振る前提の可変定数。
PICK_DIST_MAX_MM = 30.0
LEDGER_COLS = [
"timestamp", "run_stem", "trial_no",
"pick_verdict", "pick_reason", "pick_confidence",
"place_verdict", "place_reason", "place_confidence",
"block_pick_x", "block_pick_z",
"block_place_x", "block_place_z",
"box_x", "box_z",
"in_box_geom", "in_box_dist_mm",
"final_j1", "final_j2", "final_j3",
"final_j4", "final_j5", "final_j6",
"gripper", "place_capture_path",
"tip_pick_x", "tip_pick_z",
"tip_block_dx_mm", "tip_block_dz_mm", "tip_block_dist_mm",
"run_dir",
"tip_capture_path", "pick_capture_path",
# v3 ヘッダ 34-36（align 系：本実装では出所なし・空欄固定）
"align_j0_final", "align_red_x_mm", "align_converged",
# v3 ヘッダ 37-43（pick で参照した姿勢と到達時の実測角）
"pick_pose_id",
"pick_used_j0", "pick_used_j1", "pick_used_j2",
"pick_used_j3", "pick_used_j4", "pick_used_j5",
# v3 ヘッダ 44-50（place_withblock で参照した姿勢と到達時の実測角）
"place_pose_id",
"place_used_j0", "place_used_j1", "place_used_j2",
"place_used_j3", "place_used_j4", "place_used_j5",
# DAY257 試行間補正の記録 (5 列追加・v3 ヘッダ末尾 51-55)
"corr_cell_key", # 例: "x180_z260"
"corr_applied_dx", # 適用した補正 mm (x)
"corr_applied_dz", # 適用した補正 mm (z)
"target_before", # 補正前 (block) [mm]: "(+196.1,+252.5)" 等
"target_after", # 補正後 (lookup へ渡した値) [mm]
# DAY257 後段: pick 判定の VLM / dist override 内訳 (2 列追加)
"vlm_pick_verdict", # VLM 単独判定 (override 前) "yes/no/unknown/..."
"pick_dist_override", # tip_dist 二重チェックで no に倒したか "True"/"False"/""
]
# 爪先(青シール)検出パラメータ（detect_blue_markers.py DAY168 のHSV閾値を流用）
# 座標変換は本線の pixel_to_unity に統一（このモジュール独自の homography は使わない）
TIP_BLUE_LOWER = (100, 100, 50)
TIP_BLUE_UPPER = (130, 255, 255)
TIP_MIN_AREA = 80
TIP_MAX_AREA = 8000
# ---- 観測 emit 用の状態（run() が更新、_do_move が読む）-----------------
# 状態は run() スコープ所有（_observe_phase / _observe_gripper ヘルパ経由で更新）。
# 内側関数（stage_lookup_and_move）には phase 更新を持たせない。
# emit は _do_move（移動完了時）に加え、非移動 stage（stage2/5/9/9b・regrasp 再検出）でも
# _observe_emit_now() で通知する。全 stage を履歴表示するため 1 サイト集約は意図的に解除済み。
_OBSERVE_STATE = {
"phase": None,
"phase_t0_ms": None,
"gripper": None,
"verdict": None,
"block_xz": None,
"box_xz": None,
"block_place_xz": None,
"tip_pick": None, # PICK到達時の爪先vs block差分（_append_ledger が読む）
"run_dir": None, # この run の出力フォルダ（KIOXIA）。画面のファイル情報表示用
"cap_tip": None, # stage4_pick_tip 画像パス（画面/CSV用）
"cap_pick": None, # pick 判定画像パス
"cap_place": None, # place 判定画像パス
# 非移動 stage の通知専用 emit が再利用する angles キャッシュ.
# 直近 _do_move の post_actual で更新する（新規 daemon I/O ゼロ）.
"last_angles": None,
"run_stem": None,
"task": "pick_and_place_with_j0_table",
}
def _observe_phase(phase_str):
"""run() からフェーズ遷移を記録する。phase_t0 も自動更新."""
_OBSERVE_STATE["phase"] = phase_str
_OBSERVE_STATE["phase_t0_ms"] = int(time.time() * 1000)
def _observe_gripper(state):
"""run() から gripper 状態を記録する（0=open / 1=close）."""
_OBSERVE_STATE["gripper"] = state
def _detect_block_in_place_img(image_path):
"""place_img（保存済み jpg）から赤ブロックの Unity 座標 [x, z] を検出する.
stage2 と同じ detect_red_block + pixel_to_unity を使う（同 homography）.
全例外を握り、失敗時 None を返す（制御を絶対止めない）.
"""
if image_path is None:
return None
try:
frame = cv2.imread(str(image_path))
if frame is None:
return None
pixel = detect_red_block(frame)
if pixel is None:
return None
cx, cy = pixel
ux, uz = pixel_to_unity(cx, cy)
return [float(ux), float(uz)]
except Exception:
return None
def _detect_blue_tips_px(frame):
"""フレームから青シールを検出し、面積上位3点のpixel重心 [(cx,cy),...] を返す.
変換はしない（pixel座標のみ）。3点未満なら None。全例外を握って None.
"""
try:
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
lower = np.array(TIP_BLUE_LOWER)
upper = np.array(TIP_BLUE_UPPER)
mask = cv2.inRange(hsv, lower, upper)
k = np.ones((5, 5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, k)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, k)
cnts, _ = cv2.findContours(
mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
pts = []
for c in cnts:
a = cv2.contourArea(c)
if a < TIP_MIN_AREA or a > TIP_MAX_AREA:
continue
m = cv2.moments(c)
if m["m00"] == 0:
continue
cx = m["m10"] / m["m00"]
cy = m["m01"] / m["m00"]
pts.append((cx, cy, a))
if len(pts) < 3:
return None
pts.sort(key=lambda t: t[2], reverse=True) # 面積降順
top3 = [(p[0], p[1]) for p in pts[:3]] # 上位3点（ノイズ除去）
return top3
except Exception:
return None
def _measure_tip_vs_block(image_path):
"""pick到達フレームから 爪先中点 と 赤block を同一frame・同一 pixel_to_unity
で検出し差分を返す. 失敗時 None（制御を絶対止めない）.
戻り値 dict: tip_x, tip_z, block_x, block_z（Unity m）, dx_mm, dz_mm, dist_mm
姿勢非依存の爪先判定: 上位3点のうち「互いに最も近い2点＝爪先」、
残り1点＝手首. これは横/上カメラやアーム向きに依存しない（DAY168 の
「Y下方2点」は姿勢依存なので採用しない）.
ガード: 爪先間距離 < 手首までの距離 が崩れたら（開きすぎ等で前提破綻）
差分を None 記録してそのまま grip へ進む（測定が動作に干渉しない）.
"""
if image_path is None:
return None
try:
frame = cv2.imread(str(image_path))
if frame is None:
return None
# --- 爪先（青シール上位3点 → 最近2点の中点）---
top3 = _detect_blue_tips_px(frame)
if top3 is None:
return None
import itertools
pairs = list(itertools.combinations(range(3), 2))
def _d(p, q):
return math.hypot(p[0] - q[0], p[1] - q[1])
# 最近ペア＝爪先
i, j = min(pairs, key=lambda ij: _d(top3[ij[0]], top3[ij[1]]))
tip1, tip2 = top3[i], top3[j]
wrist = top3[3 - i - j] # 0+1+2=3 → 残り1点
mid_px = ((tip1[0] + tip2[0]) / 2.0, (tip1[1] + tip2[1]) / 2.0)
tip_pair_d = _d(tip1, tip2)
wrist_d = _d(mid_px, wrist)
if not (tip_pair_d < wrist_d): # 前提破綻ガード
return None
# --- block（同フレーム・同 pixel_to_unity で再検出）---
red_px = detect_red_block(frame)
if red_px is None:
return None
tip_ux, tip_uz = pixel_to_unity(mid_px[0], mid_px[1])
blk_ux, blk_uz = pixel_to_unity(red_px[0], red_px[1])
dx_mm = (tip_ux - blk_ux) * 1000.0
dz_mm = (tip_uz - blk_uz) * 1000.0
dist_mm = math.hypot(dx_mm, dz_mm)
return {
"tip_x": float(tip_ux), "tip_z": float(tip_uz),
"block_x": float(blk_ux), "block_z": float(blk_uz),
"dx_mm": float(dx_mm), "dz_mm": float(dz_mm),
"dist_mm": float(dist_mm),
}
except Exception:
return None
def _observe_verdict(raw_verdict, confidence, reason):
"""run() から VLM 判定結果を記録する。
raw_verdict ∈ {'yes','no','unknown',...} を success ∈ {True,False,None} にマップ.
reward は判定器側で未算出のため None 固定（VLM-as-Reward 実装時に充填）.
"""
if raw_verdict == "yes":
success = True
elif raw_verdict == "no":
success = False
else:
success = None
_OBSERVE_STATE["verdict"] = {
"success": success,
"reward": None,
"reason": reason,
"confidence": confidence,
"raw_verdict": raw_verdict,
}
def _observe_emit_now():
"""非移動 stage 用の通知専用 emit. シリアル/daemon 読み取りを伴わない.
angles は _do_move でキャッシュ済みの last_angles をそのまま再利用（新規 I/O ゼロ）.
fire-and-forget. send_angles / グリッパー / sleep / 制御フローには一切触れない.
"""
# in_box は台帳と同じ _ledger_in_box_geometry を再利用（判定の二重実装を避ける）.
# 戻り値は ("True"/"False"/"", dist_str). bool に正規化し、欠落時は None.
# 同じ IN_BOX_TOL_MM・同じ関数を画面/台帳が共有するため値の食い違いが起きない.
_ibg, _ = _ledger_in_box_geometry(
_OBSERVE_STATE["block_place_xz"], _OBSERVE_STATE["box_xz"])
_in_box = (True if _ibg == "True" else False if _ibg == "False" else None)
_observe_emit(
angles=_OBSERVE_STATE["last_angles"],
phase=_OBSERVE_STATE["phase"],
phase_t0_ms=_OBSERVE_STATE["phase_t0_ms"],
gripper=_OBSERVE_STATE["gripper"],
trial=({"id": _OBSERVE_STATE["run_stem"],
"task": _OBSERVE_STATE["task"]}
if _OBSERVE_STATE["run_stem"] else None),
verdict=_OBSERVE_STATE["verdict"],
block_xz=_OBSERVE_STATE["block_xz"],
box_xz=_OBSERVE_STATE["box_xz"],
block_place_xz=_OBSERVE_STATE["block_place_xz"],
in_box=_in_box,
tip_pick=_OBSERVE_STATE["tip_pick"],
run_dir=_OBSERVE_STATE["run_dir"],
cap_tip=_OBSERVE_STATE["cap_tip"],
cap_pick=_OBSERVE_STATE["cap_pick"],
cap_place=_OBSERVE_STATE["cap_place"],
)
# ---- ヘルパ ------------------------------------------------------------------
def _next_log_path():
LOG_ROOT.mkdir(parents=True, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
for n in range(1, 10000):
stem = "run_{:04d}_{}".format(n, ts)
run_dir = LOG_ROOT / stem
if not run_dir.exists():
run_dir.mkdir(parents=True, exist_ok=True)
return run_dir / (stem + ".json")
raise RuntimeError("log seq 9999 超過")
def _atomic_write_json(path, data):
tmp = Path(str(path) + ".tmp")
with open(tmp, "w") as f:
json.dump(data, f, ensure_ascii=False, indent=2, default=str)
tmp.replace(path)
def _ensure_ssd_mounted():
"""KIOXIA SSD の LEDGER_DIR が存在・書込可なら True、それ以外 False.
fire-and-forget: 例外を握り False を返す."""
try:
if not LEDGER_DIR.exists():
return False
if not LEDGER_DIR.is_dir():
return False
# 書込テスト（write_test ファイルを作って即削除）
test = LEDGER_DIR / ".write_test"
try:
test.write_text("")
test.unlink()
return True
except Exception:
return False
except Exception:
return False
# ---- 試行間補正記憶 (DAY257) ---------------------------------------------
def _corr_load():
"""補正記憶 JSON をロード。失敗時は {} を返す（制御を絶対止めない）。"""
if not CORR_MEM_PATH.exists():
return {}
try:
with open(CORR_MEM_PATH, "r", encoding="utf-8") as f:
return json.load(f)
except Exception as e:
print("[CORR ] [WARN] 読み込み失敗 ({}). 空辞書で続行".format(e))
return {}
def _corr_save(mem):
"""補正記憶を atomic に保存。KIOXIA 未マウント時は False を返してスキップ。"""
if not _ensure_ssd_mounted():
print("[CORR ] KIOXIA 未マウント → 補正記憶の保存をスキップ")
return False
try:
CORR_MEM_PATH.parent.mkdir(parents=True, exist_ok=True)
tmp = CORR_MEM_PATH.with_suffix(CORR_MEM_PATH.suffix + ".tmp")
with open(tmp, "w", encoding="utf-8") as f:
json.dump(mem, f, ensure_ascii=False, indent=2)
os.replace(str(tmp), str(CORR_MEM_PATH))
return True
except Exception as e:
print("[CORR ] [WARN] 保存失敗: {}".format(e))
return False
def _corr_key(phase, x_m, z_m):
"""(phase, x, z)[m] → セルキー文字列 (例: 'pick_x180_z260' / 'place_x-40_z100')。
DAY262: phase プレフィックスを付けて pick/place の名前空間を分離
(同一 (x,z) セルでも掴み残差と置き残差は別物なので学習を汚染しない).
CELL_MM=20 で round(中心) して mm 化."""
cx = int(round(float(x_m) * 1000.0 / CELL_MM) * CELL_MM)
cz = int(round(float(z_m) * 1000.0 / CELL_MM) * CELL_MM)
return "{}_x{}_z{}".format(phase, cx, cz)
def _corr_get(mem, key):
"""セルキー → {'dx_mm','dz_mm','n'} or None"""
return mem.get(key)
def _corr_update(mem, key, dx_mm_new, dz_mm_new):
"""EMA で 1 セル 1 値に集約 (新規は値そのまま・n=1)。"""
e = mem.get(key)
# 破損ガード: 既存エントリが dict 以外 (str/list/null 等) なら捨てて新規扱いに
if e is not None and not isinstance(e, dict):
print("[CORR ] [WARN] mem[{}] が dict 以外 ({}). 破棄して再初期化"
.format(key, type(e).__name__))
e = None
if e is None:
mem[key] = {"dx_mm": float(dx_mm_new), "dz_mm": float(dz_mm_new), "n": 1}
else:
a = EMA_ALPHA
mem[key] = {
"dx_mm": a * float(dx_mm_new) + (1 - a) * float(e.get("dx_mm", 0.0)),
"dz_mm": a * float(dz_mm_new) + (1 - a) * float(e.get("dz_mm", 0.0)),
"n": int(e.get("n", 0)) + 1,
}
return mem[key]
def _apply_correction(phase, block_xz):
"""(phase, block_xz) から補正を適用し (target_xz, meta) を返す。
DAY262: phase 引数を追加 (pick/place の名前空間分離). PICK は phase="pick",
PLACE (stage8 place_withblock) は phase="place" で呼ぶ.
USE_CORRECTION=False / KIOXIA 未マウント / 記憶なし → 素通り (meta は空欄).
MAX_CORRECTION_MM で絶対値クランプ. 狙い = block - Δ.
Δ の符号: PICK は (爪先 - ブロック), PLACE は (落下点 - 箱). どちらも「実測 - 目標」."""
bx_m = float(block_xz[0]); bz_m = float(block_xz[1])
meta = {
"phase": phase,
"target_before_mm": (bx_m * 1000.0, bz_m * 1000.0),
"target_after_mm": (bx_m * 1000.0, bz_m * 1000.0),
"cell_key": "",
"dx_mm": None, "dz_mm": None, "n": 0,
"applied": False,
}
if not USE_CORRECTION:
return (bx_m, bz_m), meta
if not _ensure_ssd_mounted():
print("[CORR ] KIOXIA 未マウント → 補正スキップ (素通り)")
return (bx_m, bz_m), meta
key = _corr_key(phase, bx_m, bz_m)
meta["cell_key"] = key
mem = _corr_load()
entry = _corr_get(mem, key)
if entry is None:
return (bx_m, bz_m), meta
# 破損ガード: エントリが dict 以外 (str/list 等) は補正せず素通り。
# 次回の _corr_update が新規扱いで上書きするので、自動で正常状態に戻る。
if not isinstance(entry, dict):
print("[CORR ] [WARN] mem[{}] が dict 以外 ({}). 補正せず素通り"
.format(key, type(entry).__name__))
return (bx_m, bz_m), meta
dx_mm = float(entry.get("dx_mm", 0.0))
dz_mm = float(entry.get("dz_mm", 0.0))
n = int(entry.get("n", 0))
# MAX_CORRECTION_MM クランプ (記憶破損ガード)
dx_mm = max(-MAX_CORRECTION_MM, min(MAX_CORRECTION_MM, dx_mm))
dz_mm = max(-MAX_CORRECTION_MM, min(MAX_CORRECTION_MM, dz_mm))
# 狙い = block - Δ (Δ は 爪先 - ブロック の符号)
corrected_x_m = bx_m - dx_mm / 1000.0
corrected_z_m = bz_m - dz_mm / 1000.0
meta["target_after_mm"] = (corrected_x_m * 1000.0, corrected_z_m * 1000.0)
meta["dx_mm"] = dx_mm
meta["dz_mm"] = dz_mm
meta["n"] = n
meta["applied"] = True
print("[CORR ] key={} dx={:+.2f}mm dz={:+.2f}mm n={} "
"target ({:+.1f},{:+.1f}) -> ({:+.1f},{:+.1f}) [mm]".format(
key, dx_mm, dz_mm, n,
bx_m * 1000.0, bz_m * 1000.0,
corrected_x_m * 1000.0, corrected_z_m * 1000.0))
return (corrected_x_m, corrected_z_m), meta
def _ledger_extract_trial_no(run_stem):
"""run_stem ("run_0001_YYYYMMDD_HHMMSS" 想定) の先頭数字を int で返す。
取得失敗時は空文字列."""
try:
for part in str(run_stem).split("_"):
if part.isdigit():
return int(part)
except Exception:
pass
return ""
def _ledger_mm_or_blank(arr, idx):
"""[x, z] m リストから idx 要素を mm 化して文字列に。取れなければ空."""
if not isinstance(arr, (list, tuple)) or len(arr) <= idx:
return ""
v = arr[idx]
if not isinstance(v, (int, float)):
return ""
return "{:.1f}".format(v * 1000.0)
def _ledger_in_box_geometry(block_place_xz, box_xz):
"""block_place と box 中心の距離 mm + IN_BOX_TOL_MM 比較.
戻り値: (in_box_geom_str, dist_mm_str). どちらか欠落で ('','') を返す."""
if not (isinstance(block_place_xz, (list, tuple)) and len(block_place_xz) == 2
and isinstance(box_xz, (list, tuple)) and len(box_xz) == 2):
return "", ""
vals = list(block_place_xz) + list(box_xz)
if not all(isinstance(v, (int, float)) for v in vals):
return "", ""
dx_mm = (block_place_xz[0] - box_xz[0]) * 1000.0
dz_mm = (block_place_xz[1] - box_xz[1]) * 1000.0
dist = math.hypot(dx_mm, dz_mm)
return str(dist < IN_BOX_TOL_MM), "{:.1f}".format(dist)
def _append_ledger(log_record, run_stem):
"""試行 1 行を KIOXIA SSD の CSV 台帳へ追記.
fire-and-forget: 例外で制御を絶対止めない. ローカルへのフォールバック書き込みはしない.
既存ヘッダ無ければ新規作成、あれば append.
"""
try:
if not _ensure_ssd_mounted():
print("[LEDGER] KIOXIA SSD ({}) 未マウント or 書込不可 → CSV スキップ"
.format(LEDGER_DIR))
return
judgments = log_record.get("judgments") or {}
pick = judgments.get("pick") or {}
place = judgments.get("place") or {}
# block/box は _OBSERVE_STATE の最終値 (regrasp 反映済み・stage9b 検出反映済み)
block_xz = _OBSERVE_STATE.get("block_xz")
box_xz = _OBSERVE_STATE.get("box_xz")
block_place_xz = _OBSERVE_STATE.get("block_place_xz")
gripper = _OBSERVE_STATE.get("gripper")
tip_pick = _OBSERVE_STATE.get("tip_pick")
# 最後の move の post_actual = 試行終了時の関節角
final_angles = ["", "", "", "", "", ""]
for m in reversed(log_record.get("moves", []) or []):
pa = m.get("post_actual")
if isinstance(pa, (list, tuple)) and len(pa) == 6:
for i in range(6):
v = pa[i]
final_angles[i] = ("{:.2f}".format(v)
if isinstance(v, (int, float)) else "")
break
# place 撮影画像のフルパス
place_capture_path = ""
for c in reversed(log_record.get("vlm_captures", []) or []):
if c.get("label") == "place" and c.get("image"):
place_capture_path = str(c["image"])
break
in_box_geom, in_box_dist_mm = _ledger_in_box_geometry(
block_place_xz, box_xz)
row = {
"timestamp": log_record.get("ended_at") or datetime.now().isoformat(),
"run_stem": run_stem,
"trial_no": _ledger_extract_trial_no(run_stem),
"pick_verdict": (pick.get("verdict") or ""),
"pick_reason": (pick.get("reason") or ""),
"pick_confidence": (pick.get("confidence")
if isinstance(pick.get("confidence"), (int, float))
else ""),
"place_verdict": (place.get("verdict") or ""),
"place_reason": (place.get("reason") or ""),
"place_confidence": (place.get("confidence")
if isinstance(place.get("confidence"), (int, float))
else ""),
"block_pick_x": _ledger_mm_or_blank(block_xz, 0),
"block_pick_z": _ledger_mm_or_blank(block_xz, 1),
"block_place_x": _ledger_mm_or_blank(block_place_xz, 0),
"block_place_z": _ledger_mm_or_blank(block_place_xz, 1),
"box_x": _ledger_mm_or_blank(box_xz, 0),
"box_z": _ledger_mm_or_blank(box_xz, 1),
"in_box_geom": in_box_geom,
"in_box_dist_mm": in_box_dist_mm,
"final_j1": final_angles[0],
"final_j2": final_angles[1],
"final_j3": final_angles[2],
"final_j4": final_angles[3],
"final_j5": final_angles[4],
"final_j6": final_angles[5],
"gripper": ("" if gripper is None else str(gripper)),
"place_capture_path": place_capture_path,
"tip_pick_x": ("{:.1f}".format(tip_pick["tip_x"] * 1000.0)
if tip_pick else ""),
"tip_pick_z": ("{:.1f}".format(tip_pick["tip_z"] * 1000.0)
if tip_pick else ""),
"tip_block_dx_mm": ("{:.1f}".format(tip_pick["dx_mm"])
if tip_pick else ""),
"tip_block_dz_mm": ("{:.1f}".format(tip_pick["dz_mm"])
if tip_pick else ""),
"tip_block_dist_mm": ("{:.1f}".format(tip_pick["dist_mm"])
if tip_pick else ""),
"run_dir": (_OBSERVE_STATE.get("run_dir") or ""),
"tip_capture_path": (_OBSERVE_STATE.get("cap_tip") or ""),
"pick_capture_path": (_OBSERVE_STATE.get("cap_pick") or ""),
}
# v3 ヘッダ 34-50: align 系 3 列（本実装では出所なし・空欄固定）と
# pick/place の pose_id・到達時実測角 14 列。
pose_used = log_record.get("pose_used") or {}
pick_used = pose_used.get("pick_used") or [None] * 6
place_used = pose_used.get("place_used") or [None] * 6
def _fmt_j(v):
return ("{:.2f}".format(v) if isinstance(v, (int, float)) else "")
row.update({
"align_j0_final": "",
"align_red_x_mm": "",
"align_converged": "",
"pick_pose_id": (pose_used.get("pick_pose_id") or ""),
"pick_used_j0": _fmt_j(pick_used[0]),
"pick_used_j1": _fmt_j(pick_used[1]),
"pick_used_j2": _fmt_j(pick_used[2]),
"pick_used_j3": _fmt_j(pick_used[3]),
"pick_used_j4": _fmt_j(pick_used[4]),
"pick_used_j5": _fmt_j(pick_used[5]),
"place_pose_id": (pose_used.get("place_pose_id") or ""),
"place_used_j0": _fmt_j(place_used[0]),
"place_used_j1": _fmt_j(place_used[1]),
"place_used_j2": _fmt_j(place_used[2]),
"place_used_j3": _fmt_j(place_used[3]),
"place_used_j4": _fmt_j(place_used[4]),
"place_used_j5": _fmt_j(place_used[5]),
})
# DAY257 補正の記録 (5 列)
corr = (log_record.get("correction") or {}).get("last") or {}
def _fmt_xz(t):
if not (isinstance(t, (list, tuple)) and len(t) == 2):
return ""
return "({:+.1f},{:+.1f})".format(float(t[0]), float(t[1]))
row.update({
"corr_cell_key": corr.get("cell_key", ""),
"corr_applied_dx": ("{:.2f}".format(corr["dx_mm"])
if isinstance(corr.get("dx_mm"), (int, float))
else ""),
"corr_applied_dz": ("{:.2f}".format(corr["dz_mm"])
if isinstance(corr.get("dz_mm"), (int, float))
else ""),
"target_before": _fmt_xz(corr.get("target_before_mm")),
"target_after": _fmt_xz(corr.get("target_after_mm")),
})
# DAY257 後段: VLM/dist override の内訳 2 列。最終 pick_verdict は既存
# "pick_verdict" 列 (override 後の結論)。最新 (=最終) pick_attempt から拾う。
attempts = log_record.get("pick_attempts") or []
last_pick = attempts[-1] if attempts else {}
vlm_pv = last_pick.get("vlm_verdict") or ""
if "dist_override" in last_pick:
dist_ov = "True" if last_pick["dist_override"] else "False"
else:
dist_ov = ""
row.update({
"vlm_pick_verdict": vlm_pv,
"pick_dist_override": dist_ov,
})
is_new = not LEDGER_CSV_PATH.exists()
# codex 指摘ガード: 既存ヘッダと LEDGER_COLS が不一致なら追記しない。
# 不一致のまま append すると列ズレでデータが汚染されるため、ここで中断する
# (制御は止めず台帳記録だけスキップ)。
if not is_new:
try:
with open(LEDGER_CSV_PATH, "r", newline="") as fchk:
existing_header = next(csv.reader(fchk), None)
except Exception as e:
print("[LEDGER] [WARN] ヘッダ読込失敗 ({}). append スキップ"
.format(e))
return
if existing_header != LEDGER_COLS:
print("[LEDGER] [ABORT-APPEND] 既存ヘッダ ({} 列) と LEDGER_COLS "
"({} 列) が不一致 → append スキップ".format(
len(existing_header) if existing_header is not None else 0,
len(LEDGER_COLS)))
if existing_header is not None:
diff_extra = set(existing_header) - set(LEDGER_COLS)
diff_missing = set(LEDGER_COLS) - set(existing_header)
if diff_extra:
print("[LEDGER] 既存にのみ存在: {}".format(sorted(diff_extra)))
if diff_missing:
print("[LEDGER] LEDGER_COLS にのみ存在: {}".format(sorted(diff_missing)))
return
with open(LEDGER_CSV_PATH, "a", newline="") as f:
w = csv.DictWriter(f, fieldnames=LEDGER_COLS)
if is_new:
w.writeheader()
w.writerow(row)
print("[LEDGER] append OK: {} (in_box_geom={} dist={})".format(
LEDGER_CSV_PATH, in_box_geom, in_box_dist_mm))
except Exception as e:
print("[LEDGER] [WARN] append 例外（制御は継続）: {}".format(e))
def _max_delta(target, actual):
if target is None or actual is None:
return None
return max(abs(float(target[i]) - float(actual[i])) for i in range(6))
def _check_guard(r, label):
"""送信前ガード（レビュー反映版）。
1. joint_limit_ok == False → ABORT
2. method != "interp" → ABORT（clamp ＝ 弧 x 範囲外）
3. x_residual_m > X_RESID_THRESH → ABORT
4. z_residual_m → ログのみ（ここでは判定しない）
戻り値: (ok: bool, reason: str | None)
"""
if not r.get("joint_limit_ok"):
return False, "joint_limit違反: {}".format(r.get("joint_limit_violations"))
# 許可 method: "interp" (j0_lookup の補間成功) / "fk_nearest" (fk_lookup の帯内最近傍成功)
# 不許可: "clamp" (j0_lookup 弧端張り付き) / "out_of_range" (fk_lookup 帯外 or 距離超過)
if r.get("method") not in ("interp", "fk_nearest"):
return False, "method={}（弧の x 範囲外 / FK 帯外＝届かない位置）".format(
r.get("method"))
xr = r.get("x_residual_m")
if xr is None or xr > X_RESID_THRESH:
return False, "x_residual_m={} > {}".format(xr, X_RESID_THRESH)
return True, None
def _do_move(target_angles, label, tol, log_record, *, dry_run):
"""target_angles へ移動し、結果を log_record["moves"] に追加。
dry_run=True の場合は送信せず計画のみ記録。
戻り値: (success: bool, post_actual: list | None)
"""
pre_actual = None if dry_run else _daemon_get_angles_filtered()
pre_max = _max_delta(target_angles, pre_actual)
move_rec = {
"label": label,
"target_angles": list(target_angles),
"reach_tol_deg": tol,
"pre_actual": list(pre_actual) if pre_actual is not None else None,
"pre_max_delta_deg": pre_max,
"dry_run": dry_run,
}
if dry_run:
move_rec["arrived"] = None
move_rec["retried"] = None
move_rec["post_actual"] = None
move_rec["post_max_delta_deg"] = None
log_record.setdefault("moves", []).append(move_rec)
print("[MOVE ] {} dry_run（送信なし）".format(label))
return True, None
arrived, retried = _send_and_wait_arrival(
target_angles, label, tol, send_wait_sec=SEND_WAIT_SEC)
post_actual = _daemon_get_angles_filtered()
# 観測 emit（脇 UDP・fire-and-forget・例外は emit 側で握る）
# post_actual を再利用するので daemon I/O は増えない。print 出力にも影響しない
_observe_emit(
angles=post_actual,
phase=_OBSERVE_STATE["phase"],
phase_t0_ms=_OBSERVE_STATE["phase_t0_ms"],
gripper=_OBSERVE_STATE["gripper"],
trial=({"id": _OBSERVE_STATE["run_stem"],
"task": _OBSERVE_STATE["task"]}
if _OBSERVE_STATE["run_stem"] else None),
verdict=_OBSERVE_STATE["verdict"],
block_xz=_OBSERVE_STATE["block_xz"],
box_xz=_OBSERVE_STATE["box_xz"],
block_place_xz=_OBSERVE_STATE["block_place_xz"],
tip_pick=_OBSERVE_STATE["tip_pick"],
run_dir=_OBSERVE_STATE["run_dir"],
cap_tip=_OBSERVE_STATE["cap_tip"],
cap_pick=_OBSERVE_STATE["cap_pick"],
cap_place=_OBSERVE_STATE["cap_place"],
)
# 非移動 stage の通知 emit 用に最後の post_actual をキャッシュ（新規 I/O ゼロ）
_OBSERVE_STATE["last_angles"] = list(post_actual) if post_actual is not None else None
post_max = _max_delta(target_angles, post_actual)
move_rec["arrived"] = bool(arrived)
move_rec["retried"] = bool(retried)
move_rec["post_actual"] = list(post_actual) if post_actual is not None else None
move_rec["post_max_delta_deg"] = post_max
log_record.setdefault("moves", []).append(move_rec)
print("[MOVE ] {} arrived={} retried={} post_max|Δ|={}".format(
label, arrived, retried,
"{:.3f}°".format(post_max) if post_max is not None else "None"))
return arrived, post_actual
def _record_lookup(r, label, target_xz, log_record):
"""j0_lookup 結果を log_record["lookups"] に追加 + コンソールに出力."""
rec = {
"label": label,
"phase": r["phase"],
"source_file": r["source_file"],
"pose_id": r.get("pose_id"),
"target_xz_m": list(target_xz),
"j0": r["j0"],
"method": r["method"],
"x_residual_m": r["x_residual_m"],
"z_residual_m": r["z_residual_m"],
"interp_xz_m": list(r["interp_xz_m"]),
"joint_limit_ok": r["joint_limit_ok"],
"joint_limit_violations": r["joint_limit_violations"],
"target_angles": list(r["target_angles"]),
"base_angles": list(r["base_angles"]),
"nearest_pts": r["nearest_pts"],
}
if "interp_t" in r:
rec["interp_t"] = r["interp_t"]
rec["interp_seg_idx"] = r["interp_seg_idx"]
log_record.setdefault("lookups", []).append(rec)
print("[LOOKUP] {} phase={} target_xz=({:+.4f},{:+.4f}) → j0={:+.3f}° "
"method={} x_resid={:.5f}m z_resid={:.5f}m joint_ok={}".format(
label, r["phase"], target_xz[0], target_xz[1], r["j0"],
r["method"], r["x_residual_m"], r["z_residual_m"],
r["joint_limit_ok"]))
return rec
def _do_gripper(action, stage_label, log_record, dry_run):
"""グリッパー操作。戻り値（result=-1 含む）は DAY175 仕様で判定に使わない。
一方、daemon TCP 例外は本物の失敗として bool False を返す（close 未遂のまま
LIFT などの事故を防ぐ）。
DAY261: close は set_gripper_value(GRIP_VALUE) で push 保持に切替（位置で
止まらず押し込み力を残す）。open は従来どおり set_gripper_state(0)。
戻り値: True = 正常 or dry_run / False = daemon 例外（呼び出し側で ABORT 連動）
"""
state = 1 if action == "close" else 0
is_close = (action == "close")
print("[GRIP ] {} action={} method={} sleep={:.1f}s dry_run={}".format(
stage_label, action,
"value({})".format(GRIP_VALUE) if is_close else "state(0)",
GRIPPER_SLEEP, dry_run))
rec = {
"stage": stage_label, "action": action, "state": state,
"sleep_sec": GRIPPER_SLEEP, "dry_run": dry_run,
# DAY261: 把持方式の判別用。dry-run でも残るよう rec 構築時に書く。
"grip_method": "value" if is_close else "state",
}
if is_close:
rec["grip_value"] = GRIP_VALUE
ok = True
if not dry_run:
try:
if is_close:
rec["daemon_return"] = daemon_set_gripper_value(GRIP_VALUE)
else:
rec["daemon_return"] = daemon_set_gripper(state)
# 戻り値（result=-1 等）は判定に使わない（DAY175・成否無関係）
except Exception as e:
rec["error"] = str(e)
ok = False
print("[GRIP ] [ERROR] {}: daemon 例外: {}".format(stage_label, e))
time.sleep(GRIPPER_SLEEP)
log_record.setdefault("gripper", []).append(rec)
return ok
def _do_stage9_release_gripper(log_record, dry_run):
"""stage9 専用のグリッパー開放。
既知問題（戻り値が成否と無関係、連続コマンド無視、sleep 必須）に合わせ、
stage9 だけ open を二重送信する。把持 close 側や他 stage の open には触れない。
"""
print("[GRIP ] stage9_release action=open pre_sleep={:.1f}s "
"attempts={} sleep_each={:.1f}s dry_run={}".format(
STAGE9_RELEASE_PRE_SLEEP, STAGE9_RELEASE_OPEN_ATTEMPTS,
GRIPPER_SLEEP, dry_run))
rec = {
"stage": "stage9_release",
"action": "open",
"state": 0,
"pre_sleep_sec": STAGE9_RELEASE_PRE_SLEEP,
"sleep_sec": GRIPPER_SLEEP,
"attempt_count": STAGE9_RELEASE_OPEN_ATTEMPTS,
"dry_run": dry_run,
"attempts": [],
}
ok = True
if not dry_run:
time.sleep(STAGE9_RELEASE_PRE_SLEEP)
ok = False
for attempt in range(1, STAGE9_RELEASE_OPEN_ATTEMPTS + 1):
attempt_rec = {"attempt": attempt}
try:
attempt_rec["daemon_return"] = daemon_set_gripper(0)
ok = True
except Exception as e:
attempt_rec["error"] = str(e)
print("[GRIP ] [ERROR] stage9_release attempt {}: daemon 例外: {}"
.format(attempt, e))
rec["attempts"].append(attempt_rec)
time.sleep(GRIPPER_SLEEP)
log_record.setdefault("gripper", []).append(rec)
return ok
def _abort(log_record, stage, reason):
log_record["abort"] = {"stage": stage, "reason": reason}
print("[ABORT ] stage={} reason={}".format(stage, reason))
def _capture_for_vlm(label, frames_dir, log_record, dry_run, run_stem=None):
"""VLM 判定用に上カメラから 1 フレーム取得して frames_dir/{run_stem}_{label}.jpg に保存.
run_stem が None なら旧仕様 {label}.jpg にフォールバック（後方互換）。
dry_run の場合は撮影せず None を返す。
戻り値: 保存パス or None（取得失敗時）"""
cap_rec = {"label": label, "image": None, "dry_run": bool(dry_run)}
if dry_run:
log_record.setdefault("vlm_captures", []).append(cap_rec)
return None
try:
frames_dir.mkdir(parents=True, exist_ok=True)
except Exception as e:
print("[CAP-VLM] {} frames_dir 作成失敗: {}".format(label, e))
cap_rec["error"] = "mkdir:{}".format(type(e).__name__)
log_record.setdefault("vlm_captures", []).append(cap_rec)
return None
try:
cams = resolve_cameras(require_top=True, require_side=False)
except Exception as e:
print("[CAP-VLM] {} camera_resolver 例外: {}".format(label, e))
cap_rec["error"] = "resolver:{}".format(type(e).__name__)
log_record.setdefault("vlm_captures", []).append(cap_rec)
return None
top_idx = cams.get("top") if cams else None
if top_idx is None:
print("[CAP-VLM] {} 上カメラ未解決".format(label))
cap_rec["error"] = "no_top_camera"
log_record.setdefault("vlm_captures", []).append(cap_rec)
return None
cap = cv2.VideoCapture(top_idx)
if not cap.isOpened():
print("[CAP-VLM] {} cap open 失敗 (index={})".format(label, top_idx))
cap_rec["error"] = "cap_open"
log_record.setdefault("vlm_captures", []).append(cap_rec)
return None
out_path = None
try:
for _ in range(VLM_CAPTURE_WARMUP_FRAMES):
cap.read()
for _ in range(VLM_CAPTURE_DROP_FRAMES):
cap.read()
ret, frame = cap.read()
if not ret or frame is None:
print("[CAP-VLM] {} フレーム取得失敗".format(label))
cap_rec["error"] = "read_fail"
return None
fn = ("{}_{}.jpg".format(run_stem, label)
if run_stem else "{}.jpg".format(label))
out_path = frames_dir / fn
if not cv2.imwrite(str(out_path), frame):
print("[CAP-VLM] {} 保存失敗: {}".format(label, out_path))
cap_rec["error"] = "imwrite_fail"
out_path = None
return None
cap_rec["image"] = str(out_path)
cap_rec["top_camera_index"] = int(top_idx)
print("[CAP-VLM] {} 保存: {}".format(label, out_path))
finally:
try:
cap.release()
except Exception:
pass
log_record.setdefault("vlm_captures", []).append(cap_rec)
return out_path
def _ask_human_verdict(image_path, vlm_judgment, kind):
"""人による目視判定を取得（pick/place 共通）。
画像パスと VLM 判定を並べて表示し、y/n/u を受ける。
戻り値: "yes" / "no" / "unknown" のいずれか（不正入力は再入力ループ）。
"""
question = "掴めている?" if kind == "pick" else "箱に入った?"
print()
print("=" * 70)
print("[HUMAN ] 目視判定をお願いします（kind={}）".format(kind))
print("=" * 70)
print(" 画像: {}".format(image_path))
print(" VLM : verdict={} confidence={} reason={}".format(
vlm_judgment.get("verdict"),
vlm_judgment.get("confidence"),
vlm_judgment.get("reason")))
while True:
try:
ans = input("{} [y/n/u(unknown)] > ".format(question)).strip().lower()
except EOFError:
print(" stdin EOF → unknown 扱いで進む")
return "unknown"
if ans in ("y", "yes"):
return "yes"
if ans in ("n", "no"):
return "no"
if ans in ("u", "unknown"):
return "unknown"
print(" 入力が無効。y / n / u のいずれかを入力してください")
def _judge_image(image_path, kind, dry_run, dummy_verdict):
"""VLM 判定を呼び出す薄ラッパー。dry_run / image 無し は unknown ダミーを返す.
image_path: str | Path (1 画像) or list/tuple (複数画像。pick の 2 枚比較で利用)。
戻り値の dict には "image" キー（参照画像パス・複数なら最後の 1 枚）を追加して返す."""
if dry_run:
verdict = dummy_verdict if dummy_verdict else "unknown"
return {
"verdict": verdict, "confidence": 0.0,
"reason": "dry_run", "model": None,
"image": None, "raw": None,
}
# 画像参照の正規化 (str/list/tuple 受け入れ)
if isinstance(image_path, (list, tuple)):
if not image_path or any(p is None for p in image_path):
return {
"verdict": "unknown", "confidence": 0.0,
"reason": "no_image", "model": None,
"image": None, "raw": None,
}
paths_for_api = [str(p) for p in image_path]
last_path = paths_for_api[-1]
else:
if image_path is None:
return {
"verdict": "unknown", "confidence": 0.0,
"reason": "no_image", "model": None,
"image": None, "raw": None,
}
paths_for_api = str(image_path)
last_path = str(image_path)
try:
r = vlm_judge_call(paths_for_api, kind)
except Exception as e:
# vlm_judge 側で例外は吸収しているはずだが念のため二重に
return {
"verdict": "unknown", "confidence": 0.0,
"reason": "judge_exception:{}".format(type(e).__name__),
"model": None, "image": last_path, "raw": None,
}
r["image"] = last_path
return r
# ---- 段ごと処理 -------------------------------------------------------------
def stage1_home_and_open(log_record, dry_run):
"""段1: HOME 復帰 + グリッパー開."""
print("[STAGE 1] HOME 復帰 + グリッパー開")
log_record["stages"].append("stage1")
ok, _ = _do_move(HOME, "stage1_to_home", TOL_HOME, log_record, dry_run=dry_run)
if not ok:
_abort(log_record, 1, "HOME 到達失敗")
return False
if not _do_gripper("open", "stage1_grip_open", log_record, dry_run):
_abort(log_record, 1, "グリッパー開 失敗")
return False
return True
def stage2_detect_block_and_box(log_record, dry_run, dummy_block_xz, dummy_box_xz,
require_box=True):
"""段2: 赤ブロック + 白箱の **同時検出**（detect_positions(repeat=DETECT_REPEAT)）.
- dry_run: カメラに触れず、block/box ともダミー値（指定なければ block=(0.05,0.30) /
box=BOX_XZ デフォルト）を返す（require_box は無視）
- 実機: detect_positions が camera_resolver → cap → warm-up → homography →
detect_red_block と 06f detect_white_box_once をワンショットで叩き、xz 中央値で集約
- ガード:
block_xz=None → 常に ABORT（赤ブロック未検出）
require_box=True 時:
box_xz=None → ABORT（白箱未検出）
box_score<THRESH → ABORT（低スコア箱位置で place に降りない）
require_box=False 時:
box の 2 ガードはスキップ。box は取れていれば値、取れなければ None を返す
（掴み直しの再検出では block だけ要るため。box は段2 初回の値を保持する想定）
戻り値: (block_xz: tuple|None, box_xz: tuple|None)
"""
print("[STAGE 2] 赤ブロック + 白箱 同時検出（block と box は同回・同 homography）")
log_record["stages"].append("stage2")
if dry_run:
block_xz = (tuple(dummy_block_xz) if dummy_block_xz is not None
else (0.05, 0.30))
box_xz = (tuple(dummy_box_xz) if dummy_box_xz is not None
else tuple(BOX_XZ))
log_record.setdefault("detect", []).append({
"stage": 2, "source": "dry_run_dummy",
"block_xz_m": list(block_xz),
"box_xz_m": list(box_xz),
"block_score": None,
"box_score": None,
"n_captures": 0,
})
print("[DETECT] dry_run dummy block_xz=({:+.4f},{:+.4f}) "
"box_xz=({:+.4f},{:+.4f})".format(
block_xz[0], block_xz[1], box_xz[0], box_xz[1]))
return block_xz, box_xz
# 実機検出（detect_positions が内部で全部やる）
try:
r = detect_positions(repeat=DETECT_REPEAT)
except Exception as e:
_abort(log_record, 2, "detect_positions 例外: {}".format(e))
return None, None
block = r.get("block_xz_m")
box = r.get("box_xz_m")
scores = r.get("detect_score") or {}
block_score = scores.get("block")
box_score = scores.get("box")
rec = {
"stage": 2, "source": "live",
"block_xz_m": list(block) if block else None,
"box_xz_m": list(box) if box else None,
"block_score": block_score,
"box_score": box_score,
"n_captures": r.get("n_captures"),
"top_camera_index": r.get("resolved_top_index"),
}
log_record.setdefault("detect", []).append(rec)
print("[DETECT] block_xz={} block_score={}".format(block, block_score))
print("[DETECT] box_xz={} box_score={}".format(box, box_score))
# 段2 ガード（block は常に必須、box は require_box=True 時のみ必須）
if block is None:
_abort(log_record, 2, "赤ブロック未検出")
return None, None
if require_box:
if box is None:
_abort(log_record, 2, "白箱未検出")
return None, None
if box_score is None or box_score < BOX_SCORE_THRESH:
_abort(log_record, 2,
"white_box score {} < {}（低スコアの箱位置で place に降りない）".format(
box_score, BOX_SCORE_THRESH))
return None, None
return (float(block[0]), float(block[1])), (float(box[0]), float(box[1]))
# require_box=False: block のみ必須。box は取れていれば値・無ければ None
block_t = (float(block[0]), float(block[1]))
box_t = (float(box[0]), float(box[1])) if box is not None else None
return block_t, box_t
def stage_lookup_and_move(phase, target_xz, label, tol, log_record, dry_run, stage_no,
j3_ref=None, j5_ref=None):
"""lookup → 送信前ガード → 移動。共通段（段3/4/7/8）.
DAY262: j3_ref / j5_ref を受け place_withblock のときのみ fk_lookup へ転送
(j3=手首捻り抑制・j5=grip-axis 保持). 他の phase (pick / approach_*) では使わない
(PICK 回帰ゼロ担保).
戻り値: (success: bool, lookup_result: dict | None)
"""
log_record["stages"].append(label)
# pick descend + DAY262 place_withblock は FK テーブル全引き、
# 中間姿勢 (approach_pick / approach_place) は j0_lookup。
use_fk = phase in FK_LOOKUP_PHASES
lookup_name = "fk_lookup" if use_fk else "j0_lookup"
try:
if use_fk:
# DAY262: place_withblock だけは箱高さに合わせた ee_y 帯を渡し、
# さらに j3_ref / j5_ref (approach_place の j3/j5) 帯で
# 手首捻り + grip-axis を抑える.
# pick は fk_lookup.py 側の EE_Y_MIN/MAX_DEFAULT (10-40mm) を使い、
# j3_ref / j5_ref も渡さない (=None 既定・従来コードと bit-perfect 一致).
if phase == "place_withblock":
r = fk_lookup(phase, target_xz,
ee_y_min=PLACE_EE_Y_MIN,
ee_y_max=PLACE_EE_Y_MAX,
j3_ref=j3_ref,
j5_ref=j5_ref)
else:
r = fk_lookup(phase, target_xz)
else:
r = j0_lookup(phase, target_xz)
except (ValueError, FileNotFoundError, RuntimeError) as e:
_abort(log_record, stage_no, "{} 失敗: {}".format(lookup_name, e))
return False, None
rec = _record_lookup(r, label, target_xz, log_record)
ok_guard, reason = _check_guard(r, label)
rec["guard_pass"] = bool(ok_guard)
rec["guard_reason"] = reason
if not ok_guard:
_abort(log_record, stage_no, "送信前ガード NG: " + reason)
return False, r
# z_residual はログのみ（ABORT しない）
if r["z_residual_m"] > 0.05:
print("[INFO ] {} z_residual_m={:.4f}m （情報のみ・ABORT しない。"
"工程跨ぎや弧高さ差で構造的に大きいことがある）".format(
label, r["z_residual_m"]))
ok_move, _ = _do_move(
r["target_angles"], label, tol, log_record, dry_run=dry_run)
if not ok_move:
_abort(log_record, stage_no, "到達失敗")
return False, r
return True, r
# ---- メイン --------------------------------------------------------------
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"] = 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
_OBSERVE_STATE["last_angles"] = 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)
# 実機モードのみ daemon 接続 + fresh_mode=1
if not dry_run:
try:
get_daemon_socket()
except Exception as e:
_abort(log_record, 0, "daemon ping 失敗: {}".format(e))
_atomic_write_json(log_path, log_record)
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))
success = False
user_interrupted = False
try:
# 段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()
# 段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
# 段3 APPROACH_PICK
_observe_phase("stage3_approach_pick" + suf)
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
_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)
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)
# 段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_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)
if not _do_gripper(
"close", "stage5_grip_close" + suf, log_record, dry_run):
_abort(log_record, 5, "グリッパー閉 失敗")
return False, log_path
_observe_gripper(1)
# 非移動 stage の通知 emit（移動なし→画面に stage5 行を出す）
_observe_emit_now()
# 段6 LIFT（その試行の approach_pick target_angles へ戻る）
_observe_phase("stage6" + suf)
log_record["stages"].append("stage6" + suf)
ok, _ = _do_move(
approach_pick_target, "stage6_lift" + suf, TOL_APPROACH,
log_record, dry_run=dry_run)
if not ok:
_abort(log_record, 6, "LIFT 到達失敗")
return False, log_path
# 判定①: 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_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")
# tip_dist 二重チェック: VLM yes でも dist > PICK_DIST_MAX_MM なら no に倒す。
# tip 測定失敗 (tip_diff is None) のときは override 判定保留 (False)。
dist_override = False
final_verdict = vlm_verdict
if (vlm_verdict == "yes" and tip_diff is not None
and tip_diff.get("dist_mm") is not None
and float(tip_diff["dist_mm"]) > PICK_DIST_MAX_MM):
final_verdict = "no"
dist_override = True
print("[OVERRIDE] vlm=yes だが tip_dist={:.1f}mm > {}mm "
"→ pick_verdict=no に倒す".format(
tip_diff["dist_mm"], PICK_DIST_MAX_MM))
adopted = final_verdict # 補正記憶の判定はこの final を見る
_observe_verdict(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,
"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 測定失敗 のΔは
# 記憶に入れない (掴めていない試行で学習を汚さない)。
# 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):
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 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):
_abort(log_record, "regrasp_open",
"掴み直し グリッパー open 失敗")
return False, log_path
_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()
# ループ次イテレーション
# 既存キー互換: judgments["pick"] は最終試行の判定で埋める
log_record["judgments"]["pick"] = final_pick_judgment
# pick 失敗時は place を行わず HOME へ
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),
}
print("[RESULT] pick 失敗（{}回試行）。place をスキップして HOME 復帰"
.format(log_record["pick_failed"]["attempts"]))
_observe_phase("pickfail_to_home")
log_record["stages"].append("pickfail_to_home")
ok_home, _ = _do_move(
HOME, "pickfail_to_home", TOL_HOME,
log_record, dry_run=dry_run)
if not ok_home:
# HOME 沈黙/未到達 → アームが中途姿勢で止まっている可能性。
# 「pick 失敗（HOME 戻り済）」と「pick 失敗かつ HOME にも戻れていない」を分ける。
_abort(log_record, "pickfail_to_home",
"pick 失敗後の HOME 復帰に失敗（アームが中途姿勢の可能性）")
log_record["result"] = "pick_failed_home_failed"
return False, log_path
log_record["result"] = "pick_failed"
return False, log_path
# 段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"])
# 段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)
log_record.setdefault("correction", {})["place_last"] = place_corr_meta
# DAY262: 手首捻り抑制 (j3) + grip-axis 保持 (j5).
# approach_place の j3 (実測 ≈0) と j5 (実測 ≈-89) を基準に
# ±j*_tol_deg (fk_lookup 既定 5.0°) で place 候補を絞る.
# approach_place_target[i] が万一 float 化できなくても None フォールバック
# (fk_lookup 側の型ガードでも二重に守るが、呼び出し側でも明示的に落とす).
try:
_j3_ref_place = float(approach_place_target[3])
except (TypeError, ValueError, IndexError):
_j3_ref_place = None
try:
_j5_ref_place = float(approach_place_target[5])
except (TypeError, ValueError, IndexError):
_j5_ref_place = None
_observe_phase("stage8_place")
ok, r_place = stage_lookup_and_move(
"place_withblock", target_xz_place, "stage8_place",
TOL_PICK_PLACE, log_record, dry_run, 8,
j3_ref=_j3_ref_place, j5_ref=_j5_ref_place)
if not ok:
return False, log_path
# 台帳の 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)
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")
if not _do_stage9_release_gripper(log_record, dry_run):
_abort(log_record, 9, "グリッパー離す 失敗")
return False, log_path
_observe_gripper(0)
# 非移動 stage の通知 emit（移動なし→画面に stage9 行を出す）
_observe_emit_now()
# 判定②: 段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("unknown", 0.0, "swept_retreat_failed")
# 非移動 stage の通知 emit（stage9b 失敗ブランチ→画面に verdict=unknown 行）
_observe_emit_now()
print("[JUDGE ] place: swept_retreat 失敗のため判定スキップ。"
"HOME 復帰へ続行")
else:
place_img = _capture_for_vlm(
"place", frames_dir, log_record, dry_run, run_stem=run_stem)
_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)
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_adopted, place_vlm["confidence"], place_vlm["reason"])
# 非移動 stage の通知 emit（stage9b 成功ブランチ→画面に verdict 行）
_observe_emit_now()
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"]))
# DAY262 (仕様1): place 補正記憶の更新.
# 学習入力は「離す前」検出 (block_place_pre_xz・stage9 open 前に撮影済み).
# 退避後 _OBSERVE_STATE["block_place_xz"] は observe 表示・台帳 block_place_x/z
# で引き続き使うが、学習には転がりズレが混じるので使わない.
# VLM 判定 yes かつ block_place_pre_xz / box_xz が両方揃うときのみ更新.
# どちらか None (検出失敗 / detect_positions 未取得) は PICK 側 tip_diff None と
# 同型で学習スキップ. Δ = (離す前 - 箱) = 「実測 - 目標」・PICK と符号整合.
# KIOXIA 未マウント時は _corr_save がスキップし自動素通り.
if (USE_CORRECTION and place_adopted == "yes"
and isinstance(block_place_pre_xz, (list, tuple))
and len(block_place_pre_xz) == 2
and isinstance(box_xz, (list, tuple))
and len(box_xz) == 2
and block_place_pre_xz[0] is not None
and block_place_pre_xz[1] is not None):
dx_mm_place = (float(block_place_pre_xz[0]) - float(box_xz[0])) * 1000.0
dz_mm_place = (float(block_place_pre_xz[1]) - float(box_xz[1])) * 1000.0
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 α={}・place VLM yes・離す前 Δ で更新)".format(
key_place, e_new_place["dx_mm"],
e_new_place["dz_mm"], e_new_place["n"],
EMA_ALPHA))
# 段10 LIFT（approach_place へ戻る）→ HOME
_observe_phase("stage10")
log_record["stages"].append("stage10")
ok, _ = _do_move(
approach_place_target, "stage10_lift_back", TOL_LIFT_BACK,
log_record, dry_run=dry_run)
if not ok:
_abort(log_record, 10, "lift_back 到達失敗")
return False, log_path
ok, _ = _do_move(
HOME, "stage10_to_home", TOL_HOME,
log_record, dry_run=dry_run)
if not ok:
_abort(log_record, 10, "HOME 復帰失敗")
return False, log_path
log_record["result"] = "completed"
success = True
print("[RESULT] 完走")
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 検出。ログを保存して終了")
return False, log_path
finally:
# 異常終了時の HOME 復帰（中途姿勢の解消）。
# 除外: 正常完走 / pick_failed 系（HOME 復帰試行済み）/ Ctrl-C（意図的停止に大移動を被せない）/ dry_run
# 注意: HOME 復帰は daemon が使えるうちに（daemon_set_fresh_mode(0)/close より先に）走らせる
skip_recover = (
success
or user_interrupted
or dry_run
or log_record.get("result") in ("pick_failed", "pick_failed_home_failed")
)
if not skip_recover:
_observe_phase("abort_recover_home")
log_record["stages"].append("abort_recover_home")
try:
ok_h, _ = _do_move(
HOME, "abort_recover_home", TOL_HOME,
log_record, dry_run=dry_run)
log_record["home_recovery"] = {"attempted": True, "ok": bool(ok_h)}
print("[RECOVER] ABORT 後 HOME 復帰: {}".format(
"OK" if ok_h else "失敗（中途姿勢の可能性・リトライしない）"))
except Exception as e:
# 例外は握りつぶしてログ保存まで必ず到達させる（リトライしない）
log_record["home_recovery"] = {
"attempted": True, "ok": False, "error": str(e)}
print("[RECOVER] HOME 復帰中に例外: {}".format(e))
log_record["ended_at"] = datetime.now().isoformat()
if not success and "result" not in log_record:
log_record["result"] = "aborted"
if not dry_run:
try:
daemon_set_fresh_mode(0)
except Exception:
pass
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 を清浄に保つ）.
if not dry_run:
_append_ledger(log_record, run_stem)
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))
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,
)
sys.exit(0 if success else 1)
if __name__ == "__main__":
main()
