新增统一力读数会话 ForceSession
把同帧测量、空载归零、模型推断与共用后处理收进一个 SDK 会话，Studio 显示与记录使用同一样本；旧 Sensor 读数接口保持兼容。以 force session 与受影响 SDK 套件、lint 与文档门禁验证，硬件与真实载荷精度未验证。 Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01CpCfJF6SvonMKmPZhzKZHb
This commit is contained in:
ykcheungab
@@ -2,11 +2,22 @@
|
||||
|
||||
Any sensor that produces a raw (fn, fx, fy) triple can use this. Calibration
|
||||
coefficients stay outside: pass ``convert`` into :meth:`ForceReadout.apply`.
|
||||
|
||||
Two things live here, deliberately apart:
|
||||
|
||||
* :class:`ForceReadout` — the historical measurement-chain readout. It filters,
|
||||
then converts (median → low-pass → convert), which is not interchangeable with
|
||||
a nonlinear conversion; existing consumers keep it unchanged.
|
||||
* :class:`ForceOutputFilter` — the shared *output* post-processing for the new
|
||||
session path: causal median → low-pass → optional per-axis threshold, applied
|
||||
to the raw features and to the unfiltered newtons as two independent states.
|
||||
Conversion happens upstream of it, so it can run on both domains.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from collections.abc import Callable
|
||||
from dataclasses import dataclass, replace
|
||||
|
||||
import numpy as np
|
||||
|
||||
@@ -14,6 +25,7 @@ from .signal_processing import (
|
||||
DEFAULT_SAMPLE_RATE_HZ,
|
||||
CausalMedianFilter,
|
||||
LowpassFilter,
|
||||
MultiAxisCausalFilter,
|
||||
SampleRateEstimator,
|
||||
)
|
||||
|
||||
@@ -392,3 +404,300 @@ class ForceReadout:
|
||||
"any_stage_applied": bool(self.any_stage_applied),
|
||||
"unfiltered_available": True,
|
||||
}
|
||||
|
||||
|
||||
FORCE_AXES: tuple[str, str, str] = ("fn", "fx", "fy")
|
||||
|
||||
|
||||
def _positive_float(value, default: float) -> float:
|
||||
try:
|
||||
number = float(value)
|
||||
except (TypeError, ValueError):
|
||||
return float(default)
|
||||
if not np.isfinite(number) or number <= 0:
|
||||
return float(default)
|
||||
return number
|
||||
|
||||
|
||||
def _non_negative_float(value, default: float = 0.0) -> float:
|
||||
try:
|
||||
number = float(value)
|
||||
except (TypeError, ValueError):
|
||||
return float(default)
|
||||
if not np.isfinite(number) or number < 0:
|
||||
return float(default)
|
||||
return number
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class OutputSettings:
|
||||
"""One resolved setting group for BOTH display and recording.
|
||||
|
||||
Every stage is switched by its own value: ``median_window=1`` and
|
||||
``lowpass_hz=0`` each mean "off", and a threshold of 0 disables that unit's
|
||||
threshold. All stages off is an exact pass-through (a NaN axis stays NaN).
|
||||
This does not switch off the upstream flow EMA; that belongs to the
|
||||
measurement context and is part of the calibration identity.
|
||||
|
||||
``sample_rate_hz`` describes the *signal* time of the input frames, declared
|
||||
by the caller (camera config or recorded signal clock). The offline
|
||||
processing speed never designs the low-pass. A cutoff at or above Nyquist is
|
||||
unusable and resolves to "off" rather than to a silent re-tune.
|
||||
"""
|
||||
|
||||
sample_rate_hz: float
|
||||
median_window: int = 1
|
||||
lowpass_hz: float = 0.0
|
||||
lowpass_order: int = 2
|
||||
raw_threshold: float = 0.0
|
||||
newton_threshold: float = 0.0
|
||||
|
||||
def normalised(self) -> OutputSettings:
|
||||
"""Clamp to values the chain can actually run; unusable values are "off"."""
|
||||
rate = _positive_float(self.sample_rate_hz, DEFAULT_SAMPLE_RATE_HZ)
|
||||
try:
|
||||
window = max(1, int(self.median_window))
|
||||
except (TypeError, ValueError):
|
||||
window = 1
|
||||
cutoff = _non_negative_float(self.lowpass_hz)
|
||||
if cutoff >= 0.5 * rate:
|
||||
cutoff = 0.0
|
||||
try:
|
||||
order = max(1, int(self.lowpass_order))
|
||||
except (TypeError, ValueError):
|
||||
order = 2
|
||||
return replace(
|
||||
self,
|
||||
sample_rate_hz=rate,
|
||||
median_window=window,
|
||||
lowpass_hz=cutoff,
|
||||
lowpass_order=order,
|
||||
raw_threshold=_non_negative_float(self.raw_threshold),
|
||||
newton_threshold=_non_negative_float(self.newton_threshold),
|
||||
)
|
||||
|
||||
@property
|
||||
def median_on(self) -> bool:
|
||||
return int(self.median_window) > 1
|
||||
|
||||
@property
|
||||
def lowpass_on(self) -> bool:
|
||||
return float(self.lowpass_hz) > 0
|
||||
|
||||
def state(self) -> dict:
|
||||
"""Effective parameters, for a record or an audit."""
|
||||
return {
|
||||
"sample_rate_hz": float(self.sample_rate_hz),
|
||||
"median_window": int(self.median_window),
|
||||
"median_applied": bool(self.median_on),
|
||||
"lowpass_hz": float(self.lowpass_hz),
|
||||
"lowpass_applied": bool(self.lowpass_on),
|
||||
"lowpass_order": int(self.lowpass_order),
|
||||
"raw_threshold": float(self.raw_threshold),
|
||||
"newton_threshold": float(self.newton_threshold),
|
||||
}
|
||||
|
||||
|
||||
class ForceOutputFilter:
|
||||
"""Median → low-pass → optional thresholds, once, for raw and N separately.
|
||||
|
||||
One state per domain: two :class:`MultiAxisCausalFilter` instances with the
|
||||
same settings. The session hands each frame's features here exactly once, so
|
||||
display and recording read the same processed sample; there is no second
|
||||
filter state for a consumer to drift from.
|
||||
|
||||
Invalid axes (NaN or an undeclared valid flag) never enter the filter
|
||||
history and stay NaN, which keeps "missing" distinct from a valid zero. With
|
||||
every stage off the input tuples are returned unchanged, NaN included.
|
||||
"""
|
||||
|
||||
def __init__(self, settings: OutputSettings):
|
||||
self._settings = settings.normalised()
|
||||
self._chain_settings = self._settings
|
||||
self._raw_chain = self._build_chain()
|
||||
self._newton_chain = self._build_chain()
|
||||
self._rate = SampleRateEstimator(initial_hz=self._settings.sample_rate_hz)
|
||||
|
||||
# ------------------------------------------------------------- settings
|
||||
def _build_chain(self) -> MultiAxisCausalFilter:
|
||||
settings = self._settings
|
||||
return MultiAxisCausalFilter(
|
||||
median_window=settings.median_window,
|
||||
lowpass_cutoff_hz=settings.lowpass_hz,
|
||||
sample_rate_hz=settings.sample_rate_hz,
|
||||
lowpass_order=settings.lowpass_order,
|
||||
)
|
||||
|
||||
@property
|
||||
def settings(self) -> OutputSettings:
|
||||
"""The effective settings currently applied (rate included)."""
|
||||
return self._settings
|
||||
|
||||
@property
|
||||
def active(self) -> bool:
|
||||
"""Any stage would change a valid reading."""
|
||||
return bool(
|
||||
self._raw_chain.enabled
|
||||
or self._settings.raw_threshold > 0
|
||||
or self._settings.newton_threshold > 0
|
||||
)
|
||||
|
||||
def configure(self, settings: OutputSettings) -> bool:
|
||||
"""Replace the setting group. State is dropped; the caller resets the stream."""
|
||||
resolved = settings.normalised()
|
||||
if resolved == self._settings:
|
||||
return False
|
||||
self._settings = resolved
|
||||
self._raw_chain = self._build_chain()
|
||||
self._newton_chain = self._build_chain()
|
||||
self._rate.reset(resolved.sample_rate_hz)
|
||||
return True
|
||||
|
||||
def reset(self) -> None:
|
||||
"""Drop all filter memory (new zero, model change, discontinuity)."""
|
||||
self._raw_chain.reset()
|
||||
self._newton_chain.reset()
|
||||
self._rate.reset(self._settings.sample_rate_hz)
|
||||
|
||||
# ---------------------------------------------------------- processing
|
||||
def _track_signal_rate(self, signal_time_s: float | None) -> None:
|
||||
"""Design the low-pass from signal time, never from processing speed."""
|
||||
if signal_time_s is None:
|
||||
return
|
||||
if not np.isfinite(float(signal_time_s)):
|
||||
return
|
||||
rate = self._rate.tick(float(signal_time_s))
|
||||
if not self._rate.measured:
|
||||
return
|
||||
current = self._settings.sample_rate_hz
|
||||
if current > 0 and abs(rate - current) <= 0.1 * current:
|
||||
return
|
||||
self._settings = replace(self._settings, sample_rate_hz=rate)
|
||||
self._raw_chain.configure(sample_rate_hz=rate)
|
||||
self._newton_chain.configure(sample_rate_hz=rate)
|
||||
|
||||
def _apply_chain(
|
||||
self,
|
||||
chain: MultiAxisCausalFilter,
|
||||
values,
|
||||
valid,
|
||||
*,
|
||||
threshold: float,
|
||||
) -> tuple[float, float, float]:
|
||||
raw = tuple(float(value) for value in values)
|
||||
if valid is None:
|
||||
usable = np.isfinite(np.asarray(raw, dtype=np.float64))
|
||||
else:
|
||||
usable = np.asarray(valid, dtype=bool).reshape(-1)
|
||||
if usable.size != 3:
|
||||
raise ValueError(f"expected 3 validity flags, got {usable.size}")
|
||||
processed = chain.process(raw, valid=usable)
|
||||
out: list[float] = []
|
||||
for index in range(3):
|
||||
value = float(processed[index])
|
||||
if not usable[index] or not np.isfinite(value):
|
||||
out.append(float("nan"))
|
||||
continue
|
||||
if threshold > 0 and abs(value) <= threshold:
|
||||
out.append(0.0)
|
||||
else:
|
||||
out.append(value)
|
||||
return (out[0], out[1], out[2])
|
||||
|
||||
def process(
|
||||
self,
|
||||
raw,
|
||||
newton,
|
||||
*,
|
||||
raw_valid=None,
|
||||
newton_valid=None,
|
||||
signal_time_s: float | None = None,
|
||||
) -> tuple[tuple[float, float, float], tuple[float, float, float]]:
|
||||
"""Process one frame's raw and newton triplets; returns both outputs."""
|
||||
self._track_signal_rate(signal_time_s)
|
||||
if not self.active:
|
||||
# Exact pass-through, NaN and signed zero included.
|
||||
return (
|
||||
tuple(float(value) for value in raw),
|
||||
tuple(float(value) for value in newton),
|
||||
)
|
||||
raw_out = self._apply_chain(
|
||||
self._raw_chain,
|
||||
raw,
|
||||
raw_valid,
|
||||
threshold=self._settings.raw_threshold,
|
||||
)
|
||||
newton_out = self._apply_chain(
|
||||
self._newton_chain,
|
||||
newton,
|
||||
newton_valid,
|
||||
threshold=self._settings.newton_threshold,
|
||||
)
|
||||
return raw_out, newton_out
|
||||
|
||||
def state(self) -> dict:
|
||||
return {
|
||||
"settings": self._settings.state(),
|
||||
"raw_chain": self._raw_chain.state(),
|
||||
"newton_chain": self._newton_chain.state(),
|
||||
"sample_rate_source": (
|
||||
"signal_time" if self._rate.measured else "declared_signal_rate"
|
||||
),
|
||||
}
|
||||
|
||||
|
||||
def process_output_sequence(
|
||||
settings: OutputSettings,
|
||||
raw_samples,
|
||||
newton_samples,
|
||||
*,
|
||||
raw_valid=None,
|
||||
newton_valid=None,
|
||||
signal_times_s=None,
|
||||
) -> tuple[list[tuple[float, float, float]], list[tuple[float, float, float]], OutputSettings]:
|
||||
"""Re-process stored unfiltered raw/N sequences through the SAME output chain.
|
||||
|
||||
This is the offline entry a replay/analysis pass uses to re-filter saved
|
||||
unfiltered samples without a second median/low-pass implementation: it runs
|
||||
the live chain in order from a clean state, including warm-up, so the result
|
||||
equals what the session would have produced for that exact sample sequence.
|
||||
|
||||
``signal_times_s`` should carry the recorded signal times (the low-pass is
|
||||
designed from signal time, never from how fast the re-analysis runs). When it
|
||||
is omitted the declared ``settings.sample_rate_hz`` stays in effect. Returns
|
||||
one output triplet per input sample, plus the effective settings.
|
||||
"""
|
||||
raw_rows = list(raw_samples)
|
||||
newton_rows = list(newton_samples)
|
||||
if len(raw_rows) != len(newton_rows):
|
||||
raise ValueError(
|
||||
"raw_samples and newton_samples must have the same length: "
|
||||
f"{len(raw_rows)} != {len(newton_rows)}"
|
||||
)
|
||||
count = len(raw_rows)
|
||||
raw_flags = None if raw_valid is None else list(raw_valid)
|
||||
newton_flags = None if newton_valid is None else list(newton_valid)
|
||||
times = None if signal_times_s is None else list(signal_times_s)
|
||||
for name, rows in (
|
||||
("raw_valid", raw_flags),
|
||||
("newton_valid", newton_flags),
|
||||
("signal_times_s", times),
|
||||
):
|
||||
if rows is not None and len(rows) != count:
|
||||
raise ValueError(
|
||||
f"{name} must have one entry per sample: {len(rows)} != {count}"
|
||||
)
|
||||
chain = ForceOutputFilter(settings)
|
||||
raw_out: list[tuple[float, float, float]] = []
|
||||
newton_out: list[tuple[float, float, float]] = []
|
||||
for index in range(count):
|
||||
processed_raw, processed_newton = chain.process(
|
||||
raw_rows[index],
|
||||
newton_rows[index],
|
||||
raw_valid=None if raw_flags is None else raw_flags[index],
|
||||
newton_valid=None if newton_flags is None else newton_flags[index],
|
||||
signal_time_s=None if times is None else float(times[index]),
|
||||
)
|
||||
raw_out.append(processed_raw)
|
||||
newton_out.append(processed_newton)
|
||||
return raw_out, newton_out, chain.settings
|
||||
|
||||
Reference in New Issue
Block a user