新增统一力读数会话 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:
+9
-3
@@ -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
|
||||||
|
|||||||
@@ -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` 不同是唯一允许的管线身份差异,其余差异仍拒绝并报告。
|
||||||
@@ -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',
|
||||||
|
|||||||
@@ -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():
|
||||||
|
|||||||
@@ -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()
|
||||||
|
|||||||
@@ -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."""
|
||||||
|
|||||||
@@ -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:
|
||||||
|
|||||||
@@ -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
@@ -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:
|
||||||
|
|||||||
+230
-14
@@ -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,16 +772,26 @@ 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)
|
||||||
self._force_readout.update_sample_rate()
|
if self._measurement_session:
|
||||||
if self._uses_calibration_model():
|
# Session path: publish the same-frame unfiltered feature only. Model
|
||||||
self._model_frame_id += 1
|
# inference and readout filtering belong to orisys.force_session, so
|
||||||
self.compute_contact_valid = False
|
# the old Newton accessors must not report a value the session did not
|
||||||
self.centroid_found = False
|
# produce (they would be stale or half-processed).
|
||||||
self._force_readout.apply(self._force_raw)
|
self.fnormal = self.fshearx = self.fsheary = float("nan")
|
||||||
self._force_filtered_raw = self._force_readout.filtered_raw
|
self._force_filtered_raw = None
|
||||||
|
self._force_unfiltered = None
|
||||||
self._force_raw_unfiltered = self._force_raw
|
self._force_raw_unfiltered = self._force_raw
|
||||||
else:
|
else:
|
||||||
self._apply_force_filter()
|
self._force_readout.update_sample_rate()
|
||||||
|
if self._uses_calibration_model():
|
||||||
|
self._model_frame_id += 1
|
||||||
|
self.compute_contact_valid = False
|
||||||
|
self.centroid_found = False
|
||||||
|
self._force_readout.apply(self._force_raw)
|
||||||
|
self._force_filtered_raw = self._force_readout.filtered_raw
|
||||||
|
self._force_raw_unfiltered = self._force_raw
|
||||||
|
else:
|
||||||
|
self._apply_force_filter()
|
||||||
|
|
||||||
vnormal_ds = self.__hhd.get_normal()
|
vnormal_ds = self.__hhd.get_normal()
|
||||||
vshear_ds = self.__hhd.get_shear()
|
vshear_ds = self.__hhd.get_shear()
|
||||||
@@ -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()
|
||||||
|
|
||||||
|
|||||||
@@ -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.
|
||||||
|
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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
@@ -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"]
|
||||||
|
|||||||
@@ -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))
|
||||||
|
|||||||
Reference in New Issue
Block a user