新增统一力读数会话 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:
@@ -19,6 +19,11 @@ from .pipeline import (
|
||||
StreamPipeline,
|
||||
)
|
||||
from .sensor import Sensor
|
||||
#: The explicit force-readout session (``studio-force/1``) used by the Studio
|
||||
#: workflow. Imported after ``Sensor`` because it composes the sensor, model and
|
||||
#: output blocks; the historical ``Sensor.read_info``/``ForceReadout.apply``
|
||||
#: entry points keep their semantics for every caller that does not construct it.
|
||||
from .force_session import ForceSession, WORKFLOW_VERSION
|
||||
|
||||
CompoundEyeSensor = Sensor
|
||||
CameraSensor = Sensor
|
||||
@@ -61,6 +66,10 @@ __all__ = [
|
||||
# force calibration
|
||||
'calibration',
|
||||
|
||||
# explicit force workflow session (studio-force/1)
|
||||
'ForceSession',
|
||||
'WORKFLOW_VERSION',
|
||||
|
||||
# configs utilities functions
|
||||
'list_config',
|
||||
'export_config',
|
||||
|
||||
@@ -6,7 +6,9 @@ curves in ``_apply_force_filter``; ``read_info(FNORMAL, …)`` returns Newtons.
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import re
|
||||
import warnings
|
||||
from collections.abc import Iterable, Mapping
|
||||
@@ -408,6 +410,53 @@ class Calibration:
|
||||
)
|
||||
return out_fn, out_fx, out_fy
|
||||
|
||||
def convert_referenced(
|
||||
self,
|
||||
raw: tuple[float, float, float],
|
||||
reference: Mapping[str, float] | None,
|
||||
) -> tuple[float | None, float | None, float | None]:
|
||||
"""Per-axis newtons against a measured unloaded reference, subtracted once.
|
||||
|
||||
Schema 1/2 curves were calibrated without a session zero: each curve maps
|
||||
a raw feature to newtons and carries its own zero point internally. A
|
||||
session baseline is an *additional* per-session raw offset, so the
|
||||
correction belongs in the force domain: an axis reports
|
||||
``curve(raw) - curve(reference_raw)``. The curve's internal offset is
|
||||
therefore part of both terms and is never subtracted a second time, and
|
||||
this does not assume the curve is linear (subtracting the baseline in the
|
||||
raw domain would).
|
||||
|
||||
An axis without a curve, without a reference, or with a non-finite raw
|
||||
value reports ``None``: unavailable is not a newtons value and must never
|
||||
be filled with zero. The historical :meth:`convert` path is unchanged.
|
||||
"""
|
||||
out: list[float | None] = []
|
||||
for index, axis in enumerate(CHANNEL_IDS):
|
||||
curve = self.channels.get(axis)
|
||||
if curve is None or reference is None or axis not in reference:
|
||||
out.append(None)
|
||||
continue
|
||||
try:
|
||||
raw_value = float(raw[index])
|
||||
reference_value = float(reference[axis])
|
||||
except (TypeError, ValueError, IndexError):
|
||||
out.append(None)
|
||||
continue
|
||||
if not (math.isfinite(raw_value) and math.isfinite(reference_value)):
|
||||
out.append(None)
|
||||
continue
|
||||
convert = curve.convert_component if curve.signed else curve.convert
|
||||
out.append(float(convert(raw_value)) - float(convert(reference_value)))
|
||||
return (out[0], out[1], out[2])
|
||||
|
||||
@property
|
||||
def fingerprint(self) -> str:
|
||||
"""Content identity of this document: no path, no mtime, stable across reloads."""
|
||||
content = json.dumps(
|
||||
self.to_dict(), sort_keys=True, separators=(",", ":"), allow_nan=False
|
||||
)
|
||||
return hashlib.sha256(content.encode("utf-8")).hexdigest()
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
channels: dict[str, Any] = {}
|
||||
for cid, curve in self.channels.items():
|
||||
|
||||
@@ -34,6 +34,15 @@ class CalibrationContext(dict):
|
||||
identity["tracker"]["hhd_downsample"] = downsample
|
||||
return _feature_signature(identity)
|
||||
|
||||
@property
|
||||
def actual_downsample(self) -> int:
|
||||
"""The HHD downsample this runtime context was actually built with."""
|
||||
raw = self.get("hhd_downsample", 1)
|
||||
try:
|
||||
return max(1, int(raw)) if raw is not None else 1
|
||||
except (TypeError, ValueError):
|
||||
return 1
|
||||
|
||||
|
||||
def sha256_file(path) -> str:
|
||||
return hashlib.sha256(Path(path).read_bytes()).hexdigest()
|
||||
|
||||
@@ -163,6 +163,38 @@ class CalibrationResult:
|
||||
diagnostics: dict[str, Any] = dataclass_field(default_factory=dict)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DownsampleReport:
|
||||
"""How the HHD resolution a model was calibrated with compares to the runtime.
|
||||
|
||||
A ``hhd_downsample`` difference is allowed by the pipeline contract (HHD
|
||||
magnitudes are normalized to the full flow grid), but it is an accuracy risk
|
||||
the operator and the record must be able to see. This report is pure data:
|
||||
``validate_for_context`` decides whether a difference is acceptable, the App
|
||||
decides where to display it.
|
||||
"""
|
||||
|
||||
declared: int
|
||||
actual: int | None
|
||||
match: bool
|
||||
allowed: bool
|
||||
accuracy_risk: str
|
||||
|
||||
@property
|
||||
def known(self) -> bool:
|
||||
"""Whether the runtime value was actually available."""
|
||||
return self.actual is not None
|
||||
|
||||
def as_dict(self) -> dict[str, Any]:
|
||||
return {
|
||||
"declared": self.declared,
|
||||
"actual": self.actual,
|
||||
"match": self.match,
|
||||
"allowed": self.allowed,
|
||||
"accuracy_risk": self.accuracy_risk,
|
||||
}
|
||||
|
||||
|
||||
class ModelCalibration:
|
||||
"""One validated model per stream. Current gain backends are stateless."""
|
||||
|
||||
@@ -245,25 +277,101 @@ class ModelCalibration:
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
return deepcopy(self._document)
|
||||
|
||||
def validate_for_context(self, pipeline: Mapping[str, Any]) -> None:
|
||||
def downsample_report(self, pipeline: Mapping[str, Any]) -> DownsampleReport:
|
||||
"""The calibrated-vs-runtime HHD resolution, as data (never raises).
|
||||
|
||||
``actual`` is ``None`` when the caller did not declare it; that is
|
||||
reported as unknown instead of being treated as a match.
|
||||
"""
|
||||
declared = max(1, int(self.pipeline.get("hhd_downsample", 1)))
|
||||
actual: int | None = None
|
||||
raw: Any = None
|
||||
if isinstance(pipeline, Mapping):
|
||||
raw = pipeline.get("hhd_downsample")
|
||||
if raw is not None:
|
||||
try:
|
||||
actual = max(1, int(raw))
|
||||
except (TypeError, ValueError):
|
||||
actual = None
|
||||
match = bool(actual is not None and actual == declared)
|
||||
risk = ""
|
||||
if actual is None:
|
||||
risk = (
|
||||
f"HHD downsample is {declared} in the calibration but unknown at "
|
||||
"runtime; only the calibrated resolution was verified"
|
||||
)
|
||||
elif not match:
|
||||
risk = (
|
||||
f"HHD downsample differs: calibration {declared}, runtime {actual}; "
|
||||
"HHD magnitudes are normalized to the full flow grid, so results are "
|
||||
"compatible in unit but only relative accuracy was verified"
|
||||
)
|
||||
return DownsampleReport(
|
||||
declared=declared,
|
||||
actual=actual,
|
||||
match=match,
|
||||
allowed=True,
|
||||
accuracy_risk=risk,
|
||||
)
|
||||
|
||||
def pipeline_compatibility(self, pipeline: Mapping[str, Any]) -> dict[str, Any]:
|
||||
"""Every pipeline identity check, as one report instead of an exception.
|
||||
|
||||
``mismatches`` holds the same messages ``validate_for_context`` raises, in
|
||||
pipeline order. A ``hhd_downsample`` difference is reported (and allowed)
|
||||
through :meth:`downsample_report` rather than being a mismatch.
|
||||
"""
|
||||
from .calibration_context import CalibrationContext
|
||||
|
||||
mismatches: dict[str, str] = {}
|
||||
for key, value in self.pipeline.items():
|
||||
if key == "hhd_downsample":
|
||||
# HHD magnitudes are normalized to the full flow grid; resolution
|
||||
# can introduce numerical deviations without changing the force unit.
|
||||
# Allowed, but reported: HHD magnitudes are normalized to the full
|
||||
# flow grid, so resolution can introduce numerical deviations
|
||||
# without changing the force unit.
|
||||
continue
|
||||
if key == "feature_signature_sha256" and isinstance(pipeline, CalibrationContext):
|
||||
_require(
|
||||
pipeline.signature_for_downsample(self.pipeline["hhd_downsample"]) == value,
|
||||
"calibration pipeline mismatch: feature_signature_sha256",
|
||||
)
|
||||
if key == "feature_signature_sha256" and isinstance(
|
||||
pipeline, CalibrationContext
|
||||
):
|
||||
if pipeline.signature_for_downsample(
|
||||
self.pipeline["hhd_downsample"]
|
||||
) != value:
|
||||
mismatches[key] = (
|
||||
"calibration pipeline mismatch: feature_signature_sha256"
|
||||
)
|
||||
elif key == "filters":
|
||||
actual = pipeline.get(key, {})
|
||||
_require(isinstance(actual, Mapping) and actual.get("measurement") == value["measurement"],
|
||||
"calibration pipeline mismatch: filters.measurement")
|
||||
else:
|
||||
_require(pipeline.get(key) == value, f"calibration pipeline mismatch: {key}")
|
||||
if not (
|
||||
isinstance(actual, Mapping)
|
||||
and actual.get("measurement") == value["measurement"]
|
||||
):
|
||||
mismatches[key] = (
|
||||
"calibration pipeline mismatch: filters.measurement"
|
||||
)
|
||||
elif pipeline.get(key) != value:
|
||||
mismatches[key] = f"calibration pipeline mismatch: {key}"
|
||||
downsample = self.downsample_report(pipeline)
|
||||
return {
|
||||
"ok": not mismatches,
|
||||
"mismatches": mismatches,
|
||||
"downsample": downsample.as_dict(),
|
||||
}
|
||||
|
||||
def validate_for_context(self, pipeline: Mapping[str, Any]) -> None:
|
||||
"""Refuse incompatible pipeline identities; allow (and report) downsample.
|
||||
|
||||
Kept as the validating entry point for existing callers: the message of a
|
||||
refused identity is unchanged, and a downsample-only difference still
|
||||
passes. Consumers that want to display the risk use
|
||||
:meth:`pipeline_compatibility`.
|
||||
"""
|
||||
report = self.pipeline_compatibility(pipeline)
|
||||
mismatches = report["mismatches"]
|
||||
if mismatches:
|
||||
from .calibration import CalibrationError
|
||||
|
||||
first = next(iter(mismatches.values()))
|
||||
raise CalibrationError(first)
|
||||
|
||||
def reset(self, reference_id: Any = None) -> None:
|
||||
"""Clear per-stream state; the field model also voids its weighted baseline."""
|
||||
|
||||
@@ -454,6 +454,60 @@ class FingerRuntime:
|
||||
self.tactile.push_frame(unrolled, already_gray=True)
|
||||
return unrolled
|
||||
|
||||
# ------------------------------------------- session measurement seam
|
||||
def enable_measurement_session(self) -> None:
|
||||
"""Hand force readout to a ``ForceSession`` (the explicit new entry)."""
|
||||
self.tactile.raw_sensor.enable_measurement_session()
|
||||
|
||||
def disable_measurement_session(self) -> None:
|
||||
"""Return to the historical automatic model/readout path."""
|
||||
self.tactile.raw_sensor.disable_measurement_session()
|
||||
|
||||
def reset_tracker(self) -> None:
|
||||
"""Reset the tactile tracker at a frame boundary (session zeroing half)."""
|
||||
self.tactile.raw_sensor.reset_tracker()
|
||||
|
||||
def read_measurement_frame(
|
||||
self,
|
||||
source_frame,
|
||||
*,
|
||||
frame_id: int,
|
||||
timestamp_s: float | None = None,
|
||||
want_field: bool = False,
|
||||
want_contact: bool = False,
|
||||
):
|
||||
"""Unroll one source frame and measure it once, without calibration."""
|
||||
del timestamp_s # signal time is carried by the caller's FrameStamp
|
||||
self.process_frame(source_frame)
|
||||
return self.tactile.raw_sensor.measure_current_frame(
|
||||
frame_id=frame_id,
|
||||
want_field=want_field,
|
||||
want_contact=want_contact,
|
||||
)
|
||||
|
||||
def current_measurement(
|
||||
self, *, want_field: bool = False, want_contact: bool = False
|
||||
):
|
||||
"""Same-frame measurement features already produced by the pipeline (no recompute).
|
||||
|
||||
Additive: forwards to the tactile sensor's session seam so a pipeline
|
||||
consumer can hand the frame to ``ForceSession`` without a second
|
||||
unroll/flow/HHD pass.
|
||||
"""
|
||||
return self.tactile.raw_sensor.current_measurement(
|
||||
want_field=want_field, want_contact=want_contact
|
||||
)
|
||||
|
||||
@property
|
||||
def calibration_context(self):
|
||||
"""The pipeline context the open model was validated against, if any."""
|
||||
return self.tactile.raw_sensor.calibration_context
|
||||
|
||||
@property
|
||||
def calibration(self):
|
||||
"""The force calibration the open runtime holds (additive read-only accessor)."""
|
||||
return getattr(self.tactile.raw_sensor, "calibration", None)
|
||||
|
||||
def grab_and_process(self, *, color=False):
|
||||
ok, frame = self.read_source()
|
||||
if not ok:
|
||||
|
||||
@@ -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
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -157,6 +157,12 @@ class FrameSnapshot:
|
||||
force_units: tuple[str, str, str] | None = None
|
||||
calibration_status: dict[str, str] | None = None
|
||||
contact_found: bool | None = None
|
||||
#: Monotonic count of tracker resets the flow stage has applied. A consumer
|
||||
#: that delegates a frame-boundary reset to :meth:`StreamPipeline.reset` can
|
||||
#: use this to tell frames measured before that reset from frames measured
|
||||
#: after it (the reset is queued, not synchronous). Additive field: producers
|
||||
#: that do not use it leave it at 0.
|
||||
tracker_reset_seq: int = 0
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -231,6 +237,10 @@ class StreamPipeline:
|
||||
#: Set by :meth:`reset` and consumed by the flow stage, so that resetting sensor state
|
||||
#: never races the stage that owns that state.
|
||||
self._reset_pending = threading.Event()
|
||||
#: How many tracker resets the flow stage has actually applied. Published on
|
||||
#: every snapshot so a consumer can tell a frame measured before a queued
|
||||
#: reset from one measured after it.
|
||||
self._tracker_reset_seq = 0
|
||||
#: Pause state. ``_pause_level`` is what a pause would hold when armed;
|
||||
#: ``_currently_capturing`` records whether the producer is still
|
||||
#: reading, which decides whether a resume has to rebase the tracker.
|
||||
@@ -316,6 +326,16 @@ class StreamPipeline:
|
||||
with self._lock:
|
||||
return self._latest
|
||||
|
||||
@property
|
||||
def tracker_reset_seq(self) -> int:
|
||||
"""Tracker resets the flow stage has applied so far (queued, not immediate).
|
||||
|
||||
A consumer that asked for a reset through :meth:`reset` uses this (and
|
||||
:attr:`FrameSnapshot.tracker_reset_seq`) to identify the first frame whose
|
||||
measurement was produced after the reset.
|
||||
"""
|
||||
return int(self._tracker_reset_seq)
|
||||
|
||||
def reset(self) -> None:
|
||||
"""Reset the sensor's tracker and force-filter state, safely.
|
||||
|
||||
@@ -570,6 +590,9 @@ class StreamPipeline:
|
||||
if self._reset_pending.is_set():
|
||||
self._reset_pending.clear()
|
||||
self.sensor.reset()
|
||||
# Published on this frame's snapshot: this is the first frame whose
|
||||
# measurement (flow) was produced after the reset was applied.
|
||||
self._tracker_reset_seq += 1
|
||||
frame, t_cap, produce_s, source_metadata = item
|
||||
t0 = time.perf_counter()
|
||||
# The frame is passed explicitly: the SDK's self.img slot is shared
|
||||
@@ -674,6 +697,7 @@ class StreamPipeline:
|
||||
hhd_ms=hhd_s * 1000.0,
|
||||
image_size_raw=image_size_raw,
|
||||
source_metadata=source_metadata,
|
||||
tracker_reset_seq=int(self._tracker_reset_seq),
|
||||
)
|
||||
|
||||
def _fps(self) -> float:
|
||||
|
||||
+230
-14
@@ -8,6 +8,7 @@ and Helmholtz-Hodge decomposition.
|
||||
|
||||
# import os # only used by commented-out stitch_cal
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum, auto
|
||||
from pathlib import Path
|
||||
|
||||
@@ -41,6 +42,30 @@ from .capture import (
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FrameMeasurement:
|
||||
"""Same-frame unfiltered measurement features, before model and readout.
|
||||
|
||||
Produced by :meth:`Sensor.read_measurement_frame` and
|
||||
:meth:`Sensor.measure_current_frame`, consumed by ``orisys.force_session``.
|
||||
``raw`` is the HHD force feature triple in raw counts (NaN when unavailable);
|
||||
``field`` is the native HHD field payload (the SDK ``FieldFrame`` profile)
|
||||
only when the caller asked for it; ``output_valid_mask`` is the physical
|
||||
valid mask in output coordinates. ``centroid`` is the same-frame contact
|
||||
location in output pixels — not a calibration coordinate: mapping through a
|
||||
model's coordinate artifact belongs to the session that owns the model.
|
||||
"""
|
||||
|
||||
raw: tuple[float, float, float]
|
||||
raw_valid: tuple[bool, bool, bool]
|
||||
field: dict | None = None
|
||||
output_valid_mask: np.ndarray | None = None
|
||||
contact_found: bool | None = None
|
||||
single_contact: bool | None = None
|
||||
centroid: tuple[float, float] | None = None
|
||||
diagnostics: tuple[tuple[str, str | float], ...] = ()
|
||||
|
||||
|
||||
class Sensor:
|
||||
"""
|
||||
Optical tactile sensor interface with deformation analysis.
|
||||
@@ -49,6 +74,10 @@ class Sensor:
|
||||
Supports both synchronous and asynchronous (pipelined) processing modes.
|
||||
"""
|
||||
|
||||
#: Explicit opt-in for the session-owned force readout (``orisys.force_session``).
|
||||
#: While False the historical automatic model/readout path runs unchanged.
|
||||
_measurement_session = False
|
||||
|
||||
# =========================================================================
|
||||
# Info Enum (for read_info API)
|
||||
# =========================================================================
|
||||
@@ -743,16 +772,26 @@ class Sensor:
|
||||
fn, fx, fy, ftor = forces
|
||||
self._force_raw = (float(fn), float(fx), float(fy))
|
||||
self.ftor = float(ftor)
|
||||
self._force_readout.update_sample_rate()
|
||||
if self._uses_calibration_model():
|
||||
self._model_frame_id += 1
|
||||
self.compute_contact_valid = False
|
||||
self.centroid_found = False
|
||||
self._force_readout.apply(self._force_raw)
|
||||
self._force_filtered_raw = self._force_readout.filtered_raw
|
||||
if self._measurement_session:
|
||||
# Session path: publish the same-frame unfiltered feature only. Model
|
||||
# inference and readout filtering belong to orisys.force_session, so
|
||||
# the old Newton accessors must not report a value the session did not
|
||||
# produce (they would be stale or half-processed).
|
||||
self.fnormal = self.fshearx = self.fsheary = float("nan")
|
||||
self._force_filtered_raw = None
|
||||
self._force_unfiltered = None
|
||||
self._force_raw_unfiltered = self._force_raw
|
||||
else:
|
||||
self._apply_force_filter()
|
||||
self._force_readout.update_sample_rate()
|
||||
if self._uses_calibration_model():
|
||||
self._model_frame_id += 1
|
||||
self.compute_contact_valid = False
|
||||
self.centroid_found = False
|
||||
self._force_readout.apply(self._force_raw)
|
||||
self._force_filtered_raw = self._force_readout.filtered_raw
|
||||
self._force_raw_unfiltered = self._force_raw
|
||||
else:
|
||||
self._apply_force_filter()
|
||||
|
||||
vnormal_ds = self.__hhd.get_normal()
|
||||
vshear_ds = self.__hhd.get_shear()
|
||||
@@ -788,7 +827,7 @@ class Sensor:
|
||||
|
||||
self.compute_deformation_valid = True
|
||||
|
||||
if self._uses_calibration_model():
|
||||
if self._uses_calibration_model() and not self._measurement_session:
|
||||
# Spatial inference must use this HHD frame's contact, even when the
|
||||
# caller did not request contact diagnostics from StreamPipeline.
|
||||
# The field model needs no locator: it consumes this frame's field.
|
||||
@@ -817,6 +856,7 @@ class Sensor:
|
||||
if self._uses_calibration_model() and not self.compute_deformation_valid:
|
||||
raise cal_mod.CalibrationError("compute current deformation before schema 3 contact")
|
||||
if (self._uses_calibration_model()
|
||||
and not self._measurement_session
|
||||
and self.calibration.input_profile == "fn_delta_autocal_uv/1"
|
||||
and self.compute_contact_valid
|
||||
and self._calibration_result_frame == self._model_frame_id):
|
||||
@@ -855,6 +895,7 @@ class Sensor:
|
||||
|
||||
self.compute_contact_valid = True
|
||||
if (self._uses_calibration_model()
|
||||
and not self._measurement_session
|
||||
and self.calibration.input_profile == "fn_delta_autocal_uv/1"):
|
||||
self._model_contact_result = result
|
||||
uv = self._calibration_contact_uv()
|
||||
@@ -865,6 +906,185 @@ class Sensor:
|
||||
return result
|
||||
|
||||
|
||||
# =========================================================================
|
||||
# Session measurement seam (explicit opt-in for orisys.force_session)
|
||||
# =========================================================================
|
||||
def enable_measurement_session(self) -> None:
|
||||
"""Hand force readout to a ``ForceSession`` (Studio's explicit new entry).
|
||||
|
||||
While enabled, ``compute_hhd`` / ``compute_contact`` publish this frame's
|
||||
raw feature, contact statistics and field payload and skip the automatic
|
||||
model inference and readout filtering. The Newton accessors report NaN
|
||||
rather than a stale value: the session owns the one model call and the one
|
||||
output. Nothing changes for callers that never call this method.
|
||||
"""
|
||||
self._measurement_session = True
|
||||
self.fnormal = self.fshearx = self.fsheary = float("nan")
|
||||
self.calibration_status = {}
|
||||
self._force_filtered_raw = None
|
||||
self._force_unfiltered = None
|
||||
self._force_raw_unfiltered = None
|
||||
|
||||
def disable_measurement_session(self) -> None:
|
||||
"""Return to the historical automatic model/readout path."""
|
||||
self._measurement_session = False
|
||||
|
||||
@property
|
||||
def measurement_session(self) -> bool:
|
||||
return bool(self._measurement_session)
|
||||
|
||||
def reset_tracker(self) -> None:
|
||||
"""Reset the optical-flow tracker at a frame boundary (session zeroing).
|
||||
|
||||
The narrow half of :meth:`reset`: tracker and readout memory only, never
|
||||
the calibration baseline. ``ForceSession`` calls this on its processing
|
||||
thread between two frames, before the first baseline frame is measured.
|
||||
"""
|
||||
self.__tracker.reset()
|
||||
self._force_readout.reset()
|
||||
self._force_unfiltered = None
|
||||
self._force_raw_unfiltered = None
|
||||
self._force_filtered_raw = None
|
||||
self.compute_deformation_valid = False
|
||||
self.compute_contact_valid = False
|
||||
self.centroid_found = False
|
||||
|
||||
def read_measurement_frame(
|
||||
self,
|
||||
source_frame,
|
||||
*,
|
||||
frame_id: int,
|
||||
timestamp_s: float | None = None,
|
||||
want_field: bool = False,
|
||||
want_contact: bool = False,
|
||||
) -> FrameMeasurement:
|
||||
"""Prepare one source frame and measure it, once, without calibration.
|
||||
|
||||
This is the measurement-block entry ``ForceSession`` calls for a bare
|
||||
Sensor: image preparation/stitch, flow, HHD and the optional contact and
|
||||
field features run exactly once and nothing is inferred or filtered
|
||||
afterwards.
|
||||
"""
|
||||
del timestamp_s # signal time is carried by the caller's FrameStamp
|
||||
if not self._measurement_session:
|
||||
raise RuntimeError(
|
||||
"enable_measurement_session() before reading session measurements"
|
||||
)
|
||||
self.get_img(frame=source_frame)
|
||||
return self.measure_current_frame(
|
||||
frame_id=frame_id,
|
||||
want_field=want_field,
|
||||
want_contact=want_contact,
|
||||
)
|
||||
|
||||
def measure_current_frame(
|
||||
self,
|
||||
*,
|
||||
frame_id: int,
|
||||
want_field: bool = False,
|
||||
want_contact: bool = False,
|
||||
) -> FrameMeasurement:
|
||||
"""Run flow/HHD (and optional contact) on the already prepared frame once."""
|
||||
if not self._measurement_session:
|
||||
raise RuntimeError(
|
||||
"enable_measurement_session() before reading session measurements"
|
||||
)
|
||||
image = self.img
|
||||
if image is None:
|
||||
raise RuntimeError("no prepared frame to measure; provide a frame first")
|
||||
self.compute_flow(image)
|
||||
self.compute_hhd(self.flow)
|
||||
raw = tuple(float(value) for value in self._force_raw)
|
||||
raw_valid = tuple(bool(np.isfinite(value)) for value in raw)
|
||||
field = None
|
||||
if want_field:
|
||||
field = self.current_field()
|
||||
output_mask = (
|
||||
None
|
||||
if self.valid_mask is None
|
||||
else np.array(self.valid_mask, dtype=bool, copy=True)
|
||||
)
|
||||
contact_found: bool | None = None
|
||||
single_contact: bool | None = None
|
||||
centroid: tuple[float, float] | None = None
|
||||
if want_contact:
|
||||
self.compute_contact()
|
||||
contact_found = bool(self.centroid_found)
|
||||
if contact_found and self.centroid is not None:
|
||||
centroid = (float(self.centroid[0]), float(self.centroid[1]))
|
||||
single_contact = True if contact_found else None
|
||||
return FrameMeasurement(
|
||||
raw=raw,
|
||||
raw_valid=raw_valid,
|
||||
field=field,
|
||||
output_valid_mask=output_mask,
|
||||
contact_found=contact_found,
|
||||
single_contact=single_contact,
|
||||
centroid=centroid,
|
||||
diagnostics=(("frame_id", int(frame_id)),),
|
||||
)
|
||||
|
||||
def current_measurement(
|
||||
self, *, want_field: bool = False, want_contact: bool = False
|
||||
) -> FrameMeasurement:
|
||||
"""Read the current frame's measurement features without recomputing anything.
|
||||
|
||||
Additive session seam for a pipeline consumer: ``compute_hhd`` (and, when
|
||||
requested, ``compute_contact``) have already produced this frame's state,
|
||||
so this only snapshots it — no second ``get_img``/``compute_flow``/
|
||||
``compute_hhd``. Arrays are copied and made read-only because the caller
|
||||
hands the row to another thread (the force session).
|
||||
"""
|
||||
if not self._measurement_session:
|
||||
raise RuntimeError(
|
||||
"enable_measurement_session() before reading session measurements"
|
||||
)
|
||||
raw = tuple(float(value) for value in self._force_raw)
|
||||
raw_valid = tuple(bool(np.isfinite(value)) for value in raw)
|
||||
field = None
|
||||
if want_field:
|
||||
field = self.current_field()
|
||||
output_mask = (
|
||||
None
|
||||
if self.valid_mask is None
|
||||
else np.array(self.valid_mask, dtype=bool, copy=True)
|
||||
)
|
||||
if output_mask is not None:
|
||||
output_mask.setflags(write=False)
|
||||
contact_found: bool | None = None
|
||||
single_contact: bool | None = None
|
||||
centroid: tuple[float, float] | None = None
|
||||
if want_contact:
|
||||
contact_found = bool(self.centroid_found)
|
||||
if contact_found and self.centroid is not None:
|
||||
centroid = (float(self.centroid[0]), float(self.centroid[1]))
|
||||
single_contact = True if contact_found else None
|
||||
return FrameMeasurement(
|
||||
raw=raw,
|
||||
raw_valid=raw_valid,
|
||||
field=field,
|
||||
output_valid_mask=output_mask,
|
||||
contact_found=contact_found,
|
||||
single_contact=single_contact,
|
||||
centroid=centroid,
|
||||
)
|
||||
|
||||
@property
|
||||
def calibration_context(self):
|
||||
"""The pipeline context this sensor's model was validated against, if any.
|
||||
|
||||
Additive read-only accessor: ``ForceSession`` can adopt an already bound
|
||||
context when its caller does not pass one explicitly.
|
||||
"""
|
||||
return self._calibration_context
|
||||
|
||||
def current_field(self) -> dict:
|
||||
"""Same-frame HHD field payload of the last decomposition (a private copy)."""
|
||||
if self.__hhd is None:
|
||||
raise RuntimeError("compute_hhd() before reading the field")
|
||||
mask = self.valid_mask_hhd
|
||||
return self.__hhd.get_field(valid_mask=mask)
|
||||
|
||||
def read_info(self, *args) -> tuple:
|
||||
"""
|
||||
Get sensor data by info type.
|
||||
@@ -920,14 +1140,10 @@ class Sensor:
|
||||
|
||||
def reset(self):
|
||||
"""Reset optical flow tracker and force-filter state."""
|
||||
self.__tracker.reset()
|
||||
self._force_readout.reset()
|
||||
self.reset_tracker()
|
||||
# The unfiltered pair belongs to the frame it was computed for: a new
|
||||
# session must not report the previous one's reading.
|
||||
self._force_unfiltered = None
|
||||
self._force_raw_unfiltered = None
|
||||
self._reset_calibration_session()
|
||||
self.compute_deformation_valid = False
|
||||
if self.img is not None:
|
||||
self.initial_img = self.img.copy()
|
||||
|
||||
|
||||
@@ -366,6 +366,143 @@ class CausalMedianFilter:
|
||||
return np.median(self._buf, axis=0)
|
||||
|
||||
|
||||
class MultiAxisCausalFilter:
|
||||
"""Causal median → low-pass over a vector, one independent state per axis.
|
||||
|
||||
The force output needs the same chain twice (raw features and newtons), and
|
||||
an unavailable axis (NaN, or an axis a model never calibrated) must neither
|
||||
enter the filter history nor pull a valid axis towards zero. One
|
||||
:class:`CausalMedianFilter` and one :class:`LowpassFilter` per axis give that
|
||||
without a second implementation of either operator: callers keep one instance
|
||||
per processed triplet and feed a per-axis validity mask.
|
||||
|
||||
``median_window <= 1`` and ``lowpass_cutoff_hz <= 0`` each switch their own
|
||||
stage off; with both off the input is returned unchanged, NaN included.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
median_window: int = 1,
|
||||
lowpass_cutoff_hz: float = 0.0,
|
||||
sample_rate_hz: float = DEFAULT_SAMPLE_RATE_HZ,
|
||||
lowpass_order: int = DEFAULT_LOWPASS_ORDER,
|
||||
n_channels: int = 3,
|
||||
):
|
||||
self.n_channels = max(1, int(n_channels))
|
||||
try:
|
||||
self._median_window = max(1, int(median_window))
|
||||
except (TypeError, ValueError):
|
||||
self._median_window = 1
|
||||
self._lowpass_cutoff_hz = float(lowpass_cutoff_hz or 0.0)
|
||||
self._sample_rate_hz = float(sample_rate_hz or DEFAULT_SAMPLE_RATE_HZ)
|
||||
try:
|
||||
self._lowpass_order = max(1, int(lowpass_order))
|
||||
except (TypeError, ValueError):
|
||||
self._lowpass_order = DEFAULT_LOWPASS_ORDER
|
||||
self._medians: list[CausalMedianFilter] = []
|
||||
self._lowpasses: list[LowpassFilter] = []
|
||||
self._build()
|
||||
|
||||
@property
|
||||
def enabled(self) -> bool:
|
||||
return bool(self._medians and self._medians[0].enabled) or bool(
|
||||
self._lowpasses and self._lowpasses[0].enabled
|
||||
)
|
||||
|
||||
@property
|
||||
def median_window(self) -> int:
|
||||
return int(self._medians[0].window) if self._medians else 1
|
||||
|
||||
@property
|
||||
def lowpass_cutoff_hz(self) -> float:
|
||||
return float(self._lowpasses[0].cutoff_hz) if self._lowpasses else 0.0
|
||||
|
||||
def configure(
|
||||
self,
|
||||
*,
|
||||
median_window: int | None = None,
|
||||
lowpass_cutoff_hz: float | None = None,
|
||||
sample_rate_hz: float | None = None,
|
||||
lowpass_order: int | None = None,
|
||||
) -> None:
|
||||
"""Re-tune the chain; every axis state is dropped (callers re-warm it)."""
|
||||
if median_window is not None:
|
||||
try:
|
||||
self._median_window = max(1, int(median_window))
|
||||
except (TypeError, ValueError):
|
||||
self._median_window = 1
|
||||
if lowpass_cutoff_hz is not None:
|
||||
self._lowpass_cutoff_hz = float(lowpass_cutoff_hz)
|
||||
if sample_rate_hz is not None:
|
||||
self._sample_rate_hz = float(sample_rate_hz)
|
||||
if lowpass_order is not None:
|
||||
try:
|
||||
self._lowpass_order = max(1, int(lowpass_order))
|
||||
except (TypeError, ValueError):
|
||||
self._lowpass_order = DEFAULT_LOWPASS_ORDER
|
||||
self._build()
|
||||
|
||||
def _build(self) -> None:
|
||||
self._medians = [
|
||||
CausalMedianFilter(window=self._median_window, n_channels=1)
|
||||
for _ in range(self.n_channels)
|
||||
]
|
||||
self._lowpasses = [
|
||||
LowpassFilter(
|
||||
cutoff_hz=self._lowpass_cutoff_hz,
|
||||
sample_rate_hz=self._sample_rate_hz,
|
||||
order=self._lowpass_order,
|
||||
n_channels=1,
|
||||
)
|
||||
for _ in range(self.n_channels)
|
||||
]
|
||||
|
||||
def reset(self) -> None:
|
||||
"""Drop every axis' filter memory (new zero, model change, discontinuity)."""
|
||||
for median in self._medians:
|
||||
median.reset()
|
||||
for lowpass in self._lowpasses:
|
||||
lowpass.reset()
|
||||
|
||||
def process(self, values, valid=None) -> np.ndarray:
|
||||
"""Filter one vector; invalid axes stay NaN and do not advance their state."""
|
||||
x = np.asarray(values, dtype=np.float64).reshape(-1)
|
||||
if x.size != self.n_channels:
|
||||
raise ValueError(f"expected {self.n_channels} channels, got {x.size}")
|
||||
if valid is None:
|
||||
usable = np.isfinite(x)
|
||||
else:
|
||||
usable = np.asarray(valid, dtype=bool).reshape(-1) & np.isfinite(x)
|
||||
if usable.size != self.n_channels:
|
||||
raise ValueError(
|
||||
f"expected {self.n_channels} validity flags, got {usable.size}"
|
||||
)
|
||||
out = np.full(self.n_channels, np.nan, dtype=np.float64)
|
||||
for index in range(self.n_channels):
|
||||
if not usable[index]:
|
||||
continue
|
||||
value = self._medians[index].process(x[index : index + 1])
|
||||
out[index] = self._lowpasses[index].process(value)[0]
|
||||
return out
|
||||
|
||||
def state(self) -> dict:
|
||||
return {
|
||||
"median_window": self.median_window,
|
||||
"median_applied": bool(self._medians and self._medians[0].enabled),
|
||||
"lowpass_cutoff_hz": self.lowpass_cutoff_hz,
|
||||
"lowpass_applied": bool(self._lowpasses and self._lowpasses[0].enabled),
|
||||
"lowpass_order": (
|
||||
int(self._lowpasses[0].order) if self._lowpasses else DEFAULT_LOWPASS_ORDER
|
||||
),
|
||||
"lowpass_sample_rate_hz": (
|
||||
float(self._lowpasses[0].sample_rate_hz)
|
||||
if self._lowpasses
|
||||
else DEFAULT_SAMPLE_RATE_HZ
|
||||
),
|
||||
}
|
||||
|
||||
|
||||
class SampleRateEstimator:
|
||||
"""Robust online estimate of the rate at which :meth:`tick` is called.
|
||||
|
||||
|
||||
Reference in New Issue
Block a user