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