mechArm 270 Pi 動的追従の限界を探る Log#50 | PLACE が送信前ガードで止まる。FKテーブルの穴を埋めて実機で置ききる
視覚サーボ Pick & Place の PLACE 工程が、実機で送信前ガードに撥ねられて止まった。関節角を実機へ送る直前、ガードは「その狙いはマトモな位置か」を検査する。撥ねられた狙いは、FKテーブル上の最近傍点まで 21.9mm 離れていた。閾値は 20mm。わずか 1.9mm の超過で PLACE は中断し、ブロックを持ったまま HOME へ戻る。
ここで取れる手は二つある。ガードの閾値を緩めて 21.9mm を通すか、閾値はそのままに、止まる原因を消すか。前者は一行で終わる。だが閾値を 22mm に上げるとは、狙いから 21.9mm ズレた場所へ実機の手首を突っ込ませることを許すことでもある。置き位置が 2cm 狂う。後者は、FKテーブルにその狙いの近くの点が薄いという事実そのものに手を入れる。時間はかかる。撥ねられた原因は、ガードではなく、テーブルの穴だった。
この記録は、その穴を三つ埋めた一連の作業と、埋めた先で PLACE が実機で最後まで動ききり、白箱への接触を記録するところまでを残す。閾値は一度も緩めていない。
送信前ガードで止まる、その二種類
PLACE が止まる前に、PICK 側でも別のガード停止が出ていた。まず区別する必要がある。送信前ガード _check_guard() は三段で検査する。関節リミット（joint_limit_ok）、method のホワイトリスト、そして最近傍点への残差（x_residual_m）。この順で、どれか一つでも落ちれば送信を止める。
一つ目の停止は、ホワイトリスト漏れによる誤検知だった。PLACE の候補選択に j4 優先の層（fk_j4_priority）を入れたとき、_check_guard() のホワイトリスト ("interp", "fk_nearest") に新しい method 名を追加し忘れた。結果、joint_ok=True・残差 1mm 以下の妥当な狙いでも、method 名がリストにないというだけで撥ねられた。これはバグで、ホワイトリストに二つの method 名を足して直した。同時に、撥ねた理由の文言も実態に合わせた。旧文言「弧の x 範囲外 / FK 帯外＝届かない位置」は、実際には範囲検査を一切していないのに範囲外だと言っており、原因調査を一度ミスリードした。
二つ目の停止が、今回の PLACE のものだ。こちらは method のホワイトリストを通過している。撥ねているのは三段目の残差検査、x_residual_m=0.0219m > X_RESID_THRESH (0.02)。これはガードの本来の仕事で、狙いから遠い点への送信を止めている。バグではない。
二つを分けて記録する意味は大きい。一つ目はコードの欠陥だから直す。二つ目はテーブルの被覆の問題だから、直すのはコードではなくデータ側になる。同じ「ガードで止まる」でも、対処する場所が違う。
ガードを緩めるとは何を許すことか
x_residual_m は、狙いの xz 位置と、FKテーブルが返した最近傍点の xz 位置との距離だ。PLACE の狙いは (-0.165, +0.176)m。ここに高さ補正が効いて、目標の ee_y を 0.116m まで上げていた。fk_j4_priority はこの高い ee_y 帯で候補を選ぶが、その帯にこの位置の点が薄く、最近傍が 21.9mm 離れていた。
閾値 20mm は、置き位置の許容誤差そのものだ。最近傍点が 21.9mm 離れているということは、その点の関節角で実機を動かすと、狙った場所から 21.9mm ズレた位置に手首が来る。閾値を上げて通せば、PLACE の精度がその分だけ狂う。白箱への出し入れは数 cm の世界で、20mm のズレは箱の縁に当たるか外すかを分ける。
だから閾値には触れない。fk_j4_priority が 21.9mm の点しか選べないなら、その位置の高い ee_y 帯の点を増やすしかない。穴を埋める。
止まる原因は穴だった ― 被覆の実測
PICK 側の out_of_range 停止から、テーブルの穴を実測で切り分けた。狙い (199.1, 300.8)mm 近傍で、点は 15,017 個ある。PICK 帯（ee_y 10〜40mm）に絞っても 2,928 個ある。ところが j1≤88 の安全マスクを通した瞬間、41 点まで激減し、最近傍が 36.4mm になって DIST_MAX 30mm を超えた。
穴の正体は「点が無い」ではなかった。低い（j1=95° の届かない）姿勢に PICK 帯の点はあるが、安全に届く（j1≤88）姿勢の密度が薄い。だから安全フィルタを通すと最近傍が急に遠くなる。この切り分けが、穴を埋める方針を決めた。勘で姿勢を作らず、その領域で高い ee_y を出している実在の姿勢をベースにして、Unity の FKScanner でスキャンし直す。
集合 | 点数 | 最近傍 xz |
|---|---|---|
±40mm ウィンドウ全体（ee_y 制限なし） | 15,017 点 | 9.4mm |
±40mm × PICK 帯 [10, 40)mm | 2,928 点 | 18.5mm |
±40mm × PICK 帯 × j1≤88 safe | 41 点 | 36.4mm（DIST_MAX 30mm 超過） |
xz 30mm × PICK 帯 × j1≤88（DIST_MAX 準拠） | 0 点 | ― |
穴を埋める三連
三つの穴を順に埋めた。いずれも Unity FKScanner_MotionBased で実在姿勢をベースにスキャンし、既存の fk_scan_motion_based_v2.csv に統合した。ファイル名は変えない。新しいバージョン番号を切らない。中身の更新で拡充する。統合のたびに Mac / Unity / KIOXIA fk_tables/ の三か所で md5 を揃え、既存点の回帰がゼロであることを確認した。
一つ目、PICK 穴。狙い (199, 301) の safe 最近傍が 36.43mm から 25.40mm に縮み、DIST_MAX 30mm 圏内の点が 0 から 12 になった。テーブルは 1,003,992 点から 1,037,606 点に増えた。
二つ目、j0 全可動域。PICK/AP_PICK の姿勢が j0（ベース回転）の狭い扇形にしか無く、横方向のカバレッジが薄かった。限界域の実在姿勢を fingerprint にして、j0 を -135° から +135° まで 30° 刻みで振ったベースを 20 本、BASE 近傍の薄い帯を埋めるベースを 12 本、合わせて 32 本をスキャンした。展開後 537,824 点。j0 の到達域が [-92°, +92°]（184°）から [-150°, +150°]（300°）に広がった。テーブルは 1,541,975 点になった。この 32 本のうち 1 本は、±15° の格子展開で j4 が hard limit を超える姿勢を含んでいたため、CSV 生成時の事前検査で捕捉し、hard-safe な別クラスタの実在姿勢に置換している。
三つ目、PLACE 穴。実機で撥ねた狙い (-0.165, +0.176) の高い ee_y 帯（PLACE_HI [100,120)mm）を埋めた。統合前の最近傍は 18.62mm、帯内 50,407 点。統合後は 1.53mm、帯内 54,416 点。実機で撥ねた 21.9mm が、閾値 20mm を大きく下回る 1.53mm になった。テーブルは 1,614,027 点になった。
穴 | 対象狙い | 最近傍（前→後） | テーブル点数（後） |
|---|---|---|---|
PICK 穴 | (199, 301)mm | 36.43mm → 25.40mm | 1,037,606 |
j0 全可動域 | 横方向カバレッジ | j0 到達域 184° → 300° | 1,541,975 |
PLACE 穴 | (-165, +176)mm・ee_y 100-120mm | 18.62mm → 1.53mm | 1,614,027 |
PLACE 穴の統合レポートを全量で残す。
step 4: 統合
現行 v2 (byte-perfect 維持) 1,541,975 行
スキャン出力 134,456 行
現行と重複 62,404
末尾追加 (純増) 72,052
新 v2 1,614,027 行
step 5: 3方向 md5 一致
Mac 600e8b626fb9d69eb10a5ee3d25885fa
KIOXIA 600e8b626fb9d69eb10a5ee3d25885fa
Unity 600e8b626fb9d69eb10a5ee3d25885fa
全 1,614,027 行で一致。ファイル名・場所とも変更なし。
step 6: 回帰 (悪化ゼロ)
PICK 帯 ee_y [10,40) mm × hard-safe: 95,869 → 103,709 行 (+7,840)
過去 pick_hole target (+0.199,+0.301) の最近傍 25.50mm → 25.50mm (完全同一行)
PLACE 帯 ee_y [85,120) mm × 55≤j1≤88: 88,089 → 95,502 行 (+7,413)
target (-0.165,+0.176) の最近傍 3.39mm → 0.95mm (改善)
step 7: 穴埋め判定
PLACE_HI 帯 ee_y [100,120) mm × 55≤j1≤88, target (-0.165,+0.176):
帯内行数 最近傍
pre 50,407 18.62 mm
post 54,416 (+4,009) 1.53 mm
post 最近傍姿勢: ee=(-0.1638, +0.1178, +0.1750) j=[+11.21, +57.00, +20.00, -52.00, -96.00, -30.00]
判定: PASS (1.53 mm < X_RESID_THRESH=20 mm)実機で PLACE が動ききった
PLACE 穴を埋めた後、実機で通しを走らせた。PICK は成功し（VLM 判定 held、confidence 0.82）、approach_place を通過し、stage8_place が今度は撥ねられずに送信された。PLACE は最後まで動いた。ブロックは置かれ、退避まで進み、白箱の位置が撮影された。
結果は completed_with_failures。VLM は PLACE を no と判定した。赤ブロックは箱の外側、手前左に置かれ、白箱の中は空だった。上から撮った写真でも、ブロックは箱の縁の外に落ちている。幾何判定（in_box_geom=True, in_box_dist_mm=26.3）と VLM 判定（箱の外）は食い違い、実写では VLM が正しい。
止まらなかったこと自体が、この試行の到達点だ。撥ねられずに送信され、置き、記録した。そのうえで置き位置は外した。失敗はしたが、失敗を最後まで実行して記録に残した。
接触も記録された。台帳の box_shift_dist は 86.92mm（dx=-85.52, dz=-15.55）。爪が白箱に当たり、箱を約 87mm 押した分がここに残っている。box_shift_reason=ok で、退避後の place 画像が撮れて接触量が測れている。ホワイトリスト修正で place 画像が撮れるようになったことが、この測定を成立させた。
実機ログの stage8 以降を全量で残す。
[CORR ] key=place_x-160_z200 dx=-2.09mm dz=+21.65mm n=18 target (-167.2,+197.6) -> (-165.1,+175.9) [mm]
[HGT ] PLACE apply: cell=place_x-160_z200 dee_y=16.0mm → target_ee_y=0.1160m
[LOOKUP] stage8_place phase=place_withblock target_xz=(-0.1651,+0.1759) → j0=+43.109° method=fk_j4_priority x_resid=0.02141m z_resid=0.01930m joint_ok=True max|Δj|=10.41° ee_y=0.1119m
[ABORT ] stage=8 reason=送信前ガード NG: x_resid=0.021414714479938146m > X_RESID_THRESH (0.02)これが穴埋め前。撥ねられている。穴埋め後の同じセルの通しでは、stage8 が送信され、PLACE が完走し、下記が記録された。
place_verdict = no
place_reason = 赤い四角のブロックは箱の外側（左側）に置かれており、白い箱の中には何も入っていない|fails:7:not_reached,8:not_reached,10:not_reached
in_box_geom = True
in_box_dist_mm = 26.3
box_shift_dx = -85.52
box_shift_dz = -15.55
box_shift_dist = 86.92
box_shift_reason = ok
place_height_cell = place_x-160_z200
place_height_dee_y_mm = 21.00
place_corr_reason = safety_rollback
place_corr_cell_key = place_x-160_z200記録はできた、学習の後半が残った
この試行の JSON の correction ブロックを全量で見ると、二つの未完が出る。
"place_last": {
"phase": "place",
"cell_key": "place_x-160_z200",
"dx_mm": null,
"dz_mm": null,
"n": 0,
"applied": false,
"safety_rollback": true,
"safety_reason": "j4=-104.20 out of [-90.0, 60.0]",
"reason": "safety_rollback",
"update_reason": "updated"
},
"place_height_last": {
"cell_key": "place_x-160_z200",
"dee_y_mm": 21.0,
"target_ee_y_m": 0.12100000000000001,
"j4_selected_deg": -89.2
}一つ目。XZ 補正が safety_rollback でキャンセルされている。理由は j4=-104.20 out of [-90.0, 60.0]。補正を適用しようとした姿勢の j4 が J4_SAFE_MIN=-90.0 を 14 度超えていた。j0 全可動域スキャンで足した限界域の姿勢（fingerprint の j4≈-96）を ±15° 格子で展開したとき、j4 が -104 まで振れる姿勢が生まれ、それが place の候補に入った。ガードはこれを止めた。j4=-104 を実機に送れば、手首が安全範囲を超えた角度で床へ向かう。safety_rollback は正しく働いた。だが、統合したテーブルに J4_SAFE を侵す姿勢が混ざっていることが、実機で初めて表面化した。place の候補選択が J4_SAFE を侵す姿勢を出さないよう、絞り込みを足す必要がある。
二つ目。接触は box_shift_dist=86.92mm として記録されたが、この 87mm が高さ補正のラチェットに接続されていない。台帳の box_hit_flag は空で、place_height_last は dee_y=21mm を記録しているが、これはこの試行で適用した値であって、「87mm 当たったから次はさらに上げる」という更新が走った形跡がない。接触を記録するところまでは動いたが、記録した接触を次の高さ補正に反映するループの後半が、まだ閉じていない。
失敗から動きが良くなっていくかという評価軸で見ると、失敗を最後まで実行して記録する前半は、実機で成立した。記録した失敗を次の一手に反映して改善する後半が、二つの具体的な穴（J4_SAFE を侵す候補の混入、接触とラチェットの非接続）として残った。
掲載コード（全量）
以下は今回の PLACE 完走・穴埋め・統合に関わったコードの全量である。省略はしていない。Mac-local パスも改変していない。
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.085 # m (85mm・DAY263 70mm 確定 → DAY268 白箱接触回避で +15mm)
PLACE_EE_Y_MAX = 0.115 # m (115mm・DAY263 100mm 確定 → DAY268 白箱接触回避で +15mm)
# DAY265: PLACE の FK 点選択に姿勢連続性 (全軸帯フィルタ) を入れる.
# approach_place 6 軸から ±PLACE_JOINT_TOL_DEG 以内の点のみ候補にし、
# その中で ee_y 帯 ∩ x-z 最近傍を選ぶ (j1/j4 に歪みが逃げるのを防ぐ).
# 値は dry-run (fk_lookup CLI で候補数と残差を tol=10/15/20/25 振り) で確定.
# 実装時仮値 15.0. dry-run 結果でエイイチ確定後に固定.
PLACE_JOINT_TOL_DEG = 15.0
# DAY265: 白箱激突二重防御 (ee_y 下限). 帯フィルタが正しければ発火しないはずの保険.
PLACE_EE_Y_FLOOR = 0.085 # m (85mm・PLACE_EE_Y_MIN と同値・DAY263 70mm 確定 → DAY268 白箱接触回避で +15mm)
# DAY274 案B': PLACE 降下の前腕姿勢下限 (箱側面激突防御).
# fk_nearest が (ee_x, ee_z) 距離のみで選ぶと、前腕を寝かせた低 j1 姿勢
# (例: 2026-07-05 実測 j1=46.65・ee_y=105.3mm) が勝ち、爪先は高くても前腕が箱を擦る.
# 弁別軸は ee_y ではなく j1. DAY235 place_withblock base j1=59.15° (実測: 当たらない
# 安全姿勢) から余裕 5° を引いた 55° を下限とする. 実装前実測 (2026-07-06):
# - 台帳 362 trial 全域で ABORT 増分 +1 (現状 80 → B' 81)
# - 今日の当たるセル (box=(-197,+179)) で j1 46.65 → 57.39 に切替わることを確認
# - resid p50 1.71→2.15mm (差 0.44mm、GUARD 20mm を大幅下回る)
# 今回対象は stage8 place_withblock のみ. stage7 approach_place は j0_lookup 経由で
# adopted JSON base_angles の j1=57.39° 固定 (既に安全) のため手当て不要.
PLACE_J1_MIN_DEG = 55.0
# 段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,
daemon_send_angles, # DAY273 (fix): PLACE 再照準 (_visual_servo_loop_place)
# で使用. run_0001_20260705_182346 が NameError で 1 ループ目
# に即死していた (未 import). PICK 側 visual_servo_loop でも
# 実運用中の関数を同じ名前で流用する.
daemon_get_angles, # DAY273 (fix): 同上. 再照準ループ内の j0 実測用.
# 既存 _daemon_get_angles_filtered (j0_sweep_scan 由来) は
# base 軸フィルタ用途で意味論が異なるため raw を使う
# (PICK 側 visual_servo_loop L792 と同じ).
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 ログ遅延（DAY268 0.3→1.0 に戻し・j4 未到達/HOME復帰不能多発のため。短縮は j4 物理ストール切り分け後に再検討）
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 = 10 # VLM 撮影前の warm-up（DAY268 30→10 短縮）
VLM_CAPTURE_DROP_FRAMES = 3 # 撮影直前のバッファクリア（DAY268 5→3 短縮）
# 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)
# PLACE 補正 (DAY268 案1): 学習採用の距離閾値.
# in_box_dist_mm がこの値以下なら place_verdict に関わらず Δ を学習に採用する.
# 箱 80x80mm・半対角 ≈56.6mm を外に余裕を取った値. 惜しい no (48〜58mm) を拾い,
# 大外し no (137/151/241mm) を捨てる. 台帳 place_x-160_z220 の 7 試行 (56mm 前後で
# 足踏み) が学習に回るようになり, no が続くセルの逆進性を解消する.
PLACE_LEARN_DIST_MAX_MM = 90.0 # 【旧定数・未使用】DAY274 で更新条件を pick=yes ゲート +
# 破綻ガード 3 種に切り替えた際にゲートとしては使わなくなった.
# 現行の外れ値ゲートは PLACE_CORR_MAX_DIST_MM=120.0.
# data_manifest 方針で対比記録として残置 (削除しない).
# DAY274 案 PLACE 補正記憶起動: 破綻ガードと補正後姿勢の安全チェック用定数.
# 起動と同時に必須 (過去に PLACE 補正を汚染で全削除した前科あり).
# 目的: 「pick=yes 全試行を対称に学習しつつ、検出破綻・過去残像を絶対に混ぜない」.
PLACE_CORR_SIGN_FLIP_GAP_MM = 150.0 # sign flip AND gap>150mm を PICK残像/別物拾いと判定
PLACE_CORR_MAX_DIST_MM = 120.0 # dist(block_place - box) > 120mm を外れ値と判定
# 補正後 target で fk_lookup した姿勢の j4 がこの範囲外なら補正を破棄し raw box_xz で降下
# (servo が深い j4 領域=swept_retreat スタール -89.2 付近へ補正で押し込まないため).
PLACE_CORR_J4_SAFE_MIN = -90.0
PLACE_CORR_J4_SAFE_MAX = +60.0
# PLACE/PICK 補正 (DAY268): 1 更新あたりの変化量クランプ.
# EMA 出力が前回値からこの幅を超えて動かないよう制限する. 60→241→24 のような
# 1 試行での過補正オーバーシュートを抑える. 絶対値天井 (MAX_CORRECTION_MM=40) とは
# 別で, こちらは「1 回でどれだけ動けるか」の制限.
MAX_CORRECTION_STEP_MM = 15.0
# ---- DAY272 案T: PLACE 再照準 (stage8.5 visual servo) ----------------------
# stage8 到達後・stage9 リリース前に、上カメラで保持ブロック X と箱中心 X の
# ズレを j0 だけで締める. Z は触らない (DAY200 地雷回避).
# PICK 視覚サーボ (visual_servo_pick.visual_servo_loop) の複製移植.
# PICK 本線関数は 1 バイトも改変しない (DAY249 長期安定運用の保全).
#
# 係数 MM_PER_DEG_J0_X_PLACE:
# PICK 側 MM_PER_DEG_J0_X = -6.63 は PICK 姿勢 (j1=87.74 / j4=-70.11) で
# 実測校正された値. PLACE 姿勢は stage8 の j1≈55 / j4≈-89 と PICK と大きく
# 異なる (j1 が 33° 浅く j4 がさらに折れている) ため係数は姿勢依存で正確に
# 一致しない. 初期値として PICK 値を採用し, 実機ログ (下記 history) から
# 後段で校正する前提. 符号 (負値) は幾何的に姿勢を跨いで保存されるため反転不要.
# 実測校正のため, サーボループの各反復で
# delta_j0_actual (実測 j0 変化) と delta_block_x_actual (実測 block_x 変化)
# を log_record["servo_place"]["history"] に残す. 台帳新列は追加しない
# (67 列ヘッダ不変・[ABORT-APPEND] 回避).
MM_PER_DEG_J0_X_PLACE = -6.63
PLACE_CONVERGE_THRESHOLD_MM = 8.0 # 箱内幅 80mm に対し PICK(5.0) より緩め
PLACE_MAX_DELTA_DEG = 5.0 # 1 ループ最大 delta (自動運転暴走保護).
# PICK は DAY249 で撤去したが PLACE は自動運転
# かつ係数未校正のため再導入.
PLACE_SERVO_MAX_ITER = 10 # ループ上限 (時間ガードと二重で強制収束)
PLACE_SERVO_MAX_SEC = 15.0 # 時間上限
PLACE_SERVO_LOOP_WAIT_SEC = 0.5 # ループ間 sleep (PICK LOOP_WAIT_SEC と同値)
PLACE_SERVO_DETECT_FAIL_MAX = 3 # 連続検出失敗許容回数 (超えたら再照準中止)
# DAY272 案T (拡張): 観測モード. True で 1 ループのみ実行し即座に抜ける
# (1 回の j0 送角 → 実測 → 送角後 block_x 追加測定 → verdict 出力で停止, 収束判定を待たない).
# 初回実機で「j0 符号が正しい向きに締まるか」「保持ブロックが箱縁に接触しないか」を
# 目視+ログで確認する用. exit_reason="observe_once" で抜ける.
# フラグ分岐追加のみで既存収束ループ本体は無変更.
PLACE_SERVO_OBSERVE_ONCE = True
# C1 (DAY268 案R): 案A (絶対差分ガード・閾値 10°) は実機テスト run_0001_20260704_191203
# で正常な LIFT 移動量 (stage6=18° / stage8=12.5° / stage10=44°) を誤 ABORT した.
# 閾値方式では「正常な大移動」と「未到達直後の急動作」を切り分けられないため撤去し,
# 案R (未到達フラグ方式) に移行. 発火条件は「対応する DESCEND move が arrived=False か」
# のみで, 呼び出し側 (stage6_lift / stage10_lift_back) が log_record["moves"] から
# 直前 DESCEND の arrived を引いて _do_move に abort_if_prev_not_reached を渡す.
# 詳細は _do_move の docstring と stage6/stage10 の呼び出し箇所のコメント参照.
CORR_MEM_PATH = Path(
"/Volumes/KIOXIA/r2r2r-data/diag/correction_memory.json")
# DAY274 層2: PLACE 高さ補正記憶 (XZ 補正と別記憶・独立管理).
# 白箱に当たったセルで目標 ee_y を上げる (ラチェット学習・上げ速く下げ遅く).
# 初期値は保守的. 実機のラチェットで自己調整される前提 (焼き付けない).
# box_shift bimodal(10mm 非当たり / 216mm 当たり) の谷を HEIGHT_HIT_THRESH に置く.
# HEIGHT_MAX_MM=40 は箱縁+ブロック厚(24mm)+余裕. これ以上は上げない.
PLACE_HEIGHT_MEM_PATH = Path(
"/Volumes/KIOXIA/r2r2r-data/diag/place_height_memory.json")
HEIGHT_HIT_THRESH_MM = 50.0 # box_shift_dist > これ で「当たった」= 上げる
HEIGHT_CLEAR_THRESH_MM = 20.0 # box_shift_dist < これ で「明確に非当たり」= 下げる
HEIGHT_STEP_MM = 5.0 # 一段の上げ (速い). 当たり続ければ +5mm 積む
HEIGHT_DECAY_MM = 1.0 # 一段の下げ (遅い. STEP の 1/5)
HEIGHT_MAX_MM = 40.0 # 上限. 箱縁+ブロック厚+余裕. これ以上は上げない
USE_CORRECTION = True # False で補正完全無効 (回帰確認用)
# 手0 (案S・DAY268): PLACE 補正の計測モード. True のときのみ:
# - _apply_correction(phase="place", ...) を素通り (target_before == target_after)
# - PLACE 側 _corr_update を skip (correction_memory.json を汚さない)
# PICK 側の apply/update は一切変更しない (掴めないと PLACE まで届かないため通常運転).
# ee_y +15mm 後の PLACE Z バイアス素値を実測する目的.
PLACE_CORR_MEASURE_MODE = False # DAY274: True→False. PLACE 補正記憶を起動.
# PICK 側 pick_x*_z* と対称の place_x*_z* セル・EMA α=0.5
# で学習開始. 破綻ガード 3 種 + 補正後姿勢の安全チェック
# (下記 update 側 / apply 直後の fk_lookup 事前検査) 併用.
# DAY257 後段: pick 判定の二重チェック閾値。VLM yes でも tip 計測の dist がこれを
# 超えていれば「掴めていない」として no に倒す (over-pickの誤学習を防ぐ)。
# 単位 mm。実機データを見て振る前提の可変定数。
PICK_DIST_MAX_MM = 30.0
# DAY273 案A: tip_dist 測定破綻ガード. この値を超えたら override を発火させず
# VLM 判定を信じる. 爪先と保持ブロックが 150mm 超離れて「掴めている」ことは
# 幾何的にありえず、それはブロック検出位置の破綻 (mechArm 作業半径 270mm・
# ブロックは爪の間にある前提).
# run_0001_20260705_183800 で vlm=yes・confidence=0.82・reason 正確だったのに
# tip_dist=362.8mm で override 発火→ regrasp_open→ 落下した反省.
# 分岐:
# 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 を信じる (新規・測定破綻)
PICK_DIST_BREAKDOWN_MM = 150.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"/""
# DAY265: 横カメラ (Iriun index=1) の記録列 (5 列追加, 58-62).
# side_ap_place_capture_path は stage7 到達後撮影 (判定なし・画像パスのみ).
# side_place_* は stage8 到達後撮影 + VLM judge (kind="place_side").
"side_ap_place_capture_path", # 58: stage7 横カメラ画像パス (判定なし)
"side_place_capture_path", # 59: stage8 横カメラ画像パス
"side_place_verdict", # 60: 横カメラ PLACE 判定 "yes/no/unknown/"
"side_place_confidence", # 61: 横カメラ PLACE 判定 confidence 0.0-1.0
"side_place_reason", # 62: 横カメラ PLACE 判定 reason 短文
# DAY265: PLACE 側の補正記憶 (5 列追加, 63-67). PICK 側の corr_cell_key /
# corr_applied_dx/dz / target_before/target_after (51-55) と同構造・列名だけ
# phase 区別のため place_ プレフィックス. apply/update は DAY262 で既に実装済.
# ここは _append_ledger が log_record.correction.place_last を読んで書き込むだけ.
"place_corr_cell_key", # 63: 例 "place_x-40_z100" (CELL_MM=20 セル中心)
"place_corr_applied_dx", # 64: 適用した X 補正量 mm (EMA 平均後)
"place_corr_applied_dz", # 65: 適用した Z 補正量 mm
"place_target_before", # 66: 補正前狙い box_xz [mm] "(+X,+Z)"
"place_target_after", # 67: 補正後狙い (lookup へ渡した値) [mm]
# DAY274: 白箱の接触前後ズレ (5 列追加, 68-72). stage2 検出の box_xz と
# stage9b 退避後の place 画像から再検出した箱位置の差分. 箱が動いた =
# 前腕/爪が箱に当たった、を毎試行データで残す. 生値 3 列は今回から埋める.
# 判定 2 列は空欄で用意 (閾値確定後に既存空列を埋める運用). 退避画像は
# 既存の VLM 判定と同じ jpg. 撮影は追加しない (観測のみ).
"box_shift_dx", # 68: (box_xz_last[0] - box_xz[0]) * 1000 [mm]
"box_shift_dz", # 69: (box_xz_last[1] - box_xz[1]) * 1000 [mm]
"box_shift_dist", # 70: sqrt(dx^2 + dz^2) [mm]
"box_hit_flag", # 71: 当面空欄. 閾値判定は後日.
"box_hit_thresh_mm", # 72: 当面空欄. 適用した閾値の記録用.
# DAY274 層2/3: PLACE 高さ補正の適用値 (3 列追加, 73-75). place_x*_z* セル,
# dee_y_mm (ラチェット学習で蓄積), 選ばれた姿勢の j4 (層3 j4 優先が効いたか
# 確認用). 過去行は空欄 (8-A・過去行保持で末尾に空セル付与).
"place_height_cell", # 73: 例 "place_x-200_z180" (PLACE XZ 補正と同格子)
"place_height_dee_y_mm", # 74: 適用した Δee_y (mm)
"place_height_j4_selected", # 75: stage8 で fk が選んだ姿勢の j4 (度)
# DAY274 記録層修正: 失敗理由と画像パスを常時記録 (3 列追加, 76-78).
# エイイチ原則「失敗しても必ず記録を残す・画像が残っていれば後から記憶できる」.
# 空欄を作らず, 全試行でいずれかの語彙で必ず埋まる. 判定ロジック無改変.
# 過去行は 8-A で末尾空セル付与 (0 行作り直しはしない).
"place_corr_reason", # 76: applied/measure_mode/no_memory/pick_not_yes/
# detect_none/box_xz_none/sign_flip/dist_over/
# safety_rollback/not_reached/disabled/
# ssd_unmounted/entry_corrupted/unknown
"box_shift_reason", # 77: ok/place_img_none/white_box_detect_fail/box_xz_none
"place_img_path", # 78: 退避後 place 画像のフルパス (検出失敗の後追い再検出用)
]
# 爪先(青シール)検出パラメータ（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 判定画像パス
"box_xz_last": None, # DAY274: stage9b 退避後の place 画像から再検出した白箱 [x,z]
# (None = 検出失敗 or 画像なし). 生値 3 列は _append_ledger が読む.
# 非移動 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_block_from_frame(frame):
"""DAY272 案T: cv2 frame から赤ブロックの Unity 座標 [x, z] を検出する.
_detect_block_in_place_img の frame 直渡し版. jpg 保存を挟まないので
サーボループから毎反復呼べる. 検出/変換 pipeline は同一 (detect_red_block
+ pixel_to_unity・同 homography). 全例外を握って None (制御を絶対止めない).
"""
if frame is None:
return None
try:
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
# DAY274: 白箱テンプレのモジュール-lazy キャッシュ.
# 初回だけ 06f._load_white_box_templates() を呼び、以降は保持したものを使う.
# 失敗 (テンプレ欠落等) は None を返し, 呼び出し側で「検出失敗=空欄」に落ちる.
_WHITE_BOX_M06F = None
_WHITE_BOX_TEMPLATES = None
_WHITE_BOX_LOAD_TRIED = False
def _get_white_box_detector():
"""06f モジュールと templates を lazy に取得する.
stage2 の detect_positions_once と同一 API (06f._load_white_box_templates).
返り値: (m06f, templates) 失敗時は (None, None).
"""
global _WHITE_BOX_M06F, _WHITE_BOX_TEMPLATES, _WHITE_BOX_LOAD_TRIED
if _WHITE_BOX_LOAD_TRIED:
return _WHITE_BOX_M06F, _WHITE_BOX_TEMPLATES
_WHITE_BOX_LOAD_TRIED = True
try:
import importlib
m06f = importlib.import_module("06f_top_white_box_detect_v7")
templates = m06f._load_white_box_templates()
_WHITE_BOX_M06F = m06f
_WHITE_BOX_TEMPLATES = templates
return m06f, templates
except (Exception, SystemExit):
# DAY274 codex ❌ B4: 06f (test_white_box_score.py) はテンプレロード失敗時
# に sys.exit(1) する経路がある. SystemExit は BaseException 直系で
# except Exception では捕まらず, PLACE 本線が停止する. 観測処理の失敗は
# 3 列空欄で完走・HOME 復帰・台帳追記まで進める設計意図に合わせ,
# SystemExit も握って None に倒す. KeyboardInterrupt は BaseException 直系で
# (Exception, SystemExit) に含まれないため Ctrl+C は生きる (意図).
# except BaseException にはしない (Ctrl+C を握らないため).
_WHITE_BOX_M06F = None
_WHITE_BOX_TEMPLATES = None
return None, None
def _detect_white_box_in_place_img(image_path):
"""DAY274: place 画像 (stage9b 退避後の保存済み jpg) から白箱の Unity 座標
[x, z] を検出する.
- stage2 と同一検出器 (06f evaluate_frame_gray) + 同一 homography
(visual_servo_pick.pixel_to_unity). 新規検出ロジックは書かない.
- detect_white_box_once は VideoCapture 引数を取るため, 静止画向けに内部
関数 evaluate_frame_gray を直接呼ぶ (これが本体検出器). しきい値は
m06f.TEMPLATE_SCORE_THRESHOLD (stage2 と同値).
- _mat_px_range は stage2 の setup_homography_and_mat_range で既に初期化済
(未初期化なら None を返す・制御は止めない).
- 全例外を握り、失敗時 None (box_shift 3 列は空欄で埋まる).
"""
if image_path is None:
return None
try:
import visual_servo_pick as vsp
if vsp._mat_px_range is None:
return None
m06f, templates = _get_white_box_detector()
if m06f is None or templates is None:
return None
frame = cv2.imread(str(image_path))
if frame is None:
return None
results = m06f.evaluate_frame_gray(frame, vsp._mat_px_range, templates)
best = results.get("overall_best") if isinstance(results, dict) else None
if best is None:
return None
score = best.get("best_score")
if score is None or score < m06f.TEMPLATE_SCORE_THRESHOLD:
return None
cx = best.get("cx"); cy = best.get("cy")
if cx is None or cy is None:
return None
ux, uz = pixel_to_unity(cx, cy)
return [float(ux), float(uz)]
except (Exception, SystemExit):
# DAY274 codex ❌ B4: 06f 内部 (evaluate_frame_gray など) が sys.exit を
# 呼ぶ経路があっても本線を止めない. SystemExit を握って None に倒す.
# KeyboardInterrupt (BaseException 直系) は含まれず、Ctrl+C は生きる (意図).
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
old_dx = float(e.get("dx_mm", 0.0))
old_dz = float(e.get("dz_mm", 0.0))
new_dx = a * float(dx_mm_new) + (1 - a) * old_dx
new_dz = a * float(dz_mm_new) + (1 - a) * old_dz
# DAY268: 1 更新あたりの変化量クランプ (跳ね幅制限).
# 60→241→24 のような 1 試行での過補正オーバーシュートを抑える.
# 絶対値天井 MAX_CORRECTION_MM=40 は _apply_correction 側で別途効く.
s = MAX_CORRECTION_STEP_MM
new_dx = max(old_dx - s, min(old_dx + s, new_dx))
new_dz = max(old_dz - s, min(old_dz + s, new_dz))
mem[key] = {
"dx_mm": new_dx,
"dz_mm": new_dz,
"n": int(e.get("n", 0)) + 1,
}
return mem[key]
# ---- DAY274 層2: PLACE 高さ補正記憶 (ラチェット学習・XZ 補正と独立) --------
def _height_load():
"""place_height_memory.json をロード. 失敗時は {} (制御を絶対止めない)."""
if not PLACE_HEIGHT_MEM_PATH.exists():
return {}
try:
with open(PLACE_HEIGHT_MEM_PATH, "r", encoding="utf-8") as f:
return json.load(f)
except Exception as e:
print("[HGT ] [WARN] 読み込み失敗 ({}). 空辞書で続行".format(e))
return {}
def _height_save(mem):
"""高さ補正記憶を atomic に保存. KIOXIA 未マウント時は False でスキップ."""
if not _ensure_ssd_mounted():
print("[HGT ] KIOXIA 未マウント → 高さ補正記憶の保存をスキップ")
return False
try:
PLACE_HEIGHT_MEM_PATH.parent.mkdir(parents=True, exist_ok=True)
tmp = PLACE_HEIGHT_MEM_PATH.with_suffix(
PLACE_HEIGHT_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(PLACE_HEIGHT_MEM_PATH))
return True
except Exception as e:
print("[HGT ] [WARN] 保存失敗: {}".format(e))
return False
def _height_key(x_m, z_m):
"""(x, z)[m] → セルキー 'place_x{X}_z{Z}' (PLACE の XZ 補正と同一格子・20mm)."""
return _corr_key("place", x_m, z_m)
def _height_get_dee_y_mm(mem, key):
"""セルキー → dee_y_mm (0.0 なら None・未登録). 破損時は 0.0."""
e = mem.get(key)
if e is None or not isinstance(e, dict):
return 0.0
try:
return float(e.get("dee_y_mm", 0.0))
except (TypeError, ValueError):
return 0.0
def _height_update_ratchet(mem, key, box_shift_dist_mm):
"""ラチェット則で 1 セルの dee_y_mm を更新して mem に書き戻す.
- box_shift_dist_mm > HEIGHT_HIT_THRESH_MM → dee_y += HEIGHT_STEP_MM (速い)
- box_shift_dist_mm < HEIGHT_CLEAR_THRESH_MM → dee_y -= HEIGHT_DECAY_MM (遅い)
- 中間帯 (CLEAR ≤ dist ≤ HIT) は現状維持 (ヒステリシス)
- dee_y は [0.0, HEIGHT_MAX_MM] にクランプ.
box_shift_dist_mm が None (検出失敗) の場合は上位で呼ばず, 呼ばれても現状維持.
戻り値: 更新後エントリ dict {'dee_y_mm', 'n'}.
"""
if box_shift_dist_mm is None:
# 上位が守るはず (呼ばない) だが二重に安全側.
e = mem.get(key)
if isinstance(e, dict):
return e
mem[key] = {"dee_y_mm": 0.0, "n": 0}
return mem[key]
e = mem.get(key)
if e is not None and not isinstance(e, dict):
print("[HGT ] [WARN] mem[{}] が dict 以外 ({}). 破棄して再初期化"
.format(key, type(e).__name__))
e = None
if e is None:
old_dee_y = 0.0
n = 0
else:
try:
old_dee_y = float(e.get("dee_y_mm", 0.0))
except (TypeError, ValueError):
old_dee_y = 0.0
try:
n = int(e.get("n", 0))
except (TypeError, ValueError):
n = 0
d = float(box_shift_dist_mm)
if d > HEIGHT_HIT_THRESH_MM:
new_dee_y = old_dee_y + HEIGHT_STEP_MM
elif d < HEIGHT_CLEAR_THRESH_MM:
new_dee_y = old_dee_y - HEIGHT_DECAY_MM
else:
new_dee_y = old_dee_y
new_dee_y = max(0.0, min(HEIGHT_MAX_MM, new_dee_y))
mem[key] = {"dee_y_mm": new_dee_y, "n": n + 1,
"last_box_shift_dist_mm": d}
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 は (落下点 - 箱). どちらも「実測 - 目標」.
DAY274 記録層修正: 判定ロジックは無改変. cell_key を全経路で必ず入れる (原則1).
reason を meta に埋め込む (原則2): applied / measure_mode / disabled /
ssd_unmounted / no_memory / entry_corrupted."""
bx_m = float(block_xz[0]); bz_m = float(block_xz[1])
# DAY274 原則1: cell_key は補正適用の成否と無関係に常に算出.
# 早期 return する経路 (USE_CORRECTION=False / measure_mode / SSD 未マウント) でも
# 「どのセルの試行か」を残す. 判定ロジックは無改変・記録だけ変える.
key = _corr_key(phase, bx_m, bz_m)
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": key,
"dx_mm": None, "dz_mm": None, "n": 0,
"applied": False,
"reason": "", # DAY274: 経路ごとに埋める.
}
if not USE_CORRECTION:
meta["reason"] = "disabled"
return (bx_m, bz_m), meta
# 手0 (案S・DAY268): PLACE 計測モードのとき phase="place" の apply を skip.
# PICK は無影響. target_before == target_after のまま返る.
if phase == "place" and PLACE_CORR_MEASURE_MODE:
print("[CORR ] PLACE measure mode: apply/update skipped")
meta["reason"] = "measure_mode"
return (bx_m, bz_m), meta
if not _ensure_ssd_mounted():
print("[CORR ] KIOXIA 未マウント → 補正スキップ (素通り)")
meta["reason"] = "ssd_unmounted"
return (bx_m, bz_m), meta
mem = _corr_load()
entry = _corr_get(mem, key)
if entry is None:
# 記憶未生成 (このセル初回). cell_key は残す (原則1).
meta["reason"] = "no_memory"
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__))
meta["reason"] = "entry_corrupted"
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
meta["reason"] = "applied"
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 列・PICK 側).
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")),
})
# DAY265: PLACE 側の補正記録 (5 列・PICK と同構造). apply/update は
# DAY262 で既に実装済 (log_record.correction.place_last に格納される).
# ここでは stage8 で適用した meta を CSV に反映するだけ.
place_corr = (log_record.get("correction") or {}).get("place_last") or {}
row.update({
"place_corr_cell_key": place_corr.get("cell_key", ""),
"place_corr_applied_dx": ("{:.2f}".format(place_corr["dx_mm"])
if isinstance(place_corr.get("dx_mm"), (int, float))
else ""),
"place_corr_applied_dz": ("{:.2f}".format(place_corr["dz_mm"])
if isinstance(place_corr.get("dz_mm"), (int, float))
else ""),
"place_target_before": _fmt_xz(place_corr.get("target_before_mm")),
"place_target_after": _fmt_xz(place_corr.get("target_after_mm")),
})
# DAY274: 白箱の接触前後ズレ (5 列・生値 3 + 判定 2 空欄).
# 最初 (接触前) = stage2 検出 box_xz (既存. 誰も触っていない).
# 最後 (接触後) = stage9b 退避後の place 画像から再検出 (box_xz_last).
# どちらかが None なら 3 列とも空欄 (箱位置比較不能・クラッシュしない).
# 判定 (box_hit_flag / box_hit_thresh_mm) は当面空欄で保持し, 閾値確定後に埋める.
box_xz_last = _OBSERVE_STATE.get("box_xz_last")
_box_shift_dx_str = ""
_box_shift_dz_str = ""
_box_shift_dist_str = ""
if (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(box_xz_last, (list, tuple)) and len(box_xz_last) == 2
and isinstance(box_xz_last[0], (int, float))
and isinstance(box_xz_last[1], (int, float))):
_dx_mm = (float(box_xz_last[0]) - float(box_xz[0])) * 1000.0
_dz_mm = (float(box_xz_last[1]) - float(box_xz[1])) * 1000.0
_dist_mm = math.sqrt(_dx_mm * _dx_mm + _dz_mm * _dz_mm)
_box_shift_dx_str = "{:.2f}".format(_dx_mm)
_box_shift_dz_str = "{:.2f}".format(_dz_mm)
_box_shift_dist_str = "{:.2f}".format(_dist_mm)
row.update({
"box_shift_dx": _box_shift_dx_str,
"box_shift_dz": _box_shift_dz_str,
"box_shift_dist": _box_shift_dist_str,
"box_hit_flag": "", # 閾値判定は後日 (空で保持)
"box_hit_thresh_mm": "", # 適用した閾値の記録用 (空で保持)
})
# DAY274 層2/3: PLACE 高さ補正の適用値 (3 列). 未適用の試行は空欄.
# log_record.correction.place_height_last に stage8 で格納された meta を読む.
place_hgt = (log_record.get("correction") or {}).get("place_height_last") or {}
_hgt_cell = place_hgt.get("cell_key", "") or ""
_hgt_dee_y = place_hgt.get("dee_y_mm")
_hgt_j4 = place_hgt.get("j4_selected_deg")
row.update({
"place_height_cell": _hgt_cell,
"place_height_dee_y_mm": ("{:.2f}".format(_hgt_dee_y)
if isinstance(_hgt_dee_y, (int, float))
else ""),
"place_height_j4_selected": ("{:.2f}".format(_hgt_j4)
if isinstance(_hgt_j4, (int, float))
else ""),
})
# DAY274 原則2: 記録層の欠損理由 3 列 (place_corr_reason / box_shift_reason /
# place_img_path). 判定ロジックは無改変. どの試行でも必ずどれかの語彙で埋まる.
# ---- place_corr_reason (PLACE 補正の apply/update 理由) ----
# 優先度: (1) place_last meta 未設定 = PLACE 段8まで到達せず (not_reached)
# (2) apply 側 reason (measure_mode/disabled/ssd_unmounted/entry_corrupted/
# safety_rollback) は「補正できない」で hard-stop
# (3) update 側 reason があればそれ (pick_not_yes/detect_none/box_xz_none/
# sign_flip/dist_over/updated).
# (4) update 側 reason 未設定 (未到達で update スキップ) は apply 側 reason
# (applied/no_memory) を採用.
# updated → "applied" にマップ (成功の意味で User 語彙の "applied" に統合).
place_last = (log_record.get("correction") or {}).get("place_last")
if not isinstance(place_last, dict):
_place_corr_reason = "not_reached"
else:
_apply_r = place_last.get("reason", "") or ""
_upd_r = place_last.get("update_reason", "") or ""
if _apply_r in ("measure_mode", "disabled", "ssd_unmounted",
"entry_corrupted", "safety_rollback"):
_place_corr_reason = _apply_r
elif _upd_r in ("pick_not_yes", "detect_none", "box_xz_none",
"sign_flip", "dist_over"):
_place_corr_reason = _upd_r
elif _upd_r == "updated":
_place_corr_reason = "applied"
elif _apply_r in ("applied", "no_memory"):
_place_corr_reason = _apply_r
else:
_place_corr_reason = "unknown"
# ---- box_shift_reason (box_shift 3 列が埋まらない理由) ----
# 優先度: box_xz なし → box_xz_none / place 画像なし (swept_retreat 失敗等)
# → place_img_none / 画像あるが白箱検出失敗 → white_box_detect_fail / 揃った → ok.
_cap_place = _OBSERVE_STATE.get("cap_place")
_bx_valid = (isinstance(box_xz, (list, tuple)) and len(box_xz) == 2
and isinstance(box_xz[0], (int, float))
and isinstance(box_xz[1], (int, float)))
_bxl_valid = (isinstance(box_xz_last, (list, tuple))
and len(box_xz_last) == 2
and isinstance(box_xz_last[0], (int, float))
and isinstance(box_xz_last[1], (int, float)))
if not _bx_valid:
_box_shift_reason = "box_xz_none"
elif not _cap_place:
_box_shift_reason = "place_img_none"
elif not _bxl_valid:
_box_shift_reason = "white_box_detect_fail"
else:
_box_shift_reason = "ok"
# ---- place_img_path (退避後 place 画像パス. 検出失敗の後追い再検出用) ----
_place_img_path = _cap_place or ""
row.update({
"place_corr_reason": _place_corr_reason,
"box_shift_reason": _box_shift_reason,
"place_img_path": _place_img_path,
})
# 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,
})
# DAY265: 横カメラ関連の 5 列を埋める.
# 画像パスは vlm_captures から camera_role=="side" + label で拾う (関節/上カメラ既存に非干渉).
side_ap_path = ""
side_place_path = ""
for c in log_record.get("vlm_captures", []) or []:
if c.get("camera_role") != "side":
continue
lbl = c.get("label", "")
img = c.get("image") or ""
if lbl == "ap_place_side" and img and not side_ap_path:
side_ap_path = str(img)
elif lbl == "place_side" and img and not side_place_path:
side_place_path = str(img)
side_pj = (log_record.get("judgments") or {}).get("place_side") or {}
side_verdict = side_pj.get("verdict") or ""
side_conf = (side_pj.get("confidence")
if isinstance(side_pj.get("confidence"), (int, float))
else "")
side_reason = side_pj.get("reason") or ""
row.update({
"side_ap_place_capture_path": side_ap_path,
"side_place_capture_path": side_place_path,
"side_place_verdict": side_verdict,
"side_place_confidence": side_conf,
"side_place_reason": side_reason,
})
# DAY265: 失敗完走時、stage_failures を pick_reason / place_reason に注記.
# v3 57 列に新列を足せないため既存の自由文列に |fails:... を append する.
# 検索: CSV で "|fails:" を grep すれば失敗完走行だけ抜ける.
# 振り分けは stage 番号で pick 側 (1-6, regrasp, pick_overall) と
# place 側 (7-10) を分離. 通常 (失敗なし) では両列とも VLM reason のまま.
stage_failures = log_record.get("stage_failures") or []
if stage_failures:
def _encode(fails):
if not fails:
return ""
parts = []
for f in fails:
st = f.get("stage")
kd = f.get("kind", "")
parts.append("{}:{}".format(st, kd))
return "|fails:" + ",".join(parts)
pick_side = []
place_side = []
for f in stage_failures:
st = f.get("stage")
# 数値 stage 番号 1-6 か "regrasp_*" か "pick_overall" が pick 側.
if isinstance(st, int):
(pick_side if st <= 6 else place_side).append(f)
else:
s = str(st)
if s.startswith("regrasp") or s.startswith("pick"):
pick_side.append(f)
else:
place_side.append(f)
if pick_side:
row["pick_reason"] = (row["pick_reason"] or "") + _encode(pick_side)
if place_side:
row["place_reason"] = (row["place_reason"] or "") + _encode(place_side)
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 が送信許可リスト外 → ABORT
3. x_residual_m > X_RESID_THRESH → ABORT
4. z_residual_m → ログのみ（ここでは判定しない）
戻り値: (ok: bool, reason: str | None)
DAY274 修正: method 白リストに層3 の "fk_j4_priority" (j4 優先スコアで
選ばれた PLACE 姿勢) と "fk_nearest_ee_y_tol_empty" (ee_y 帯内候補ゼロ時の
xz 最近傍フォールバック) を追加. これらは fk_lookup 内部で JOINT_LIMITS +
j1<=88 の safe_mask + PLACE_J1_MIN_DEG 帯を通過した安全姿勢だが, 白リストが
更新されていなかったため joint_ok=True・x_resid≈1mm でも撥ねられていた
(2026-07-06 の place_x-160_z200 で dee_y=5 の 5 連続 place_img_none の原因).
ガードロジック本体 (joint_ok / x_resid / X_RESID_THRESH) は無改変.
reason 文言は実際に撥ねた判定に対応するように差し替え (旧「弧の x 範囲外 /
FK 帯外」は method 文字列比較の実態と一致しないコスメティック文言だった).
"""
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 の帯内最近傍成功 (従来の PLACE 経路)
# "fk_j4_priority" : DAY274 層3 の j4 優先スコアで選択成功
# "fk_nearest_ee_y_tol_empty" : 層3 の ee_y 帯内候補ゼロ時 xz 最近傍 fallback
# 不許可:
# "clamp" : j0_lookup 弧端張り付き
# "out_of_range" : fk_lookup 帯外 or 距離超過
_ALLOWED = ("interp", "fk_nearest",
"fk_j4_priority", "fk_nearest_ee_y_tol_empty")
if r.get("method") not in _ALLOWED:
return False, "method={} は送信許可リスト外".format(r.get("method"))
xr = r.get("x_residual_m")
if xr is None or xr > X_RESID_THRESH:
return False, "x_resid={}m > X_RESID_THRESH ({})".format(xr, X_RESID_THRESH)
return True, None
def _do_move(target_angles, label, tol, log_record, *, dry_run,
abort_if_prev_not_reached=False):
"""target_angles へ移動し、結果を log_record["moves"] に追加。
dry_run=True の場合は送信せず計画のみ記録。
戻り値: (success: bool, post_actual: list | None)
C1 (DAY268 案R): abort_if_prev_not_reached=True が渡されたときだけ,
送信前に ABORT する. 呼び出し側 (stage6_lift / stage10_lift_back) が
log_record["moves"] から直前 DESCEND (stage4_pick / stage8_place) の arrived を
引いて渡す. 全 stage 到達時は False が渡り発火しない (案A の絶対差分方式で
正常な大移動を誤 ABORT した経路を解消).
"""
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,
}
# C1 (DAY268 案R): 直前 DESCEND が未到達なら, この LIFT を送らず ABORT.
# 未到達で姿勢がズレた状態から理論姿勢へ急送角するのを防ぐ. 呼び出し側が
# 直前 DESCEND の arrived を見て abort_if_prev_not_reached を決める.
# 全 stage 到達時は False が渡るので発火しない (絶対差分方式の誤 ABORT を解消).
if not dry_run and abort_if_prev_not_reached:
move_rec["arrived"] = False
move_rec["retried"] = False
move_rec["post_actual"] = list(pre_actual) if pre_actual is not None else None
move_rec["post_max_delta_deg"] = pre_max
move_rec["aborted_reason"] = "prev_descend_not_reached"
log_record.setdefault("moves", []).append(move_rec)
print("[MOVE ] {} ABORT (直前 DESCEND 未到達・送角せず)".format(label))
return False, (list(pre_actual) if pre_actual is not None else None)
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"]
# DAY265: fk_lookup の全軸帯 (ref_joints) 適用時は max_dj_deg と ee_y も残す.
if "max_dj_deg" in r and r.get("max_dj_deg") is not None:
rec["max_dj_deg"] = r["max_dj_deg"]
if "ee_y_m_selected" in r and r.get("ee_y_m_selected") is not None:
rec["ee_y_m_selected"] = r["ee_y_m_selected"]
if "joint_tol_deg_used" in r and r.get("joint_tol_deg_used") is not None:
rec["joint_tol_deg_used"] = r["joint_tol_deg_used"]
log_record.setdefault("lookups", []).append(rec)
# DAY265: place_withblock 経路 (ref_joints 使用) のときは max|Δjoint| と ee_y を表示.
extra = ""
if r.get("max_dj_deg") is not None:
extra = " max|Δj|={:.2f}°".format(r["max_dj_deg"])
if r.get("ee_y_m_selected") is not None:
extra += " ee_y={:.4f}m".format(r["ee_y_m_selected"])
# DAY265: out_of_range で sel=-1 のとき j0/x_resid/z_resid が None or inf のことがある.
# 従来 format {:+.3f} だと None で TypeError を出したのでヘルパで防御.
def _f3(v):
return "{:+.3f}".format(v) if isinstance(v, (int, float)) and v == v else "None"
def _f5(v):
return "{:.5f}".format(v) if isinstance(v, (int, float)) and v == v else "inf"
print("[LOOKUP] {} phase={} target_xz=({:+.4f},{:+.4f}) → j0={}° "
"method={} x_resid={}m z_resid={}m joint_ok={}{}".format(
label, r["phase"], target_xz[0], target_xz[1], _f3(r.get("j0")),
r["method"], _f5(r.get("x_residual_m")), _f5(r.get("z_residual_m")),
r["joint_limit_ok"], extra))
if r.get("method") == "out_of_range" and r.get("abort_reason"):
print("[LOOKUP] abort_reason: {}".format(r["abort_reason"]))
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,
camera_role="top"):
"""VLM 判定用に上カメラ or 横カメラから 1 フレーム取得して frames_dir/{run_stem}_{label}.jpg に保存.
camera_role="top" (既定): iPhone Continuity index (既存挙動・完全互換).
camera_role="side": Iriun index=1 (DAY265 追加. approach_place / place の横視点判定用).
run_stem が None なら旧仕様 {label}.jpg にフォールバック（後方互換）.
dry_run の場合は撮影せず None を返す.
戻り値: 保存パス or None（取得失敗時）"""
cap_rec = {"label": label, "image": None, "dry_run": bool(dry_run),
"camera_role": camera_role}
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
# camera_role に応じて resolve_cameras の要求を切り替え. side でも top を要求
# しないよう対称に. 未解決なら error 記録して None (通しは継続).
require_side_flag = (camera_role == "side")
require_top_flag = (camera_role == "top")
try:
cams = resolve_cameras(require_top=require_top_flag,
require_side=require_side_flag)
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
if camera_role == "side":
cam_idx = cams.get("side") if cams else None
role_name = "横カメラ"
else:
cam_idx = cams.get("top") if cams else None
role_name = "上カメラ"
if cam_idx is None:
print("[CAP-VLM] {} {}未解決".format(label, role_name))
cap_rec["error"] = "no_{}_camera".format(camera_role)
log_record.setdefault("vlm_captures", []).append(cap_rec)
return None
cap = cv2.VideoCapture(cam_idx)
if not cap.isOpened():
print("[CAP-VLM] {} cap open 失敗 (index={} role={})".format(
label, cam_idx, camera_role))
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)
# 後方互換のため top_camera_index キーは残しつつ、side は別キーで残す.
if camera_role == "side":
cap_rec["side_camera_index"] = int(cam_idx)
else:
cap_rec["top_camera_index"] = int(cam_idx)
print("[CAP-VLM] {} 保存 ({}): {}".format(label, role_name, out_path))
finally:
try:
cap.release()
except Exception:
pass
log_record.setdefault("vlm_captures", []).append(cap_rec)
return out_path
# ---- DAY272 案T: PLACE 再照準 (stage8.5) -----------------------------------
def _visual_servo_loop_place(cap, j0_current, target_x_mm,
servo_base_j1_j5, servo_rec):
"""PLACE 再照準の中核ループ. PICK の visual_servo_pick.visual_servo_loop を
PLACE 用に複製移植 (PICK 本線関数は 1 バイトも触らない・DAY272 案T).
j0 以外の 5 軸 (servo_base_j1_j5) は stage8 到達姿勢に固定. j0 だけ更新.
符号論理は PICK と同一:
diff_x_mm = target_x_mm - block_x_mm (目標 - 実測)
delta_deg = diff_x_mm / MM_PER_DEG_J0_X_PLACE (負値の係数)
j0_new = j0_current + delta_deg
PLACE 姿勢が j0=+44° 帯で PICK の j0=-30° 帯と逆符号側でも, 更新式は
幾何的に同じで通る (係数はモジュール定数側で姿勢依存差を吸収).
ガード:
- PLACE_CONVERGE_THRESHOLD_MM (=8.0mm) で収束
- PLACE_MAX_DELTA_DEG (=5.0°) で 1 ループ delta をクランプ
- PLACE_SERVO_MAX_ITER / PLACE_SERVO_MAX_SEC で強制収束
- PLACE_SERVO_DETECT_FAIL_MAX 連続検出失敗で再照準中止
校正観測:
各反復で delta_j0_actual (実測 j0 変化) と delta_block_x_actual
(実測 block_x 変化) を servo_rec["history"] に残す. 後段で
MM_PER_DEG_J0_X_PLACE を実測校正する材料.
戻り値: (final_j0, converged, loop_count, exit_reason)
"""
t_start = time.time()
prev_block_x_mm = None
consecutive_fail = 0
loop_count = 0
while True:
elapsed = time.time() - t_start
servo_rec["elapsed_sec"] = elapsed
# 時間ガード + ループガード (強制収束)
if elapsed > PLACE_SERVO_MAX_SEC:
return j0_current, False, loop_count, "servo_forced_converge_time"
if loop_count >= PLACE_SERVO_MAX_ITER:
return j0_current, False, loop_count, "servo_forced_converge_iter"
loop_count += 1
# 1. フレーム取得 + 保持ブロック検出.
ret, frame = cap.read()
block_xz = _detect_block_from_frame(frame) if ret else None
if block_xz is None:
consecutive_fail += 1
servo_rec["history"].append({
"iter": loop_count,
"note": "detect_fail (consecutive={})".format(consecutive_fail),
})
if consecutive_fail >= PLACE_SERVO_DETECT_FAIL_MAX:
return j0_current, False, loop_count, "detection_failed"
time.sleep(PLACE_SERVO_LOOP_WAIT_SEC)
continue
consecutive_fail = 0
# 2. diff 計算.
block_x_mm = float(block_xz[0]) * 1000.0
diff_x_mm = target_x_mm - block_x_mm
# 3. 収束判定 (唯一の自動成功停止条件).
if abs(diff_x_mm) <= PLACE_CONVERGE_THRESHOLD_MM:
servo_rec["history"].append({
"iter": loop_count,
"block_x_mm": round(block_x_mm, 2),
"target_x_mm": round(target_x_mm, 2),
"diff_x_mm": round(diff_x_mm, 2),
"j0_before": round(j0_current, 3),
"converged": True,
})
print("[SERVO] [CONVERGED] iter={} block_x={:+.1f} target={:+.1f} "
"diff={:+.1f}mm <= {}mm".format(
loop_count, block_x_mm, target_x_mm,
diff_x_mm, PLACE_CONVERGE_THRESHOLD_MM))
return j0_current, True, loop_count, "converged"
# 4. delta 計算 + クランプ (暴走保護・PICK は DAY249 で撤去だが PLACE は再導入).
delta_deg_raw = diff_x_mm / MM_PER_DEG_J0_X_PLACE
delta_deg = max(-PLACE_MAX_DELTA_DEG,
min(PLACE_MAX_DELTA_DEG, delta_deg_raw))
j0_new = j0_current + delta_deg
# 5. 送角 (j0 のみ変更・j1..j5 は stage8 到達姿勢固定).
new_angles = [j0_new] + list(servo_base_j1_j5)
j0_before_send = j0_current
try:
daemon_send_angles(new_angles, wait=2)
except Exception as e:
servo_rec["note"] = "daemon_send_angles 例外: {}".format(e)
return j0_current, False, loop_count, "send_angle_error"
time.sleep(PLACE_SERVO_LOOP_WAIT_SEC)
# 6. 実測 j0 (校正観測用).
measured = daemon_get_angles()
j0_measured = (measured[0]
if measured and len(measured) == 6 else None)
delta_j0_actual = (j0_measured - j0_before_send
if j0_measured is not None else None)
delta_block_x_actual = (block_x_mm - prev_block_x_mm
if prev_block_x_mm is not None else None)
# 7. 履歴に記録 (MM_PER_DEG_J0_X_PLACE 実測校正材料).
servo_rec["history"].append({
"iter": loop_count,
"block_x_mm": round(block_x_mm, 2),
"target_x_mm": round(target_x_mm, 2),
"diff_x_mm": round(diff_x_mm, 2),
"j0_before": round(j0_before_send, 3),
"delta_deg_raw": round(delta_deg_raw, 3),
"delta_deg_clamped": round(delta_deg, 3),
"j0_sent": round(j0_new, 3),
"j0_measured": (round(j0_measured, 3)
if j0_measured is not None else None),
"delta_j0_actual": (round(delta_j0_actual, 3)
if delta_j0_actual is not None else None),
"delta_block_x_actual": (round(delta_block_x_actual, 2)
if delta_block_x_actual is not None else None),
})
print("[SERVO] iter={} block_x={:+.1f} target={:+.1f} diff={:+.1f} "
"j0 {:+.2f}→{:+.2f} (raw={:+.2f} clamp={:+.2f})".format(
loop_count, block_x_mm, target_x_mm, diff_x_mm,
j0_before_send, j0_new, delta_deg_raw, delta_deg))
# DAY272 案T (拡張): 観測モード = 1 ループのみで即座に抜ける.
# 送角後の block_x を追加測定し「箱中心へ近づいた／遠ざかった」を verdict
# 出力. 目視で「箱接触なし」を並行確認する用. 既存収束ループ本体は無変更.
# 検出失敗・カメラ例外時は verdict 出せずに抜けるが exit は observe_once.
if PLACE_SERVO_OBSERVE_ONCE:
observe = {
"delta_j0_sent": round(delta_deg, 3),
"j0_before": round(j0_before_send, 3),
"j0_after_measured": (round(j0_measured, 3)
if j0_measured is not None else None),
"block_x_before_mm": round(block_x_mm, 2),
"target_x_mm": round(target_x_mm, 2),
"diff_before_mm": round(diff_x_mm, 2),
}
# 送角後の block_x を追加測定 (動作完了を待つため 1 ループ分 sleep).
time.sleep(PLACE_SERVO_LOOP_WAIT_SEC)
ret2, frame2 = cap.read()
block_xz_after = (_detect_block_from_frame(frame2)
if ret2 else None)
if block_xz_after is not None:
block_x_after_mm = float(block_xz_after[0]) * 1000.0
diff_after = target_x_mm - block_x_after_mm
abs_before = abs(diff_x_mm)
abs_after = abs(diff_after)
if abs_after < abs_before:
verdict = "箱中心へ近づいた (縮んだ・符号方向 OK)"
elif abs_after > abs_before:
verdict = ("箱中心から遠ざかった (広がった・符号反転の疑い"
"・MM_PER_DEG_J0_X_PLACE の符号確認要)")
else:
verdict = "変わらない (誤差内・要判断)"
observe.update({
"block_x_after_mm": round(block_x_after_mm, 2),
"diff_after_mm": round(diff_after, 2),
"abs_diff_change_mm": round(abs_after - abs_before, 2),
"verdict": verdict,
})
print("[SERVO] ============ 観測モード (OBSERVE_ONCE) ============")
print("[SERVO] 送角 delta_j0={:+.2f}° j0 {:+.2f}→{}"
.format(delta_deg, j0_before_send,
("{:+.2f}".format(j0_measured)
if j0_measured is not None else "?")))
print("[SERVO] block_x before={:+.1f}mm after={:+.1f}mm"
.format(block_x_mm, block_x_after_mm))
print("[SERVO] diff_x before={:+.1f}mm after={:+.1f}mm"
.format(diff_x_mm, diff_after))
print("[SERVO] |diff| before={:.1f}mm after={:.1f}mm "
"Δ={:+.1f}mm".format(
abs_before, abs_after, abs_after - abs_before))
print("[SERVO] verdict: {}".format(verdict))
print("[SERVO] ====================================================")
else:
observe["verdict"] = "送角後 block 検出失敗 (verdict 出せず)"
print("[SERVO] 観測モード: 送角後 block 検出失敗. "
"verdict 出せず.")
servo_rec["observe_once"] = observe
# 現在姿勢を final_j0 に反映して抜ける.
j0_current = (j0_measured if j0_measured is not None else j0_new)
return j0_current, False, loop_count, "observe_once"
# 8. 次ループへ状態更新.
j0_current = (j0_measured if j0_measured is not None else j0_new)
prev_block_x_mm = block_x_mm
def _run_place_visual_servo(box_xz, log_record, dry_run):
"""stage8 到達後の PLACE 再照準 (stage8.5) を実行する.
- stage8 到達姿勢の j0 と j1..j5 を moves から取り出し, サーボループへ渡す
- 上カメラを 1 回だけ open してループ内で毎反復 read
- servo_rec = log_record["servo_place"] に history / exit_reason 等を残す
- 制御は絶対止めない (失敗時は skip して stage9 へ)
- dry_run は送角しない
- 補正記憶とは独立 (計測モード PLACE_CORR_MEASURE_MODE=True でも動く実時間制御)
"""
servo_rec = {
"history": [],
"exit_reason": "",
"converged": False,
"iter_count": 0,
"elapsed_sec": 0.0,
"j0_start": None,
"j0_final": None,
"target_x_mm": None,
"note": "",
}
log_record["servo_place"] = servo_rec
if dry_run:
servo_rec["exit_reason"] = "dry_run"
servo_rec["note"] = "dry_run のため送角せず"
print("[SERVO] PLACE 再照準 dry_run (送角なし)")
return
# stage8 到達姿勢を moves から取得 (握り潰しなし・実測値のみ使用).
angles_after_stage8 = None
for m in reversed(log_record.get("moves") or []):
if str(m.get("label", "")).startswith("stage8_place"):
pa = m.get("post_actual")
if isinstance(pa, (list, tuple)) and len(pa) == 6:
angles_after_stage8 = list(pa)
break
if angles_after_stage8 is None:
servo_rec["exit_reason"] = "no_stage8_post_actual"
servo_rec["note"] = "stage8 の post_actual が取れず再照準スキップ"
print("[SERVO] {}".format(servo_rec["note"]))
return
j0_current = float(angles_after_stage8[0])
servo_base_j1_j5 = [float(v) for v in angles_after_stage8[1:]]
servo_rec["j0_start"] = round(j0_current, 3)
servo_rec["servo_base_j1_j5"] = [round(v, 3) for v in servo_base_j1_j5]
print("[SERVO] stage8 到達姿勢: j0={:+.2f} base_j1..j5={}"
.format(j0_current, [round(v, 2) for v in servo_base_j1_j5]))
# box_xz の有効性.
if not (isinstance(box_xz, (list, tuple)) and len(box_xz) == 2
and box_xz[0] is not None):
servo_rec["exit_reason"] = "no_box_xz"
servo_rec["note"] = "box_xz が取れず再照準スキップ"
print("[SERVO] {}".format(servo_rec["note"]))
return
target_x_mm = float(box_xz[0]) * 1000.0
servo_rec["target_x_mm"] = round(target_x_mm, 2)
print("[SERVO] target_x_mm={:+.2f} (box_xz[0]={:+.4f}m)"
.format(target_x_mm, float(box_xz[0])))
# 上カメラ open (1 回・ループ内で reuse).
try:
cams = resolve_cameras(require_top=True, require_side=False)
except Exception as e:
servo_rec["exit_reason"] = "resolver_error"
servo_rec["note"] = "camera_resolver 例外: {}".format(e)
print("[SERVO] {}".format(servo_rec["note"]))
return
top_idx = cams.get("top") if cams else None
if top_idx is None:
servo_rec["exit_reason"] = "no_top_camera"
servo_rec["note"] = "上カメラ未解決"
print("[SERVO] {}".format(servo_rec["note"]))
return
cap = cv2.VideoCapture(top_idx)
if not cap.isOpened():
servo_rec["exit_reason"] = "cap_open_fail"
servo_rec["note"] = "cap.open() 失敗 index={}".format(top_idx)
print("[SERVO] {}".format(servo_rec["note"]))
return
try:
# warm-up.
for _ in range(VLM_CAPTURE_WARMUP_FRAMES):
cap.read()
# サーボ実行.
(final_j0, converged, loop_count,
exit_reason) = _visual_servo_loop_place(
cap, j0_current, target_x_mm, servo_base_j1_j5, servo_rec)
servo_rec["exit_reason"] = exit_reason
servo_rec["converged"] = bool(converged)
servo_rec["iter_count"] = int(loop_count)
servo_rec["j0_final"] = round(final_j0, 3)
except Exception as e:
servo_rec["exit_reason"] = "loop_exception"
servo_rec["note"] = "servo_loop 例外: {}".format(e)
print("[SERVO] loop 例外: {}".format(e), file=sys.stderr)
finally:
try:
cap.release()
except Exception:
pass
print("[SERVO] PLACE 再照準 完了 converged={} iter={} exit={} "
"j0 {}→{} elapsed={:.2f}s".format(
servo_rec.get("converged"),
servo_rec.get("iter_count"),
servo_rec.get("exit_reason"),
servo_rec.get("j0_start"),
servo_rec.get("j0_final"),
servo_rec.get("elapsed_sec", 0.0)))
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 復帰 + グリッパー開.
DAY265: 失敗完走化. どちらの sub-step が失敗しても記録だけ残して True を返し、
通しを止めない. arrived 情報は _do_move が log_record.moves に記録済み.
"""
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:
log_record.setdefault("stage_failures", []).append(
{"stage": 1, "label": "stage1_to_home", "kind": "not_reached"})
print("[STAGE 1] HOME 未到達. 記録のみ残して継続")
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] グリッパー開失敗. 記録のみ残して継続")
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,
ref_joints=None, joint_tol_deg=None,
target_ee_y_m=None):
"""lookup → 送信前ガード → 移動。共通段（段3/4/7/8）.
DAY262: j3_ref / j5_ref を受け place_withblock のとき fk_lookup へ転送していたが、
DAY265 で全軸帯 (ref_joints/joint_tol_deg) に統合. place_withblock は ref_joints
経路を使い、j3_ref/j5_ref の受け口は残す (他の用途で使えるように・PICK には無影響).
他の phase (pick / approach_*) では使わない (PICK 回帰ゼロ担保).
DAY274 層3: target_ee_y_m (m) を受け, place_withblock のとき fk_lookup へ転送する
(j4 優先スコア・案X). None なら従来 (xz 最近傍・bit-perfect 一致).
戻り値: (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:
# DAY265: place_withblock は箱高さ帯 + 全軸帯 (approach 連続性) +
# ee_y 下限ガードで候補を絞る. j3_ref/j5_ref は全軸帯に包含されるため
# stage8 呼び出し側からは渡さない (default None).
# pick は fk_lookup.py 側の EE_Y_MIN/MAX_DEFAULT (10-40mm) を使い、
# ref_joints も渡さない (=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,
ref_joints=ref_joints,
joint_tol_deg=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)
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:
# DAY265: 到達失敗しても継続. arrived の真偽は _do_move が log_record.moves
# の arrived に記録済み. 失敗事実だけ stage_failures / rec に残して True 返し.
rec["arrived_ok"] = False
log_record.setdefault("stage_failures", []).append(
{"stage": stage_no, "label": label, "kind": "not_reached"})
print("[STAGE {}] 到達失敗 (label={}). 記録のみで次 stage へ".format(
stage_no, label))
return True, r
rec["arrived_ok"] = True
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)
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()
# 段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_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(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))
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 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()
# ループ次イテレーション
# 既存キー互換: 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")
# 段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)
# 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 帯中央
# target_ee_y は None で「層3 無効」・dee_y>0 で有効化. 焼き付け回避.
_target_ee_y_m = (_base_ee_y_m + _dee_y_mm / 1000.0
if _dee_y_mm > 0.0 else None)
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 = ""
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,
}
_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,
ref_joints=_ref_joints_place,
joint_tol_deg=PLACE_JOINT_TOL_DEG,
target_ee_y_m=_target_ee_y_m)
# 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")
_run_place_visual_servo(box_xz, log_record, dry_run)
# 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")
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")))
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)
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()
# 判定②: 段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)
# 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_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"]))
# 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
# 段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 検出。ログを保存して終了")
return False, log_path
finally:
# DAY265: 自動 HOME 復帰は無効化. エイイチが目視で判断して手動で HOME に戻す.
# 途中失敗でも通しを完走させ 1 行追記させるため、finally での自動送信は行わない.
log_record["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:
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()fk_lookup.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""fk_lookup.py — FK テーブル全引きで 6 関節を返す lookup (代表抽出なし・runtime 検索)
j0_lookup は j0 のみ補間で j1..j5 を採用 JSON の base_angles に固定する。
fk_lookup はそれを捨て、ブロックの (x,z) ごとに FK テーブルから 6 関節すべてを
runtime で引く。代表姿勢の事前抽出はしない (毎回 帯フィルタ後の全点から探す)。
入力:
phase: 呼び出し元の識別子 (lookup ロジック上は分岐しない)
target_xz: (unity_x, unity_z) [m]
ee_y_min, ee_y_max: 掴める高さ帯 [m] (デフォルト 0.010, 0.040)
dist_max_m: 最近傍までの x-z 距離がこれを超えたら out_of_range [m] (デフォルト 0.030)
j3_ref: j3 基準値 [度]. 指定時は j3_ref±j3_tol_deg の帯で候補を絞る (DAY262).
None なら j3 制約なし (PICK 経路の既定・回帰ゼロ担保).
j3_tol_deg: j3 帯許容 [度]. デフォルト 5.0.
j5_ref: j5 基準値 [度]. 指定時は j5_ref±j5_tol_deg の帯で候補を絞る (DAY262).
None なら j5 制約なし. j3 帯と同型 (PICK 回帰ゼロ担保).
j5_tol_deg: j5 帯許容 [度]. デフォルト 5.0.
戻り値: j0_lookup と互換のキー構成 + used_angles / selected_idx / ee_y_band_m / dist_m。
絶対ルール:
- FK CSV は読み取り専用。台帳・採用 JSON も触らない。
- daemon / send_angles / send_coords / release_all_servos を呼ばない。
- IK を解かない。実在 FK 行を選ぶだけ。
- 返す 6 関節を JOINT_LIMITS の hard 範囲 + j1 <= J1_SAFE_MAX で検査。
descend 限定で使う運用に切替えたため (DAY257 後段)、j1 安全マージンを復活し、
silent drop の温床となる j1>88° の姿勢は候補から除外する。
- 帯外 / 距離超過は method="out_of_range" を返し joint_limit_ok=False で
_check_guard が ABORT できるようにする。
公開 API:
fk_lookup(phase, target_xz, ee_y_min=..., ee_y_max=..., dist_max_m=...) -> dict
CLI:
python3 fk_lookup.py --phase pick --x 0.196 --z 0.252
"""
from __future__ import print_function
import argparse
import json
import sys
import threading
import types
from pathlib import Path
import numpy as np
# ---- パス解決 -----------------------------------------------------------------
_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 の JOINT_LIMITS 参照のため) -----------
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(
"fk_lookup.py: MyCobot stub is not callable. "
"fk_lookup は JOINT_LIMITS 参照のためだけに waypoint_replay を import します。")
_mod.MyCobot = _MyCobotStub
sys.modules["pymycobot"] = _pkg
sys.modules["pymycobot.mycobot"] = _mod
import waypoint_replay as wr # noqa: E402
# ---- 定数 ---------------------------------------------------------------------
FK_CSV = _SCRIPTS_ROOT / "fk_scan_motion_based_v2.csv"
EE_Y_MIN_DEFAULT = 0.010 # m (10mm)
EE_Y_MAX_DEFAULT = 0.040 # m (40mm) 床近く帯
DIST_MAX_DEFAULT = 0.030 # m (30mm) 最近傍まで
J1_SAFE_MAX = 88.0 # j1 安全マージン deg (DAY257 後段で復活)
# DAY274 案B': PLACE 降下の前腕姿勢下限 (箱側面激突防御).
# fk_nearest が (ee_x, ee_z) 距離だけで選ぶと、前腕を寝かせた低 j1 姿勢が勝ち、
# 爪先 (ee) は高くても前腕が箱側面を擦る (今日 j1=46.65・ee_y=105.3mm の当たる姿勢).
# 弁別軸は ee_y ではなく j1 (前腕の空間占有). DAY235 place_withblock base j1=59.15°
# (当たらない安全姿勢) から余裕 5° を引いた 55° を下限に. 呼び出し側 (run_pick_place)
# で phase=="place_withblock" のときだけ渡す. pick 経路には影響しない (PICK 回帰ゼロ).
PLACE_J1_MIN_DEG_DEFAULT = 55.0
# DAY274 層3 (案X): j4 優先スコアの重み (place_withblock 限定).
# 目標 ee_y ± PLACE_EE_Y_TOL_M の候補に対し
# score = W_XZ_DIST * xz_dist + W_J1_PENALTY*|Δj1| + W_J2_PENALTY*|Δj2| + W_J4_PENALTY*|Δj4|
# を最小化. w_j1 > w_j2 > w_j4 で「同じ ee_y なら j4 で稼いだ姿勢」が勝つ.
# 初期値は保守的. 実機のラチェット学習で自己調整される前提 (焼き付けない).
# xz_dist は m 単位 (0.02m=20mm が GUARD), penalty 側の |Δj*| は度単位で入る.
# 主項 (xz) を優先しつつ, 同 xz 位置での姿勢配分を j4 に寄せる意図.
PLACE_W_XZ_DIST_DEFAULT = 1.0
PLACE_W_J1_PENALTY_DEFAULT = 0.3 # j1 動かす姿勢へ最大ペナルティ
PLACE_W_J2_PENALTY_DEFAULT = 0.1 # j2 中
PLACE_W_J4_PENALTY_DEFAULT = 0.02 # j4 最小 (優先して使う)
PLACE_EE_Y_TOL_M_DEFAULT = 0.005 # 目標 ee_y の許容帯 (±5mm)
J3_TOL_DEG_DEFAULT = 5.0 # j3 帯許容 deg (DAY262: PLACE 手首捻り抑制. j3_ref が
# None のとき無効・PICK 回帰ゼロ担保)
J5_TOL_DEG_DEFAULT = 5.0 # j5 帯許容 deg (DAY262: PLACE grip-axis 保持. j5_ref が
# None のとき無効・PICK 回帰ゼロ担保・j3 帯と同型)
# DAY265: 全軸帯フィルタ (approach 姿勢からの連続性担保) と ee_y 下限ガード の既定値.
# joint_tol_deg は「コードに焼かない」方針で fk_lookup 側デフォルトは None (呼び出し側必須).
# PLACE_EE_Y_FLOOR は place 系の白箱激突二重防御 (呼び出し側で明示的に渡すときのみ有効).
# 運用安全マージン (j1<=88°)。waypoint_replay の hard limit (123°) より厳しい。
# J1_SAFE_MAX: descend で fk_lookup を使う運用 (DAY257 後段) で復活。
# 88°を超える姿勢は silent drop の温床。hard limit (123°) と併用し、
# 候補集合から事前に j1>88° を弾く。
VALID_PHASES = ("approach_pick", "pick", "approach_place",
"place_withblock", "place_noblock")
# ---- FK テーブル lazy キャッシュ (スレッドセーフ・1度だけロード) ------------
# safe_mask: JOINT_LIMITS hard 範囲 + j1<=J1_SAFE_MAX を満たす行の bool マスク。
# DAY257 後段の descend 限定運用で j1 safety を復活させた。ロード時に 1 度だけ
# 計算し、以降の lookup では帯マスクと AND するだけ。
_FK_CACHE = {"loaded": False, "joints": None, "ees": None, "safe_mask": None}
_FK_LOCK = threading.Lock()
def _load_fk_table():
"""FK CSV を一度だけメモリにロードする。
return: (joints[N,6], ees[N,3], safe_mask[N]) joints/ees は np.float64、
safe_mask は JOINT_LIMITS hard 範囲 + j1<=J1_SAFE_MAX を満たす行のみ True
(descend 限定運用 DAY257 後段で j1 safety を復活)。
"""
if _FK_CACHE["loaded"]:
return _FK_CACHE["joints"], _FK_CACHE["ees"], _FK_CACHE["safe_mask"]
with _FK_LOCK:
if _FK_CACHE["loaded"]:
return _FK_CACHE["joints"], _FK_CACHE["ees"], _FK_CACHE["safe_mask"]
if not FK_CSV.exists():
raise FileNotFoundError(
"FK テーブルが見つかりません: {}".format(FK_CSV))
arr = np.loadtxt(FK_CSV, delimiter=",", skiprows=1, dtype=np.float64)
joints = arr[:, 3:9].copy()
ees = arr[:, 0:3].copy()
# JOINT_LIMITS hard 範囲を全軸 + j1 safety マージン を事前計算
# (1 度だけ・以降は再利用)。
safe = np.ones(len(joints), dtype=bool)
for ax, (lo, hi) in enumerate(wr.JOINT_LIMITS):
safe &= (joints[:, ax] >= lo) & (joints[:, ax] <= hi)
safe &= (joints[:, 1] <= J1_SAFE_MAX)
_FK_CACHE["joints"] = joints
_FK_CACHE["ees"] = ees
_FK_CACHE["safe_mask"] = safe
_FK_CACHE["loaded"] = True
return joints, ees, safe
def _check_limits(joints):
"""JOINT_LIMITS hard 範囲 + j1<=J1_SAFE_MAX を検査 (DAY257 後段で safety 復活)。
return: [(axis, value, lo, hi, kind), ...] 違反リスト (空なら OK)
"""
out = []
for ax, val, lo, hi in wr._check_joint_limits(joints):
out.append((int(ax), float(val), float(lo), float(hi), "hard"))
if joints[1] > J1_SAFE_MAX:
out.append((1, float(joints[1]),
float(wr.JOINT_LIMITS[1][0]), float(J1_SAFE_MAX),
"safety(j1<={:.0f})".format(J1_SAFE_MAX)))
return out
def _out_of_range_result(phase, target_xz, ee_y_min, ee_y_max, reason,
dist_m=float("inf"), sel=-1, ee_sel=None,
joints_sel=None, violations=None):
"""送角不可の戻り値を組み立てる (joint_limit_ok=False で _check_guard が ABORT する)"""
j = list(joints_sel) if joints_sel is not None else [None] * 6
ee = (list(ee_sel) if ee_sel is not None else [None, None, None])
return {
"phase": phase,
"source_file": FK_CSV.name,
"pose_id": ("fk_row_{}".format(sel) if sel >= 0 else None),
"j0": (j[0] if j[0] is not None else None),
"base_angles": j,
"target_angles": j,
"used_angles": j,
"method": "out_of_range",
"x_residual_m": (abs(ee[0] - float(target_xz[0]))
if ee[0] is not None else float("inf")),
"z_residual_m": (abs(ee[2] - float(target_xz[1]))
if ee[2] is not None else float("inf")),
"interp_xz_m": ([ee[0], ee[2]] if ee[0] is not None else [None, None]),
"nearest_pts": ([
{"ee_xz_m": [ee[0], ee[2]], "ee_y_m": ee[1], "joints": j}
] if ee[0] is not None else []),
"joint_limit_ok": False,
"joint_limit_violations": [
{"axis": ax, "value": v, "lo": lo, "hi": hi, "kind": k}
for ax, v, lo, hi, k in (violations or [])
],
"ee_y_band_m": [ee_y_min, ee_y_max],
"selected_idx": sel,
"dist_m": dist_m,
"abort_reason": reason,
}
# ---- 公開 API -----------------------------------------------------------------
def fk_lookup(phase, target_xz,
ee_y_min=EE_Y_MIN_DEFAULT,
ee_y_max=EE_Y_MAX_DEFAULT,
dist_max_m=DIST_MAX_DEFAULT,
j3_ref=None,
j3_tol_deg=J3_TOL_DEG_DEFAULT,
j5_ref=None,
j5_tol_deg=J5_TOL_DEG_DEFAULT,
ref_joints=None,
joint_tol_deg=None,
place_ee_y_floor=None,
place_j1_min_deg=None,
target_ee_y_m=None,
place_ee_y_tol_m=PLACE_EE_Y_TOL_M_DEFAULT,
w_xz_dist=PLACE_W_XZ_DIST_DEFAULT,
w_j1_penalty=PLACE_W_J1_PENALTY_DEFAULT,
w_j2_penalty=PLACE_W_J2_PENALTY_DEFAULT,
w_j4_penalty=PLACE_W_J4_PENALTY_DEFAULT):
"""FK テーブル全引きで 6 関節を返す。j0_lookup と互換の dict 構成。
DAY262: j3_ref を指定すると j3_ref±j3_tol_deg の帯でも候補を絞る (手首捻り抑制).
j5_ref も同型で j5 帯フィルタを追加 (grip-axis 保持).
どちらも None (デフォルト) のときは制約なし＝PICK 既定・従来コードと bit-perfect
に一致 (回帰ゼロ担保). 非数値 (str/list 等) が来たら型ガードで None に落として
従来挙動へ.
DAY265: ref_joints (6 要素・度) と joint_tol_deg (スカラー・度) を指定すると、
全 6 軸帯フィルタ (|joints[:,i] - ref_joints[i]| <= joint_tol_deg for all i) を
追加で AND する. approach_place から連続する姿勢のみを候補にして、j1/j4 に
歪みが逃げるのを防ぐ. ref_joints=None (デフォルト) は無効・PICK 回帰ゼロ.
joint_tol_deg のデフォルトは None (コード焼き付けなし・呼び出し側必須).
place_ee_y_floor (m) を指定すると、選択された最終 1 点の ee_y がそれ未満のとき
out_of_range を返す (白箱激突二重防御・PLACE 呼び出しのみ・PICK には渡さない).
DAY274 案B': place_j1_min_deg (度) を指定すると、候補集合を j1 >= place_j1_min_deg
でさらに絞る (箱側面激突防御・前腕を寝かせた低 j1 姿勢を締め出す). 呼び出し側は
place_withblock のときだけ渡す. None のときは制約なし (PICK 経路は影響なし・
従来コードと bit-perfect 一致).
DAY274 層3 (案X): target_ee_y_m (m) を指定すると、上記フィルタ後の候補を
|ee_y - target_ee_y_m| <= place_ee_y_tol_m でさらに絞り, ref_joints を base 姿勢
として重み付きスコア (xz_dist + j1/j2/j4 の |Δ| ペナルティ) を最小化する点を
選ぶ. w_j1 > w_j2 > w_j4 で「同じ ee_y なら j4 で稼いだ姿勢」が勝つ.
target_ee_y_m=None (デフォルト) は従来の xz 最近傍のまま (bit-perfect 一致).
j4 の安全 (JOINT_LIMITS + J1_SAFE_MAX) は前段の safe_mask で担保. J4_SAFE 範囲は
呼び出し側 (run_pick_place) の事前検査で追加確認する.
戻り値 dict のキー (j0_lookup 互換 + 追加メタ):
phase, source_file, pose_id, j0,
base_angles, target_angles, used_angles, # すべて選んだ FK 行の 6 関節
method ("fk_nearest" | "out_of_range"),
x_residual_m, z_residual_m, interp_xz_m,
nearest_pts ([{ee_xz_m, ee_y_m, joints}]),
joint_limit_ok, joint_limit_violations,
ee_y_band_m, selected_idx, dist_m,
(out_of_range 時のみ) abort_reason
"""
if phase not in VALID_PHASES:
raise ValueError(
"未知の phase: {!r} (許可: {})".format(phase, VALID_PHASES))
if (not isinstance(target_xz, (list, tuple))
or len(target_xz) != 2):
raise ValueError("target_xz は (x, z) の 2 要素タプル/リスト")
tx = float(target_xz[0]); tz = float(target_xz[1])
if ee_y_min >= ee_y_max:
raise ValueError(
"ee_y_min ({}) >= ee_y_max ({}) は不正".format(ee_y_min, ee_y_max))
# DAY262: j3_ref の型ガード. 非数値 (str/list/dict 等) は無視して従来挙動に倒す
# (approach_place の target_angles[3] が万一欠損でも fk_lookup を汚さない).
j3_ref_f = None
j3_tol_f = None
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
j3_tol_f = None
# DAY262: j5_ref も同じ型ガード (j3 と対称).
j5_ref_f = None
j5_tol_f = None
if j5_ref is not None:
try:
j5_ref_f = float(j5_ref)
j5_tol_f = float(j5_tol_deg)
except (TypeError, ValueError):
j5_ref_f = None
j5_tol_f = None
# DAY265: ref_joints (6 要素) と joint_tol_deg の型ガード. 片方でも非数値/欠損なら
# 両方 None に落として全軸帯フィルタをスキップ (安全側フォールバック).
ref_joints_arr = None
joint_tol_f = None
if ref_joints is not None:
try:
_rj_list = list(ref_joints)
if len(_rj_list) == 6:
_rj_floats = [float(v) for v in _rj_list]
_tol_f = float(joint_tol_deg) if joint_tol_deg is not None else None
if _tol_f is not None and _tol_f > 0:
ref_joints_arr = np.array(_rj_floats, dtype=np.float64)
joint_tol_f = _tol_f
except (TypeError, ValueError):
ref_joints_arr = None
joint_tol_f = None
# DAY265: place_ee_y_floor の型ガード.
place_ee_y_floor_f = None
if place_ee_y_floor is not None:
try:
place_ee_y_floor_f = float(place_ee_y_floor)
except (TypeError, ValueError):
place_ee_y_floor_f = None
# DAY274 案B': place_j1_min_deg の型ガード. 非数値なら None に落として制約なし.
place_j1_min_f = None
if place_j1_min_deg is not None:
try:
place_j1_min_f = float(place_j1_min_deg)
except (TypeError, ValueError):
place_j1_min_f = None
# DAY274 層3: target_ee_y_m と重み群の型ガード. 非数値/None なら層3 は無効
# (従来の xz 最近傍・bit-perfect 一致). 重みは呼び出し側で明示指定不要 (既定値).
target_ee_y_f = None
if target_ee_y_m is not None:
try:
target_ee_y_f = float(target_ee_y_m)
except (TypeError, ValueError):
target_ee_y_f = None
place_ee_y_tol_f = None
try:
place_ee_y_tol_f = float(place_ee_y_tol_m)
except (TypeError, ValueError):
place_ee_y_tol_f = None
try:
w_xz_dist_f = float(w_xz_dist)
w_j1_pen_f = float(w_j1_penalty)
w_j2_pen_f = float(w_j2_penalty)
w_j4_pen_f = float(w_j4_penalty)
except (TypeError, ValueError):
# 重みが壊れていたら層3 無効化 (安全側).
target_ee_y_f = None
joints, ees, safe_mask = _load_fk_table()
# 高さ帯フィルタ (半開区間: min<=ee_y<max) と JOINT_LIMITS+j1 安全マスクを
# AND して、「届く高さ帯 × 送れる安全姿勢」だけを候補集合にする。
# この絞り込みを 2D 最近傍より前にやることで、j1=110° のような届かない行を
# そもそも候補に含めない (送角寸前で ABORT する事故を防ぐ)。
# DAY262: j3_ref_f が指定されているときはさらに j3 帯 AND を追加する
# (approach_place の j3≈0 を保って PLACE descend の手首捻りを抑える).
mask_y = (ees[:, 1] >= ee_y_min) & (ees[:, 1] < ee_y_max)
mask = mask_y & safe_mask
n_before_j3 = int(mask.sum()) # abort message 用: j3 で絞る前の safe∩ee_y
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_j3
# DAY262: j5 帯 (j3 の直後に AND. 順序は AND なので可換だが j3→j5 で
# abort_reason の情報が分かりやすい).
n_before_j5 = int(mask.sum())
if j5_ref_f is not None:
mask_j5 = ((joints[:, 5] >= j5_ref_f - j5_tol_f)
& (joints[:, 5] <= j5_ref_f + j5_tol_f))
mask = mask & mask_j5
# DAY265: 全軸帯フィルタ (approach 姿勢からの連続性担保). ref_joints/joint_tol_deg が
# 揃っているときのみ. numpy broadcast: joints[N,6] - ref[6] → [N,6] → abs → all(axis=1).
n_before_ref = int(mask.sum())
if ref_joints_arr is not None and joint_tol_f is not None:
mask_ref = np.all(np.abs(joints - ref_joints_arr) <= joint_tol_f, axis=1)
mask = mask & mask_ref
# DAY274 案B': place_j1_min_deg (前腕姿勢下限) を最後に AND. place_withblock で
# 呼ばれたときだけ非 None. pick 経路には影響しない.
n_before_j1min = int(mask.sum())
if place_j1_min_f is not None:
mask_j1min = (joints[:, 1] >= place_j1_min_f)
mask = mask & mask_j1min
n_band = int(mask.sum())
if n_band == 0:
n_y_only = int(mask_y.sum())
# abort_reason 文言に j3/j5/全軸/j1_min 帯情報を追記 (どこで落ちたかを切り分ける).
band_parts = "JOINT_LIMITS(hard) ∩ j1<={:.0f}".format(J1_SAFE_MAX)
extra_msg = "帯のみだと {} 行・safe で {} 行".format(
n_y_only, n_before_j3)
if j3_ref_f is not None:
band_parts += " ∩ j3[{:+.1f}±{:.1f}]".format(j3_ref_f, j3_tol_f)
extra_msg += "・j3 後 {} 行".format(n_before_j5)
if j5_ref_f is not None:
band_parts += " ∩ j5[{:+.1f}±{:.1f}]".format(j5_ref_f, j5_tol_f)
extra_msg += "・j5 後 {} 行".format(n_before_ref)
if ref_joints_arr is not None and joint_tol_f is not None:
band_parts += " ∩ 全軸[ref±{:.1f}]".format(joint_tol_f)
extra_msg += "・全軸帯後 {} 行".format(n_before_j1min)
if place_j1_min_f is not None:
band_parts += " ∩ j1>={:.1f}".format(place_j1_min_f)
extra_msg += "・j1_min 後 0 行 (no_candidate_above_j1_min)"
elif ref_joints_arr is not None and joint_tol_f is not None:
extra_msg += "・全軸帯後 0 行"
elif j5_ref_f is not None or j3_ref_f is not None:
extra_msg += "・最終 0 行"
else:
extra_msg = "帯のみだと {} 行・safe 集合で絞ると 0".format(n_y_only)
reason_msg = (
"ee_y band [{:.4f},{:.4f}) ∩ {} に行ゼロ ({})".format(
ee_y_min, ee_y_max, band_parts, extra_msg))
return _out_of_range_result(
phase, (tx, tz), ee_y_min, ee_y_max, reason_msg)
band_idx = np.where(mask)[0]
band_ees = ees[band_idx]
band_joints = joints[band_idx]
# DAY274 層3 (案X): target_ee_y_f が指定されているときは, さらに
# |ee_y - target_ee_y_f| <= place_ee_y_tol_f で絞り, base 姿勢 (ref_joints)
# を基準とした重み付きスコアを最小化する点を選ぶ.
# 従来 (target_ee_y_f=None) は xz 最近傍のまま (bit-perfect 一致).
method_str = "fk_nearest"
used_target_ee_y = None
if (target_ee_y_f is not None and place_ee_y_tol_f is not None
and ref_joints_arr is not None and len(band_idx) > 0):
used_target_ee_y = target_ee_y_f
# ee_y 帯フィルタ (target 中心 ±tol)
mask_ey_tol = (np.abs(band_ees[:, 1] - target_ee_y_f)
<= place_ee_y_tol_f)
if int(mask_ey_tol.sum()) > 0:
sub_ees = band_ees[mask_ey_tol]
sub_joints = band_joints[mask_ey_tol]
sub_idx = band_idx[mask_ey_tol]
xz_dist = np.sqrt((sub_ees[:, 0] - tx) ** 2
+ (sub_ees[:, 2] - tz) ** 2)
# base 姿勢 (ref_joints) からの |Δj1|, |Δj2|, |Δj4| (度)
dj1 = np.abs(sub_joints[:, 1] - ref_joints_arr[1])
dj2 = np.abs(sub_joints[:, 2] - ref_joints_arr[2])
dj4 = np.abs(sub_joints[:, 4] - ref_joints_arr[4])
score = (w_xz_dist_f * xz_dist
+ w_j1_pen_f * dj1
+ w_j2_pen_f * dj2
+ w_j4_pen_f * dj4)
best_local = int(np.argmin(score))
sel = int(sub_idx[best_local])
dist_m = float(xz_dist[best_local])
method_str = "fk_j4_priority"
else:
# ee_y 帯内候補ゼロ → 従来 xz 最近傍にフォールバック (安全側).
# abort ではなく "帯外のまま xz 最近傍を選ぶ" のは, target_ee_y が
# 過剰に上がった場合の劣化を段階的に. 呼び出し側で ee_y 実測を確認可能.
dxz2 = (band_ees[:, 0] - tx) ** 2 + (band_ees[:, 2] - tz) ** 2
best_in_band = int(np.argmin(dxz2))
sel = int(band_idx[best_in_band])
dist_m = float(np.sqrt(dxz2[best_in_band]))
method_str = "fk_nearest_ee_y_tol_empty"
else:
# 従来: x-z 平面の 2D 距離 (運用上は 30mm 以内に最近傍があるかどうかで判定).
dxz2 = (band_ees[:, 0] - tx) ** 2 + (band_ees[:, 2] - tz) ** 2
best_in_band = int(np.argmin(dxz2))
sel = int(band_idx[best_in_band])
dist_m = float(np.sqrt(dxz2[best_in_band]))
ee_sel = ees[sel].tolist()
j_sel = [float(v) for v in joints[sel]]
# JOINT_LIMITS + j1 安全マージン
violations = _check_limits(j_sel)
joint_limit_ok = (len(violations) == 0)
# 距離超過 → out_of_range (joint_limit_ok=False にして _check_guard で ABORT)
if dist_m > dist_max_m:
return _out_of_range_result(
phase, (tx, tz), ee_y_min, ee_y_max,
"x-z 最近傍距離 {:.4f}m > {:.4f}m (DIST_MAX 超過)".format(
dist_m, dist_max_m),
dist_m=dist_m, sel=sel, ee_sel=ee_sel,
joints_sel=j_sel, violations=violations)
# DAY265: ee_y 下限ガード (place 系の白箱激突二重防御).
# place_ee_y_floor が明示的に渡されているときのみ発火 (PICK には影響しない).
# 帯フィルタが正しければ発火しないはずの保険.
if place_ee_y_floor_f is not None and ee_sel[1] < place_ee_y_floor_f:
return _out_of_range_result(
phase, (tx, tz), ee_y_min, ee_y_max,
"選択点 ee_y={:.4f}m < place_ee_y_floor={:.4f}m (白箱激突防御)".format(
ee_sel[1], place_ee_y_floor_f),
dist_m=dist_m, sel=sel, ee_sel=ee_sel,
joints_sel=j_sel, violations=violations)
# DAY265: ref_joints からの max|Δjoint| を計算 (ログ用. ref なしなら None).
max_dj_deg = None
if ref_joints_arr is not None:
max_dj_deg = float(np.max(np.abs(np.array(j_sel) - ref_joints_arr)))
# JOINT_LIMITS 違反 → joint_limit_ok=False で _check_guard が拾う
# (method は "fk_nearest" / "fk_j4_priority" / "fk_nearest_ee_y_tol_empty".
# ガード側のメッセージで違反を出す)
return {
"phase": phase,
"source_file": FK_CSV.name,
"pose_id": "fk_row_{}".format(sel),
"j0": j_sel[0],
"base_angles": list(j_sel),
"target_angles": list(j_sel),
"used_angles": list(j_sel),
"method": method_str,
"x_residual_m": abs(ee_sel[0] - tx),
"z_residual_m": abs(ee_sel[2] - tz),
"interp_xz_m": [ee_sel[0], ee_sel[2]],
"nearest_pts": [
{"ee_xz_m": [ee_sel[0], ee_sel[2]],
"ee_y_m": ee_sel[1],
"joints": list(j_sel)},
],
"joint_limit_ok": joint_limit_ok,
"joint_limit_violations": [
{"axis": ax, "value": v, "lo": lo, "hi": hi, "kind": k}
for ax, v, lo, hi, k in violations
],
"ee_y_band_m": [ee_y_min, ee_y_max],
"selected_idx": sel,
"dist_m": dist_m,
# DAY265: 全軸帯フィルタ適用時のメタ (ref なしなら None). ログ・観測用.
"ref_joints_used": (list(ref_joints_arr.tolist())
if ref_joints_arr is not None else None),
"joint_tol_deg_used": joint_tol_f,
"max_dj_deg": max_dj_deg,
"ee_y_m_selected": ee_sel[1],
# DAY274 案B': 前腕姿勢下限 (place_withblock で有効. None なら未適用).
"place_j1_min_deg_used": place_j1_min_f,
"j1_selected_deg": j_sel[1],
# DAY274 層3 (案X): 目標 ee_y と選ばれた姿勢の j4 (ログ用. None なら未適用).
"target_ee_y_m_used": used_target_ee_y,
"j4_selected_deg": j_sel[4],
}
# ---- CLI ----------------------------------------------------------------------
def main():
ap = argparse.ArgumentParser(description="FK 全引き lookup (代表抽出なし)")
ap.add_argument("--phase", choices=list(VALID_PHASES), required=True)
ap.add_argument("--x", type=float, required=True, help="target unity_x [m]")
ap.add_argument("--z", type=float, required=True, help="target unity_z [m]")
ap.add_argument("--ee-y-min", type=float, default=EE_Y_MIN_DEFAULT,
help="ee_y 下限 [m] (デフォルト {})".format(EE_Y_MIN_DEFAULT))
ap.add_argument("--ee-y-max", type=float, default=EE_Y_MAX_DEFAULT,
help="ee_y 上限 [m] (デフォルト {})".format(EE_Y_MAX_DEFAULT))
ap.add_argument("--dist-max-m", type=float, default=DIST_MAX_DEFAULT,
help="最近傍距離上限 [m] (デフォルト {})".format(DIST_MAX_DEFAULT))
# DAY262: j3 帯フィルタ CLI (単体で j3 制約時の候補数/最近傍/選ばれた j3 を確認可能).
ap.add_argument("--j3-ref", type=float, default=None,
help="j3 基準 (度). 指定すると j3_ref±j3_tol_deg の帯で候補を絞る. "
"デフォルト None=絞らない (PICK 既定)")
ap.add_argument("--j3-tol-deg", type=float, default=J3_TOL_DEG_DEFAULT,
help="j3 帯許容 (度) (デフォルト {})".format(J3_TOL_DEG_DEFAULT))
# DAY262: j5 帯フィルタ CLI (j3 と同型).
ap.add_argument("--j5-ref", type=float, default=None,
help="j5 基準 (度). 指定すると j5_ref±j5_tol_deg の帯で候補を絞る. "
"デフォルト None=絞らない (PICK 既定)")
ap.add_argument("--j5-tol-deg", type=float, default=J5_TOL_DEG_DEFAULT,
help="j5 帯許容 (度) (デフォルト {})".format(J5_TOL_DEG_DEFAULT))
# DAY265: 全軸帯フィルタ + ee_y 下限ガード CLI.
def _parse_ref_joints(s):
parts = s.split(",")
if len(parts) != 6:
raise argparse.ArgumentTypeError(
"--ref-joints は 6 要素・カンマ区切り (got {} 要素)".format(len(parts)))
try:
return [float(x) for x in parts]
except ValueError as e:
raise argparse.ArgumentTypeError(
"--ref-joints の各要素は数値: {}".format(e))
ap.add_argument("--ref-joints", type=_parse_ref_joints, default=None,
help="全軸帯フィルタの基準姿勢 6 要素 (度・カンマ区切り). "
"例: '43.52,57.39,17.49,0.08,-89.20,-29.44'. "
"指定時は --joint-tol-deg 必須.")
ap.add_argument("--joint-tol-deg", type=float, default=None,
help="全軸帯許容 (度). --ref-joints と一緒に指定.")
ap.add_argument("--place-ee-y-floor", type=float, default=None,
help="ee_y 下限ガード (m). 選択点 ee_y < これ で out_of_range "
"(place 系のみ・PICK には渡さない).")
# DAY274 案B': j1 下限フィルタ CLI.
ap.add_argument("--place-j1-min-deg", type=float, default=None,
help="j1 下限 (度). 候補を j1 >= これ で絞る. "
"place_withblock 用. PICK には渡さない.")
# DAY274 層3 (案X): target_ee_y + j4 優先スコア CLI.
ap.add_argument("--target-ee-y-m", type=float, default=None,
help="目標 ee_y (m). 指定すると j4 優先スコアで選択. "
"place_withblock 用. --ref-joints 必須.")
ap.add_argument("--place-ee-y-tol-m", type=float,
default=PLACE_EE_Y_TOL_M_DEFAULT,
help="目標 ee_y 許容帯 (m). デフォルト {:.4f}".format(
PLACE_EE_Y_TOL_M_DEFAULT))
args = ap.parse_args()
r = fk_lookup(args.phase, (args.x, args.z),
ee_y_min=args.ee_y_min, ee_y_max=args.ee_y_max,
dist_max_m=args.dist_max_m,
j3_ref=args.j3_ref, j3_tol_deg=args.j3_tol_deg,
j5_ref=args.j5_ref, j5_tol_deg=args.j5_tol_deg,
ref_joints=args.ref_joints,
joint_tol_deg=args.joint_tol_deg,
place_ee_y_floor=args.place_ee_y_floor,
place_j1_min_deg=args.place_j1_min_deg,
target_ee_y_m=args.target_ee_y_m,
place_ee_y_tol_m=args.place_ee_y_tol_m)
print(json.dumps(r, ensure_ascii=False, indent=2))
if __name__ == "__main__":
main()merge_scan_output.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""merge_scan_output.py — DAY275 FK 穴埋めスキャン出力の統合スクリプト.
Unity FKScanner_MotionBased で motion_log_addition_DAY275_pick_hole_x199_z301.csv
を playback スキャンした結果 (追加の FK 表 CSV) を, 既存の
fk_scan_motion_based_v2.csv とマージする.
前提 (arm_kinematics.md 穴の拡充手順 5):
- ファイル名 fk_scan_motion_based_v2.csv は変えない (v3 番号を切らない・DAY274 で確立)
- 完全重複 (全 9 列一致) は 1 本に集約
- Mac / Unity / KIOXIA fk_tables/ 3 か所に同 md5 で配置 (step 5 で確認)
入力想定スキーマ (Unity 出力 = 現行 v2 と同):
ee_x,ee_y,ee_z,j0,j1,j2,j3,j4,j5 (9 列)
使い方:
python3 merge_scan_output.py \
--scan-output /path/to/unity_scan_output.csv \
[--current /Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based_v2.csv]
出力: 現行 v2 を上書き (archive/fk_tables_premerge_20260706/ に退避済み).
続けて step 5 で Unity / KIOXIA へ配布 (別スクリプト or 手作業).
読み取り専用: archive・元の Unity 出力 CSV は改変しない.
"""
from __future__ import print_function
import argparse
import csv
import hashlib
import shutil
import sys
from pathlib import Path
DEFAULT_CURRENT = Path(
"/Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based_v2.csv")
ARCHIVE = Path(
"/Volumes/KIOXIA/r2r2r-data/archive/fk_tables_premerge_20260706_step4_place_hole"
"/fk_scan_motion_based_v2.csv")
EXPECTED_ARCHIVE_MD5 = "61477fc7a16195d37729d0597d8c2677"
EXPECTED_ARCHIVE_ROWS = 1_541_975 # ヘッダを除いたデータ行数
# 完全重複判定の精度. Unity と Mac の float 表示揺れを吸収するため
# 小数点以下 4 桁で丸めてキー化 (v2 の値表示精度に合わせる).
FLOAT_ROUND = 4
def _md5_of(path):
h = hashlib.md5()
with open(path, "rb") as f:
for chunk in iter(lambda: f.read(1 << 20), b""):
h.update(chunk)
return h.hexdigest()
def _load_v2(path):
"""v2 スキーマ (9 列) を読み込む. (header, rows_as_list) を返す.
rows は生の文字列リストのまま (float 化しない). エラーは全て例外を上げる.
"""
with open(path, newline="") as f:
r = csv.reader(f)
header = next(r)
rows = list(r)
if len(header) != 9:
raise ValueError(
"スキーマ不一致: {} は {} 列 (期待 9). header={}".format(
path, len(header), header))
for i, row in enumerate(rows):
if len(row) != 9:
raise ValueError(
"行 {} が {} 列 (期待 9): path={}, row[:3]={}".format(
i, len(row), path, row[:3]))
return header, rows
def _row_key(row):
"""完全重複判定用のキー. 全 9 列を FLOAT_ROUND 桁で丸めた tuple."""
return tuple(round(float(v), FLOAT_ROUND) for v in row)
def _fmt_row(row):
"""元の文字列表現をなるべく保つ. float 化して四捨五入は行わない.
Unity 出力と Mac 元表示が異なる場合は Mac 元表示を優先 (統合先の v2 は
元の表示を保持する)."""
return row # そのまま
def main():
ap = argparse.ArgumentParser(
description="Unity 新スキャン出力を既存 v2 と統合 (完全重複集約)")
ap.add_argument("--scan-output", type=Path, required=True,
help="Unity FKScanner_MotionBased の出力 CSV パス (9 列想定)")
ap.add_argument("--current", type=Path, default=DEFAULT_CURRENT,
help="現行 fk_scan_motion_based_v2.csv (デフォルト Mac 本流)")
ap.add_argument("--no-write", action="store_true",
help="ドライラン. マージ結果を出力せず統計だけ表示")
args = ap.parse_args()
# 前提: archive の md5 が期待値
if not ARCHIVE.exists():
print("[FATAL] archive が見つからない: {}".format(ARCHIVE))
return 2
arc_md5 = _md5_of(ARCHIVE)
if arc_md5 != EXPECTED_ARCHIVE_MD5:
print("[FATAL] archive の md5 が期待値と違う (退避品が変質):")
print(" 期待: {}".format(EXPECTED_ARCHIVE_MD5))
print(" 実測: {}".format(arc_md5))
return 2
print("[OK] archive md5 一致: {} (退避品健在)".format(EXPECTED_ARCHIVE_MD5))
# 現行 v2 が archive と一致するはず (統合前の状態)
cur_md5 = _md5_of(args.current)
print("[OK] 現行 md5: {} ({})".format(cur_md5, args.current))
if cur_md5 != EXPECTED_ARCHIVE_MD5:
print("[WARN] 現行 md5 が archive と違う. 既に別処理で書き換えられている.")
print(" 続けるなら --force を検討 (このスクリプトには未実装).")
return 2
# ロード
print("[REPORT] 現行 v2 をロード...")
hdr_cur, rows_cur = _load_v2(args.current)
if len(rows_cur) != EXPECTED_ARCHIVE_ROWS:
print("[WARN] 現行 v2 の行数 {} が期待値 {} と違う".format(
len(rows_cur), EXPECTED_ARCHIVE_ROWS))
print(" header: {}".format(hdr_cur))
print(" rows: {:,}".format(len(rows_cur)))
print("[REPORT] Unity 新スキャン出力をロード: {}".format(args.scan_output))
hdr_new, rows_new = _load_v2(args.scan_output)
print(" header: {}".format(hdr_new))
print(" rows: {:,}".format(len(rows_new)))
if hdr_cur != hdr_new:
print("[FATAL] ヘッダ不一致 (v2 スキーマと違う出力):")
print(" 現行: {}".format(hdr_cur))
print(" 新スキャン: {}".format(hdr_new))
return 2
# 完全重複判定 → 集約
print("[REPORT] 重複集約中 (FLOAT_ROUND={} 桁で丸めたキーで判定)...".format(
FLOAT_ROUND))
seen = {} # key -> row (現行を優先)
for row in rows_cur:
seen[_row_key(row)] = row
n_cur_unique = len(seen)
n_dup_new_vs_cur = 0
n_new_unique = 0
for row in rows_new:
k = _row_key(row)
if k in seen:
n_dup_new_vs_cur += 1
else:
seen[k] = row
n_new_unique += 1
n_new_total_after = len(seen)
print("[REPORT] 集約結果:")
print(" 現行 v2 データ行数: {:,}".format(len(rows_cur)))
print(" 現行 v2 ユニーク行数: {:,}".format(n_cur_unique))
if len(rows_cur) != n_cur_unique:
print(" (現行 v2 内にも {} 行の重複あり)".format(
len(rows_cur) - n_cur_unique))
print(" 新スキャン行数: {:,}".format(len(rows_new)))
print(" 新のうち現行と重複: {:,}".format(n_dup_new_vs_cur))
print(" 新のうち追加 (真の新規点): {:,}".format(n_new_unique))
print(" 統合後の総ユニーク行数: {:,}".format(n_new_total_after))
print(" 純増分: {:,}".format(
n_new_total_after - n_cur_unique))
if args.no_write:
print("[REPORT] --no-write 指定・書き込みしない. 統計のみ完了.")
return 0
# 書き込み: 現行 v2 を tmp に書いて atomic rename
# 元の v2 は archive にコピー退避済 (md5 照合済).
# 保守方針 (DAY275 codex 相当): 既存 v2 は byte-perfect に維持する.
# 既存 v2 内の 1,830 行の内部重複 (実測) は今回は集約しない (別トラックの
# データ整理). 新スキャン出力側の "既存と重複しない行" だけを追記する.
# これで Mac / Unity / KIOXIA 3 か所の md5 揃え (step 5) が既存部分について
# 保存され, 新スキャン分だけ純増する.
tmp = args.current.with_suffix(".csv.merged_tmp")
print("[REPORT] tmp に書き出し (既存 v2 は byte-perfect 維持・新分のみ末尾追加):")
print(" {}".format(tmp))
# 既存の順序・重複を bit-perfect に保持
existing_keyset = set(_row_key(row) for row in rows_cur)
n_new_written = 0
with open(tmp, "w", newline="") as f:
w = csv.writer(f)
w.writerow(hdr_cur)
# 既存 v2 を丸ごとそのまま (順序・重複含む)
for row in rows_cur:
w.writerow(row)
# 新スキャン側で既存にない行だけ末尾追加
for row in rows_new:
k = _row_key(row)
if k in existing_keyset:
continue
existing_keyset.add(k)
w.writerow(row)
n_new_written += 1
print("[REPORT] 既存 {:,} 行を byte-perfect で保持. 新規追加 {:,} 行を末尾に追記.".format(
len(rows_cur), n_new_written))
# 検算 (保守方針: 既存 v2 の重複も byte-perfect 維持なので
# 期待行数は len(rows_cur) + n_new_written).
with open(tmp, newline="") as f:
r = csv.reader(f); hdr_out = next(r); rows_out = list(r)
if hdr_out != hdr_cur:
print("[FATAL] tmp のヘッダが期待と違う. abort. tmp を残して手動確認へ.")
return 3
expected_rows = len(rows_cur) + n_new_written
if len(rows_out) != expected_rows:
print("[FATAL] tmp の行数 {} が期待 {} と違う (既存 {} + 新規 {})".format(
len(rows_out), expected_rows, len(rows_cur), n_new_written))
return 3
print("[OK] tmp 検算: {:,} 行 (期待 {:,}: 既存 {:,} byte-perfect + 新規 {:,})".format(
len(rows_out), expected_rows, len(rows_cur), n_new_written))
# atomic rename (現行 v2 を上書き)
print("[REPORT] tmp → 現行 v2 に atomic rename...")
tmp.replace(args.current)
new_md5 = _md5_of(args.current)
print("[OK] 新 v2 md5: {} ({})".format(new_md5, args.current))
print()
print("次のステップ (step 5): Mac / Unity / KIOXIA 3 か所に新 md5 で揃える.")
return 0
if __name__ == "__main__":
sys.exit(main())
