mechArm 270 Pi 動的追従の限界を探る Log#56 | Pick & Place の全過程を映像+関節ログで残す。学習データ収集への第一歩
Pick & Place の1試行を、映像と関節データの同期ストリームとして丸ごと残す仕組みを作った。これまでカメラ画像は VLM 判定用に1試行6枚程度をイベント駆動で撮るだけだったが、模倣学習（LeRobot / SmolVLA 系）が要求するのは「一定周波数で関節状態と同期した連続記録」であり、まばらなスナップショットでは行動の軌跡として読めない。将来のテレオペによるデータ収集を見据え、その記録パイプラインを mechArm の現行本線で先に検証する——それが今回の目的になる。
設計には一つの絶対原則を置いた。記録係（recorder）は観測者であり、制御経路に一切入らない。recorder が起動していなくても、途中で死んでも、応答しなくても、本線の Pick & Place は一切影響を受けずに完走する。この原則を最後まで守れたかどうかが、今回の成否を分ける基準になった。
結果を先に書くと、実機併走の初回試行で pick=yes・place=yes の完走に成功し、89.7秒・2443フレームの720p映像と、送信時刻つきの関節指令ログ、82列に拡張した台帳の新4列がすべて自動で埋まった。recorder を止めた対照試行も従来どおり完走した。以下、計測から実機成功までの全過程を記録する。
計測が設計を決めた
実装の前に、設計を左右する未知数を2本の計測スクリプトで潰した。「測定してから推論する」の原則どおり、ここで出た数字がそのまま設計パラメータになった。
daemon ポーリング: 静止時 54.35Hz・エラー0——しかし片側検定
関節の測定値（measured）を recorder が別プロセスから取れるか。Pi 側 daemon（TCP:5555）への get_angles 連続ポーリングを計測した。
計測結果の summary を示す。静止時の実効 Hz とレイテンシ分布が設計判断の根拠になった数字そのものである。
{
"day": 287,
"started_at": "2026-07-15T16:02:24",
"ended_at": "2026-07-15T16:04:54",
"session_interrupted": false,
"call_timeout_sec": 2.0,
"phase3_target_hz": [30, 10, 5],
"daemon_client_mode": "single",
"daemon_audit_note": "listen(1) + synchronous serve_client, no thread, no serial lock (DAY287 audit)",
"phase2_removed": true,
"phase2_removal_reason": "single-client daemon: cross-process measured polling is structurally impossible",
"measured_verdict": null,
"static_effective_hz": 54.351,
"phases": [
{
"phase": "phase1_static",
"ok_count": 3262,
"err_count": 0,
"total_polls": 3262,
"elapsed_sec": 60.018,
"effective_hz": 54.351,
"error_rate": 0.0,
"latency_ms_p50": 19.779,
"latency_ms_p95": 21.837,
"latency_ms_max": 29.877
},
{
"phase": "phase3_target_30hz",
"ok_count": 900,
"err_count": 0,
"total_polls": 900,
"elapsed_sec": 30.001,
"effective_hz": 29.999,
"error_rate": 0.0,
"latency_ms_p50": 18.872,
"latency_ms_p95": 22.822,
"latency_ms_max": 26.72,
"target_hz": 30,
"hz_delta": -0.001
},
{
"phase": "phase3_target_10hz",
"ok_count": 300,
"err_count": 0,
"total_polls": 300,
"elapsed_sec": 30.001,
"effective_hz": 10.0,
"error_rate": 0.0,
"latency_ms_p50": 19.841,
"latency_ms_p95": 26.024,
"latency_ms_max": 30.729,
"target_hz": 10,
"hz_delta": 0.0
},
{
"phase": "phase3_target_5hz",
"ok_count": 150,
"err_count": 0,
"total_polls": 150,
"elapsed_sec": 30.0,
"effective_hz": 5.0,
"error_rate": 0.0,
"latency_ms_p50": 17.814,
"latency_ms_p95": 26.873,
"latency_ms_max": 35.081,
"target_hz": 5,
"hz_delta": 0.0
}
]
}54Hz は十分に速い。だがこの計測は片側検定でしかなかった。静止時に低い数字が出れば「daemon を改修しても measured は使えない」と確定できるが、高い数字が出ても「動作中に取れる」ことは意味しない。send_angles がシリアルポートを占有している間の挙動は、この方法では測れない。
そして実は、この計測の設計段階でもっと根本的な事実が判明していた。実機 daemon のコードを SSH で読むと、listen(1) で accept した後に serve_client() を同期実行する single-client 構造で、並行クライアント処理もシリアルアクセスの Lock も存在しなかった。つまり本線が daemon に接続している間、別プロセスの recorder が接続する経路は構造的に無い。当初の計測スクリプトは2ソケット並行で動作中ポーリングを測る Phase 2 を持っていたが、成立しない設計だったため、send_angles を含むコードごと削除して読み取り専用スクリプトに作り直した。物理リスクを持つ dead code を skip ガードで残すより、消して git 履歴に置くほうが安全だからだ。
この監査で関節状態の取得方法は確定した。measured（実測値ポーリング）は断念し、commanded（本線が送角する瞬間の指令値）を UDP で流してもらう。mechArm の動きはもともと「指令→待ち→指令」の離散運動なので、指令値の列がそのまま軌跡になる。中間の連続性は映像側が持つ。
横カメラ: fps 指定は無視される・720p が 480p より速い
記録側のカメラ（Iriun 経由 iPhone 11）の実力を、解像度×fps×コーデックの9条件で各60秒計測した。3つの想定外があった。
条件 | 実効fps | interval stdev | interval p95 | ファイル(60s) | 700試行換算 |
|---|---|---|---|---|---|
640x480 / 30fps / mp4v | 23.66 | 20.6ms | 98.1ms | 2.91MB | 1.00GB |
640x480 / 30fps / avc1 | 23.52 | 21.7ms | 100.0ms | 0.38MB | 0.13GB |
640x480 / 15fps / mp4v | 23.69 | 20.7ms | 98.5ms | 2.90MB | 0.99GB |
640x480 / 15fps / avc1 | 23.51 | 23.8ms | 96.9ms | 0.67MB | 0.23GB |
1280x720 / 30fps / mp4v | 26.13 | 23.4ms | 95.1ms | 17.87MB | 6.11GB |
1280x720 / 30fps / avc1 | 27.41 | 19.2ms | 93.7ms | 3.38MB | 1.16GB |
1280x720 / 15fps / mp4v | 27.37 | 19.3ms | 93.9ms | 18.38MB | 6.28GB |
1280x720 / 15fps / avc1 | 27.44 | 19.1ms | 95.4ms | 7.77MB | 2.65GB |
640x480 / 30fps / mp4v / threaded | 23.60 | 20.8ms | 97.0ms | 2.87MB | 0.98GB |
第一に、requested_fps が完全に無視される。30 指定も 15 指定も実効は同じで、reported_fps は全条件 60.0 を返す。記録周波数は指定できず、read() が返る速度がすべてを決める。したがって mp4 コンテナに書く fps 値は嘘になる。真実は frame_log.csv の capture_timestamp に持たせるしかない——この発見が「mp4 はフレームの入れ物・タイムスタンプ列が正」という設計を決めた。
第二に、720p のほうが 480p より速い。Iriun が 720p でネイティブに流しており、640x480 はダウンスケール分だけ遅くなると読める。解像度を落とす理由が消えた。720p/avc1 は実効 27.4fps・揺らぎ最小（stdev 19.2ms）・ファイル 3.38MB/60s で、全項目の最良だった。
第三に、書き込みの別スレッド化に効果がない。single と threaded の差はゼロ、queue からのドロップも0。ボトルネックは書き込みではなく read() であり、recorder は単一スレッドで組めることが確定した。
唯一の懸念はフレーム間隔の揺らぎで、p50 35.7ms（約28Hz）に対し p95 は 93.7ms（約10.7Hz）。5%のフレームが 90ms 以上の穴を空ける。学習データ化の際のリサンプル目標は 10Hz が安全という制約がここから出る。
設計: 観測者は制御に触れない
計測結果を受けて構成を確定した。
[Mac]
run_pick_place.py（本線・変更最小）
│ UDP 127.0.0.1:5556（1行JSON・fire-and-forget）
▼
recorder.py（新規・別プロセス・単一スレッド）
├── 横カメラ（Iriun）を専有し 1280x720/avc1/fps無指定で連続キャプチャ
├── episode.mp4 + frame_log.csv + states.csv を run_stem 配下へ
└── frame_request に直近フレームの jpg 切り出しで応答
保存先: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/{run_stem}/recording/本線から recorder へは6種のイベントを UDP で流す。trial_start / trial_end / stage（ステージ遷移）/ commanded（送角）/ gripper / frame_request。時計は Mac 1台なので同期問題が存在しない——この構成の最大の利点で、崩さないことを設計の柱にした。
横カメラには先客がいた。本線が PLACE 判定用の側面静止画を撮るのに同じ Iriun を開いており、recorder が専有すると衝突する。解き方は「recorder を唯一のカメラ所有者にし、本線は録画ストリームから該当フレームの切り出しを依頼する」方式にした。カメラの開閉そのものが競合の温床だという過去の教訓に沿えば、開閉の受け渡しより所有の一本化のほうが構造的に安全になる。本線側の側面撮影は frame_request の送信に置き換わり、タイムアウト（2.0秒）や recorder 不在の場合は画像 None として既存の欠損時経路にそのまま流れる。
台帳（pick_place_ledger_v3.csv）には recording_path / recording_fps / recording_frames / recording_status の4列を追加した。手順は 8-A（過去行を保持して空セルを付与）で、既存647行は1セルも変えずに 78→82列化した。recorder 不在の試行は空欄ではなく recording_status=none と明示的に書く。
なお、この実装準備の探索で2件の「作業がディスクに定着していない」事故が見つかった。台帳ルール文書 data_manifest_rules.md は、68〜78列を転記した改訂が執筆されたつもりでコミットされておらず、Mac 上は67列版のまま止まっていた。FK テーブルも、PLACE 領域67,228点を追加した中身更新が2週間コミットされないまま git status に modified として滞留していた（Mac / KIOXIA 正本の md5 は 6f069cd5... で一致しており中身は正常。未定着だっただけ）。どちらも今回の作業で定着させた。文書の焼き付き乖離は仕組みで潰しても、今度は「直した版が保存されない」という別の穴から漏れる。
外部レビュー3周: 指摘が設計を鍛えた
物理リスクのある変更（本線の送角経路周辺に触る）なので、実装→codex レビュー→修正のサイクルを3周回した。主要な指摘と対応を残す。
R1→R2: 送信時刻はどこで取るか（A1）
初版は commanded イベントの時刻を「送角関数が到達確認を終えて戻った直後」に取っていた。これでは全 commanded 行が実際の指令より数秒遅れて記録され、映像と突合できない。しかも呼び出し直前で時刻を取る安直な修正も不正確だとレビューが指摘した——送角ヘルパー _send_and_wait_arrival の内部には実送信前の姿勢取得 RPC があり、最大1回の再送もあるからだ。
正しい修正は、共有モジュール j0_sweep_scan.py の送角関数に observer コールバックを追加し、初回送信と再送それぞれの daemon_send_angles 直前で ts_send を確保、tol 内到達を最初に確認した瞬間を ts_arrival として送信1回ごとに通知する、というものだった。さらに、到達確認を経ずに直接 daemon_send_angles する視覚サーボ2箇所（PICK servo / PLACE servo）も記録対象に追加した。サーボの微調整送角こそ映像と突合したい動きであり、ここが抜けると片手落ちになる。
R1→R2: 台帳への転記と「待たない」原則の衝突（A4）
recorder が recording_meta.json を書き終える前に、本線の台帳追記が走る競合がある。無待機・無 ACK のまま最終 status を確実に台帳へ載せることは構造的に不可能——レビューはそう指摘し、3案（後処理 reconcile / 有界待ち / recorder 側の後更新）の設計判断を求めた。
台帳は追記のみ・既存行の上書き禁止が規約なので、後から埋める2案は禁止事項に正面から当たる。採用したのは有界待ちで、台帳追記の直前に meta の出現を最大2.0秒・100ms 間隔でポーリングし、タイムアウトなら none で確定する。観測者原則が禁じているのは「recorder の不調が制御に波及すること」であり、台帳追記は全送角・全グリッパー操作が終わった後の bookkeeping なので、ここでの待ちは物理リスクを持たない。原則の適用範囲を言語化できたのは、この衝突のおかげだった。
このほか R2 では、frame_request が返してよいフレームの鮮度上限（0.5秒）、CSV の1秒周期 flush（突然死時の損失を最大1秒に限定）、frames_captured と frames_written の分離（write 失敗を数えない）、マウントガードの実体化（ismount + 書き込みテスト、パス存在確認では USB が抜けても True になる）を入れた。
R2→R3: 未閉鎖経路の刈り取り
差分レビューがさらに2つの穴を見つけた。camera_lost 確定時にクールダウン判定が先に効いて camera.close() が呼ばれず、古いフレームが frame_request に返り得る経路。そして試行中の frame_request がマウントガードなしで mkdir / imwrite に到達し、マウント消失直後ならローカル側に /Volumes/KIOXIA/... を再作成し得る経路。どちらも「close の保証」と「ガードの全経路適用」で閉じた。あわせて observer 未指定時には time.time() の取得すら実行しない厳密化を入れ、共有モジュールの実行トレースが未指定時に1バイトも変わらないことを grep で確認できる状態にした。
recorder.py 全文
記録プロセスの本体。556行すべてを掲載する。docstring に運用制約と既知の制約（後述する実測2件）まで焼き込んであり、このファイル単体で仕様が読める状態を目指した。
#!/usr/bin/env python3
"""試行の連続記録 recorder (別プロセス・観測者専任・DAY287 R2).
- UDP 127.0.0.1:5556 で本線 (run_pick_place.py) からのイベントを受け,
横カメラ (Iriun) の連続録画 + commanded / stage / gripper 状態を
1 試行 = 1 フォルダで書き出す.
- **観測者原則**: アーム・Pi daemon TCP ポートに一切触れない (禁止動詞
4 種と daemon ポート番号は本ファイルに 1 度も現れない・grep で保証).
recorder が起動していなくても・死んでも・応答しなくても本線は完走する.
出力: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/{run_stem}/recording/
- episode.mp4 (avc1, 1280x720, コンテナ fps=30 は参考値, 真実は frame_log)
(突然死時は moov 欠落で mp4 破損しうる. 真実は frame_log)
- frame_log.csv (frame_idx, capture_ts)
- states.csv (ts, event_type, stage, j0..j5, speed, gripper_state,
ts_send, ts_arrival, attempt, arrived, note)
DAY287 R2 A1: commanded 行に ts_send/ts_arrival/attempt/arrived
を追加. ts は受信時刻として維持.
- recording_meta.json (fps・frames_captured・frames_written・status 等. atomic write)
- extracted/ (frame_request 応答で切り出した jpg)
status 語彙: ok / truncated / camera_lost / mount_lost / none (最後は本線側で使用)
**運用制約** (DAY287 R3 D5 で許容): マウント消失時は recording_meta.json も
SSD に書けず, 永続 status が標準出力にしか残らない. ローカル fallback は
上位ルールで禁止 (代替の書き込み先を作らない) のため, この制約は仕様として
受け入れる. 実運用では KIOXIA マウントを外さない前提.
**既知の制約 1 — Iriun 切断は検出不能** (DAY287 スモーク実測):
iPhone 側で Iriun アプリを終了しても Mac 側の仮想カメラデバイスは生存し
`cap.read()` が失敗しない (直近フレームがフリーズしたまま ok=True で返る)
ため, camera_lost は発火しない. 再接続は同一デバイス上で透過的に復帰する
(実測: 復帰後のライブフレームが即座に取得できた). camera_lost 経路は
物理 USB カメラ (将来の ELP 等) で機能する設計. 実運用では**試行前に
Iriun の接続を目視確認**すること (代替検知手段は現時点で無い).
**既知の制約 2 — mount_lost 時の残骸パターン** (DAY287 スモーク実測):
マウント消失時は `recording_meta.json` の atomic write が失敗する
(`os.replace` が `No such file or directory` を投げる). 結果として
`recording/` には episode.mp4 / frame_log.csv / states.csv のみ残り,
`recording_meta.json` が欠ける状態になる. **「meta なし = 異常終了の痕跡」**
として読む (集計/検査ツール側でこの規約に依拠して良い). 例外はログ 1 行に
収まりプロセスは生存, その後カメラは再解決される.
Usage:
python3 recorder.py
(Ctrl-C で停止. daemon 化はしない — 本線と同じ常駐運用感覚)
"""
from __future__ import annotations
import csv
import json
import os
import socket
import sys
import time
from datetime import datetime
from pathlib import Path
import cv2
_HERE = Path(__file__).resolve().parent
if str(_HERE) not in sys.path:
sys.path.insert(0, str(_HERE))
import camera_resolver # noqa: E402
# ---- 定数 --------------------------------------------------------------------
UDP_HOST = "127.0.0.1"
UDP_PORT = 5556
RUNS_ROOT = Path("/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place")
KIOXIA_MOUNT = Path("/Volumes/KIOXIA")
ORPHAN_FRAMES_DIR = Path("/Volumes/KIOXIA/r2r2r-data/diag/recorder_orphan_frames")
CAMERA_WIDTH = 1280
CAMERA_HEIGHT = 720
MP4_CONTAINER_FPS = 30.0
FOURCC = "avc1"
# DAY287 R2 A3: frame_request で返してよい直近フレームの経過時間上限.
FRAME_AGE_LIMIT_SEC = 0.5
# DAY287 R2 C4: 1 試行中の CSV flush 周期 (突然死時の損失を最大 1 秒に限定).
CSV_FLUSH_INTERVAL_SEC = 1.0
# DAY287 R2 D5: マウントガードの writable-check 一時ファイル名.
MOUNT_WRITE_TEST_NAME = ".recorder_mount_write_test"
# read 失敗の連続許容数. 超えたら camera_lost.
READ_FAIL_LIMIT = 30
# read fail 後の camera 再解決までのクールダウン秒.
CAMERA_REOPEN_COOLDOWN_SEC = 2.0
# UDP recv のポーリング周期 (blocking recv は使わず, camera read と交互).
UDP_POLL_INTERVAL_SEC = 0.005
STATES_COLS = ["ts", "event_type", "stage",
"j0", "j1", "j2", "j3", "j4", "j5",
"speed", "gripper_state",
"ts_send", "ts_arrival", "attempt", "arrived",
"note"]
FRAME_LOG_COLS = ["frame_idx", "capture_ts"]
# ---- DAY287 R2 D5: マウントガードの実体化 ---------------------------------
def ensure_kioxia_mounted() -> bool:
"""os.path.ismount で真マウント確認 + 一時ファイル書き込みテスト.
どちらか失敗なら False. shim (単なるパス存在) を弾く."""
try:
if not os.path.ismount(str(KIOXIA_MOUNT)):
return False
except Exception:
return False
try:
RUNS_ROOT.parent.parent.mkdir(parents=True, exist_ok=True)
test = RUNS_ROOT.parent.parent / MOUNT_WRITE_TEST_NAME
test.write_text("x")
test.unlink()
return True
except Exception:
return False
# ---- 試行ごとの writer 束 ---------------------------------------------------
class TrialWriter:
"""1 試行 = 1 インスタンス. trial_start で作り, trial_end で close する."""
def __init__(self, run_stem: str, actual_width: int, actual_height: int):
self.run_stem = run_stem
self.dir = RUNS_ROOT / run_stem / "recording"
self.dir.mkdir(parents=True, exist_ok=True)
(self.dir / "extracted").mkdir(parents=True, exist_ok=True)
self.mp4_path = self.dir / "episode.mp4"
self.frame_log_path = self.dir / "frame_log.csv"
self.states_path = self.dir / "states.csv"
self.meta_path = self.dir / "recording_meta.json"
self.meta_tmp_path = self.dir / "recording_meta.json.tmp"
fourcc = cv2.VideoWriter_fourcc(*FOURCC)
self.writer = cv2.VideoWriter(
str(self.mp4_path), fourcc, MP4_CONTAINER_FPS,
(actual_width, actual_height))
self.writer_ok = self.writer.isOpened()
if not self.writer_ok:
print(f"[RECO] [WARN] VideoWriter open 失敗 ({FOURCC}): {self.mp4_path}")
self.frame_log_file = self.frame_log_path.open("w", newline="")
self.frame_log_writer = csv.writer(self.frame_log_file)
self.frame_log_writer.writerow(FRAME_LOG_COLS)
self.states_file = self.states_path.open("w", newline="")
self.states_writer = csv.writer(self.states_file)
self.states_writer.writerow(STATES_COLS)
# DAY287 R2 C4: frames_captured (read 成功数) と frames_written
# (writer.write 成功数) を分けて記録. write 失敗は書込数を増やさない.
self.frames_captured = 0
self.frames_written = 0
self.started_ts = time.time()
self.current_stage = ""
self.status = "ok"
self.ended_ts: float | None = None
self.last_flush_ts = self.started_ts
def _maybe_flush(self) -> None:
"""DAY287 R2 C4: 1 秒周期で CSV を flush. 突然死時の損失を最大 1 秒に."""
now = time.time()
if now - self.last_flush_ts >= CSV_FLUSH_INTERVAL_SEC:
try:
self.frame_log_file.flush()
self.states_file.flush()
except Exception:
pass
self.last_flush_ts = now
def write_frame(self, frame, capture_ts: float) -> None:
# frames_captured は常に増える (read 成功数).
self.frames_captured += 1
wrote = False
if self.writer_ok:
try:
self.writer.write(frame)
wrote = True
except Exception as e:
print(f"[RECO] [WARN] mp4 write 失敗: {e}")
if wrote:
self.frames_written += 1
# frame_log は「captured」を正とする (真実は frame_log の capture_ts).
self.frame_log_writer.writerow(
[self.frames_captured - 1, f"{capture_ts:.6f}"])
self._maybe_flush()
def write_state(self, event_type: str, payload: dict) -> None:
ts = float(payload.get("ts") or time.time())
if event_type == "stage":
self.current_stage = str(payload.get("stage") or "")
angles = payload.get("angles") or [None] * 6
row = [
f"{ts:.6f}",
event_type,
self.current_stage,
]
for i in range(6):
v = angles[i] if i < len(angles) else None
row.append("" if v is None else f"{float(v):.3f}")
speed = payload.get("speed")
row.append("" if speed is None else str(speed))
gs = payload.get("state") if event_type == "gripper" else None
row.append("" if gs is None else str(gs))
# DAY287 R2 A1: commanded 行の ts_send / ts_arrival / attempt / arrived.
ts_send = payload.get("ts_send")
ts_arrival = payload.get("ts_arrival")
attempt = payload.get("attempt")
arrived = payload.get("arrived")
row.append("" if ts_send is None else f"{float(ts_send):.6f}")
row.append("" if ts_arrival is None else f"{float(ts_arrival):.6f}")
row.append("" if attempt is None else str(attempt))
row.append("" if arrived is None else ("True" if arrived else "False"))
note = payload.get("note", "")
row.append(str(note or ""))
self.states_writer.writerow(row)
self._maybe_flush()
def _atomic_meta_write(self, meta: dict) -> None:
"""DAY287 R2 A4: tmp に書いて os.replace で原子的置換.
本線側の bounded poll と組み合わせて「半端な JSON」を読ませない."""
try:
self.meta_tmp_path.write_text(
json.dumps(meta, indent=2, ensure_ascii=False))
os.replace(str(self.meta_tmp_path), str(self.meta_path))
except Exception as e:
print(f"[RECO] [WARN] meta atomic write 失敗: {e}")
def close(self, status: str) -> None:
self.ended_ts = time.time()
self.status = status
try:
self.frame_log_file.flush(); self.frame_log_file.close()
except Exception:
pass
try:
self.states_file.flush(); self.states_file.close()
except Exception:
pass
if self.writer_ok:
try:
self.writer.release()
except Exception:
pass
elapsed = (self.ended_ts - self.started_ts) if self.ended_ts else 0.0
eff_fps = (self.frames_captured / elapsed) if elapsed > 0 else 0.0
meta = {
"run_stem": self.run_stem,
"started_ts": self.started_ts,
"ended_ts": self.ended_ts,
"elapsed_sec": round(elapsed, 3),
"frames_captured": self.frames_captured,
"frames_written": self.frames_written,
"frame_count": self.frames_captured, # 後方互換 (本線読取用)
"effective_fps": round(eff_fps, 3),
"container_fps": MP4_CONTAINER_FPS,
"width": CAMERA_WIDTH,
"height": CAMERA_HEIGHT,
"fourcc": FOURCC,
"status": status,
"recording_path": "recording/",
}
self._atomic_meta_write(meta)
print(f"[RECO] trial 終了 run_stem={self.run_stem} "
f"captured={self.frames_captured} written={self.frames_written} "
f"eff_fps={eff_fps:.2f} status={status}")
# ---- カメラ管理 --------------------------------------------------------------
class SideCamera:
def __init__(self):
self.cap: cv2.VideoCapture | None = None
self.actual_width = CAMERA_WIDTH
self.actual_height = CAMERA_HEIGHT
self.last_frame = None
self.last_frame_ts: float | None = None
self.consecutive_read_fails = 0
self.last_reopen_attempt = 0.0
def open(self) -> bool:
try:
cams = camera_resolver.resolve_cameras(
require_top=False, require_side=True, verbose=False)
except Exception as e:
print(f"[RECO] [WARN] camera_resolver: {e}")
return False
idx = cams.get("side")
if idx is None:
print("[RECO] [WARN] Iriun (side) 未解決")
return False
cap = cv2.VideoCapture(idx)
if not cap.isOpened():
print(f"[RECO] [WARN] VideoCapture open 失敗 index={idx}")
return False
cap.set(cv2.CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT)
# fps は無指定. Iriun 側の実力に任せる (実効 ~27fps 想定).
for _ in range(5):
cap.read()
self.cap = cap
self.actual_width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) or CAMERA_WIDTH
self.actual_height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) or CAMERA_HEIGHT
print(f"[RECO] カメラ open 成功 index={idx} {self.actual_width}x{self.actual_height}")
return True
def close(self) -> None:
# DAY287 R2 A3: close 時に last_frame をクリア.
# camera_lost / mount_lost 時に古いフレームが frame_request に返るのを防ぐ.
if self.cap is not None:
try:
self.cap.release()
except Exception:
pass
self.cap = None
self.last_frame = None
self.last_frame_ts = None
def read(self) -> tuple[bool, object]:
if self.cap is None:
return False, None
ok, frame = self.cap.read()
ts = time.time()
if ok and frame is not None:
self.last_frame = frame
self.last_frame_ts = ts
self.consecutive_read_fails = 0
return True, frame
self.consecutive_read_fails += 1
return False, None
def try_reopen_if_needed(self) -> None:
now = time.time()
if now - self.last_reopen_attempt < CAMERA_REOPEN_COOLDOWN_SEC:
return
self.last_reopen_attempt = now
self.close()
if self.open():
self.consecutive_read_fails = 0
# ---- orphan 保存 -------------------------------------------------------------
def save_orphan_frame(frame, capture_ts: float, tag: str) -> Path | None:
if frame is None:
return None
# DAY287 R2 D5: orphan 保存にもマウントガード. ガード不成立ならログのみ.
if not ensure_kioxia_mounted():
print(f"[RECO] [WARN] orphan 保存断念 (KIOXIA マウントなし) tag={tag}")
return None
try:
ORPHAN_FRAMES_DIR.mkdir(parents=True, exist_ok=True)
ts_str = datetime.fromtimestamp(capture_ts).strftime("%Y%m%d_%H%M%S_%f")[:-3]
path = ORPHAN_FRAMES_DIR / f"{tag}_{ts_str}.jpg"
if cv2.imwrite(str(path), frame):
return path
except Exception as e:
print(f"[RECO] [WARN] orphan 保存失敗: {e}")
return None
# ---- main loop ---------------------------------------------------------------
def main() -> int:
if not ensure_kioxia_mounted():
print(f"[RECO] [ABORT] KIOXIA 未マウント (ismount + write test 失敗): {KIOXIA_MOUNT}")
return 2
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
sock.bind((UDP_HOST, UDP_PORT))
sock.setblocking(False)
print(f"[RECO] UDP listen {UDP_HOST}:{UDP_PORT}")
camera = SideCamera()
if not camera.open():
print("[RECO] [WARN] 起動時カメラ open 失敗 — 定期再試行しながら待機")
trial: TrialWriter | None = None
reply_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
def handle_frame_request(payload: dict) -> None:
nonlocal trial
tag = str(payload.get("tag") or "frame")
reply_port = payload.get("reply_port")
if not isinstance(reply_port, int):
print(f"[RECO] [WARN] frame_request reply_port 無効: {reply_port}")
return
frame = camera.last_frame
capture_ts = camera.last_frame_ts
now = time.time()
# DAY287 R2 A3: frame age 上限 (FRAME_AGE_LIMIT_SEC) を判定.
if frame is None or capture_ts is None:
reply = {"path": None, "capture_ts": None, "frame_age_ms": None,
"reason": "no_frame_available"}
else:
age_ms = (now - capture_ts) * 1000.0
if age_ms > FRAME_AGE_LIMIT_SEC * 1000.0:
reply = {"path": None, "capture_ts": capture_ts,
"frame_age_ms": round(age_ms, 1),
"reason": "stale_frame"}
elif trial is not None:
# DAY287 R3 D5: 試行内 frame_request のマウントガード.
# extracted_dir.mkdir / cv2.imwrite に到達させない (ローカル側
# /Volumes/KIOXIA/... 再作成の余地を残さない・fallback 禁止).
if not ensure_kioxia_mounted():
print("[RECO] [WARN] frame_request 時 KIOXIA マウント消失 → mount_lost で trial+camera を閉じる")
trial.close("mount_lost")
trial = None
camera.close()
reply = {"path": None, "capture_ts": capture_ts,
"frame_age_ms": round(age_ms, 1),
"reason": "mount_lost"}
else:
extracted_dir = trial.dir / "extracted"
ts_str = datetime.fromtimestamp(capture_ts).strftime(
"%Y%m%d_%H%M%S_%f")[:-3]
out_path = extracted_dir / f"{tag}_{ts_str}.jpg"
try:
extracted_dir.mkdir(parents=True, exist_ok=True)
if cv2.imwrite(str(out_path), frame):
reply = {"path": str(out_path),
"capture_ts": capture_ts,
"frame_age_ms": round(age_ms, 1),
"reason": "ok"}
else:
reply = {"path": None, "capture_ts": None,
"frame_age_ms": round(age_ms, 1),
"reason": "imwrite_failed"}
except Exception as e:
reply = {"path": None, "capture_ts": None,
"frame_age_ms": round(age_ms, 1),
"reason": f"exception:{type(e).__name__}"}
else:
# 試行外. save_orphan_frame 内で ensure_kioxia_mounted 済み.
orphan_path = save_orphan_frame(frame, capture_ts, tag)
reply = {"path": str(orphan_path) if orphan_path else None,
"capture_ts": capture_ts,
"frame_age_ms": round(age_ms, 1),
"reason": "ok_orphan" if orphan_path else "orphan_save_failed",
"orphan": True}
try:
reply_sock.sendto(
(json.dumps(reply) + "\n").encode("utf-8"),
(UDP_HOST, reply_port))
except Exception as e:
print(f"[RECO] [WARN] frame_request 応答失敗: {e}")
def start_trial(payload: dict) -> None:
nonlocal trial
run_stem = str(payload.get("run_stem") or "")
if not run_stem:
print("[RECO] [WARN] trial_start run_stem 空 → 無視")
return
if trial is not None:
print(f"[RECO] 二重 trial_start: 前 {trial.run_stem} を truncated で閉じる")
trial.close("truncated")
trial = None
if not ensure_kioxia_mounted():
print("[RECO] [WARN] trial_start 時 KIOXIA 未マウント → 記録断念")
return
try:
trial = TrialWriter(run_stem, camera.actual_width, camera.actual_height)
print(f"[RECO] trial 開始 run_stem={run_stem} → {trial.dir}")
except Exception as e:
print(f"[RECO] [WARN] TrialWriter 作成失敗: {e}")
trial = None
def end_trial(payload: dict) -> None:
nonlocal trial
if trial is None:
print("[RECO] trial_end 受領 (該当 trial なし)")
return
trial.close(trial.status)
trial = None
print("[RECO] 待機中. 本線からの trial_start を待つ (Ctrl-C で終了)")
try:
while True:
for _ in range(64):
try:
data, _addr = sock.recvfrom(65536)
except BlockingIOError:
break
except Exception as e:
print(f"[RECO] [WARN] UDP recv: {e}")
break
try:
payload = json.loads(data.decode("utf-8").strip())
except Exception as e:
print(f"[RECO] [WARN] 壊れた JSON を破棄: {e}")
continue
ev = payload.get("type")
if ev == "trial_start":
start_trial(payload)
elif ev == "trial_end":
end_trial(payload)
elif ev == "frame_request":
handle_frame_request(payload)
elif ev in ("stage", "commanded", "gripper"):
if trial is not None:
try:
trial.write_state(ev, payload)
except Exception as e:
print(f"[RECO] [WARN] state 書き込み: {e}")
else:
print(f"[RECO] [WARN] 未知 type を破棄: {ev}")
if camera.cap is None:
camera.try_reopen_if_needed()
else:
ok, frame = camera.read()
if ok:
if trial is not None:
if ensure_kioxia_mounted():
trial.write_frame(frame, camera.last_frame_ts or time.time())
else:
# DAY287 R2 D5 + R3 A3: 試行中マウント消失は
# mount_lost で閉じる. カメラも即 close して
# last_frame を無効化 (以後の frame_request に古い
# フレームを返させない).
if trial.status == "ok":
print("[RECO] [WARN] 試行中 KIOXIA マウント消失 → mount_lost で閉じる")
trial.close("mount_lost")
trial = None
camera.close()
elif camera.consecutive_read_fails >= READ_FAIL_LIMIT:
if trial is not None and trial.status == "ok":
print(f"[RECO] read 30 連続失敗 → trial を camera_lost で閉じる")
trial.close("camera_lost")
trial = None
# DAY287 R3 A3: 30 連続失敗の確定時は cooldown 無関係に
# 即 camera.close() で last_frame をクリア.
# cooldown 判定は「close 後の再 open 試行」にのみ適用する.
camera.close()
camera.try_reopen_if_needed()
else:
time.sleep(UDP_POLL_INTERVAL_SEC)
except KeyboardInterrupt:
print("\n[RECO] Ctrl-C 受領 → 終了")
finally:
if trial is not None:
trial.close("truncated")
camera.close()
try:
sock.close()
except Exception:
pass
try:
reply_sock.close()
except Exception:
pass
return 0
if __name__ == "__main__":
sys.exit(main())本線の変更: 挿入と置換1箇所のみ
run_pick_place.py への変更は、通知ヘルパー2本の追加、イベント送信の挿入、側面撮影の置換、台帳転記ブロックの追加に限られる。制御ロジック・姿勢・タイミングには触れていない。変更した関数とブロックを全量掲載する。
recorder への通知ヘルパー。fire-and-forget の実装がどこまで例外を握るかが観測者原則の実体なので、この2関数が変更の心臓部にあたる。
# ---- DAY287: recorder (別プロセス) 通知ヘルパ ------------------------------
# fire-and-forget UDP. 送信失敗は握りつぶす. recorder が起動していなくても
# 応答しなくても本線は一切影響を受けない (観測者原則).
RECORDER_UDP_HOST = "127.0.0.1"
RECORDER_UDP_PORT = 5556
RECORDER_FRAME_REQUEST_TIMEOUT_SEC = 2.0
_recorder_notify_sock = None
def recorder_notify(payload):
"""recorder へ 1 行 JSON を UDP sendto する. 例外は全て握りつぶす."""
global _recorder_notify_sock
try:
if _recorder_notify_sock is None:
_recorder_notify_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
if "ts" not in payload:
payload = dict(payload)
payload["ts"] = time.time()
line = (json.dumps(payload) + "\n").encode("utf-8")
_recorder_notify_sock.sendto(line, (RECORDER_UDP_HOST, RECORDER_UDP_PORT))
except Exception:
pass
def _recorder_request_frame(tag):
"""recorder へ frame_request を送り, 応答 (path, capture_ts, reason) を返す.
タイムアウト 2.0s. 失敗時は (None, None, reason) を返す (path=None なら本線は
既存の side なし経路に流す).
reason 語彙 (DAY287 R3 B1 で recorder_absent を削除・timeout に統合):
"ok" / "ok_orphan" / "timeout" / "no_frame_available" / "stale_frame" /
"mount_lost" / "imwrite_failed" / "orphan_save_failed" /
"exception:{Type}" / "malformed_reply".
"recorder 不在" と「応答遅延」はいずれも "timeout" として観測される
(UDP は fire-and-forget で送信側から到達不能を検知できない・仕様).
既知の制約 (今回追わない): 旧 `_capture_for_vlm` が返していた
`resolver:*` / `cap_open` / `read_fail` の粒度は, recorder 経由では
frame_request 応答からは区別不能 (どれも `no_frame_available` /
`stale_frame` に丸まる). 観測粒度の細分化は recorder 側の将来課題
(recorder の内部エラー状態を frame_request 応答に載せる拡張).
"""
reply_sock = None
try:
reply_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
reply_sock.bind(("127.0.0.1", 0))
reply_port = reply_sock.getsockname()[1]
recorder_notify({
"type": "frame_request",
"tag": str(tag),
"reply_port": reply_port,
"ts": time.time(),
})
reply_sock.settimeout(RECORDER_FRAME_REQUEST_TIMEOUT_SEC)
try:
data, _addr = reply_sock.recvfrom(65536)
except (socket.timeout, TimeoutError):
# recorder 停止 or 応答遅延の区別は付かない (同じ現象).
return None, None, "timeout"
try:
resp = json.loads(data.decode("utf-8").strip())
except Exception:
return None, None, "malformed_reply"
path = resp.get("path")
cts = resp.get("capture_ts")
reason = str(resp.get("reason") or ("ok" if path else "unknown"))
if path:
return path, cts, reason
return None, cts, reason
except Exception as e:
return None, None, "exception:{}".format(type(e).__name__)
finally:
if reply_sock is not None:
try:
reply_sock.close()
except Exception:
pass送角の observer 化。共通の移動関数 _do_move 内で、送信1回ごとに (ts_send, ts_arrival, attempt, arrived) を recorder へ流す。
# DAY287 R2 A1: observer 経由で送信 1 回ごとに (ts_send, ts_arrival, attempt,
# arrived) を recorder へ通知. j0_sweep_scan は observer=None 未指定時は完全同一.
def _rec_observer(ts_send, ts_arrival, attempt, arrived):
recorder_notify({
"type": "commanded",
"angles": list(target_angles),
"speed": SPEED,
"ts_send": ts_send,
"ts_arrival": ts_arrival,
"attempt": attempt,
"arrived": bool(arrived),
"note": label,
})
arrived, retried = _send_and_wait_arrival(
target_angles, label, tol, send_wait_sec=SEND_WAIT_SEC,
observer=_rec_observer)到達確認を経ない直接送角（視覚サーボ）の記録。PLACE servo 側を示す（PICK servo 側も note 以外同一の構造）。
# 5. 送角 (j0 のみ変更・j1..j5 は stage8 到達姿勢固定).
new_angles = [j0_new] + list(servo_base_j1_j5)
j0_before_send = j0_current
# DAY287 R2 A1: 直接送角経路 (PLACE servo) も commanded 記録対象.
# _send_and_wait_arrival を経ないので送角直前に ts_send を確保し直接通知.
_ts_send_place = time.time()
try:
daemon_send_angles(new_angles, wait=2)
except Exception as e:
servo_rec["note"] = "daemon_send_angles 例外: {}".format(e)
recorder_notify({
"type": "commanded", "angles": list(new_angles), "speed": SPEED,
"ts_send": _ts_send_place, "ts_arrival": None,
"attempt": 1, "arrived": False,
"note": "place_visual_servo_error",
})
return j0_current, False, loop_count, "send_angle_error"
recorder_notify({
"type": "commanded", "angles": list(new_angles), "speed": SPEED,
"ts_send": _ts_send_place, "ts_arrival": None,
"attempt": 1, "arrived": None, # servo は到達確認なし (settle 待ちのみ)
"note": "place_visual_servo",
})
time.sleep(PLACE_SERVO_LOOP_WAIT_SEC)グリッパーと試行開始・終了の通知は各1〜2行の挿入で済んでいる。
# DAY287: recorder へ gripper 通知 (dry_run 含めて 1 度 emit).
recorder_notify({"type": "gripper", "state": state, "note": stage_label})
run_stem = log_path.stem
# DAY287: recorder に trial 開始を通知 (fire-and-forget・recorder 不在でも本線続行).
recorder_notify({"type": "trial_start", "run_stem": run_stem}) # DAY287: recorder に trial 終了を通知 (以後の finally 内 I/O 中に
# recorder が recording_meta.json を書き終える時間を稼ぐ・待たない).
recorder_notify({"type": "trial_end", "run_stem": run_stem})側面撮影の置換。stage7 側を示す（stage8 側も tag と変数名以外同一の構造）。従来この位置で横カメラを直接開いていた処理が、frame_request の送信に置き換わった。
# DAY265→DAY287: stage7 側面 1 枚は recorder (別プロセス) の直近フレームを
# frame_request で切り出す. 横カメラ (Iriun) 二重 open を避けるため.
# recorder 不在・タイムアウト時は path=None → 既存の side なし経路と同じ扱い.
# DAY287 R2 B1: reason を旧 _capture_for_vlm 相当の粒度で log_record に残す.
ap_place_side_img = None
if not dry_run:
_ap_path, _ap_cts, _ap_reason = _recorder_request_frame("ap_place_side")
if _ap_path:
ap_place_side_img = Path(_ap_path)
_cap_rec = {"label": "ap_place_side", "image": _ap_path,
"camera_role": "side", "dry_run": False,
"source": "recorder_frame_request",
"capture_ts": _ap_cts,
"reason": _ap_reason}
if not _ap_path:
_cap_rec["error"] = "recorder:{}".format(_ap_reason)
log_record.setdefault("vlm_captures", []).append(_cap_rec)
else:
log_record.setdefault("vlm_captures", []).append(
{"label": "ap_place_side", "image": None, "camera_role": "side",
"dry_run": True, "source": "recorder_frame_request"})
_observe_capture_push(log_record)台帳への転記。有界待ち（最大2.0秒・deadline 越えを起こさない sleep 上限厳密化込み）の実装がここにある。
# DAY287 R2 A4: recorder が書く recording_meta.json を **最大 2.0 秒・
# 100ms 間隔** の有界待ちで存在確認して転記. 台帳は追記のみ原則 (7節)
# なので後からの reconcile はしない. タイムアウト時は "none" で確定.
# 有界待ちは全送角・全グリッパー完了後の finally 経路なので物理リスク 0.
# recorder は atomic write (tmp+os.replace) するので half-written は
# 発生しない (A4).
_rec_path = ""
_rec_fps = ""
_rec_frames = ""
_rec_status = "none"
_POLL_TIMEOUT_SEC = 2.0
_POLL_INTERVAL_SEC = 0.1
try:
_meta_path = Path(_OBSERVE_STATE.get("run_dir") or "") / "recording" / "recording_meta.json"
_deadline = time.monotonic() + _POLL_TIMEOUT_SEC
# DAY287 R3 A4: sleep 上限を厳密化. deadline 越えを絶対に起こさない
# (前版は 100ms 固定 sleep で最悪 2.1s 使う可能性があった).
while not _meta_path.is_file():
_remaining = _deadline - time.monotonic()
if _remaining <= 0:
break
time.sleep(min(_POLL_INTERVAL_SEC, _remaining))
if _meta_path.is_file():
_meta = json.loads(_meta_path.read_text())
_rec_path = str(_meta.get("recording_path") or "recording/")
_f = _meta.get("effective_fps")
_rec_fps = "{:.3f}".format(_f) if isinstance(_f, (int, float)) else ""
_fc = _meta.get("frames_captured")
if not isinstance(_fc, int):
_fc = _meta.get("frame_count") # 後方互換
_rec_frames = str(_fc) if isinstance(_fc, int) else ""
_rec_status = str(_meta.get("status") or "none")
except Exception:
_rec_status = "none"
row.update({
"recording_path": _rec_path,
"recording_fps": _rec_fps,
"recording_frames": _rec_frames,
"recording_status": _rec_status,
})j0_sweep_scan.py: observer hook の追加
送角の共有モジュールに observer を追加した関数の全文。observer 未指定時は time.time() の取得も通知関数の呼び出しも一切実行されず、既存の全呼び出し元の実行トレースが1バイトも変わらない——この厳密さが、共有モジュールに触るときの監査コストを下げる。
def _send_and_wait_arrival(target, label, reach_tol_deg, retry_axis=None,
send_wait_sec=SEND_WAIT_SEC, observer=None):
"""target を単発 send_angles で送信し、tol 内に入るまで到達確認する。
本スクリプトは set_gripper(close) を一切呼ばないため、DAY228 で問題となった
「close 直後の単発 send_angles 沈黙」事象は本スクリプトには該当しない。
一方、ストリーミング送信（0.1s 周期連打）は PICK 基準姿勢からのスイープ移動で
コントローラに完全無視される事象が実機2セッションで再現したため、
08_home_from_anywhere 等で実績のある単発 send_angles 方式に統一する。
手順:
1. 送信前姿勢を取得し、wr._compute_arrival_timeout(pre_max) で timeout 算出
（送信前姿勢が取れない場合は ABORT。fallback はしない）
2. target を 1 回だけ daemon_send_angles(target, speed=SPEED, wait=send_wait_sec) で送信
（送信回数は増やさず、daemon側の送信直後 get_angles ログを少し遅らせる）
3. timeout 窓で get_angles 実測ポーリング、tol reach_tol_deg で到達判定
4. 1回目がタイムアウトした場合、pre_actual と最後の実測 last_actual の移動量を見て
RETRY_NO_MOVE_THRESHOLD_DEG 未満なら「送信無視」とみなし、RETRY_PAUSE_SEC 待って
現在姿勢を取り直して target を1回だけ単発再送（最大1回・ループしない）。
1.0° 以上動いている（部分移動/ストール）場合はリトライしない。
base移動は全軸 max|Δ|、j0スイープ点は j0 の移動量で判定する。
返り値: (success: bool, retried: bool)。
retried は「タイムアウト時に1回限りの再送を実行したか」のフラグ。
ABORT 経路は変更しない。
DAY287 R2 A1: 任意引数 observer(ts_send, ts_arrival, attempt, arrived) を追加.
- 既定 None. 未指定時は本関数の挙動が完全に現行同一 (通知を一度も呼ばない).
- 指定時: daemon_send_angles 直前に ts_send=time.time() を確保し, 送信 1 回ごとに
(ts_send, ts_arrival, attempt, arrived) を通知する.
ts_arrival は tol 内到達を最初に確認した瞬間 (time.time()). 未到達は None.
attempt=1 (初回) / 2 (再送). arrived=True/False.
- 送角ロジック・再送条件・tol 判定・timeout 算出には一切触れない.
- observer 内での例外は握って捨てる (bad observer が制御を止めないための保険).
"""
def _notify(ts_send, ts_arrival, attempt, arrived):
if observer is None:
return
try:
observer(ts_send, ts_arrival, attempt, arrived)
except Exception:
pass
# 1. 送信前姿勢 → timeout 算出（取れない場合は ABORT。fallback はしない）
pre_actual = _daemon_get_angles_filtered()
if pre_actual is None:
print("[SEND] [ABORT] {}: 送信前実測が取得できないため中止".format(label))
return False, False
pre_max, _ = wr._max_axis_diff(target, pre_actual)
timeout_sec = max(REACH_TIMEOUT_SEC, wr._compute_arrival_timeout(pre_max))
# 2. 単発送信（target を 1 回だけ送る）
print("[SEND] {}: send_angles (single) {} speed={} wait={:.1f}s "
"(pre max|Δ|={:.2f}°, post-send timeout {:.1f}s)".format(
label, _fmt_angles(target), SPEED, send_wait_sec, pre_max, timeout_sec))
# DAY287 R3 ①: observer 未指定時は time.time() も _notify() も一切実行しない
# (未指定時=本関数の実行トレースが 1 バイトも変わらない・監査コスト最小化).
ts_send_1 = time.time() if observer is not None else None
try:
daemon_send_angles(target, speed=SPEED, wait=send_wait_sec)
except Exception as e:
print("[SEND] [ERROR] daemon_send_angles 例外: {}".format(e))
if observer is not None:
_notify(ts_send_1, None, 1, False)
return False, False
# 3. 到達確認ポーリング
t0 = time.time()
last_err = None
last_actual = None
while time.time() - t0 < timeout_sec:
actual = _daemon_get_angles_filtered()
if actual is not None:
max_d, _ = wr._max_axis_diff(actual, target)
last_err = max_d
last_actual = actual
if max_d <= reach_tol_deg:
print("[SEND] arrival OK: max|Δ|={:.2f}° (tol {:.1f}°)".format(
max_d, reach_tol_deg))
if observer is not None:
_notify(ts_send_1, time.time(), 1, True)
return True, False
time.sleep(POLL_SLEEP)
le = "{:.2f}".format(last_err) if last_err is not None else "None"
la = _fmt_angles(last_actual) if last_actual is not None else "None"
print("[SEND] [TIMEOUT] {:.1f}s 内に未到達: max|Δ|={}° actual={}".format(
timeout_sec, le, la))
if observer is not None:
_notify(ts_send_1, None, 1, False)
# 4. リトライ判定（1回目タイムアウト時のみ・最大1回・ループしない）
# pre_actual → last_actual の移動量を見る:
# < RETRY_NO_MOVE_THRESHOLD_DEG → 送信無視疑い → 1回だけ単発再送
# >= RETRY_NO_MOVE_THRESHOLD_DEG → 部分移動/ストール → リトライしない
if last_actual is None:
print("[SEND] リトライ判定不可: タイムアウト中に実測が一度も取れなかった。ABORT へ")
return False, False
move_d, move_label, unexpected_other_move = _retry_progress(
pre_actual, last_actual, reach_tol_deg, retry_axis=retry_axis)
if unexpected_other_move is not None:
print("[SEND] リトライしません: j0以外の保持軸が {:.2f}° > tol {:.1f}° 動いたため "
"(姿勢崩れ疑い)".format(unexpected_other_move, reach_tol_deg))
return False, False
if move_d >= RETRY_NO_MOVE_THRESHOLD_DEG:
print("[SEND] リトライしません: {} {:.2f}° >= {:.1f}° "
"(部分移動/ストール疑い)".format(
move_label, move_d, RETRY_NO_MOVE_THRESHOLD_DEG))
return False, False
print("[RETRY] 1回目送信が無視された（{} {:.2f}°）。"
"{:.1f}s 待って再送します".format(move_label, move_d, RETRY_PAUSE_SEC))
time.sleep(RETRY_PAUSE_SEC)
# 現在姿勢を取り直し、target を1回だけ単発再送
pre_actual_retry = _daemon_get_angles_filtered()
if pre_actual_retry is None:
print("[RETRY] 失敗。再送前実測が取得できないため中止。ABORT へ")
return False, True
pre_max_retry, _ = wr._max_axis_diff(target, pre_actual_retry)
timeout_retry_sec = max(REACH_TIMEOUT_SEC, wr._compute_arrival_timeout(pre_max_retry))
print("[RETRY] {} (retry): send_angles (single) {} speed={} wait={:.1f}s "
"(pre max|Δ|={:.2f}°, post-send timeout {:.1f}s)".format(
label, _fmt_angles(target), SPEED, send_wait_sec, pre_max_retry, timeout_retry_sec))
# DAY287 R3 ①: 再送側も同様に observer 未指定時は time.time() / _notify 抑止.
ts_send_2 = time.time() if observer is not None else None
try:
daemon_send_angles(target, speed=SPEED, wait=send_wait_sec)
except Exception as e:
print("[RETRY] [ERROR] daemon_send_angles 例外: {}. ABORT へ".format(e))
if observer is not None:
_notify(ts_send_2, None, 2, False)
return False, True
# 再送の到達確認（同じ tol・同じ算出ルールの timeout 窓で1回だけ）
t0 = time.time()
last_err_retry = None
last_actual_retry = None
while time.time() - t0 < timeout_retry_sec:
actual = _daemon_get_angles_filtered()
if actual is not None:
max_d, _ = wr._max_axis_diff(actual, target)
last_err_retry = max_d
last_actual_retry = actual
if max_d <= reach_tol_deg:
print("[RETRY] 成功: arrival OK max|Δ|={:.2f}° (tol {:.1f}°)".format(
max_d, reach_tol_deg))
if observer is not None:
_notify(ts_send_2, time.time(), 2, True)
return True, True
time.sleep(POLL_SLEEP)
le_r = "{:.2f}".format(last_err_retry) if last_err_retry is not None else "None"
la_r = _fmt_angles(last_actual_retry) if last_actual_retry is not None else "None"
print("[RETRY] 失敗。ABORT へ. {:.1f}s 内に未到達: max|Δ|={}° actual={}".format(
timeout_retry_sec, le_r, la_r))
if observer is not None:
_notify(ts_send_2, None, 2, False)
return False, True台帳 78→82列: 過去647行を1セルも変えずに
台帳への4列追加は 8-A 手順（過去行を保持して空セルを付与）で行った。旧台帳を archive へコピー退避し md5 を記録、ヘッダに4列名を追記して全647データ行の末尾に空セル4つを付与、正規CSVパーサで検証した。
項目 | 値 |
|---|---|
退避先 | archive/pick_place_ledger_prerestore_20260715/pick_place_ledger_v3.csv |
退避 md5 | 384b1e9a3bc8482213157afb5673c204（退避前 live と一致） |
新 live md5 | 91dac34289c6701e3b530c15315f2f78 |
総行数 | 648（header + 647 data・不変） |
列数 | 82（全行整合） |
新ヘッダ末尾4列 | recording_path, recording_fps, recording_frames, recording_status |
過去647行の79-82列 | 全て空（csv.reader 検証） |
過去647行の1-78列 | 原本と1セル差異なし（row[:78] == old_row 検証） |
以前、単純な列追加を 8-B（0行から作り直し）で処理して過去393試行が現台帳から消える事故があり、その反省から 8-A / 8-B の使い分けが規約化された。今回は初めから 8-A で、過去行は台帳の中に残ったままになっている。
単独スモーク: 異常系まで潰す
実機に併走させる前に、recorder 単独（アーム不使用）で正常系と異常系を検証した。
正常系。手打ちの trial_start → stage / commanded / gripper → trial_end で約9秒記録し、成果物を突合した。
{
"run_stem": "smoke_day287_01",
"started_ts": 1784106172.691199,
"ended_ts": 1784106181.661804,
"elapsed_sec": 8.971,
"frames_captured": 228,
"frames_written": 228,
"frame_count": 228,
"effective_fps": 25.416,
"container_fps": 30.0,
"width": 1280,
"height": 720,
"fourcc": "avc1",
"status": "ok",
"recording_path": "recording/"
}frame_log.csv の再計算では 228 フレーム・capture_ts 単調増加・実効 25.42fps（meta と一致）・interval p50 35.1ms / p95 100.5ms / max 121.6ms。計測フェーズで見た揺らぎと同傾向で、記録系は素直に振る舞っている。
frame_request は両経路を確認した。試行中の要求は extracted/ 配下に切り出されて reason=ok・frame_age_ms 6.3、試行外の要求は orphan 保存で reason=ok_orphan・frame_age_ms 0.6 が返った。
{"path": "/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/smoke_day287_abn/recording/extracted/smoke_intrial_20260715_181345_630.jpg", "capture_ts": 1784106825.6305, "frame_age_ms": 6.3, "reason": "ok"}二重 trial_start は、前の試行を truncated で閉じて新規を開く設計どおりに動いた。
[RECO] trial 開始 run_stem=smoke_dup_A → /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/smoke_dup_A/recording
[RECO] 二重 trial_start: 前 smoke_dup_A を truncated で閉じる
[RECO] trial 終了 run_stem=smoke_dup_A captured=74 written=74 eff_fps=24.85 status=truncated
[RECO] trial 開始 run_stem=smoke_dup_B → /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/smoke_dup_B/recording
[RECO] trial 終了 run_stem=smoke_dup_B captured=72 written=72 eff_fps=25.48 status=okマウント消失は、録画中に KIOXIA の USB を物理的に抜いて検証した。mount_lost で試行が閉じ、meta の atomic write が失敗し（マウントの無い先へは書けない）、プロセスは生存してカメラの再解決まで進んだ。再マウント後の台帳 md5 は 91dac342... のまま無傷だった。
[RECO] trial 開始 run_stem=smoke_mount_lost → /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/smoke_mount_lost/recording
[RECO] [WARN] 試行中 KIOXIA マウント消失 → mount_lost で閉じる
[RECO] [WARN] meta atomic write 失敗: [Errno 2] No such file or directory: '/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/smoke_mount_lost/recording/recording_meta.json.tmp'
[RECO] trial 終了 run_stem=smoke_mount_lost captured=858 written=858 eff_fps=27.00 status=mount_lost
[RECO] カメラ open 成功 index=1 1280x720
そして想定外がひとつ。カメラ切断テストで、iPhone 側の Iriun アプリを終了しても camera_lost が発火しなかった。Mac 側の仮想カメラデバイスが生存し read() が失敗しないため、切断が観測できない。再接続も同一デバイス上で透過的に復帰し、復帰後の frame_request にはライブの絵が即座に返ってきた（frame_age_ms 7.6）。camera_lost の検出ロジック自体は物理 USB カメラで機能する設計だが、Iriun という仮想カメラでは「切断」が read 失敗として現れない。実運用では試行前に Iriun の接続を目視確認する、という運用ルールで受けることにし、docstring に既知の制約として焼き込んだ。カメラの揺らぎ（p95 93.7ms）に続いて、将来有線 USB カメラへ替える実測根拠がもうひとつ積まれた形になる。
実機併走: 初回で pick=yes・place=yes
recorder を起動した状態で本線を実機実行した。試行1が recorder あり、試行2が recorder を止めた対照。両試行の本線ログを全量掲載する。
試行1。連続記録つきで Pick & Place が完走した回のログである。
[LOOP] === 試行 #1 開始 ===
======================================================================
[RUN] run_pick_place.py dry_run=False log=run_0001_20260715_183550.json
======================================================================
[Pi] fresh_mode=1
[INIT ] fresh_mode=1
[STAGE 1] HOME 復帰 + グリッパー開
[STAGE 1] 実測が HOME 一致 (max|Δ|=1.93° ≤ TOL_HOME=2.50°) → HOME 送角スキップ
[GRIP ] stage1_grip_open action=open method=state(0) sleep=2.0s dry_run=False
[Pi] GRIPPER: OPEN result= None
[STAGE 1] 内訳時間: home_move=0.00s (skipped=True) / gripper_open=6.02s
[STAGE 2] 赤ブロック + 白箱 同時検出（block と box は同回・同 homography）
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[405, 1627] Y=[77, 959]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=87.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1404,594) source=template_fallback score=0.629 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 4.55s)
[DETECT][1/3] box Unity=(-0.198,+0.178) score=0.6292820572853088
赤ブロック pixel=(632, 746) → Unity=(0.1964, 0.2540)
[DETECT][1/3] block Unity=(+0.196,+0.254) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=86.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1404,594) source=template_fallback score=0.629 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 4.55s)
[DETECT][2/3] box Unity=(-0.198,+0.178) score=0.6290791630744934
赤ブロック pixel=(632, 746) → Unity=(0.1964, 0.2540)
[DETECT][2/3] block Unity=(+0.196,+0.254) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=86.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1404,594) source=template_fallback score=0.629 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 4.44s)
[DETECT][3/3] box Unity=(-0.198,+0.178) score=0.6288208365440369
赤ブロック pixel=(632, 747) → Unity=(0.1964, 0.2545)
[DETECT][3/3] block Unity=(+0.196,+0.254) score=null
[DETECT] block_xz=[0.19641192626953125, 0.25398309326171875] block_score=None
[DETECT] box_xz=[-0.1976074676513672, 0.1775328826904297] box_score=0.6290791630744934
[CORR ] key=pick_x200_z260 dx=+4.75mm dz=-15.00mm n=39 target (+196.4,+254.0) -> (+191.7,+269.0) [mm]
[LOOKUP] stage3_approach_pick phase=approach_pick target_xz=(+0.1917,+0.2690) → j0=-31.930° method=fk_nearest x_resid=0.00222m z_resid=0.00009m joint_ok=True
[SEND] stage3_approach_pick: send_angles (single) [ -31.93, +87.74, -43.33, +8.60, -65.11, -30.00] speed=20 wait=1.0s (pre max|Δ|=86.16°, post-send timeout 14.9s)
[SEND] arrival OK: max|Δ|=1.92° (tol 2.0°)
[MOVE ] stage3_approach_pick arrived=True retried=False post_max|Δ|=1.920°
[LOOKUP] stage4_pick phase=pick target_xz=(+0.1917,+0.2690) → j0=-31.930° method=fk_nearest x_resid=0.00479m z_resid=0.00449m joint_ok=True
[SEND] stage4_pick: send_angles (single) [ -31.93, +87.74, -33.33, +8.60, -50.11, -30.00] speed=20 wait=1.0s (pre max|Δ|=13.61°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.48° (tol 3.0°)
[MOVE ] stage4_pick arrived=True retried=False post_max|Δ|=1.480°
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[CAP-VLM] stage4_pick_tip 保存 (上カメラ): /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260715_183550/run_0001_20260715_183550_stage4_pick_tip.jpg
[TIP ] attempt=1 dist=3.9mm (dx=2.5 dz=-3.0)
[GRIP ] stage5_grip_close action=close method=value(20) sleep=2.0s dry_run=False
[Pi] GRIPPER: VALUE=20 speed=100 result= None
[SEND] stage6_lift: send_angles (single) [ -31.93, +87.74, -43.33, +8.60, -65.11, -30.00] speed=20 wait=1.0s (pre max|Δ|=12.91°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.50° (tol 2.0°)
[MOVE ] stage6_lift arrived=True retried=False post_max|Δ|=1.500°
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[CAP-VLM] pick 保存 (上カメラ): /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260715_183550/run_0001_20260715_183550_pick.jpg
[JUDGE ] pick attempt=1 adopted=yes (vlm=yes human=None dist_override=False) confidence=0.82 reason=2枚目でグリッパーの爪が閉じており、赤ブロックが爪の間に挟まれた状態でマット面から浮き上がっている（影や位置からブロックが持ち上がっていると判断）
[CORR ] mem key=pick_x200_z260 dx=+6.01 dz=-115.78 n=40 (EMA α=0.5・最終 yes のみ更新)
[LOOKUP] stage7_approach_place phase=approach_place target_xz=(-0.1976,+0.1775) → j0=+39.031° method=interp x_resid=0.00000m z_resid=0.05049m joint_ok=True
[INFO ] stage7_approach_place z_residual_m=0.0505m （情報のみ・ABORT しない。工程跨ぎや弧高さ差で構造的に大きいことがある）
[SEND] stage7_approach_place: send_angles (single) [ +39.03, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=71.54°, post-send timeout 12.7s)
[SEND] arrival OK: max|Δ|=2.46° (tol 3.0°)
[MOVE ] stage7_approach_place arrived=True retried=False post_max|Δ|=2.460°
[CORR ] PLACE measure mode: apply/update skipped
[HGT ] PLACE apply: cell=place_x-200_z180 dee_y=15.0mm → target_ee_y=0.1150m
[PLACE_LOOKUP_OBS] tag=stage8 method=fk_j4_priority_j4safe_pref ee_y_selected=+0.1039m d_ee_y_vs_prev=(first) max_dj=+12.080° target_angles=[+41.22,+57.00,+20.00,-12.00,-86.00,-30.00]
[LOOKUP] stage8_place phase=place_withblock target_xz=(-0.1976,+0.1775) → j0=+41.215° method=fk_j4_priority_j4safe_pref x_resid=0.00525m z_resid=0.00004m joint_ok=True max|Δj|=12.08° ee_y=0.1039m
[SEND] stage8_place: send_angles (single) [ +41.22, +57.00, +20.00, -12.00, -86.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=12.00°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=2.67° (tol 3.0°)
[MOVE ] stage8_place arrived=True retried=False post_max|Δ|=2.670°
[SERVO] stage8 到達姿勢: j0=+41.04 base_j1..j5=[59.67, 19.86, -11.68, -88.59, -29.61]
[SERVO] target_x_mm=-197.61 (box_xz[0]=-0.1976m)
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[SERVO] iter=1 block_x=-219.1 target=-197.6 diff=+21.4 j0 +41.04→+37.81 (raw=-3.23 clamp=-3.23)
[SERVO] [CONVERGED] iter=2 block_x=-203.5 target=-197.6 diff=+5.9mm <= 8.0mm
[PLACE_LOOKUP_OBS] tag=zservo_i1 method=fk_j4_priority_j4safe_pref ee_y_selected=+0.1039m d_ee_y_vs_prev=(first) max_dj=+12.080° target_angles=[+36.22,+57.00,+20.00,-12.00,-86.00,-30.00]
[SEND] stage8_place_zservo_i1: send_angles (single) [ +36.22, +57.00, +20.00, -12.00, -86.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=3.82°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=2.41° (tol 3.0°)
[MOVE ] stage8_place_zservo_i1 arrived=True retried=False post_max|Δ|=2.410°
[SERVO] [CONVERGED] iter=1 block_x=-200.4 target=-197.6 diff=+2.8mm <= 8.0mm
[PLACE_LOOKUP_OBS] tag=zservo_i2 method=out_of_range ee_y_selected=None d_ee_y_vs_prev=(first) max_dj=None target_angles=[+36.22,+57.00,+20.00,-12.00,-86.00,-30.00]
[SERVO] Z-GUARD-STOP iter=2 action=guard_stop:joint_limit違反: [] (last_safe_pose のまま Z 詰め終了)
[SERVO] PLACE 再照準 完了 converged=True iter=3 exit=converged z_iter=2 z_exit=guard_stop:joint_limit違反: [] j0 41.04→36.215 elapsed=4.41s
[JUDGE ] place_side: verdict=yes confidence=0.72 reason=グリッパーの爪の底部は白い箱の上縁よりも上方に位置しており、縦方向に接触している様子は見られない。ただし横からの視点のため奥行き方向の重なりは不明瞭。
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[CAP-VLM] place_pre 保存 (上カメラ): /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260715_183550/run_0001_20260715_183550_place_pre.jpg
[PLACE_PRE] block xz_m=(-0.1988,+0.2103)
[GRIP ] stage9_release action=open pre_sleep=2.0s attempts=2 sleep_each=2.0s dry_run=False
[Pi] GRIPPER: OPEN result= None
[Pi] GRIPPER: OPEN result= None
[SEND] stage9b_swept_retreat: send_angles (single) [ +85.00, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=48.79°, post-send timeout 9.3s)
[SEND] arrival OK: max|Δ|=2.02° (tol 3.0°)
[MOVE ] stage9b_swept_retreat arrived=True retried=False post_max|Δ|=2.020°
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[CAP-VLM] place 保存 (上カメラ): /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260715_183550/run_0001_20260715_183550_place.jpg
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[HGT ] update place_x-200_z180 dist=183.4mm → dee_y=20.0mm (n=4)
[JUDGE ] place: adopted=yes (vlm=yes human=None) confidence=0.98 reason=白い箱の内側に赤い四角いブロックが明確に確認できる
[CORR ] PLACE measure mode: apply/update skipped
[SEND] stage10_lift_back: send_angles (single) [ +39.03, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=45.87°, post-send timeout 8.9s)
[SEND] arrival OK: max|Δ|=2.02° (tol 3.0°)
[MOVE ] stage10_lift_back arrived=True retried=False post_max|Δ|=2.020°
[SEND] stage10_to_home: send_angles (single) [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=89.91°, post-send timeout 15.5s)
[SEND] arrival OK: max|Δ|=2.37° (tol 2.5°)
[MOVE ] stage10_to_home arrived=True retried=False post_max|Δ|=2.370°
[RESULT] 完走
[Pi] fresh_mode=0
[LOG ] /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260715_183550/run_0001_20260715_183550.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (in_box_geom=True dist=43.5)
[LOOP] === 試行 #1 終了: success=True log=/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260715_183550/run_0001_20260715_183550.json ===このログの place_side の判定行に注目してほしい。verdict=yes confidence=0.72 の横カメラ画像は、recorder の録画ストリームから frame_request で切り出されたものだ。カメラの所有を recorder に一本化した設計が、実機で初めて機能した瞬間になる。
試行2。recorder を Ctrl-C で止めた対照試行のログである。recorder 不在でも本線が従来どおり動くことの実機確認になる。
[LOOP] === 試行 #2 開始 ===
======================================================================
[RUN] run_pick_place.py dry_run=False log=run_0001_20260715_183739.json
======================================================================
[Pi] fresh_mode=1
[INIT ] fresh_mode=1
[STAGE 1] HOME 復帰 + グリッパー開
[STAGE 1] 実測が HOME 一致 (max|Δ|=2.10° ≤ TOL_HOME=2.50°) → HOME 送角スキップ
[GRIP ] stage1_grip_open action=open method=state(0) sleep=2.0s dry_run=False
[Pi] GRIPPER: OPEN result= None
[STAGE 1] 内訳時間: home_move=0.00s (skipped=True) / gripper_open=6.02s
[STAGE 2] 赤ブロック + 白箱 同時検出（block と box は同回・同 homography）
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[406, 1628] Y=[77, 960]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=86.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1404,594) source=template_fallback score=0.629 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 4.34s)
[DETECT][1/3] box Unity=(-0.197,+0.178) score=0.6292060017585754
赤ブロック pixel=(622, 759) → Unity=(0.2020, 0.2603)
[DETECT][1/3] block Unity=(+0.202,+0.260) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=87.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1404,594) source=template_fallback score=0.629 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 4.40s)
[DETECT][2/3] box Unity=(-0.197,+0.178) score=0.6290402412414551
赤ブロック pixel=(622, 759) → Unity=(0.2020, 0.2603)
[DETECT][2/3] block Unity=(+0.202,+0.260) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=87.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1404,594) source=template_fallback score=0.630 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 4.28s)
[DETECT][3/3] box Unity=(-0.197,+0.178) score=0.6296788454055786
赤ブロック pixel=(622, 759) → Unity=(0.2020, 0.2603)
[DETECT][3/3] block Unity=(+0.202,+0.260) score=null
[DETECT] block_xz=[0.2020367431640625, 0.260343017578125] block_score=None
[DETECT] box_xz=[-0.19705598449707032, 0.17754719543457032] box_score=0.6292060017585754
[CORR ] key=pick_x200_z260 dx=+6.01mm dz=-15.00mm n=40 target (+202.0,+260.3) -> (+196.0,+275.3) [mm]
[LOOKUP] stage3_approach_pick phase=approach_pick target_xz=(+0.1960,+0.2753) → j0=-31.930° method=fk_nearest x_resid=0.00214m z_resid=0.00645m joint_ok=True
[SEND] stage3_approach_pick: send_angles (single) [ -31.93, +87.74, -43.33, +8.60, -65.11, -30.00] speed=20 wait=1.0s (pre max|Δ|=86.16°, post-send timeout 14.9s)
[SEND] arrival OK: max|Δ|=1.50° (tol 2.0°)
[MOVE ] stage3_approach_pick arrived=True retried=False post_max|Δ|=1.500°
[LOOKUP] stage4_pick phase=pick target_xz=(+0.1960,+0.2753) → j0=-31.930° method=fk_nearest x_resid=0.00915m z_resid=0.01085m joint_ok=True
[SEND] stage4_pick: send_angles (single) [ -31.93, +87.74, -33.33, +8.60, -50.11, -30.00] speed=20 wait=1.0s (pre max|Δ|=14.05°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.65° (tol 3.0°)
[MOVE ] stage4_pick arrived=True retried=False post_max|Δ|=1.650°
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[CAP-VLM] stage4_pick_tip 保存 (上カメラ): /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260715_183739/run_0001_20260715_183739_stage4_pick_tip.jpg
[TIP ] attempt=1 dist=8.2mm (dx=-3.3 dz=-7.5)
[GRIP ] stage5_grip_close action=close method=value(20) sleep=2.0s dry_run=False
[Pi] GRIPPER: VALUE=20 speed=100 result= None
[SEND] stage6_lift: send_angles (single) [ -31.93, +87.74, -43.33, +8.60, -65.11, -30.00] speed=20 wait=1.0s (pre max|Δ|=12.73°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.66° (tol 2.0°)
[MOVE ] stage6_lift arrived=True retried=False post_max|Δ|=1.660°
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[CAP-VLM] pick 保存 (上カメラ): /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260715_183739/run_0001_20260715_183739_pick.jpg
[JUDGE ] pick attempt=1 adopted=yes (vlm=yes human=None dist_override=False) confidence=0.82 reason=2枚目の画像でグリッパーの爪の間に赤ブロックが挟まれており、1枚目と比較してブロックの位置が爪と共に持ち上がっている様子が確認できる
[CORR ] mem key=pick_x200_z260 dx=+4.38 dz=-119.53 n=41 (EMA α=0.5・最終 yes のみ更新)
[LOOKUP] stage7_approach_place phase=approach_place target_xz=(-0.1971,+0.1775) → j0=+38.916° method=interp x_resid=0.00000m z_resid=0.05084m joint_ok=True
[INFO ] stage7_approach_place z_residual_m=0.0508m （情報のみ・ABORT しない。工程跨ぎや弧高さ差で構造的に大きいことがある）
[SEND] stage7_approach_place: send_angles (single) [ +38.92, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=71.43°, post-send timeout 12.7s)
[SEND] arrival OK: max|Δ|=2.63° (tol 3.0°)
[MOVE ] stage7_approach_place arrived=True retried=False post_max|Δ|=2.630°
[CORR ] PLACE measure mode: apply/update skipped
[HGT ] PLACE apply: cell=place_x-200_z180 dee_y=20.0mm → target_ee_y=0.1200m
[PLACE_LOOKUP_OBS] tag=stage8 method=fk_j4_priority_j4safe_pref ee_y_selected=+0.1065m d_ee_y_vs_prev=(first) max_dj=+12.080° target_angles=[+41.22,+57.00,+15.00,-12.00,-81.00,-30.00]
[LOOKUP] stage8_place phase=place_withblock target_xz=(-0.1971,+0.1775) → j0=+41.215° method=fk_j4_priority_j4safe_pref x_resid=0.01144m z_resid=0.00711m joint_ok=True max|Δj|=12.08° ee_y=0.1065m
[SEND] stage8_place: send_angles (single) [ +41.22, +57.00, +15.00, -12.00, -81.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=11.83°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=2.75° (tol 3.0°)
[MOVE ] stage8_place arrived=True retried=False post_max|Δ|=2.670°
[SERVO] stage8 到達姿勢: j0=+41.39 base_j1..j5=[59.67, 16.52, -11.77, -83.67, -29.53]
[SERVO] target_x_mm=-197.06 (box_xz[0]=-0.1971m)
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[SERVO] iter=1 block_x=-231.3 target=-197.1 diff=+34.2 j0 +41.39→+36.39 (raw=-5.16 clamp=-5.00)
[SERVO] iter=2 block_x=-208.4 target=-197.1 diff=+11.4 j0 +36.56→+34.84 (raw=-1.72 clamp=-1.72)
[SERVO] [CONVERGED] iter=3 block_x=-201.2 target=-197.1 diff=+4.1mm <= 8.0mm
[PLACE_LOOKUP_OBS] tag=zservo_i1 method=fk_j4_priority_j4safe_pref ee_y_selected=+0.1065m d_ee_y_vs_prev=(first) max_dj=+12.080° target_angles=[+36.22,+57.00,+15.00,-12.00,-81.00,-30.00]
[SEND] stage8_place_zservo_i1: send_angles (single) [ +36.22, +57.00, +15.00, -12.00, -81.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=3.82°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=2.41° (tol 3.0°)
[MOVE ] stage8_place_zservo_i1 arrived=True retried=False post_max|Δ|=2.410°
[SERVO] iter=1 block_x=-214.1 target=-197.1 diff=+17.0 j0 +36.22→+33.65 (raw=-2.57 clamp=-2.57)
[SERVO] iter=2 block_x=-183.1 target=-197.1 diff=-13.9 j0 +33.66→+35.76 (raw=+2.10 clamp=+2.10)
[SERVO] [CONVERGED] iter=3 block_x=-190.9 target=-197.1 diff=-6.1mm <= 8.0mm
[PLACE_LOOKUP_OBS] tag=zservo_i2 method=out_of_range ee_y_selected=None d_ee_y_vs_prev=(first) max_dj=None target_angles=[+36.22,+57.00,+15.00,-12.00,-81.00,-30.00]
[SERVO] Z-GUARD-STOP iter=2 action=guard_stop:joint_limit違反: [] (last_safe_pose のまま Z 詰め終了)
[SERVO] PLACE 再照準 完了 converged=True iter=6 exit=converged z_iter=2 z_exit=guard_stop:joint_limit違反: [] j0 41.39→35.33 elapsed=12.36s
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[CAP-VLM] place_pre 保存 (上カメラ): /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260715_183739/run_0001_20260715_183739_place_pre.jpg
[PLACE_PRE] block xz_m=(-0.1904,+0.2227)
[GRIP ] stage9_release action=open pre_sleep=2.0s attempts=2 sleep_each=2.0s dry_run=False
[Pi] GRIPPER: OPEN result= None
[Pi] GRIPPER: OPEN result= None
[SEND] stage9b_swept_retreat: send_angles (single) [ +85.00, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=49.67°, post-send timeout 9.5s)
[SEND] arrival OK: max|Δ|=2.72° (tol 3.0°)
[MOVE ] stage9b_swept_retreat arrived=True retried=False post_max|Δ|=2.720°
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[CAP-VLM] place 保存 (上カメラ): /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260715_183739/run_0001_20260715_183739_place.jpg
[HGT ] update place_x-200_z180 dist=184.4mm → dee_y=25.0mm (n=5)
[JUDGE ] place: adopted=yes (vlm=yes human=None) confidence=0.98 reason=白い四角い箱の中に赤い四角形のブロックが明確に入っているのが確認できる
[CORR ] PLACE measure mode: apply/update skipped
[SEND] stage10_lift_back: send_angles (single) [ +38.92, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=45.89°, post-send timeout 8.9s)
[SEND] arrival OK: max|Δ|=1.93° (tol 3.0°)
[MOVE ] stage10_lift_back arrived=True retried=False post_max|Δ|=1.930°
[SEND] stage10_to_home: send_angles (single) [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=88.85°, post-send timeout 15.3s)
[SEND] arrival OK: max|Δ|=2.28° (tol 2.5°)
[MOVE ] stage10_to_home arrived=True retried=False post_max|Δ|=2.280°
[RESULT] 完走
[Pi] fresh_mode=0
[LOG ] /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260715_183739/run_0001_20260715_183739.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (in_box_geom=True dist=45.0)
[LOOP] === 試行 #2 終了: success=True log=/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260715_183739/run_0001_20260715_183739.json ===試行2では place_side の判定行が存在しない。frame_request がタイムアウトして画像 None となり、既存の side なし経路に流れた上で、上カメラの place 判定（confidence 0.98 yes）が従来どおり返っている。recorder-off が「side 画像が付かない」以外は現行と同一——設計の最重要要件が実機で確認できた。
記録された成果物の突合
台帳の新規2行。79〜82列の埋まり方が2試行の対照になっている。
run_stem: run_0001_20260715_183550
recording_path : recording/
recording_fps : 27.237
recording_frames: 2443
recording_status: ok
run_stem: run_0001_20260715_183739
recording_path :
recording_fps :
recording_frames:
recording_status: none試行1の recording_meta.json。89.7秒・2443フレーム・captured=written・実効 27.24fps。計測フェーズの予測（720p/avc1 で約27fps）どおりの数字が実機で再現した。
{
"run_stem": "run_0001_20260715_183550",
"started_ts": 1784108150.6671722,
"ended_ts": 1784108240.361095,
"elapsed_sec": 89.694,
"frames_captured": 2443,
"frames_written": 2443,
"frame_count": 2443,
"effective_fps": 27.237,
"container_fps": 30.0,
"width": 1280,
"height": 720,
"fourcc": "avc1",
"status": "ok",
"recording_path": "recording/"
}states.csv の冒頭。stage 遷移・グリッパー・送角が一本の時系列に並び、commanded 行には ts_send（送信時刻）と ts_arrival（到達確認時刻）が分かれて入っている。
ts,event_type,stage,j0,j1,j2,j3,j4,j5,speed,gripper_state,ts_send,ts_arrival,attempt,arrived,note
1784108150.621998,stage,stage1,,,,,,,,,,,,,
1784108156.663297,gripper,stage1,,,,,,,,0,,,,,stage1_grip_open
1784108156.665343,stage,stage2,,,,,,,,,,,,,
1784108176.145909,stage,stage3_approach_pick,,,,,,,,,,,,,
1784108182.411492,commanded,stage3_approach_pick,-31.930,87.740,-43.330,8.600,-65.110,-30.000,20,,1784108177.167378,1784108182.411485,1,True,stage3_approach_pick
1784108182.433683,stage,stage4_pick,,,,,,,,,,,,,
1784108183.562288,commanded,stage4_pick,-31.930,87.740,-33.330,8.600,-50.110,-30.000,20,,1784108182.487511,1784108183.562280,1,True,stage4_pick
1784108184.960893,stage,stage5,,,,,,,,,,,,,
1784108186.977066,gripper,stage5,,,,,,,,1,,,,,stage5_grip_close
1784108186.982768,stage,stage6,,,,,,,,,,,,,
1784108190.345179,commanded,stage6,-31.930,87.740,-43.330,8.600,-65.110,-30.000,20,,1784108187.007335,1784108190.345175,1,True,stage6_lift
1784108195.608771,stage,stage7_approach_place,,,,,,,,,,,,,
1784108198.315290,commanded,stage7_approach_place,39.031,57.390,17.490,0.080,-89.200,-29.440,20,,1784108195.662214,1784108198.315286,1,True,stage7_approach_place
1784108198.379523,stage,stage8_place,,,,,,,,,,,,,
1784108199.555332,commanded,stage8_place,41.215,57.000,20.000,-12.000,-86.000,-30.000,20,,1784108198.506827,1784108199.555328,1,True,stage8_place
1784108199.577803,stage,stage8_5_replace_align,,,,,,,,,,,,,
1784108203.046143,commanded,stage8_5_replace_align,37.805,59.670,19.860,-11.680,-88.590,-29.610,20,,1784108201.014781,,1,,place_visual_servo
1784108204.822419,commanded,stage8_5_replace_align,36.215,57.000,20.000,-12.000,-86.000,-30.000,20,,1784108203.778836,1784108204.822414,1,True,stage8_place_zservo_i1commanded 行数は10で、本線ログの送角回数（stage3/4/6/7/8・視覚サーボ直接送角・zservo・stage9b・lift_back・to_home）と完全に一致した。stage3 の行を見ると ts_send と ts_arrival の差は5.24秒——アームが86度動くのにかかった実時間がそのまま記録されている。この差こそ、レビューで指摘された「到着後の時刻では映像と突合できない」問題を修正した成果になる。視覚サーボの行（note=place_visual_servo）は ts_arrival と arrived が空で、到達確認を経ない送角の性質がデータ形式にそのまま表れている。
何が閉じて、何が残ったか
これで1試行あたり、82列の台帳1行・約90秒の720p映像・タイムスタンプ列（frame_log.csv）・送信時刻つきの関節指令列（states.csv）が、本線に一切干渉せず自動で残る状態になった。試行を重ねるほど、模倣学習用データセットへの変換素材が蓄積されていく。
同時に、この構成の限界も実測で明確になった。関節の実測値（measured）は single-client daemon の構造上取れず、指令値のみの記録である。カメラは fps を指定できず、p95 で 90ms 超の穴が空き、切断も検出できない。これらは全て「どの部品を、何の実測を根拠に、いつ替えるか」の判断材料として残る。精度はあとから解決できる——まず記録の体験を成立させることを優先した回だった。
次は、この記録から LeRobot データセット形式への変換可否の検証に進む。正本（台帳＋recording/）は自分の設計のまま持ち、学習形式は変換スクリプトが生成する派生物とする方針で、形式の要求が変わっても正本側は無傷で済む構えにする。
