ykcheungab
341 lines
13 KiB
Python
341 lines
13 KiB
Python
"""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 <dir> --unit both
|
|
python scripts/measure_zero_drift.py --recording <dir> --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 sys
|
|
from collections.abc import Sequence
|
|
from pathlib import Path
|
|
|
|
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 &= ~np.isnan(times) # drop NaN timestamps (unparsable TIME cells)
|
|
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())
|