From ee830d85053af97dc244f1f44c9f0509446ee12d Mon Sep 17 00:00:00 2001 From: ykcheungab Date: Tue, 20 Oct 2026 07:07:22 +0000 Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E7=BB=9F=E4=B8=80=E5=8A=9B?= =?UTF-8?q?=E8=AF=BB=E6=95=B0=E4=BC=9A=E8=AF=9D=20ForceSession?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 把同帧测量、空载归零、模型推断与共用后处理收进一个 SDK 会话，Studio 显示与记录使用同一样本；旧 Sensor 读数接口保持兼容。以 force session 与受影响 SDK 套件、lint 与文档门禁验证，硬件与真实载荷精度未验证。 Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01CpCfJF6SvonMKmPZhzKZHb --- docs/api.md | 12 +- docs/force-workflow.zh.md | 101 +++ src/orisys/__init__.py | 9 + src/orisys/calibration.py | 49 + src/orisys/calibration_context.py | 9 + src/orisys/calibration_models.py | 132 ++- src/orisys/finger/runtime.py | 54 ++ src/orisys/force_readout.py | 309 +++++++ src/orisys/force_session.py | 1374 +++++++++++++++++++++++++++++ src/orisys/pipeline.py | 24 + src/orisys/sensor.py | 244 ++++- src/orisys/signal_processing.py | 137 +++ tests/test_calibration.py | 82 ++ tests/test_force_diagnostics.py | 219 +++++ tests/test_force_session.py | 1212 +++++++++++++++++++++++++ tests/test_model_runtime.py | 163 ++++ tests/test_signal_processing.py | 74 ++ 17 files changed, 4175 insertions(+), 29 deletions(-) create mode 100644 docs/force-workflow.zh.md create mode 100644 src/orisys/force_session.py create mode 100644 tests/test_force_diagnostics.py create mode 100644 tests/test_force_session.py diff --git a/docs/api.md b/docs/api.md index 417ba6d..e86118f 100644 --- a/docs/api.md +++ b/docs/api.md @@ -183,7 +183,7 @@ flow = sensor.read_info(sensor.info.VRAW) 释放传感器相关资源并停止后台异步线程。 调用后将关闭视频/摄像头输入源，并销毁所有 OpenCV 窗口。 -建议在程序退出前调用以确保资源被正确释放。 +建议退出前调用。 ### `Sensor.configure` 低通滤波设置 @@ -197,7 +197,7 @@ flow = sensor.read_info(sensor.info.VRAW) sensor.configure(lowpass_cutoff_hz=5.0, lowpass_sample_rate_hz=30.0) ``` -注意：`lowpass_cutoff_hz` 必须小于 Nyquist 频率，即 `lowpass_sample_rate_hz / 2`。例如 30 fps 时，截止频率必须小于 15 Hz。 +注意：`lowpass_cutoff_hz` 必须小于 `lowpass_sample_rate_hz / 2`（30 fps 时 <15 Hz）。 #### 输入参数 @@ -228,7 +228,7 @@ fn, fx, fy = sensor.read_info( 对一维、等间隔采样数据执行零相位 Butterworth 低通滤波，适合示例程序或图表中对历史数据做平滑处理。 -默认参数为 `cutoff_hz=5.0`、`sample_rate_hz=30.0`、`order=2`。这表示在 30 fps 数据上使用 5 Hz 低通滤波。 +默认参数为 `cutoff_hz=5.0`、`sample_rate_hz=30.0`、`order=2`。 #### 输入参数 @@ -279,6 +279,12 @@ filt = LowpassFilter(cutoff_hz=5.0, sample_rate_hz=30.0, n_channels=3) filtered = filt.process([fnormal, fshearx, fsheary]) ``` +## 力读数工作流 + +显式会话入口 `ForceSession`（`orisys.force_session`，`WORKFLOW_VERSION="studio-force/1"`）： +同帧测量、空载归零、每帧一次模型推断、raw/N 共用后处理；旧 `Sensor` 接口语义不变。 +接口与语义见 [force-workflow](force-workflow.zh.md)。 + ## Finger Pipeline API ### `orisys.finger` 手指 UV pipeline diff --git a/docs/force-workflow.zh.md b/docs/force-workflow.zh.md new file mode 100644 index 0000000..d5f5151 --- /dev/null +++ b/docs/force-workflow.zh.md @@ -0,0 +1,101 @@ +# 力读数工作流（`ForceSession`） + +用途：SDK 显式力读数会话（`studio-force/1`）的公共接口参考。状态：已实现并接入 Studio，2026-10-20； +硬件、真实相机与真实载荷精度未验证。需求与验收矩阵见 +[流程规格](../../docs/workflow/studio-force-workflow.zh.md)，实施状态见 +[重写计划](../../docs/workflow/studio-force-rewrite-plan.zh.md)。 + +测量、标定模型、空载基线与输出后处理全部由本会话拥有；调用方只提供源帧与帧身份， +发送归零/换模型请求，并分发返回的 `ForceOutput`。这样同一帧只测量、推断、后处理各一次， +显示与记录拿到同一个不可变输出。 + +## 入口与版本 + +```python +from orisys import ForceSession, WORKFLOW_VERSION +``` + +`WORKFLOW_VERSION = "studio-force/1"` 是处理顺序与语义的稳定身份，随每个输出返回， +记录、缓存键与诊断引用它；改变顺序或语义时必须提升该版本。 + +构造显式启用测量源（`Sensor` 或 `FingerRuntime`）的会话接缝： + +```python +session = ForceSession( + source=runtime, # 已打开、由调用方负责 close + sensor_family="finger", # box | mini | finger | finger-mems + device_id="FINGER-001", # 可选，用于标定身份校验 + context=context, # schema 3 模型需要的实际管线身份 + calibration=model, # Calibration（schema 1/2）、ModelCalibration（schema 3）或 None + settings=output_settings, # 可选；默认 OutputSettings(sample_rate_hz=30.0) +) +``` + +构造失败不会留下“半切换”的会话；旧 `Sensor.read_info` / `ForceReadout.apply` 路径在 +未构造 `ForceSession` 时保持原语义，本模块不与旧路径并行运行。 + +## 主要方法与属性 + +| 成员 | 说明 | +|---|---| +| `process_frame(source_frame, stamp) -> ForceOutput` | 唯一逐帧入口：测量 → 归零状态 → 标定 → 后处理。同一 `(session_id, frame_id)` 重复调用返回同一缓存对象且不推进状态；乱序帧显式失败并清除完成标志 | +| `read_sdk_frame(source_frame, stamp) -> SdkReading` | 只做同帧测量（图像准备、光流/HHD、可选接触/场），不推断、不读数滤波 | +| `request_zero(*, reference_id, declared_unloaded, replay_window=None) -> ZeroProgress` | 启动唯一空载归零操作；`declared_unloaded` 必须为空载确认；拒绝带载相对置零 | +| `update_zero(reading) -> ZeroProgress` | 推进归零状态机并执行本帧一次模型推断；迟到代次的结果被忽略 | +| `invalidate(*, reason) -> ZeroProgress` | 断流、取消、换模型或测量配置变化时作废基线/完成状态与滤波历史 | +| `apply_calibration(reading, zero) -> CalibratedReading` | 不复用旧结果、不重复推断，给出未做读数滤波的 raw/N 与逐轴状态 | +| `apply_filter(measurement) -> ForceOutput` | 唯一后处理：raw 与 N 各自独立地 median → low-pass → 可关闭逐轴阈值 | +| `set_calibration(calibration, *, context=None)` | 帧边界原子换模型；校验失败保留原模型、基线与完成状态 | +| `zero`、`calibration_fingerprint`、`output_settings`、`last_output` | 只读状态/身份；`output_settings` 是实际生效参数 | + +## 帧身份 + +`FrameStamp(session_id, frame_id, source_frame_id, signal_time_s, time_basis)`；后处理低通的 +采样率来自信号时间，不取离线处理耗时。实时与顺序录像只替换源适配器，其余流程相同。 + +## 归零（空载基线） + +1. `request_zero(declared_unloaded=True, reference_id=...)` 只登记请求并递增参考代次； +2. 处理线程在下一个帧边界重置 tracker，然后才测量该帧； +3. 采集重置后连续 30 个有效处理帧，按模型类型得到基线：标量模型取 raw 中位数， + 场模型委托 `ModelCalibration` 用同一增益图加权后的 30 帧中位数； +4. 第 30 帧仍为基线帧；第 31 帧起 `phase="ready"`、`completed=True` 并输出 N， + `effective_from_frame_id` 指向该帧。 + +基线未完成、无效帧、窗口不足或已取消时 N 为 `NaN`、状态显式；窗口不足不越界补帧。 +录像重算用 `replay_window=(start, end)` 在声明区间执行同一操作。 + +## 后处理与有效设置 + +`OutputSettings(sample_rate_hz, median_window=1, lowpass_hz=0.0, lowpass_order=2, +raw_threshold=0.0, newton_threshold=0.0)`。每阶段由自身值关闭（median=1、lowpass=0、 +threshold=0），全关时对有效值严格直通（NaN 保留）；无效轴不进入滤波历史。 +`ForceOutput.settings` 返回实际生效的参数（例如截止频率达到 Nyquist 时解析为 off）。 + +## 离线重滤波 + +```python +from orisys.force_readout import process_output_sequence + +raw_out, newton_out, effective = process_output_sequence( + settings, raw_samples, newton_samples, + raw_valid=None, newton_valid=None, signal_times_s=times, +) +``` + +该函数只重放输出级（带完整 warm-up），用于从已存未滤波序列生成新参数下的最终输出； +不重新测量、不打开模型链。 + +## 状态与诊断 + +逐轴状态：`ok`、`out_of_range`、`zero_unavailable`、`no_contact`、`coordinate_unmapped`、 +`outside_coverage`、`invalid_input`、`uncalibrated`、`measurement_failed`、`frame_order`、 +`signal_time_order`。仅 `ok`/`out_of_range` 携带可用牛顿值；模型不输出的轴（例如 FN-only +模型的 FX/FY）恒为 `NaN` + `uncalibrated`。缺失值与有效零分开。 + +`CalibratedReading.diagnostics` 提供 `reference_id`、`baseline_N`、`baseline_raw_fn/fx/fy`、 +`model_fingerprint`、`mapping`、`extrapolated_fraction`、`coverage_fraction`、`area_scale`、 +`pending_reference_frames` 等；调用方存储或展示，不据此改写逐轴有效性。 + +场模型质量（外推、覆盖）与 downsample 差异属于诊断，不替代有效性判断； +`hhd_downsample` 不同是唯一允许的管线身份差异，其余差异仍拒绝并报告。 diff --git a/src/orisys/__init__.py b/src/orisys/__init__.py index f221596..c414946 100644 --- a/src/orisys/__init__.py +++ b/src/orisys/__init__.py @@ -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', diff --git a/src/orisys/calibration.py b/src/orisys/calibration.py index ad70906..9921a9c 100644 --- a/src/orisys/calibration.py +++ b/src/orisys/calibration.py @@ -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(): diff --git a/src/orisys/calibration_context.py b/src/orisys/calibration_context.py index ae4e089..cf10a68 100644 --- a/src/orisys/calibration_context.py +++ b/src/orisys/calibration_context.py @@ -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() diff --git a/src/orisys/calibration_models.py b/src/orisys/calibration_models.py index d8fb6c6..3c3bd55 100644 --- a/src/orisys/calibration_models.py +++ b/src/orisys/calibration_models.py @@ -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.""" diff --git a/src/orisys/finger/runtime.py b/src/orisys/finger/runtime.py index f68d398..3949999 100644 --- a/src/orisys/finger/runtime.py +++ b/src/orisys/finger/runtime.py @@ -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: diff --git a/src/orisys/force_readout.py b/src/orisys/force_readout.py index 0d3e1a2..2a8103b 100644 --- a/src/orisys/force_readout.py +++ b/src/orisys/force_readout.py @@ -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 diff --git a/src/orisys/force_session.py b/src/orisys/force_session.py new file mode 100644 index 0000000..8cd6167 --- /dev/null +++ b/src/orisys/force_session.py @@ -0,0 +1,1374 @@ +"""SDK-owned force readout session: measurement, unloaded zero, model, output. + +This is the ONE session entry for the rewrite. It owns the model call, the +unloaded-zero state machine, the baseline and the output post-processing. +Consumers (the App) only supply source frames and stamps, ask for a zero, and +distribute the returned output; they must not read the sensor's private force +buffers or assemble same-frame features themselves. + + source frame + FrameStamp + -> read_sdk_frame: unfiltered same-frame features (SdkReading) + -> update_zero: one unloaded-zero state machine (ZeroProgress) + -> apply_calibration: model output newtons, unfiltered (CalibratedReading) + -> apply_filter: median -> low-pass -> thresholds, raw and N apart + -> process_frame: one cached ForceOutput for display AND recording + +Ordering rules this module enforces: + +* **One zero is one operation.** ``request_zero`` bumps the reference + generation and schedules a tracker reset; the reset runs at the next frame + boundary *before* that frame is measured, and the baseline is the median of the + next 30 consecutive valid processed frames. The 30th frame is still a baseline + frame: ``completed`` turns true and newtons become available from the next + frame. There is no loaded relative tare. +* **One model inference per frame.** A schema 3 model is asked once per processed + frame; ``apply_calibration`` reuses that result instead of inferring again. + Repeated reads of the same frame return the cached sample and advance nothing. +* **Missing is not zero.** Unavailable raw/Newton axes are NaN with an explicit + per-axis status; a measured zero is ``0.0``. Nothing converts a failure into a + plausible zero. +* **The old entry points are untouched.** ``Sensor.read_info`` / + ``ForceReadout.apply`` / ``Calibration.convert`` keep their behaviour; this + module is only reached when a caller constructs a :class:`ForceSession` + (which explicitly enables the sensor's session measurement seam). +""" + +from __future__ import annotations + +import json +from collections.abc import Mapping +from dataclasses import dataclass, replace +from typing import Literal, Protocol + +import numpy as np + +from .calibration import CHANNEL_IDS, Calibration, CalibrationError, validate_for_device +from .calibration_context import CalibrationContext +from .calibration_models import CalibrationResult, ModelCalibration +from .field_calibration import FIELD_PROFILE, ZERO_FRAME_COUNT, FieldFrame, coerce_field +from .force_readout import ForceOutputFilter, OutputSettings +from .sensor import FrameMeasurement +from .signal_processing import DEFAULT_SAMPLE_RATE_HZ + +#: Stable identity of this workflow, recorded with every output and referenced by +#: caches. Change it when the processing order or semantics change. +WORKFLOW_VERSION = "studio-force/1" + +#: Products the session supports. ``finger-mems`` shares the finger finger bundle. +SENSOR_FAMILIES: tuple[str, ...] = ("box", "mini", "finger", "finger-mems") + +ForceTriplet = tuple[float, float, float] +AxisStatuses = tuple[str, str, str] +SensorFamily = Literal["box", "mini", "finger", "finger-mems"] +AxisStatus = Literal[ + "ok", + "out_of_range", + "zero_unavailable", + "no_contact", + "coordinate_unmapped", + "outside_coverage", + "invalid_input", + "uncalibrated", + "measurement_failed", + "frame_order", + "signal_time_order", +] + +NA = float("nan") + +#: Statuses that carry a usable Newton value. ``out_of_range`` is reported, not +#: discarded: the reading exists, it is just outside the declared force range. +OK_STATUSES = frozenset({"ok", "out_of_range"}) + +_MAX_FRAME_CACHE = 8 + + +def _frame_key(stamp: "FrameStamp") -> tuple[str, int]: + return (str(stamp.session_id), int(stamp.frame_id)) + + +def _finite(values) -> bool: + try: + return all(bool(np.isfinite(float(value))) for value in values) + except (TypeError, ValueError): + return False + + +def _status_tuple(status: str) -> AxisStatuses: + return (status, status, status) + + +def _bounded(cache: dict, key, value) -> None: + cache[key] = value + while len(cache) > _MAX_FRAME_CACHE: + cache.pop(next(iter(cache))) + + +def _diagnostic_value(value) -> str | float: + """One flat, JSON-safe diagnostic entry (App stores it, it does not parse it).""" + if isinstance(value, bool): + return "true" if value else "false" + if isinstance(value, (int, float)) and not isinstance(value, bool): + return float(value) if np.isfinite(float(value)) else str(value) + if isinstance(value, str): + return value + return json.dumps(value, sort_keys=True, default=str) + + +def _flat_diagnostics(values: Mapping | None) -> tuple[tuple[str, str | float], ...]: + if not isinstance(values, Mapping): + return () + return tuple( + (str(key), _diagnostic_value(value)) for key, value in sorted(values.items()) + ) + + +@dataclass(frozen=True) +class FrameStamp: + """One source identity and session-relative seconds, unchanged in all blocks.""" + + session_id: str + frame_id: int + source_frame_id: int | None + signal_time_s: float + time_basis: Literal["capture_monotonic", "recorded_signal", "video_pts"] + + +@dataclass(frozen=True) +class SdkReading: + """SDK -> calibration: same-frame, unfiltered force features and context. + + ``generation`` is the zero/reference generation this frame was measured in. + It is what makes late results from an invalidated request rejectable. + """ + + stamp: FrameStamp + sensor_family: SensorFamily + raw: ForceTriplet + raw_status: AxisStatuses + context: CalibrationContext | Mapping | None = None + field: FieldFrame | None = None + output_valid_mask: np.ndarray | None = None + contact_uv: tuple[float, float] | None = None + contact_found: bool | None = None + single_contact: bool | None = None + mapping_valid: bool = False + generation: int = 0 + diagnostics: tuple[tuple[str, str | float], ...] = () + + +@dataclass(frozen=True) +class ZeroProgress: + """Public progress only; the SDK model/session owns the actual baseline.""" + + reference_id: str | None + phase: Literal["not_ready", "resetting", "collecting", "ready", "failed"] + collected_frames: int = 0 + required_frames: int = ZERO_FRAME_COUNT + first_frame_id: int | None = None + last_frame_id: int | None = None + effective_from_frame_id: int | None = None + failure_reason: str = "" + + @property + def completed(self) -> bool: + """Completion has one source of truth; a reset request is not completion.""" + return self.phase == "ready" + + +@dataclass(frozen=True) +class CalibratedReading: + """Calibration -> filter AND calibration pane; N are still unfiltered.""" + + sdk: SdkReading + newton: ForceTriplet + newton_status: AxisStatuses + calibration_fingerprint: str + zero: ZeroProgress + diagnostics: tuple[tuple[str, str | float], ...] = () + + +@dataclass(frozen=True) +class ForceOutput: + """Filter -> display/recording: the SAME object and numerical values.""" + + measurement: CalibratedReading + raw: ForceTriplet + newton: ForceTriplet + settings: OutputSettings + workflow_version: str = WORKFLOW_VERSION + + +class MeasurementSource(Protocol): + """What the session needs from a measurement block (Sensor or FingerRuntime). + + ``read_measurement_frame`` runs image preparation, flow/HHD, and the optional + same-frame contact/field features once and returns them without model + inference or readout filtering. ``reset_tracker`` performs the frame-boundary + tracker reset and must be synchronous: the session only collects baseline + frames after it returns. + """ + + def read_measurement_frame( + self, + source_frame, + *, + frame_id: int, + timestamp_s: float | None, + want_field: bool = False, + want_contact: bool = False, + ) -> FrameMeasurement: # pragma: no cover - protocol declaration + ... + + def reset_tracker(self) -> None: # pragma: no cover - protocol declaration + ... + + +def _coerce_frame_stamp(stamp: FrameStamp) -> None: + if not isinstance(stamp, FrameStamp): + raise TypeError("stamp must be a FrameStamp") + if not isinstance(stamp.session_id, str) or not stamp.session_id: + raise ValueError("FrameStamp.session_id must be a non-empty string") + if type(stamp.frame_id) is not int: + raise ValueError("FrameStamp.frame_id must be an integer") + if stamp.source_frame_id is not None and type(stamp.source_frame_id) is not int: + raise ValueError("FrameStamp.source_frame_id must be an integer or None") + if not np.isfinite(float(stamp.signal_time_s)): + raise ValueError("FrameStamp.signal_time_s must be finite") + + +def _coerce_settings(settings: OutputSettings | None) -> OutputSettings: + if settings is None: + return OutputSettings(sample_rate_hz=DEFAULT_SAMPLE_RATE_HZ) + if not isinstance(settings, OutputSettings): + raise TypeError("settings must be an OutputSettings") + return settings.normalised() + + +def _coerce_replay_window( + window: tuple[int, int] | None, +) -> tuple[int, int] | None: + if window is None: + return None + if not isinstance(window, (tuple, list)) or len(window) != 2: + raise ValueError("replay_window must be an inclusive (start, end) pair") + start, end = window + if type(start) is not int or type(end) is not int: + raise ValueError("replay_window frame bounds must be integers") + if start < 0 or end < start: + raise ValueError("replay_window must satisfy 0 <= start <= end") + return (int(start), int(end)) + + +def _coerce_field(payload: Mapping | None) -> FieldFrame | None: + """Freeze the same-frame field before it can cross a thread boundary.""" + if payload is None: + return None + if isinstance(payload, FieldFrame): + frame = payload + else: + frame = coerce_field(payload) + d = np.array(frame.d, dtype=np.float32, copy=True) + mask = np.array(frame.valid_mask, dtype=bool, copy=True) + d.setflags(write=False) + mask.setflags(write=False) + return FieldFrame( + d=d, + valid_mask=mask, + area_scale=float(frame.area_scale), + grid_shape=tuple(frame.grid_shape), + reference_shape=( + None if frame.reference_shape is None else tuple(frame.reference_shape) + ), + ) + + +class ForceSession: + """One source session: measurement, unloaded zero, model call and output. + + Construct it with an already opened measurement source (``Sensor`` or + ``FingerRuntime``). Construction explicitly enables the source's session + measurement seam, so the old automatic model/readout path cannot run in + parallel with this session. The caller keeps runtime close ownership. + + ``calibration`` may be: + + * a schema 3 :class:`~orisys.calibration_models.ModelCalibration` — requires + the actual ``context`` (``CalibrationContext``); its internal field + baseline is delegated to, never recomputed here; + * a schema 1/2 :class:`~orisys.calibration.Calibration` — its curves are + applied in the force domain against the session baseline; + * ``None`` — explicit raw-only session: raw features are reported, newtons + stay unavailable with status ``uncalibrated``. + + The session is not thread-safe by design: one instance belongs to one + processing thread, and UI actions must be enqueued to that thread. Model, + zero and filter state all live here. + """ + + def __init__( + self, + *, + source: MeasurementSource, + sensor_family: SensorFamily, + device_id: str | None = None, + context: CalibrationContext | Mapping | None = None, + calibration: Calibration | ModelCalibration | None = None, + settings: OutputSettings | None = None, + ) -> None: + if sensor_family not in SENSOR_FAMILIES: + raise ValueError( + f"unsupported sensor_family: {sensor_family!r}; " + f"expected one of {SENSOR_FAMILIES}" + ) + for name in ("read_measurement_frame", "reset_tracker"): + if not callable(getattr(source, name, None)): + raise TypeError(f"measurement source must provide {name}()") + self._source = source + self._family: SensorFamily = sensor_family # type: ignore[assignment] + self._device_id = device_id + self._context = context + if self._context is None: + # A caller that does not pass the context explicitly may still have + # validated one on the source (FingerRuntime.open binds it). Adopting + # it keeps the constructor contract unchanged for explicit callers. + self._context = getattr(source, "calibration_context", None) + self._settings = _coerce_settings(settings) + self._filter = ForceOutputFilter(self._settings) + self._calibration: Calibration | ModelCalibration | None = None + self._calibration_fingerprint = "" + #: The model-vs-runtime pipeline report (downsample risk included); data + #: only, the validation decision was already made when the model was set. + self._calibration_compatibility: dict = {} + + self._generation = 0 + self._last_stamp: FrameStamp | None = None + + self._readings: dict[tuple[str, int], SdkReading] = {} + self._zero_by_frame: dict[tuple[int, str, int], ZeroProgress] = {} + self._model_results: dict[tuple[int, str, int], CalibrationResult] = {} + self._calibrated: dict[tuple[int, str, int], CalibratedReading] = {} + self._filtered: dict[tuple[int, str, int], ForceOutput] = {} + self._outputs: dict[tuple[str, int], ForceOutput] = {} + + self._zero_state: Literal[ + "not_ready", "resetting", "collecting", "ready", "failed" + ] = "not_ready" + self._zero = ZeroProgress( + reference_id=None, phase="not_ready", required_frames=ZERO_FRAME_COUNT + ) + self._pending_reset: dict | None = None + #: A queued zero whose tracker reset is performed by an external stage + #: (the App delegates it to ``StreamPipeline.reset``). The value is the + #: pipeline reset serial current when the request was made; the first + #: processed frame with a newer serial begins the baseline window. + self._pending_external_reset: int | None = None + self._zero_samples: list[tuple[float, float, float]] = [] + self._replay_window: tuple[int, int] | None = None + self._baseline_raw: tuple[float, float, float] | None = None + self._baseline_field: float | None = None + self._baseline_committed = False + self._ready_exposed = False + + selected = ( + calibration + if calibration is not None + else getattr(source, "calibration", None) + ) + if selected is not None: + self._install_calibration(selected, self._context) + + # Enable the source seam only after the session is fully usable: a + # rejected calibration must not leave the caller's sensor half-switched. + enable = getattr(source, "enable_measurement_session", None) + if callable(enable): + enable() + + # ----------------------------------------------------------- properties + @property + def workflow_version(self) -> str: + return WORKFLOW_VERSION + + @property + def sensor_family(self) -> SensorFamily: + return self._family + + @property + def calibration(self) -> Calibration | ModelCalibration | None: + return self._calibration + + @property + def calibration_fingerprint(self) -> str: + return self._calibration_fingerprint + + @property + def zero(self) -> ZeroProgress: + """Current unloaded-zero progress; one source of truth.""" + return self._zero + + @property + def output_settings(self) -> OutputSettings: + """The effective post-processing settings used for the newest output.""" + return self._filter.settings + + @property + def last_output(self) -> ForceOutput | None: + return next(reversed(self._outputs.values()), None) if self._outputs else None + + @property + def measurement_requirements(self) -> tuple[bool, bool]: + """``(want_field, want_contact)`` this session's model needs per frame. + + Additive read-only hint for a pipeline consumer: it lets the caller build + the same-frame :class:`FrameMeasurement` only with the features the model + actually asks for, without inspecting the model itself. + """ + return self._measurement_requirements() + + # ------------------------------------------------------------- calibration + def _install_calibration( + self, + calibration: Calibration | ModelCalibration | None, + context: CalibrationContext | Mapping | None, + ) -> None: + """Validate first, mutate second: a rejected model leaves the old one intact.""" + if calibration is None: + self._calibration = None + self._calibration_fingerprint = "" + self._calibration_compatibility = {} + return + schema = getattr(calibration, "schema_version", None) + if schema == 3: + if not isinstance(calibration, ModelCalibration): + raise CalibrationError("schema 3 calibration must parse into ModelCalibration") + if context is None: + raise CalibrationError( + "schema 3 calibration needs the actual pipeline context before use" + ) + try: + calibration.validate_for_context(context) + except (TypeError, ValueError) as exc: + raise CalibrationError(f"calibration context mismatch: {exc}") from exc + if not calibration.outputs: + raise CalibrationError("schema 3 model declares no force outputs") + if self._device_id is not None: + validate_for_device( + calibration, + self._family, + self._device_id, + pipeline=dict(context) if isinstance(context, Mapping) else None, + ) + fingerprint = str(calibration.fingerprint) + elif isinstance(calibration, Calibration): + if str(calibration.unit) != "N": + raise CalibrationError("session calibration unit must be N") + if not calibration.channels: + raise CalibrationError("session calibration has no usable channel curves") + identity_family = str(calibration.identity.get("sensor_type") or "") + if identity_family and identity_family != self._family: + raise CalibrationError( + f"calibration identity mismatch: {identity_family!r} != " + f"{self._family!r}" + ) + if self._device_id is not None: + validate_for_device(calibration, self._family, self._device_id) + fingerprint = str(calibration.fingerprint) + else: + raise CalibrationError( + f"unsupported calibration object: {type(calibration).__name__}" + ) + self._calibration = calibration + self._calibration_fingerprint = fingerprint + self._calibration_compatibility = self._compatibility_report(calibration, context) + + @staticmethod + def _compatibility_report(calibration, context) -> dict: + """The pipeline compatibility report, as data (an empty one when unknown). + + A model was already validated before it got here; this keeps the allowed + differences (the HHD downsample) available to the output diagnostics so + the App can show the accuracy risk and the record can state it. + """ + if not isinstance(calibration, ModelCalibration) or context is None: + return {} + try: + return dict(calibration.pipeline_compatibility(context)) + except Exception: # noqa: BLE001 - diagnostics must never stop a session + return {} + + def _compatibility_diagnostics(self) -> tuple[tuple[str, str | float], ...]: + """Flatten the downsample report into stable, record-friendly diagnostic keys.""" + report = dict(self._calibration_compatibility.get("downsample") or {}) + if not report: + return () + declared = report.get("declared") + actual = report.get("actual") + entries: list[tuple[str, str | float]] = [] + if declared is not None: + entries.append(("hhd_downsample_calibrated", float(declared))) + entries.append( + ( + "hhd_downsample_actual", + float(actual) if actual is not None else "unknown", + ) + ) + entries.append( + ( + "hhd_downsample_mismatch", + 0.0 if report.get("match") else 1.0, + ) + ) + entries.append( + ("downsample_accuracy_risk", str(report.get("accuracy_risk") or "")) + ) + return tuple(entries) + + @property + def calibration_compatibility(self) -> dict: + """The model-vs-runtime pipeline report the session was opened with.""" + return dict(self._calibration_compatibility) + + def set_calibration( + self, + calibration: Calibration | ModelCalibration | None, + *, + context: CalibrationContext | Mapping | None = None, + ) -> ZeroProgress: + """Replace the model atomically; a rejected model keeps the old state. + + A successful swap starts a new reference generation: the old baseline and + completion flag are gone and a new ``request_zero`` is required. + """ + selected_context = self._context if context is None else context + previous = self._calibration + self._install_calibration(calibration, selected_context) + self._context = selected_context + if isinstance(previous, ModelCalibration): + previous.reset(None) + return self.invalidate(reason="model changed") + + # ------------------------------------------------------------- frames + def read_sdk_frame(self, source_frame, stamp: FrameStamp) -> SdkReading: + """Measure one frame: same-frame unfiltered features, no model call. + + Applies a pending tracker reset *before* measuring (frame boundary), so + the first frame after a zero request is the first baseline frame. A + compute failure yields a reading with NaN raw and explicit statuses; the + previous frame's numbers are never reused. + """ + _coerce_frame_stamp(stamp) + key = _frame_key(stamp) + cached = self._readings.get(key) + if cached is not None: + return cached + self._apply_pending_reset() + want_field, want_contact = self._measurement_requirements() + try: + measurement = self._source.read_measurement_frame( + source_frame, + frame_id=int(stamp.frame_id), + timestamp_s=float(stamp.signal_time_s), + want_field=want_field, + want_contact=want_contact, + ) + except Exception as exc: # a failed frame must stay failed + row = FrameMeasurement( + raw=(NA, NA, NA), + raw_valid=(False, False, False), + diagnostics=(("error", str(exc) or type(exc).__name__),), + ) + reading = self._build_reading(stamp, row, measurement_failed=True) + _bounded(self._readings, key, reading) + self._last_stamp = stamp + return reading + reading = self._build_reading(stamp, measurement) + _bounded(self._readings, key, reading) + self._last_stamp = stamp + return reading + + def _measurement_requirements(self) -> tuple[bool, bool]: + model = self._calibration + if isinstance(model, ModelCalibration): + if model.input_profile == FIELD_PROFILE: + return (True, False) + if model.input_profile == "fn_delta_autocal_uv/1": + return (False, True) + return (False, False) + + def _build_reading( + self, stamp: FrameStamp, row: FrameMeasurement, *, measurement_failed: bool = False + ) -> SdkReading: + raw = tuple(float(value) for value in row.raw) + raw_valid = tuple(bool(flag) and np.isfinite(value) for flag, value in zip(row.raw_valid, raw)) + if measurement_failed: + raw_status = ("measurement_failed",) * 3 + else: + raw_status = tuple("ok" if flag else "invalid_input" for flag in raw_valid) + field = _coerce_field(row.field) + mask = row.output_valid_mask + if mask is not None: + mask = np.array(mask, dtype=bool, copy=True) + mask.setflags(write=False) + contact_uv, mapping_valid = self._contact_mapping(row) + contact_found = row.contact_found + if contact_found is None and row.centroid is not None: + contact_found = True + single_contact = row.single_contact + if single_contact is None and contact_found: + single_contact = True + diagnostics = tuple(row.diagnostics) + if field is None and self._measurement_requirements()[0]: + diagnostics += (("field_status", "missing"),) + return SdkReading( + stamp=stamp, + sensor_family=self._family, + raw=raw, + raw_status=raw_status, + context=self._context, + field=field, + output_valid_mask=mask, + contact_uv=contact_uv, + contact_found=contact_found, + single_contact=single_contact, + mapping_valid=mapping_valid, + generation=self._generation, + diagnostics=diagnostics, + ) + + def _contact_mapping( + self, row: FrameMeasurement + ) -> tuple[tuple[float, float] | None, bool]: + """Map the same-frame contact centroid into calibration (u, v), or fail it.""" + model = self._calibration + if not isinstance(model, ModelCalibration) or model.input_profile != "fn_delta_autocal_uv/1": + return (None, False) + if not row.contact_found or row.centroid is None: + return (None, False) + coordinate_map = getattr(model, "coordinate_map", None) + coordinate_mask = getattr(model, "coordinate_valid_mask", None) + if coordinate_map is None or coordinate_mask is None: + return (None, False) + try: + x, y = np.rint(np.asarray(row.centroid, dtype=np.float64)).astype(int) + except (TypeError, ValueError): + return (None, False) + if not (0 <= y < coordinate_mask.shape[0] and 0 <= x < coordinate_mask.shape[1]): + return (None, False) + if not bool(coordinate_mask[y, x]): + return (None, False) + uv = coordinate_map[y, x] + if not np.isfinite(uv).all(): + return (None, False) + return ((float(uv[0]), float(uv[1])), True) + + # --------------------------------------------------------------- zero + def request_zero( + self, + *, + reference_id: str, + declared_unloaded: bool, + replay_window: tuple[int, int] | None = None, + ) -> ZeroProgress: + """Start the ONE unloaded-zero operation (live or sequential replay). + + ``declared_unloaded`` is the operator's zero-load confirmation; without it + the request is refused. The next frame boundary resets the tracker, then + the following 30 consecutive valid processed frames form the baseline. + Only ``declared_unloaded`` is accepted — there is no loaded relative tare. + + The reference generation is bumped immediately, so any reading measured + before this request can never fill the new window. + """ + if not declared_unloaded: + raise ValueError( + "unloaded zero requires the operator to confirm there is no load" + ) + if not isinstance(reference_id, str) or not reference_id.strip(): + raise ValueError("reference_id must be a non-empty string") + window = _coerce_replay_window(replay_window) + self._generation += 1 + self._clear_frame_caches() + self._pending_reset = { + "reference_id": str(reference_id), + "replay_window": window, + } + self._pending_external_reset = None + self._zero_state = "resetting" + self._zero_samples = [] + self._replay_window = window + self._baseline_raw = None + self._baseline_field = None + self._baseline_committed = False + self._ready_exposed = False + self._drop_model_baseline() + self._filter.reset() + self._zero = ZeroProgress( + reference_id=str(reference_id), + phase="resetting", + required_frames=ZERO_FRAME_COUNT, + ) + return self._zero + + def _apply_pending_reset(self) -> None: + """The frame-boundary half of the zero operation: tracker reset, then window.""" + pending = self._pending_reset + if pending is None: + return + self._pending_reset = None + reference_id = str(pending["reference_id"]) + try: + self._source.reset_tracker() + model = self._calibration + if isinstance(model, ModelCalibration): + if model.model_type == "field_gain/1": + model.begin_reference(reference_id) + else: + model.reset(reference_id) + except Exception as exc: # the zero operation must fail explicitly + self._fail_zero(f"tracker reset failed: {exc}") + return + self._zero_samples = [] + self._zero_state = "collecting" + self._zero = replace( + self._zero, + phase="collecting", + collected_frames=0, + first_frame_id=None, + last_frame_id=None, + effective_from_frame_id=None, + failure_reason="", + ) + + def apply_pending_reset(self) -> ZeroProgress: + """Frame-boundary half of a session-owned zero, callable by a serial consumer. + + Additive seam: a caller that measured the current frame itself calls this + between two frames, before measuring the next one. The tracker reset then + happens at that boundary and the next measured frame is the first baseline + frame -- the same order :meth:`read_sdk_frame` enforces. It is a no-op when + no zero is queued. + """ + self._apply_pending_reset() + return self._zero + + def request_zero_external_reset( + self, + *, + reference_id: str, + declared_unloaded: bool, + replay_window: tuple[int, int] | None = None, + tracker_reset_seq: int, + ) -> ZeroProgress: + """Request the one unloaded zero when another stage performs the tracker reset. + + Additive variant of :meth:`request_zero` for a consumer that drives its + measurement through a concurrent pipeline: the caller queues the tracker + reset on the pipeline's flow stage (``StreamPipeline.reset``) and passes + the pipeline's reset serial at request time. Frames published with that + same serial were measured before the reset and are never baseline frames; + the first processed frame with a newer serial begins the 30-frame window. + Generation, the declared-unloaded requirement and the filter reset are + identical to :meth:`request_zero`. + """ + if not declared_unloaded: + raise ValueError( + "unloaded zero requires the operator to confirm there is no load" + ) + if not isinstance(reference_id, str) or not reference_id.strip(): + raise ValueError("reference_id must be a non-empty string") + if type(tracker_reset_seq) is not int or tracker_reset_seq < 0: + raise ValueError("tracker_reset_seq must be a non-negative integer") + window = _coerce_replay_window(replay_window) + self._generation += 1 + self._clear_frame_caches() + self._pending_reset = None + self._pending_external_reset = int(tracker_reset_seq) + self._zero_state = "resetting" + self._zero_samples = [] + self._replay_window = window + self._baseline_raw = None + self._baseline_field = None + self._baseline_committed = False + self._ready_exposed = False + self._drop_model_baseline() + self._filter.reset() + self._zero = ZeroProgress( + reference_id=str(reference_id), + phase="resetting", + required_frames=ZERO_FRAME_COUNT, + ) + return self._zero + + def _begin_external_collection(self, reference_id: str) -> None: + """Model-side frame-boundary half when the tracker reset was external.""" + model = self._calibration + try: + if isinstance(model, ModelCalibration): + if model.model_type == "field_gain/1": + model.begin_reference(reference_id) + else: + model.reset(reference_id) + except Exception as exc: # the zero operation must fail explicitly + self._fail_zero(f"external reset hand-off failed: {exc}") + return + self._zero_samples = [] + self._zero_state = "collecting" + self._zero = replace( + self._zero, + phase="collecting", + collected_frames=0, + first_frame_id=None, + last_frame_id=None, + effective_from_frame_id=None, + failure_reason="", + ) + + def update_zero(self, reading: SdkReading) -> ZeroProgress: + """Advance the zero state machine for this exact frame; idempotent. + + Also runs the session's once-per-frame schema 3 inference. A late result + from an earlier reference generation is ignored: it can neither fill the + new window nor restore ``completed``. + """ + if not isinstance(reading, SdkReading): + raise TypeError("reading must be an SdkReading") + key = (reading.generation, reading.stamp.session_id, reading.stamp.frame_id) + cached = self._zero_by_frame.get(key) + if cached is not None: + return cached + if reading.generation != self._generation: + # Late result from a superseded reference generation. + _bounded(self._zero_by_frame, key, self._zero) + return self._zero + if self._zero_state == "resetting": + _bounded(self._zero_by_frame, key, self._zero) + return self._zero + if self._baseline_committed and not self._ready_exposed: + self._expose_ready(reading) + self._predict_and_store(reading) + if self._zero_state == "collecting": + self._collect_zero(reading) + progress = self._zero + _bounded(self._zero_by_frame, key, progress) + return progress + + def _expose_ready(self, reading: SdkReading) -> None: + """The baseline is confirmed: N readiness starts on this frame.""" + self._zero_state = "ready" + self._ready_exposed = True + # Newtons start here: the output filters begin from this first sample. + self._filter.reset() + self._zero = replace( + self._zero, + phase="ready", + effective_from_frame_id=int(reading.stamp.frame_id), + failure_reason="", + ) + + def _collect_zero(self, reading: SdkReading) -> None: + frame_id = int(reading.stamp.frame_id) + window = self._replay_window + if window is not None: + if frame_id > window[1]: + self._fail_zero( + f"declared replay window {window[0]}..{window[1]} ended before " + f"{ZERO_FRAME_COUNT} valid baseline frames" + ) + return + if frame_id < window[0]: + return + if not _finite(reading.raw) or any( + status != "ok" for status in reading.raw_status + ): + self._fail_zero("invalid input frame during the unloaded baseline window") + return + model = self._calibration + is_field = isinstance(model, ModelCalibration) and model.model_type == "field_gain/1" + if is_field: + result = self._model_results.get( + (reading.generation, reading.stamp.session_id, reading.stamp.frame_id) + ) + if result is None: + self._fail_zero("field model produced no result for a baseline frame") + return + bad = sorted( + status + for status in result.status.values() + if status in ("invalid_input", "coordinate_unmapped") + ) + if bad: + self._fail_zero( + f"the field model rejected an unloaded frame ({bad[0]})" + ) + return + # This frame is valid and belongs to the consecutive window. + self._zero_samples.append(tuple(reading.raw)) + collected = len(self._zero_samples) + first_frame = ( + frame_id if self._zero.first_frame_id is None else self._zero.first_frame_id + ) + self._zero = replace( + self._zero, + collected_frames=collected, + first_frame_id=first_frame, + last_frame_id=frame_id, + ) + baseline_ready = ( + is_field and model.field_baseline_N is not None + ) or (not is_field and collected >= ZERO_FRAME_COUNT) + if baseline_ready: + self._commit_baseline(reading) + + def _commit_baseline(self, reading: SdkReading) -> None: + """Record the unloaded baseline; completion is exposed on the NEXT frame.""" + model = self._calibration + if isinstance(model, ModelCalibration) and model.model_type == "field_gain/1": + baseline = model.field_baseline_N + if baseline is None: + self._fail_zero("field model did not commit its weighted baseline") + return + # Delegated: the model medians the 30 weighted sums through the same + # gain map. The session stores the reported value, never a second one. + self._baseline_field = float(baseline) + self._baseline_raw = None + else: + samples = np.asarray(self._zero_samples, dtype=np.float64) + if samples.shape[0] < ZERO_FRAME_COUNT: + self._fail_zero("baseline window is incomplete") + return + self._baseline_raw = tuple( + float(value) for value in np.median(samples, axis=0) + ) + self._baseline_field = None + self._baseline_committed = True + self._zero = replace( + self._zero, + collected_frames=ZERO_FRAME_COUNT, + last_frame_id=int(reading.stamp.frame_id), + ) + + def _fail_zero(self, reason: str) -> None: + collected = len(self._zero_samples) + self._zero_samples = [] + self._pending_external_reset = None + self._baseline_raw = None + self._baseline_field = None + self._baseline_committed = False + self._ready_exposed = False + self._replay_window = None + self._drop_model_baseline() + self._filter.reset() + self._zero_state = "failed" + self._zero = replace( + self._zero, + phase="failed", + collected_frames=collected, + failure_reason=str(reason), + ) + + def _drop_model_baseline(self) -> None: + model = self._calibration + if isinstance(model, ModelCalibration): + model.reset(None) + + def _reference_raw(self) -> dict[str, float]: + if self._baseline_raw is None: + return {} + return { + axis: value for axis, value in zip(CHANNEL_IDS, self._baseline_raw) + } + + def invalidate(self, *, reason: str) -> ZeroProgress: + """Void the reference generation: disconnect, cancel, model or context change. + + A late completion from the invalidated generation is rejected by + :meth:`update_zero`, and the output filter starts clean. + """ + if not isinstance(reason, str) or not reason.strip(): + raise ValueError("invalidate requires an explicit reason") + self._generation += 1 + self._pending_reset = None + self._pending_external_reset = None + self._clear_frame_caches() + self._zero_samples = [] + self._replay_window = None + self._baseline_raw = None + self._baseline_field = None + self._baseline_committed = False + self._ready_exposed = False + self._drop_model_baseline() + self._filter.reset() + self._zero_state = "failed" + self._zero = ZeroProgress( + reference_id=None, + phase="failed", + required_frames=ZERO_FRAME_COUNT, + failure_reason=str(reason), + ) + return self._zero + + # ------------------------------------------------------------ model call + def _predict_and_store(self, reading: SdkReading) -> None: + """One model inference for this frame (schema 3 only), cached by frame identity.""" + model = self._calibration + if not isinstance(model, ModelCalibration): + return + key = (reading.generation, reading.stamp.session_id, reading.stamp.frame_id) + if key in self._model_results: + return + try: + result = model.predict( + frame_id=int(reading.stamp.frame_id), + timestamp_s=float(reading.stamp.signal_time_s), + features=self._model_features(reading), + reference_id=self._zero.reference_id, + ) + except Exception as exc: # one failed frame, explicit status + result = CalibrationResult( + {axis: None for axis in model.outputs}, + {axis: "invalid_input" for axis in model.outputs}, + {"error": str(exc) or type(exc).__name__}, + ) + _bounded(self._model_results, key, result) + + def _model_features(self, reading: SdkReading) -> dict: + model = self._calibration + assert isinstance(model, ModelCalibration) + if model.model_type == "field_gain/1": + features: dict = {} + if reading.field is not None: + features["field"] = { + "d": reading.field.d, + "valid_mask": reading.field.valid_mask, + "area_scale": reading.field.area_scale, + "grid_shape": reading.field.grid_shape, + "reference_shape": reading.field.reference_shape, + } + mask = reading.output_valid_mask + if mask is None: + coordinate_map = getattr(model, "coordinate_map", None) + if coordinate_map is not None: + # Same fallback the old Sensor path used: the whole output + # grid is physically valid when the source declares no mask. + mask = np.ones(coordinate_map.shape[:2], dtype=bool) + if mask is not None: + features["output_valid_mask"] = mask + return features + reference = self._reference_raw() + deltas = { + axis: ( + float(reading.raw[index]) - float(reference[axis]) + if axis in reference + else NA + ) + for index, axis in enumerate(CHANNEL_IDS) + } + features = { + "delta_raw": deltas["fn"], + "force_delta_raw": deltas, + "delta_raw_by_axis": deltas, + "zero_valid": bool( + reference and all(axis in reference for axis in model.outputs) + ), + "mapping_valid": bool(reading.mapping_valid), + "single_contact": ( + True if reading.single_contact is None else bool(reading.single_contact) + ), + "u": None if reading.contact_uv is None else reading.contact_uv[0], + "v": None if reading.contact_uv is None else reading.contact_uv[1], + } + if reading.contact_found is not None: + features["contact_found"] = bool(reading.contact_found) + return features + + # --------------------------------------------------------- calibration + def apply_calibration( + self, reading: SdkReading, zero: ZeroProgress + ) -> CalibratedReading: + """Convert this frame's unfiltered features to newtons exactly once. + + The model result computed by :meth:`update_zero` is reused, never + recomputed. While the unloaded baseline is not confirmed, raw is kept and + every Newton axis stays NaN. Axes a model does not output (FN-only models) + stay NaN with status ``uncalibrated``. + """ + if not isinstance(reading, SdkReading): + raise TypeError("reading must be an SdkReading") + key = (reading.generation, reading.stamp.session_id, reading.stamp.frame_id) + cached = self._calibrated.get(key) + if cached is not None: + return cached + current = self._zero + use_zero = current if zero.reference_id != current.reference_id else zero + ready = bool( + use_zero.completed + and self._zero_state == "ready" + and reading.generation == self._generation + ) + model = self._calibration + newton: list[float] = [NA, NA, NA] + status: list[str] = ["invalid_input"] * 3 + diagnostics: list[tuple[str, str | float]] = list(reading.diagnostics) + fingerprint = self._calibration_fingerprint + if isinstance(model, ModelCalibration): + self._fill_model_newtons(reading, key, model, ready, newton, status) + result = self._model_results.get(key) + if result is not None: + diagnostics.extend(_flat_diagnostics(result.diagnostics)) + diagnostics.append(("reference_id", use_zero.reference_id or "")) + diagnostics.extend(self._compatibility_diagnostics()) + if self._baseline_field is not None: + diagnostics.append(("baseline_N", float(self._baseline_field))) + elif isinstance(model, Calibration): + self._fill_curve_newtons(reading, model, ready, newton, status) + if ready and self._baseline_raw is not None: + diagnostics.extend( + (f"baseline_raw_{axis}", float(value)) + for axis, value in zip(CHANNEL_IDS, self._baseline_raw) + ) + else: + for index in range(3): + status[index] = "uncalibrated" + measurement = CalibratedReading( + sdk=reading, + newton=(newton[0], newton[1], newton[2]), + newton_status=(status[0], status[1], status[2]), + calibration_fingerprint=fingerprint, + zero=use_zero, + diagnostics=tuple(diagnostics), + ) + _bounded(self._calibrated, key, measurement) + return measurement + + def _fill_model_newtons( + self, + reading: SdkReading, + key: tuple[int, str, int], + model: ModelCalibration, + ready: bool, + newton: list[float], + status: list[str], + ) -> None: + result = self._model_results.get(key) + for index, axis in enumerate(CHANNEL_IDS): + if axis not in model.outputs: + status[index] = "uncalibrated" + continue + if result is None: + status[index] = "zero_unavailable" if not ready else "invalid_input" + continue + value = result.values_N.get(axis) + axis_status = str(result.status.get(axis, "invalid_input")) + status[index] = axis_status + if value is None or axis_status not in OK_STATUSES: + newton[index] = NA + else: + converted = float(value) + newton[index] = converted if np.isfinite(converted) else NA + + def _fill_curve_newtons( + self, + reading: SdkReading, + model: Calibration, + ready: bool, + newton: list[float], + status: list[str], + ) -> None: + reference = self._reference_raw() if ready else None + values = model.convert_referenced(reading.raw, reference) + for index, axis in enumerate(CHANNEL_IDS): + if reading.raw_status[index] != "ok" or not np.isfinite(reading.raw[index]): + status[index] = "invalid_input" + continue + if axis not in model.channels: + status[index] = "uncalibrated" + continue + if not ready: + status[index] = "zero_unavailable" + continue + value = values[index] + if value is None or not np.isfinite(value): + status[index] = "invalid_input" + continue + newton[index] = float(value) + status[index] = "ok" + + # ------------------------------------------------------------ output + def apply_filter(self, measurement: CalibratedReading) -> ForceOutput: + """Post-process one calibrated frame exactly once (raw and N separately).""" + if not isinstance(measurement, CalibratedReading): + raise TypeError("measurement must be a CalibratedReading") + reading = measurement.sdk + key = (reading.generation, reading.stamp.session_id, reading.stamp.frame_id) + cached = self._filtered.get(key) + if cached is not None: + return cached + raw_valid = tuple( + status == "ok" and bool(np.isfinite(value)) + for status, value in zip(reading.raw_status, reading.raw) + ) + newton_valid = tuple( + status in OK_STATUSES and bool(np.isfinite(value)) + for status, value in zip(measurement.newton_status, measurement.newton) + ) + raw_out, newton_out = self._filter.process( + reading.raw, + measurement.newton, + raw_valid=raw_valid, + newton_valid=newton_valid, + signal_time_s=float(reading.stamp.signal_time_s), + ) + output = ForceOutput( + measurement=measurement, + raw=raw_out, + newton=newton_out, + settings=self._filter.settings, + workflow_version=WORKFLOW_VERSION, + ) + _bounded(self._filtered, key, output) + return output + + def process_frame(self, source_frame, stamp: FrameStamp) -> ForceOutput: + """The ONE frame entry: measure, zero, calibrate and filter exactly once. + + Repeated calls for the same ``(session_id, frame_id)`` return the cached + immutable output object and advance nothing. A frame that violates + identity or time order returns NaN values with an explicit status, and an + interrupted stream loses its completion flag instead of reusing old + numbers. + """ + _coerce_frame_stamp(stamp) + key = _frame_key(stamp) + cached = self._outputs.get(key) + if cached is not None: + return cached + problem = self._continuity_problem(stamp) + if problem is not None: + if problem == "session": + self.invalidate(reason="source session changed") + self._last_stamp = None + else: + self.invalidate(reason=problem) + output = self._invalid_output(stamp, problem) + self._cache_output(key, output) + return output + reading = self.read_sdk_frame(source_frame, stamp) + zero = self.update_zero(reading) + measurement = self.apply_calibration(reading, zero) + output = self.apply_filter(measurement) + self._cache_output(key, output) + return output + + def process_measurement( + self, + row: FrameMeasurement, + stamp: FrameStamp, + *, + tracker_reset_seq: int | None = None, + ) -> ForceOutput: + """The one frame entry when the caller already measured the frame. + + Additive seam for a concurrent pipeline: the HHD stage produced ``row`` + (same-frame, unfiltered features) and the consumer hands it here, so this + runs zero -> model -> output exactly once without a second + ``get_img``/flow/HHD. Identity rules, generation rejection, caching and the + once-per-frame model call are the same as :meth:`process_frame`. + + ``tracker_reset_seq`` is the publishing pipeline's reset serial + (:attr:`~orisys.pipeline.FrameSnapshot.tracker_reset_seq`). It is only + consulted by a zero requested through + :meth:`request_zero_external_reset`: frames with the recorded serial (or an + older one) were measured before the reset and are not baseline frames. + """ + if not isinstance(row, FrameMeasurement): + raise TypeError("row must be a FrameMeasurement") + _coerce_frame_stamp(stamp) + key = _frame_key(stamp) + cached = self._outputs.get(key) + if cached is not None: + return cached + problem = self._continuity_problem(stamp) + if problem is not None: + if problem == "session": + self.invalidate(reason="source session changed") + self._last_stamp = None + else: + self.invalidate(reason=problem) + output = self._invalid_output(stamp, problem) + self._cache_output(key, output) + return output + normal_reset_after = self._pending_reset is not None + external = self._pending_external_reset + if external is not None: + if tracker_reset_seq is None: + raise ValueError( + "tracker_reset_seq is required while an external-reset zero is queued" + ) + if int(tracker_reset_seq) <= external: + # Measured before the externally queued tracker reset: not a + # baseline frame, and the session-owned reset path is not involved. + normal_reset_after = False + else: + self._pending_external_reset = None + self._begin_external_collection(str(self._zero.reference_id or "")) + reading = self._build_reading(stamp, row) + _bounded(self._readings, key, reading) + self._last_stamp = stamp + zero = self.update_zero(reading) + measurement = self.apply_calibration(reading, zero) + output = self.apply_filter(measurement) + if normal_reset_after: + # This row was measured before the queued reset; the reset belongs to + # this frame boundary, so the NEXT measured frame is baseline frame 1. + self._apply_pending_reset() + self._cache_output(key, output) + return output + + def _cache_output(self, key: tuple[str, int], output: ForceOutput) -> None: + _bounded(self._outputs, key, output) + + def _continuity_problem(self, stamp: FrameStamp) -> str | None: + last = self._last_stamp + if last is None: + return None + if stamp.session_id != last.session_id: + return "session" + if int(stamp.frame_id) < int(last.frame_id): + return "frame_order" + if float(stamp.signal_time_s) <= float(last.signal_time_s): + return "signal_time_order" + return None + + def _invalid_output(self, stamp: FrameStamp, reason: str) -> ForceOutput: + reading = SdkReading( + stamp=stamp, + sensor_family=self._family, + raw=(NA, NA, NA), + raw_status=_status_tuple(reason), + context=self._context, + generation=self._generation, + diagnostics=(("reason", reason),), + ) + measurement = CalibratedReading( + sdk=reading, + newton=(NA, NA, NA), + newton_status=_status_tuple(reason), + calibration_fingerprint=self._calibration_fingerprint, + zero=self._zero, + diagnostics=(("reason", reason),), + ) + return ForceOutput( + measurement=measurement, + raw=(NA, NA, NA), + newton=(NA, NA, NA), + settings=self._filter.settings, + workflow_version=WORKFLOW_VERSION, + ) + + def _clear_frame_caches(self) -> None: + self._readings.clear() + self._zero_by_frame.clear() + self._model_results.clear() + self._calibrated.clear() + self._filtered.clear() + self._outputs.clear() diff --git a/src/orisys/pipeline.py b/src/orisys/pipeline.py index e1faad8..e17c694 100644 --- a/src/orisys/pipeline.py +++ b/src/orisys/pipeline.py @@ -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: diff --git a/src/orisys/sensor.py b/src/orisys/sensor.py index 7e1cbc8..c223e2e 100644 --- a/src/orisys/sensor.py +++ b/src/orisys/sensor.py @@ -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() diff --git a/src/orisys/signal_processing.py b/src/orisys/signal_processing.py index 28b748f..a96622c 100644 --- a/src/orisys/signal_processing.py +++ b/src/orisys/signal_processing.py @@ -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. diff --git a/tests/test_calibration.py b/tests/test_calibration.py index 1515e49..50544f0 100644 --- a/tests/test_calibration.py +++ b/tests/test_calibration.py @@ -628,3 +628,85 @@ def test_every_preset_reports_what_it_does( assert fp["measurement"]["flow_ema_alpha"] == pytest.approx(alpha) assert fp["readout"]["force_median_window"] == median assert fp["readout"]["force_lowpass_cutoff_hz"] == pytest.approx(cutoff) + + +# ------------------------------------------------- session reference adapter +def _nonlinear_scalar_calibration(schema_version: int = 2): + return parse( + { + "schema_version": schema_version, + "unit": "N", + "channels": { + "normal": { + "method": "piecewise_linear", + "points": [[0.0, 0.0], [100.0, 1.0], [200.0, 3.0]], + "extrapolate": True, + "clamp_output_min": None, + }, + "shear_x": { + "method": "piecewise_linear", + "signed": True, + "input": "components", + "points": [[0.0, 0.0], [100.0, 2.0]], + "extrapolate": True, + "clamp_output_min": None, + }, + }, + } + ) + + +@pytest.mark.parametrize("schema_version", [1, 2]) +def test_convert_referenced_subtracts_the_session_baseline_once(schema_version: int): + calibration = _nonlinear_scalar_calibration(schema_version) + curve = calibration.get("fn") + assert curve is not None + reference = {"fn": 150.0} + converted = calibration.convert_referenced((250.0, 0.0, 0.0), reference) + # curve(250) - curve(150) = 4.0 - 2.0: one subtraction, and the curve's own + # offset/zero point is not removed a second time. + assert converted[0] == pytest.approx(4.0 - 2.0) + assert converted[0] != pytest.approx(4.0 - 2.0 * 2.0) + # A raw-domain delta would convert 250-150 = 100 (1.0) instead of 4.0-2.0. + assert converted[0] != pytest.approx(1.0) + # The historical entry point is unchanged. + assert calibration.convert(250.0, 0.0, 0.0)[0] == pytest.approx(4.0) + + +def test_convert_referenced_keeps_signed_shear_and_missing_axes_unavailable(): + calibration = _nonlinear_scalar_calibration() + converted = calibration.convert_referenced( + (250.0, -50.0, 12.0), {"fn": 150.0, "fx": 10.0} + ) + assert converted[0] == pytest.approx(2.0) + # signed curve: convert_component(-50) - convert_component(10) = -1.0 - 0.2. + assert converted[1] == pytest.approx(-1.2) + # FY has no curve, no reference: unavailable, never a zero. + assert converted[2] is None + missing_reference = calibration.convert_referenced((250.0, 0.0, 0.0), None) + assert missing_reference == (None, None, None) + invalid_raw = calibration.convert_referenced( + (float("nan"), 0.0, 0.0), {"fn": 150.0} + ) + assert invalid_raw[0] is None + + +def test_legacy_fingerprint_is_content_identity(): + calibration = _nonlinear_scalar_calibration() + assert len(calibration.fingerprint) == 64 + assert calibration.fingerprint == _nonlinear_scalar_calibration().fingerprint + changed = parse( + { + "schema_version": 2, + "unit": "N", + "channels": { + "normal": { + "method": "piecewise_linear", + "points": [[0.0, 0.0], [100.0, 1.0], [200.0, 4.0]], + "extrapolate": True, + "clamp_output_min": None, + } + }, + } + ) + assert changed.fingerprint != calibration.fingerprint diff --git a/tests/test_force_diagnostics.py b/tests/test_force_diagnostics.py new file mode 100644 index 0000000..d522705 --- /dev/null +++ b/tests/test_force_diagnostics.py @@ -0,0 +1,219 @@ +"""HHD downsample compatibility diagnostics and non-divisible grid integration. + +The pipeline contract allows a model calibrated at one ``hhd_downsample`` to run at +another, but the difference is an accuracy risk that must be visible instead of a +silent pass. These tests pin the report, the unchanged refusal of every other +identity mismatch, and the analytic block integration on grids that do not divide +the output image evenly. +""" + +from __future__ import annotations + +import numpy as np +import pytest + +from orisys.calibration import CalibrationError, parse +from orisys.calibration_context import build_context +from orisys.calibration_models import ModelCalibration +from orisys.field_calibration import FieldFrame, _block_starts, build_field_gain_map, reduce_field +from orisys.force_readout import OutputSettings +from orisys.force_session import ForceSession, FrameStamp +from orisys.sensor import FrameMeasurement + +FRAME = np.zeros((2, 2, 3), dtype=np.uint8) + + +def _config(downsample=1, *, dis_mode=4): + return { + "camera": {"resolution_raw": [1280, 720], "resolution_stitch": [700, 750]}, + "tracker": {"dis_mode": dis_mode, "hhd_downsample": downsample}, + } + + +def _document(config) -> dict: + return { + "schema_version": 3, + "unit": "N", + "identity": {"sensor_type": "box", "sn": "A1"}, + "pipeline": dict(build_context(config)), + "model": { + "type": "global_gain/1", + "input_profile": "force_delta/1", + "output_frame": "sdk_force/1", + "outputs": ["fn"], + "params": { + "gains_N_per_raw": {"fn": 0.001}, + "valid_force_range_N": [0, 3], + }, + }, + } + + +def _model(config) -> ModelCalibration: + loaded = parse(_document(config)) + assert isinstance(loaded, ModelCalibration) + return loaded + + +# ------------------------------------------------------------------ the report +def test_downsample_only_difference_runs_and_is_reported(): + model = _model(_config(2)) + runtime = build_context(_config(3)) + + model.validate_for_context(runtime) # allowed: no exception + + report = model.pipeline_compatibility(runtime) + assert report["ok"] is True + downsample = report["downsample"] + assert downsample["declared"] == 2 + assert downsample["actual"] == 3 + assert downsample["match"] is False + assert downsample["allowed"] is True + assert downsample["accuracy_risk"] + assert model.downsample_report(runtime).known is True + + +def test_a_matching_downsample_has_no_accuracy_risk(): + model = _model(_config(3)) + report = model.pipeline_compatibility(build_context(_config(3))) + assert report["downsample"]["match"] is True + assert report["downsample"]["accuracy_risk"] == "" + + +def test_an_unknown_runtime_downsample_is_unknown_not_a_match(): + model = _model(_config(2)) + report = model.downsample_report({}) + assert report.actual is None + assert report.match is False + assert report.allowed is True + assert "unknown" in report.accuracy_risk + + +def test_every_other_identity_mismatch_is_still_refused(): + model = _model(_config(2)) + runtime = build_context(_config(2, dis_mode=5)) + report = model.pipeline_compatibility(runtime) + assert report["ok"] is False + assert "calibration pipeline mismatch: dis_mode" in report["mismatches"].values() + with pytest.raises(CalibrationError, match="calibration pipeline mismatch: dis_mode"): + model.validate_for_context(runtime) + + +@pytest.mark.parametrize("raw, expected", [(None, 1), ("bad", 1), (0, 1), (-3, 1), ("3", 3), (3.7, 3)]) +def test_context_reports_its_actual_downsample(raw, expected): + config = _config(1) + config["tracker"]["hhd_downsample"] = raw + assert build_context(config).actual_downsample == expected + + +# ------------------------------------------------- non-divisible grid integration +def _identity_coordinates(rows: int, cols: int) -> np.ndarray: + uv = np.empty((rows, cols, 2), dtype=np.float64) + uv[..., 0] = np.arange(cols)[None, :] + uv[..., 1] = np.arange(rows)[:, None] + return uv + + +@pytest.mark.parametrize("grid_shape", [(1, 1), (2, 3), (3, 4), (4, 4)]) +def test_a_constant_field_integrates_exactly_on_a_non_divisible_grid(grid_shape): + rows, cols = 37, 53 + uv = _identity_coordinates(rows, cols) + mask = np.ones((rows, cols), dtype=bool) + points = np.array( + [ + [0.0, 0.0], + [cols - 1.0, 0.0], + [cols - 1.0, rows - 1.0], + [0.0, rows - 1.0], + ] + ) + gains = np.ones(4, dtype=np.float64) + triangles = np.array([[0, 1, 2], [0, 2, 3]], dtype=int) + + # The block layout is an exact partition of the output image, for every grid + # size that does not divide it: no lost pixels and no double counting. + for divisions, size in ((grid_shape[0], rows), (grid_shape[1], cols)): + starts, ends = _block_starts(divisions, size) + assert starts[0] == 0 + assert ends[-1] == size + assert np.array_equal(starts[1:], ends[:-1]) + assert np.all(ends > starts) + + gain_map = build_field_gain_map(points, gains, triangles, uv, mask, grid_shape, mask) + row_starts, row_ends = _block_starts(grid_shape[0], rows) + col_starts, col_ends = _block_starts(grid_shape[1], cols) + block_area = np.outer(row_ends - row_starts, col_ends - col_starts) + # A constant unit gain and a fully mapped mask: the area-weighted coverage is + # the whole image, exactly (analytic area, not a sampled sum). + assert float(np.sum(gain_map.coverage * block_area)) == pytest.approx(rows * cols) + assert np.allclose(gain_map.gain, 1.0) + assert np.allclose(gain_map.inside, 1.0) + + field = FieldFrame( + d=np.ones((grid_shape[0], grid_shape[1], 2), dtype=np.float32), + valid_mask=np.ones(grid_shape, dtype=bool), + area_scale=1.0, + grid_shape=grid_shape, + reference_shape=None, + ) + reduction = reduce_field(field, gain_map) + # q = |d_x| + |d_y| = 2 per active HHD cell; the gain map's block sums are exact + # whatever the block boundaries are, so the integral is exactly 2 * grid cells. + cells = grid_shape[0] * grid_shape[1] + assert reduction.weighted_N == pytest.approx(2.0 * cells) + assert reduction.extrapolated_fraction == 0.0 + assert reduction.coverage_fraction == pytest.approx(1.0) + + +# ------------------------------------------------------- session output diagnostics +class _Source: + """The measurement-source surface ForceSession needs; no camera involved.""" + + def __init__(self) -> None: + self.session_enabled = False + self.reset_count = 0 + + def enable_measurement_session(self) -> None: + self.session_enabled = True + + def disable_measurement_session(self) -> None: + self.session_enabled = False + + def reset_tracker(self) -> None: + self.reset_count += 1 + + def read_measurement_frame( + self, source_frame, *, frame_id, timestamp_s=None, want_field=False, want_contact=False + ) -> FrameMeasurement: + del source_frame, timestamp_s, want_field, want_contact + return FrameMeasurement( + raw=(10.0, 20.0, 30.0), + raw_valid=(True, True, True), + ) + + +def test_the_session_output_states_the_downsample_risk(): + model = _model(_config(2)) + session = ForceSession( + source=_Source(), + sensor_family="box", + context=build_context(_config(3)), + calibration=model, + settings=OutputSettings(sample_rate_hz=30.0), + ) + output = session.process_frame( + FRAME, + FrameStamp( + session_id="s1", + frame_id=0, + source_frame_id=0, + signal_time_s=0.0, + time_basis="recorded_signal", + ), + ) + diagnostics = dict(output.measurement.diagnostics) + assert diagnostics["hhd_downsample_calibrated"] == 2.0 + assert diagnostics["hhd_downsample_actual"] == 3.0 + assert diagnostics["hhd_downsample_mismatch"] == 1.0 + assert diagnostics["downsample_accuracy_risk"] + assert session.calibration_compatibility["downsample"]["actual"] == 3 diff --git a/tests/test_force_session.py b/tests/test_force_session.py new file mode 100644 index 0000000..d541fce --- /dev/null +++ b/tests/test_force_session.py @@ -0,0 +1,1212 @@ +"""SDK force session: fake-input chain, zero lifecycle, models and output chain. + +The measurement block is injected (deterministic fake features / fields), so +these tests need no camera and no hardware. Real calibration documents are used +where the model semantics matter: the shipped box file for schema 2, minimal +valid schema 3 documents built from ``build_context`` for the gain models. +""" + +from __future__ import annotations + +import hashlib + +import numpy as np +import pytest + +from orisys import calibration as cal_mod +from orisys.calibration import CalibrationError +from orisys.calibration_context import build_context +from orisys.calibration_models import parse_model +from orisys.field_calibration import ( + BASELINE_POLICY, + FIELD_IDENTITY, + MAPPING_POLICY, +) +from orisys.force_readout import OutputSettings, process_output_sequence +from orisys.force_session import ( + WORKFLOW_VERSION, + ForceOutput, + ForceSession, + FrameStamp, +) +from orisys.sensor import FrameMeasurement +from orisys.signal_processing import LowpassFilter + +FRAME_S = 1.0 / 30.0 + + +# --------------------------------------------------------------------------- +# Deterministic injected measurement source +# --------------------------------------------------------------------------- +class FakeMeasurementSource: + """Injected measurement/feature source: no camera, no image processing.""" + + def __init__( + self, + raw_for=None, + *, + field_for=None, + centroid_for=None, + output_mask=None, + fail_frames=(), + ): + self._raw_for = raw_for or (lambda frame_id: (10.0 + frame_id, 20.0, 30.0)) + self._field_for = field_for + self._centroid_for = centroid_for + self.output_mask = output_mask + self.fail_frames = set(fail_frames) + self.events: list[tuple[str, int | None]] = [] + self.calls: list[tuple[int, bool, bool]] = [] + self.session_enabled = False + self.reset_count = 0 + + def enable_measurement_session(self) -> None: + self.session_enabled = True + + def disable_measurement_session(self) -> None: + self.session_enabled = False + + def reset_tracker(self) -> None: + self.reset_count += 1 + self.events.append(("reset", None)) + + def read_measurement_frame( + self, + source_frame, + *, + frame_id, + timestamp_s=None, + want_field=False, + want_contact=False, + ) -> FrameMeasurement: + del source_frame, timestamp_s + self.calls.append((frame_id, want_field, want_contact)) + if frame_id in self.fail_frames: + self.events.append(("measure_failed", frame_id)) + raise RuntimeError(f"compute failed on frame {frame_id}") + raw = tuple(float(value) for value in self._raw_for(frame_id)) + field = self._field_for(frame_id) if want_field else None + centroid = self._centroid_for(frame_id) if want_contact else None + self.events.append(("measure", frame_id)) + return FrameMeasurement( + raw=raw, + raw_valid=tuple(bool(np.isfinite(value)) for value in raw), + field=field, + output_valid_mask=self.output_mask, + contact_found=(centroid is not None) if want_contact else None, + single_contact=True if (want_contact and centroid is not None) else None, + centroid=centroid, + diagnostics=(("frame_id", frame_id),), + ) + + +def stamp(frame_id: int, *, session_id: str = "s1", t: float | None = None) -> FrameStamp: + return FrameStamp( + session_id=session_id, + frame_id=frame_id, + source_frame_id=frame_id, + signal_time_s=FRAME_S * frame_id if t is None else t, + time_basis="recorded_signal", + ) + + +def image() -> np.ndarray: + return np.zeros((2, 2, 3), dtype=np.uint8) + + +_BOX_CALIBRATION = None + + +def box_cal(): + """The shipped schema 2 box document, loaded once (real file, no fake curves).""" + global _BOX_CALIBRATION + if _BOX_CALIBRATION is None: + loaded = cal_mod.load(sensor_type="box", strict=True) + assert loaded is not None + _BOX_CALIBRATION = loaded + return _BOX_CALIBRATION + + +def legacy_settings(**overrides) -> OutputSettings: + base = {"sample_rate_hz": 30.0} + base.update(overrides) + return OutputSettings(**base) + + +def make_session(source, calibration=None, *, context=None, settings=None, family="box"): + return ForceSession( + source=source, + sensor_family=family, + context=context, + calibration=calibration, + settings=settings if settings is not None else legacy_settings(), + ) + + +def process(session, source, frame_id, *, session_id="s1", t=None) -> ForceOutput: + return session.process_frame(image(), stamp(frame_id, session_id=session_id, t=t)) + + +def collect_baseline(session, source, *, start=1, count=30, reference_id="ref-1"): + session.request_zero(reference_id=reference_id, declared_unloaded=True) + return [ + process(session, source, frame_id) + for frame_id in range(start, start + count) + ] + + +# --------------------------------------------------------------------------- +# T1: executable fake chain, one model call, cached reads, missing is not zero +# --------------------------------------------------------------------------- +def test_fake_chain_runs_and_same_output_is_cached() -> None: + source = FakeMeasurementSource(lambda frame_id: (10000.0 + 100.0 * frame_id, 0.0, 0.0)) + session = make_session(source, box_cal()) + assert source.session_enabled, "constructing the session must enable the SDK seam" + assert session.workflow_version == WORKFLOW_VERSION + + outputs = collect_baseline(session, source) + assert len(outputs) == 30 + assert session.zero.phase == "collecting" + assert session.zero.completed is False + ready = process(session, source, 31) + assert session.zero.phase == "ready" + assert session.zero.completed is True + assert session.zero.first_frame_id == 1 + assert session.zero.last_frame_id == 30 + assert session.zero.effective_from_frame_id == 31 + assert ready.workflow_version == WORKFLOW_VERSION + + # Same frame again: the SAME immutable output, no new measurement. + repeat = process(session, source, 31) + assert repeat is ready + assert len(source.calls) == 31 + assert ready.raw == pytest.approx((13100.0, 0.0, 0.0)) + assert ready.newton[0] == pytest.approx(ready.measurement.newton[0]) + + +def test_one_model_call_per_frame_and_repeated_reads_do_not_advance() -> None: + context = build_context( + { + "camera": {"resolution_stitch": [4, 4]}, + "tracker": {"dis_mode": 1, "hhd_downsample": 1}, + } + ) + document = _global_gain_document(context, gains={"fn": 0.01}) + model = parse_model(document) + calls: list[dict] = [] + original_predict = model.predict + + def counting_predict(**kwargs): + calls.append(kwargs) + return original_predict(**kwargs) + + model.predict = counting_predict + source = FakeMeasurementSource(lambda frame_id: (100.0 + frame_id, 0.0, 0.0)) + session = ForceSession( + source=source, + sensor_family="box", + context=context, + calibration=model, + settings=legacy_settings(), + ) + session.request_zero(reference_id="ref", declared_unloaded=True) + for frame_id in range(1, 31): + process(session, source, frame_id) + assert len(calls) == 30, "one inference per processed frame, baseline included" + baseline_value = float(np.median([100.0 + frame_id for frame_id in range(1, 31)])) + ready = process(session, source, 31) + assert len(calls) == 31 + expected = (100.0 + 31 - baseline_value) * 0.01 + assert ready.newton[0] == pytest.approx(expected) + assert ready.measurement.newton_status[0] == "ok" + + calls_before = len(calls) + for _ in range(3): + assert process(session, source, 31) is ready + assert len(calls) == calls_before, "repeated reads must not infer again" + + reading = session.read_sdk_frame(image(), stamp(31)) + assert session.read_sdk_frame(image(), stamp(31)) is reading + assert len(source.calls) == 31, "repeated reads must not measure again" + + +def test_missing_axes_stay_nan_and_a_valid_zero_is_zero() -> None: + context = build_context( + { + "camera": {"resolution_stitch": [4, 4]}, + "tracker": {"dis_mode": 1, "hhd_downsample": 1}, + } + ) + document = _global_gain_document(context, gains={"fn": 0.01}) + source = FakeMeasurementSource(lambda frame_id: (200.0, 7.0, -7.0)) + session = ForceSession( + source=source, + sensor_family="box", + context=context, + calibration=parse_model(document), + settings=legacy_settings(), + ) + # Before any unloaded reference: raw is valid, N is unavailable (not zero). + first = process(session, source, 1) + assert np.isfinite(first.raw).all() + assert np.isnan(first.newton).all() + assert first.measurement.newton_status[0] == "zero_unavailable" + + collect_from = 2 + session.request_zero(reference_id="ref", declared_unloaded=True) + for frame_id in range(collect_from, collect_from + 30): + process(session, source, frame_id) + ready = process(session, source, collect_from + 30) + assert ready.measurement.zero.completed is True + # Unloaded reading: N is a measured zero, not a missing value. + assert ready.newton[0] == pytest.approx(0.0) + assert ready.measurement.newton_status[0] == "ok" + # FN-only model: FX/FY can never be filled in. + assert np.isnan(ready.newton[1]) and np.isnan(ready.newton[2]) + assert ready.measurement.newton_status[1:] == ("uncalibrated", "uncalibrated") + + +def test_failed_frame_never_reuses_the_previous_numbers() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0), fail_frames={5}) + session = make_session(source, box_cal()) + session.request_zero(reference_id="ref", declared_unloaded=True) + for frame_id in (1, 2, 3): + process(session, source, frame_id) + failed = process(session, source, 5) + assert np.isnan(failed.raw).all() + assert np.isnan(failed.newton).all() + assert failed.measurement.sdk.raw_status == ("measurement_failed",) * 3 + assert session.zero.phase == "failed", "an invalid baseline frame fails the window" + assert session.zero.completed is False + + +# --------------------------------------------------------------------------- +# T2: one unloaded-zero lifecycle for live and sequential replay +# --------------------------------------------------------------------------- +def test_twenty_nine_thirty_thirty_one_frames() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0)) + session = make_session(source, box_cal()) + session.request_zero(reference_id="ref", declared_unloaded=True) + for frame_id in range(1, 30): + output = process(session, source, frame_id) + assert np.isnan(output.newton).all() + assert session.zero.collected_frames == 29 + assert session.zero.completed is False + + thirtieth = process(session, source, 30) + assert session.zero.collected_frames == 30 + assert session.zero.phase == "collecting" + assert session.zero.completed is False, "the 30th frame is still a baseline frame" + assert np.isnan(thirtieth.newton).all() + assert thirtieth.measurement.newton_status[0] == "zero_unavailable" + + thirty_first = process(session, source, 31) + assert session.zero.completed is True + assert session.zero.effective_from_frame_id == 31 + assert thirty_first.measurement.newton_status[0] == "ok" + assert np.isfinite(thirty_first.newton[0]) + + +def test_reset_is_applied_before_measuring_the_first_baseline_frame() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0)) + session = make_session(source, box_cal()) + session.request_zero(reference_id="ref", declared_unloaded=True) + first = process(session, source, 7) + assert source.events[:2] == [("reset", None), ("measure", 7)] + assert source.reset_count == 1 + assert session.zero.first_frame_id == 7 + assert np.isnan(first.newton).all() + + # A second request while collecting is a NEW atomic operation. + session.request_zero(reference_id="ref-2", declared_unloaded=True) + assert session.zero.phase == "resetting" + assert session.zero.reference_id == "ref-2" + process(session, source, 8) + assert source.events[-2:] == [("reset", None), ("measure", 8)] + assert source.reset_count == 2 + assert session.zero.first_frame_id == 8 + assert session.zero.collected_frames == 1 + + +def test_invalid_frame_fails_the_window_and_a_new_request_restarts_it() -> None: + source = FakeMeasurementSource( + lambda frame_id: (100.0 if frame_id != 10 else float("nan"), 0.0, 0.0) + ) + session = make_session(source, box_cal()) + session.request_zero(reference_id="ref", declared_unloaded=True) + for frame_id in (1, 2, 3): + process(session, source, frame_id) + process(session, source, 10) + assert session.zero.phase == "failed" + assert session.zero.completed is False + assert "invalid input frame" in session.zero.failure_reason + for frame_id in (11, 12, 13): + output = process(session, source, frame_id) + assert np.isnan(output.newton).all() + assert session.zero.completed is False + + # A fresh request re-runs the whole operation from a confirmed reset. + source._raw_for = lambda frame_id: (100.0, 0.0, 0.0) + session.request_zero(reference_id="ref-2", declared_unloaded=True) + for frame_id in range(14, 44): + process(session, source, frame_id) + assert session.zero.collected_frames == 30 + ready = process(session, source, 44) + assert session.zero.completed is True + assert ready.measurement.newton_status[0] == "ok" + + +def test_cancel_and_late_results_cannot_restore_completion() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0)) + session = make_session(source, box_cal()) + stale = session.read_sdk_frame(image(), stamp(1)) + session.request_zero(reference_id="ref", declared_unloaded=True) + # A reading measured before the request can never fill the new window. + progress = session.update_zero(stale) + assert progress.collected_frames == 0 + assert progress.phase == "resetting" + + for frame_id in range(2, 32): + process(session, source, frame_id) + assert session.zero.collected_frames == 30 + assert session.zero.completed is False + process(session, source, 32) + assert session.zero.completed is True + + late = session.read_sdk_frame(image(), stamp(33)) + session.invalidate(reason="cancelled") + assert session.zero.completed is False + assert session.zero.phase == "failed" + assert session.zero.failure_reason == "cancelled" + progress = session.update_zero(late) + assert progress.completed is False + assert progress.phase == "failed" + for frame_id in (34, 35, 36): + output = process(session, source, frame_id) + assert np.isnan(output.newton).all() + assert session.zero.completed is False + + +def test_replay_window_is_enforced_and_live_and_replay_agree() -> None: + live_source = FakeMeasurementSource(lambda frame_id: (100.0 + frame_id, 0.0, 0.0)) + live = make_session(live_source, box_cal()) + live.request_zero(reference_id="live", declared_unloaded=True) + live_outputs = [process(live, live_source, frame_id) for frame_id in range(1, 32)] + + replay_source = FakeMeasurementSource(lambda frame_id: (100.0 + frame_id, 0.0, 0.0)) + replay = make_session(replay_source, box_cal()) + replay.request_zero( + reference_id="replay", declared_unloaded=True, replay_window=(1, 30) + ) + replay_outputs = [process(replay, replay_source, frame_id) for frame_id in range(1, 32)] + + assert [output.raw for output in live_outputs] == [ + output.raw for output in replay_outputs + ] + assert [output.newton for output in live_outputs] == [ + output.newton for output in replay_outputs + ] + assert live.zero.effective_from_frame_id == replay.zero.effective_from_frame_id + + short_source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0)) + short = make_session(short_source, box_cal()) + short.request_zero( + reference_id="short", declared_unloaded=True, replay_window=(1, 20) + ) + for frame_id in range(1, 21): + process(short, short_source, frame_id) + assert short.zero.phase == "collecting" + process(short, short_source, 21) + assert short.zero.phase == "failed" + assert "replay window" in short.zero.failure_reason + assert short.zero.completed is False + + +def test_zero_request_requires_the_unloaded_declaration_and_a_reference_id() -> None: + source = FakeMeasurementSource() + session = make_session(source, box_cal()) + with pytest.raises(ValueError, match="no load"): + session.request_zero(reference_id="ref", declared_unloaded=False) + with pytest.raises(ValueError, match="reference_id"): + session.request_zero(reference_id="", declared_unloaded=True) + with pytest.raises(ValueError, match="replay_window"): + session.request_zero( + reference_id="ref", declared_unloaded=True, replay_window=(5, 2) + ) + assert session.zero.phase == "not_ready" + + +def test_measurement_failure_after_completion_keeps_the_baseline() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0), fail_frames={33}) + session = make_session(source, box_cal()) + collect_baseline(session, source) + process(session, source, 31) + assert session.zero.completed is True + failed = process(session, source, 33) + assert np.isnan(failed.raw).all() + assert np.isnan(failed.newton).all() + assert session.zero.completed is True, "a failed frame is not a disconnect" + resumed = process(session, source, 34) + assert np.isfinite(resumed.raw).all() + assert resumed.measurement.newton_status[0] == "ok" + + +def test_model_swap_clears_completion_and_a_rejected_swap_keeps_state() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0)) + session = make_session(source, box_cal()) + collect_baseline(session, source) + process(session, source, 31) + assert session.zero.completed is True + + wrong = cal_mod.parse( + { + "schema_version": 2, + "unit": "N", + "channels": { + "normal": {"method": "linear", "slope": 1.0, "offset": 0.0} + }, + "meta": {"sensor_type": "mini", "sn": "OTHER"}, + } + ) + with pytest.raises(CalibrationError): + session.set_calibration(wrong) + assert session.calibration is box_cal() + assert session.zero.completed is True, "a rejected swap keeps model, baseline, state" + + session.set_calibration(box_cal()) + assert session.zero.completed is False + assert session.zero.phase == "failed" + assert session.zero.failure_reason == "model changed" + output = process(session, source, 32) + assert np.isnan(output.newton[0]) + + +def test_new_source_session_invalidates_the_reference_automatically() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0)) + session = make_session(source, box_cal()) + collect_baseline(session, source) + process(session, source, 31) + assert session.zero.completed is True + output = process(session, source, 1, session_id="s2") + assert session.zero.completed is False + assert np.isfinite(output.raw).all() + assert np.isnan(output.newton[0]) + + +def test_out_of_order_frame_is_explicit_and_clears_completion() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0)) + session = make_session(source, box_cal()) + collect_baseline(session, source) + process(session, source, 31) + assert session.zero.completed is True + late = process(session, source, 20) + assert np.isnan(late.raw).all() + assert late.measurement.sdk.raw_status == ("frame_order",) * 3 + assert session.zero.completed is False + # And the same time going backwards is a discontinuity too. + session.request_zero(reference_id="r2", declared_unloaded=True) + process(session, source, 40) + backwards = process(session, source, 41, t=0.5) + assert backwards.measurement.sdk.raw_status == ("signal_time_order",) * 3 + + +# --------------------------------------------------------------------------- +# T3: legacy scalar models subtract the session baseline exactly once +# --------------------------------------------------------------------------- +def _nonlinear_calibration(): + """A cross-breakpoint, deliberately nonlinear scalar document (schema 2).""" + return cal_mod.parse( + { + "schema_version": 2, + "unit": "N", + "channels": { + "normal": { + "method": "piecewise_linear", + "points": [[0.0, 0.0], [100.0, 1.0], [200.0, 3.0]], + "extrapolate": True, + "clamp_output_min": None, + }, + "shear_x": { + "method": "piecewise_linear", + "signed": True, + "input": "components", + "points": [[0.0, 0.0], [100.0, 2.0]], + "extrapolate": True, + "clamp_output_min": None, + }, + "shear_y": { + "method": "piecewise_linear", + "signed": True, + "input": "components", + "points": [[0.0, 0.0], [100.0, 4.0]], + "extrapolate": True, + "clamp_output_min": None, + }, + }, + "meta": {"sensor_type": "box", "sn": "T3-1"}, + } + ) + + +def test_legacy_baseline_is_subtracted_once_across_a_breakpoint() -> None: + calibration = _nonlinear_calibration() + baseline_raw = (150.0, 10.0, 10.0) + source = FakeMeasurementSource(lambda frame_id: baseline_raw) + session = make_session(source, calibration) + session.request_zero(reference_id="ref", declared_unloaded=True) + for frame_id in range(1, 31): + process(session, source, frame_id) + process(session, source, 31) + assert session.zero.completed is True + + loaded_raw = (250.0, 10.0, 10.0) + source._raw_for = lambda frame_id: loaded_raw + loaded = process(session, source, 32) + # curve(250) - curve(150) = 4.0 - 2.0 = 2.0. A raw-domain delta would give + # curve(100) = 1.0; a double subtraction would give curve(250)-2*curve(150)=0. + assert loaded.newton[0] == pytest.approx(2.0) + # The historical entry point keeps its own semantics. + assert calibration.convert(*loaded_raw)[0] == pytest.approx(4.0) + + +def test_legacy_zero_is_in_the_force_domain_and_negative_noise_survives() -> None: + calibration = _nonlinear_calibration() + source = FakeMeasurementSource(lambda frame_id: (150.0, 0.0, 0.0)) + session = make_session(source, calibration) + collect_baseline(session, source) + process(session, source, 31) + assert session.zero.completed is True + source._raw_for = lambda frame_id: (180.0, -50.0, 0.0) + output = process(session, source, 32) + # fn: curve(180) - curve(150) = 2.6 - 2.0 (one subtraction only). + assert output.newton[0] == pytest.approx(0.6) + # signed shear: curve(-50) - curve(0) = -1.0 (sign preserved, no clamp). + assert output.newton[1] == pytest.approx(-1.0) + assert output.newton[2] == pytest.approx(0.0) + + +def test_legacy_conversion_runs_before_output_filtering() -> None: + calibration = _nonlinear_calibration() + source = FakeMeasurementSource(lambda frame_id: (150.0, 0.0, 0.0)) + session = make_session( + source, + calibration, + settings=legacy_settings(lowpass_hz=5.0, lowpass_order=2), + ) + collect_baseline(session, source) + ready = process(session, source, 31) + assert ready.measurement.zero.completed is True + assert ready.measurement.newton[0] == pytest.approx(0.0) + + baseline_force = calibration.convert(150.0, 0.0, 0.0)[0] + raws = [150.0, 20.0, 400.0, 400.0] + converted = [ + calibration.convert(raw, 0.0, 0.0)[0] - baseline_force for raw in raws + ] + reference_filter = LowpassFilter( + cutoff_hz=5.0, sample_rate_hz=30.0, order=2, n_channels=1 + ) + expected = np.array( + [float(reference_filter.process([value])[0]) for value in converted] + ) + raw_strategy = LowpassFilter( + cutoff_hz=5.0, sample_rate_hz=30.0, order=2, n_channels=1 + ) + filtered_raw = [ + float(raw_strategy.process([raw])[0]) for raw in raws + ] + wrong = np.array( + [ + calibration.convert(raw, 0.0, 0.0)[0] - baseline_force + for raw in filtered_raw + ] + ) + actual = [ready.newton[0]] + for index, raw in enumerate(raws[1:], start=32): + source._raw_for = lambda frame_id, raw=raw: (raw, 0.0, 0.0) + actual.append(process(session, source, index).newton[0]) + assert np.allclose(actual, expected) + assert not np.allclose(actual, wrong), "raw must be converted before it is filtered" + + +def test_legacy_median_follows_the_converted_triplet() -> None: + calibration = _nonlinear_calibration() + source = FakeMeasurementSource(lambda frame_id: (150.0, 0.0, 0.0)) + session = make_session( + source, calibration, settings=legacy_settings(median_window=3) + ) + collect_baseline(session, source) + ready = process(session, source, 31) + baseline_force = calibration.convert(150.0, 0.0, 0.0)[0] + loaded = [(200.0, 0.0, 0.0), (300.0, 0.0, 0.0), (250.0, 0.0, 0.0)] + converted = [ + calibration.convert(*raw)[0] - baseline_force for raw in loaded + ] + latest = [] + for index, raw in enumerate(loaded, start=32): + source._raw_for = lambda frame_id, raw=raw: raw + latest.append(process(session, source, index).newton[0]) + # A causal median over the converted newtons, warmed up from the zero frame. + window = [ready.measurement.newton[0], *converted] + expected = [float(np.median(window[max(0, k - 2) : k + 1])) for k in range(len(window))] + assert latest == pytest.approx(expected[1:]) + + +# --------------------------------------------------------------------------- +# T5: shared output post-processing +# --------------------------------------------------------------------------- +def test_output_off_is_an_exact_passthrough_including_nan() -> None: + source = FakeMeasurementSource( + lambda frame_id: (100.0, 0.0, 0.0), fail_frames={33} + ) + session = make_session(source, box_cal(), settings=legacy_settings()) + collect_baseline(session, source) + ready = process(session, source, 31) + assert ready.settings.median_window == 1 + assert ready.settings.lowpass_hz == 0.0 + assert ready.settings.raw_threshold == 0.0 + assert ready.settings.newton_threshold == 0.0 + assert ready.raw == ready.measurement.sdk.raw + assert ready.newton == ready.measurement.newton + failed = process(session, source, 33) + assert np.isnan(failed.raw).all() + assert np.isnan(failed.newton).all() + + +def test_nan_axis_does_not_enter_the_filter_history() -> None: + chain = None + from orisys.signal_processing import MultiAxisCausalFilter + + chain = MultiAxisCausalFilter(median_window=3, n_channels=3) + first = chain.process((10.0, 20.0, 30.0)) + assert np.isfinite(first).all() + skipped = chain.process((float("nan"), 20.0, 30.0), valid=(False, True, True)) + assert np.isnan(skipped[0]) + assert skipped[1] == pytest.approx(20.0) + resumed = chain.process((10.0, 20.0, 30.0)) + # The NaN never entered the window: FN history is [10, 10], not [10, nan]. + assert resumed[0] == pytest.approx(10.0) + + +def test_thresholds_are_absolute_per_axis_and_do_not_touch_missing_axes() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 50.0, -50.0)) + session = make_session( + source, + box_cal(), + settings=legacy_settings(raw_threshold=51.0, newton_threshold=0.001), + ) + collect_baseline(session, source) + process(session, source, 31) + assert session.zero.completed is True + source._raw_for = lambda frame_id: (100.0, 50.0, -50.0) + output = process(session, source, 32) + # raw: |50| and |-50| are not above 51 -> suppressed to 0; FN stays. + assert output.raw == pytest.approx((100.0, 0.0, 0.0)) + # newton: |N| <= threshold -> 0 on the calibrated axes, NaN stays NaN. + assert output.newton[0] == pytest.approx(0.0) + assert output.newton[1] == pytest.approx(0.0) + assert np.isnan(output.newton[2]) or output.newton[2] == pytest.approx(0.0) + + +def test_effective_settings_report_a_cutoff_at_nyquist_as_off() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0)) + session = make_session( + source, + box_cal(), + settings=legacy_settings(sample_rate_hz=30.0, lowpass_hz=15.0), + ) + output = process(session, source, 1) + assert output.settings.lowpass_hz == 0.0 + assert output.settings.sample_rate_hz == pytest.approx(30.0) + assert output.raw == output.measurement.sdk.raw + + +def test_filter_sample_rate_follows_signal_time_not_processing_speed() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0)) + settings = legacy_settings(sample_rate_hz=30.0, lowpass_hz=5.0) + session = make_session(source, box_cal(), settings=settings) + for frame_id in range(1, 21): + process(session, source, frame_id, t=frame_id / 60.0) + assert session.output_settings.sample_rate_hz == pytest.approx(60.0, rel=0.05) + + +def test_offline_reprocessing_uses_the_same_chain_and_warm_up() -> None: + source = FakeMeasurementSource(lambda frame_id: (150.0 + 10.0 * (frame_id % 7), 0.0, 0.0)) + settings = legacy_settings(median_window=3, lowpass_hz=4.0, lowpass_order=2) + session = make_session(source, _nonlinear_calibration(), settings=settings) + collect_baseline(session, source) + ready_output = process(session, source, 31) + assert ready_output.measurement.zero.completed is True + + frames = list(range(31, 46)) + outputs = [ready_output, *(process(session, source, frame_id) for frame_id in frames[1:])] + unfiltered_raw = [output.measurement.sdk.raw for output in outputs] + unfiltered_newton = [output.measurement.newton for output in outputs] + signal_times = [frame * FRAME_S for frame in frames] + reprocessed_raw, reprocessed_newton, effective = process_output_sequence( + settings, + unfiltered_raw, + unfiltered_newton, + signal_times_s=signal_times, + ) + assert [output.raw for output in outputs] == reprocessed_raw + assert [output.newton for output in outputs] == reprocessed_newton + assert effective.sample_rate_hz == pytest.approx(30.0, rel=0.01) + + +def test_zero_completion_resets_the_output_filter_history() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0)) + session = make_session( + source, _nonlinear_calibration(), settings=legacy_settings(lowpass_hz=5.0) + ) + collect_baseline(session, source) + ready = process(session, source, 31) + assert ready.measurement.zero.completed is True + # A fresh IIR starts at its first sample: the first post-zero value is not + # dragged down by the baseline frames' zero newtons. + first_loaded = ready.newton[0] + assert np.isfinite(first_loaded) + clean = LowpassFilter(cutoff_hz=5.0, sample_rate_hz=30.0, order=2, n_channels=1) + expected = float( + clean.process([ready.measurement.newton[0]])[0] + ) + assert first_loaded == pytest.approx(expected) + + +# --------------------------------------------------------------------------- +# Minimal legal schema 3 documents for the global-gain tests +# --------------------------------------------------------------------------- +def _global_gain_document(context, *, gains, outputs=None): + axes = list(outputs or gains) + return { + "schema_version": 3, + "unit": "N", + "identity": {"sensor_type": "box", "sn": "TEST-1"}, + "pipeline": dict(context), + "model": { + "type": "global_gain/1", + "input_profile": "force_delta/1", + "output_frame": "sdk_force/1", + "outputs": axes, + "params": { + "gains_N_per_raw": {axis: float(gains[axis]) for axis in axes}, + "valid_force_range_N": [-100.0, 100.0], + }, + }, + } + + +# --------------------------------------------------------------------------- +# T4: schema 3 spatial gain and field models +# --------------------------------------------------------------------------- +_SQUARE_UV = [[0.0, 0.0], [3.0, 0.0], [0.0, 3.0], [3.0, 3.0]] +_SQUARE_TRIANGLES = [[0, 1, 2], [1, 3, 2]] + + +def _artifact(tmp_path, *, shape=(4, 4), uv=None, valid=None, name="map.npz"): + height, width = shape + if uv is None: + rows, cols = np.mgrid[0:height, 0:width] + uv = np.stack((cols, rows), axis=-1).astype(np.float64) + if valid is None: + valid = np.ones((height, width), dtype=bool) + path = tmp_path / name + np.savez(path, uv_by_output_px=uv, valid_mask=valid, origin_xy=np.array([0, 0])) + return path, hashlib.sha256(path.read_bytes()).hexdigest() + + +def _uv_context(*, field_identity=None, coordinates): + config = { + "type": "compound_eye", + "camera": {"resolution_raw": [4, 4], "resolution_stitch": [4, 4]}, + "tracker": {"dis_mode": 1, "hhd_downsample": 1}, + "filters": {"measurement": {"flow_ema_alpha": 1.0}}, + "contact": {}, + } + return build_context( + config, + config["filters"], + geometry_sha256="b" * 64, + source_scale=0.5, + coordinates=coordinates, + processing_backend="uv_unroll", + preprocessing={"flip_source_vertical": True, "mirror": False}, + field=field_identity, + ) + + +def _spatial_document(tmp_path, *, gains=None, coordinate_valid=None): + path, digest = _artifact(tmp_path, valid=coordinate_valid) + coordinates = { + "frame": "autocal_flat", + "unit": "px", + "origin_xy": [0, 0], + "map_sha256": digest, + } + context = _uv_context(coordinates=coordinates) + document = { + "schema_version": 3, + "unit": "N", + "identity": {"sensor_type": "finger", "sn": "T4-SPATIAL"}, + "pipeline": dict(context), + "model": { + "type": "spatial_gain/1", + "input_profile": "fn_delta_autocal_uv/1", + "output_frame": "sdk_force/1", + "outputs": ["fn"], + "params": { + "points_uv": _SQUARE_UV, + "gains_N_per_raw": gains or [0.001, 0.002, 0.003, 0.004], + "triangles": _SQUARE_TRIANGLES, + "valid_force_range_N": [-100.0, 100.0], + }, + "artifacts": [ + {"role": "coordinate_map", "path": path.name, "sha256": digest} + ], + }, + } + return document, context + + +def _field_document(tmp_path, *, gains=None, coordinate_valid=None): + path, digest = _artifact(tmp_path, valid=coordinate_valid) + coordinates = { + "frame": "autocal_flat", + "unit": "px", + "origin_xy": [0, 0], + "map_sha256": digest, + } + context = _uv_context(field_identity=FIELD_IDENTITY, coordinates=coordinates) + document = { + "schema_version": 3, + "unit": "N", + "identity": {"sensor_type": "finger", "sn": "T4-FIELD"}, + "pipeline": dict(context), + "model": { + "type": "field_gain/1", + "input_profile": "hhd_normal_l1_field/1", + "output_frame": "sdk_force/1", + "outputs": ["fn"], + "params": { + "points_uv": _SQUARE_UV, + "gains_N_per_raw": gains or [1.0, 1.0, 1.0, 1.0], + "triangles": _SQUARE_TRIANGLES, + "valid_force_range_N": [-100.0, 100.0], + "mapping": dict(MAPPING_POLICY), + "baseline": dict(BASELINE_POLICY), + }, + "artifacts": [ + {"role": "coordinate_map", "path": path.name, "sha256": digest} + ], + }, + } + return document, context + + +def _field_payload(dx=0.5, dy=0.5, *, shape=(2, 2), area_scale=4.0, mask=None): + data = np.zeros((*shape, 2), dtype=np.float32) + data[:, :, 0] = dx + data[:, :, 1] = dy + if mask is None: + mask = np.ones(shape, dtype=bool) + mask = np.asarray(mask, dtype=bool) + data[~mask] = 0.0 + return { + "d": data, + "valid_mask": mask, + "area_scale": float(area_scale), + "grid_shape": tuple(shape), + "reference_shape": (4, 4), + } + + +def _field_session(tmp_path, *, gains=None, coordinate_valid=None, settings=None): + document, context = _field_document( + tmp_path, gains=gains, coordinate_valid=coordinate_valid + ) + model = parse_model(document, tmp_path / "calibration.json") + calls: list[dict] = [] + original_predict = model.predict + + def counting_predict(**kwargs): + calls.append(kwargs) + return original_predict(**kwargs) + + model.predict = counting_predict + source = FakeMeasurementSource( + lambda frame_id: (0.0, 0.0, 0.0), + field_for=lambda frame_id: _field_payload(), + output_mask=np.ones((4, 4), dtype=bool), + ) + session = ForceSession( + source=source, + sensor_family="finger", + context=context, + calibration=model, + settings=settings if settings is not None else legacy_settings(), + ) + return session, source, model, calls + + +def _spatial_session(tmp_path): + coordinate_valid = np.ones((4, 4), dtype=bool) + coordinate_valid[0, 0] = False + document, context = _spatial_document( + tmp_path, coordinate_valid=coordinate_valid + ) + model = parse_model(document, tmp_path / "calibration.json") + calls: list[dict] = [] + original_predict = model.predict + + def counting_predict(**kwargs): + calls.append(kwargs) + return original_predict(**kwargs) + + model.predict = counting_predict + loaded = {"raw": 100.0, "centroid": (1.0, 1.0)} + source = FakeMeasurementSource( + lambda frame_id: (loaded["raw"], 0.0, 0.0), + centroid_for=lambda frame_id: loaded["centroid"], + ) + session = ForceSession( + source=source, + sensor_family="finger", + context=context, + calibration=model, + settings=legacy_settings(), + ) + return session, source, model, calls, loaded + + +def test_spatial_model_uses_contact_mapping_and_one_inference_per_frame(tmp_path): + session, source, model, calls, loaded = _spatial_session(tmp_path) + session.request_zero(reference_id="ref", declared_unloaded=True) + for frame_id in range(1, 31): + process(session, source, frame_id) + ready = process(session, source, 31) + assert session.zero.completed is True + assert len(calls) == 31, "exactly one inference per processed frame" + assert ready.measurement.newton_status[0] == "ok" + assert ready.newton[0] == pytest.approx(0.0) + + # Load: delta 100 raw at (1,1) -> barycentric gain (0.001+0.002+0.003)/3. + loaded["raw"] = 200.0 + loaded_frame = process(session, source, 32) + assert loaded_frame.newton[0] == pytest.approx(100.0 * 0.002) + assert np.isnan(loaded_frame.newton[1]) + assert loaded_frame.measurement.newton_status[1:] == ("uncalibrated", "uncalibrated") + assert len(calls) == 32 + + # A contact on an unmapped coordinate cannot become a newton value. + loaded["centroid"] = (0.0, 0.0) + unmapped = process(session, source, 33) + assert unmapped.measurement.newton_status[0] == "coordinate_unmapped" + assert np.isnan(unmapped.newton[0]) + assert np.isfinite(unmapped.raw).all() + + +def test_field_model_baseline_is_weighted_and_delegated(tmp_path): + session, source, model, calls = _field_session(tmp_path) + session.request_zero(reference_id="field-ref", declared_unloaded=True) + for frame_id in range(1, 31): + output = process(session, source, frame_id) + assert np.isnan(output.newton[0]) + assert model.field_baseline_N == pytest.approx(16.0) + ready = process(session, source, 31) + assert session.zero.completed is True + assert len(calls) == 31 + diagnostics = dict(ready.measurement.diagnostics) + assert diagnostics["baseline_N"] == pytest.approx(16.0) + assert "baseline_raw_fn" not in diagnostics, "the weighted field baseline is not scalar" + # The field model does not gate the unloaded window on contact location: the + # measurement block is never even asked for a contact feature. + assert all(not contact for _frame_id, _field, contact in source.calls) + # Unloaded: the weighted sum equals the baseline through the same gain map. + assert ready.newton[0] == pytest.approx(0.0) + assert ready.measurement.newton_status[0] == "ok" + + # A loaded frame: q=3 per cell, P=48, FN = 48-16 = 32. + source._field_for = lambda frame_id: _field_payload(1.5, 1.5) + loaded = process(session, source, 32) + assert loaded.newton[0] == pytest.approx(32.0) + assert len(calls) == 32 + # FN-only field model never fills shear axes. + assert np.isnan(loaded.newton[1]) and np.isnan(loaded.newton[2]) + assert loaded.measurement.newton_status[1:] == ("uncalibrated", "uncalibrated") + + +def test_field_model_mapping_error_is_explicit_and_does_not_fill_newtons(tmp_path): + session, source, model, calls = _field_session(tmp_path) + session.request_zero(reference_id="field-ref", declared_unloaded=True) + for frame_id in range(1, 31): + process(session, source, frame_id) + ready = process(session, source, 31) + assert ready.measurement.newton_status[0] == "ok" + source.output_mask = np.ones((3, 3), dtype=bool) + broken = process(session, source, 32) + assert np.isfinite(broken.raw).all(), "raw stays available on a failed frame" + assert np.isnan(broken.newton[0]) + assert broken.measurement.newton_status[0] == "invalid_input" + assert len(calls) == 32 + + +def test_scalar_and_field_baselines_are_separate(tmp_path): + scalar_source = FakeMeasurementSource(lambda frame_id: (17.0, 3.0, 5.0)) + scalar = make_session(scalar_source, _nonlinear_calibration()) + collect_baseline(scalar, scalar_source) + scalar_ready = process(scalar, scalar_source, 31) + scalar_diagnostics = dict(scalar_ready.measurement.diagnostics) + assert scalar_diagnostics["baseline_raw_fn"] == pytest.approx(17.0) + assert "baseline_N" not in scalar_diagnostics + + field_session, field_source, _model, _calls = _field_session(tmp_path) + field_session.request_zero(reference_id="field-ref", declared_unloaded=True) + for frame_id in range(1, 31): + process(field_session, field_source, frame_id) + field_ready = process(field_session, field_source, 31) + field_diagnostics = dict(field_ready.measurement.diagnostics) + assert field_diagnostics["baseline_N"] == pytest.approx(16.0) + assert "baseline_raw_fn" not in field_diagnostics + # The two baselines are different quantities: a scalar raw median is not a + # weighted field sum, and neither session may share the other's zero. + assert scalar.zero.reference_id != field_session.zero.reference_id + + +# --------------------------------------------------------------------------- +# T5 (continued): stage composition, independent raw/N states, reset triggers +# --------------------------------------------------------------------------- +def test_median_then_lowpass_then_threshold_composition() -> None: + settings = legacy_settings( + median_window=3, lowpass_hz=10.0, lowpass_order=2, raw_threshold=0.5 + ) + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0)) + session = make_session(source, _nonlinear_calibration(), settings=settings) + collect_baseline(session, source) + ready = process(session, source, 31) + assert ready.measurement.zero.completed is True + samples = [0.0, 4.0, 0.0, 4.0, 4.0] + raws = [(150.0 + sample, 0.0, 0.0) for sample in samples] + outputs = [] + for index, raw in enumerate(raws, start=32): + source._raw_for = lambda frame_id, raw=raw: raw + outputs.append(process(session, source, index).raw[0]) + from orisys.signal_processing import CausalMedianFilter + + median = CausalMedianFilter(window=3, n_channels=1) + lowpass = LowpassFilter(cutoff_hz=10.0, sample_rate_hz=30.0, order=2, n_channels=1) + expected = [] + # Warm the reference chain from the ready frame's sample, as the session did. + warmup = ready.measurement.sdk.raw[0] + med = float(median.process([warmup])[0]) + assert float(lowpass.process([med])[0]) == pytest.approx(warmup) + for raw in raws: + med = float(median.process([raw[0]])[0]) + smooth = float(lowpass.process([med])[0]) + expected.append(0.0 if abs(smooth) <= 0.5 else smooth) + assert outputs == pytest.approx(expected) + + +def test_raw_and_newton_states_are_independent() -> None: + source = FakeMeasurementSource(lambda frame_id: (100.0, 0.0, 0.0)) + session = make_session( + source, + _nonlinear_calibration(), + settings=legacy_settings(lowpass_hz=6.0), + ) + collect_baseline(session, source) + ready = process(session, source, 31) + assert ready.measurement.zero.completed is True + samples = [(160.0 + 5.0 * index, 0.0, 0.0) for index in range(6)] + outputs = [ready] + for index, raw in enumerate(samples, start=32): + source._raw_for = lambda frame_id, raw=raw: raw + outputs.append(process(session, source, index)) + raw_reference = LowpassFilter(cutoff_hz=6.0, sample_rate_hz=30.0, order=2, n_channels=1) + newton_reference = LowpassFilter( + cutoff_hz=6.0, sample_rate_hz=30.0, order=2, n_channels=1 + ) + expected_raw = [ + float(raw_reference.process([output.measurement.sdk.raw[0]])[0]) + for output in outputs + ] + expected_newton = [ + float(newton_reference.process([output.measurement.newton[0]])[0]) + for output in outputs + ] + assert [output.raw[0] for output in outputs] == pytest.approx(expected_raw) + assert [output.newton[0] for output in outputs] == pytest.approx(expected_newton) + # Both domains advance once per frame from their own valid samples only. + assert all(np.isfinite(output.raw[0]) for output in outputs) + assert all(np.isfinite(output.newton[0]) for output in outputs) + + +def test_invalid_newton_axis_stays_missing_under_active_filtering(tmp_path): + session, source, _model, _calls = _field_session( + tmp_path, settings=legacy_settings(lowpass_hz=5.0) + ) + session.request_zero(reference_id="field-ref", declared_unloaded=True) + for frame_id in range(1, 31): + process(session, source, frame_id) + ready = process(session, source, 31) + assert ready.measurement.zero.completed is True + loaded = process(session, source, 32) + assert np.isfinite(loaded.raw).all(), "raw is still filtered normally" + assert np.isnan(loaded.newton[1]), "an uncalibrated axis never becomes zero" + assert np.isnan(loaded.newton[2]) + + +def test_model_change_and_new_zero_reset_the_output_filter(tmp_path): + def settings(): + return legacy_settings(lowpass_hz=5.0, median_window=3) + + first_source = FakeMeasurementSource(lambda frame_id: (150.0, 0.0, 0.0)) + first = make_session(first_source, _nonlinear_calibration(), settings=settings()) + collect_baseline(first, first_source) + process(first, first_source, 31) + assert first.zero.completed is True + for index, raw in enumerate([(400.0, 0.0, 0.0)] * 5, start=32): + first_source._raw_for = lambda frame_id, raw=raw: raw + process(first, first_source, index) + + # A model swap clears completion and the filter history; a new zero then + # starts both from scratch, exactly like a brand-new session. + first.set_calibration(_nonlinear_calibration()) + assert first.zero.completed is False + first.request_zero(reference_id="after-swap", declared_unloaded=True) + first_source._raw_for = lambda frame_id: (150.0, 0.0, 0.0) + for frame_id in range(40, 70): + process(first, first_source, frame_id) + swapped_ready = process(first, first_source, 70) + assert first.zero.completed is True + + fresh_source = FakeMeasurementSource(lambda frame_id: (150.0, 0.0, 0.0)) + fresh = make_session(fresh_source, _nonlinear_calibration(), settings=settings()) + collect_baseline(fresh, fresh_source) + fresh_ready = process(fresh, fresh_source, 31) + assert swapped_ready.raw == fresh_ready.raw + assert swapped_ready.newton == fresh_ready.newton + + # Same comparison one loaded frame later, where history would show. + loaded = (400.0, 0.0, 0.0) + first_source._raw_for = lambda frame_id: loaded + swapped_loaded = process(first, first_source, 71) + fresh_source._raw_for = lambda frame_id: loaded + fresh_loaded = process(fresh, fresh_source, 32) + assert swapped_loaded.raw == fresh_loaded.raw + assert swapped_loaded.newton == fresh_loaded.newton + + +def test_old_readout_entry_keeps_filter_then_convert_order() -> None: + """The historical entry point is not silently reordered by the new path.""" + from orisys.force_readout import ForceReadout + + calibration = _nonlinear_calibration() + readout = ForceReadout({"force_median_window": 3}, config_fps=30.0) + raws = [(200.0, 0.0, 0.0), (300.0, 0.0, 0.0), (250.0, 0.0, 0.0)] + outputs = [readout.apply(raw, calibration.convert)[0] for raw in raws] + # median of the raw FN values is 250 -> curve(250) = 4.0; the new session + # ordering (convert first) would report curve(250)-curve(0) = 4.0 too, but + # the point here is that the OLD call still filters before converting, which + # is exactly the recorded historical behaviour. + assert outputs[-1] == pytest.approx(4.0) + assert readout.raw_unfiltered == (250.0, 0.0, 0.0) diff --git a/tests/test_model_runtime.py b/tests/test_model_runtime.py index 27bfee0..5bf6c84 100644 --- a/tests/test_model_runtime.py +++ b/tests/test_model_runtime.py @@ -455,3 +455,166 @@ def test_finger_runtime_binds_field_identity_and_forwards_the_window(monkeypatch runtime.declare_unloaded_reference("runtime-window") assert sensor.calibration.begin_calls == ["runtime-window"] + + +# ------------------------------------------------- session measurement seam +def test_measurement_session_skips_old_inference_and_readout(): + """The new entry publishes same-frame raw only: no model call, no readout.""" + sensor = make_sensor() + sensor.cuda = sensor.verbose = False + sensor.valid_mask = sensor.valid_mask_hhd = None + sensor._hhd_downsample = 1 + sensor._hhd_grid_shape = sensor._hhd_ref_shape = (2, 2) + sensor._model_frame_id = 0 + field = np.zeros((2, 2, 2), dtype=np.float32) + sensor._Sensor__hhd = SimpleNamespace( + get_force=lambda *args, **kwargs: (None, (10., 0., 0., 0.)), + get_normal=lambda: field, + get_shear=lambda: field, + ) + sensor.enable_measurement_session() + assert sensor.measurement_session is True + sensor.compute_hhd(field) + assert sensor.calibration.calls == [], "the session owns the one model call" + assert sensor._force_readout.filtered_raw is None + assert sensor._force_raw == (10., 0., 0.) + assert np.isnan(sensor.fnormal) + assert sensor.calibration_status == {} + + # Contact statistics stay available, still without a model inference. + sensor._Sensor__contact3d = SimpleNamespace( + compute_foe=lambda *args, **kwargs: { + "centroid": (0, 1), "centroid_found": True, + "divergence_map": np.zeros((2, 2)), + } + ) + sensor.compute_contact() + assert sensor.calibration.calls == [] + assert sensor.centroid_found is True + + sensor.disable_measurement_session() + assert sensor.measurement_session is False + + +def test_reset_tracker_confirms_without_touching_the_calibration_reference(): + sensor = make_sensor() + resets = [] + sensor._Sensor__tracker = SimpleNamespace(reset=lambda: resets.append("tracker")) + sensor.set_calibration_reference({"fn": 10.}, reference_id="reference") + sensor._force_unfiltered = (1., 2., 3.) + sensor.compute_deformation_valid = True + sensor.reset_tracker() + assert resets == ["tracker"] + assert sensor._force_unfiltered is None + assert sensor._calibration_reference == {"fn": 10.} + assert sensor._calibration_reference_id == "reference" + assert sensor.compute_deformation_valid is False + sensor.enable_measurement_session() + sensor.reset_tracker() + assert sensor.measurement_session is True, "the seam survives a frame-boundary reset" + + +def test_measure_current_frame_returns_this_frames_features_once(): + sensor = make_sensor() + sensor.cuda = sensor.verbose = False + sensor.valid_mask = np.ones((2, 2), dtype=bool) + sensor.valid_mask_hhd = np.ones((2, 2), dtype=bool) + sensor._hhd_mask_f32 = None + sensor._hhd_downsample = 1 + sensor._hhd_grid_shape = sensor._hhd_ref_shape = (2, 2) + sensor._model_frame_id = 0 + field = np.zeros((2, 2, 2), dtype=np.float32) + sensor._Sensor__hhd = SimpleNamespace( + get_force=lambda *args, **kwargs: (None, (10., 20., 30., 0.)), + get_normal=lambda: field, + get_shear=lambda: field, + get_field=lambda valid_mask=None: { + "d": field, "valid_mask": np.ones((2, 2), dtype=bool), + "area_scale": 4.0, "grid_shape": (2, 2), "reference_shape": (2, 2), + }, + ) + sensor._Sensor__contact3d = SimpleNamespace( + compute_foe=lambda *args, **kwargs: { + "centroid": (0.4, 1.2), "centroid_found": True, + "divergence_map": np.zeros((2, 2)), + } + ) + flow_calls = [] + sensor.compute_flow = lambda image=None: ( + flow_calls.append(image), setattr(sensor, "flow", field) + ) + sensor.enable_measurement_session() + sensor.img = np.zeros((2, 2), dtype=np.uint8) + + measurement = sensor.measure_current_frame( + frame_id=5, want_field=True, want_contact=True + ) + assert flow_calls == [sensor.img] + assert measurement.raw == (10., 20., 30.) + assert measurement.raw_valid == (True, True, True) + assert measurement.field["grid_shape"] == (2, 2) + assert measurement.output_valid_mask.shape == (2, 2) + assert measurement.contact_found is True + assert measurement.centroid == (0.4, 1.2) + assert measurement.single_contact is True + + +def test_read_measurement_frame_prepares_the_source_frame_once(): + sensor = make_sensor() + sensor.cuda = sensor.verbose = False + sensor.valid_mask = sensor.valid_mask_hhd = None + sensor._hhd_downsample = 1 + sensor._hhd_grid_shape = sensor._hhd_ref_shape = (2, 2) + sensor._model_frame_id = 0 + field = np.zeros((2, 2, 2), dtype=np.float32) + sensor._Sensor__hhd = SimpleNamespace( + get_force=lambda *args, **kwargs: (None, (1., 2., 3., 0.)), + get_normal=lambda: field, + get_shear=lambda: field, + ) + prepared = [] + sensor.get_img = lambda frame=None: (prepared.append(frame), setattr(sensor, "img", frame)) + sensor.compute_flow = lambda image=None: setattr(sensor, "flow", field) + sensor.enable_measurement_session() + frame = np.ones((2, 2, 3), dtype=np.uint8) + measurement = sensor.read_measurement_frame(frame, frame_id=7) + assert prepared == [frame] + assert measurement.raw == (1., 2., 3.) + assert measurement.field is None, "no field unless the model asked for it" + assert measurement.contact_found is None, "no contact unless it was requested" + + +def test_finger_runtime_session_seam_delegates_to_its_sensor(): + from orisys.finger.runtime import FingerRuntime + + events = [] + + class FakeSensor: + def enable_measurement_session(self): + events.append("enable") + + def disable_measurement_session(self): + events.append("disable") + + def reset_tracker(self): + events.append("reset") + + def measure_current_frame(self, *, frame_id, want_field=False, want_contact=False): + events.append(("measure", frame_id, want_field, want_contact)) + return "measurement" + + runtime = object.__new__(FingerRuntime) + runtime.tactile = SimpleNamespace(raw_sensor=FakeSensor()) + processed = [] + runtime.process_frame = lambda frame, color=False: processed.append((frame, color)) + frame = np.zeros((2, 2, 3), dtype=np.uint8) + result = runtime.read_measurement_frame( + frame, frame_id=9, want_field=True, want_contact=False + ) + assert processed == [(frame, False)] + assert result == "measurement" + assert events[-1] == ("measure", 9, True, False) + runtime.enable_measurement_session() + runtime.reset_tracker() + runtime.disable_measurement_session() + assert events[1:] == ["enable", "reset", "disable"] diff --git a/tests/test_signal_processing.py b/tests/test_signal_processing.py index 0a48d98..614d078 100644 --- a/tests/test_signal_processing.py +++ b/tests/test_signal_processing.py @@ -14,6 +14,7 @@ from orisys.signal_processing import ( NORMAL_FORCE_CAL_OFFSET, NORMAL_FORCE_CAL_SLOPE, LowpassFilter, + MultiAxisCausalFilter, calibrate_normal_force, calibrate_shear_components, calibrate_shear_force, @@ -494,3 +495,76 @@ def test_the_calibration_note_names_the_bundle_that_was_found(tmp_path, monkeypa # A dict always keys ``found_root``, so guard its value: an unguarded copy of # this branch prints "bundle found at None". assert "None" not in note + + +# --------------------------------------------- multi-axis causal force chain +class _ReferenceMedian: + """Independent median window, so the operator test has its own expectation.""" + + def __init__(self, window): + self.window = window + self.history = [] + + def process(self, value): + self.history.append(float(value)) + self.history = self.history[-self.window :] + return float(np.median(self.history)) + + +def test_multi_axis_median_matches_a_plain_window_per_axis(): + chain = MultiAxisCausalFilter(median_window=3) + reference = [_ReferenceMedian(3) for _ in range(3)] + samples = [(10.0, 20.0, 30.0), (90.0, 20.0, 30.0), (10.0, 20.0, 30.0), + (10.0, 20.0, 30.0)] + for sample in samples: + actual = chain.process(sample) + expected = [reference[index].process(value) for index, value in enumerate(sample)] + np.testing.assert_allclose(actual, expected) + + +def test_multi_axis_nan_axis_does_not_enter_history_or_pull_the_others(): + chain = MultiAxisCausalFilter(median_window=3) + chain.process((10.0, 20.0, 30.0)) + skipped = chain.process( + (float("nan"), 20.0, 30.0), valid=(False, True, True) + ) + assert np.isnan(skipped[0]) + assert skipped[1] == pytest.approx(20.0) + # Valid axes keep advancing while the missing axis is skipped. + resumed = chain.process((10.0, 20.0, 30.0)) + assert resumed[0] == pytest.approx(10.0) + # A NaN arriving without an explicit validity flag is treated as unavailable. + assert np.isnan(chain.process((float("nan"), 20.0, 30.0))[0]) + + +def test_multi_axis_all_stages_off_is_exact_passthrough_including_nan(): + chain = MultiAxisCausalFilter(median_window=1, lowpass_cutoff_hz=0.0) + assert chain.enabled is False + sample = (1.5, -2.0, float("nan")) + first = chain.process(sample) + second = chain.process(sample) + assert first[0] == 1.5 and first[1] == -2.0 and np.isnan(first[2]) + assert second[0] == 1.5 and second[1] == -2.0 and np.isnan(second[2]) + + +def test_multi_axis_lowpass_ignores_missing_axes_and_resets(): + chain = MultiAxisCausalFilter( + median_window=1, lowpass_cutoff_hz=5.0, sample_rate_hz=30.0, n_channels=3 + ) + values = [] + for index in range(20): + values.append( + chain.process( + (float(index), float(index), float(index)), + valid=(True, True, index % 2 == 0), + ) + ) + for index, value in enumerate(values): + assert np.isfinite(value[0]) + assert np.isfinite(value[1]) + # The third axis is only fed on even frames, so it is missing on odd ones. + assert np.isnan(value[2]) == bool(index % 2) + chain.reset() + after = chain.process((1.0, 2.0, 3.0)) + # A reset IIR starts from its first sample's steady state. + np.testing.assert_allclose(after, (1.0, 2.0, 3.0))