新增统一力读数会话 ForceSession

把同帧测量、空载归零、模型推断与共用后处理收进一个 SDK 会话,Studio 显示与记录使用同一样本;旧 Sensor 读数接口保持兼容。以 force session 与受影响 SDK 套件、lint 与文档门禁验证,硬件与真实载荷精度未验证。

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01CpCfJF6SvonMKmPZhzKZHb
This commit is contained in:
ykcheungab
2026-10-20 07:07:22 +00:00
parent dc8a630df3
commit ee830d8505
17 changed files with 4175 additions and 29 deletions
+9 -3
View File
@@ -183,7 +183,7 @@ flow = sensor.read_info(sensor.info.VRAW)
释放传感器相关资源并停止后台异步线程。 调用后将关闭视频/摄像头输入源,并销毁所有 OpenCV 窗口。 释放传感器相关资源并停止后台异步线程。 调用后将关闭视频/摄像头输入源,并销毁所有 OpenCV 窗口。
建议在程序退出前调用以确保资源被正确释放。 建议退出前调用。
### `Sensor.configure` 低通滤波设置 ### `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) 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 低通滤波,适合示例程序或图表中对历史数据做平滑处理。 对一维、等间隔采样数据执行零相位 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]) 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 ## Finger Pipeline API
### `orisys.finger` 手指 UV pipeline ### `orisys.finger` 手指 UV pipeline
+101
View File
@@ -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` 不同是唯一允许的管线身份差异,其余差异仍拒绝并报告。
+9
View File
@@ -19,6 +19,11 @@ from .pipeline import (
StreamPipeline, StreamPipeline,
) )
from .sensor import Sensor 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 CompoundEyeSensor = Sensor
CameraSensor = Sensor CameraSensor = Sensor
@@ -61,6 +66,10 @@ __all__ = [
# force calibration # force calibration
'calibration', 'calibration',
# explicit force workflow session (studio-force/1)
'ForceSession',
'WORKFLOW_VERSION',
# configs utilities functions # configs utilities functions
'list_config', 'list_config',
'export_config', 'export_config',
+49
View File
@@ -6,7 +6,9 @@ curves in ``_apply_force_filter``; ``read_info(FNORMAL, …)`` returns Newtons.
from __future__ import annotations from __future__ import annotations
import hashlib
import json import json
import math
import re import re
import warnings import warnings
from collections.abc import Iterable, Mapping from collections.abc import Iterable, Mapping
@@ -408,6 +410,53 @@ class Calibration:
) )
return out_fn, out_fx, out_fy 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]: def to_dict(self) -> dict[str, Any]:
channels: dict[str, Any] = {} channels: dict[str, Any] = {}
for cid, curve in self.channels.items(): for cid, curve in self.channels.items():
+9
View File
@@ -34,6 +34,15 @@ class CalibrationContext(dict):
identity["tracker"]["hhd_downsample"] = downsample identity["tracker"]["hhd_downsample"] = downsample
return _feature_signature(identity) 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: def sha256_file(path) -> str:
return hashlib.sha256(Path(path).read_bytes()).hexdigest() return hashlib.sha256(Path(path).read_bytes()).hexdigest()
+119 -11
View File
@@ -163,6 +163,38 @@ class CalibrationResult:
diagnostics: dict[str, Any] = dataclass_field(default_factory=dict) 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: class ModelCalibration:
"""One validated model per stream. Current gain backends are stateless.""" """One validated model per stream. Current gain backends are stateless."""
@@ -245,25 +277,101 @@ class ModelCalibration:
def to_dict(self) -> dict[str, Any]: def to_dict(self) -> dict[str, Any]:
return deepcopy(self._document) 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 from .calibration_context import CalibrationContext
mismatches: dict[str, str] = {}
for key, value in self.pipeline.items(): for key, value in self.pipeline.items():
if key == "hhd_downsample": if key == "hhd_downsample":
# HHD magnitudes are normalized to the full flow grid; resolution # Allowed, but reported: HHD magnitudes are normalized to the full
# can introduce numerical deviations without changing the force unit. # flow grid, so resolution can introduce numerical deviations
# without changing the force unit.
continue continue
if key == "feature_signature_sha256" and isinstance(pipeline, CalibrationContext): if key == "feature_signature_sha256" and isinstance(
_require( pipeline, CalibrationContext
pipeline.signature_for_downsample(self.pipeline["hhd_downsample"]) == value, ):
"calibration pipeline mismatch: feature_signature_sha256", if pipeline.signature_for_downsample(
self.pipeline["hhd_downsample"]
) != value:
mismatches[key] = (
"calibration pipeline mismatch: feature_signature_sha256"
) )
elif key == "filters": elif key == "filters":
actual = pipeline.get(key, {}) actual = pipeline.get(key, {})
_require(isinstance(actual, Mapping) and actual.get("measurement") == value["measurement"], if not (
"calibration pipeline mismatch: filters.measurement") isinstance(actual, Mapping)
else: and actual.get("measurement") == value["measurement"]
_require(pipeline.get(key) == value, f"calibration pipeline mismatch: {key}") ):
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: def reset(self, reference_id: Any = None) -> None:
"""Clear per-stream state; the field model also voids its weighted baseline.""" """Clear per-stream state; the field model also voids its weighted baseline."""
+54
View File
@@ -454,6 +454,60 @@ class FingerRuntime:
self.tactile.push_frame(unrolled, already_gray=True) self.tactile.push_frame(unrolled, already_gray=True)
return unrolled 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): def grab_and_process(self, *, color=False):
ok, frame = self.read_source() ok, frame = self.read_source()
if not ok: if not ok:
+309
View File
@@ -2,11 +2,22 @@
Any sensor that produces a raw (fn, fx, fy) triple can use this. Calibration Any sensor that produces a raw (fn, fx, fy) triple can use this. Calibration
coefficients stay outside: pass ``convert`` into :meth:`ForceReadout.apply`. 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 __future__ import annotations
from collections.abc import Callable from collections.abc import Callable
from dataclasses import dataclass, replace
import numpy as np import numpy as np
@@ -14,6 +25,7 @@ from .signal_processing import (
DEFAULT_SAMPLE_RATE_HZ, DEFAULT_SAMPLE_RATE_HZ,
CausalMedianFilter, CausalMedianFilter,
LowpassFilter, LowpassFilter,
MultiAxisCausalFilter,
SampleRateEstimator, SampleRateEstimator,
) )
@@ -392,3 +404,300 @@ class ForceReadout:
"any_stage_applied": bool(self.any_stage_applied), "any_stage_applied": bool(self.any_stage_applied),
"unfiltered_available": True, "unfiltered_available": True,
} }
FORCE_AXES: tuple[str, str, str] = ("fn", "fx", "fy")
def _positive_float(value, default: float) -> float:
try:
number = float(value)
except (TypeError, ValueError):
return float(default)
if not np.isfinite(number) or number <= 0:
return float(default)
return number
def _non_negative_float(value, default: float = 0.0) -> float:
try:
number = float(value)
except (TypeError, ValueError):
return float(default)
if not np.isfinite(number) or number < 0:
return float(default)
return number
@dataclass(frozen=True)
class OutputSettings:
"""One resolved setting group for BOTH display and recording.
Every stage is switched by its own value: ``median_window=1`` and
``lowpass_hz=0`` each mean "off", and a threshold of 0 disables that unit's
threshold. All stages off is an exact pass-through (a NaN axis stays NaN).
This does not switch off the upstream flow EMA; that belongs to the
measurement context and is part of the calibration identity.
``sample_rate_hz`` describes the *signal* time of the input frames, declared
by the caller (camera config or recorded signal clock). The offline
processing speed never designs the low-pass. A cutoff at or above Nyquist is
unusable and resolves to "off" rather than to a silent re-tune.
"""
sample_rate_hz: float
median_window: int = 1
lowpass_hz: float = 0.0
lowpass_order: int = 2
raw_threshold: float = 0.0
newton_threshold: float = 0.0
def normalised(self) -> OutputSettings:
"""Clamp to values the chain can actually run; unusable values are "off"."""
rate = _positive_float(self.sample_rate_hz, DEFAULT_SAMPLE_RATE_HZ)
try:
window = max(1, int(self.median_window))
except (TypeError, ValueError):
window = 1
cutoff = _non_negative_float(self.lowpass_hz)
if cutoff >= 0.5 * rate:
cutoff = 0.0
try:
order = max(1, int(self.lowpass_order))
except (TypeError, ValueError):
order = 2
return replace(
self,
sample_rate_hz=rate,
median_window=window,
lowpass_hz=cutoff,
lowpass_order=order,
raw_threshold=_non_negative_float(self.raw_threshold),
newton_threshold=_non_negative_float(self.newton_threshold),
)
@property
def median_on(self) -> bool:
return int(self.median_window) > 1
@property
def lowpass_on(self) -> bool:
return float(self.lowpass_hz) > 0
def state(self) -> dict:
"""Effective parameters, for a record or an audit."""
return {
"sample_rate_hz": float(self.sample_rate_hz),
"median_window": int(self.median_window),
"median_applied": bool(self.median_on),
"lowpass_hz": float(self.lowpass_hz),
"lowpass_applied": bool(self.lowpass_on),
"lowpass_order": int(self.lowpass_order),
"raw_threshold": float(self.raw_threshold),
"newton_threshold": float(self.newton_threshold),
}
class ForceOutputFilter:
"""Median → low-pass → optional thresholds, once, for raw and N separately.
One state per domain: two :class:`MultiAxisCausalFilter` instances with the
same settings. The session hands each frame's features here exactly once, so
display and recording read the same processed sample; there is no second
filter state for a consumer to drift from.
Invalid axes (NaN or an undeclared valid flag) never enter the filter
history and stay NaN, which keeps "missing" distinct from a valid zero. With
every stage off the input tuples are returned unchanged, NaN included.
"""
def __init__(self, settings: OutputSettings):
self._settings = settings.normalised()
self._chain_settings = self._settings
self._raw_chain = self._build_chain()
self._newton_chain = self._build_chain()
self._rate = SampleRateEstimator(initial_hz=self._settings.sample_rate_hz)
# ------------------------------------------------------------- settings
def _build_chain(self) -> MultiAxisCausalFilter:
settings = self._settings
return MultiAxisCausalFilter(
median_window=settings.median_window,
lowpass_cutoff_hz=settings.lowpass_hz,
sample_rate_hz=settings.sample_rate_hz,
lowpass_order=settings.lowpass_order,
)
@property
def settings(self) -> OutputSettings:
"""The effective settings currently applied (rate included)."""
return self._settings
@property
def active(self) -> bool:
"""Any stage would change a valid reading."""
return bool(
self._raw_chain.enabled
or self._settings.raw_threshold > 0
or self._settings.newton_threshold > 0
)
def configure(self, settings: OutputSettings) -> bool:
"""Replace the setting group. State is dropped; the caller resets the stream."""
resolved = settings.normalised()
if resolved == self._settings:
return False
self._settings = resolved
self._raw_chain = self._build_chain()
self._newton_chain = self._build_chain()
self._rate.reset(resolved.sample_rate_hz)
return True
def reset(self) -> None:
"""Drop all filter memory (new zero, model change, discontinuity)."""
self._raw_chain.reset()
self._newton_chain.reset()
self._rate.reset(self._settings.sample_rate_hz)
# ---------------------------------------------------------- processing
def _track_signal_rate(self, signal_time_s: float | None) -> None:
"""Design the low-pass from signal time, never from processing speed."""
if signal_time_s is None:
return
if not np.isfinite(float(signal_time_s)):
return
rate = self._rate.tick(float(signal_time_s))
if not self._rate.measured:
return
current = self._settings.sample_rate_hz
if current > 0 and abs(rate - current) <= 0.1 * current:
return
self._settings = replace(self._settings, sample_rate_hz=rate)
self._raw_chain.configure(sample_rate_hz=rate)
self._newton_chain.configure(sample_rate_hz=rate)
def _apply_chain(
self,
chain: MultiAxisCausalFilter,
values,
valid,
*,
threshold: float,
) -> tuple[float, float, float]:
raw = tuple(float(value) for value in values)
if valid is None:
usable = np.isfinite(np.asarray(raw, dtype=np.float64))
else:
usable = np.asarray(valid, dtype=bool).reshape(-1)
if usable.size != 3:
raise ValueError(f"expected 3 validity flags, got {usable.size}")
processed = chain.process(raw, valid=usable)
out: list[float] = []
for index in range(3):
value = float(processed[index])
if not usable[index] or not np.isfinite(value):
out.append(float("nan"))
continue
if threshold > 0 and abs(value) <= threshold:
out.append(0.0)
else:
out.append(value)
return (out[0], out[1], out[2])
def process(
self,
raw,
newton,
*,
raw_valid=None,
newton_valid=None,
signal_time_s: float | None = None,
) -> tuple[tuple[float, float, float], tuple[float, float, float]]:
"""Process one frame's raw and newton triplets; returns both outputs."""
self._track_signal_rate(signal_time_s)
if not self.active:
# Exact pass-through, NaN and signed zero included.
return (
tuple(float(value) for value in raw),
tuple(float(value) for value in newton),
)
raw_out = self._apply_chain(
self._raw_chain,
raw,
raw_valid,
threshold=self._settings.raw_threshold,
)
newton_out = self._apply_chain(
self._newton_chain,
newton,
newton_valid,
threshold=self._settings.newton_threshold,
)
return raw_out, newton_out
def state(self) -> dict:
return {
"settings": self._settings.state(),
"raw_chain": self._raw_chain.state(),
"newton_chain": self._newton_chain.state(),
"sample_rate_source": (
"signal_time" if self._rate.measured else "declared_signal_rate"
),
}
def process_output_sequence(
settings: OutputSettings,
raw_samples,
newton_samples,
*,
raw_valid=None,
newton_valid=None,
signal_times_s=None,
) -> tuple[list[tuple[float, float, float]], list[tuple[float, float, float]], OutputSettings]:
"""Re-process stored unfiltered raw/N sequences through the SAME output chain.
This is the offline entry a replay/analysis pass uses to re-filter saved
unfiltered samples without a second median/low-pass implementation: it runs
the live chain in order from a clean state, including warm-up, so the result
equals what the session would have produced for that exact sample sequence.
``signal_times_s`` should carry the recorded signal times (the low-pass is
designed from signal time, never from how fast the re-analysis runs). When it
is omitted the declared ``settings.sample_rate_hz`` stays in effect. Returns
one output triplet per input sample, plus the effective settings.
"""
raw_rows = list(raw_samples)
newton_rows = list(newton_samples)
if len(raw_rows) != len(newton_rows):
raise ValueError(
"raw_samples and newton_samples must have the same length: "
f"{len(raw_rows)} != {len(newton_rows)}"
)
count = len(raw_rows)
raw_flags = None if raw_valid is None else list(raw_valid)
newton_flags = None if newton_valid is None else list(newton_valid)
times = None if signal_times_s is None else list(signal_times_s)
for name, rows in (
("raw_valid", raw_flags),
("newton_valid", newton_flags),
("signal_times_s", times),
):
if rows is not None and len(rows) != count:
raise ValueError(
f"{name} must have one entry per sample: {len(rows)} != {count}"
)
chain = ForceOutputFilter(settings)
raw_out: list[tuple[float, float, float]] = []
newton_out: list[tuple[float, float, float]] = []
for index in range(count):
processed_raw, processed_newton = chain.process(
raw_rows[index],
newton_rows[index],
raw_valid=None if raw_flags is None else raw_flags[index],
newton_valid=None if newton_flags is None else newton_flags[index],
signal_time_s=None if times is None else float(times[index]),
)
raw_out.append(processed_raw)
newton_out.append(processed_newton)
return raw_out, newton_out, chain.settings
File diff suppressed because it is too large Load Diff
+24
View File
@@ -157,6 +157,12 @@ class FrameSnapshot:
force_units: tuple[str, str, str] | None = None force_units: tuple[str, str, str] | None = None
calibration_status: dict[str, str] | None = None calibration_status: dict[str, str] | None = None
contact_found: bool | 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 @dataclass
@@ -231,6 +237,10 @@ class StreamPipeline:
#: Set by :meth:`reset` and consumed by the flow stage, so that resetting sensor state #: Set by :meth:`reset` and consumed by the flow stage, so that resetting sensor state
#: never races the stage that owns that state. #: never races the stage that owns that state.
self._reset_pending = threading.Event() 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; #: Pause state. ``_pause_level`` is what a pause would hold when armed;
#: ``_currently_capturing`` records whether the producer is still #: ``_currently_capturing`` records whether the producer is still
#: reading, which decides whether a resume has to rebase the tracker. #: reading, which decides whether a resume has to rebase the tracker.
@@ -316,6 +326,16 @@ class StreamPipeline:
with self._lock: with self._lock:
return self._latest 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: def reset(self) -> None:
"""Reset the sensor's tracker and force-filter state, safely. """Reset the sensor's tracker and force-filter state, safely.
@@ -570,6 +590,9 @@ class StreamPipeline:
if self._reset_pending.is_set(): if self._reset_pending.is_set():
self._reset_pending.clear() self._reset_pending.clear()
self.sensor.reset() 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 frame, t_cap, produce_s, source_metadata = item
t0 = time.perf_counter() t0 = time.perf_counter()
# The frame is passed explicitly: the SDK's self.img slot is shared # 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, hhd_ms=hhd_s * 1000.0,
image_size_raw=image_size_raw, image_size_raw=image_size_raw,
source_metadata=source_metadata, source_metadata=source_metadata,
tracker_reset_seq=int(self._tracker_reset_seq),
) )
def _fps(self) -> float: def _fps(self) -> float:
+222 -6
View File
@@ -8,6 +8,7 @@ and Helmholtz-Hodge decomposition.
# import os # only used by commented-out stitch_cal # import os # only used by commented-out stitch_cal
import time import time
from dataclasses import dataclass
from enum import Enum, auto from enum import Enum, auto
from pathlib import Path 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: class Sensor:
""" """
Optical tactile sensor interface with deformation analysis. Optical tactile sensor interface with deformation analysis.
@@ -49,6 +74,10 @@ class Sensor:
Supports both synchronous and asynchronous (pipelined) processing modes. 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) # Info Enum (for read_info API)
# ========================================================================= # =========================================================================
@@ -743,6 +772,16 @@ class Sensor:
fn, fx, fy, ftor = forces fn, fx, fy, ftor = forces
self._force_raw = (float(fn), float(fx), float(fy)) self._force_raw = (float(fn), float(fx), float(fy))
self.ftor = float(ftor) self.ftor = float(ftor)
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._force_readout.update_sample_rate() self._force_readout.update_sample_rate()
if self._uses_calibration_model(): if self._uses_calibration_model():
self._model_frame_id += 1 self._model_frame_id += 1
@@ -788,7 +827,7 @@ class Sensor:
self.compute_deformation_valid = True 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 # Spatial inference must use this HHD frame's contact, even when the
# caller did not request contact diagnostics from StreamPipeline. # caller did not request contact diagnostics from StreamPipeline.
# The field model needs no locator: it consumes this frame's field. # 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: if self._uses_calibration_model() and not self.compute_deformation_valid:
raise cal_mod.CalibrationError("compute current deformation before schema 3 contact") raise cal_mod.CalibrationError("compute current deformation before schema 3 contact")
if (self._uses_calibration_model() if (self._uses_calibration_model()
and not self._measurement_session
and self.calibration.input_profile == "fn_delta_autocal_uv/1" and self.calibration.input_profile == "fn_delta_autocal_uv/1"
and self.compute_contact_valid and self.compute_contact_valid
and self._calibration_result_frame == self._model_frame_id): and self._calibration_result_frame == self._model_frame_id):
@@ -855,6 +895,7 @@ class Sensor:
self.compute_contact_valid = True self.compute_contact_valid = True
if (self._uses_calibration_model() if (self._uses_calibration_model()
and not self._measurement_session
and self.calibration.input_profile == "fn_delta_autocal_uv/1"): and self.calibration.input_profile == "fn_delta_autocal_uv/1"):
self._model_contact_result = result self._model_contact_result = result
uv = self._calibration_contact_uv() uv = self._calibration_contact_uv()
@@ -865,6 +906,185 @@ class Sensor:
return result 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: def read_info(self, *args) -> tuple:
""" """
Get sensor data by info type. Get sensor data by info type.
@@ -920,14 +1140,10 @@ class Sensor:
def reset(self): def reset(self):
"""Reset optical flow tracker and force-filter state.""" """Reset optical flow tracker and force-filter state."""
self.__tracker.reset() self.reset_tracker()
self._force_readout.reset()
# The unfiltered pair belongs to the frame it was computed for: a new # The unfiltered pair belongs to the frame it was computed for: a new
# session must not report the previous one's reading. # session must not report the previous one's reading.
self._force_unfiltered = None
self._force_raw_unfiltered = None
self._reset_calibration_session() self._reset_calibration_session()
self.compute_deformation_valid = False
if self.img is not None: if self.img is not None:
self.initial_img = self.img.copy() self.initial_img = self.img.copy()
+137
View File
@@ -366,6 +366,143 @@ class CausalMedianFilter:
return np.median(self._buf, axis=0) 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: class SampleRateEstimator:
"""Robust online estimate of the rate at which :meth:`tick` is called. """Robust online estimate of the rate at which :meth:`tick` is called.
+82
View File
@@ -628,3 +628,85 @@ def test_every_preset_reports_what_it_does(
assert fp["measurement"]["flow_ema_alpha"] == pytest.approx(alpha) assert fp["measurement"]["flow_ema_alpha"] == pytest.approx(alpha)
assert fp["readout"]["force_median_window"] == median assert fp["readout"]["force_median_window"] == median
assert fp["readout"]["force_lowpass_cutoff_hz"] == pytest.approx(cutoff) 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
+219
View File
@@ -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
File diff suppressed because it is too large Load Diff
+163
View File
@@ -455,3 +455,166 @@ def test_finger_runtime_binds_field_identity_and_forwards_the_window(monkeypatch
runtime.declare_unloaded_reference("runtime-window") runtime.declare_unloaded_reference("runtime-window")
assert sensor.calibration.begin_calls == ["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"]
+74
View File
@@ -14,6 +14,7 @@ from orisys.signal_processing import (
NORMAL_FORCE_CAL_OFFSET, NORMAL_FORCE_CAL_OFFSET,
NORMAL_FORCE_CAL_SLOPE, NORMAL_FORCE_CAL_SLOPE,
LowpassFilter, LowpassFilter,
MultiAxisCausalFilter,
calibrate_normal_force, calibrate_normal_force,
calibrate_shear_components, calibrate_shear_components,
calibrate_shear_force, 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 # A dict always keys ``found_root``, so guard its value: an unguarded copy of
# this branch prints "bundle found at None". # this branch prints "bundle found at None".
assert "None" not in note 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))