mechArm 270 Pi 動的追従の限界を探る Log#45 | mechArm の横カメラを開け、PLACE 補正記憶を入れる。だが j4 が pick 到達を止めた
右側にブロックを置くと、爪がブロックの上面に乗る。爪の間にブロックが入らないまま閉じるので、掴めない。掴めないと通しが完走せず、PLACE 以降に進めない。修正の機会そのものが手前で消える。ここを開けたくて、横カメラを生かして高さのズレを補正記憶で直す、というのが出発点だった。だが実機を回すと、掴めるかどうかの手前に別の壁があった。
掴めないの手前にあるもの
上カメラの x-z 合わせは効いている。爪はブロックの真上にちゃんと来ている。破綻しているのは高さ（y）だけだった。
高さを見る目は横カメラ（Iriun Webcam / index=1）。それが機能しておらず、画像を手で撮っている状態だった。上から見る top カメラでは、爪がブロックの上に乗っているという高さのズレは見えない。横カメラが要る理由がまさにこれで、P008「測れる手段を持つことが理屈より先」に照らせば、補正ロジックを書くより先に目を開ける。目が閉じたまま補正を組むのは、入力の無いまま組むことになる。
だから作業を二段に分けた。第一段は横カメラを機能させる診断。第二段は横視点で高さのズレを測って補正を記憶する設計。第二段は目が生き返ってからでないと、測る対象が取れず設計できない。
横カメラを開ける（実機は動かさない診断）
最初に投げたのは、関節を一切動かさない診断だけの指示。カメラが開くか開かないかで、次が分岐する。
横カメラ（Iriun Webcam・index=1）が取得できるか診断する。実機の関節は動かさない。報告のみ。
1. python3 -c "import cv2; [print(i, cv2.VideoCapture(i).isOpened()) for i in range(4)]"
2. index=1 が開くなら1フレーム保存し解像度と中身を確認。開かないなら
Iriunアプリ起動状態/USB・WiFi接続/他プロセスのカメラ占有 を切り分け
3. run_pick_place.py で横カメラ（index=1）を参照する箱所を grep で特定し、
現状どう呼ばれ なぜ機能していないか（未初期化/例外握りつぶし/未使用）を報告
新規ファイルは作らない。既存コードを読むだけ。以下は Claude Code が返した診断報告の全文。
【要差し込み：横カメラ診断報告（接続カメラ列挙の結果・index=1 の開閉・index=1 参照箱所の grep 結果）を、省略せずここに貼る】止まる設計をやめる
診断と並行して、実機側の設計に一つ変更が入った。ABORT 時に自動で HOME へ戻す挙動の削除依頼。
タイムアウトで HOME に戻るのを削除してほしい。ダメか危険かは目視で判断しているので、不要な機能だ。
自動で HOME に戻す復帰は、危険の判断を機械側に持たせる設計になっている。止めるかどうかを目視で判断する前提なら、失敗しても止めずに最後まで走らせて台帳に 1 行残す方が、失敗から学ぶループには合う。アップロードしたコードでは、abort_recover_home / pickfail_to_home の経路を実行から外し（attempted=False, note=auto_home_recover_disabled_DAY265）、HOME 送信は正常末尾の stage10_to_home のみになっている。
実機で起きたこと — stage4 で j4 が取り残される
下のログは、自動復帰を削除する前の段階で出たもの。stage4（pick の降下）で何が起きたかがそのまま出ている。
[DETECT][3/3] block Unity=(-0.156,+0.273) score=null
[DETECT] block_xz=[-0.15649317932128906, 0.27312460327148436] block_score=None
[DETECT] box_xz=[0.20358358764648438, 0.20781069946289063] box_score=0.6446372866630554
[CORR ] key=pick_x-160_z280 dx=+2.73mm dz=-0.95mm n=1 target (-156.5,+273.1) -> (-159.2,+274.1) [mm]
[LOOKUP] stage3_approach_pick phase=approach_pick target_xz=(-0.1592,+0.2741) → j0=+29.589° method=interp x_resid=0.00000m z_resid=0.02884m joint_ok=True
[SEND] stage3_approach_pick: send_angles (single) [ +29.59, +87.74, -38.33, +3.60, -70.11, -30.00] speed=20 wait=1.0s (pre max|Δ|=86.25°, post-send timeout 14.9s)
[SEND] arrival OK: max|Δ|=1.29° (tol 2.0°)
[MOVE ] stage3_approach_pick arrived=True retried=False post_max|Δ|=1.290°
[LOOKUP] stage4_pick phase=pick target_xz=(-0.1592,+0.2741) → j0=+23.000° method=fk_nearest x_resid=0.00056m z_resid=0.00007m joint_ok=True
[SEND] stage4_pick: send_angles (single) [ +23.00, +81.91, -22.14, -24.08, -38.27, -29.44] speed=20 wait=1.0s (pre max|Δ|=30.72°, post-send timeout 8.0s)
[SEND] [TIMEOUT] 8.0s 内に未到達: max|Δ|=13.23° actual=[ +23.11, +82.35, -21.88, -23.55, -51.50, -29.88]
[SEND] リトライしません: pre→last max|Δ| 26.80° >= 1.0° (部分移動/ストール疑い)
[MOVE ] stage4_pick arrived=False retried=False post_max|Δ|=13.320°
[ABORT ] stage=4 reason=到達失敗
[SEND] abort_recover_home: send_angles (single) [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=82.35°, post-send timeout 14.4s)
[SEND] [TIMEOUT] 14.4s 内に未到達: max|Δ|=66.70° actual=[ -0.17, +1.31, +0.79, -0.35, +66.70, -30.41]
[SEND] リトライしません: pre→last max|Δ| 118.29° >= 1.0° (部分移動/ストール疑い)
[MOVE ] abort_recover_home arrived=False retried=False post_max|Δ|=66.700°
[RECOVER] ABORT 後 HOME 復帰: 失敗（中途姿勢の可能性・リトライしない）
[Pi] fresh_mode=0
[LOG ] /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191533/run_0001_20260703_191533.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (in_box_geom= dist=)
(base) shimazueiichi@shimadzueiichinoMacBook-Pro place_modules %読み解くと、stage4_pick で指令 [+23.00, +81.91, -22.14, -24.08, -38.27, -29.44] に対し、8 秒以内に届かず TIMEOUT。actual を見ると j4 が指令 -38.27° に対して -51.50° で止まっている。13.23° 残したまま停止して ABORT。掴む／掴まないの判定に入る前に、pick 姿勢に物理的に届いていない。さらに ABORT 後の HOME 復帰でも j4 が +66.70° 付近で止まり、復帰も失敗して中途姿勢で止まった。
爪で PICK しない理由は、爪の開閉でも VLM 判定でもなく、アームが pick 姿勢に到達できていないこと。到達しないから掴む段階に進めない。しかも取り残されるのは stage4 でも復帰でも同じ j4 で、他の軸はほぼ指令通り。ランダムな不調ではなく j4 に集中した症状だった。候補は三つある。j4 サーボの物理的な問題（トルク不足・過負荷・配線・発熱）。j4 の移動量が大きくタイムアウト内に終わらない。爪がブロックか床に接触して j4 方向に押し戻されストールしている。上面に乗る画像と合わせると三つ目が見えるが、j4 集中は接触だけでは説明しきれず、サーボ側も否定できない。
失敗しても完走して記録する、横カメラ VLM 判定、PLACE 補正記憶
当日のコード変更は三つ。一つは ABORT 即停止と自動 HOME 復帰をやめ、失敗しても最後まで走らせて台帳に 1 行残すこと。二つは横視点で撮って VLM で成否を判定すること（報酬モデル方針の適用）。三つは PLACE 側にも補正記憶を追加し、実測と目標の差をセル単位で覚えて次回に適用すること。
これらはすべて run_pick_place.py の中に組み込んだ（codex/gemini レビュー反映版、LEDGER_COLS は 67 列）。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""run_pick_place.py — 通し Pick & Place 本体（j0 対応表方式・実装単位2）
赤ブロックを上カメラで検出 → j0 対応表で各 phase の j0 を引く → 単発 send_angles で
基準姿勢へ移動 → グリッパー閉/開 → 箱（DAY232 確定固定値）へ運ぶ。
段ごとに lookup 結果と移動結果を構造化ログに全量記録する。
設計の要点（codex/gemini レビュー反映版）:
- 流用: j0_lookup / j0_sweep_scan._send_and_wait_arrival / visual_servo_pick の検出と daemon I/F
- 姿勢: lookup の target_angles をそのまま送る。J5 を -30 へ強制上書きしない
- 送信: 単発 send_angles のみ（_send_and_wait_arrival 経由）。ストリーミングしない
- ガード: joint_limit_ok / method=interp / x_residual<=THRESH の3条件を送信前に検査
- z_residual: 工程跨ぎ問い合わせでは構造的に大きい（pick弧 vs 箱z）。情報ログのみで ABORT しない
- グリッパー: daemon_set_gripper の戻り値は判定に使わない。各操作後に sleep>=2s（DAY175 対策）
- 工程跨ぎ位置データ禁止: block_xz は pick 系のみ、BOX_XZ は place 系のみ
絶対ルール:
- send_coords() / release_all_servos() を呼ばない
- Stiffness/Damping を触らない
- fk_scan_motion_based_v2.csv を書き換えない
CLI:
python3 run_pick_place.py # 一本通し実機実行
python3 run_pick_place.py --dry-run # 送信せず lookup+ガード判定だけ出力
python3 run_pick_place.py --dry-run --dummy-block-x 0.05 --dummy-block-z 0.30
"""
from __future__ import print_function
import argparse
import csv
import json
import math
import os
import sys
import time
import types
from datetime import datetime
from pathlib import Path
# ---- パス解決 ----------------------------------------------------------------
_THIS = Path(__file__).resolve()
_PLACE_MODULES_DIR = _THIS.parent
_SCRIPTS_ROOT = _THIS.parents[3]
_DRAG_TEACHING_DIR = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "drag_teaching"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES_DIR))
sys.path.insert(0, str(_DRAG_TEACHING_DIR))
# ---- pymycobot スタブ（waypoint_replay 経由の import を Mac 上で通すため） --
if "pymycobot" not in sys.modules:
_pkg = types.ModuleType("pymycobot")
_mod = types.ModuleType("pymycobot.mycobot")
class _MyCobotStub(object):
def __init__(self, *args, **kwargs):
raise RuntimeError(
"run_pick_place.py: MyCobot stub is not callable. "
"アーム制御は daemon TCP 経由のみ。")
_mod.MyCobot = _MyCobotStub
sys.modules["pymycobot"] = _pkg
sys.modules["pymycobot.mycobot"] = _mod
# ---- 流用 import ------------------------------------------------------------
# 段3/4/7/8 の j0 決定（B 案 = x のみマッチ）
from j0_lookup import j0_lookup # noqa: E402 (全 stage で利用 / DAY257 復旧)
from fk_lookup import fk_lookup # noqa: E402 (将来の実験用に温存。現状未呼出)
# DAY257 後段: descend (stage4_pick) のみ fk_lookup に切替。
# DAY262: place_withblock も fk_lookup 化 (円弧 lookup では x-z が円弧上のみで Z を狙えない.
# in_box=False 2 本 (Z 残差 +36.4/+83.6mm・両方奥) の解決策として X-Z 独立化)。
# approach_pick / approach_place は j0_lookup 維持 (中間姿勢を壊さない)。
# fk_lookup は j1<=88 safety 復活版で、descend で z を動かせるようにする目的。
# OFF にする場合は () に戻すか、"pick" / "place_withblock" を抜く。
FK_LOOKUP_PHASES = ("pick", "place_withblock")
# DAY262: place_withblock 用の ee_y 帯 (PICK 用 10-40mm 床近く帯とは別).
# 箱高さ 43mm を中心に ±10mm 強. 仮値 (measure-first: 初回実機で out_of_range 頻度と
# 落下点を見て調整). 帯外 → fk_lookup が method="out_of_range" を返して _check_guard が
# ABORT する (送角しない). PICK 側 (EE_Y_MIN/MAX_DEFAULT) は fk_lookup.py 側で保持.
PLACE_EE_Y_MIN = 0.070 # m (70mm・DAY263 確定)
PLACE_EE_Y_MAX = 0.100 # m (100mm・DAY263 確定)
# 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.070 # m (70mm・PLACE_EE_Y_MIN と同値・下限を焼き直しで固める)
# 段1/3/4/6/7/8/10 の送信・到達確認（単発 send_angles + 沈黙リトライ最大1回）
from j0_sweep_scan import ( # noqa: E402
_send_and_wait_arrival,
_daemon_get_angles_filtered,
)
# 段2 の block+box 同時検出（detect_positions が内部で camera_resolver →
# cv2.VideoCapture → warm-up → setup_homography_and_mat_range → detect_red_block と
# 06f detect_white_box_once を一括で叩く。中央値集約 repeat 対応）
from detect_positions_once import detect_positions # noqa: E402
from visual_servo_pick import ( # noqa: E402
get_daemon_socket,
close_daemon_socket,
daemon_set_gripper,
daemon_set_gripper_value, # DAY261: close の push 保持用 (open は state(0) のまま)
daemon_set_fresh_mode,
detect_red_block,
pixel_to_unity,
)
# VLM 判定用のフレーム撮影と Anthropic API 呼び出し
import cv2 # noqa: E402
import numpy as np # noqa: E402
from camera_resolver import resolve_cameras # noqa: E402
from vlm_judge import judge as vlm_judge_call # noqa: E402
# 観測 emit（脇 UDP・fire-and-forget・例外は emit 側で握る）
# 観測モジュールが理由で制御が止まらないよう、import を guard する
try:
from observe.emit import emit as _observe_emit # noqa: E402
except Exception:
def _observe_emit(*_args, **_kwargs): # 観測無効でも制御は動く
pass
# ---- 定数（仕様準拠） -------------------------------------------------------
HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
BOX_XZ = (-0.010, 0.108) # DAY232 実測値。dry-run のデフォルト + 緊急時固定値として残置
# 実機 run では段2 で実検出した box_xz を使う（DAY236〜）
X_RESID_THRESH = 0.02 # m。これ超で ABORT
BOX_SCORE_THRESH = 0.5 # 06f のテンプレ score。これ未満は箱位置不確実として段2 ABORT
# 根拠: DAY230 実績 score 0.618、06f 既定 TEMPLATE_SCORE_THRESHOLD=0.5
GRIPPER_SLEEP = 2.0 # s。DAY175 対策（>=2s）
GRIP_VALUE = 20 # close の push 保持目標値（DAY261 tour 実測の暫定値）。
# 閉じ切り state(1) は「位置で止まり握り直さず落ちる」(DAY259)。
# set_gripper_value(20) で閉じ切らず押し込み力を残す。
# gv は 開=97 / 把持厚み相当≈36 / これより小さいほど押し込み深い。
# 効かなければこの値のみ振る（descend 把持は tour と条件が違う点に留意）。
STAGE9_RELEASE_PRE_SLEEP = 2.0 # s。stage9 release は直前 PLACE 後に一呼吸置く
STAGE9_RELEASE_OPEN_ATTEMPTS = 2 # open→sleep→open で stage9 の離しを確実化
TOL_APPROACH = 2.0 # approach 系
TOL_APPROACH_PLACE = 3.0 # stage7 approach_place 専用（大移動で最後の数度が埋まらない対策）
TOL_SWEPT_RETREAT = 3.0 # stage9b swept_retreat 専用（残差2.55°で未到達になる対策・他stage非共有）
TOL_LIFT_BACK = 3.0 # stage10 lift_back 専用。実測残差 2.46〜2.55°（j2主因）を吸収。
# stage9b TOL_SWEPT_RETREAT=3.0 と同形。他stageと非共有
TOL_PICK_PLACE = 3.0 # pick / place 系
TOL_HOME = 2.5 # HOME 復帰
SEND_WAIT_SEC = 1.0 # daemon 側送信直後の get_angles ログ遅延
DETECT_REPEAT = 3 # 段2 検出の中央値集約サンプル数
SWEPT_J0_FOR_PLACE_JUDGE = 85.0 # 段9b の退避 j0（approach_place 採用版スイープ端＝採用済み値）
MAX_PICK_ATTEMPTS = 3 # pick=no 掴み直しの最大試行回数（初回含む）
SWEPT_J0_FOR_PICK_RETREAT = 85.0 # 掴み直し前のカメラ退避 j0（approach_pick base で振る・place の退避と同形）
VLM_CAPTURE_WARMUP_FRAMES = 30 # VLM 撮影前の warm-up
VLM_CAPTURE_DROP_FRAMES = 5 # 撮影直前のバッファクリア
# JSON・画像の出力先（KIOXIA・1 run = 1 フォルダで集約）。
# LEDGER_DIR と同じドライブ。未マウント時の誤書き込みは run() 冒頭ガードで防ぐ。
LOG_ROOT = Path("/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place")
# ---- 試行台帳 CSV（KIOXIA SSD 専用・ローカルへフォールバックしない）-------
LEDGER_DIR = Path("/Volumes/KIOXIA/r2r2r-data/runs")
LEDGER_CSV_PATH = LEDGER_DIR / "pick_place_ledger_v3.csv"
# in_box 判定の閾値（mm）: 白箱半幅 40mm + ブロック半幅 12mm ≈ 52mm 目安.
# 実機を見て調整可能な 1 定数.
IN_BOX_TOL_MM = 52.0
# ---- 試行間補正記憶 (DAY257) ---------------------------------------------
# tip 測定で得た Δ(dx,dz) をブロック (x,z) のセル別に蓄え、次回 lookup 直前に
# block_xz から Δ を引いて狙いを補正する。EMA で 1 セル 1 値に集約。
# 記憶 JSON は KIOXIA SSD の diag/ 配下。実機ローカルには書き込まない。
CELL_MM = 20.0 # セル幅 (mm)
EMA_ALPHA = 0.5 # EMA 重み (新 : 旧)
MAX_CORRECTION_MM = 40.0 # 補正量の絶対値クランプ (mm)
CORR_MEM_PATH = Path(
"/Volumes/KIOXIA/r2r2r-data/diag/correction_memory.json")
USE_CORRECTION = True # False で補正完全無効 (回帰確認用)
# DAY257 後段: pick 判定の二重チェック閾値。VLM yes でも tip 計測の dist がこれを
# 超えていれば「掴めていない」として no に倒す (over-pickの誤学習を防ぐ)。
# 単位 mm。実機データを見て振る前提の可変定数。
PICK_DIST_MAX_MM = 30.0
LEDGER_COLS = [
"timestamp", "run_stem", "trial_no",
"pick_verdict", "pick_reason", "pick_confidence",
"place_verdict", "place_reason", "place_confidence",
"block_pick_x", "block_pick_z",
"block_place_x", "block_place_z",
"box_x", "box_z",
"in_box_geom", "in_box_dist_mm",
"final_j1", "final_j2", "final_j3",
"final_j4", "final_j5", "final_j6",
"gripper", "place_capture_path",
"tip_pick_x", "tip_pick_z",
"tip_block_dx_mm", "tip_block_dz_mm", "tip_block_dist_mm",
"run_dir",
"tip_capture_path", "pick_capture_path",
# v3 ヘッダ 34-36（align 系：本実装では出所なし・空欄固定）
"align_j0_final", "align_red_x_mm", "align_converged",
# v3 ヘッダ 37-43（pick で参照した姿勢と到達時の実測角）
"pick_pose_id",
"pick_used_j0", "pick_used_j1", "pick_used_j2",
"pick_used_j3", "pick_used_j4", "pick_used_j5",
# v3 ヘッダ 44-50（place_withblock で参照した姿勢と到達時の実測角）
"place_pose_id",
"place_used_j0", "place_used_j1", "place_used_j2",
"place_used_j3", "place_used_j4", "place_used_j5",
# DAY257 試行間補正の記録 (5 列追加・v3 ヘッダ末尾 51-55)
"corr_cell_key", # 例: "x180_z260"
"corr_applied_dx", # 適用した補正 mm (x)
"corr_applied_dz", # 適用した補正 mm (z)
"target_before", # 補正前 (block) [mm]: "(+196.1,+252.5)" 等
"target_after", # 補正後 (lookup へ渡した値) [mm]
# DAY257 後段: pick 判定の VLM / dist override 内訳 (2 列追加)
"vlm_pick_verdict", # VLM 単独判定 (override 前) "yes/no/unknown/..."
"pick_dist_override", # tip_dist 二重チェックで no に倒したか "True"/"False"/""
# 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]
]
# 爪先(青シール)検出パラメータ（detect_blue_markers.py DAY168 のHSV閾値を流用）
# 座標変換は本線の pixel_to_unity に統一（このモジュール独自の homography は使わない）
TIP_BLUE_LOWER = (100, 100, 50)
TIP_BLUE_UPPER = (130, 255, 255)
TIP_MIN_AREA = 80
TIP_MAX_AREA = 8000
# ---- 観測 emit 用の状態（run() が更新、_do_move が読む）-----------------
# 状態は run() スコープ所有（_observe_phase / _observe_gripper ヘルパ経由で更新）。
# 内側関数（stage_lookup_and_move）には phase 更新を持たせない。
# emit は _do_move（移動完了時）に加え、非移動 stage（stage2/5/9/9b・regrasp 再検出）でも
# _observe_emit_now() で通知する。全 stage を履歴表示するため 1 サイト集約は意図的に解除済み。
_OBSERVE_STATE = {
"phase": None,
"phase_t0_ms": None,
"gripper": None,
"verdict": None,
"block_xz": None,
"box_xz": None,
"block_place_xz": None,
"tip_pick": None, # PICK到達時の爪先vs block差分（_append_ledger が読む）
"run_dir": None, # この run の出力フォルダ（KIOXIA）。画面のファイル情報表示用
"cap_tip": None, # stage4_pick_tip 画像パス（画面/CSV用）
"cap_pick": None, # pick 判定画像パス
"cap_place": None, # place 判定画像パス
# 非移動 stage の通知専用 emit が再利用する angles キャッシュ.
# 直近 _do_move の post_actual で更新する（新規 daemon I/O ゼロ）.
"last_angles": None,
"run_stem": None,
"task": "pick_and_place_with_j0_table",
}
def _observe_phase(phase_str):
"""run() からフェーズ遷移を記録する。phase_t0 も自動更新."""
_OBSERVE_STATE["phase"] = phase_str
_OBSERVE_STATE["phase_t0_ms"] = int(time.time() * 1000)
def _observe_gripper(state):
"""run() から gripper 状態を記録する（0=open / 1=close）."""
_OBSERVE_STATE["gripper"] = state
def _detect_block_in_place_img(image_path):
"""place_img（保存済み jpg）から赤ブロックの Unity 座標 [x, z] を検出する.
stage2 と同じ detect_red_block + pixel_to_unity を使う（同 homography）.
全例外を握り、失敗時 None を返す（制御を絶対止めない）.
"""
if image_path is None:
return None
try:
frame = cv2.imread(str(image_path))
if frame is None:
return None
pixel = detect_red_block(frame)
if pixel is None:
return None
cx, cy = pixel
ux, uz = pixel_to_unity(cx, cy)
return [float(ux), float(uz)]
except Exception:
return None
def _detect_blue_tips_px(frame):
"""フレームから青シールを検出し、面積上位3点のpixel重心 [(cx,cy),...] を返す.
変換はしない（pixel座標のみ）。3点未満なら None。全例外を握って None.
"""
try:
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
lower = np.array(TIP_BLUE_LOWER)
upper = np.array(TIP_BLUE_UPPER)
mask = cv2.inRange(hsv, lower, upper)
k = np.ones((5, 5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, k)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, k)
cnts, _ = cv2.findContours(
mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
pts = []
for c in cnts:
a = cv2.contourArea(c)
if a < TIP_MIN_AREA or a > TIP_MAX_AREA:
continue
m = cv2.moments(c)
if m["m00"] == 0:
continue
cx = m["m10"] / m["m00"]
cy = m["m01"] / m["m00"]
pts.append((cx, cy, a))
if len(pts) < 3:
return None
pts.sort(key=lambda t: t[2], reverse=True) # 面積降順
top3 = [(p[0], p[1]) for p in pts[:3]] # 上位3点（ノイズ除去）
return top3
except Exception:
return None
def _measure_tip_vs_block(image_path):
"""pick到達フレームから 爪先中点 と 赤block を同一frame・同一 pixel_to_unity
で検出し差分を返す. 失敗時 None（制御を絶対止めない）.
戻り値 dict: tip_x, tip_z, block_x, block_z（Unity m）, dx_mm, dz_mm, dist_mm
姿勢非依存の爪先判定: 上位3点のうち「互いに最も近い2点＝爪先」、
残り1点＝手首. これは横/上カメラやアーム向きに依存しない（DAY168 の
「Y下方2点」は姿勢依存なので採用しない）.
ガード: 爪先間距離 < 手首までの距離 が崩れたら（開きすぎ等で前提破綻）
差分を None 記録してそのまま grip へ進む（測定が動作に干渉しない）.
"""
if image_path is None:
return None
try:
frame = cv2.imread(str(image_path))
if frame is None:
return None
# --- 爪先（青シール上位3点 → 最近2点の中点）---
top3 = _detect_blue_tips_px(frame)
if top3 is None:
return None
import itertools
pairs = list(itertools.combinations(range(3), 2))
def _d(p, q):
return math.hypot(p[0] - q[0], p[1] - q[1])
# 最近ペア＝爪先
i, j = min(pairs, key=lambda ij: _d(top3[ij[0]], top3[ij[1]]))
tip1, tip2 = top3[i], top3[j]
wrist = top3[3 - i - j] # 0+1+2=3 → 残り1点
mid_px = ((tip1[0] + tip2[0]) / 2.0, (tip1[1] + tip2[1]) / 2.0)
tip_pair_d = _d(tip1, tip2)
wrist_d = _d(mid_px, wrist)
if not (tip_pair_d < wrist_d): # 前提破綻ガード
return None
# --- block（同フレーム・同 pixel_to_unity で再検出）---
red_px = detect_red_block(frame)
if red_px is None:
return None
tip_ux, tip_uz = pixel_to_unity(mid_px[0], mid_px[1])
blk_ux, blk_uz = pixel_to_unity(red_px[0], red_px[1])
dx_mm = (tip_ux - blk_ux) * 1000.0
dz_mm = (tip_uz - blk_uz) * 1000.0
dist_mm = math.hypot(dx_mm, dz_mm)
return {
"tip_x": float(tip_ux), "tip_z": float(tip_uz),
"block_x": float(blk_ux), "block_z": float(blk_uz),
"dx_mm": float(dx_mm), "dz_mm": float(dz_mm),
"dist_mm": float(dist_mm),
}
except Exception:
return None
def _observe_verdict(raw_verdict, confidence, reason):
"""run() から VLM 判定結果を記録する。
raw_verdict ∈ {'yes','no','unknown',...} を success ∈ {True,False,None} にマップ.
reward は判定器側で未算出のため None 固定（VLM-as-Reward 実装時に充填）.
"""
if raw_verdict == "yes":
success = True
elif raw_verdict == "no":
success = False
else:
success = None
_OBSERVE_STATE["verdict"] = {
"success": success,
"reward": None,
"reason": reason,
"confidence": confidence,
"raw_verdict": raw_verdict,
}
def _observe_emit_now():
"""非移動 stage 用の通知専用 emit. シリアル/daemon 読み取りを伴わない.
angles は _do_move でキャッシュ済みの last_angles をそのまま再利用（新規 I/O ゼロ）.
fire-and-forget. send_angles / グリッパー / sleep / 制御フローには一切触れない.
"""
# in_box は台帳と同じ _ledger_in_box_geometry を再利用（判定の二重実装を避ける）.
# 戻り値は ("True"/"False"/"", dist_str). bool に正規化し、欠落時は None.
# 同じ IN_BOX_TOL_MM・同じ関数を画面/台帳が共有するため値の食い違いが起きない.
_ibg, _ = _ledger_in_box_geometry(
_OBSERVE_STATE["block_place_xz"], _OBSERVE_STATE["box_xz"])
_in_box = (True if _ibg == "True" else False if _ibg == "False" else None)
_observe_emit(
angles=_OBSERVE_STATE["last_angles"],
phase=_OBSERVE_STATE["phase"],
phase_t0_ms=_OBSERVE_STATE["phase_t0_ms"],
gripper=_OBSERVE_STATE["gripper"],
trial=({"id": _OBSERVE_STATE["run_stem"],
"task": _OBSERVE_STATE["task"]}
if _OBSERVE_STATE["run_stem"] else None),
verdict=_OBSERVE_STATE["verdict"],
block_xz=_OBSERVE_STATE["block_xz"],
box_xz=_OBSERVE_STATE["box_xz"],
block_place_xz=_OBSERVE_STATE["block_place_xz"],
in_box=_in_box,
tip_pick=_OBSERVE_STATE["tip_pick"],
run_dir=_OBSERVE_STATE["run_dir"],
cap_tip=_OBSERVE_STATE["cap_tip"],
cap_pick=_OBSERVE_STATE["cap_pick"],
cap_place=_OBSERVE_STATE["cap_place"],
)
# ---- ヘルパ ------------------------------------------------------------------
def _next_log_path():
LOG_ROOT.mkdir(parents=True, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
for n in range(1, 10000):
stem = "run_{:04d}_{}".format(n, ts)
run_dir = LOG_ROOT / stem
if not run_dir.exists():
run_dir.mkdir(parents=True, exist_ok=True)
return run_dir / (stem + ".json")
raise RuntimeError("log seq 9999 超過")
def _atomic_write_json(path, data):
tmp = Path(str(path) + ".tmp")
with open(tmp, "w") as f:
json.dump(data, f, ensure_ascii=False, indent=2, default=str)
tmp.replace(path)
def _ensure_ssd_mounted():
"""KIOXIA SSD の LEDGER_DIR が存在・書込可なら True、それ以外 False.
fire-and-forget: 例外を握り False を返す."""
try:
if not LEDGER_DIR.exists():
return False
if not LEDGER_DIR.is_dir():
return False
# 書込テスト（write_test ファイルを作って即削除）
test = LEDGER_DIR / ".write_test"
try:
test.write_text("")
test.unlink()
return True
except Exception:
return False
except Exception:
return False
# ---- 試行間補正記憶 (DAY257) ---------------------------------------------
def _corr_load():
"""補正記憶 JSON をロード。失敗時は {} を返す（制御を絶対止めない）。"""
if not CORR_MEM_PATH.exists():
return {}
try:
with open(CORR_MEM_PATH, "r", encoding="utf-8") as f:
return json.load(f)
except Exception as e:
print("[CORR ] [WARN] 読み込み失敗 ({}). 空辞書で続行".format(e))
return {}
def _corr_save(mem):
"""補正記憶を atomic に保存。KIOXIA 未マウント時は False を返してスキップ。"""
if not _ensure_ssd_mounted():
print("[CORR ] KIOXIA 未マウント → 補正記憶の保存をスキップ")
return False
try:
CORR_MEM_PATH.parent.mkdir(parents=True, exist_ok=True)
tmp = CORR_MEM_PATH.with_suffix(CORR_MEM_PATH.suffix + ".tmp")
with open(tmp, "w", encoding="utf-8") as f:
json.dump(mem, f, ensure_ascii=False, indent=2)
os.replace(str(tmp), str(CORR_MEM_PATH))
return True
except Exception as e:
print("[CORR ] [WARN] 保存失敗: {}".format(e))
return False
def _corr_key(phase, x_m, z_m):
"""(phase, x, z)[m] → セルキー文字列 (例: 'pick_x180_z260' / 'place_x-40_z100')。
DAY262: phase プレフィックスを付けて pick/place の名前空間を分離
(同一 (x,z) セルでも掴み残差と置き残差は別物なので学習を汚染しない).
CELL_MM=20 で round(中心) して mm 化."""
cx = int(round(float(x_m) * 1000.0 / CELL_MM) * CELL_MM)
cz = int(round(float(z_m) * 1000.0 / CELL_MM) * CELL_MM)
return "{}_x{}_z{}".format(phase, cx, cz)
def _corr_get(mem, key):
"""セルキー → {'dx_mm','dz_mm','n'} or None"""
return mem.get(key)
def _corr_update(mem, key, dx_mm_new, dz_mm_new):
"""EMA で 1 セル 1 値に集約 (新規は値そのまま・n=1)。"""
e = mem.get(key)
# 破損ガード: 既存エントリが dict 以外 (str/list/null 等) なら捨てて新規扱いに
if e is not None and not isinstance(e, dict):
print("[CORR ] [WARN] mem[{}] が dict 以外 ({}). 破棄して再初期化"
.format(key, type(e).__name__))
e = None
if e is None:
mem[key] = {"dx_mm": float(dx_mm_new), "dz_mm": float(dz_mm_new), "n": 1}
else:
a = EMA_ALPHA
mem[key] = {
"dx_mm": a * float(dx_mm_new) + (1 - a) * float(e.get("dx_mm", 0.0)),
"dz_mm": a * float(dz_mm_new) + (1 - a) * float(e.get("dz_mm", 0.0)),
"n": int(e.get("n", 0)) + 1,
}
return mem[key]
def _apply_correction(phase, block_xz):
"""(phase, block_xz) から補正を適用し (target_xz, meta) を返す。
DAY262: phase 引数を追加 (pick/place の名前空間分離). PICK は phase="pick",
PLACE (stage8 place_withblock) は phase="place" で呼ぶ.
USE_CORRECTION=False / KIOXIA 未マウント / 記憶なし → 素通り (meta は空欄).
MAX_CORRECTION_MM で絶対値クランプ. 狙い = block - Δ.
Δ の符号: PICK は (爪先 - ブロック), PLACE は (落下点 - 箱). どちらも「実測 - 目標」."""
bx_m = float(block_xz[0]); bz_m = float(block_xz[1])
meta = {
"phase": phase,
"target_before_mm": (bx_m * 1000.0, bz_m * 1000.0),
"target_after_mm": (bx_m * 1000.0, bz_m * 1000.0),
"cell_key": "",
"dx_mm": None, "dz_mm": None, "n": 0,
"applied": False,
}
if not USE_CORRECTION:
return (bx_m, bz_m), meta
if not _ensure_ssd_mounted():
print("[CORR ] KIOXIA 未マウント → 補正スキップ (素通り)")
return (bx_m, bz_m), meta
key = _corr_key(phase, bx_m, bz_m)
meta["cell_key"] = key
mem = _corr_load()
entry = _corr_get(mem, key)
if entry is None:
return (bx_m, bz_m), meta
# 破損ガード: エントリが dict 以外 (str/list 等) は補正せず素通り。
# 次回の _corr_update が新規扱いで上書きするので、自動で正常状態に戻る。
if not isinstance(entry, dict):
print("[CORR ] [WARN] mem[{}] が dict 以外 ({}). 補正せず素通り"
.format(key, type(entry).__name__))
return (bx_m, bz_m), meta
dx_mm = float(entry.get("dx_mm", 0.0))
dz_mm = float(entry.get("dz_mm", 0.0))
n = int(entry.get("n", 0))
# MAX_CORRECTION_MM クランプ (記憶破損ガード)
dx_mm = max(-MAX_CORRECTION_MM, min(MAX_CORRECTION_MM, dx_mm))
dz_mm = max(-MAX_CORRECTION_MM, min(MAX_CORRECTION_MM, dz_mm))
# 狙い = block - Δ (Δ は 爪先 - ブロック の符号)
corrected_x_m = bx_m - dx_mm / 1000.0
corrected_z_m = bz_m - dz_mm / 1000.0
meta["target_after_mm"] = (corrected_x_m * 1000.0, corrected_z_m * 1000.0)
meta["dx_mm"] = dx_mm
meta["dz_mm"] = dz_mm
meta["n"] = n
meta["applied"] = True
print("[CORR ] key={} dx={:+.2f}mm dz={:+.2f}mm n={} "
"target ({:+.1f},{:+.1f}) -> ({:+.1f},{:+.1f}) [mm]".format(
key, dx_mm, dz_mm, n,
bx_m * 1000.0, bz_m * 1000.0,
corrected_x_m * 1000.0, corrected_z_m * 1000.0))
return (corrected_x_m, corrected_z_m), meta
def _ledger_extract_trial_no(run_stem):
"""run_stem ("run_0001_YYYYMMDD_HHMMSS" 想定) の先頭数字を int で返す。
取得失敗時は空文字列."""
try:
for part in str(run_stem).split("_"):
if part.isdigit():
return int(part)
except Exception:
pass
return ""
def _ledger_mm_or_blank(arr, idx):
"""[x, z] m リストから idx 要素を mm 化して文字列に。取れなければ空."""
if not isinstance(arr, (list, tuple)) or len(arr) <= idx:
return ""
v = arr[idx]
if not isinstance(v, (int, float)):
return ""
return "{:.1f}".format(v * 1000.0)
def _ledger_in_box_geometry(block_place_xz, box_xz):
"""block_place と box 中心の距離 mm + IN_BOX_TOL_MM 比較.
戻り値: (in_box_geom_str, dist_mm_str). どちらか欠落で ('','') を返す."""
if not (isinstance(block_place_xz, (list, tuple)) and len(block_place_xz) == 2
and isinstance(box_xz, (list, tuple)) and len(box_xz) == 2):
return "", ""
vals = list(block_place_xz) + list(box_xz)
if not all(isinstance(v, (int, float)) for v in vals):
return "", ""
dx_mm = (block_place_xz[0] - box_xz[0]) * 1000.0
dz_mm = (block_place_xz[1] - box_xz[1]) * 1000.0
dist = math.hypot(dx_mm, dz_mm)
return str(dist < IN_BOX_TOL_MM), "{:.1f}".format(dist)
def _append_ledger(log_record, run_stem):
"""試行 1 行を KIOXIA SSD の CSV 台帳へ追記.
fire-and-forget: 例外で制御を絶対止めない. ローカルへのフォールバック書き込みはしない.
既存ヘッダ無ければ新規作成、あれば append.
"""
try:
if not _ensure_ssd_mounted():
print("[LEDGER] KIOXIA SSD ({}) 未マウント or 書込不可 → CSV スキップ"
.format(LEDGER_DIR))
return
judgments = log_record.get("judgments") or {}
pick = judgments.get("pick") or {}
place = judgments.get("place") or {}
# block/box は _OBSERVE_STATE の最終値 (regrasp 反映済み・stage9b 検出反映済み)
block_xz = _OBSERVE_STATE.get("block_xz")
box_xz = _OBSERVE_STATE.get("box_xz")
block_place_xz = _OBSERVE_STATE.get("block_place_xz")
gripper = _OBSERVE_STATE.get("gripper")
tip_pick = _OBSERVE_STATE.get("tip_pick")
# 最後の move の post_actual = 試行終了時の関節角
final_angles = ["", "", "", "", "", ""]
for m in reversed(log_record.get("moves", []) or []):
pa = m.get("post_actual")
if isinstance(pa, (list, tuple)) and len(pa) == 6:
for i in range(6):
v = pa[i]
final_angles[i] = ("{:.2f}".format(v)
if isinstance(v, (int, float)) else "")
break
# place 撮影画像のフルパス
place_capture_path = ""
for c in reversed(log_record.get("vlm_captures", []) or []):
if c.get("label") == "place" and c.get("image"):
place_capture_path = str(c["image"])
break
in_box_geom, in_box_dist_mm = _ledger_in_box_geometry(
block_place_xz, box_xz)
row = {
"timestamp": log_record.get("ended_at") or datetime.now().isoformat(),
"run_stem": run_stem,
"trial_no": _ledger_extract_trial_no(run_stem),
"pick_verdict": (pick.get("verdict") or ""),
"pick_reason": (pick.get("reason") or ""),
"pick_confidence": (pick.get("confidence")
if isinstance(pick.get("confidence"), (int, float))
else ""),
"place_verdict": (place.get("verdict") or ""),
"place_reason": (place.get("reason") or ""),
"place_confidence": (place.get("confidence")
if isinstance(place.get("confidence"), (int, float))
else ""),
"block_pick_x": _ledger_mm_or_blank(block_xz, 0),
"block_pick_z": _ledger_mm_or_blank(block_xz, 1),
"block_place_x": _ledger_mm_or_blank(block_place_xz, 0),
"block_place_z": _ledger_mm_or_blank(block_place_xz, 1),
"box_x": _ledger_mm_or_blank(box_xz, 0),
"box_z": _ledger_mm_or_blank(box_xz, 1),
"in_box_geom": in_box_geom,
"in_box_dist_mm": in_box_dist_mm,
"final_j1": final_angles[0],
"final_j2": final_angles[1],
"final_j3": final_angles[2],
"final_j4": final_angles[3],
"final_j5": final_angles[4],
"final_j6": final_angles[5],
"gripper": ("" if gripper is None else str(gripper)),
"place_capture_path": place_capture_path,
"tip_pick_x": ("{:.1f}".format(tip_pick["tip_x"] * 1000.0)
if tip_pick else ""),
"tip_pick_z": ("{:.1f}".format(tip_pick["tip_z"] * 1000.0)
if tip_pick else ""),
"tip_block_dx_mm": ("{:.1f}".format(tip_pick["dx_mm"])
if tip_pick else ""),
"tip_block_dz_mm": ("{:.1f}".format(tip_pick["dz_mm"])
if tip_pick else ""),
"tip_block_dist_mm": ("{:.1f}".format(tip_pick["dist_mm"])
if tip_pick else ""),
"run_dir": (_OBSERVE_STATE.get("run_dir") or ""),
"tip_capture_path": (_OBSERVE_STATE.get("cap_tip") or ""),
"pick_capture_path": (_OBSERVE_STATE.get("cap_pick") or ""),
}
# v3 ヘッダ 34-50: align 系 3 列（本実装では出所なし・空欄固定）と
# pick/place の pose_id・到達時実測角 14 列。
pose_used = log_record.get("pose_used") or {}
pick_used = pose_used.get("pick_used") or [None] * 6
place_used = pose_used.get("place_used") or [None] * 6
def _fmt_j(v):
return ("{:.2f}".format(v) if isinstance(v, (int, float)) else "")
row.update({
"align_j0_final": "",
"align_red_x_mm": "",
"align_converged": "",
"pick_pose_id": (pose_used.get("pick_pose_id") or ""),
"pick_used_j0": _fmt_j(pick_used[0]),
"pick_used_j1": _fmt_j(pick_used[1]),
"pick_used_j2": _fmt_j(pick_used[2]),
"pick_used_j3": _fmt_j(pick_used[3]),
"pick_used_j4": _fmt_j(pick_used[4]),
"pick_used_j5": _fmt_j(pick_used[5]),
"place_pose_id": (pose_used.get("place_pose_id") or ""),
"place_used_j0": _fmt_j(place_used[0]),
"place_used_j1": _fmt_j(place_used[1]),
"place_used_j2": _fmt_j(place_used[2]),
"place_used_j3": _fmt_j(place_used[3]),
"place_used_j4": _fmt_j(place_used[4]),
"place_used_j5": _fmt_j(place_used[5]),
})
# DAY257 補正の記録 (5 列・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")),
})
# 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 != "interp" → ABORT（clamp ＝ 弧 x 範囲外）
3. x_residual_m > X_RESID_THRESH → ABORT
4. z_residual_m → ログのみ（ここでは判定しない）
戻り値: (ok: bool, reason: str | None)
"""
if not r.get("joint_limit_ok"):
return False, "joint_limit違反: {}".format(r.get("joint_limit_violations"))
# 許可 method: "interp" (j0_lookup の補間成功) / "fk_nearest" (fk_lookup の帯内最近傍成功)
# 不許可: "clamp" (j0_lookup 弧端張り付き) / "out_of_range" (fk_lookup 帯外 or 距離超過)
if r.get("method") not in ("interp", "fk_nearest"):
return False, "method={}（弧の x 範囲外 / FK 帯外＝届かない位置）".format(
r.get("method"))
xr = r.get("x_residual_m")
if xr is None or xr > X_RESID_THRESH:
return False, "x_residual_m={} > {}".format(xr, X_RESID_THRESH)
return True, None
def _do_move(target_angles, label, tol, log_record, *, dry_run):
"""target_angles へ移動し、結果を log_record["moves"] に追加。
dry_run=True の場合は送信せず計画のみ記録。
戻り値: (success: bool, post_actual: list | None)
"""
pre_actual = None if dry_run else _daemon_get_angles_filtered()
pre_max = _max_delta(target_angles, pre_actual)
move_rec = {
"label": label,
"target_angles": list(target_angles),
"reach_tol_deg": tol,
"pre_actual": list(pre_actual) if pre_actual is not None else None,
"pre_max_delta_deg": pre_max,
"dry_run": dry_run,
}
if dry_run:
move_rec["arrived"] = None
move_rec["retried"] = None
move_rec["post_actual"] = None
move_rec["post_max_delta_deg"] = None
log_record.setdefault("moves", []).append(move_rec)
print("[MOVE ] {} dry_run（送信なし）".format(label))
return True, None
arrived, retried = _send_and_wait_arrival(
target_angles, label, tol, send_wait_sec=SEND_WAIT_SEC)
post_actual = _daemon_get_angles_filtered()
# 観測 emit（脇 UDP・fire-and-forget・例外は emit 側で握る）
# post_actual を再利用するので daemon I/O は増えない。print 出力にも影響しない
_observe_emit(
angles=post_actual,
phase=_OBSERVE_STATE["phase"],
phase_t0_ms=_OBSERVE_STATE["phase_t0_ms"],
gripper=_OBSERVE_STATE["gripper"],
trial=({"id": _OBSERVE_STATE["run_stem"],
"task": _OBSERVE_STATE["task"]}
if _OBSERVE_STATE["run_stem"] else None),
verdict=_OBSERVE_STATE["verdict"],
block_xz=_OBSERVE_STATE["block_xz"],
box_xz=_OBSERVE_STATE["box_xz"],
block_place_xz=_OBSERVE_STATE["block_place_xz"],
tip_pick=_OBSERVE_STATE["tip_pick"],
run_dir=_OBSERVE_STATE["run_dir"],
cap_tip=_OBSERVE_STATE["cap_tip"],
cap_pick=_OBSERVE_STATE["cap_pick"],
cap_place=_OBSERVE_STATE["cap_place"],
)
# 非移動 stage の通知 emit 用に最後の post_actual をキャッシュ（新規 I/O ゼロ）
_OBSERVE_STATE["last_angles"] = list(post_actual) if post_actual is not None else None
post_max = _max_delta(target_angles, post_actual)
move_rec["arrived"] = bool(arrived)
move_rec["retried"] = bool(retried)
move_rec["post_actual"] = list(post_actual) if post_actual is not None else None
move_rec["post_max_delta_deg"] = post_max
log_record.setdefault("moves", []).append(move_rec)
print("[MOVE ] {} arrived={} retried={} post_max|Δ|={}".format(
label, arrived, retried,
"{:.3f}°".format(post_max) if post_max is not None else "None"))
return arrived, post_actual
def _record_lookup(r, label, target_xz, log_record):
"""j0_lookup 結果を log_record["lookups"] に追加 + コンソールに出力."""
rec = {
"label": label,
"phase": r["phase"],
"source_file": r["source_file"],
"pose_id": r.get("pose_id"),
"target_xz_m": list(target_xz),
"j0": r["j0"],
"method": r["method"],
"x_residual_m": r["x_residual_m"],
"z_residual_m": r["z_residual_m"],
"interp_xz_m": list(r["interp_xz_m"]),
"joint_limit_ok": r["joint_limit_ok"],
"joint_limit_violations": r["joint_limit_violations"],
"target_angles": list(r["target_angles"]),
"base_angles": list(r["base_angles"]),
"nearest_pts": r["nearest_pts"],
}
if "interp_t" in r:
rec["interp_t"] = r["interp_t"]
rec["interp_seg_idx"] = r["interp_seg_idx"]
# 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
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):
"""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 回帰ゼロ担保).
戻り値: (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)
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)
ok, _ = _do_move(
approach_pick_target, "stage6_lift" + suf, TOL_APPROACH,
log_record, dry_run=dry_run)
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")
# tip_dist 二重チェック: VLM yes でも dist > PICK_DIST_MAX_MM なら no に倒す。
# tip 測定失敗 (tip_diff is None) のときは override 判定保留 (False)。
dist_override = False
final_verdict = vlm_verdict
if (vlm_verdict == "yes" and tip_diff is not None
and tip_diff.get("dist_mm") is not None
and float(tip_diff["dist_mm"]) > PICK_DIST_MAX_MM):
final_verdict = "no"
dist_override = True
print("[OVERRIDE] vlm=yes だが tip_dist={:.1f}mm > {}mm "
"→ pick_verdict=no に倒す".format(
tip_diff["dist_mm"], PICK_DIST_MAX_MM))
adopted = final_verdict # 補正記憶の判定はこの final を見る
_observe_verdict(adopted, pj["confidence"], pj["reason"])
log_record["pick_attempts"].append({
"attempt": attempt,
"verdict": adopted,
"vlm_verdict": vlm_verdict,
"human_verdict": human_verdict,
"dist_override": dist_override,
"confidence": pj["confidence"],
"reason": pj["reason"],
"block_xz_m": list(cur_block_xz),
"x_residual_m": r_pick["x_residual_m"],
"z_residual_m": r_pick["z_residual_m"],
})
print("[JUDGE ] pick attempt={} adopted={} (vlm={} human={} "
"dist_override={}) confidence={} reason={}".format(
attempt, adopted, vlm_verdict, human_verdict,
dist_override, pj["confidence"], pj["reason"]))
# judgments["pick"] は最終試行の dict を保存（verdict は adopted・VLM/人を併記）
final_pick_judgment = dict(pj)
final_pick_judgment["verdict"] = adopted
final_pick_judgment["vlm_verdict"] = vlm_verdict
final_pick_judgment["human_verdict"] = human_verdict
final_pick_judgment["dist_override"] = dist_override
# 補正記憶の更新: 最終 yes (掴み成功確定) かつ tip 測定成功時のみ。
# override で no に倒した試行 / VLM no / tip 測定失敗 のΔは
# 記憶に入れない (掴めていない試行で学習を汚さない)。
# KIOXIA 未マウントや USE_CORRECTION=False 時は _corr_save がスキップ。
if (USE_CORRECTION and adopted == "yes"
and tip_diff is not None
and tip_diff.get("dx_mm") is not None
and tip_diff.get("dz_mm") is not None):
key = _corr_key("pick", cur_block_xz[0], cur_block_xz[1])
mem = _corr_load()
e_new = _corr_update(mem, key,
tip_diff["dx_mm"], tip_diff["dz_mm"])
if _corr_save(mem):
print("[CORR ] mem key={} dx={:+.2f} dz={:+.2f} n={} "
"(EMA α={}・最終 yes のみ更新)".format(
key, e_new["dx_mm"], e_new["dz_mm"],
e_new["n"], EMA_ALPHA))
if adopted == "yes":
pick_ok = True
break
if adopted != "no":
# unknown 等（VLM/撮影/API 失敗 or 人が unknown 入力）は「掴めていない証拠」ではない。
# ここで open すると掴めていたブロックを落とす機序があるため regrasp しない。
# ループを抜けて pick_failed 経路（HOME 復帰）へ流す。
print("[REGRASP] verdict={} は no ではないため掴み直さず終了"
.format(adopted))
break
if attempt == MAX_PICK_ATTEMPTS:
# 最終試行で yes でなければ break（外側で pick 失敗処理）
break
# ---- 掴み直し準備: open → 退避 → 再検出 ----
if not _do_gripper(
"open", "regrasp_open_a{}".format(attempt),
log_record, dry_run):
# 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)
log_record.setdefault("correction", {})["place_last"] = place_corr_meta
# 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
_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)
if not ok:
return False, log_path
# 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)
place_vlm = _judge_image(
place_img, "place", dry_run, dummy_place_verdict)
place_vlm_verdict = place_vlm["verdict"]
place_human_verdict = None
if human_judge and not dry_run:
place_human_verdict = _ask_human_verdict(
place_img, place_vlm, kind="place")
place_adopted = place_vlm_verdict # 分岐は常に VLM。記録のみ人
_observe_verdict(place_adopted, place_vlm["confidence"], place_vlm["reason"])
# 非移動 stage の通知 emit（stage9b 成功ブランチ→画面に verdict 行）
_observe_emit_now()
place_judgment = dict(place_vlm)
place_judgment["verdict"] = place_adopted
place_judgment["vlm_verdict"] = place_vlm_verdict
place_judgment["human_verdict"] = place_human_verdict
log_record["judgments"]["place"] = place_judgment
print("[JUDGE ] place: adopted={} (vlm={} human={}) "
"confidence={} reason={}".format(
place_adopted, place_vlm_verdict, place_human_verdict,
place_vlm["confidence"], place_vlm["reason"]))
# DAY262 (仕様1): place 補正記憶の更新.
# 学習入力は「離す前」検出 (block_place_pre_xz・stage9 open 前に撮影済み).
# 退避後 _OBSERVE_STATE["block_place_xz"] は observe 表示・台帳 block_place_x/z
# で引き続き使うが、学習には転がりズレが混じるので使わない.
# VLM 判定 yes かつ block_place_pre_xz / box_xz が両方揃うときのみ更新.
# どちらか None (検出失敗 / detect_positions 未取得) は PICK 側 tip_diff None と
# 同型で学習スキップ. Δ = (離す前 - 箱) = 「実測 - 目標」・PICK と符号整合.
# KIOXIA 未マウント時は _corr_save がスキップし自動素通り.
if (USE_CORRECTION and place_adopted == "yes"
and isinstance(block_place_pre_xz, (list, tuple))
and len(block_place_pre_xz) == 2
and isinstance(box_xz, (list, tuple))
and len(box_xz) == 2
and block_place_pre_xz[0] is not None
and block_place_pre_xz[1] is not None):
dx_mm_place = (float(block_place_pre_xz[0]) - float(box_xz[0])) * 1000.0
dz_mm_place = (float(block_place_pre_xz[1]) - float(box_xz[1])) * 1000.0
key_place = _corr_key("place", box_xz[0], box_xz[1])
mem_place = _corr_load()
e_new_place = _corr_update(
mem_place, key_place, dx_mm_place, dz_mm_place)
if _corr_save(mem_place):
print("[CORR ] mem key={} dx={:+.2f} dz={:+.2f} n={} "
"(EMA α={}・place VLM yes・離す前 Δ で更新)".format(
key_place, e_new_place["dx_mm"],
e_new_place["dz_mm"], e_new_place["n"],
EMA_ALPHA))
# 段10 lift_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")
ok, _ = _do_move(
approach_place_target, "stage10_lift_back", TOL_LIFT_BACK,
log_record, dry_run=dry_run)
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()効き始めたが、まだ振動している
当日は 63 試行を台帳に残した。PICK は 63 中 41 が yes。tip-block 距離（爪先とブロックの距離）はほぼ一桁 mm に寄り、最小 0.9mm。外れ値は残る（未到達時の 161.6mm、pick=no 時の 50.2mm など）。
PLACE 補正記憶はセッション途中から投入した（place_corr_cell_key が run_..._202225 以降の行にのみ入る）。投入後の箱中心までの距離（in_box_dist_mm）の推移。
順 | run_stem | place | in_box_dist_mm |
|---|---|---|---|
1 | run_0001_20260703_202225 | yes | 60.5 |
2 | run_0001_20260703_202400 | no | 241.3 |
3 | run_0001_20260703_202550 | yes | 24.0 |
4 | run_0001_20260703_202741 | yes | 34.7 |
5 | run_0001_20260703_203003 | no | 17.6 |
6 | run_0001_20260703_203351 | yes | 10.2 |
7 | run_0001_20260703_203547 | no | 137.5 |
60.5 → 241.3 → 24.0 → 34.7 → 17.6 → 10.2 → 137.5。掴みは寄り、PLACE も一度は 10mm 台まで入るが、241mm・137mm の大きなオーバーシュートを交えて振動している。箱に収まりきらない。補正が効き始めたことは見えるが、行き過ぎて戻る振れ幅が収束していない。
当日（LOG266）書き込まれた台帳。pick_place_ledger_v3.csv は 67 列、63 行の記録。
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,align_j0_final,align_red_x_mm,align_converged,pick_pose_id,pick_used_j0,pick_used_j1,pick_used_j2,pick_used_j3,pick_used_j4,pick_used_j5,place_pose_id,place_used_j0,place_used_j1,place_used_j2,place_used_j3,place_used_j4,place_used_j5,corr_cell_key,corr_applied_dx,corr_applied_dz,target_before,target_after,vlm_pick_verdict,pick_dist_override,side_ap_place_capture_path,side_place_capture_path,side_place_verdict,side_place_confidence,side_place_reason,place_corr_cell_key,place_corr_applied_dx,place_corr_applied_dz,place_target_before,place_target_after
2026-07-03T13:00:25.903409,run_0001_20260703_125934,1,,,,,,,,,,,,,,,-0.43,1.31,1.05,0.26,0.17,-29.70,0,,,,,,,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_125934,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
2026-07-03T13:01:15.770236,run_0001_20260703_130029,1,unknown,no_api_key,0.0,,,,211.1,249.1,,,-181.1,229.0,,,-0.43,1.40,0.96,-0.61,-1.93,-29.79,1,,216.0,246.1,4.4,-3.0,5.3,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_130029,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_130029/run_0001_20260703_130029_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_130029/run_0001_20260703_130029_pick.jpg,,,,fk_row_21369,-32.08,89.03,-37.08,17.57,-47.19,-30.76,,,,,,,,pick_x220_z240,2.70,-7.90,"(+211.1,+249.1)","(+208.4,+257.0)",unknown,False,,,,,,,,,,
2026-07-03T13:02:13.785468,run_0001_20260703_130124,1,unknown,no_api_key,0.0,,,,210.8,240.7,,,-181.5,229.0,,,-0.08,1.40,0.96,-0.43,-2.19,-29.97,1,,215.2,239.1,0.9,-0.1,0.9,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_130124,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_130124/run_0001_20260703_130124_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_130124/run_0001_20260703_130124_pick.jpg,,,,fk_row_16580,-41.66,89.03,-32.78,-6.50,-51.85,-29.61,,,,,,,,pick_x220_z240,2.70,-7.90,"(+210.8,+240.7)","(+208.1,+248.6)",unknown,False,,,,,,,,,,
2026-07-03T13:05:21.138523,run_0001_20260703_130431,1,unknown,no_api_key,0.0,,,,217.3,252.1,,,-181.1,228.9,,,-0.43,1.40,0.96,-0.52,-2.28,-30.32,1,,216.3,238.7,-7.2,-9.4,11.8,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_130431,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_130431/run_0001_20260703_130431_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_130431/run_0001_20260703_130431_pick.jpg,,,,fk_row_16580,-41.74,89.03,-32.78,-6.50,-51.76,-30.05,,,,,,,,pick_x220_z260,-2.74,-8.33,"(+217.3,+252.1)","(+220.1,+260.4)",unknown,False,,,,,,,,,,
2026-07-03T13:15:42.468334,run_0001_20260703_131450,1,unknown,no_api_key,0.0,,,,209.9,252.6,,,-181.8,228.3,,,-0.26,1.49,0.96,-0.96,-2.28,-29.70,1,,215.6,246.0,5.6,-6.2,8.3,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_131450,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_131450/run_0001_20260703_131450_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_131450/run_0001_20260703_131450_pick.jpg,,,,fk_row_21369,-32.25,89.03,-37.17,17.31,-47.46,-31.02,,,,,,,,pick_x200_z260,3.93,-7.99,"(+209.9,+252.6)","(+206.0,+260.6)",unknown,False,,,,,,,,,,
2026-07-03T13:17:55.641559,run_0001_20260703_131703,1,unknown,no_api_key,0.0,,,,218.3,255.2,,,-181.3,228.6,,,0.08,1.49,0.96,-1.05,-2.28,-29.61,1,,215.2,246.8,-2.6,-8.9,9.3,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_131703,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_131703/run_0001_20260703_131703_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_131703/run_0001_20260703_131703_pick.jpg,,,,fk_row_21369,-31.90,89.03,-37.17,17.75,-47.46,-30.23,,,,,,,,pick_x220_z260,-2.74,-8.33,"(+218.3,+255.2)","(+221.1,+263.5)",unknown,False,,,,,,,,,,
2026-07-03T13:24:36.219608,run_0001_20260703_132310,1,yes,2枚目でグリッパーが閉じており、赤ブロックがグリッパーの爪の間に挟まれたまま位置が維持されている。1枚目と比較してブロックが床面から浮いた状態でグリッパーと共に移動しているように見える。,0.82,yes,白い箱の内側に赤い四角いブロックが置かれているのが明確に確認できる,0.97,216.4,259.0,-167.5,258.9,-181.8,228.9,True,33.3,-0.08,1.58,1.93,-0.35,-2.28,-29.53,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132310/run_0001_20260703_132310_place.jpg,215.5,246.5,-0.9,-13.0,13.0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132310,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132310/run_0001_20260703_132310_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132310/run_0001_20260703_132310_pick.jpg,,,,fk_row_21369,-32.08,89.03,-37.17,17.57,-47.46,-30.58,fk_row_818398,37.26,67.76,4.04,-1.66,-80.68,-29.53,pick_x220_z260,-2.74,-8.33,"(+216.4,+259.0)","(+219.1,+267.3)",yes,False,,,,,,,,,,
2026-07-03T13:30:06.821757,run_0001_20260703_132841,1,yes,2枚目でグリッパーの爪の間に赤ブロックが挟まれたまま腕全体が持ち上がっており、ブロックがマット面から浮いている位置にある,0.82,yes,白い箱の内側に赤い四角いブロックが明確に確認できる,0.97,251.9,210.4,-151.1,251.9,-166.9,238.1,True,21.0,-0.17,1.58,1.93,-0.79,-2.46,-29.70,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132841/run_0001_20260703_132841_place.jpg,257.4,193.5,-1.7,-7.8,8.0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132841,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132841/run_0001_20260703_132841_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132841/run_0001_20260703_132841_pick.jpg,,,,fk_row_14200,-47.10,89.03,-32.95,7.82,-51.76,-30.93,fk_row_817944,34.27,65.21,6.50,-1.66,-80.85,-29.61,pick_x260_z220,,,"(+251.9,+210.4)","(+251.9,+210.4)",yes,False,,,,,,,,,,
2026-07-03T18:53:40.784877,run_0001_20260703_185219,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比較してブロックの位置が爪に密着した状態で持ち上がっているように見える。床面との間に隙間が生じていると判断できる。,0.82,yes,白い箱の内側に赤い四角いブロックが明確に見える,0.97,217.3,257.5,-175.3,242.0,-169.3,218.5,True,24.2,1.40,1.49,2.02,-0.79,-0.35,-29.61,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185219/run_0001_20260703_185219_place.jpg,214.0,247.5,-3.8,-10.0,10.7,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185219,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185219/run_0001_20260703_185219_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185219/run_0001_20260703_185219_pick.jpg,,,,fk_row_21369,-31.72,89.03,-37.08,17.57,-47.19,-30.84,fk_row_844945,37.26,70.92,10.98,-0.35,-88.50,-29.44,pick_x220_z260,-1.81,-10.65,"(+217.3,+257.5)","(+219.1,+268.1)",yes,False,,,,,,,,,,
2026-07-03T18:55:26.783499,run_0001_20260703_185406,1,yes,2枚目でグリッパーの爪が閉じており、赤ブロックがグリッパー下部に保持されたまま腕全体が持ち上がった位置にある。床面との接触が見られず、ブロックが爪間に挟まれて浮いていると判断できる。,0.82,yes,白い箱の内側に赤い四角いブロックが明確に確認できる,0.97,177.7,271.7,-134.5,294.2,-136.8,258.7,True,35.6,0.08,1.49,1.93,-0.70,-2.46,-29.61,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185406/run_0001_20260703_185406_place.jpg,185.4,264.1,4.7,-7.1,8.5,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185406,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185406/run_0001_20260703_185406_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185406/run_0001_20260703_185406_pick.jpg,,,,fk_row_117472,-33.31,89.03,-33.39,-2.90,-49.30,-31.37,fk_row_816851,26.98,67.76,4.04,-0.61,-80.85,-29.53,pick_x180_z280,0.43,-7.13,"(+177.7,+271.7)","(+177.3,+278.9)",yes,False,,,,,,,,,,
2026-07-03T18:57:03.509360,run_0001_20260703_185541,1,yes,2枚目でグリッパーが閉じており、赤ブロックがグリッパーの爪の間に挟まれた状態でアームとともに位置が変化しており、マット面から浮いていると判断できる,0.82,yes,白い箱の中に赤い四角形のブロックが明確に確認できる,0.97,257.4,211.2,-234.6,193.6,-239.2,187.4,True,7.7,-0.08,1.49,1.75,-0.61,-2.46,-29.61,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185541/run_0001_20260703_185541_place.jpg,252.9,208.2,-3.4,-3.9,5.2,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185541,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185541/run_0001_20260703_185541_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185541/run_0001_20260703_185541_pick.jpg,,,,fk_row_16567,-41.48,89.03,-37.17,17.57,-47.28,-30.76,fk_row_820387,50.62,67.76,3.95,-1.49,-80.85,-29.61,pick_x260_z220,-1.71,-7.79,"(+257.4,+211.2)","(+259.1,+219.0)",yes,False,,,,,,,,,,
2026-07-03T18:58:14.682635,run_0001_20260703_185708,1,yes,2枚目の画像でグリッパーの爪が赤ブロックを両側から挟んでおり、ブロックがグリッパーと一体となって持ち上がっている様子が確認できる。床面との接触が見られない。,0.82,,,,-177.3,274.4,,,202.3,251.7,,,0.35,1.49,1.93,0.00,-2.37,-29.70,1,,-176.1,279.0,1.2,2.6,2.9,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185708,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185708/run_0001_20260703_185708_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185708/run_0001_20260703_185708_pick.jpg,,,,fk_row_444690,27.59,83.14,-34.54,-23.55,-36.65,-28.56,,,,,,,,pick_x-180_z280,,,"(-177.3,+274.4)","(-177.3,+274.4)",yes,False,,,,,,,,,,
2026-07-03T18:59:36.104069,run_0001_20260703_185829,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比較してブロックの位置がグリッパーとともに持ち上がっている様子が確認できる,0.82,,,,-153.9,300.6,,,202.2,251.7,,,0.08,1.49,1.84,0.00,-2.37,-29.70,1,,-145.8,315.2,8.1,13.6,15.8,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185829,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185829/run_0001_20260703_185829_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185829/run_0001_20260703_185829_pick.jpg,,,,fk_row_445311,27.07,86.83,-42.45,0.43,-36.65,-28.56,,,,,,,,pick_x-160_z300,,,"(-153.9,+300.6)","(-153.9,+300.6)",yes,False,,,,,,,,,,
2026-07-03T19:00:59.562463,run_0001_20260703_185959,1,yes,2枚目ではグリッパーの爪が赤ブロックを両側から挟んでおり、ブロックがグリッパーと共に持ち上がって床から浮いているように見える。1枚目と比較してブロックの位置がグリッパー内部に収まっており、落下や滑りの兆候は見られない。,0.82,,,,-154.7,281.3,,,202.3,251.6,,,0.00,1.49,1.93,0.00,-2.28,-29.88,1,,-147.9,285.5,2.7,-1.0,2.9,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185959,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185959/run_0001_20260703_185959_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_185959/run_0001_20260703_185959_pick.jpg,,,,fk_row_436833,23.02,76.46,-26.10,-23.11,-36.73,-28.65,,,,,,,,pick_x-160_z280,,,"(-154.7,+281.3)","(-154.7,+281.3)",yes,False,,,,,,,,,,
2026-07-03T19:02:20.302389,run_0001_20260703_190132,1,,,,,,,-196.5,217.1,,,202.4,251.7,,,0.00,1.40,0.70,0.43,2.10,-30.49,0,,,,,,,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_190132,,,,,,,,,,,,,,,,,,,,pick_x-200_z220,,,"(-196.5,+217.1)","(-196.5,+217.1)",,,,,,,,,,,,
2026-07-03T19:03:31.182819,run_0001_20260703_190230,1,,,,,,,-116.5,283.0,,,201.8,251.7,,,0.08,1.49,0.96,0.00,-2.10,-30.32,1,,-114.9,297.3,-3.6,12.7,13.2,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_190230,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_190230/run_0001_20260703_190230_stage4_pick_tip.jpg,,,,,fk_row_423959,15.38,74.44,-14.85,-19.68,-52.38,-28.56,,,,,,,,pick_x-120_z280,,,"(-116.5,+283.0)","(-116.5,+283.0)",,,,,,,,,,,,
2026-07-03T19:04:31.679524,run_0001_20260703_190343,1,,,,,,,-101.0,290.4,,,202.4,251.7,,,1.49,1.66,0.43,-0.17,1.75,-30.84,0,,,,,,,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_190343,,,,,,,,,,,,,,,,,,,,pick_x-100_z300,,,"(-101.0,+290.4)","(-101.0,+290.4)",,,,,,,,,,,,
2026-07-03T19:05:26.901715,run_0001_20260703_190434,1,,,,,,,-75.2,286.3,,,202.5,251.8,,,-0.17,1.23,0.79,-0.52,1.93,-30.58,0,,,,,,,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_190434,,,,,,,,,,,,,,,,,,,,pick_x-80_z280,,,"(-75.2,+286.3)","(-75.2,+286.3)",,,,,,,,,,,,
2026-07-03T19:06:48.047629,run_0001_20260703_190551,1,,,,,,,-169.3,256.3,,,202.1,251.4,,,0.08,1.14,0.79,-0.26,73.65,-30.41,0,,,,,,,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_190551,,,,,,,,,,,,,,,,,,,,pick_x-160_z260,,,"(-169.3,+256.3)","(-169.3,+256.3)",,,,,,,,,,,,
2026-07-03T19:11:21.143717,run_0001_20260703_191025,1,,,,,,,-167.0,261.7,,,202.1,252.2,,,0.08,1.31,0.79,-0.35,1.40,-29.79,0,,,,,,,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191025,,,,,,,,,,,,,,,,,,,,pick_x-160_z260,,,"(-167.0,+261.7)","(-167.0,+261.7)",,,,,,,,,,,,
2026-07-03T19:13:11.883410,run_0001_20260703_191124,1,yes,2枚目でグリッパーの爪が赤ブロックを両側から挟んでおり、ブロックがグリッパーと一体化して位置が上方にシフトしている。床面との接触が見られず持ち上がっていると判断。,0.82,yes,白い箱の内側に赤い四角いブロックが明確に確認できる,0.97,-132.9,281.4,218.6,250.6,200.9,251.6,True,17.7,-0.43,1.49,1.93,0.26,0.08,-29.61,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191124/run_0001_20260703_191124_place.jpg,-121.5,283.4,4.3,-3.1,5.3,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191124,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191124/run_0001_20260703_191124_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191124/run_0001_20260703_191124_pick.jpg,,,,fk_row_444344,26.89,80.33,-18.01,3.60,-52.38,-30.58,fk_row_807127,-40.25,67.76,4.04,-1.58,-80.94,-29.61,pick_x-140_z280,,,"(-132.9,+281.4)","(-132.9,+281.4)",yes,False,,,,,,,,,,
2026-07-03T19:14:11.994737,run_0001_20260703_191322,1,,,,,,,-155.5,288.5,,,193.0,214.0,,,0.17,1.40,0.96,-0.52,-0.35,-29.61,0,,,,,,,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191322,,,,,,,,,,,,,,,,,,,,pick_x-160_z280,2.73,-0.95,"(-155.5,+288.5)","(-158.2,+289.5)",,,,,,,,,,,,
2026-07-03T19:15:25.438753,run_0001_20260703_191414,1,yes,2枚目でグリッパーの爪が赤ブロックを両側から挟んでおり、ブロックがグリッパーと同じ高さに位置している。1枚目と比べてアームが収縮・上昇しており、赤ブロックがグリッパーと一体として持ち上がっていると判断できる。,0.82,,,,-135.4,297.5,,,192.4,214.2,,,0.00,1.49,1.75,0.52,-0.96,-29.70,1,,-127.6,299.4,3.7,-0.2,3.7,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191414,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191414/run_0001_20260703_191414_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191414/run_0001_20260703_191414_pick.jpg,,,,fk_row_232074,22.50,88.94,-34.80,-8.34,-47.10,-29.35,,,,,,,,pick_x-140_z300,,,"(-135.4,+297.5)","(-135.4,+297.5)",yes,False,,,,,,,,,,
2026-07-03T19:16:33.494283,run_0001_20260703_191533,1,,,,,,,-156.5,273.1,,,203.6,207.8,,,-0.17,1.31,0.79,-0.35,66.70,-30.41,0,,,,,,,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191533,,,,,,,,,,,,,,,,,,,,pick_x-160_z280,2.73,-0.95,"(-156.5,+273.1)","(-159.2,+274.1)",,,,,,,,,,,,
2026-07-03T19:18:24.774453,run_0001_20260703_191709,1,,,,,,,-148.4,291.1,,,201.4,208.0,,,-0.35,1.49,0.96,-0.26,26.36,-29.61,1,,-139.1,300.5,4.7,-1.3,4.8,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191709,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191709/run_0001_20260703_191709_stage4_pick_tip.jpg,,,,,fk_row_458523,28.38,87.53,-38.40,3.42,-37.00,-29.88,,,,,,,,pick_x-140_z300,3.72,-0.21,"(-148.4,+291.1)","(-152.1,+291.3)",,,,,,,,,,,,
2026-07-03T19:20:56.227142,run_0001_20260703_191945,1,,,,,,,-158.9,279.0,,,201.7,207.6,,,-0.17,1.40,0.87,-0.26,37.00,-30.32,1,,-146.3,287.5,-3.3,0.3,3.3,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191945,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_191945/run_0001_20260703_191945_stage4_pick_tip.jpg,,,,,fk_row_457937,28.56,83.05,-29.70,-3.51,-40.07,-29.35,,,,,,,,pick_x-160_z280,2.73,-0.95,"(-158.9,+279.0)","(-161.6,+280.0)",,,,,,,,,,,,
2026-07-03T19:22:16.098637,run_0001_20260703_192106,1,yes,2枚目でグリッパーの爪の間に赤ブロックが挟まれており、アームが持ち上がった状態でもブロックがグリッパーと共に位置しているため、床から浮いていると判断できる,0.82,,,,254.6,205.7,,,-177.2,238.5,,,-0.61,1.58,1.93,-0.61,-1.49,-29.61,1,,256.0,190.0,-2.7,-12.2,12.5,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192106,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192106/run_0001_20260703_192106_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192106/run_0001_20260703_192106_pick.jpg,,,,fk_row_40654,-49.83,87.89,-30.05,1.93,-52.47,-33.66,,,,,,,,pick_x260_z200,,,"(+254.6,+205.7)","(+254.6,+205.7)",yes,False,,,,,,,,,,
2026-07-03T19:23:53.176446,run_0001_20260703_192243,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比較してアームの姿勢が変化しているにもかかわらず赤ブロックがグリッパーと同じ位置関係を保っている。床面には赤ブロックが見当たらないため、持ち上げられていると判断。,0.82,,,,248.4,179.9,,,-177.4,238.8,,,-0.35,1.49,1.93,-0.52,-1.40,-29.61,1,,252.4,161.4,2.2,3.9,4.5,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192243,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192243/run_0001_20260703_192243_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192243/run_0001_20260703_192243_pick.jpg,,,,fk_row_766399,-52.47,89.03,-18.89,3.86,-70.57,-30.49,,,,,,,,pick_x240_z180,,,"(+248.4,+179.9)","(+248.4,+179.9)",yes,False,,,,,,,,,,
2026-07-03T19:26:23.424974,run_0001_20260703_192446,1,yes,2枚目の画像でグリッパーが閉じており、赤ブロックがグリッパーの爪の間に挟まれた状態で位置が上方にシフトしている。1枚目と比べてブロックがグリッパーと一体的に動いており、床から浮いていると判断できる。ただし真上からの視点のため高さの変化を直接確認できず確信度はやや低め。,0.75,no,赤い四角のブロックは白い箱の外側（箱の上方）に独立して置かれており、箱の中には入っていない,0.95,158.5,282.5,-130.6,195.2,-163.1,238.5,False,54.1,-0.26,1.49,1.93,0.35,-0.96,-29.61,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192446/run_0001_20260703_192446_place.jpg,165.2,276.4,2.6,-6.1,6.6,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192446,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192446/run_0001_20260703_192446_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192446/run_0001_20260703_192446_pick.jpg,,,,fk_row_21375,-32.25,89.03,-32.95,-11.33,-51.67,-29.70,fk_row_817900,33.66,56.60,25.31,-0.52,-80.94,-29.88,pick_x160_z280,1.21,-12.87,"(+158.5,+282.5)","(+157.3,+295.3)",yes,False,,,,,,,,,,
2026-07-03T19:27:43.203671,run_0001_20260703_192632,1,yes,2枚目でグリッパーが閉じており、赤ブロックが爪の間に挟まれた状態で位置が1枚目より上方にシフトしている。床面との接触が見られず、リフトされていると判断。,0.82,,,,262.8,187.0,,,-216.1,87.3,,,1.23,1.49,1.93,-0.70,1.31,-29.26,1,,265.8,174.6,0.5,-8.8,8.9,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192632,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192632/run_0001_20260703_192632_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192632/run_0001_20260703_192632_pick.jpg,,,,fk_row_40668,-49.65,87.80,-30.05,10.81,-52.29,-34.71,,,,,,,,pick_x260_z180,,,"(+262.8,+187.0)","(+262.8,+187.0)",yes,False,,,,,,,,,,
2026-07-03T19:29:00.272264,run_0001_20260703_192811,1,,,,,,,228.0,161.0,,,-248.2,77.0,,,0.17,1.49,0.61,0.87,1.84,-29.26,0,,,,,,,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192811,,,,,,,,,,,,,,,,,,,,pick_x220_z160,,,"(+228.0,+161.0)","(+228.0,+161.0)",,,,,,,,,,,,
2026-07-03T19:29:40.932775,run_0001_20260703_192926,1,,,,,,,,,,,,,,,-0.17,1.49,0.70,-0.43,2.37,-29.70,,,,,,,,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_192926,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
2026-07-03T19:31:34.645567,run_0001_20260703_193018,1,,,,,,,233.9,158.2,,,-248.3,77.2,,,-0.17,1.14,0.79,-0.26,89.91,-29.53,1,,246.5,139.4,-11.4,-4.6,12.3,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193018,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193018/run_0001_20260703_193018_stage4_pick_tip.jpg,,,,,fk_row_716305,-56.33,83.93,-5.18,1.75,-73.30,-32.08,,,,,,,,pick_x240_z160,,,"(+233.9,+158.2)","(+233.9,+158.2)",,,,,,,,,,,,
2026-07-03T19:33:01.791715,run_0001_20260703_193147,1,yes,2枚目でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比較してブロックの位置がグリッパーと共に移動・上昇しているため、持ち上げられていると判断できる,0.82,,,,254.1,184.7,,,-247.9,77.9,,,1.58,1.49,1.93,-0.87,-0.43,-29.53,1,,262.5,179.9,6.8,3.8,7.8,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193147,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193147/run_0001_20260703_193147_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193147/run_0001_20260703_193147_pick.jpg,,,,fk_row_40661,-50.09,87.80,-29.88,6.59,-52.11,-34.45,,,,,,,,pick_x260_z180,0.50,-8.84,"(+254.1,+184.7)","(+253.6,+193.6)",yes,False,,,,,,,,,,
2026-07-03T19:34:15.660643,run_0001_20260703_193315,1,,,,,,,165.9,205.3,,,-180.4,128.4,,,0.08,1.49,1.05,-0.79,-2.28,-29.61,1,,175.7,194.0,0.1,2.3,2.3,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193315,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193315/run_0001_20260703_193315_stage4_pick_tip.jpg,,,,,fk_row_807182,-39.63,77.60,10.63,-0.87,-80.85,-29.70,,,,,,,,pick_x160_z200,,,"(+165.9,+205.3)","(+165.9,+205.3)",,,,,,,,,,,,
2026-07-03T19:35:33.024688,run_0001_20260703_193422,1,yes,2枚目でグリッパーの爪の間に赤ブロックが挟まれており、ブロックの位置が1枚目より下方（アーム先端方向）に移動していることから、床から離れて持ち上がっていると判断できる,0.82,,,,126.2,277.1,,,-180.1,128.3,,,-0.26,1.58,1.93,0.26,-0.43,-29.79,1,,144.1,262.4,6.2,-6.1,8.8,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193422,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193422/run_0001_20260703_193422_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193422/run_0001_20260703_193422_pick.jpg,,,,fk_row_772479,-23.99,88.94,-18.98,3.60,-69.16,-29.97,,,,,,,,pick_x120_z280,,,"(+126.2,+277.1)","(+126.2,+277.1)",yes,False,,,,,,,,,,
2026-07-03T19:38:01.826075,run_0001_20260703_193536,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比較してブロックの位置がグリッパーと共に移動・上昇している様子が確認できる,0.82,,|fails:7:not_reached,,212.9,259.7,,,-180.2,127.7,,,-33.57,100.54,-38.05,2.02,-64.07,-30.67,1,,215.6,245.8,-0.3,-13.4,13.4,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193536,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193536/run_0001_20260703_193536_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193536/run_0001_20260703_193536_pick.jpg,,,,fk_row_21369,-32.08,89.03,-37.00,17.66,-47.63,-30.32,,,,,,,,pick_x220_z260,-2.81,-10.32,"(+212.9,+259.7)","(+215.7,+270.0)",yes,False,,,,,,,,,,
2026-07-03T19:39:21.355428,run_0001_20260703_193814,1,yes,"2枚目の画像では赤ブロックがグリッパーの爪の間に挟まれた状態で、1枚目より明らかに位置が変化しており（腕全体が伸びた方向に移動）、床面から浮いて持ち上げられていると判断できる|fails:4:not_reached,6:not_reached",0.82,,,,242.8,206.3,,,-180.1,127.6,,,35.77,59.76,17.66,1.14,-86.57,-28.82,1,,254.7,186.8,3.7,-6.8,7.7,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193814,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193814/run_0001_20260703_193814_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_193814/run_0001_20260703_193814_pick.jpg,,,,fk_row_11835,-51.32,88.33,-27.94,-0.87,-53.87,-29.61,,,,,,,,pick_x240_z200,,,"(+242.8,+206.3)","(+242.8,+206.3)",yes,False,,,,,,,,,,
2026-07-03T19:41:36.411707,run_0001_20260703_194006,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比較してブロックの位置がグリッパーとともに持ち上がっている様子が確認できる,0.82,yes,白い箱の内側に赤い四角いブロックが明確に見える|fails:7:not_reached,0.97,207.1,256.9,-177.4,202.5,-194.4,178.5,True,29.4,37.96,58.27,18.72,0.96,-88.68,-29.35,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194006/run_0001_20260703_194006_place.jpg,213.4,248.5,4.8,-7.4,8.8,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194006,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194006/run_0001_20260703_194006_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194006/run_0001_20260703_194006_pick.jpg,,,,fk_row_21369,-31.55,89.03,-37.08,17.49,-47.46,-30.49,fk_row_819643,46.23,51.24,28.03,-0.96,-76.20,-29.00,pick_x200_z260,3.93,-7.99,"(+207.1,+256.9)","(+203.1,+264.9)",yes,False,,,,,,,,,,
2026-07-03T19:42:57.928253,run_0001_20260703_194205,1,yes,2枚目の画像では赤ブロックがグリッパーの爪に挟まれたまま1枚目より低い位置（グリッパーが下方向に移動）に見えるが、ブロックは爪の間に保持されており床面から浮いている状態と判断できる,0.82,,,,261.6,209.9,,,8.1,185.3,,,0.96,59.85,17.92,-0.87,-88.50,-29.61,1,,255.4,205.1,-9.2,-0.2,9.2,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194205,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194205/run_0001_20260703_194205_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194205/run_0001_20260703_194205_pick.jpg,,,,fk_row_16567,-42.01,89.03,-37.52,17.75,-47.28,-31.20,,,,,,,,pick_x260_z200,-2.75,-12.16,"(+261.6,+209.9)","(+264.4,+222.1)",yes,False,,,,,,,,,,
2026-07-03T19:45:05.008951,run_0001_20260703_194404,1,yes,2枚目の画像では赤ブロックがグリッパーの爪の間に保持されたまま床面から離れ、グリッパーと共に持ち上がっている様子が確認できる。1枚目と比較してブロックの位置がグリッパー内部に収まっており、床のマット面との接触がなくなっている。,0.82,,|fails:7:not_reached,,261.5,228.6,,,6.0,185.8,,,0.87,58.62,17.31,-0.52,-70.22,-29.88,1,,257.4,202.2,-7.6,-29.0,29.9,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194404,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194404/run_0001_20260703_194404_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194404/run_0001_20260703_194404_pick.jpg,,,,fk_row_16567,-42.27,89.03,-37.08,18.36,-47.81,-31.11,,,,,,,,pick_x260_z220,-2.56,-5.87,"(+261.5,+228.6)","(+264.0,+234.5)",yes,False,,,,,,,,,,
2026-07-03T19:49:14.891787,run_0001_20260703_194754,1,yes,2枚目の画像でグリッパーの爪が赤ブロックを両側から挟み込んでおり、ブロックがグリッパーと一体となって床面から浮いた状態に見える。1枚目と比較してブロックの位置がグリッパー内部に収まっており、持ち上げられていると判断できる。,0.82,unknown,swept_retreat_failed,0.0,211.6,244.1,,,-179.3,208.6,,,35.85,59.41,17.75,-0.52,-86.48,-29.00,0,,216.1,237.5,-3.6,-2.0,4.1,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194754,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194754/run_0001_20260703_194754_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_194754/run_0001_20260703_194754_pick.jpg,,,,fk_row_16580,-41.66,89.03,-32.69,-5.62,-52.38,-30.23,fk_row_845375,39.81,68.37,13.62,0.00,-88.68,-30.05,pick_x220_z240,2.70,-7.90,"(+211.6,+244.1)","(+208.9,+252.0)",yes,False,,,,,,,,,,
2026-07-03T19:53:39.904021,run_0001_20260703_195219,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、アーム全体が下方向に伸びた姿勢で赤ブロックがグリッパーと共に位置している。1枚目と比較してアームの姿勢が変化し、赤ブロックがグリッパーに保持されたまま移動していることが確認できる。,0.85,no,赤いブロックは白い箱の外側（右上付近）に置かれており、箱の中には何も入っていない,0.95,209.9,203.0,-211.5,150.1,-202.0,188.0,True,39.1,0.00,1.58,1.93,-0.17,-2.37,-29.61,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195219/run_0001_20260703_195219_place.jpg,218.2,191.2,6.2,-5.2,8.1,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195219,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195219/run_0001_20260703_195219_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195219/run_0001_20260703_195219_pick.jpg,,,,fk_row_717773,-45.17,85.25,-10.28,0.96,-72.59,-32.25,fk_row_820796,53.70,60.11,17.92,-1.66,-80.59,-29.61,pick_x200_z200,,,"(+209.9,+203.0)","(+209.9,+203.0)",yes,False,,,,,,,,,,
2026-07-03T19:55:20.670760,run_0001_20260703_195359,1,yes,2枚目でグリッパーの爪の間に赤ブロックが挟まれており、アーム全体が後退・収縮した姿勢になっているため、ブロックが床から浮いて持ち上げられていると判断できる。,0.82,no,赤いブロックは白い箱の外側（箱の左上）に独立して置かれており、箱の中には入っていない,0.95,166.3,273.3,-207.4,142.0,-202.5,188.0,True,46.3,0.00,1.58,1.84,0.35,-2.46,-29.61,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195359/run_0001_20260703_195359_place.jpg,173.6,270.3,2.8,-2.0,3.4,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195359,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195359/run_0001_20260703_195359_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195359/run_0001_20260703_195359_pick.jpg,,,,fk_row_21382,-32.08,89.03,-32.78,-5.88,-51.76,-29.53,fk_row_820796,53.70,59.94,17.84,-1.05,-81.03,-30.05,pick_x160_z280,1.92,-9.48,"(+166.3,+273.3)","(+164.3,+282.8)",yes,False,,,,,,,,,,
2026-07-03T19:57:07.799830,run_0001_20260703_195537,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比較してブロックの位置が腕全体とともに上方にシフトしているため、床から持ち上げられていると判断できる,0.82,no,赤いブロックは白い箱の外側（右上）に独立して置かれており、箱の中には何も入っていない,0.97,151.6,293.2,-223.8,140.8,-201.9,188.5,False,52.4,0.17,1.49,1.75,0.17,-1.14,-29.53,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195537/run_0001_20260703_195537_place.jpg,156.9,282.5,1.8,-10.2,10.4,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195537,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195537/run_0001_20260703_195537_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195537/run_0001_20260703_195537_pick.jpg,,,,fk_row_119873,-27.68,89.03,-33.48,-4.13,-50.00,-31.37,fk_row_820796,53.52,59.94,17.92,-1.31,-76.81,-29.35,pick_x160_z300,,,"(+151.6,+293.2)","(+151.6,+293.2)",yes,False,,,,,,,,,,
2026-07-03T19:58:49.591796,run_0001_20260703_195716,1,yes,2枚目でグリッパーの爪が閉じており、赤ブロックがグリッパーと同じ高さに位置し床から浮いているように見える。ブロックの影も消えており持ち上がっていると判断。,0.82,yes,白い箱の内側に赤いひし形（四角）のブロックが明確に確認できる|fails:7:not_reached,0.97,140.6,296.7,-164.2,203.6,-202.6,188.2,True,41.4,0.70,1.58,1.93,0.00,1.66,-29.35,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195716/run_0001_20260703_195716_place.jpg,149.7,285.3,2.0,-8.9,9.2,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195716,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195716/run_0001_20260703_195716_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195716/run_0001_20260703_195716_pick.jpg,,,,fk_row_122288,-22.76,89.03,-33.39,5.62,-49.74,-32.51,fk_row_820796,53.61,59.94,17.84,-0.52,-77.87,-29.26,pick_x140_z300,,,"(+140.6,+296.7)","(+140.6,+296.7)",yes,False,,,,,,,,,,
2026-07-03T20:01:08.872479,run_0001_20260703_195905,1,no,"2枚目の画像でグリッパーは赤ブロックの位置から離れており、赤ブロックは依然として床面（マット上）に残っている。グリッパーが赤ブロックを挟んで持ち上げた様子が見られない。|fails:3:not_reached,pick_overall:pick_failed",0.82,unknown,swept_retreat_failed,0.0,164.4,285.7,,,-187.5,188.0,,,0.70,1.49,1.93,-0.79,-1.66,-29.26,0,,172.3,272.6,-46.7,18.3,50.2,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195905,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195905/run_0001_20260703_195905_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_195905/run_0001_20260703_195905_pick.jpg,,,,fk_row_119880,-28.65,88.41,-32.95,2.28,-47.98,-30.76,fk_row_846040,44.56,68.37,17.75,0.00,-87.18,-29.53,pick_x160_z280,2.36,-5.74,"(+164.4,+285.7)","(+162.0,+291.4)",no,False,,,,,,,,,,
2026-07-03T20:02:58.039899,run_0001_20260703_200121,1,yes,2枚目でグリッパーの爪の間に赤ブロックが挟まれており、アーム全体が持ち上がった状態でブロックがグリッパーと共に浮いている位置関係が確認できる,0.82,unknown,swept_retreat_failed|fails:7:not_reached,0.0,177.6,261.6,,,-218.2,147.7,,,0.61,1.58,2.10,-0.61,-1.49,-29.61,0,,194.0,247.5,2.8,-4.0,4.9,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_200121,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_200121/run_0001_20260703_200121_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_200121/run_0001_20260703_200121_pick.jpg,,,,fk_row_19029,-37.26,89.03,-28.12,-5.44,-57.12,-29.61,fk_row_847575,55.01,65.83,19.86,-0.35,-87.27,-29.00,pick_x180_z260,,,"(+177.6,+261.6)","(+177.6,+261.6)",yes,False,,,,,,,,,,
2026-07-03T20:05:53.933752,run_0001_20260703_200341,1,no,"2枚の画像でグリッパーの位置と姿勢がほぼ同一であり、赤ブロックは1枚目と同じ左下の位置に静止したままで、グリッパーに挟まれた様子が見られない。ブロックが持ち上がった形跡がない。|fails:3:not_reached,4:not_reached,5:gripper_close_fail,6:not_reached,regrasp_open:gripper_open_fail,pick_overall:pick_failed",0.85,unknown,"swept_retreat_failed|fails:7:not_reached,8:not_reached,9:gripper_open_fail,10:not_reached,10:not_reached",0.0,258.3,208.8,,,-218.8,158.5,,,-0.35,1.58,2.10,-0.26,0.96,-29.70,0,,117.1,289.5,-139.7,81.2,161.6,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_200341,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_200341/run_0001_20260703_200341_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_200341/run_0001_20260703_200341_pick.jpg,,,,fk_row_14200,,,,,,,fk_row_847365,,,,,,,pick_x260_z200,-5.99,-6.19,"(+258.3,+208.8)","(+264.3,+215.0)",no,False,,,,,,,,,,
2026-07-03T20:11:01.410882,run_0001_20260703_200942,1,yes,2枚目でグリッパーの爪の間に赤ブロックが挟まれており、アーム全体が伸展して床面から持ち上がった位置にある。ブロックが爪と共に移動しているため、リフト成功と判断。,0.82,yes,白い箱の内側に赤い四角いブロックが明確に確認できる,0.97,217.1,215.8,-230.5,195.6,-219.5,158.3,True,38.9,0.00,1.49,1.84,-0.35,-0.43,-29.61,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_200942/run_0001_20260703_200942_place.jpg,220.9,201.3,0.7,-10.0,10.0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_200942,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_200942/run_0001_20260703_200942_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_200942/run_0001_20260703_200942_pick.jpg,,,,fk_row_768319,-43.33,89.03,-18.98,3.16,-69.60,-29.70,fk_row_847340,53.61,63.28,17.75,-0.26,-87.53,-29.61,pick_x220_z220,,,"(+217.1,+215.8)","(+217.1,+215.8)",yes,False,,,,,,,,,,
2026-07-03T20:12:38.679395,run_0001_20260703_201115,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比較してブロックの位置がグリッパーとともに持ち上がっている様子が確認できる。ただし真上からの視点のため高さの変化が直接確認しづらく、若干の不確実性がある。,0.82,no,赤いブロックは白い箱の外側（箱の左上方）に置かれており、箱の中には入っていない,0.95,175.0,271.5,-178.2,132.2,-220.4,158.8,True,49.9,1.58,1.66,2.02,-0.26,-1.40,-29.61,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_201115/run_0001_20260703_201115_place.jpg,183.1,264.0,5.1,-6.6,8.3,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_201115,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_201115/run_0001_20260703_201115_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_201115/run_0001_20260703_201115_pick.jpg,,,,fk_row_21389,-32.34,89.03,-32.78,-0.96,-51.59,-29.79,fk_row_847319,54.14,59.50,29.70,-0.26,-90.52,-30.05,pick_x180_z280,2.55,-7.11,"(+175.0,+271.5)","(+172.5,+278.6)",yes,False,,,,,,,,,,
2026-07-03T20:18:51.698452,run_0001_20260703_201717,1,yes,2枚目でグリッパーの爪の間に赤ブロックが挟まれたまま腕全体が持ち上がっており、ブロックの位置がマット面から浮いている状態に見える,0.82,no,赤い四角のブロックは白い箱の外側（箱の上方）に独立して置かれており、箱の中には入っていない,0.95,157.1,274.1,-206.7,125.1,-219.7,158.9,True,36.2,0.79,1.58,2.10,-0.52,-0.87,-29.53,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_201717/run_0001_20260703_201717_place.jpg,165.6,277.0,9.0,1.4,9.1,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_201717,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_201717/run_0001_20260703_201717_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_201717/run_0001_20260703_201717_pick.jpg,,,,fk_row_122302,-22.50,88.85,-33.66,15.29,-49.39,-32.95,fk_row_847319,54.05,59.50,29.70,-0.52,-90.70,-30.14,pick_x160_z280,2.36,-5.74,"(+157.1,+274.1)","(+154.8,+279.8)",yes,False,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_201717/run_0001_20260703_201717_ap_place_side.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_201717/run_0001_20260703_201717_place_side.jpg,no,0.75,赤いブロックは箱の縁付近に位置しているが、箱の内側に完全に収まって縁以下まで下りているとは確認できず、箱の上端付近または縁に引っかかっているように見える,,,,,
2026-07-03T20:23:55.371332,run_0001_20260703_202225,1,yes,2枚目でグリッパーの爪が赤ブロックを両側から挟み込み、ブロックがグリッパーと共に持ち上がった位置にある。床面との接触が見られず浮いていると判断できる。,0.82,yes,白い箱の内側に赤い四角いブロックが明確に見える,0.97,188.7,271.1,-207.2,217.8,-219.5,158.5,False,60.5,-0.08,1.58,2.10,-0.35,-2.46,-29.44,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202225/run_0001_20260703_202225_place.jpg,189.1,260.1,-1.7,-9.6,9.7,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202225,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202225/run_0001_20260703_202225_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202225/run_0001_20260703_202225_pick.jpg,,,,fk_row_21396,-31.64,89.03,-32.87,3.77,-51.67,-30.84,fk_row_847319,53.61,59.67,29.79,0.17,-91.05,-29.44,pick_x180_z280,3.83,-6.83,"(+188.7,+271.1)","(+184.9,+278.0)",yes,False,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202225/run_0001_20260703_202225_ap_place_side.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202225/run_0001_20260703_202225_place_side.jpg,no,0.82,赤いブロックはロボットアームの爪に掴まれた状態で箱の縁付近にあるが、箱の内側に収まらず箱の縁の上または外側に位置しており、箱の高さ以下まで下りていない,place_x-220_z160,-20.82,17.85,"(-219.5,+158.5)","(-198.6,+140.6)"
2026-07-03T20:25:44.372024,run_0001_20260703_202400,1,yes,"2枚目でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比べてブロックの位置がグリッパーと共に移動しているため、床から持ち上がっていると判断できる|fails:4:not_reached,6:not_reached",0.82,no,赤いブロックはグリッドマット上の中央下部に単独で置かれており、白い箱（右側）の外にある,0.97,179.1,259.2,-30.7,302.9,-226.1,161.5,False,241.3,0.08,1.58,2.10,-0.35,-2.46,-29.79,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202400/run_0001_20260703_202400_place.jpg,184.7,256.3,5.9,5.7,8.2,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202400,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202400/run_0001_20260703_202400_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202400/run_0001_20260703_202400_pick.jpg,,,,fk_row_21446,-31.46,88.33,-27.94,3.86,-54.05,-30.32,fk_row_820761,53.70,53.87,27.94,-1.31,-79.89,-29.00,pick_x180_z260,2.77,-4.00,"(+179.1,+259.2)","(+176.4,+263.2)",yes,False,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202400/run_0001_20260703_202400_ap_place_side.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202400/run_0001_20260703_202400_place_side.jpg,no,0.97,赤いブロックは箱の外側（作業台上）に置かれており、箱から離れた位置にある,place_x-220_z160,-10.64,12.51,"(-226.1,+161.5)","(-215.5,+149.0)"
2026-07-03T20:27:29.608296,run_0001_20260703_202550,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比べてブロックの位置が爪と共に上方に移動している様子が確認できる。ブロックが床面から浮いていると判断できる。,0.82,yes,白い正方形の箱の内側に赤い四角いブロックが明確に確認できる,0.97,168.6,276.9,-230.9,184.3,-226.0,160.8,True,24.0,0.17,1.58,2.10,-0.70,-2.46,-29.61,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202550/run_0001_20260703_202550_place.jpg,177.2,270.1,2.1,-7.4,7.7,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202550,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202550/run_0001_20260703_202550_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202550/run_0001_20260703_202550_pick.jpg,,,,fk_row_117465,-33.39,89.03,-33.39,-8.17,-50.09,-31.37,fk_row_820761,53.52,53.08,27.59,-2.10,-76.64,-29.44,pick_x160_z280,5.67,-2.18,"(+168.6,+276.9)","(+162.9,+279.1)",yes,False,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202550/run_0001_20260703_202550_ap_place_side.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202550/run_0001_20260703_202550_place_side.jpg,unknown,0.6,ロボットアームの先端（グリッパー）が白い箱の上付近に位置しているが、赤いブロックが存在するか、また箱の中に収まっているかを画像から明確に確認できない。グリッパーが何かを掴んでいる可能性はあるが、赤いブロックの存在自体が不鮮明で判別不能。,place_x-220_z160,-10.64,12.51,"(-226.0,+160.8)","(-215.4,+148.3)"
2026-07-03T20:29:22.514063,run_0001_20260703_202741,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比べてブロックの位置がグリッパーと共に移動・上昇しているように見える。ブロックがグリッパー内に保持されていると判断できる。,0.82,yes,白い箱の内側に赤い四角いブロックが明確に写っている|fails:10:not_reached,0.97,201.1,267.0,-209.0,212.2,-217.4,178.5,True,34.7,0.17,1.58,1.84,-0.35,50.97,-29.79,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202741/run_0001_20260703_202741_place.jpg,198.4,253.1,-2.7,-13.9,14.2,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202741,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202741/run_0001_20260703_202741_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202741/run_0001_20260703_202741_pick.jpg,,,,fk_row_21403,-32.25,89.03,-32.95,7.82,-51.85,-30.32,fk_row_820144,49.30,62.66,14.15,-0.87,-80.94,-29.26,pick_x200_z260,4.35,-7.68,"(+201.1,+267.0)","(+196.7,+274.7)",yes,False,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202741/run_0001_20260703_202741_ap_place_side.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_202741/run_0001_20260703_202741_place_side.jpg,no,0.8,赤いブロックはロボットアームのグリッパーに把持されており、箱の縁より上の位置にあり、まだ箱の内側に収まっていない,place_x-220_z180,,,"(-217.4,+178.5)","(-217.4,+178.5)"
2026-07-03T20:31:44.282689,run_0001_20260703_203003,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、アーム全体が下方向に伸びた姿勢で赤ブロックがグリッパーと一体となって位置している。1枚目と比較してアームの姿勢が変化しているが赤ブロックはグリッパーに保持されたまま移動しており、床から浮いていると判断できる。,0.82,no,赤いブロックは白い箱の外側（箱の手前）に置かれており、箱の内部には入っていない|fails:7:not_reached,0.85,256.0,175.1,-242.8,134.7,-237.8,117.8,True,17.6,-0.43,1.58,1.93,-0.08,1.40,-29.70,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203003/run_0001_20260703_203003_place.jpg,256.7,153.2,-6.6,-0.5,6.6,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203003,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203003/run_0001_20260703_203003_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203003/run_0001_20260703_203003_pick.jpg,,,,fk_row_766079,-54.22,89.03,-18.80,4.13,-70.31,-30.32,fk_row_848655,63.01,60.82,22.41,-0.96,-86.39,-29.97,pick_x260_z180,3.65,-2.51,"(+256.0,+175.1)","(+252.3,+177.6)",yes,False,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203003/run_0001_20260703_203003_ap_place_side.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203003/run_0001_20260703_203003_place_side.jpg,no,0.82,赤いブロックは白い箱の内側ではなく、箱の外側（手前側面）に接した状態で表面に置かれており、箱の縁の高さ以下まで収まっていない,place_x-240_z120,,,"(-237.8,+117.8)","(-237.8,+117.8)"
2026-07-03T20:33:30.069111,run_0001_20260703_203157,1,yes,2枚目の画像では赤ブロックがグリッパーの爪の間に挟まれた状態で、1枚目より明らかに持ち上がった位置（床から浮いた状態）に見える。ブロックの位置がグリッパーと一体となって移動しており、床面との接触が確認できない。,0.82,unknown,swept_retreat_failed,0.0,138.1,278.6,,,-136.6,208.8,,,0.00,1.58,1.93,0.43,3.16,-29.61,0,,150.6,258.5,1.4,-13.0,13.1,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203157,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203157/run_0001_20260703_203157_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203157/run_0001_20260703_203157_pick.jpg,,,,fk_row_772159,-25.57,89.03,-18.89,3.51,-69.52,-30.14,fk_row_844225,32.51,59.58,29.70,0.35,-89.29,-29.17,pick_x140_z280,,,"(+138.1,+278.6)","(+138.1,+278.6)",yes,False,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203157/run_0001_20260703_203157_ap_place_side.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203157/run_0001_20260703_203157_place_side.jpg,no,0.82,赤いブロックは白い箱の縁より上に突き出ており、箱の高さ以下まで収まっていない,place_x-140_z200,,,"(-136.6,+208.8)","(-136.6,+208.8)"
2026-07-03T20:35:38.205818,run_0001_20260703_203351,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比較してブロックの位置が持ち上がってグリッパーと一体化している様子が確認できる|fails:1:not_reached,0.82,yes,白い箱の内側に赤い四角いブロックが明確に確認できる|fails:10:not_reached,0.97,158.8,251.0,-147.3,248.2,-146.8,238.1,True,10.2,-0.17,1.66,1.93,-0.52,-2.28,-29.61,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203351/run_0001_20260703_203351_place.jpg,171.0,239.1,3.6,-9.4,10.1,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203351,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203351/run_0001_20260703_203351_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203351/run_0001_20260703_203351_pick.jpg,,,,fk_row_719676,-31.90,87.97,-14.94,1.23,-71.71,-31.72,fk_row_844060,30.67,71.10,8.61,0.35,-88.15,-29.61,pick_x160_z260,,,"(+158.8,+251.0)","(+158.8,+251.0)",yes,False,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203351/run_0001_20260703_203351_ap_place_side.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203351/run_0001_20260703_203351_place_side.jpg,no,0.85,赤いブロックは白い箱の縁より上に位置しており、箱の内側に収まっていない。ブロックは箱の外側・上部に載っている状態に見える。,place_x-140_z240,,,"(-146.8,+238.1)","(-146.8,+238.1)"
2026-07-03T20:37:34.180523,run_0001_20260703_203547,1,yes,2枚目でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比べてブロックの位置がグリッパーと共に移動・上昇しているため、持ち上げられていると判断できる,0.82,no,"赤いブロックはカッティングマットの左下に置かれており、白い箱（右側）の外にある|fails:7:not_reached,10:not_reached",0.95,182.9,283.4,-57.6,250.8,-167.5,168.1,False,137.5,1.05,1.66,1.93,0.26,-1.05,-29.35,0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203547/run_0001_20260703_203547_place.jpg,182.8,264.0,-4.2,-18.8,19.3,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203547,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203547/run_0001_20260703_203547_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203547/run_0001_20260703_203547_pick.jpg,,,,fk_row_21389,-32.34,89.03,-32.69,-1.05,-51.67,-29.70,fk_row_845993,44.38,58.79,29.88,0.26,-86.74,-29.53,pick_x180_z280,1.07,-8.20,"(+182.9,+283.4)","(+181.9,+291.6)",yes,False,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203547/run_0001_20260703_203547_ap_place_side.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203547/run_0001_20260703_203547_place_side.jpg,no,0.95,赤いブロックは作業台の左端に置かれており、白い箱からは大きく離れた位置にある。ロボットアームは白い箱の方向を向いているが、赤いブロックはまだ箱の外側にある。,place_x-160_z160,,,"(-167.5,+168.1)","(-167.5,+168.1)"
2026-07-03T20:39:16.755307,run_0001_20260703_203809,1,yes,2枚目でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比較してブロックの位置がグリッパーと共に移動しており床から離れていると判断できる|fails:6:not_reached,0.82,,,,261.4,208.8,,,-169.8,117.6,,,33.39,59.85,17.66,-0.43,-87.18,-28.91,1,,273.3,183.4,-4.4,-10.2,11.1,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203809,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203809/run_0001_20260703_203809_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203809/run_0001_20260703_203809_pick.jpg,,,,fk_row_14166,-46.31,88.94,-38.84,18.19,-47.81,-29.44,,,,,,,,pick_x260_z200,-5.99,-6.19,"(+261.4,+208.8)","(+267.4,+215.0)",yes,False,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203809/run_0001_20260703_203809_ap_place_side.jpg,,,,,place_x-160_z120,,,"(-169.8,+117.6)","(-169.8,+117.6)"
2026-07-03T20:40:52.109979,run_0001_20260703_203944,1,yes,2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比較してブロックの位置が上昇し、グリッパーと一体化して持ち上がっているように見える,0.82,,|fails:7:not_reached,,53.2,312.5,,,-169.0,117.7,,,32.69,58.79,17.31,0.26,-59.58,-29.61,1,,66.4,305.4,-6.1,0.5,6.1,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203944,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203944/run_0001_20260703_203944_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203944/run_0001_20260703_203944_pick.jpg,,,,fk_row_160125,-1.23,88.06,-27.07,19.42,-57.21,-31.81,,,,,,,,pick_x60_z320,,,"(+53.2,+312.5)","(+53.2,+312.5)",yes,False,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_203944/run_0001_20260703_203944_ap_place_side.jpg,,,,,place_x-160_z120,,,"(-169.0,+117.7)","(-169.0,+117.7)"
2026-07-03T20:46:05.428056,run_0001_20260703_204419,1,yes,2枚目でグリッパーの爪の間に赤ブロックが挟まれた状態で位置が上方にシフトしており、床面から浮いている様子が確認できる|fails:6:not_reached,0.82,unknown,swept_retreat_failed,0.0,176.0,273.0,,,-163.6,211.6,,,0.08,1.58,1.93,-0.79,-1.14,-29.44,0,,179.6,266.7,1.5,-5.8,6.0,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_204419,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_204419/run_0001_20260703_204419_stage4_pick_tip.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_204419/run_0001_20260703_204419_pick.jpg,,,,fk_row_21389,-31.37,89.03,-32.69,-0.87,-50.62,-29.79,fk_row_845109,39.02,59.50,29.70,-0.17,-89.56,-29.88,pick_x180_z280,-1.55,-13.51,"(+176.0,+273.0)","(+177.6,+286.5)",yes,False,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_204419/run_0001_20260703_204419_ap_place_side.jpg,/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_204419/run_0001_20260703_204419_place_side.jpg,no,0.82,赤いブロックはロボットアームの爪に把持された状態で箱の縁付近に位置しており、箱の内側に収まっておらず、まだ箱の上方または縁付近に浮いている状態に見える,place_x-160_z220,-9.57,24.46,"(-163.6,+211.6)","(-154.0,+187.1)"
レビューが実機を止めた
物理リスクのある変更なので、実機の前に codex レビューを通した。結果は ❌ が 2 件、実機テストはまだ進めない判定。レビュー全文。
重要 Findings
❌ B1 NG（台帳 67列移行）
LEDGER_COLS は 67 列、row も 67 列分を組んでいますが、既存 pick_place_ledger_v3.csv が 57 列ヘッダのままだと完全一致ガードで必ず [ABORT-APPEND] になり、追記されません。
該当: run_pick_place.py:191, run_pick_place.py:880
修正案: 57列ヘッダを検出したら 67列ヘッダへ安全にマイグレーションする、または実機前に台帳ヘッダを 67列へ更新する明示手順を入れる。
❌ C1 NG（未到達後の姿勢起点）
未到達後も、次 stage は現在実姿勢を起点に再計算していません。stage6_lift は approach_pick_target、stage8 の ref_joints と stage10 lift_back は approach_place_target という「届いたはずの理論姿勢」を使います。
該当: run_pick_place.py:1600, run_pick_place.py:1663, run_pick_place.py:1826, run_pick_place.py:1844
修正案: arrived=False 時は直近 post_actual を次 stage の参照姿勢に使う、または未到達後の大移動だけを明示的に安全制限する。
項目別
- ✅ A1 OK — _send_and_wait_arrival() は残り、moves[].arrived に記録。未到達でも stage_lookup_and_move() は stage_failures 記録後に継続します。
- ✅ A2 OK — grip close/open 失敗は stage_failures に積まれ、後続へ進みます。
- ✅ A3 OK — PLACE 補正符号は 実測 - 目標 を保存し、次回 target - Δ で適用。実設置が箱より +Δ なら次回目標は -Δ 方向で、PICK側と整合しています。該当: run_pick_place.py:600, run_pick_place.py:1981
- ✅ A4 OK — PLACE 補正は stage_lookup_and_move("place_withblock", target_xz_place...) の前に適用されています。
- ✅ A5 OK — place_side は judgments["place_side"] に別記録のみ。上カメラ judgments["place"] の分岐には使っていません。
- ❌ B1 NG — 上記。既存57列台帳では追記停止リスク大。
- ✅ B2 OK — abort_recover_home / pickfail_to_home の実行経路は復活していません。HOME送信は stage10 到達後の stage10_to_home のみ。
- ✅ B3 OK — 補正キーは pick_x... / place_x... で名前空間分離されています。
- ⚠️ B4 要注意 — 正常フロー自体は通りますが、既存台帳が57列なら記録だけ止まります。
- ⚠️ C2 要注意 — stage10_to_home は到達失敗でも記録のみで終了し、自動リトライなし。ただし finally の home_recovery.attempted=False は、正常末尾のHOME試行済みrunでも誤解を招きます。auto_abort_recovery など別名が安全です。
- ✅ C3 OK — MAX_CORRECTION_MM=40.0 で補正適用時にクランプあり。
- ⚠️ C4 要注意 — FK系の pick/place descend は J1_SAFE_MAX=88 を通りますが、j0_lookup の approach 系は hard joint limit のみです。
- ✅ C5 OK — 横カメラ取得失敗/API失敗は unknown 記録で継続します。
- ✅ D1 OK — send_coords() 実呼び出しなし。
- ✅ D2 OK — release_all_servos() 実呼び出しなし。
- ✅ D3 OK — FK CSV は np.loadtxt() 読み取りのみ。
- ✅ D4 OK — 台帳ファイル名は pick_place_ledger_v3.csv のまま。
- ✅ D5 OK — 新規スクリプト追加は見当たりません。
サマリ
実機テストは まだ進めない方がよい です。最低限、B1 の台帳67列移行と C1 の未到達後に理論姿勢を前提にする問題を直してからが安全です。
検証として python3 -m py_compile run_pick_place.py は通りました。いま立っている場所
B1（台帳 57 → 67 列）は、実は当日のうちに移行が済んでいる。アップロードした pick_place_ledger_v3.csv のヘッダは 67 列で、LOG266 の 63 行が実際に追記されている。つまり codex が警告した 57 列ケースはこの台帳には当てはまらず、追記ガードは通っている。ガードが仕事をし、移行が行われた後だと見てよい。
C1（未到達後も理論姿勢を起点にする）は、アップロード版でも未対処のままだ。stage6_lift は approach_pick_target、stage8 place_withblock の ref_joints と stage10_lift_back は approach_place_target という理論姿勢を起点にしており、post_actual（実測姿勢）はログには残すが次移動の起点には使っていない。修正の軸は、arrived の真偽に関わらず直近の実測姿勢を次 stage の起点にすること。実測姿勢と理論姿勢が乖離した状態で大移動が走るのは物理リスクに直結するので、ここは直してから実機に戻る。
現在地は、止まる設計をやめて失敗を記録に残す形にしたところ。横カメラ判定と PLACE 補正記憶を入れ、PICK は 63 中 41 が掴まり、PLACE は効き始めたが 60mm・241mm・24mm と振動している。レビューで実機前の課題が 2 件出た。次は C1 を直し、再レビューを通してから実機に戻る。補正を仕込んで、まだ振動している途中の記録として、そのまま置いておく。
