"""check_258.py が so101/check_258/ に残した記録を解析する (依頼 258). アーム・カメラにはつながない. 出力: so101/check_258/analysis.json と, 画面に表のもとになる数字. 使い方 (conda の lerobot 環境で): python so101/check_258_analyze.py """ import csv import json import subprocess import sys from pathlib import Path import cv2 import numpy as np import pandas as pd HERE = Path(__file__).resolve().parent OUT = HERE / "check_258" MOTORS = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"] FPS = 30 DT = 1 / FPS FFPROBE = "/opt/homebrew/Caskroom/miniforge/base/envs/lerobot/bin/ffprobe" def stats(x): x = np.asarray([v for v in x if np.isfinite(v)], dtype=float) if len(x) == 0: return None return {"n": int(len(x)), "median": round(float(np.median(x)), 2), "p95": round(float(np.percentile(x, 95)), 2), "max": round(float(x.max()), 2), "mean": round(float(x.mean()), 2), "std": round(float(x.std()), 2)} # ---- s1 ------------------------------------------------------------------------------ def s1(): d = OUT / "s1" res = {"saved": []} for js in sorted(d.glob("desk_*.json")): j = json.loads(js.read_text()) res["saved"].append({"image": j["image"], "focus": {k: round(v, 1) for k, v in j["focus"].items()}, "center_max_so_far": round(j.get("center_max_so_far", 0), 1)}) ref = d / "ref_248_score.txt" res["ref_248"] = ref.read_text().strip() if ref.exists() else None return res # ---- s2 ------------------------------------------------------------------------------ def joints(path): df = pd.read_csv(path) res = {"n": len(df), "errors": int((df["err"].fillna("") != "").sum()), "follower_ms": stats(df["follower_ms"]), "leader_ms": stats(df["leader_ms"]), "both_ms": stats(df["follower_ms"] + df["leader_ms"]), "total_ms": stats(df["total_ms"]), "over_33ms": int((df["total_ms"] > 1000 / FPS).sum())} for reg in ["Present_Current", "Present_Load", "Present_Temperature", "Present_Voltage"]: if reg + "_ms" in df: res[reg + "_ms"] = stats(df[reg + "_ms"]) res[reg + "_sample"] = df[reg].iloc[len(df) // 2] return res def cam(stem): df = pd.read_csv(f"{stem}.csv") props = json.loads(Path(f"{stem}_props.json").read_text()) ok = df[df["pos_msec"] != -1] t = ok["t"].to_numpy() if len(t) < 3: return {"frames": len(t), "props": props} iv = np.diff(t) * 1e3 drops = int(sum(max(0, round(x / (1000 / FPS)) - 1) for x in iv if x > 1.5 * 1000 / FPS)) pm = ok["pos_msec"].to_numpy() t_ = t[t >= t[0] + 1.0] # 開いた直後の1秒を除いたもの iv1 = np.diff(t_) * 1e3 after = {"fps": round((len(t_) - 1) / (t_[-1] - t_[0]), 2), "interval_ms": stats(iv1), "interval_over_50ms": int((iv1 > 50).sum()), "drops_est": int(sum(max(0, round(x / (1000 / FPS)) - 1) for x in iv1 if x > 1.5 * 1000 / FPS))} return {"frames": int(len(t)), "read_fail": int(len(df) - len(ok)), "fps": round((len(t) - 1) / (t[-1] - t[0]), 2), "interval_ms": stats(iv), "interval_over_50ms": int((iv > 50).sum()), "drops_est": drops, "after_first_1s": after, "pos_msec_varies": bool(np.ptp(pm) > 0), "pos_msec_interval_ms": stats(np.diff(pm)) if np.ptp(pm) > 0 else None, "resolution": [int(ok["w"].iloc[0]), int(ok["h"].iloc[0])], "blur_center": stats(ok["blur_center"]), "props": props["props"], "t": t} def cam_pair(a, b): """手首のコマごとに, いちばん近い机側のコマの届いた時刻の差 (ms). 2台はつながっていないので位相はばらばら.""" ta, tb = a.get("t"), b.get("t") if ta is None or tb is None: return None j = np.clip(np.searchsorted(tb, ta), 1, len(tb) - 1) near = np.where(np.abs(tb[j] - ta) < np.abs(tb[j - 1] - ta), tb[j], tb[j - 1]) d = (near - ta) * 1e3 keep = ta >= max(ta[0], tb[0]) + 1.0 return {"wrist_minus_desk_nearest_ms": stats(np.abs(d)), "after_first_1s": stats(np.abs(d[keep]))} def s2(): d = OUT / "s2" res = {} for k, f in [("a_joints", "a_joints.csv"), ("b_joints_extras", "b_joints_extras.csv"), ("e_joints", "e_joints.csv")]: if (d / f).exists(): res[k] = joints(d / f) for tag in ["c_cam_1280x720", "d_cam_640x480", "e_cam"]: cams = {k: cam(d / f"{tag}_{k}") for k in ("wrist", "desk") if (d / f"{tag}_{k}.csv").exists()} if len(cams) == 2: res[tag + "_pair"] = cam_pair(cams["wrist"], cams["desk"]) for k, v in cams.items(): v.pop("t", None) res[f"{tag}_{k}"] = v return res # ---- s34 ----------------------------------------------------------------------------- def load_cam34(key): stem = OUT / "s34" / f"cam_{key}" df = pd.read_csv(f"{stem}.csv") df = df[df["pos_msec"] != -1].reset_index(drop=True) small = np.load(f"{stem}_small.npy").astype(np.float32) n = min(len(df), len(small)) return df.iloc[:n], small[:n] def onset_series(t, x, t_from, t_to, base_from, base_to, k=6.0, floor=0.0): """t_from〜t_to の中で, 基準区間の中央値から k×MAD (かつ floor) を越えた最初の時刻.""" base = x[(t >= base_from) & (t < base_to)] if len(base) < 3: return None med = np.median(base) mad = np.median(np.abs(base - med)) * 1.4826 + 1e-9 thr = max(k * mad, floor) m = (t >= t_from) & (t < t_to) & (np.abs(x - med) > thr) idx = np.flatnonzero(m) return float(t[idx[0]]) if len(idx) else None def s34(): d = OUT / "s34" tele = pd.read_csv(d / "teleop.csv") phases = json.loads((d / "phases.json").read_text())["phases"] ph_t = [(float(t), n) for t, n in phases] def span(name): for i, (t, n) in enumerate(ph_t): if n == name: return t, (ph_t[i + 1][0] if i + 1 < len(ph_t) else tele["t0"].iloc[-1]) return None res = {"loops": len(tele), "loop_interval_ms": stats(np.diff(tele["t0"]) * 1e3), "loop_work_ms": stats((tele["t_extra"] - tele["t0"]) * 1e3), "leader_read_ms": stats((tele["t_act"] - tele["t0"]) * 1e3), "send_ms": stats((tele["t_sent"] - tele["t_act"]) * 1e3), "follower_pos_read_ms": stats((tele["t_pos"] - tele["t_sent"]) * 1e3), "extra_reads_ms": stats((tele["t_extra"] - tele["t_pos"]) * 1e3)} # 爪のできごと: リーダーの爪 (action) が動き始めた時刻を基準に, フォロワーの爪 (state)・手首カメラ・机側カメラ. # 画像は, 合図の区間で画素ごとの揺れが大きい上位 3% (= 爪が動く場所) だけを見て, 合図の前の平均からの差で決める cams = {k: load_cam34(k) for k in ("wrist", "desk")} tg = tele["t_pos"].to_numpy() ta = tele["t_act"].to_numpy() gp = tele["pos_gripper"].to_numpy() ga = tele["act_gripper"].to_numpy() def first(t, x, t0, t1, base, thr): i = np.flatnonzero((t >= t0) & (t < t1) & (np.abs(x - base) > thr)) return float(t[i[0]]) if len(i) else None events = [] for k in range(1, 6): sp = span(f"grip_{k}") if sp is None: continue s0, b0, w1 = sp[0], sp[0] - 1.0, sp[0] + 5.0 ev = {"event": k, "leader_gripper_max_in_window": round(float(ga[(ta >= s0) & (ta < w1)].max()), 1)} la = first(ta, ga, s0 - 0.5, w1, np.median(ga[(ta >= b0) & (ta < s0)]), 3.0) ev["leader_action"] = la ev["follower_state"] = first(tg, gp, s0 - 0.5, w1, np.median(gp[(tg >= b0) & (tg < s0)]), 3.0) for cname, (df, small) in cams.items(): if la is None: ev[f"{cname}_cam"] = None continue tc = df["t"].to_numpy() mw, mb = (tc >= s0 - 0.5) & (tc < w1), (tc >= b0) & (tc < s0) sd = small[mw].std(axis=0) mask = sd > np.percentile(sd, 97) base = small[mb].mean(axis=0) sig = np.abs(small[:, mask] - base[mask]).mean(axis=1) bs = sig[mb] thr = np.median(bs) + 6 * (np.median(np.abs(bs - np.median(bs))) * 1.4826 + 0.3) ev[f"{cname}_cam"] = first(tc, sig, la - 0.3, w1, 0.0, thr) events.append(ev) rel = {} def rel_ms(a_, b_): return [round((e[a_] - e[b_]) * 1e3) if (e.get(a_) and e.get(b_)) else None for e in events] for a_, b_ in [("follower_state", "leader_action"), ("wrist_cam", "leader_action"), ("desk_cam", "leader_action"), ("wrist_cam", "follower_state"), ("desk_cam", "follower_state"), ("wrist_cam", "desk_cam")]: rel[f"{a_}_minus_{b_}_ms_per_event"] = rel_ms(a_, b_) res["grip_events"] = [{k: (round(v, 4) if isinstance(v, float) else v) for k, v in e.items()} for e in events] res["grip_lag"] = rel # リーダー (action) に対するフォロワー (state) の遅れ: 速い区間での相互相関 (関節ごと) lag = {} for m in MOTORS: a = np.interp(np.arange(ta[0], ta[-1], 0.005), ta, tele[f"act_{m}"]) s = np.interp(np.arange(ta[0], ta[-1], 0.005), tg, tele[f"pos_{m}"]) va, vs = np.diff(a), np.diff(s) best, bl = -1, 0 for L in range(0, 101): # 0〜500ms c = np.corrcoef(va[:len(va) - L], vs[L:])[0, 1] if L else np.corrcoef(va, vs)[0, 1] if np.isfinite(c) and c > best: best, bl = c, L lag[m] = {"lag_ms": bl * 5, "corr": round(float(best), 3)} res["follower_lag_xcorr"] = lag # 速さ別 seg = {} for name in ["slow", "normal", "fast"]: sp = span(name) if sp is None: continue m_ = (tele["t0"] >= sp[0]) & (tele["t0"] < sp[1]) sub = tele[m_] t = sub["t_pos"].to_numpy() r = {"seconds": round(sp[1] - sp[0], 1), "joints": {}} for mo in MOTORS: p = sub[f"pos_{mo}"].to_numpy() v = np.abs(np.diff(p) / np.diff(t)) err = (sub[f"act_{mo}"] - sub[f"pos_{mo}"]).abs().to_numpy() # rustypot が符号つきにして返す (負の値が出る). 電流は 6.5mA/目盛, 負荷は 0.1%/目盛, 速さは 目盛/秒 cur = sub[f"cur_{mo}"].to_numpy() load = sub[f"load_{mo}"].to_numpy() sv = sub[f"vel_{mo}"].to_numpy() temps = sub[f"temp_{mo}"].dropna().to_numpy() if f"temp_{mo}" in sub else np.array([]) unit = "%" if mo == "gripper" else "deg" # 離れた区間: |差| > 10 (deg か %) が 0.1 秒以上続いた区間 far = err > 10 runs, start = [], None for i, f in enumerate(far): if f and start is None: start = i if (not f or i == len(far) - 1) and start is not None: end = i if not f else i + 1 if t[min(end, len(t) - 1)] - t[start] >= 0.1: runs.append(round(float(t[min(end, len(t) - 1)] - t[start]), 2)) start = None r["joints"][mo] = { f"max_speed_{unit}_s": round(float(np.percentile(v, 99)), 1) if len(v) else None, f"peak_speed_{unit}_s": round(float(v.max()), 1) if len(v) else None, "servo_velocity_raw_max": int(np.abs(sv).max()), "servo_velocity_deg_s_max": round(float(np.abs(sv).max() * 360 / 4096), 1), f"max_leader_diff_{unit}": round(float(err.max()), 1), f"p95_leader_diff_{unit}": round(float(np.percentile(err, 95)), 1), "far_runs_s": runs, "current_raw_max": int(np.abs(cur).max()), "current_mA_max": round(float(np.abs(cur).max() * 6.5)), "load_raw_max": int(np.abs(load).max()), "load_pct_max": round(float(np.abs(load).max() / 10), 1), "temp_start_end": [float(temps[0]), float(temps[-1])] if len(temps) else None, } for cname, (df, _s) in cams.items(): mm = (df["t"] >= sp[0]) & (df["t"] < sp[1]) r[f"{cname}_blur"] = stats(df.loc[mm, "blur_center"]) seg[name] = r res["speed_segments"] = seg # ぶれと関節の速さの関係 (s34 の全コマ): 関節1〜5 の速さの最大で区切って, 手首・机側のピントの中央値 tt = tele["t_pos"].to_numpy() body_v = np.zeros(len(tt)) for mo in MOTORS[:5]: p = tele[f"pos_{mo}"].to_numpy() v = np.r_[0, np.abs(np.diff(p) / np.maximum(np.diff(tt), 1e-3))] body_v = np.maximum(body_v, v) bins = [0, 10, 30, 60, 100, 150, 200, 300, 1e9] blur_by_speed = {} for cname, (df, _s) in cams.items(): vv = np.interp(df["t"], tt, body_v) rows = [] for lo, hi in zip(bins[:-1], bins[1:]): m_ = (vv >= lo) & (vv < hi) if m_.sum() >= 5: rows.append({"speed_deg_s": f"{lo}-{hi if hi < 1e9 else ''}", "frames": int(m_.sum()), "blur_median": round(float(np.median(df.loc[m_, "blur_center"])), 1)}) blur_by_speed[cname] = rows res["blur_by_body_speed"] = blur_by_speed # 爪の値と手首カメラの爪の開き: 爪区間の手首のコマで, 開いた状態 (grip_pre) からの差の大きさと爪の値の相関 sp0, sp_end = span("grip_pre"), span("grip_5_rest") if sp0 and sp_end: df, small = cams["wrist"] tc = df["t"].to_numpy() m_ = (tc >= sp0[0]) & (tc < sp_end[1]) ref = small[(tc >= sp0[0]) & (tc < sp0[1])].mean(axis=0) dev = np.abs(small[m_] - ref).mean(axis=(1, 2)) g = np.interp(tc[m_], tg, gp) res["gripper_vs_wrist_image"] = {"frames": int(m_.sum()), "pearson_r_gripper_vs_image_change": round(float(np.corrcoef(g, dev)[0, 1]), 3), "gripper_range_in_events": [round(float(g.min()), 1), round(float(g.max()), 1)]} save_gripper_montage(tc, tg, gp, sp0[0], sp_end[1]) return res def save_gripper_montage(tc, tg, gp, t0, t1): """手首の動画から, 爪の値がちがう6コマを並べた画像を作る (目で合っているか見る用).""" cap = cv2.VideoCapture(str(OUT / "s34" / "wrist.mp4")) frames = [] i = 0 m_idx = set(np.flatnonzero((tc >= t0) & (tc < t1)).tolist()) while True: ok, f = cap.read() if not ok: break if i in m_idx: frames.append((i, f)) i += 1 cap.release() if not frames: return vals = [(float(np.interp(tc[i], tg, gp)), i, f) for i, f in frames] vals.sort(key=lambda x: x[0]) pick = [vals[int(k * (len(vals) - 1) / 5)] for k in range(6)] tiles = [] for g, i, f in pick: im = cv2.resize(f, (320, 240)) cv2.putText(im, f"gripper {g:.1f}", (8, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 0), 4) cv2.putText(im, f"gripper {g:.1f}", (8, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2) tiles.append(im) cv2.imwrite(str(OUT / "gripper_vs_wrist.png"), np.vstack([np.hstack(tiles[:3]), np.hstack(tiles[3:])])) # ---- s5 ------------------------------------------------------------------------------ def s5(): root = OUT / "dataset" if not root.exists(): return None info = json.loads((root / "meta" / "info.json").read_text()) res = {"info": {k: v for k, v in info.items() if k != "features"}, "features": info["features"]} files = sorted((root / "data").rglob("*.parquet")) df = pd.concat([pd.read_parquet(f) for f in files]) res["parquet_files"] = [str(f.relative_to(root)) for f in files] res["parquet_columns"] = {c: str(df[c].dtype) for c in df.columns} res["parquet_head"] = df.head(3).astype(str).to_dict("records") eps = {} for ep, g in df.groupby("episode_index"): ts = g["timestamp"].to_numpy() eps[int(ep)] = {"frames": int(len(g)), "timestamp_first_last": [float(ts[0]), float(ts[-1])], "timestamp_step": stats(np.diff(ts) * 1e3), "timestamp_equals_index_over_fps": bool(np.allclose(ts, g["frame_index"].to_numpy() / FPS, atol=1e-4))} res["episodes"] = eps vids = sorted((root / "videos").rglob("*.mp4")) res["videos"] = [] for v in vids: pr = subprocess.run([FFPROBE, "-v", "error", "-select_streams", "v:0", "-count_frames", "-show_entries", "stream=codec_name,pix_fmt,width,height,r_frame_rate,avg_frame_rate,nb_read_frames,duration", "-of", "json", str(v)], capture_output=True, text=True) st = json.loads(pr.stdout).get("streams", [{}])[0] if pr.returncode == 0 else {"err": pr.stderr[-300:]} st["file"] = str(v.relative_to(root)) st["MB"] = round(v.stat().st_size / 1e6, 3) res["videos"].append(st) total = sum(p.stat().st_size for p in root.rglob("*") if p.is_file()) secs = sum(e["frames"] for e in eps.values()) / FPS res["size"] = {"total_MB": round(total / 1e6, 2), "seconds": round(secs, 1), "MB_per_s": round(total / 1e6 / secs, 3) if secs else None, "video_MB": round(sum(v.stat().st_size for v in vids) / 1e6, 2), "parquet_MB": round(sum(f.stat().st_size for f in files) / 1e6, 3)} mbps = res["size"]["MB_per_s"] if mbps: res["size"]["estimate_MB"] = {f"{sec}s": {"1": round(mbps * sec, 1), "100": round(mbps * sec * 100), "1000": round(mbps * sec * 1000)} for sec in (10, 20, 30, 60)} return res # ---- s6 ------------------------------------------------------------------------------ def s6(episode=0): root = OUT / "dataset" logs = [OUT / f"s6_replay{k}.csv" for k in (1, 2)] if not all(p.exists() for p in logs) or not root.exists(): return None files = sorted((root / "data").rglob("*.parquet")) df = pd.concat([pd.read_parquet(f) for f in files]) g = df[df["episode_index"] == episode].sort_values("frame_index") rec_state = np.stack(g["observation.state"].to_numpy()) rec_action = np.stack(g["action"].to_numpy()) reps = [] for p in logs: r = pd.read_csv(p) o = r[r["kind"] == "obs"].sort_values("frame") reps.append(o[MOTORS].to_numpy()) n = min(len(rec_state), *(len(x) for x in reps)) # replay の obs[i] は i 番目の指令を送る前に読んだ位置. 記録の state[i] も action[i] を送る前に読んだ位置なので同じ並び res = {"frames": int(n), "joints": {}} for j, m in enumerate(MOTORS): d_rec1 = np.abs(reps[0][:n, j] - rec_state[:n, j]) d_rec2 = np.abs(reps[1][:n, j] - rec_state[:n, j]) d_12 = np.abs(reps[0][:n, j] - reps[1][:n, j]) res["joints"][m] = {"replay1_vs_record_mean": round(float(d_rec1.mean()), 2), "replay1_vs_record_max": round(float(d_rec1.max()), 2), "replay2_vs_record_mean": round(float(d_rec2.mean()), 2), "replay2_vs_record_max": round(float(d_rec2.max()), 2), "replay1_vs_replay2_mean": round(float(d_12.mean()), 2), "replay1_vs_replay2_max": round(float(d_12.max()), 2), "final_rec_r1_r2": [round(float(rec_state[n - 1, j]), 2), round(float(reps[0][n - 1, j]), 2), round(float(reps[1][n - 1, j]), 2)], "record_action_vs_state_max": round(float(np.abs(rec_action[:n, j] - rec_state[:n, j]).max()), 2)} return res def main(): res = {} for name, fn in [("s1", s1), ("s2", s2), ("s34", s34), ("s5", s5), ("s6", s6)]: try: res[name] = fn() except FileNotFoundError as e: res[name] = {"missing": str(e)} except Exception as e: res[name] = {"error": repr(e)} cal = OUT / "calibration_check.json" if cal.exists(): res["calibration"] = json.loads(cal.read_text()) (OUT / "analysis.json").write_text(json.dumps(res, ensure_ascii=False, indent=1, default=str)) print(json.dumps(res, ensure_ascii=False, indent=1, default=str)[:20000]) if __name__ == "__main__": sys.exit(main())