"""Force readout for FN/FX/FY: EMA, median, low-pass, and optional convert. 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 from .signal_processing import ( DEFAULT_SAMPLE_RATE_HZ, CausalMedianFilter, LowpassFilter, MultiAxisCausalFilter, SampleRateEstimator, ) # Default signal-conditioning settings (config: top-level "filters" block). # # A key a config does not state is **off**, not "whatever we thought was # sensible": the file has to name what runs, or a device can be filtering a # measurement nobody asked it to filter and no reader of the file could tell. # The shipped bundles state all three values explicitly, so they are unaffected # by these defaults; a config with no "filters" block at all filters nothing. # # Which settings are worth asking for is measured, not guessed. On dd02-0703 at # 280x300 (see the noise analysis in the perf notes) flow EMA 0.5 + median 3 # costs ~2 frames at 30 fps and cuts steady-press force jitter by roughly half # while also stabilising vnormal/contact (which a force-level filter cannot do); # that combination is the "balanced" preset. The force low-pass stays off there # too: on top of the EMA it added no measurable noise benefit at the same # latency, so it is an opt-in stage for sites with different noise # characteristics (the "low_noise" preset). DEFAULT_FORCE_LOWPASS_HZ = 0.0 DEFAULT_FORCE_MEDIAN_WINDOW = 1 DEFAULT_FLOW_EMA_ALPHA = 1.0 #: Named filter presets, so a recommendation is *executable* instead of a #: comment in a JSON file (``json.load`` cannot carry comments). A config may #: write ``"filters": {"preset": "balanced"}`` and override any single key on #: top of it; the resolved values are what ``Sensor.filters_state()`` reports. #: The latency/noise trade-offs behind these numbers are measured in #: ``docs/performance/perf-measurements/`` (moved out of the once-gitignored #: ``orisys-sdk/_perf/`` on 2026-09-27) and tabulated in ``docs/filters.md``. FILTER_PRESETS = { # Everything off: the file says what runs, and the raw reading is the record. "off": { "flow_ema_alpha": 1.0, "force_median_window": 1, "force_lowpass_cutoff_hz": 0.0, }, # One frame of latency @30 fps: median only. The EMA changes the # measurement, so it stays off when latency matters more than jitter. "low_latency": { "flow_ema_alpha": 1.0, "force_median_window": 3, "force_lowpass_cutoff_hz": 0.0, }, # Two frames @30 fps: the EMA removes the spatially-coherent flow jitter # that no force-level filter can reach, the median removes its spikes. "balanced": { "flow_ema_alpha": 0.5, "force_median_window": 3, "force_lowpass_cutoff_hz": 0.0, }, # Three frames @30 fps and the quietest readout: as ``balanced`` plus the # force low-pass on top of the median. "low_noise": { "flow_ema_alpha": 0.3, "force_median_window": 3, "force_lowpass_cutoff_hz": 10.0, }, } def available_filter_presets() -> dict: """The presets, for UIs that want to offer them (the studio's gear dialog).""" return {name: dict(values) for name, values in FILTER_PRESETS.items()} ConvertFn = Callable[[float, float, float], tuple[float, float, float]] class ForceReadout: """Median → low-pass → convert for a three-channel force triple.""" def __init__( self, filters_cfg: dict | None = None, *, config_fps: float = DEFAULT_SAMPLE_RATE_HZ, verbose: bool = False, ) -> None: # These settings change the reported force values, so they are part of # the measurement-pipeline identity and are recorded in the calibration # metadata / mismatch check alongside dis_mode and hhd_downsample. # # Every stage is switched by its **own value**, so the config states what # runs: ``flow_ema_alpha >= 1``, ``force_median_window <= 1`` and # ``force_lowpass_cutoff_hz <= 0`` each mean "stage off". Values are never # overridden: there is no master switch that could make the file say 0.5 # while the device runs 1.0. An invalid value falls back to **off**, never # to a default that filters. ``enabled`` is kept only as a deprecated # alias for "turn every stage off" and is honoured when it is explicitly # false. ``state()`` reports what actually took effect. if not isinstance(filters_cfg, dict): filters_cfg = {} legacy_off = filters_cfg.get("enabled", None) is False preset_name = filters_cfg.get("preset", None) preset_values = FILTER_PRESETS.get(str(preset_name), {}) if preset_name else {} self.preset = str(preset_name) if preset_name in FILTER_PRESETS else None def _cfg(key, default): if legacy_off: return default value = filters_cfg.get(key, None) if value is None: value = preset_values.get(key, None) return default if value is None else value try: self.config_fps = float(config_fps or 0.0) except (TypeError, ValueError): self.config_fps = 0.0 if self.config_fps <= 0: self.config_fps = float(DEFAULT_SAMPLE_RATE_HZ) self.lowpass_order = 2 self.lowpass_sample_rate_hz = self.config_fps self.lowpass_cutoff_hz = 0.0 self.lowpass_rate_auto = True try: self.lowpass_order = max(1, int(_cfg("force_lowpass_order", 2))) except (TypeError, ValueError): self.lowpass_order = 2 rate_cfg = _cfg("force_lowpass_sample_rate_hz", "auto") if isinstance(rate_cfg, str): self.lowpass_rate_auto = rate_cfg.strip().lower() in ("auto", "", "measured") else: try: rate = float(rate_cfg) except (TypeError, ValueError): rate = 0.0 self.lowpass_rate_auto = rate <= 0.0 if not self.lowpass_rate_auto: self.lowpass_sample_rate_hz = rate try: self.lowpass_cutoff_hz = float( _cfg("force_lowpass_cutoff_hz", DEFAULT_FORCE_LOWPASS_HZ) ) except (TypeError, ValueError): self.lowpass_cutoff_hz = 0.0 if self.lowpass_sample_rate_hz <= 0: self.lowpass_sample_rate_hz = self.config_fps # An unusable cutoff is "off", never a silent jump to the default. if not (0.0 <= self.lowpass_cutoff_hz < 0.5 * self.lowpass_sample_rate_hz): self.lowpass_cutoff_hz = 0.0 self.lowpass_enabled = self.lowpass_cutoff_hz > 0 self._force_filter = LowpassFilter( cutoff_hz=self.lowpass_cutoff_hz, sample_rate_hz=self.lowpass_sample_rate_hz, order=self.lowpass_order, n_channels=3, ) self._force_rate = SampleRateEstimator(initial_hz=self.config_fps) self.sample_rate_source = "config" try: self.force_median_window = max( 1, int(_cfg("force_median_window", DEFAULT_FORCE_MEDIAN_WINDOW)) ) except (TypeError, ValueError): self.force_median_window = 1 self._force_median = CausalMedianFilter( window=self.force_median_window, n_channels=3 ) self.flow_ema_alpha = 1.0 try: alpha = float(_cfg("flow_ema_alpha", DEFAULT_FLOW_EMA_ALPHA)) except (TypeError, ValueError): alpha = 1.0 if 0.0 < alpha < 1.0: self.flow_ema_alpha = alpha self._ema_flow = None if legacy_off and verbose: print( "Note: filters.enabled is deprecated and means 'all stages off'; " "remove it and switch each stage by its own value " "(flow_ema_alpha=1, force_median_window=1, force_lowpass_cutoff_hz=0)." ) self.any_stage_applied = bool( self.flow_ema_alpha < 1.0 or self.force_median_window > 1 or self.lowpass_enabled ) self.filtered_raw: tuple[float, float, float] | None = None self.raw_unfiltered: tuple[float, float, float] | None = None self.unfiltered_newton: tuple[float, float, float] | None = None @property def sample_rate_hz(self) -> float: return float(self.lowpass_sample_rate_hz) def pin_file_rate(self) -> None: """Wall-clock cadence is not signal time (video file / offline replay).""" self.lowpass_rate_auto = False self.sample_rate_source = "config (file)" self.lowpass_sample_rate_hz = self.config_fps self._force_rate.reset(self.config_fps) def apply_flow_ema(self, flow): """Temporally smooth the optical-flow field before HHD. The frame-to-frame jitter of the estimated flow is spatially coherent (a near-global shift: it survives ~40x less averaging than per-pixel noise would, see the noise analysis), so it cannot be removed by spatial smoothing, a coarser HHD grid or a different DIS setting - only by averaging along time. Applying it here also stabilises ``vnormal``/contact, which a force-level filter cannot reach. ``flow_ema_alpha >= 1`` disables the stage. """ alpha = self.flow_ema_alpha if flow is None or alpha >= 1.0: return flow prev = self._ema_flow if prev is None or prev.shape != flow.shape or prev.dtype != flow.dtype: self._ema_flow = np.array(flow, copy=True) return flow prev += alpha * (flow - prev) return prev def update_sample_rate(self) -> None: """Track the achieved force update rate for the low-pass design.""" if not self.lowpass_rate_auto: return hz = self._force_rate.tick() if not (1.0 <= hz <= 240.0): return self.sample_rate_source = "measured" used = self.lowpass_sample_rate_hz if used > 0 and abs(hz - used) <= 0.1 * used: return self.lowpass_sample_rate_hz = hz if self.lowpass_enabled: self._force_filter.configure( cutoff_hz=self.lowpass_cutoff_hz, sample_rate_hz=hz, order=self.lowpass_order, ) def apply( self, raw, convert: ConvertFn | None = None, ) -> tuple[float, float, float]: """Median → low-pass → optional convert. Returns filtered newton triple.""" pre_filter = np.asarray(raw, dtype=np.float64) values = pre_filter # Median first: it deletes the heavy-tailed spikes that the flow chain # produces, so the low-pass does not have to spread them over time. if self._force_median.enabled: values = self._force_median.process(values) if self.lowpass_enabled and self.lowpass_cutoff_hz > 0: values = self._force_filter.process(values) self.filtered_raw = (float(values[0]), float(values[1]), float(values[2])) fn, fx, fy = self.filtered_raw if convert is not None: fn, fx, fy = convert(fn, fx, fy) fn, fx, fy = float(fn), float(fx), float(fy) ufn, ufx, ufy = ( float(pre_filter[0]), float(pre_filter[1]), float(pre_filter[2]), ) self.raw_unfiltered = (ufn, ufx, ufy) if convert is not None: ufn, ufx, ufy = convert(ufn, ufx, ufy) ufn, ufx, ufy = float(ufn), float(ufx), float(ufy) self.unfiltered_newton = (ufn, ufx, ufy) return fn, fx, fy def configure( self, *, lowpass_cutoff_hz=None, lowpass_sample_rate_hz=None, lowpass_order=None, lowpass_enabled=None, flow_ema_alpha=None, force_median_window=None, ) -> bool: """Update filter settings. Returns True when the force filter changed.""" filter_changed = False if lowpass_cutoff_hz is not None: self.lowpass_cutoff_hz = float(lowpass_cutoff_hz) filter_changed = True if lowpass_enabled is None: self.lowpass_enabled = self.lowpass_cutoff_hz > 0 if lowpass_sample_rate_hz is not None: # Accept a positive number to pin the rate, or "auto"/<=0 to track # the measured output rate (see update_sample_rate). auto = False rate = 0.0 if isinstance(lowpass_sample_rate_hz, str): auto = lowpass_sample_rate_hz.strip().lower() in ("auto", "", "measured") else: try: rate = float(lowpass_sample_rate_hz) except (TypeError, ValueError): auto = True else: auto = rate <= 0.0 self.lowpass_rate_auto = auto if not auto: self.lowpass_sample_rate_hz = rate elif self.lowpass_sample_rate_hz <= 0: self.lowpass_sample_rate_hz = self.config_fps self._force_rate.reset(self.config_fps) filter_changed = True if lowpass_order is not None: order = int(lowpass_order) if order < 1: raise ValueError("lowpass_order must be >= 1") self.lowpass_order = order filter_changed = True if lowpass_enabled is not None: self.lowpass_enabled = bool(lowpass_enabled) filter_changed = True if flow_ema_alpha is not None: alpha = float(flow_ema_alpha) if not (0.0 < alpha <= 1.0): raise ValueError("flow_ema_alpha must be in (0, 1]") self.flow_ema_alpha = alpha self._ema_flow = None if force_median_window is not None: try: window = max(1, int(force_median_window)) except (TypeError, ValueError) as exc: raise ValueError("force_median_window must be an integer >= 1") from exc self.force_median_window = window self._force_median.configure(window) filter_changed = True if filter_changed: cutoff = self.lowpass_cutoff_hz if self.lowpass_enabled else 0.0 self._force_filter.configure( cutoff_hz=cutoff, sample_rate_hz=self.lowpass_sample_rate_hz, order=self.lowpass_order, ) self.any_stage_applied = bool( self.flow_ema_alpha < 1.0 or self.force_median_window > 1 or self.lowpass_enabled ) return filter_changed def reset(self) -> None: self._force_filter.reset() self._force_median.reset() self._ema_flow = None self.filtered_raw = None self.raw_unfiltered = None self.unfiltered_newton = None def state(self, *, dis_mode=None) -> dict: """Effective filter chain, for records and audits.""" return { "preset": self.preset, "measurement": { "flow_ema_alpha": float(self.flow_ema_alpha), "flow_ema_applied": bool(self.flow_ema_alpha < 1.0), "dis_mode": dis_mode, }, "readout": { "force_median_window": int(self.force_median_window), "force_median_applied": bool(self._force_median.enabled), "force_lowpass_cutoff_hz": float(self.lowpass_cutoff_hz), "force_lowpass_order": int(self.lowpass_order), "force_lowpass_sample_rate_hz": float(self.lowpass_sample_rate_hz), "force_lowpass_rate_auto": bool(self.lowpass_rate_auto), "force_lowpass_applied": bool(self.lowpass_enabled), "sample_rate_source": str(self.sample_rate_source), }, "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