#!/usr/bin/env python3 """measure_timing_alignment.py — 録画と記録から「映像と関節の時刻のずれ」と「制御の1回の間隔」を測る (依頼 249). 読むだけ. アーム・サーボ・daemon には一切つながない. runs/ と datasets/ の既存ファイルは書き換えない. 対象 (3 組): demo : datasets/mecharm_redblock_box_split10_v2 の全エピソード 元の記録 = runs/run_pick_place/{run}/ (record_teach_demo.py. build_demo_episodes.py で作った) policy : runs/pi05_policy/ の直近 N 本 (recording/ がある run. 既定 20) pad_corr : ゲームパッド補正 (log.jsonl に type=pad) の run のうち, datasets/mecharm_redblock_box_corr_v5 に入っている本 (build_correction_episodes.py --gap-fill speed20 --trim-hold で作った) 測ること: 1. 映像と関節の時刻のずれ - 1 コマごとに capture_ts といちばん近い actual の時刻の差 - コマ間隔・関節を読む間隔 (狙いの値と実際. 読み取りの失敗の数) - 学習データでの組み方 (state = 10Hz の格子 t に actual を線形補間. 画像 = t にいちばん近いコマ) を作り直し, エピソードの state と突き合わせて同じ格子を見つけたうえで, 組のずれを出す - カメラ自体の遅れ: 動き出しの場面で, 映像のコマ間差分 (動きの量) と関節の速さの立ち上がりの時刻を比べる. あわせて全体の相互相関の山の位置も出す 2. 制御の1回の間隔 - 指令を送った間隔・送ってから届くまで・サーボの読み取り時間・推論 1 回の待ち (latency_s) - 中央値の 2 倍を超えて空いた所の数と, つかむ前/つかんだ後/放した後 のどこか 3. demo / policy / pad_corr の比べ 出力: /Volumes/KIOXIA/r2r2r-data/diag/timing_249/{YYYYmmdd_HHMMSS}/ per_run.csv pairs_dataset.csv frame_actual_offset.csv gaps.csv camera_lag_onsets.csv camera_lag_xcorr.csv summary.md fig_*.png Usage: python3 diag/measure_timing_alignment.py python3 diag/measure_timing_alignment.py --policy-n 20 --no-video # カメラの遅れを測らない (速い) """ import argparse import csv import json import os import sys from datetime import datetime from pathlib import Path import cv2 import numpy as np REPO = Path(__file__).resolve().parent.parent sys.path.insert(0, str(REPO / "pi05")) import build_demo_episodes as bde # noqa: E402 (読むための関数だけ使う) import build_correction_episodes as bce # noqa: E402 from rewind_correct import excluded_windows, step_keep_mask # noqa: E402 DATA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data") OUT_ROOT = DATA_ROOT / "diag" / "timing_249" DEMO_DS = DATA_ROOT / "datasets" / "mecharm_redblock_box_split10_v2" CORR_DS = DATA_ROOT / "datasets" / "mecharm_redblock_box_corr_v5" RUNS_PP = DATA_ROOT / "runs" / "run_pick_place" RUNS_POL = DATA_ROOT / "runs" / "pi05_policy" FPS = bde.FPS # カメラの遅れの見積もり VID_SMALL = (96, 54) # 差分を取る大きさ STILL_DEG_S = 1.0 # これ未満は止まり MOVE_DEG_S = 5.0 # これを超えたら動き STILL_BEFORE_SEC = 0.8 # 動き出しの前に止まっている長さ MAX_JOINT_GAP_SEC = 0.35 # 動き出しをはさむ 2 つの読みの間がこれ以下のときだけ使う VID_K_MAD = 6.0 # 映像の動き出し = 止まりの中央値 + この倍の MAD を超えた最初のコマ XCORR_LAGS = np.arange(-0.30, 0.6001, 0.01) XCORR_GRID = 0.01 def abort(msg): sys.exit("[ABORT] " + msg) def ensure_ssd_mounted(): if not os.path.ismount("/Volumes/KIOXIA"): abort("KIOXIA が未マウント。ローカルには書きません。") if not DATA_ROOT.exists(): abort(f"データの場所がない: {DATA_ROOT}") def st(x): """中央値・95%点・最大 (と数).""" x = np.asarray([v for v in x if v is not None and np.isfinite(v)], dtype=float) if len(x) == 0: return {"n": 0, "median": None, "p95": None, "max": None} return {"n": int(len(x)), "median": float(np.median(x)), "p95": float(np.percentile(x, 95)), "max": float(np.max(x))} def fmt(s, scale=1.0, nd=1): if not s or s["n"] == 0: return "—" return f"{s['median'] * scale:.{nd}f} / {s['p95'] * scale:.{nd}f} / {s['max'] * scale:.{nd}f} (n={s['n']})" def visible(names): return sorted(n for n in names if not n.startswith("._")) # ---------------------------------------------------------------- 記録を読む def read_states(path): """states.csv を全部読む (actual は build_demo_episodes と同じ決まりで弾いた数も数える).""" rows = [] with path.open(newline="") as f: for r in csv.DictReader(f): try: r["_ts"] = float(r["ts"]) except Exception: continue rows.append(r) act_t, act_note, invalid = [], [], 0 for r in rows: if r.get("event_type") != "actual": continue try: q = [float(r[f"j{i}"]) for i in range(6)] except Exception: invalid += 1 continue if any(abs(v) > bde.ANGLE_ABS_MAX for v in q): invalid += 1 continue act_t.append(r["_ts"]); act_note.append(r.get("note") or "") return rows, np.array(act_t), act_note, invalid def gripper_events(rows): ev = sorted((r["_ts"], int(float(r["gripper_state"]))) for r in rows if r.get("event_type") == "gripper_cmd" and r.get("gripper_state") not in (None, "")) return ev def phase_of(ts, gev): """つかむ前 / つかんだ後 / 放した後. 最初の閉じる指令とその後の最初の開く指令で分ける.""" t_close = next((t for t, s in gev if s == 1), None) if t_close is None: return "つかむ前(閉じる指令なし)" t_open = next((t for t, s in gev if s == 0 and t > t_close), None) if ts < t_close: return "つかむ前" if t_open is None or ts < t_open: return "つかんだ後" return "放した後" def stage_at(rows, ts): """その時刻の stage 列 (demo は区間名, policy は model/correct/wait など).""" s = "" for r in rows: if r["_ts"] > ts: break if r.get("stage"): s = r["stage"] return s def read_currents_ms(path): out = [] if not path.exists(): return out with path.open(newline="") as f: for r in csv.DictReader(f): try: out.append(float(r["rpc_ms"])) except Exception: pass return out def read_log(path): out = [] if not path.exists(): return out for line in path.open(errors="ignore"): try: out.append(json.loads(line)) except Exception: pass return out def note_val(note, key): for tok in (note or "").split(): if tok.startswith(key + "="): try: return float(tok.split("=", 1)[1]) except ValueError: return None return None # ---------------------------------------------------------------- カメラの遅れ def video_motion(mp4, fts): """コマ間差分の大きさ (1 秒あたり). 時刻は 2 コマの capture_ts の中点.""" cap = cv2.VideoCapture(str(mp4)) prev, vals, mids, dts, i = None, [], [], [], 0 while i < len(fts): ok, fr = cap.read() if not ok: break g = cv2.cvtColor(cv2.resize(fr, VID_SMALL, interpolation=cv2.INTER_AREA), cv2.COLOR_BGR2GRAY) g = cv2.GaussianBlur(g, (3, 3), 0).astype(np.float32) if prev is not None: dt = fts[i] - fts[i - 1] if dt > 0: vals.append(float(np.mean(np.abs(g - prev))) / dt) mids.append((fts[i] + fts[i - 1]) / 2); dts.append(dt) prev = g; i += 1 cap.release() return np.array(mids), np.array(vals), np.array(dts), i def joint_speed(at, aq): dt = np.diff(at) ok = dt > 1e-3 v = np.full(len(dt), np.nan) v[ok] = np.max(np.abs(np.diff(aq, axis=0))[ok], axis=1) / dt[ok] return v, dt def onset_lags(at, aq, vm_t, vm_v, vm_dt): """動き出しごとに 映像の立ち上がり − 関節の立ち上がり.""" v, dt = joint_speed(at, aq) res = [] if len(vm_v) < 10: return res for k in range(1, len(v)): if not (np.isfinite(v[k]) and v[k] > MOVE_DEG_S and dt[k] <= MAX_JOINT_GAP_SEC): continue # 区間 k (at[k]→at[k+1]) で動き出した. その前 STILL_BEFORE_SEC は止まり j = k - 1; still_ok = True while j >= 0 and at[k] - at[j] < STILL_BEFORE_SEC: if not np.isfinite(v[j]) or v[j] >= STILL_DEG_S or dt[j] > MAX_JOINT_GAP_SEC: still_ok = False; break j -= 1 if not still_ok or j < 0: continue t_joint = (at[k] + at[k + 1]) / 2 unc_joint = (at[k + 1] - at[k]) / 2 base = (vm_t >= at[k] - STILL_BEFORE_SEC) & (vm_t <= at[k]) if base.sum() < 2: continue b = vm_v[base]; med = np.median(b); mad = np.median(np.abs(b - med)) + 1e-6 thr = med + VID_K_MAD * 1.4826 * mad win = np.where((vm_t > at[k] - 0.2) & (vm_t <= at[k] + 1.5) & (vm_v > thr))[0] if len(win) == 0: continue n = win[0] res.append({"t_joint_onset": t_joint, "joint_unc_s": unc_joint, "t_video_onset": vm_t[n], "video_unc_s": vm_dt[n] / 2, "lag_s": vm_t[n] - t_joint, "unc_s": unc_joint + vm_dt[n] / 2, "joint_speed": float(v[k]), "video_thr": float(thr), "video_val": float(vm_v[n])}) return res def xcorr_lag(at, aq, vm_t, vm_v): """関節の速さと映像の動きの量の相互相関. 正 = 映像が遅れている.""" v, dt = joint_speed(at, aq) mid = (at[1:] + at[:-1]) / 2 ok = np.isfinite(v) & (dt <= MAX_JOINT_GAP_SEC) if ok.sum() < 20 or len(vm_v) < 20: return None, None jt, jv = mid[ok], v[ok] t0 = max(jt[0], vm_t[0]) + 0.7; t1 = min(jt[-1], vm_t[-1]) - 0.7 if t1 - t0 < 5: return None, None grid = np.arange(t0, t1, XCORR_GRID) # 関節の速さが分かっている所 (読みの間が短い所) だけを使う near = np.min(np.abs(grid[:, None] - jt[None, :]), axis=1) if len(grid) * len(jt) < 4e7 else None jg = np.interp(grid, jt, jv) best, best_r = None, -2 for L in XCORR_LAGS: vg = np.interp(grid + L, vm_t, vm_v) m = np.ones(len(grid), bool) if near is None else near <= MAX_JOINT_GAP_SEC if m.sum() < 50: continue a = jg[m] - jg[m].mean(); b = vg[m] - vg[m].mean() den = np.sqrt((a * a).sum() * (b * b).sum()) r = float((a * b).sum() / den) if den > 0 else -2 if r > best_r: best_r, best = r, float(L) return best, best_r # ---------------------------------------------------------------- 学習データの組を作り直す def rebuild_pairs(group, run_dir, ep_dir): """エピソードの state と同じ 10Hz 格子を見つけ, 各ステップの画像と関節の組のずれを出す.""" rec = run_dir / "recording" at, aq, _gt, _gs = bde.load_states(rec / "states.csv") order = np.argsort(at); at, aq = at[order], aq[order] at, aq = bde.drop_spikes(at, aq) fidx, fts = bde.load_frame_log(rec / "top_frame_log.csv") t0, t1 = max(fts[0], at[0]), min(fts[-1], at[-1]) t = np.arange(t0, t1, 1.0 / FPS) meta = json.loads((ep_dir / "meta.json").read_text()) if group == "demo": q = np.stack([np.interp(t, at, aq[:, k]) for k in range(6)], axis=1) cand = np.ones(len(fts), bool) else: mode = (meta.get("gap_fill") or {}).get("mode", "linear") q, _ = bce.fill_motion_gaps(t, at, aq, mode=mode) log = bce.load_log(run_dir / "log.jsonl") mode_ts, modes = bce.load_stage_times(rec / "states.csv") cand, _ = step_keep_mask(fts, mode_ts, modes, excluded_windows(log)) ep = np.load(ep_dir / "episode.npz") trel = ep["t_orig"] if "t_orig" in ep.files else ep["t"] offs = np.round(trel * FPS).astype(int) S = ep["state"][:, :6].astype(np.float64) best_n0, best_err = None, np.inf for n0 in range(0, len(t) - offs[-1]): e = float(np.max(np.abs(q[n0 + offs] - S))) if e < best_err: best_err, best_n0 = e, n0 if e < 1e-3: break tt = t[best_n0 + offs] cf = fts[cand] nn = np.clip(np.searchsorted(cf, tt), 1, len(cf) - 1) left, right = cf[nn - 1], cf[nn] nn = np.where(np.abs(tt - left) <= np.abs(right - tt), nn - 1, nn) frame_off = cf[nn] - tt k = np.clip(np.searchsorted(at, tt), 1, len(at) - 1) near_actual = np.minimum(np.abs(at[k - 1] - tt), np.abs(at[k] - tt)) bracket = at[k] - at[k - 1] moved = np.max(np.abs(aq[k] - aq[k - 1]), axis=1) rows = [] for i in range(len(tt)): rows.append({"group": group, "run": run_dir.name, "step": i, "t_abs": f"{tt[i]:.6f}", "frame_minus_state_s": round(float(frame_off[i]), 4), "nearest_actual_s": round(float(near_actual[i]), 4), "actual_bracket_s": round(float(bracket[i]), 4), "bracket_move_deg": round(float(moved[i]), 3), "interp_over_gap": int(bracket[i] > bce.GAP_SEC)}) return rows, best_err # ---------------------------------------------------------------- 補間の誤差の見積もり DEMO_BLOCK_SEC = 1.04 # お手本の送る命令の往復 (send_rtt 中央値). この間 actual が読めない def interp_error_sim(run_dir, block=DEMO_BLOCK_SEC): """実機推論の密な actual で, 送るたびに block 秒読めなかったことにして線形補間し, 消した実測との差を出す. 戻り値は 消した実測 1 点ごとの 6 関節の最大誤差 (度) と, その 2 点の間に動いた量 (度).""" rows, _t, _n, _i = read_states(run_dir / "recording" / "states.csv") at, aq, _gt, _gs = bde.load_states(run_dir / "recording" / "states.csv") o = np.argsort(at); at, aq = bde.drop_spikes(at[o], aq[o]) sends = [r["_ts"] for r in rows if r.get("event_type") == "send_angles"] errs, moves = [], [] for ts in sends: drop = (at > ts) & (at < ts + block) keep_l = np.where(at <= ts)[0]; keep_r = np.where(at >= ts + block)[0] if drop.sum() == 0 or len(keep_l) == 0 or len(keep_r) == 0: continue a, b = keep_l[-1], keep_r[0] if at[b] - at[a] > block + 0.3: # もともと抜けていた所は使わない continue idx = np.where(drop)[0] w = (at[idx] - at[a]) / (at[b] - at[a]) est = aq[a][None, :] + w[:, None] * (aq[b] - aq[a])[None, :] e = np.max(np.abs(est - aq[idx]), axis=1) errs += list(e); moves += [float(np.max(np.abs(aq[b] - aq[a])))] * len(e) return errs, moves # ---------------------------------------------------------------- 1 run を測る def measure_run(group, run_dir, do_video): rec = run_dir / "recording" rows, at_all, notes, invalid = read_states(rec / "states.csv") at_all = np.sort(at_all) fidx, fts = bde.load_frame_log(rec / "top_frame_log.csv") gev = gripper_events(rows) res = {"group": group, "run": run_dir.name} out_off, out_gaps, out_on = [], [], [] # コマ間隔・読み間隔 fdt = np.diff(fts); adt = np.diff(at_all) res["frame_dt"] = st(fdt); res["actual_dt"] = st(adt) res["n_frames"] = int(len(fts)); res["n_actual"] = int(len(at_all)); res["actual_invalid"] = invalid if group == "demo": trial = json.loads((run_dir / f"{run_dir.name}.json").read_text()) rm = json.loads((rec / "currents_meta.json").read_text()) if (rec / "currents_meta.json").exists() else {} asmp = trial.get("actual_sampling") or {} res["frame_target"] = "指定なし (カメラ任せ. 約30fps)" res["actual_target"] = f"本体 {asmp.get('target_hz')}Hz + 記録スレッド 最大 {rm.get('target_hz_max')}Hz (本体が眠る間だけ)" res["read_fail"] = int((asmp.get("reads_attempted", 0) - asmp.get("reads_ok", 0)) + (asmp.get("reads_once_attempted", 0) - asmp.get("reads_once_ok", 0)) + rm.get("currents_rows_incomplete", 0) + rm.get("loop_errors", 0)) tm_p = rec / "top_meta.json" res["top_status"] = json.loads(tm_p.read_text()).get("status") if tm_p.exists() else "top_meta.json なし" else: tm_p = rec / "top_meta.json" tm = json.loads(tm_p.read_text()) if tm_p.exists() else {"status": "top_meta.json なし"} res["frame_target"] = "10.0fps" # policy_recorder.RECORD_FPS res["actual_target"] = "記録スレッド 10Hz + 本体の読み (note=main)" res["read_fail"] = int(tm.get("actual_read_failures", 0)) + int(tm.get("currents_rows_incomplete", 0)) res["top_status"] = tm.get("status") # 1 コマごとの いちばん近い actual との差 (actual − capture) k = np.clip(np.searchsorted(at_all, fts), 1, len(at_all) - 1) dl, dr = at_all[k - 1] - fts, at_all[k] - fts near = np.where(np.abs(dl) <= np.abs(dr), dl, dr) res["frame_actual_abs"] = st(np.abs(near)) for i in range(len(fts)): out_off.append({"group": group, "run": run_dir.name, "frame_idx": int(fidx[i]), "capture_ts": f"{fts[i]:.6f}", "nearest_actual_minus_capture_s": round(float(near[i]), 4)}) # サーボの読み取り時間 res["servo_read_ms"] = st(read_currents_ms(rec / "currents.csv")) # 指令・到着・推論 if group == "demo": trial = json.loads((run_dir / f"{run_dir.name}.json").read_text()) segs = trial.get("segments") or [] send_ts = [r["_ts"] for r in rows if r.get("event_type") == "stage"] res["send_dt"] = st(np.diff(send_ts)) res["send_rpc"] = st([s.get("send_rtt_sec") for s in segs if s.get("kind") == "move"]) res["send_to_arrival"] = st([s.get("segment_elapsed_sec") for s in segs if s.get("kind") == "move"]) res["latency"] = st([]) res["infer_dt"] = st([]) res["send_note"] = "stage 行 = 区間 1 つにつき 1 回送る (single_send_per_segment). 届く = 到着確認まで (segment_elapsed_sec)" else: sends = [r for r in rows if r.get("event_type") == "send_angles"] send_ts = [r["_ts"] for r in sends] res["send_dt"] = st(np.diff(send_ts)) res["send_rpc"] = st([note_val(r.get("note"), "rpc_sec") for r in sends]) log = read_log(run_dir / "log.jsonl") chunks = [j for j in log if "chunk" in j and "type" not in j] res["latency"] = st([c.get("latency_s") for c in chunks]) res["infer_dt"] = st(np.diff([c["t"] for c in chunks if "t" in c])) ex = [c["t_exec_end"] - c["t_exec_start"] for c in chunks if c.get("t_exec_end") and c.get("t_exec_start")] pads = [p.get("elapsed_sec") for p in log if p.get("type") == "pad" and p.get("elapsed_sec") is not None] res["send_to_arrival"] = st(ex) res["pad_send_to_reach"] = st(pads) res["ts_arrival_recorded"] = sum(1 for r in sends if r.get("ts_arrival")) res["send_note"] = "send_angles 行. 届く = チャンク 1 つの実行 (t_exec_start→t_exec_end. 中継点こみ). ts_arrival は空" # 間隔が大きく空いた所 for kind, ts in (("send", np.array(send_ts)), ("actual", at_all), ("frame", fts)): if len(ts) < 3: continue d = np.diff(ts); med = float(np.median(d)) for i in np.where(d > 2 * med)[0]: out_gaps.append({"group": group, "run": run_dir.name, "kind": kind, "t_start": f"{ts[i]:.6f}", "gap_s": round(float(d[i]), 4), "median_s": round(med, 4), "phase": phase_of(ts[i], gev), "stage": stage_at(rows, ts[i])}) # カメラの遅れ if do_video: at, aq, _gt, _gs = bde.load_states(rec / "states.csv") o = np.argsort(at); at, aq = bde.drop_spikes(at[o], aq[o]) vm_t, vm_v, vm_dt, nread = video_motion(rec / "top.mp4", fts) res["video_frames_read"] = nread for r in onset_lags(at, aq, vm_t, vm_v, vm_dt): r.update({"group": group, "run": run_dir.name, "phase": phase_of(r["t_joint_onset"], gev)}) out_on.append(r) res["xcorr_lag_s"], res["xcorr_r"] = xcorr_lag(at, aq, vm_t, vm_v) return res, out_off, out_gaps, out_on # ---------------------------------------------------------------- まとめ def pick_runs(policy_n): demo_eps = visible(os.listdir(DEMO_DS / "raw_episodes")) demo = [(RUNS_PP / e, DEMO_DS / "raw_episodes" / e) for e in demo_eps] pol_all = [d for d in visible(os.listdir(RUNS_POL)) if d.startswith("pi05_") and (RUNS_POL / d / "recording" / "states.csv").exists() and (RUNS_POL / d / "recording" / "top_frame_log.csv").exists()] policy = [(RUNS_POL / d, None) for d in pol_all[-policy_n:]] pad_all = [d for d in pol_all if any('"type": "pad"' in ln for ln in (RUNS_POL / d / "log.jsonl").open(errors="ignore"))] corr_eps = set(visible(os.listdir(CORR_DS / "raw_episodes"))) pad = [(RUNS_POL / d, CORR_DS / "raw_episodes" / d) for d in pad_all if d in corr_eps] return demo, policy, pad, pad_all def write_csv(path, rows): if not rows: path.write_text("") return keys = list(rows[0].keys()) for r in rows: for kk in r: if kk not in keys: keys.append(kk) with path.open("w", newline="") as f: w = csv.DictWriter(f, fieldnames=keys) w.writeheader(); w.writerows(rows) def main(): ap = argparse.ArgumentParser() ap.add_argument("--policy-n", type=int, default=20) ap.add_argument("--no-video", action="store_true", help="カメラの遅れを測らない") args = ap.parse_args() ensure_ssd_mounted() out = OUT_ROOT / datetime.now().strftime("%Y%m%d_%H%M%S") if out.exists(): abort(f"同じ名前の出力先がある: {out}") out.mkdir(parents=True) demo, policy, pad, pad_all = pick_runs(args.policy_n) print(f"[INFO] demo {len(demo)} / policy {len(policy)} / pad_corr {len(pad)} (pad の run 全体 {len(pad_all)})") results, offs, gaps, onsets, pairs, match = [], [], [], [], [], [] for group, items in (("demo", demo), ("policy", policy), ("pad_corr", pad)): for run_dir, ep_dir in items: print(f"[RUN] {group} {run_dir.name}") try: r, o, g, on = measure_run(group, run_dir, not args.no_video) except Exception as e: print(f"[WARN] {run_dir.name}: {type(e).__name__}: {e}") results.append({"group": group, "run": run_dir.name, "error": f"{type(e).__name__}: {e}"}) continue results.append(r); offs += o; gaps += g; onsets += on if ep_dir is not None: try: pr, err = rebuild_pairs(group, run_dir, ep_dir) pairs += pr match.append({"group": group, "run": run_dir.name, "state_match_max_err_deg": round(err, 5)}) except Exception as e: print(f"[WARN] 組の作り直し {run_dir.name}: {type(e).__name__}: {e}") match.append({"group": group, "run": run_dir.name, "state_match_max_err_deg": None, "error": f"{type(e).__name__}: {e}"}) def vals(rows, group, key, fn=float): return [fn(r[key]) for r in rows if r["group"] == group and r.get(key) not in (None, "")] groups = ["demo", "policy", "pad_corr"] G = {} for g in groups: rr = [r for r in results if r["group"] == g and "error" not in r] G[g] = {"runs": len(rr)} G[g]["frame_actual_abs"] = st([abs(v) for v in vals(offs, g, "nearest_actual_minus_capture_s")]) G[g]["frame_actual_signed_med"] = st(vals(offs, g, "nearest_actual_minus_capture_s"))["median"] G[g]["n_frames"] = sum(r["n_frames"] for r in rr) G[g]["n_actual"] = sum(r["n_actual"] for r in rr) G[g]["read_fail"] = sum(r.get("read_fail", 0) for r in rr) G[g]["actual_invalid"] = sum(r.get("actual_invalid", 0) for r in rr) G[g]["frame_target"] = rr[0]["frame_target"] if rr else "—" G[g]["actual_target"] = rr[0]["actual_target"] if rr else "—" G[g]["send_note"] = rr[0]["send_note"] if rr else "—" G[g]["xcorr"] = [(r["run"], r.get("xcorr_lag_s"), r.get("xcorr_r")) for r in rr] G[g]["pairs_frame"] = st([abs(v) for v in vals(pairs, g, "frame_minus_state_s")]) G[g]["pairs_frame_signed"] = st(vals(pairs, g, "frame_minus_state_s")) G[g]["pairs_near_actual"] = st(vals(pairs, g, "nearest_actual_s")) G[g]["pairs_bracket"] = st(vals(pairs, g, "actual_bracket_s")) pg = [p for p in pairs if p["group"] == g] G[g]["pairs_n"] = len(pg) G[g]["pairs_over_gap"] = sum(p["interp_over_gap"] for p in pg) G[g]["pairs_over_gap_moving"] = sum(1 for p in pg if p["interp_over_gap"] and p["bracket_move_deg"] > 0.5) G[g]["onset"] = st([o["lag_s"] for o in onsets if o["group"] == g]) G[g]["onset_unc"] = st([o["unc_s"] for o in onsets if o["group"] == g]) G[g]["gaps"] = {} for p in [x for x in gaps if x["group"] == g]: key = (p["kind"], p["phase"]) G[g]["gaps"][key] = G[g]["gaps"].get(key, 0) + 1 # 元の値を合わせた統計 (run ごとの統計の統計にしない) pooled = pool_raw(results, demo, policy, pad) for g in groups: G[g].update(pooled[g]) sim_e, sim_m = [], [] for run_dir, _ in policy: try: e, m = interp_error_sim(run_dir) sim_e += e; sim_m += m except Exception as ex: print(f"[WARN] 補間の見積もり {run_dir.name}: {type(ex).__name__}: {ex}") G["sim"] = {"err": st(sim_e), "err_moving": st([e for e, m in zip(sim_e, sim_m) if m > 0.5]), "n_send_windows": len(set(sim_m)) if sim_m else 0} write_csv(out / "per_run.csv", [flatten(r) for r in results]) write_csv(out / "frame_actual_offset.csv", offs) write_csv(out / "gaps.csv", gaps) write_csv(out / "camera_lag_onsets.csv", onsets) write_csv(out / "camera_lag_xcorr.csv", [{"group": g, "run": a, "lag_s": b, "r": c} for g in groups for a, b, c in G[g]["xcorr"]]) write_csv(out / "pairs_dataset.csv", pairs) write_csv(out / "state_match.csv", match) make_figs(out, offs, pairs, onsets, G) write_summary(out, G, demo, policy, pad, pad_all, match, args) print(f"[DONE] {out}") def pool_raw(results, demo, policy, pad): """各 run の元の値を合わせた統計.""" out = {} for g, items in (("demo", demo), ("policy", policy), ("pad_corr", pad)): fdt, adt, srv, sdt, rpc, arr, lat, inf, padr = [], [], [], [], [], [], [], [], [] for run_dir, _ in items: rec = run_dir / "recording" try: rows, at_all, _n, _inv = read_states(rec / "states.csv") _fi, fts = bde.load_frame_log(rec / "top_frame_log.csv") except Exception: continue fdt += list(np.diff(fts)); adt += list(np.diff(np.sort(at_all))) srv += read_currents_ms(rec / "currents.csv") if g == "demo": trial = json.loads((run_dir / f"{run_dir.name}.json").read_text()) segs = [s for s in trial.get("segments") or [] if s.get("kind") == "move"] sdt += list(np.diff([r["_ts"] for r in rows if r.get("event_type") == "stage"])) rpc += [s.get("send_rtt_sec") for s in segs] arr += [s.get("segment_elapsed_sec") for s in segs] else: sends = [r for r in rows if r.get("event_type") == "send_angles"] sdt += list(np.diff([r["_ts"] for r in sends])) rpc += [note_val(r.get("note"), "rpc_sec") for r in sends] log = read_log(run_dir / "log.jsonl") ch = [j for j in log if "chunk" in j and "type" not in j] lat += [c.get("latency_s") for c in ch] inf += list(np.diff([c["t"] for c in ch if "t" in c])) arr += [c["t_exec_end"] - c["t_exec_start"] for c in ch if c.get("t_exec_end") and c.get("t_exec_start")] padr += [p.get("elapsed_sec") for p in log if p.get("type") == "pad" and p.get("elapsed_sec") is not None] out[g] = {"P_frame_dt": st(fdt), "P_actual_dt": st(adt), "P_servo_ms": st(srv), "P_send_dt": st(sdt), "P_send_rpc": st(rpc), "P_arrival": st(arr), "P_latency": st(lat), "P_infer_dt": st(inf), "P_pad_reach": st(padr)} return out def flatten(r): o = {} for k, v in r.items(): if isinstance(v, dict) and "median" in v: for kk in ("n", "median", "p95", "max"): o[f"{k}_{kk}"] = None if v[kk] is None else (round(v[kk], 5) if isinstance(v[kk], float) else v[kk]) else: o[k] = v return o def make_figs(out, offs, pairs, onsets, G): try: import matplotlib matplotlib.use("Agg") import matplotlib.pyplot as plt except Exception as e: print(f"[WARN] 図を作れない: {e}") return colors = {"demo": "#2a6fdb", "policy": "#e07b39", "pad_corr": "#3a9e5f"} fig, axes = plt.subplots(1, 3, figsize=(15, 4)) for g, c in colors.items(): v = [abs(float(o["nearest_actual_minus_capture_s"])) * 1000 for o in offs if o["group"] == g] if v: axes[0].hist(v, bins=60, range=(0, 600), alpha=0.5, color=c, label=g, density=True) p = [float(x["frame_minus_state_s"]) * 1000 for x in pairs if x["group"] == g] if p: axes[1].hist(p, bins=40, range=(-80, 80), alpha=0.5, color=c, label=g, density=True) on = [o["lag_s"] * 1000 for o in onsets if o["group"] == g] if on: axes[2].hist(on, bins=30, range=(-300, 600), alpha=0.5, color=c, label=g) axes[0].set_title("1 frame: |nearest actual - capture_ts| (ms)") axes[1].set_title("dataset pair: frame capture_ts - state t (ms)") axes[2].set_title("camera lag by motion onset (ms, + = video later)") for a in axes: a.legend(); a.grid(alpha=0.3) fig.tight_layout(); fig.savefig(out / "fig_offsets.png", dpi=110); plt.close(fig) def write_summary(out, G, demo, policy, pad, pad_all, match, args): L = [] gn = {"demo": "お手本 (split10_v2)", "policy": f"実機推論 (直近{len(policy)}本)", "pad_corr": "補正お手本 (corr_v5 のゲームパッド分)"} groups = ["demo", "policy", "pad_corr"] L.append("# 依頼249 映像と関節の時刻のずれ・制御の間隔\n") L.append(f"作成 {datetime.now().isoformat(timespec='seconds')} / `diag/measure_timing_alignment.py`" f"{' --no-video' if args.no_video else ''}\n") L.append("読んだ記録:\n") L.append(f"- お手本: {DEMO_DS}/raw_episodes ({len(demo)} 本) ← 元 {RUNS_PP}//recording") L.append(f"- 実機推論: {RUNS_POL} の {policy[0][0].name} 〜 {policy[-1][0].name} ({len(policy)} 本)" if policy else "- 実機推論: なし") L.append(f"- 補正お手本: {CORR_DS}/raw_episodes のうちゲームパッド補正の {len(pad)} 本 " f"({', '.join(p[0].name for p in pad)}) ← 元 {RUNS_POL}/. ゲームパッド補正の run は全部で {len(pad_all)} 本\n") L.append("表の値は 中央値 / 95%点 / 最大 (n=数).\n") L.append("## 1. 映像と関節の時刻のずれ (記録そのもの)\n") L.append("| 項目 | " + " | ".join(gn[g] for g in groups) + " |") L.append("|---|" + "---|" * len(groups)) L.append("| 1コマと最寄りの actual の差 (ms) | " + " | ".join(fmt(G[g]["frame_actual_abs"], 1000, 0) for g in groups) + " |") L.append("| 映像のコマ間隔 狙い | " + " | ".join(str(G[g]["frame_target"]) for g in groups) + " |") L.append("| 映像のコマ間隔 実際 (ms) | " + " | ".join(fmt(G[g]["P_frame_dt"], 1000, 0) for g in groups) + " |") L.append("| 関節を読む間隔 狙い | " + " | ".join(str(G[g]["actual_target"]) for g in groups) + " |") L.append("| 関節を読む間隔 実際 (ms) | " + " | ".join(fmt(G[g]["P_actual_dt"], 1000, 0) for g in groups) + " |") L.append("| 読み取りの失敗 (数) / 弾いた値 | " + " | ".join(f"{G[g]['read_fail']} / {G[g]['actual_invalid']}" for g in groups) + " |") L.append("| コマ数 / actual 数 | " + " | ".join(f"{G[g]['n_frames']} / {G[g]['n_actual']}" for g in groups) + " |") L.append("") L.append("## 1b. 学習データでの組のずれ (作り直した格子で)\n") L.append("組み方 (コードで確認): 10Hz の格子 t を置き, state の関節 6 列 = actual の **線形補間** (np.interp)" " (補正は actual の間が 0.25 秒を超える所を speed20 の直線で埋める), 爪 = その時刻までの最後の指令," " 画像 = capture_ts が t に **いちばん近い** コマ (補正は除いた区間のコマを候補から外す).\n") L.append("| 項目 | " + " | ".join(gn[g] for g in groups if G[g]["pairs_n"]) + " |") L.append("|---|" + "---|" * sum(1 for g in groups if G[g]["pairs_n"])) pg = [g for g in groups if G[g]["pairs_n"]] L.append("| ステップ数 | " + " | ".join(str(G[g]["pairs_n"]) for g in pg) + " |") L.append("| 画像の時刻 − state の時刻 の絶対値 (ms) | " + " | ".join(fmt(G[g]["pairs_frame"], 1000, 0) for g in pg) + " |") L.append("| state の時刻と最寄りの実測の差 (ms) | " + " | ".join(fmt(G[g]["pairs_near_actual"], 1000, 0) for g in pg) + " |") L.append("| 補間に使った 2 つの実測の間 (ms) | " + " | ".join(fmt(G[g]["pairs_bracket"], 1000, 0) for g in pg) + " |") L.append("| 0.25 秒を超える間をまたいで補間したステップ (うち 0.5° 超動いた) | " + " | ".join(f"{G[g]['pairs_over_gap']} ({G[g]['pairs_over_gap_moving']})" for g in pg) + " |") me = [m for m in match if m.get("state_match_max_err_deg") is not None] L.append(f"\n補間の誤差の見積もり (実機推論 {len(policy)} 本の密な actual で, 送るたびに {DEMO_BLOCK_SEC} 秒" f" 読めなかったことにして線形補間し, 消した実測と比べた. 6 関節の最大誤差): " f"全体 {fmt(G['sim']['err'], 1, 2)}°, 前後で 0.5° 超動いた所 {fmt(G['sim']['err_moving'], 1, 2)}°." " 実機推論の 1 回の動きはお手本の区間の動きより小さいので, お手本の誤差はこれより大きくなりうる.") L.append(f"\n格子の見つけ方の確かめ: エピソードの state と作り直した state の差の最大 = " f"{max((m['state_match_max_err_deg'] for m in me), default=None)}° ({len(me)}/{len(match)} 本で一致を確認. state_match.csv)\n") L.append("## 1c. カメラ自体の遅れの見積もり\n") L.append(f"やり方 A (動き出し): 関節の速さが {STILL_BEFORE_SEC} 秒以上 {STILL_DEG_S}°/秒未満のあと {MOVE_DEG_S}°/秒を超えた区間" f" (前後の読みの間 ≤ {MAX_JOINT_GAP_SEC} 秒) を関節の動き出し (区間の中点) とし," f" 映像のコマ間差分 (96x54 灰色・1 秒あたり) が直前の止まりの 中央値 + {VID_K_MAD}×MAD を初めて超えたコマ間 (中点) を映像の動き出しとする." " 差 = 映像 − 関節 (正 = 映像が遅れる). 1 件ごとの確からしさ = 関節の読みの間の半分 + コマ間隔の半分.\n") L.append("やり方 B (相互相関): 関節の速さと映像の動きの量を 10ms の格子にのせ, −0.3〜+0.6 秒ずらして相関がいちばん高いずれ.\n") L.append("| 項目 | " + " | ".join(gn[g] for g in groups) + " |") L.append("|---|" + "---|" * len(groups)) L.append("| A: 映像の遅れ (ms) | " + " | ".join(fmt(G[g]["onset"], 1000, 0) for g in groups) + " |") L.append("| A: 1件の確からしさ ± (ms) | " + " | ".join(fmt(G[g]["onset_unc"], 1000, 0) for g in groups) + " |") for g in groups: xs = [(a, b, c) for a, b, c in G[g]["xcorr"] if b is not None] lags = [b for _, b, _ in xs]; rs = [c for _, _, c in xs] G[g]["xc"] = (st(lags), st(rs)) L.append("| B: 相関の山のずれ (ms) run ごと | " + " | ".join(fmt(G[g]["xc"][0], 1000, 0) for g in groups) + " |") L.append("| B: 山の相関係数 | " + " | ".join(fmt(G[g]["xc"][1], 1, 2) for g in groups) + " |") L.append("") L.append("## 2. 制御の1回の間隔\n") L.append("| 項目 | " + " | ".join(gn[g] for g in groups) + " |") L.append("|---|" + "---|" * len(groups)) L.append("| 指令を送った間隔 (s) | " + " | ".join(fmt(G[g]["P_send_dt"], 1, 2) for g in groups) + " |") L.append("| 送る命令の往復 (s) | " + " | ".join(fmt(G[g]["P_send_rpc"], 1, 3) for g in groups) + " |") L.append("| 送ってから届くまで (s) | " + " | ".join(fmt(G[g]["P_arrival"], 1, 2) for g in groups) + " |") L.append("| パッドの 1 刻み 送って着くまで (s) | " + " | ".join(fmt(G[g]["P_pad_reach"], 1, 2) for g in groups) + " |") L.append("| サーボを読む時間 (ms) | " + " | ".join(fmt(G[g]["P_servo_ms"], 1, 0) for g in groups) + " |") L.append("| 推論 1 回の待ち latency_s (s) | " + " | ".join(fmt(G[g]["P_latency"], 1, 2) for g in groups) + " |") L.append("| 推論の間隔 (s) | " + " | ".join(fmt(G[g]["P_infer_dt"], 1, 2) for g in groups) + " |") L.append("\n送る・届くの意味: " + " / ".join(f"{gn[g]}: {G[g]['send_note']}" for g in groups) + "\n") L.append("### 中央値の 2 倍を超えて空いた所 (数)\n") L.append("| 組 | 種類 | つかむ前 | つかんだ後 | 放した後 | その他 |") L.append("|---|---|---|---|---|---|") for g in groups: for kind in ("send", "actual", "frame"): d = {ph: n for (k, ph), n in G[g]["gaps"].items() if k == kind} other = sum(n for ph, n in d.items() if ph not in ("つかむ前", "つかんだ後", "放した後")) L.append(f"| {gn[g]} | {kind} | {d.get('つかむ前', 0)} | {d.get('つかんだ後', 0)} | {d.get('放した後', 0)} | {other} |") L.append("\n(stage 別の内訳は gaps.csv)\n") (out / "summary.md").write_text("\n".join(L) + "\n") if __name__ == "__main__": main()