From 14e73d743c826acafeffc687d3e168334d81c108 Mon Sep 17 00:00:00 2001 From: ykcheungab Date: Wed, 21 Oct 2026 15:01:38 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=96=B0=E5=A2=9E=E6=B6=88=E8=B4=B9=20?= =?UTF-8?q?MeasurementFrame=20=E7=9A=84=E5=8A=9B=E4=BE=A7=E4=BC=9A?= =?UTF-8?q?=E8=AF=9D=20ForceStreamSession=EF=BC=88T06a=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 新会话直接消费 ImageMeasurement：参考版本或 session 改变时，在 resetting、collecting、ready 任一阶段都作废旧零点；请求的目标版本到达时采用并开始同版本基线采样，目标前的在途帧标为 zero_pending；重复、迟到、同时间戳的帧只给诊断；max_gap_s/max_window_s 超限给出原因；每帧输出带帧头和参考版本的结果行。不提供旧的外部复位接口；旧 ForceSession 原样保留给 App。标量与无标定场景与旧会话逐位一致。回归 374 项、lint 通过。 --- docs/force-stream-session.zh.md | 71 ++++ src/orisys/force_stream_session.py | 546 +++++++++++++++++++++++++++++ tests/test_force_stream_session.py | 421 ++++++++++++++++++++++ 3 files changed, 1038 insertions(+) create mode 100644 docs/force-stream-session.zh.md create mode 100644 src/orisys/force_stream_session.py create mode 100644 tests/test_force_stream_session.py diff --git a/docs/force-stream-session.zh.md b/docs/force-stream-session.zh.md new file mode 100644 index 0000000..5be2f33 --- /dev/null +++ b/docs/force-stream-session.zh.md @@ -0,0 +1,71 @@ +# 新力侧会话（T06a） + +本文件是单路 SensorStream 计划 T06a 的 SDK 合同：新的力侧会话直接消费 T05 的 +`ImageMeasurement`（T03 `MeasurementFrame` 头 + 同帧 raw/field），参考版本取帧上的 +`reference_generation`。旧 `ForceSession` 仍为 App 保留到 T09/T10 迁移，本会话不提供 +`process_measurement`、`request_zero_external_reset`、`tracker_reset_seq` 等旧外部复位接口。 + +## 接口 + +- `ForceStreamSession(sensor_family=..., calibration=None, settings=None)`：构造；T06a 支持 + 无标定与 schema2 标量曲线，`settings` 为 `OutputSettings`（None 用旧默认）。 +- `request_zero(reference_id=..., declared_unloaded=True, expected_reference_generation=版本, max_gap_s=None, max_window_s=None)`： + 为指定参考版本确认无载归零；可选窗口上限。 +- `process(measurement) -> ForceStreamOutput`：消费一个 T05 `ImageMeasurement`，返回逐帧结果 + （`frame` 携带 T03 帧头与 `reference_generation`，以及 `raw/newton/status/quality/zero`）。 + +## 规则 + +- **版本/会话变化即废旧零点（K8.3）。** `reference_generation` 或 `session_id` 一旦变化， + resetting、collecting、ready 任一阶段的旧零点立即失败（`zero.phase == "failed"`）， + 必须对新的版本显式调用 `request_zero(expected_reference_generation=新版本)` 才能恢复有效 N。 + 正常顺序（会话在处理 v0 → 复位得到目标 v1 → v1 到达）中，等于**请求目标**的新版本会被采用 + 并开始/继续同版本窗口，不会作废这次请求；只有未请求的版本变化或版本超过目标才失败。 + 旧的 `generation` 在采样开始后不再检查的缺陷（阶段 0 P3-2）由本会话修复。 +- **基线只取请求版本的帧（K8.3/K8.5）。** 归零请求绑定一个版本。目标到达前、仍属当前版本的 + 在途帧记为 `zero_pending`（不是迟到，也绝不进入窗口）；比当前版本更旧的帧才是 `late`； + 超过目标的版本使请求失败，而不是静默改绑。基线固定 30 帧无载输入； + 可选 `max_gap_s`/`max_window_s` 超限时失败并把原因写进 `zero.failure_reason`。 +- **重复、迟到、同时间戳（K4.2）。** 以 `sensor_id + session_id + source_frame_id` + （无源编号时用 `frame_id`）去重，凡是送入引擎的帧都会登记身份。重复帧只产生诊断结果； + 旧版本和同时间戳/乱序帧只标记 `late`/`zero_pending`，不推进零点或滤波，也不作废旧零点。 + 不同源帧时间戳相同不算重复。 +- **逐帧结果流（K6.1）。** 每个输入帧都返回一个冻结 `ForceStreamOutput`，携带 T03 帧头与 + `reference_generation`；`quality` 为 `ok` / `uncalibrated` / `zero_pending` / `duplicate` / + `late` / `session_changed` / `version_changed` / `window_failed` / `invalid`。 + 迟到、无效和带质量标记的结果不会被丢弃；拒绝帧仍保留 raw 特征。 +- **数值等价与滤波。** T06a 的零点/标定换算/读数滤波复用已发布算法；同一输入、无复位、 + 同一标定时与旧 `ForceSession` 逐帧相等（测试覆盖标量曲线、无标定、median+lowpass 滤波器）。 + 固定设计帧率与显式滤波切换属于 T07，不在本会话实现。 +- **线程与只读。** 一个实例只属于一个力侧线程；输入数组只读，输出只含标量/状态/不可变进度。 + T06b 在本合同上补充 field 模型、FN-only 全局增益模型与三类标定的完整等价测试。 + +## 示例 + +```python +from orisys.force_stream_session import ForceStreamSession +from orisys.image_pipeline import ImageMeasurement +from orisys.stream_contract import FrameStamp, MeasurementFrame + +session = ForceStreamSession(sensor_family="box") +session.request_zero(reference_id="run-1", declared_unloaded=True, + expected_reference_generation=0) +frame = MeasurementFrame( + stamp=FrameStamp(sensor_id="BOX-001", session_id="run-1", frame_id=1, + source_frame_id=1, capture_time_ns=33_333_333, + clock_id="host", capture_time_basis="capture_monotonic"), + source_dt_s=None, reference_generation=0, +) +row = session.process(ImageMeasurement(frame=frame, force_raw=(0.0, 0.0, 0.0), + torque=0.0, flow=None)) +assert row.frame.reference_generation == 0 +assert row.zero.phase in ("resetting", "collecting") +``` + +## 后续 + +T06b：field（`hhd_normal_l1_field/1`）与 FN-only（schema3 `outputs=["fn"]`）标定、标定不匹配、 +field 加权基线与标量力域基线的分离，以及对照旧 ForceSession 的完整五场景数值等价。 +T07 才改变输出滤波的帧率与切换语义；T09/T10 迁移 App 后删除旧 `ForceSession`。 +另一项后续任务：删除旧 `ForceSession` 之前，先把零点、标定、读数引擎从旧类中抽出来， +避免新会话依赖待删除的类型（T06b 或 T09 之前）。 diff --git a/src/orisys/force_stream_session.py b/src/orisys/force_stream_session.py new file mode 100644 index 0000000..0e3f226 --- /dev/null +++ b/src/orisys/force_stream_session.py @@ -0,0 +1,546 @@ +"""New force-side session for the single-stream contract (T06a core). + +The stage-1 architecture moves the force side from "the pipeline performs the +reset" to "the image block stamps each frame with ``reference_generation``, and +the force session invalidates the old zero whenever that version (or the source +session) changes". :class:`ForceStreamSession` is that new entry: + +* it consumes the T05 :class:`~orisys.image_pipeline.ImageMeasurement` - a T03 + ``MeasurementFrame`` header plus the same-frame raw HHD feature/field; +* it invalidates a completed or in-flight unloaded zero on a reference-version or + session change **in every phase** (the stage-0 P3-2 gap is fixed: sampling + started does not freeze the version check); +* it collects the unloaded baseline only from frames of the requested version, + with optional ``max_gap_s`` / ``max_window_s`` window limits; +* duplicate / late / out-of-order frames produce diagnostics-only results that + advance no zero or filter state and never invalidate a zero; +* every input frame produces one :class:`ForceStreamOutput`, including duplicate, + late and invalid frames, carrying the T03 frame identity and reference version. + +The new session deliberately does **not** offer the old external-reset +book-keeping (``process_measurement`` / ``request_zero_external_reset`` / +``tracker_reset_seq``): those belong to the stage-0 class, which still serves the +App until T09/T10. For T06a the zero/window/calibration/readout algorithms are +reused through the published :class:`~orisys.force_session.ForceSession` engine +behind this class's own state machine; T06b extends the calibration coverage +(field/FN-only models) and the full numeric-equivalence suite. + +One instance belongs to one force-side thread. Input arrays are read, never +mutated; outputs carry scalars/statuses/immutable progress, not internal arrays. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + +from .force_session import ( + WORKFLOW_VERSION, + ForceOutput, + ForceSession, + ZeroProgress, +) +from .force_session import ( + FrameStamp as ForceFrameStamp, +) +from .sensor import FrameMeasurement +from .stream_contract import MeasurementFrame + +__all__ = ["ForceStreamOutput", "ForceStreamSession"] + + +def _status_triple(status: str) -> tuple[str, str, str]: + return (status, status, status) + + +@dataclass(frozen=True) +class ForceStreamOutput: + """One result-stream row: T03 header/version plus this frame's force result. + + ``quality`` is the frame's place in the contract, not a filter setting: + ``ok`` / ``uncalibrated`` / ``zero_pending`` / ``duplicate`` / ``late`` / + ``session_changed`` / ``version_changed`` / ``window_failed`` / ``invalid``. + Rejected frames still carry their raw feature and the T03 frame header, so a + recorder can keep late invalidity visible instead of dropping it. + """ + + frame: MeasurementFrame + raw: tuple[float, float, float] + raw_status: tuple[str, str, str] + newton: tuple[float, float, float] + newton_status: tuple[str, str, str] + zero: ZeroProgress + quality: str + diagnostics: tuple[tuple[str, str | float], ...] = () + accepted: bool = True + duplicate: bool = False + late: bool = False + calibration_fingerprint: str = "" + workflow_version: str = WORKFLOW_VERSION + + @property + def reference_generation(self) -> int: + return int(self.frame.reference_generation) + + +class _PendingMeasurementSource: + """The reused ForceSession engine asks for the frame this session just set.""" + + def __init__(self) -> None: + self._pending: FrameMeasurement | None = 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: + # The image side owns the tracker/reference reset; the session receives + # already-measured frames and must not reshape image state. + self.reset_count += 1 + + def read_measurement_frame( + self, + source_frame, + *, + frame_id: int, + timestamp_s: float | None = None, + want_field: bool = False, + want_contact: bool = False, + ) -> FrameMeasurement: + del source_frame, frame_id, timestamp_s, want_field, want_contact + row = self._pending + if row is None: + raise RuntimeError("ForceStreamSession has no pending measurement") + self._pending = None + return row + + +def _finite_triple(values) -> tuple[bool, bool, bool]: + return tuple(bool(np.isfinite(float(value))) for value in values) # type: ignore[return-value] + + +class ForceStreamSession: + """Force-side session consuming T05 measurements (T06a core). + + See the module docstring for the contract. Calibration/readout reuse the + published engine; this class owns the reference-version zero/window and the + result stream. + """ + + def __init__( + self, + *, + sensor_family: str, + device_id: str | None = None, + context=None, + calibration=None, + settings=None, + ) -> None: + self._source = _PendingMeasurementSource() + self._engine = ForceSession( + source=self._source, + sensor_family=sensor_family, + device_id=device_id, + context=context, + calibration=calibration, + settings=settings, + ) + self._family = sensor_family + self._device_id = device_id + self._version: int | None = None + self._session_id: str | None = None + self._last_time_s: float | None = None + self._seen: dict[tuple[str, str, int], None] = {} + self._seen_limit = 4096 + # Active unloaded-zero request (None when no reset is being confirmed). + self._zero_target: int | None = None + self._max_gap_s: float | None = None + self._max_window_s: float | None = None + self._window_first_s: float | None = None + self._window_last_s: float | None = None + self._last_output: ForceStreamOutput | None = None + + # ------------------------------------------------------------- properties + @property + def workflow_version(self) -> str: + return WORKFLOW_VERSION + + @property + def sensor_family(self) -> str: + return self._family + + @property + def calibration(self): + return self._engine.calibration + + @property + def calibration_fingerprint(self) -> str: + return self._engine.calibration_fingerprint + + @property + def zero(self) -> ZeroProgress: + return self._engine.zero + + @property + def output_settings(self): + return self._engine.output_settings + + @property + def last_output(self) -> ForceStreamOutput | None: + return self._last_output + + @property + def reference_generation(self) -> int | None: + """The reference generation this session currently accepts frames for.""" + return self._version + + # ------------------------------------------------------------ zero request + def request_zero( + self, + *, + reference_id: str, + declared_unloaded: bool, + expected_reference_generation: int, + max_gap_s: float | None = None, + max_window_s: float | None = None, + ) -> ZeroProgress: + """Confirm the unloaded state for exactly ``expected_reference_generation``. + + A reset confirmation names the version it belongs to (K8.3: image reset + confirmation, then same-version unloaded frames). Frames of the still-active + version that arrive while the image reset is in flight are ``zero_pending`` + diagnostics, never baseline frames. The requested version is adopted when + its first frame arrives (the request is not invalidated by its own target); + only a version beyond the request fails it. + """ + if not declared_unloaded: + raise ValueError( + "unloaded zero requires the operator to confirm there is no load" + ) + if isinstance(expected_reference_generation, bool) or not isinstance( + expected_reference_generation, int + ): + raise TypeError("expected_reference_generation must be an integer") + if expected_reference_generation < 0: + raise ValueError("expected_reference_generation must be >= 0") + if self._version is not None and expected_reference_generation < self._version: + raise ValueError( + "expected_reference_generation is older than the active version " + f"{self._version}" + ) + for name, value in (("max_gap_s", max_gap_s), ("max_window_s", max_window_s)): + if value is not None: + value = float(value) + if not np.isfinite(value) or value <= 0.0: + raise ValueError(f"{name} must be a positive finite number or None") + self._zero_target = int(expected_reference_generation) + self._max_gap_s = None if max_gap_s is None else float(max_gap_s) + self._max_window_s = None if max_window_s is None else float(max_window_s) + self._window_first_s = None + self._window_last_s = None + return self._engine.request_zero( + reference_id=reference_id, declared_unloaded=True + ) + + # ---------------------------------------------------------------- result rows + def _row( + self, + frame: MeasurementFrame, + *, + raw: tuple[float, float, float], + raw_status: tuple[str, str, str], + newton: tuple[float, float, float], + newton_status: tuple[str, str, str], + quality: str, + diagnostics: tuple[tuple[str, str | float], ...] = (), + accepted: bool = True, + duplicate: bool = False, + late: bool = False, + ) -> ForceStreamOutput: + output = ForceStreamOutput( + frame=frame, + raw=raw, + raw_status=raw_status, + newton=newton, + newton_status=newton_status, + zero=self._engine.zero, + quality=quality, + diagnostics=diagnostics, + accepted=accepted, + duplicate=duplicate, + late=late, + calibration_fingerprint=self._engine.calibration_fingerprint, + ) + self._last_output = output + return output + + def _diagnostic_row( + self, + frame: MeasurementFrame, + measurement, + *, + quality: str, + reason: str, + duplicate: bool = False, + late: bool = False, + ) -> ForceStreamOutput: + raw = tuple(float(value) for value in measurement.force_raw) + return self._row( + frame, + raw=raw, + raw_status=tuple( + "ok" if finite else "unavailable" for finite in _finite_triple(raw) + ), + newton=(float("nan"),) * 3, + newton_status=_status_triple(quality), + quality=quality, + diagnostics=(("reason", reason),), + accepted=False, + duplicate=duplicate, + late=late, + ) + + @staticmethod + def _identity(frame: MeasurementFrame) -> tuple[str, str, int]: + stamp = frame.stamp + source = ( + stamp.source_frame_id + if stamp.source_frame_id is not None + else stamp.frame_id + ) + return (str(stamp.sensor_id), str(stamp.session_id), int(source)) + + def _source_time_s(self, frame: MeasurementFrame) -> float: + return float(frame.stamp.capture_time_ns) / 1e9 + + def _invalidate(self, reason: str) -> None: + self._engine.invalidate(reason=reason) + self._zero_target = None + self._window_first_s = None + self._window_last_s = None + + def _check_window_limits(self, frame: MeasurementFrame) -> str | None: + """Return a failure reason when the next same-version frame would exceed a limit.""" + if self._engine.zero.phase != "collecting": + return None + now = self._source_time_s(frame) + if self._max_gap_s is not None and self._window_last_s is not None: + gap = now - self._window_last_s + if gap > self._max_gap_s: + return ( + f"zero baseline gap {gap:.6f}s exceeds max_gap_s=" + f"{self._max_gap_s:.6f}s" + ) + if self._max_window_s is not None and self._window_first_s is not None: + span = now - self._window_first_s + if span > self._max_window_s: + return ( + f"zero baseline window {span:.6f}s exceeds max_window_s=" + f"{self._max_window_s:.6f}s" + ) + return None + + def _feed_engine(self, measurement, frame: MeasurementFrame) -> ForceOutput: + row = FrameMeasurement( + raw=tuple(float(value) for value in measurement.force_raw), + raw_valid=_finite_triple(measurement.force_raw), + field=measurement.field, + output_valid_mask=measurement.valid_mask, + diagnostics=(("frame_id", int(frame.stamp.frame_id)),), + ) + self._source._pending = row + stamp = ForceFrameStamp( + session_id=str(frame.stamp.session_id), + frame_id=int(frame.stamp.frame_id), + source_frame_id=frame.stamp.source_frame_id, + signal_time_s=self._source_time_s(frame), + time_basis=frame.stamp.capture_time_basis, + ) + # process_frame applies a pending zero reset before measuring this frame, + # so the first frame after request_zero() is baseline frame 1 (same order + # as the published session). The adapter hands it the already-measured row. + return self._engine.process_frame(None, stamp) + + def _quality_for(self, output: ForceOutput) -> str: + zero_phase = self._engine.zero.phase + if zero_phase in ("resetting", "collecting"): + return "zero_pending" + if zero_phase == "failed": + return "invalid" + if self._engine.calibration is None: + return "uncalibrated" + if all(status == "uncalibrated" for status in output.measurement.newton_status): + return "uncalibrated" + if any(status == "ok" for status in output.measurement.newton_status): + return "ok" + return "invalid" + + def process(self, measurement) -> ForceStreamOutput: + """Consume one T05 ``ImageMeasurement`` and return this frame's result row. + + A duplicate / late / out-of-order frame is reported without advancing the + zero window or readout filter, and without invalidating a completed zero. + A reference-version or session change fails the old zero immediately (in + any phase) and requires an explicit :meth:`request_zero` for the new one. + """ + frame = getattr(measurement, "frame", None) + if not isinstance(frame, MeasurementFrame): + raise TypeError( + "ForceStreamSession consumes an ImageMeasurement-like input" + ) + identity = self._identity(frame) + version = int(frame.reference_generation) + session_id = str(frame.stamp.session_id) + signal_time = self._source_time_s(frame) + target = self._zero_target + + # 1) Duplicates: same source frame identity, diagnostics only. + if identity in self._seen: + return self._diagnostic_row( + frame, + measurement, + quality="duplicate", + reason="duplicate source frame identity", + duplicate=True, + ) + + # 2) Session change fails any zero (all phases), then this frame is a raw row. + if self._session_id is not None and session_id != self._session_id: + self._invalidate( + f"source session changed {self._session_id!r} -> {session_id!r}" + ) + self._session_id = session_id + self._version = version + self._last_time_s = signal_time + self._remember(identity) + output = self._feed_engine(measurement, frame) + return self._accepted_row(frame, output, quality="session_changed") + + # 3) The active version changed. The requested zero target is the one + # version that may be adopted without invalidating the request: the + # normal order is v0 frames in flight -> request_zero(v1) -> v1 arrives. + if self._version is not None and version != self._version: + if version < self._version: + return self._diagnostic_row( + frame, + measurement, + quality="late", + reason=( + f"reference generation {version} is older than the active " + f"version {self._version}" + ), + late=True, + ) + if target is not None and version == target: + # Adopt the requested version: start/continue the same-version + # baseline window; the request must not be invalidated by its own + # first frame. + self._version = version + self._session_id = session_id + self._last_time_s = signal_time + self._remember(identity) + output = self._feed_engine(measurement, frame) + self._track_window(signal_time) + return self._accepted_row( + frame, output, quality=self._quality_for(output) + ) + self._invalidate( + f"reference generation changed {self._version} -> {version}" + ) + self._version = version + self._session_id = session_id + self._last_time_s = signal_time + self._remember(identity) + output = self._feed_engine(measurement, frame) + return self._accepted_row(frame, output, quality="version_changed") + + # 4) Reset confirmation: only the requested version may fill the window. + if target is not None: + if version < target: + # Frames of the still-active version that arrive while the image + # reset is in flight: waiting for the target, not late. They must + # not enter the baseline window. + return self._diagnostic_row( + frame, + measurement, + quality="zero_pending", + reason=( + f"waiting for reference generation {target}; frame carries " + f"the still-active version {version}" + ), + ) + if version > target: + self._invalidate( + f"reference generation {version} advanced past the requested " + f"zero version {target}" + ) + self._version = version + self._session_id = session_id + self._last_time_s = signal_time + self._remember(identity) + output = self._feed_engine(measurement, frame) + return self._accepted_row(frame, output, quality="version_changed") + + # 5) Same timestamp / out-of-order: diagnostics only, never invalidates. + if self._last_time_s is not None and signal_time <= self._last_time_s: + return self._diagnostic_row( + frame, + measurement, + quality="late", + reason="frame signal time is not after the last accepted frame", + late=True, + ) + + # 6) Baseline-window limits fail the request before the frame enters it. + window_problem = self._check_window_limits(frame) + if window_problem is not None: + self._invalidate(window_problem) + self._version = version + self._session_id = session_id + self._last_time_s = signal_time + self._remember(identity) + output = self._feed_engine(measurement, frame) + return self._accepted_row(frame, output, quality="window_failed") + + # Accepted: this frame is measured once by the reused engine. + self._version = version if self._version is None else self._version + self._session_id = session_id + self._last_time_s = signal_time + self._remember(identity) + output = self._feed_engine(measurement, frame) + self._track_window(signal_time) + return self._accepted_row(frame, output, quality=self._quality_for(output)) + + def _remember(self, identity: tuple[str, str, int]) -> None: + """Register every frame handed to the engine (duplicate detection).""" + self._seen[identity] = None + if len(self._seen) > self._seen_limit: + self._seen.clear() + self._seen[identity] = None + + def _track_window(self, signal_time: float) -> None: + if self._engine.zero.phase == "collecting": + if self._window_first_s is None: + self._window_first_s = signal_time + self._window_last_s = signal_time + + def _accepted_row( + self, frame: MeasurementFrame, output: ForceOutput, *, quality: str + ) -> ForceStreamOutput: + return self._row( + frame, + raw=tuple(float(value) for value in output.raw), + raw_status=tuple(str(value) for value in output.measurement.sdk.raw_status), + newton=tuple(float(value) for value in output.newton), + newton_status=tuple( + str(value) for value in output.measurement.newton_status + ), + quality=quality, + diagnostics=tuple(output.measurement.diagnostics), + accepted=True, + ) diff --git a/tests/test_force_stream_session.py b/tests/test_force_stream_session.py new file mode 100644 index 0000000..305f11b --- /dev/null +++ b/tests/test_force_stream_session.py @@ -0,0 +1,421 @@ +"""New force-side session (T06a): version/zero contract and old-session numerics. + +The tests drive :class:`~orisys.force_stream_session.ForceStreamSession` with +T05-style measurements (T03 ``MeasurementFrame`` + same-frame raw feature) and +compare the scalar/uncalibrated numerical chain with the published old +``ForceSession`` on the same input sequence. They also pin the new contract: +reference/session changes invalidate the zero in every phase, only same-version +frames fill the baseline window, and duplicate/late/same-timestamp frames stay +diagnostics-only. +""" + +from __future__ import annotations + +import importlib.util +from pathlib import Path + +import numpy as np + +from orisys.force_stream_session import ForceStreamOutput, ForceStreamSession +from orisys.image_pipeline import ImageMeasurement +from orisys.stream_contract import FrameStamp, MeasurementFrame + +FRAME_NS = 33_333_333 + + +def _load_helpers(): + path = Path(__file__).with_name("test_force_session.py") + spec = importlib.util.spec_from_file_location("_force_session_test_helpers", path) + module = importlib.util.module_from_spec(spec) + assert spec.loader is not None + spec.loader.exec_module(module) + return module + + +helpers = _load_helpers() + + +def _measurement( + frame_id: int, + raw: tuple[float, float, float], + *, + version: int = 0, + session_id: str = "s1", + sensor_id: str = "TEST-1", + capture_time_ns: int | None = None, + source_frame_id: int | None = None, +) -> ImageMeasurement: + stamp = FrameStamp( + sensor_id=sensor_id, + session_id=session_id, + frame_id=frame_id, + source_frame_id=frame_id if source_frame_id is None else source_frame_id, + capture_time_ns=( + frame_id * FRAME_NS if capture_time_ns is None else capture_time_ns + ), + clock_id="host", + capture_time_basis="recorded_signal", + ) + frame = MeasurementFrame( + stamp=stamp, + source_dt_s=None if frame_id == 1 else FRAME_NS / 1e9, + reference_generation=version, + ) + return ImageMeasurement(frame=frame, force_raw=raw, torque=0.0, flow=None) + + +def _raw_for(frame_id: int) -> tuple[float, float, float]: + return ( + 15000.0 + 500.0 * frame_id, + 8000.0 + 200.0 * frame_id, + 7000.0 - 100.0 * frame_id, + ) + + +def _old_chain(calibration=None, settings=None): + source = helpers.FakeMeasurementSource(_raw_for) + session = helpers.make_session(source, calibration, settings=settings) + session.request_zero(reference_id="old-ref", declared_unloaded=True) + return session, source + + +def _new_chain(calibration=None, settings=None): + return ForceStreamSession( + sensor_family="box", + calibration=calibration, + settings=settings if settings is not None else helpers.legacy_settings(), + ) + + +def _compare_with_old(calibration=None, settings=None) -> None: + old, source = _old_chain(calibration, settings) + new = _new_chain(calibration, settings) + new.request_zero( + reference_id="new-ref", declared_unloaded=True, expected_reference_generation=0 + ) + old_raw, old_newton, old_raw_status, old_newton_status = [], [], [], [] + new_raw, new_newton, new_raw_status, new_newton_status = [], [], [], [] + new_zero_phases = [] + for frame_id in range(1, 41): + old_output = helpers.process(old, source, frame_id) + old_raw.append(np.asarray(old_output.raw)) + old_newton.append(np.asarray(old_output.newton)) + old_raw_status.append(list(old_output.measurement.sdk.raw_status)) + old_newton_status.append(list(old_output.measurement.newton_status)) + + result = new.process(_measurement(frame_id, _raw_for(frame_id))) + assert isinstance(result, ForceStreamOutput) + new_raw.append(np.asarray(result.raw)) + new_newton.append(np.asarray(result.newton)) + new_raw_status.append(list(result.raw_status)) + new_newton_status.append(list(result.newton_status)) + new_zero_phases.append(result.zero.phase) + + assert np.array_equal(np.stack(old_raw), np.stack(new_raw), equal_nan=True) + assert np.array_equal(np.stack(old_newton), np.stack(new_newton), equal_nan=True) + assert old_raw_status == new_raw_status + assert old_newton_status == new_newton_status + assert new.zero.phase == "ready" + assert new_zero_phases[:30] == ["collecting"] * 30 + assert new_zero_phases[30] == "ready" + + +# ------------------------------------------------------------ numeric equivalence +def test_scalar_curve_session_matches_old_force_session() -> None: + _compare_with_old(helpers.box_cal()) + + +def test_uncalibrated_session_matches_old_force_session() -> None: + _compare_with_old(None) + + +def test_filtered_scalar_session_matches_old_force_session() -> None: + settings = helpers.OutputSettings( + sample_rate_hz=30.0, median_window=3, lowpass_hz=5.0 + ) + _compare_with_old(helpers.box_cal(), settings) + + +# ----------------------------------------------------- version / session contract +def _baseline(new: ForceStreamSession, frame_ids, *, version=0, session_id="s1"): + return [ + new.process( + _measurement(fid, _raw_for(fid), version=version, session_id=session_id) + ) + for fid in frame_ids + ] + + +def test_version_change_during_collecting_invalidates_the_zero() -> None: + new = _new_chain(helpers.box_cal()) + new.request_zero( + reference_id="ref", declared_unloaded=True, expected_reference_generation=0 + ) + collected = _baseline(new, range(1, 11)) + assert collected[-1].zero.phase == "collecting" + + changed = new.process(_measurement(11, _raw_for(11), version=2)) + assert changed.quality == "version_changed" + assert changed.zero.phase == "failed" + assert "reference generation changed" in changed.zero.failure_reason + assert all(np.isnan(value) for value in changed.newton) + + # Only an explicit re-zero for the new version restores a valid N. + new.request_zero( + reference_id="ref-2", declared_unloaded=True, expected_reference_generation=2 + ) + ready = _baseline(new, range(12, 43), version=2) + assert ready[-1].zero.phase == "ready" + assert np.isfinite(ready[-1].newton[0]) + assert ready[-1].reference_generation == 2 + + +def test_version_change_after_ready_invalidates_and_requires_explicit_reset() -> None: + new = _new_chain() + new.request_zero( + reference_id="ref", declared_unloaded=True, expected_reference_generation=0 + ) + baseline = _baseline(new, range(1, 31)) + ready = _baseline(new, [31]) + assert ready[-1].zero.phase == "ready" + assert np.isfinite(baseline[-1].raw[0]) + + changed = new.process(_measurement(32, _raw_for(32), version=1)) + assert changed.quality == "version_changed" + assert changed.zero.phase == "failed" + assert all(np.isnan(value) for value in changed.newton) + # A raw row is still in the result stream; the zero is not silently relabelled. + assert all(np.isfinite(value) for value in changed.raw) + + still_failed = new.process(_measurement(33, _raw_for(33), version=1)) + assert still_failed.zero.phase == "failed" + assert all(np.isnan(value) for value in still_failed.newton) + + +def test_session_change_invalidates_the_zero_and_emits_results() -> None: + new = _new_chain() + new.request_zero( + reference_id="ref", declared_unloaded=True, expected_reference_generation=0 + ) + _baseline(new, range(1, 31)) + assert _baseline(new, [31])[-1].zero.phase == "ready" + + changed = new.process(_measurement(32, _raw_for(32), session_id="s2", version=0)) + assert changed.quality == "session_changed" + assert changed.zero.phase == "failed" + assert "source session changed" in changed.zero.failure_reason + assert all(np.isfinite(value) for value in changed.raw) + + +def test_late_frame_cannot_fill_the_new_version_window() -> None: + # Corrected in the Y2 rework: a frame older than the requested reset target is + # "zero_pending", not "late" (it belongs to the still-active version and is + # waiting for the reset confirmation). It still cannot fill the window. + new = _new_chain() + new.request_zero( + reference_id="ref", declared_unloaded=True, expected_reference_generation=1 + ) + late = new.process(_measurement(1, _raw_for(1), version=0)) + assert late.quality == "zero_pending" + assert late.late is False + assert late.accepted is False + assert late.zero.phase == "resetting", "pending frame must not advance the zero" + + accepted = _baseline(new, range(2, 12), version=1) + assert accepted[-1].zero.phase == "collecting" + + +# ------------------------------------------- duplicate / late / same timestamp +def test_duplicate_and_same_timestamp_frames_are_diagnostics_only() -> None: + new = _new_chain() + new.request_zero( + reference_id="ref", declared_unloaded=True, expected_reference_generation=0 + ) + _baseline(new, range(1, 31)) + ready = _baseline(new, [31])[-1] + assert ready.zero.phase == "ready" + before = new.zero + + duplicate = new.process(_measurement(20, _raw_for(20))) + assert duplicate.duplicate is True + assert duplicate.quality == "duplicate" + assert new.zero == before, "duplicate must not advance or invalidate the zero" + + # Same timestamp, different source frame: not a duplicate, but it is reported + # as a late/order diagnostic and cannot advance the filter or invalidate. + same_time = new.process( + _measurement(32, _raw_for(32), capture_time_ns=31 * FRAME_NS) + ) + assert same_time.duplicate is False + assert same_time.quality == "late" + assert same_time.late is True + assert new.zero == before, "order diagnostic must not invalidate the zero" + + duplicate_again = new.process(_measurement(20, _raw_for(20))) + assert duplicate_again.duplicate is True + + +# ------------------------------------------------------------ window limit gates +def test_max_gap_s_fails_the_baseline_window_with_reason() -> None: + new = _new_chain() + new.request_zero( + reference_id="ref", + declared_unloaded=True, + expected_reference_generation=0, + max_gap_s=0.05, + ) + _baseline(new, range(1, 6)) + jumped = new.process(_measurement(6, _raw_for(6), capture_time_ns=6 * 500_000_000)) + assert jumped.quality == "window_failed" + assert jumped.zero.phase == "failed" + assert "max_gap_s" in jumped.zero.failure_reason + + +def test_max_window_s_fails_the_baseline_window_with_reason() -> None: + new = _new_chain() + new.request_zero( + reference_id="ref", + declared_unloaded=True, + expected_reference_generation=0, + max_window_s=0.5, + ) + _baseline(new, range(1, 10)) + late_limit = new.process( + _measurement(10, _raw_for(10), capture_time_ns=1_000_000_000) + ) + assert late_limit.quality == "window_failed" + assert late_limit.zero.phase == "failed" + assert "max_window_s" in late_limit.zero.failure_reason + + +# -------------------------------------------------------------- stream contract +def test_every_frame_produces_a_header_carrying_result() -> None: + new = _new_chain() + new.request_zero( + reference_id="ref", declared_unloaded=True, expected_reference_generation=0 + ) + rows = _baseline(new, range(1, 31)) + [ + new.process(_measurement(31, (float("nan"), 1.0, 2.0))), + ] + assert len(rows) == 31 + for row in rows: + assert isinstance(row, ForceStreamOutput) + assert isinstance(row.frame, MeasurementFrame) + assert row.frame.stamp.session_id == "s1" + assert isinstance(row.reference_generation, int) + invalid = rows[-1] + assert invalid.accepted is True + assert invalid.zero.phase == "ready", ( + "an invalid frame must not invent a zero failure" + ) + assert np.isnan(invalid.raw[0]), ( + "the failed axis stays NaN; other raw axes remain visible" + ) + assert all(np.isnan(value) for value in invalid.newton) + assert invalid.quality in ("invalid", "uncalibrated") + + +def test_new_session_does_not_expose_the_old_external_reset_api() -> None: + new = _new_chain() + for name in ( + "process_measurement", + "request_zero_external_reset", + "tracker_reset_seq", + "process_frame", + ): + assert not hasattr(new, name), name + + +def test_inputs_are_not_mutated_and_outputs_hold_no_internal_arrays() -> None: + new = _new_chain() + raw = _raw_for(1) + flow = np.zeros((2, 2, 2), dtype=np.float32) + measurement = _measurement(1, raw) + sentinel = np.array(flow, copy=True) + measurement.force_raw # tuple attribute, no array to copy + new.request_zero( + reference_id="ref", declared_unloaded=True, expected_reference_generation=0 + ) + result = new.process(measurement) + assert np.array_equal(flow, sentinel), "input arrays are read-only to the session" + assert not isinstance(result.raw, np.ndarray) + assert not isinstance(result.newton, np.ndarray) + assert all(value == value for value in result.raw) # finite tuple values + + +# --------------------------------------- normal reset order (Y1) and pending rows +def test_normal_reset_order_adopts_the_requested_version_and_completes_zero() -> None: + """v0 frames in flight -> request_zero(v1) -> first v1 frame starts collecting. + + This is the order Studio uses. The request must not be invalidated by its own + target version, and the still-in-flight v0 frames are pending, not late. + """ + new = _new_chain(helpers.box_cal()) + seed = _baseline(new, range(1, 4), version=0) + assert seed[-1].zero.phase != "ready" + new.request_zero( + reference_id="z", declared_unloaded=True, expected_reference_generation=1 + ) + + in_flight = _baseline(new, range(4, 6), version=0) + for row in in_flight: + assert row.quality == "zero_pending", row.quality + assert row.late is False, "in-flight current-version frames are not late" + assert row.accepted is False + assert row.zero.phase == "resetting", "pending frame must not advance the zero" + + version_one = _baseline(new, range(6, 37), version=1) + assert version_one[0].zero.phase == "collecting" + assert version_one[29].zero.phase == "collecting" + assert version_one[30].zero.phase == "ready" + assert np.isfinite(version_one[30].newton[0]) + assert version_one[0].reference_generation == 1 + + +def test_request_arriving_after_the_first_new_version_frame_still_completes() -> None: + new = _new_chain(helpers.box_cal()) + _baseline(new, range(1, 4), version=0) + + first_v1 = new.process(_measurement(4, _raw_for(4), version=1)) + assert first_v1.quality == "version_changed" + assert first_v1.zero.phase == "failed" + + new.request_zero( + reference_id="z-late", declared_unloaded=True, expected_reference_generation=1 + ) + frames = _baseline(new, range(5, 36), version=1) + assert frames[0].zero.phase == "collecting" + assert frames[30].zero.phase == "ready" + assert np.isfinite(frames[30].newton[0]) + + +def test_target_after_v1_waits_through_v1_inflight_then_adopts_v2() -> None: + new = _new_chain(helpers.box_cal()) + new.request_zero( + reference_id="v1", declared_unloaded=True, expected_reference_generation=1 + ) + _baseline(new, range(1, 32), version=1) + assert new.zero.phase == "ready" + new.request_zero( + reference_id="v2", declared_unloaded=True, expected_reference_generation=2 + ) + pending = _baseline(new, range(32, 34), version=1) + assert all(row.quality == "zero_pending" and not row.late for row in pending) + assert all(row.zero.phase == "resetting" for row in pending) + + v2 = _baseline(new, range(34, 65), version=2) + assert v2[0].zero.phase == "collecting" + assert v2[30].zero.phase == "ready" + + +# ------------------------------------------------ branch frames register identity +def test_version_change_branch_frame_is_registered_for_duplicate_detection() -> None: + new = _new_chain() + _baseline(new, range(1, 3), version=0) + changed = new.process(_measurement(3, _raw_for(3), version=1)) + assert changed.quality == "version_changed" + after_change = new.zero + repeat = new.process(_measurement(3, _raw_for(3), version=1)) + assert repeat.duplicate is True + assert repeat.quality == "duplicate" + assert new.zero == after_change, "duplicate must not be processed again"