diff --git a/.gitignore b/.gitignore index d440858..186b14e 100644 --- a/.gitignore +++ b/.gitignore @@ -24,6 +24,7 @@ __pycache__/ # Local tools / editor state /.cursor/ /.vscode/ +/_perf/ # Local data / models /models/ diff --git a/scripts/measure_zero_drift.py b/scripts/measure_zero_drift.py new file mode 100644 index 0000000..d5b2978 --- /dev/null +++ b/scripts/measure_zero_drift.py @@ -0,0 +1,341 @@ +"""Measure normal-force zero-point drift, live or from a recording. + +Phase 0 of the zero-point plan: quantify the drift before changing the pipeline. +The number this prints is what the acceptance thresholds get set from, so it is +deliberately explicit about which rows it used and what it could not conclude. + +Usage +----- +Live capture (needs a sensor; writes a recording directory when --out is given): + + python scripts/measure_zero_drift.py --live --seconds 1800 --out D:\\tmp\\idle + +From an existing recording (the directory holding forces.csv): + + python scripts/measure_zero_drift.py --recording --unit both + python scripts/measure_zero_drift.py --recording --warmup 5 --start 10 --stop 60 + +What it reports +--------------- +Per axis, over the analysed window: + +* mean / median / stdev / peak-to-peak, in raw counts and (with --unit N) newtons +* a least-squares slope in counts/s -- the ramp that "zero drift" actually means +* a drift ratio (end-third mean minus start-third mean), which is insensitive to + whether the drift is linear or a slow wander +* the band left after removing the window mean, i.e. what a perfect single-point + re-zero would achieve -- this separates "the zero moved" from "the noise is wide" + +The raw columns are ``FN_raw, FX_raw, FY_raw`` and the calibrated ones are +``FN_N, FX_N, FY_N`` (see ``FORCES_HEADER`` in +``orisys_app_shared.record.experiment``); rows with ``valid != 1`` are excluded +rather than read as zeros. +""" + +from __future__ import annotations + +import argparse +import csv +import statistics +import sys +from pathlib import Path +from typing import Sequence + +import numpy as np + +RAW_AXES = ("FN_raw", "FX_raw", "FY_raw") +NEWTON_AXES = ("FN_N", "FX_N", "FY_N") +TIME_COLUMN = "t_rel_s" +VALID_COLUMN = "valid" + + +def _load_rows(source: Path) -> list[dict[str, str]]: + path = source / "forces.csv" if source.is_dir() else source + if not path.is_file(): + raise SystemExit(f"no forces.csv at {path}") + with path.open(encoding="utf-8", newline="") as handle: + return list(csv.DictReader(handle)) + + +def _column(rows: Sequence[dict[str, str]], name: str) -> np.ndarray: + values = [] + for row in rows: + raw = row.get(name) + try: + values.append(float(raw)) # type: ignore[arg-type] + except (TypeError, ValueError): + values.append(np.nan) + return np.asarray(values, dtype=np.float64) + + +def _window( + rows: list[dict[str, str]], + *, + start: float | None, + stop: float | None, + warmup: float, +) -> tuple[np.ndarray, list[dict[str, str]]]: + times = _column(rows, TIME_COLUMN) + valid = _column(rows, VALID_COLUMN) + mask = np.ones(times.shape, dtype=bool) + if times.size: + mask &= times == times # drop NaN timestamps + if start is not None: + mask &= times >= float(start) + if stop is not None: + mask &= times <= float(stop) + # ``valid`` is authoritative: an invalid row is not a zero reading. + mask &= (valid == 1) | np.isnan(valid) + kept = [row for row, keep in zip(rows, mask) if keep] + if not kept: + return np.asarray([], dtype=np.float64), [] + times = np.asarray([float(row[TIME_COLUMN]) for row in kept], dtype=np.float64) + if warmup > 0 and times.size: + t0 = times[0] + kept = [row for row in kept if float(row[TIME_COLUMN]) - t0 >= warmup] + times = np.asarray([float(row[TIME_COLUMN]) for row in kept], dtype=np.float64) + return times, kept + + +def _describe(label: str, values: np.ndarray, times: np.ndarray) -> dict[str, float]: + finite = values[np.isfinite(values)] + if finite.size == 0: + return {"axis": label, "n": 0} + mean = float(finite.mean()) + median = float(np.median(finite)) + # A min-to-max span does not shrink when a constant is removed, so it cannot + # answer "what would a re-zero achieve". The robust band and the inter- + # percentile width can, and they are what the noise is measured against in + # the existing perf analysis. + centred = finite - median + band_1sigma = float( + (np.percentile(centred, 84.1) - np.percentile(centred, 15.9)) / 2.0 + ) + width_99 = float(np.percentile(finite, 99.5) - np.percentile(finite, 0.5)) + slope = float("nan") + if times.size == finite.size and times.size > 2 and float(times[-1] - times[0]) > 0: + slope = float(np.polyfit(times - times[0], finite, 1)[0]) + third = max(1, finite.size // 3) + drift_ratio = float(finite[-third:].mean() - finite[:third].mean()) + return { + "axis": label, + "n": int(finite.size), + "mean": mean, + "median": median, + "stdev": float(finite.std(ddof=0)), + "p2p": float(finite.max() - finite.min()), + "band_1sigma": band_1sigma, + "width_99": width_99, + "slope_per_s": slope, + "drift_ratio": drift_ratio, + } + + +def _print_table(title: str, stats: list[dict[str, float]], unit: str) -> None: + usable = [s for s in stats if s.get("n")] + print(f"\n=== {title} ===") + if not usable: + print(" no rows in the analysed window") + return + print( + f" {'axis':<8} {'n':>7} {'mean':>13} {'median':>13} {'stdev':>12} " + f"{'band1s':>11} {'w99':>11} {'slope/s':>11} {'drift':>11} [{unit}]" + ) + for s in usable: + print( + f" {s['axis']:<8} {s['n']:>7} {s['mean']:>13.3f} {s['median']:>13.3f} " + f"{s['stdev']:>12.3f} {s['band_1sigma']:>11.3f} {s['width_99']:>11.3f} " + f"{s['slope_per_s']:>11.4f} {s['drift_ratio']:>11.3f}" + ) + print( + " slope/s = least-squares ramp (the drift rate)\n" + " band1s = robust 1-sigma half-width about the median\n" + " w99 = 0.5-99.5 percentile width (tail-resistant spread)\n" + " drift = mean(last third) - mean(first third): moved, not just noisy" + ) + + +def _analyse(rows: list[dict[str, str]], args: argparse.Namespace) -> int: + times, kept = _window( + rows, start=args.start, stop=args.stop, warmup=args.warmup + ) + print(f"recording rows : {len(rows)}") + print(f"analysed rows : {len(kept)}") + if times.size: + dt = np.diff(times) + print( + f"analysed window : {times[0]:.3f} .. {times[-1]:.3f} s " + f"({times[-1] - times[0]:.3f} s)" + ) + if dt.size and dt[dt > 0].size: + rate = 1.0 / float(np.median(dt[dt > 0])) + print(f"effective sample rate : {rate:.2f} Hz") + if len(kept) < 30: + print( + "\nNOTE: fewer than 30 analysed rows. This cannot characterise a slow\n" + " zero drift -- capture an idle run of at least several minutes." + ) + + if args.unit in ("raw", "both"): + _print_table( + "raw counts (pre-calibration)", + [_describe(axis, _column(kept, axis), times) for axis in RAW_AXES], + "counts", + ) + if args.unit in ("N", "both"): + _print_table( + "newtons (what the operator sees)", + [_describe(axis, _column(kept, axis), times) for axis in NEWTON_AXES], + "N", + ) + + if args.out: + out = Path(args.out) + out.parent.mkdir(parents=True, exist_ok=True) + with out.open("w", encoding="utf-8", newline="") as handle: + writer = csv.writer(handle) + writer.writerow( + ["axis", "unit", "n", "mean", "median", "stdev", "band_1sigma", + "width_99", "slope_per_s", "drift_ratio"] + ) + groups = [] + if args.unit in ("raw", "both"): + groups.append((RAW_AXES, "counts")) + if args.unit in ("N", "both"): + groups.append((NEWTON_AXES, "N")) + for axes, unit in groups: + for axis in axes: + s = _describe(axis, _column(kept, axis), times) + if not s.get("n"): + continue + writer.writerow([ + s["axis"], unit, s["n"], f"{s['mean']:.6f}", + f"{s['median']:.6f}", f"{s['stdev']:.6f}", + f"{s['band_1sigma']:.6f}", f"{s['width_99']:.6f}", + f"{s['slope_per_s']:.6f}", f"{s['drift_ratio']:.6f}", + ]) + print(f"\nwrote {out}") + return 0 + + +def _capture_live(args: argparse.Namespace) -> int: + """Sample a real sensor with no load applied. + + Uses only the public ``read_info``/``disconnect`` surface, so this keeps + working if the internals move. + """ + import time as _time + + from orisys import Sensor + + sensor = Sensor( + "external", + config_name=args.config, + verbose=args.verbose, + backend=args.backend, + ) + seconds = float(args.seconds) + period = 1.0 / float(args.sample_rate) + rows: list[tuple[float, float, float, float, float, float, float]] = [] + try: + print( + f"capturing {seconds:.0f}s at ~{args.sample_rate:.0f} Hz from config " + f"{args.config!r} ({args.backend}); keep the sensor unloaded" + ) + t0 = _time.monotonic() + while True: + elapsed = _time.monotonic() - t0 + if elapsed >= seconds: + break + raw = sensor.read_info( + sensor.info.FNORMAL_RAW, + sensor.info.FSHEARX_RAW, + sensor.info.FSHEARY_RAW, + ) + newtons = sensor.read_info( + sensor.info.FNORMAL, + sensor.info.FSHEARX, + sensor.info.FSHEARY, + ) + rows.append((elapsed, *[float(v) for v in raw], *[float(v) for v in newtons])) + _time.sleep(max(0.0, period - ((_time.monotonic() - t0) - elapsed))) + finally: + sensor.disconnect() + + if not rows: + print("no samples captured (external-frame mode may need a frame source)") + return 1 + + times = np.asarray([row[0] for row in rows], dtype=np.float64) + print(f"\ncaptured {len(rows)} samples over {times[-1]:.1f}s") + + if args.out: + out = Path(args.out) + out.mkdir(parents=True, exist_ok=True) + path = out / "forces.csv" + with path.open("w", encoding="utf-8", newline="") as handle: + writer = csv.writer(handle) + writer.writerow( + ["session_id", "frame_id", TIME_COLUMN, *RAW_AXES, *NEWTON_AXES, + VALID_COLUMN, "calibration_id"] + ) + for index, row in enumerate(rows): + t_rel, *rest = row + writer.writerow( + [0, index, f"{t_rel:.6f}", *[f"{v:.6f}" for v in rest], 1, ""] + ) + print(f"wrote {path}") + return 0 + + _print_table( + "live raw counts", + [ + _describe(axis, np.asarray([row[1 + i] for row in rows]), times) + for i, axis in enumerate(RAW_AXES) + ], + "counts", + ) + _print_table( + "live newtons", + [ + _describe(axis, np.asarray([row[4 + i] for row in rows]), times) + for i, axis in enumerate(NEWTON_AXES) + ], + "N", + ) + return 0 + + +def main(argv: Sequence[str] | None = None) -> int: + parser = argparse.ArgumentParser( + description="Measure normal-force zero-point drift (Phase 0)." + ) + source = parser.add_mutually_exclusive_group(required=True) + source.add_argument("--recording", type=Path, + help="recording directory or forces.csv path") + source.add_argument("--live", action="store_true", + help="capture from a Sensor instead (external-frame mode)") + parser.add_argument("--seconds", type=float, default=1800.0, + help="live capture length (default 1800)") + parser.add_argument("--sample-rate", type=float, default=30.0, + help="live polling rate Hz (default 30)") + parser.add_argument("--config", default="box", help="sensor config for --live") + parser.add_argument("--backend", default="cpu", help="poisson backend for --live") + parser.add_argument("--unit", choices=("raw", "N", "both"), default="both") + parser.add_argument("--start", type=float, default=None, help="window start (s)") + parser.add_argument("--stop", type=float, default=None, help="window stop (s)") + parser.add_argument("--warmup", type=float, default=0.0, + help="discard this many seconds from the window start") + parser.add_argument("--out", default=None, + help="write the per-axis table to this CSV path, or the " + "capture directory when used with --live") + parser.add_argument("--verbose", action="store_true") + args = parser.parse_args(argv) + + if args.live: + return _capture_live(args) + return _analyse(_load_rows(args.recording), args) + + +if __name__ == "__main__": + sys.exit(main())