added calibration data handling functions
This commit is contained in:
ykcheungab
@@ -11,6 +11,7 @@ __author__ = "Orisys"
|
||||
from .sensor import Sensor
|
||||
from . import util
|
||||
from . import configs
|
||||
from . import calibration
|
||||
from . import finger
|
||||
from . import signal_processing
|
||||
from . import capture
|
||||
@@ -22,6 +23,8 @@ CameraSensor = Sensor
|
||||
# Re-export list_configs as list_config for convenience
|
||||
list_config = configs.list_config
|
||||
export_config = configs.export_config
|
||||
export_sensor_bundle = configs.export_sensor_bundle
|
||||
export_sensor_bundles = configs.export_sensor_bundles
|
||||
|
||||
__all__ = [
|
||||
# class
|
||||
@@ -41,9 +44,14 @@ __all__ = [
|
||||
# capture helpers
|
||||
'capture',
|
||||
|
||||
# force calibration
|
||||
'calibration',
|
||||
|
||||
# configs utilities functions
|
||||
'list_config',
|
||||
'export_config',
|
||||
'export_sensor_bundle',
|
||||
'export_sensor_bundles',
|
||||
|
||||
# test
|
||||
'FFCFrameProcessor'
|
||||
|
||||
@@ -0,0 +1,782 @@
|
||||
"""Force calibration load, parse, check, build, and write.
|
||||
|
||||
Owns ``force_calibration.json`` schema v1/v2. ``Sensor`` loads once and applies
|
||||
curves in ``_apply_force_filter``; ``read_info(FNORMAL, …)`` returns Newtons.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import warnings
|
||||
from copy import deepcopy
|
||||
from dataclasses import asdict, dataclass, field
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import Any, Iterable, Mapping
|
||||
|
||||
CURRENT_SCHEMA = 2
|
||||
SUPPORTED_SCHEMAS = (1, 2)
|
||||
CHANNEL_IDS: tuple[str, ...] = ("fn", "fx", "fy")
|
||||
|
||||
CHANNEL_ALIASES: dict[str, str] = {
|
||||
"fn": "fn",
|
||||
"FN": "fn",
|
||||
"normal": "fn",
|
||||
"fnormal": "fn",
|
||||
"fx": "fx",
|
||||
"FX": "fx",
|
||||
"shear_x": "fx",
|
||||
"fshearx": "fx",
|
||||
"fy": "fy",
|
||||
"FY": "fy",
|
||||
"shear_y": "fy",
|
||||
"fsheary": "fy",
|
||||
}
|
||||
|
||||
_CANONICAL_TO_JSON = {
|
||||
"fn": "normal",
|
||||
"fx": "shear_x",
|
||||
"fy": "shear_y",
|
||||
}
|
||||
_JSON_RAW_KEY = {
|
||||
"normal": "fnormal",
|
||||
"shear_x": "fshearx",
|
||||
"shear_y": "fsheary",
|
||||
}
|
||||
_KNOWN_TOP_KEYS = frozenset(
|
||||
{"schema_version", "unit", "channels", "meta", "identity", "pipeline", "provenance", "measurements", "quality"}
|
||||
)
|
||||
|
||||
|
||||
class CalibrationError(ValueError):
|
||||
"""Invalid or unusable force calibration document / path."""
|
||||
|
||||
|
||||
def canonicalize_channel(name: str) -> str:
|
||||
"""Map FN / normal / shear_x / … to canonical ``fn`` / ``fx`` / ``fy``."""
|
||||
key = str(name or "").strip()
|
||||
if key in CHANNEL_ALIASES:
|
||||
return CHANNEL_ALIASES[key]
|
||||
lower = key.lower()
|
||||
if lower in CHANNEL_ALIASES:
|
||||
return CHANNEL_ALIASES[lower]
|
||||
raise CalibrationError(f"unknown calibration channel: {name!r}")
|
||||
|
||||
|
||||
def _parse_point(point: Any) -> tuple[float, float]:
|
||||
if isinstance(point, dict):
|
||||
raw = point.get("fnormal", point.get("raw", point.get("x")))
|
||||
force_n = point.get("force_n", point.get("n", point.get("y")))
|
||||
if raw is None or force_n is None:
|
||||
raise CalibrationError(f"invalid calibration point: {point!r}")
|
||||
return float(raw), float(force_n)
|
||||
if isinstance(point, (list, tuple)) and len(point) >= 2:
|
||||
return float(point[0]), float(point[1])
|
||||
raise CalibrationError(f"invalid calibration point: {point!r}")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ChannelCurve:
|
||||
"""One force channel: linear coeffs and/or piecewise-linear breakpoints."""
|
||||
|
||||
id: str
|
||||
method: str = "piecewise_linear"
|
||||
points: tuple[tuple[float, float], ...] = ()
|
||||
slope: float | None = None
|
||||
offset: float | None = None
|
||||
signed: bool = False
|
||||
extrapolate: bool = True
|
||||
clamp_output_min: float | None = 0.0
|
||||
raw_key: str = ""
|
||||
input_mode: str = "magnitude"
|
||||
unit: str = "N"
|
||||
|
||||
def convert(self, raw: float) -> float:
|
||||
raw = float(raw)
|
||||
if self.method in ("linear", "affine"):
|
||||
if self.slope is None or self.offset is None:
|
||||
return raw
|
||||
return float(self.slope) * raw + float(self.offset)
|
||||
|
||||
points = self.points
|
||||
if len(points) < 2:
|
||||
return raw
|
||||
|
||||
if raw <= points[0][0]:
|
||||
segment = (points[0], points[1])
|
||||
if not self.extrapolate:
|
||||
return self._clamp(points[0][1])
|
||||
elif raw >= points[-1][0]:
|
||||
segment = (points[-2], points[-1])
|
||||
if not self.extrapolate:
|
||||
return self._clamp(points[-1][1])
|
||||
else:
|
||||
segment = None
|
||||
for left, right in zip(points, points[1:]):
|
||||
if left[0] <= raw <= right[0]:
|
||||
segment = (left, right)
|
||||
break
|
||||
if segment is None:
|
||||
return self._clamp(points[-1][1])
|
||||
|
||||
(raw0, n0), (raw1, n1) = segment
|
||||
if raw1 == raw0:
|
||||
value = n0
|
||||
else:
|
||||
ratio = (raw - raw0) / (raw1 - raw0)
|
||||
value = n0 + ratio * (n1 - n0)
|
||||
return self._clamp(value)
|
||||
|
||||
def convert_component(self, raw: float) -> float:
|
||||
if not self.signed:
|
||||
return self.convert(raw)
|
||||
raw = float(raw)
|
||||
sign = -1.0 if raw < 0.0 else 1.0
|
||||
return sign * self.convert(abs(raw))
|
||||
|
||||
def _clamp(self, value: float) -> float:
|
||||
if self.clamp_output_min is not None:
|
||||
return max(float(self.clamp_output_min), float(value))
|
||||
return float(value)
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
json_name = _CANONICAL_TO_JSON.get(self.id, self.id)
|
||||
if self.method in ("linear", "affine"):
|
||||
block: dict[str, Any] = {
|
||||
"method": "linear",
|
||||
"slope": self.slope,
|
||||
"offset": self.offset,
|
||||
}
|
||||
else:
|
||||
block = {
|
||||
"method": "piecewise_linear",
|
||||
"raw_key": self.raw_key or _JSON_RAW_KEY.get(json_name, ""),
|
||||
"extrapolate": self.extrapolate,
|
||||
"clamp_output_min": self.clamp_output_min,
|
||||
"points": [list(p) for p in self.points],
|
||||
}
|
||||
if self.signed:
|
||||
block["signed"] = True
|
||||
if self.input_mode and self.input_mode != "magnitude":
|
||||
block["input"] = self.input_mode
|
||||
return block
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CalSample:
|
||||
weight_n: float
|
||||
raw_mean: float
|
||||
n: int
|
||||
std: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class CheckResult:
|
||||
ok: bool
|
||||
errors: list[str] = field(default_factory=list)
|
||||
warnings: list[str] = field(default_factory=list)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Calibration:
|
||||
"""Parsed force calibration document."""
|
||||
|
||||
schema_version: int
|
||||
unit: str
|
||||
origin: str
|
||||
identity: dict[str, Any]
|
||||
pipeline: dict[str, Any]
|
||||
provenance: dict[str, Any]
|
||||
channels: dict[str, ChannelCurve]
|
||||
measurements: dict[str, Any]
|
||||
extras: dict[str, Any] = field(default_factory=dict)
|
||||
source_path: Path | None = None
|
||||
# Legacy shared shear (v1) for magnitude-mode fallback in helpers.
|
||||
_legacy_shear: ChannelCurve | None = None
|
||||
|
||||
def get(self, channel: str) -> ChannelCurve | None:
|
||||
try:
|
||||
cid = canonicalize_channel(channel)
|
||||
except CalibrationError:
|
||||
return self.channels.get(str(channel))
|
||||
return self.channels.get(cid)
|
||||
|
||||
def has(self, channel: str) -> bool:
|
||||
return self.get(channel) is not None
|
||||
|
||||
def convert(self, fn: float, fx: float, fy: float) -> tuple[float, float, float]:
|
||||
"""Apply present curves; missing channel stays raw."""
|
||||
out_fn = float(fn)
|
||||
out_fx = float(fx)
|
||||
out_fy = float(fy)
|
||||
cal_fn = self.channels.get("fn")
|
||||
cal_fx = self.channels.get("fx")
|
||||
cal_fy = self.channels.get("fy")
|
||||
if cal_fn is not None:
|
||||
out_fn = cal_fn.convert(out_fn)
|
||||
if cal_fx is not None:
|
||||
out_fx = (
|
||||
cal_fx.convert_component(out_fx)
|
||||
if cal_fx.signed
|
||||
else cal_fx.convert(out_fx)
|
||||
)
|
||||
if cal_fy is not None:
|
||||
out_fy = (
|
||||
cal_fy.convert_component(out_fy)
|
||||
if cal_fy.signed
|
||||
else cal_fy.convert(out_fy)
|
||||
)
|
||||
return out_fn, out_fx, out_fy
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
channels: dict[str, Any] = {}
|
||||
for cid, curve in self.channels.items():
|
||||
json_name = _CANONICAL_TO_JSON.get(cid, cid)
|
||||
channels[json_name] = curve.to_dict()
|
||||
# Prefer emitting v2 axis names even when parsed from v1 shared shear.
|
||||
doc: dict[str, Any] = {
|
||||
"schema_version": max(int(self.schema_version), 2) if self.channels else int(self.schema_version),
|
||||
"unit": self.unit,
|
||||
"channels": channels,
|
||||
}
|
||||
meta: dict[str, Any] = {}
|
||||
meta.update(self.provenance)
|
||||
meta.update({k: v for k, v in self.identity.items() if k not in meta})
|
||||
for key in ("stitch_resolution", "dis_mode", "hhd_downsample", "config_ref"):
|
||||
if key in self.pipeline and self.pipeline[key] is not None:
|
||||
meta[key] = self.pipeline[key]
|
||||
if self.measurements:
|
||||
meta["samples"] = self.measurements
|
||||
if self.origin and "origin" not in meta:
|
||||
meta["origin"] = self.origin
|
||||
if meta:
|
||||
doc["meta"] = meta
|
||||
for key, value in self.extras.items():
|
||||
if key not in doc:
|
||||
doc[key] = deepcopy(value)
|
||||
return doc
|
||||
|
||||
|
||||
def _channel_from_block(
|
||||
channel_id: str,
|
||||
block: Any,
|
||||
*,
|
||||
unit: str = "N",
|
||||
default_method: str = "piecewise_linear",
|
||||
) -> ChannelCurve | None:
|
||||
if not isinstance(block, dict):
|
||||
return None
|
||||
method = str(block.get("method", block.get("model", default_method))).lower()
|
||||
if method in ("linear", "affine"):
|
||||
slope = block.get("slope")
|
||||
offset = block.get("offset")
|
||||
if slope is None or offset is None:
|
||||
return None
|
||||
return ChannelCurve(
|
||||
id=channel_id,
|
||||
method="linear",
|
||||
slope=float(slope),
|
||||
offset=float(offset),
|
||||
unit=str(block.get("unit", unit)),
|
||||
raw_key=str(block.get("raw_key", "")),
|
||||
)
|
||||
if method != "piecewise_linear":
|
||||
return None
|
||||
raw_points = block.get("points", [])
|
||||
if not isinstance(raw_points, (list, tuple)) or len(raw_points) < 2:
|
||||
return None
|
||||
try:
|
||||
points = tuple(sorted(_parse_point(p) for p in raw_points))
|
||||
except (TypeError, ValueError, CalibrationError):
|
||||
return None
|
||||
if len(points) < 2:
|
||||
return None
|
||||
return ChannelCurve(
|
||||
id=channel_id,
|
||||
method="piecewise_linear",
|
||||
points=points,
|
||||
extrapolate=bool(block.get("extrapolate", True)),
|
||||
clamp_output_min=block.get("clamp_output_min", 0.0),
|
||||
input_mode=str(block.get("input", "magnitude")).lower(),
|
||||
signed=bool(block.get("signed", False)),
|
||||
unit=str(block.get("unit", unit)),
|
||||
raw_key=str(block.get("raw_key", _JSON_RAW_KEY.get(_CANONICAL_TO_JSON.get(channel_id, ""), ""))),
|
||||
)
|
||||
|
||||
|
||||
def check(doc: dict | Calibration, *, pipeline: dict | None = None) -> CheckResult:
|
||||
"""Validate a document or Calibration. Errors refuse apply/write."""
|
||||
errors: list[str] = []
|
||||
warns: list[str] = []
|
||||
|
||||
if isinstance(doc, Calibration):
|
||||
data = doc.to_dict()
|
||||
channels_obj = doc.channels
|
||||
schema = doc.schema_version
|
||||
identity = doc.identity
|
||||
elif isinstance(doc, dict):
|
||||
data = doc
|
||||
schema_raw = data.get("schema_version", 1)
|
||||
try:
|
||||
schema = int(schema_raw)
|
||||
except (TypeError, ValueError):
|
||||
errors.append(f"invalid schema_version: {schema_raw!r}")
|
||||
return CheckResult(ok=False, errors=errors, warnings=warns)
|
||||
if schema not in SUPPORTED_SCHEMAS:
|
||||
errors.append(f"unsupported schema_version: {schema}")
|
||||
channels = data.get("channels")
|
||||
if not isinstance(channels, dict):
|
||||
errors.append("missing or invalid channels object")
|
||||
return CheckResult(ok=False, errors=errors, warnings=warns)
|
||||
channels_obj = {}
|
||||
unit = str(data.get("unit", "N"))
|
||||
for json_name, block in channels.items():
|
||||
try:
|
||||
cid = canonicalize_channel(json_name) if json_name != "shear" else "shear"
|
||||
except CalibrationError:
|
||||
# Future / unknown channel names: skip strict mapping.
|
||||
continue
|
||||
if json_name == "shear":
|
||||
curve = _channel_from_block("fx", block, unit=unit)
|
||||
else:
|
||||
curve = _channel_from_block(cid, block, unit=unit)
|
||||
if curve is None:
|
||||
method = None
|
||||
if isinstance(block, dict):
|
||||
method = block.get("method", block.get("model"))
|
||||
if isinstance(block, dict) and method not in (
|
||||
None,
|
||||
"piecewise_linear",
|
||||
"linear",
|
||||
"affine",
|
||||
):
|
||||
errors.append(f"unknown method for {json_name}: {method!r}")
|
||||
elif isinstance(block, dict):
|
||||
pts = block.get("points", [])
|
||||
if method in ("piecewise_linear", None) and (
|
||||
not isinstance(pts, (list, tuple)) or len(pts) < 2
|
||||
):
|
||||
errors.append(f"{json_name}: need at least 2 points")
|
||||
else:
|
||||
errors.append(f"invalid channel block: {json_name}")
|
||||
else:
|
||||
channels_obj[cid if json_name != "shear" else "fx"] = curve
|
||||
meta = data.get("meta") if isinstance(data.get("meta"), dict) else {}
|
||||
identity = {
|
||||
"sensor_type": meta.get("sensor_type", ""),
|
||||
"sn": meta.get("sn", ""),
|
||||
}
|
||||
else:
|
||||
return CheckResult(ok=False, errors=["document must be dict or Calibration"])
|
||||
|
||||
if not channels_obj and not errors:
|
||||
errors.append("no usable channel curves")
|
||||
|
||||
if "fn" not in channels_obj and "fx" not in channels_obj and "fy" not in channels_obj:
|
||||
if not errors:
|
||||
errors.append("no FN/FX/FY channels")
|
||||
|
||||
if "fx" not in channels_obj:
|
||||
warns.append("missing FX channel")
|
||||
if "fy" not in channels_obj:
|
||||
warns.append("missing FY channel")
|
||||
|
||||
sn = str(identity.get("sn") or "")
|
||||
origin = str(identity.get("origin") or data.get("meta", {}).get("origin", "") if isinstance(data, dict) else "")
|
||||
if not sn and origin in ("factory",):
|
||||
warns.append("empty sn on factory calibration")
|
||||
|
||||
if pipeline is not None and isinstance(doc, Calibration):
|
||||
for key in ("stitch_resolution", "dis_mode", "hhd_downsample"):
|
||||
if key in doc.pipeline and key in pipeline and doc.pipeline[key] != pipeline[key]:
|
||||
warns.append(f"pipeline mismatch: {key}")
|
||||
|
||||
return CheckResult(ok=not errors, errors=errors, warnings=warns)
|
||||
|
||||
|
||||
def parse(data: dict, *, source_path: Path | None = None) -> Calibration:
|
||||
"""Parse a force_calibration dict into a Calibration object."""
|
||||
if not isinstance(data, dict):
|
||||
raise CalibrationError("calibration document must be a JSON object")
|
||||
|
||||
schema_raw = data.get("schema_version", 1)
|
||||
try:
|
||||
schema = int(schema_raw)
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise CalibrationError(f"invalid schema_version: {schema_raw!r}") from exc
|
||||
if schema not in SUPPORTED_SCHEMAS:
|
||||
raise CalibrationError(f"unsupported schema_version: {schema}")
|
||||
|
||||
unit = str(data.get("unit", "N"))
|
||||
channels_raw = data.get("channels")
|
||||
if not isinstance(channels_raw, dict):
|
||||
raise CalibrationError("missing or invalid channels object")
|
||||
|
||||
channels: dict[str, ChannelCurve] = {}
|
||||
legacy_shear: ChannelCurve | None = None
|
||||
|
||||
# Prefer explicit axis channels (v2).
|
||||
for json_name, cid in (("normal", "fn"), ("shear_x", "fx"), ("shear_y", "fy")):
|
||||
if json_name in channels_raw:
|
||||
curve = _channel_from_block(cid, channels_raw[json_name], unit=unit)
|
||||
if curve is None:
|
||||
raise CalibrationError(f"invalid channel block: {json_name}")
|
||||
channels[cid] = curve
|
||||
|
||||
# v1 shared shear → both fx and fy when axes absent.
|
||||
if "shear" in channels_raw and ("fx" not in channels or "fy" not in channels):
|
||||
shear_block = channels_raw["shear"]
|
||||
legacy_shear = _channel_from_block("fx", shear_block, unit=unit)
|
||||
if legacy_shear is None:
|
||||
raise CalibrationError("invalid channel block: shear")
|
||||
# Copy with correct ids for each axis.
|
||||
shared = ChannelCurve(
|
||||
id="fx",
|
||||
method=legacy_shear.method,
|
||||
points=legacy_shear.points,
|
||||
slope=legacy_shear.slope,
|
||||
offset=legacy_shear.offset,
|
||||
signed=legacy_shear.signed or legacy_shear.input_mode == "components",
|
||||
extrapolate=legacy_shear.extrapolate,
|
||||
clamp_output_min=legacy_shear.clamp_output_min,
|
||||
raw_key=legacy_shear.raw_key,
|
||||
input_mode=legacy_shear.input_mode,
|
||||
unit=legacy_shear.unit,
|
||||
)
|
||||
if "fx" not in channels:
|
||||
channels["fx"] = shared
|
||||
if "fy" not in channels:
|
||||
channels["fy"] = ChannelCurve(
|
||||
id="fy",
|
||||
method=shared.method,
|
||||
points=shared.points,
|
||||
slope=shared.slope,
|
||||
offset=shared.offset,
|
||||
signed=shared.signed,
|
||||
extrapolate=shared.extrapolate,
|
||||
clamp_output_min=shared.clamp_output_min,
|
||||
raw_key=_JSON_RAW_KEY["shear_y"],
|
||||
input_mode=shared.input_mode,
|
||||
unit=shared.unit,
|
||||
)
|
||||
legacy_shear = shared
|
||||
|
||||
# Legacy single-channel file: {"normal_force": {"model": "linear", ...}}
|
||||
if "fn" not in channels:
|
||||
block = data.get("normal_force", data if "slope" in data else None)
|
||||
if isinstance(block, dict):
|
||||
curve = _channel_from_block("fn", block, unit=unit, default_method="linear")
|
||||
if curve is not None:
|
||||
channels["fn"] = curve
|
||||
|
||||
if not channels:
|
||||
raise CalibrationError("no usable channel curves")
|
||||
|
||||
meta = data.get("meta") if isinstance(data.get("meta"), dict) else {}
|
||||
identity_in = data.get("identity") if isinstance(data.get("identity"), dict) else {}
|
||||
pipeline_in = data.get("pipeline") if isinstance(data.get("pipeline"), dict) else {}
|
||||
provenance_in = data.get("provenance") if isinstance(data.get("provenance"), dict) else {}
|
||||
measurements_in = data.get("measurements") if isinstance(data.get("measurements"), dict) else {}
|
||||
|
||||
identity = {
|
||||
"sensor_type": identity_in.get("sensor_type", meta.get("sensor_type", "")),
|
||||
"sn": identity_in.get("sn", meta.get("sn", "")),
|
||||
"hardware_rev": identity_in.get("hardware_rev", meta.get("hardware_rev", "")),
|
||||
"gel_id": identity_in.get("gel_id", meta.get("gel_id", "")),
|
||||
}
|
||||
pipeline = {
|
||||
"stitch_resolution": pipeline_in.get(
|
||||
"stitch_resolution", meta.get("stitch_resolution")
|
||||
),
|
||||
"dis_mode": pipeline_in.get("dis_mode", meta.get("dis_mode")),
|
||||
"hhd_downsample": pipeline_in.get("hhd_downsample", meta.get("hhd_downsample")),
|
||||
"config_ref": pipeline_in.get("config_ref", meta.get("config_ref")),
|
||||
"config_hash": pipeline_in.get("config_hash", meta.get("config_hash")),
|
||||
}
|
||||
provenance = {
|
||||
"origin": provenance_in.get("origin", meta.get("origin", "")),
|
||||
"created_at": provenance_in.get(
|
||||
"created_at", meta.get("date", meta.get("created_at", ""))
|
||||
),
|
||||
"calibration_id": provenance_in.get("calibration_id", meta.get("calibration_id", "")),
|
||||
"tool": provenance_in.get("tool", meta.get("tool")),
|
||||
"operator": provenance_in.get("operator", meta.get("operator", "")),
|
||||
"station": provenance_in.get("station", meta.get("station", "")),
|
||||
"record_duration_s": provenance_in.get(
|
||||
"record_duration_s", meta.get("record_duration_s")
|
||||
),
|
||||
}
|
||||
measurements = measurements_in or meta.get("samples") or {}
|
||||
origin = str(provenance.get("origin") or "default")
|
||||
|
||||
extras = {k: deepcopy(v) for k, v in data.items() if k not in _KNOWN_TOP_KEYS}
|
||||
|
||||
cal = Calibration(
|
||||
schema_version=schema,
|
||||
unit=unit,
|
||||
origin=origin,
|
||||
identity=identity,
|
||||
pipeline=pipeline,
|
||||
provenance=provenance,
|
||||
channels=channels,
|
||||
measurements=measurements if isinstance(measurements, dict) else {},
|
||||
extras=extras,
|
||||
source_path=Path(source_path).resolve() if source_path is not None else None,
|
||||
_legacy_shear=legacy_shear,
|
||||
)
|
||||
result = check(cal)
|
||||
if not result.ok:
|
||||
raise CalibrationError("; ".join(result.errors))
|
||||
# Missing FX/FY are optional; leave them on CheckResult for callers.
|
||||
# Only surface non-channel warnings as Python warnings.
|
||||
for w in result.warnings:
|
||||
if w.startswith("missing F"):
|
||||
continue
|
||||
warnings.warn(f"calibration: {w}", stacklevel=2)
|
||||
return cal
|
||||
|
||||
|
||||
def load(
|
||||
source: str | Path | None = None,
|
||||
*,
|
||||
sensor_type: str | None = None,
|
||||
sn: str | None = None, # noqa: ARG001 — reserved for SN files
|
||||
origin: str | None = None, # noqa: ARG001
|
||||
strict: bool = False,
|
||||
) -> Calibration | None:
|
||||
"""Load calibration from an explicit path or product bundle.
|
||||
|
||||
Load order:
|
||||
1. ``source`` file path when it exists
|
||||
2. ``sensors/<sensor_type>/force_calibration.json``
|
||||
3. ``None`` (missing bundle file)
|
||||
|
||||
Explicit ``source`` that is missing or invalid raises when the path was
|
||||
provided (or when ``strict=True``). Missing bundle file returns ``None``.
|
||||
"""
|
||||
del sn, origin # reserved
|
||||
|
||||
path: Path | None = None
|
||||
explicit = source is not None and str(source).strip() != ""
|
||||
|
||||
if explicit:
|
||||
path = Path(str(source)).expanduser()
|
||||
if not path.is_file():
|
||||
msg = f"calibration file not found: {path}"
|
||||
if strict or explicit:
|
||||
raise CalibrationError(msg)
|
||||
return None
|
||||
elif sensor_type:
|
||||
from .configs import resolve_sensor_bundle
|
||||
|
||||
bundle = resolve_sensor_bundle(str(sensor_type))
|
||||
if bundle is not None and bundle.force_calibration_json is not None:
|
||||
path = bundle.force_calibration_json
|
||||
elif strict:
|
||||
raise CalibrationError(
|
||||
f"no force_calibration.json for sensor_type={sensor_type!r}"
|
||||
)
|
||||
else:
|
||||
return None
|
||||
else:
|
||||
if strict:
|
||||
raise CalibrationError("load() requires source or sensor_type")
|
||||
return None
|
||||
|
||||
assert path is not None
|
||||
try:
|
||||
data = json.loads(path.read_text(encoding="utf-8"))
|
||||
except Exception as exc:
|
||||
msg = f"failed to read calibration JSON: {path}: {exc}"
|
||||
if strict or explicit:
|
||||
raise CalibrationError(msg) from exc
|
||||
return None
|
||||
|
||||
try:
|
||||
return parse(data, source_path=path)
|
||||
except CalibrationError:
|
||||
if strict or explicit:
|
||||
raise
|
||||
return None
|
||||
|
||||
|
||||
def pipeline_from_sensor_json(path: str | Path | None) -> dict[str, Any]:
|
||||
"""Pull stitch / tracker fields from a sensor.json for export pipeline/meta."""
|
||||
pipeline: dict[str, Any] = {
|
||||
"stitch_resolution": None,
|
||||
"dis_mode": None,
|
||||
"hhd_downsample": None,
|
||||
"config_ref": None,
|
||||
}
|
||||
if path is None:
|
||||
return pipeline
|
||||
p = Path(path)
|
||||
pipeline["config_ref"] = str(p)
|
||||
if not p.is_file():
|
||||
return pipeline
|
||||
try:
|
||||
data = json.loads(p.read_text(encoding="utf-8"))
|
||||
except Exception:
|
||||
return pipeline
|
||||
if not isinstance(data, dict):
|
||||
return pipeline
|
||||
camera = data.get("camera") if isinstance(data.get("camera"), dict) else {}
|
||||
tracker = data.get("tracker") if isinstance(data.get("tracker"), dict) else {}
|
||||
stitch = camera.get("resolution_stitch")
|
||||
if isinstance(stitch, (list, tuple)) and len(stitch) >= 2:
|
||||
pipeline["stitch_resolution"] = [int(stitch[0]), int(stitch[1])]
|
||||
if "dis_mode" in tracker:
|
||||
try:
|
||||
pipeline["dis_mode"] = int(tracker["dis_mode"])
|
||||
except (TypeError, ValueError):
|
||||
pipeline["dis_mode"] = tracker.get("dis_mode")
|
||||
if "hhd_downsample" in tracker:
|
||||
try:
|
||||
pipeline["hhd_downsample"] = int(tracker["hhd_downsample"])
|
||||
except (TypeError, ValueError):
|
||||
pipeline["hhd_downsample"] = tracker.get("hhd_downsample")
|
||||
return pipeline
|
||||
|
||||
|
||||
def _piecewise_from_samples(
|
||||
channel_id: str,
|
||||
samples: Iterable[CalSample],
|
||||
*,
|
||||
signed: bool,
|
||||
) -> ChannelCurve | None:
|
||||
rows = sorted(samples, key=lambda s: abs(float(s.raw_mean)))
|
||||
if len(rows) < 2:
|
||||
return None
|
||||
points = tuple((abs(float(s.raw_mean)), float(s.weight_n)) for s in rows)
|
||||
json_name = _CANONICAL_TO_JSON[channel_id]
|
||||
return ChannelCurve(
|
||||
id=channel_id,
|
||||
method="piecewise_linear",
|
||||
points=points,
|
||||
signed=signed,
|
||||
extrapolate=True,
|
||||
clamp_output_min=0.0,
|
||||
raw_key=_JSON_RAW_KEY[json_name],
|
||||
input_mode="components" if signed else "magnitude",
|
||||
unit="N",
|
||||
)
|
||||
|
||||
|
||||
def build(
|
||||
samples: Mapping[str, list[CalSample]],
|
||||
*,
|
||||
identity: dict[str, Any] | None = None,
|
||||
pipeline: dict[str, Any] | None = None,
|
||||
origin: str = "user",
|
||||
extra_meta: dict[str, Any] | None = None,
|
||||
) -> Calibration:
|
||||
"""Fit piecewise curves from samples; omit a channel if fewer than 2 points."""
|
||||
channels: dict[str, ChannelCurve] = {}
|
||||
warnings_list: list[str] = []
|
||||
for key in CHANNEL_IDS:
|
||||
rows = list(samples.get(key, []) or samples.get(key.upper(), []) or [])
|
||||
# Also accept FN/FX/FY aliases in the mapping keys.
|
||||
if not rows:
|
||||
for alias, cid in CHANNEL_ALIASES.items():
|
||||
if cid == key and alias in samples:
|
||||
rows = list(samples[alias] or [])
|
||||
break
|
||||
signed = key in ("fx", "fy")
|
||||
curve = _piecewise_from_samples(key, rows, signed=signed)
|
||||
if curve is None:
|
||||
if rows:
|
||||
warnings_list.append(f"{key}: need at least 2 points (have {len(rows)})")
|
||||
else:
|
||||
warnings_list.append(f"{key}: no samples")
|
||||
continue
|
||||
channels[key] = curve
|
||||
|
||||
if not channels:
|
||||
raise CalibrationError(
|
||||
"No complete channels to build (each needs ≥2 points). "
|
||||
+ "; ".join(warnings_list)
|
||||
)
|
||||
|
||||
identity = dict(identity or {})
|
||||
pipeline = dict(pipeline or {})
|
||||
measurements: dict[str, list[dict[str, float | int]]] = {}
|
||||
for key in CHANNEL_IDS:
|
||||
rows = list(samples.get(key, []) or [])
|
||||
if not rows:
|
||||
for alias, cid in CHANNEL_ALIASES.items():
|
||||
if cid == key and alias in samples:
|
||||
rows = list(samples[alias] or [])
|
||||
break
|
||||
measurements[key] = [asdict(s) for s in rows]
|
||||
|
||||
provenance: dict[str, Any] = {
|
||||
"origin": origin,
|
||||
"created_at": datetime.now(timezone.utc).astimezone().isoformat(timespec="seconds"),
|
||||
}
|
||||
if extra_meta:
|
||||
for k, v in extra_meta.items():
|
||||
if k == "samples":
|
||||
continue
|
||||
if k in ("stitch_resolution", "dis_mode", "hhd_downsample", "config_ref"):
|
||||
if k not in pipeline or pipeline[k] is None:
|
||||
pipeline[k] = v
|
||||
elif k in ("sensor_type", "sn", "hardware_rev", "gel_id"):
|
||||
if k not in identity or not identity[k]:
|
||||
identity[k] = v
|
||||
else:
|
||||
provenance[k] = v
|
||||
|
||||
cal = Calibration(
|
||||
schema_version=CURRENT_SCHEMA,
|
||||
unit="N",
|
||||
origin=origin,
|
||||
identity=identity,
|
||||
pipeline=pipeline,
|
||||
provenance=provenance,
|
||||
channels=channels,
|
||||
measurements=measurements,
|
||||
extras={},
|
||||
source_path=None,
|
||||
)
|
||||
for w in warnings_list:
|
||||
warnings.warn(f"calibration build: {w}", stacklevel=2)
|
||||
return cal
|
||||
|
||||
|
||||
def write(
|
||||
cal: Calibration,
|
||||
path: str | Path,
|
||||
*,
|
||||
overwrite: bool = False,
|
||||
) -> Path:
|
||||
"""Write calibration JSON. Runs check() first; raises on errors."""
|
||||
result = check(cal)
|
||||
if not result.ok:
|
||||
raise CalibrationError("; ".join(result.errors))
|
||||
path = Path(path)
|
||||
if path.exists() and not overwrite:
|
||||
raise FileExistsError(f"calibration file exists: {path}")
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
doc = cal.to_dict()
|
||||
# Always emit schema 2 for new writes.
|
||||
doc["schema_version"] = CURRENT_SCHEMA
|
||||
path.write_text(
|
||||
json.dumps(doc, indent=2, ensure_ascii=False) + "\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
return path.resolve()
|
||||
|
||||
|
||||
def write_user(
|
||||
cal: Calibration,
|
||||
*,
|
||||
bundle_dir: str | Path,
|
||||
sn: str,
|
||||
overwrite: bool = True,
|
||||
) -> Path:
|
||||
"""Write a per-SN user calibration under ``bundle_dir/calibrations/<SN>.json``."""
|
||||
sn_clean = str(sn or "").strip()
|
||||
if not sn_clean:
|
||||
raise CalibrationError("write_user requires a non-empty sn")
|
||||
dest = Path(bundle_dir) / "calibrations" / f"{sn_clean}.json"
|
||||
return write(cal, dest, overwrite=overwrite)
|
||||
@@ -1,4 +1,5 @@
|
||||
import json
|
||||
import shutil
|
||||
from contextlib import ExitStack
|
||||
from dataclasses import dataclass
|
||||
from importlib.resources import as_file, files
|
||||
@@ -166,6 +167,85 @@ def load_force_calibration(name: str) -> dict | None:
|
||||
return _load_json(bundle.force_calibration_json)
|
||||
|
||||
|
||||
_BUNDLE_JSON_FILES = ("sensor.json", "force_calibration.json", "manifest.json")
|
||||
|
||||
|
||||
def export_sensor_bundle(
|
||||
name: str,
|
||||
dest_dir: str | Path,
|
||||
*,
|
||||
include_assets: bool = False,
|
||||
overwrite: bool = True,
|
||||
) -> Path:
|
||||
"""Copy one resolved ``sensors/<name>/`` bundle into ``dest_dir/<name>/``.
|
||||
|
||||
By default only JSON metadata is copied (``sensor.json``, and when present
|
||||
``force_calibration.json`` / ``manifest.json``). Pass ``include_assets=True``
|
||||
to copy the full tree (including ``assets/``) for packaging finger meshes.
|
||||
"""
|
||||
bundle = resolve_sensor_bundle(name)
|
||||
if bundle is None:
|
||||
available = ", ".join(list_sensor_bundles()) or "none"
|
||||
raise FileNotFoundError(
|
||||
f"Sensor bundle not found: {name!r}. Available: {available}"
|
||||
)
|
||||
|
||||
dest_root = Path(dest_dir).expanduser().resolve()
|
||||
dest = dest_root / bundle.name
|
||||
if dest.exists():
|
||||
if not overwrite:
|
||||
raise FileExistsError(f"Destination already exists: {dest}")
|
||||
if dest.is_dir():
|
||||
shutil.rmtree(dest)
|
||||
else:
|
||||
dest.unlink()
|
||||
|
||||
if include_assets:
|
||||
shutil.copytree(bundle.root, dest)
|
||||
return dest
|
||||
|
||||
dest.mkdir(parents=True, exist_ok=True)
|
||||
for filename in _BUNDLE_JSON_FILES:
|
||||
src = bundle.root / filename
|
||||
if src.is_file():
|
||||
shutil.copy2(src, dest / filename)
|
||||
if not (dest / "sensor.json").is_file():
|
||||
raise FileNotFoundError(f"sensor.json missing after export from {bundle.root}")
|
||||
return dest
|
||||
|
||||
|
||||
def export_sensor_bundles(
|
||||
dest_dir: str | Path,
|
||||
*,
|
||||
names: list[str] | tuple[str, ...] | None = None,
|
||||
include_assets: bool = False,
|
||||
overwrite: bool = True,
|
||||
) -> list[Path]:
|
||||
"""Export several sensor bundles into ``dest_dir``.
|
||||
|
||||
When ``names`` is omitted, exports every name in ``PRODUCT_BUNDLES`` that
|
||||
currently resolves via :func:`resolve_sensor_bundle`.
|
||||
"""
|
||||
if names is None:
|
||||
selected = [name for name in PRODUCT_BUNDLES if resolve_sensor_bundle(name) is not None]
|
||||
else:
|
||||
selected = [str(name) for name in names]
|
||||
if not selected:
|
||||
raise FileNotFoundError("No sensor bundles selected for export")
|
||||
|
||||
exported: list[Path] = []
|
||||
for name in selected:
|
||||
exported.append(
|
||||
export_sensor_bundle(
|
||||
name,
|
||||
dest_dir,
|
||||
include_assets=include_assets,
|
||||
overwrite=overwrite,
|
||||
)
|
||||
)
|
||||
return exported
|
||||
|
||||
|
||||
def load_config(config_name_or_path):
|
||||
"""
|
||||
Load sensor configuration from a JSON file path or config name.
|
||||
|
||||
@@ -0,0 +1,70 @@
|
||||
"""CLI: export packaged sensor bundles to a destination folder.
|
||||
|
||||
Examples::
|
||||
|
||||
python -m orisys.export_sensors --dest ./out/sensors --names box,mini
|
||||
python -m orisys.export_sensors --dest ./out/sensors --include-assets
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
from .configs import export_sensor_bundles, list_sensor_bundles
|
||||
|
||||
|
||||
def _parse_args(argv: list[str] | None = None) -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Export orisys sensors/<name>/ bundles to a destination directory"
|
||||
)
|
||||
parser.add_argument(
|
||||
"--dest",
|
||||
required=True,
|
||||
type=Path,
|
||||
help="Destination directory (creates <dest>/<name>/ for each bundle)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--names",
|
||||
default="",
|
||||
help="Comma-separated bundle names (default: all resolvable product bundles)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--include-assets",
|
||||
action="store_true",
|
||||
help="Copy full bundle trees including assets/",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-overwrite",
|
||||
action="store_true",
|
||||
help="Fail if a destination bundle folder already exists",
|
||||
)
|
||||
return parser.parse_args(argv)
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
args = _parse_args(argv)
|
||||
names = [part.strip() for part in str(args.names).split(",") if part.strip()] or None
|
||||
try:
|
||||
exported = export_sensor_bundles(
|
||||
args.dest,
|
||||
names=names,
|
||||
include_assets=bool(args.include_assets),
|
||||
overwrite=not bool(args.no_overwrite),
|
||||
)
|
||||
except (FileNotFoundError, FileExistsError) as exc:
|
||||
print(f"error: {exc}", file=sys.stderr)
|
||||
available = list_sensor_bundles()
|
||||
if available:
|
||||
print(f"available bundles: {', '.join(available)}", file=sys.stderr)
|
||||
return 1
|
||||
|
||||
for path in exported:
|
||||
print(f"exported {path}")
|
||||
print(f"exported {len(exported)} bundle(s) to {Path(args.dest).resolve()}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
+58
-22
@@ -19,6 +19,7 @@ from typing import Optional, Tuple
|
||||
from .Stitcher import Stitcher
|
||||
from .tracker import Tracker
|
||||
from .hhd import nHHD
|
||||
from . import calibration as cal_mod
|
||||
from . import configs
|
||||
from .get_depth import Contact3D
|
||||
from .backend_availability import (check_cupy_available, check_opencl_available)
|
||||
@@ -26,8 +27,6 @@ from .frame_processors import create_frame_processor
|
||||
from .signal_processing import (
|
||||
DEFAULT_SAMPLE_RATE_HZ,
|
||||
LowpassFilter,
|
||||
calibrate_normal_force,
|
||||
calibrate_shear_components,
|
||||
)
|
||||
from .capture import (
|
||||
CaptureConfig,
|
||||
@@ -87,7 +86,7 @@ class Sensor:
|
||||
isstitch: Enable stitching mode for multi-tile sensor (default: False)
|
||||
verbose: Print initialization logs (default: True)
|
||||
config_name: Configuration name (default: "prototype")
|
||||
cal_path: Path to calibration file (default: None)
|
||||
cal_path: Explicit force_calibration.json path (overrides bundle default)
|
||||
override: Skip auto-detection, use provided box (default: False)
|
||||
box: Manual bounding boxes when override=True (default: None)
|
||||
backend: Poisson backend (default: "auto")
|
||||
@@ -152,7 +151,9 @@ class Sensor:
|
||||
self.n_boxes = None
|
||||
self.fnormal = self.fshearx = self.fsheary = self.ftor = 0.0
|
||||
self._force_raw = (0.0, 0.0, 0.0)
|
||||
self._force_cal_sensor_name = self._resolve_force_calibration_sensor_name()
|
||||
self.calibration: Optional[cal_mod.Calibration] = None
|
||||
self._force_cal_sensor_name: Optional[str] = None
|
||||
self._init_force_calibration()
|
||||
self._lowpass_enabled = False
|
||||
self._lowpass_cutoff_hz = 0.0
|
||||
self._lowpass_sample_rate_hz = float(DEFAULT_SAMPLE_RATE_HZ)
|
||||
@@ -164,7 +165,9 @@ class Sensor:
|
||||
n_channels=3,
|
||||
)
|
||||
if self.verbose:
|
||||
if self._force_cal_sensor_name:
|
||||
if self.calibration is not None and self.calibration.source_path is not None:
|
||||
print(f"Force calibration: {self.calibration.source_path}")
|
||||
elif self._force_cal_sensor_name:
|
||||
print(
|
||||
f"Force calibration: sensors/{self._force_cal_sensor_name}/force_calibration.json"
|
||||
)
|
||||
@@ -802,17 +805,11 @@ class Sensor:
|
||||
self._update_hhd_mask_cache()
|
||||
return self
|
||||
|
||||
def _resolve_force_calibration_sensor_name(self) -> Optional[str]:
|
||||
"""Return sensors/<name> when force_calibration.json exists; else None.
|
||||
|
||||
Lookup order:
|
||||
1. ``config_name`` stem / product alias (e.g. ``box``)
|
||||
2. ``sensors/<name>/...`` path segment in ``config_name``
|
||||
Missing JSON means ``FNORMAL/FSHEAR*`` stay raw (no hardcoded fallback).
|
||||
"""
|
||||
def _bundle_name_candidates(self) -> list[str]:
|
||||
"""Product bundle names implied by ``config_name``."""
|
||||
text = str(self.config_name or "").strip().replace("\\", "/")
|
||||
if not text:
|
||||
return None
|
||||
return []
|
||||
|
||||
candidates: list[str] = []
|
||||
stem = Path(text).stem if text else ""
|
||||
@@ -828,15 +825,55 @@ class Sensor:
|
||||
candidates.append(name)
|
||||
|
||||
seen: set[str] = set()
|
||||
ordered: list[str] = []
|
||||
for candidate in candidates:
|
||||
alias = configs.CONFIG_ALIASES.get(candidate, candidate)
|
||||
for name in (alias, candidate):
|
||||
if name in seen:
|
||||
continue
|
||||
seen.add(name)
|
||||
if configs.load_force_calibration(name) is not None:
|
||||
return name
|
||||
return None
|
||||
ordered.append(name)
|
||||
return ordered
|
||||
|
||||
def _init_force_calibration(self) -> None:
|
||||
"""Load force calibration from ``cal_path`` or the product bundle."""
|
||||
self.calibration = None
|
||||
self._force_cal_sensor_name = None
|
||||
|
||||
if self.cal_path is not None and str(self.cal_path).strip():
|
||||
# Explicit path: missing/invalid raises (no silent fallback).
|
||||
self.calibration = cal_mod.load(self.cal_path, strict=True)
|
||||
return
|
||||
|
||||
for name in self._bundle_name_candidates():
|
||||
cal = cal_mod.load(sensor_type=name)
|
||||
if cal is not None:
|
||||
self.calibration = cal
|
||||
self._force_cal_sensor_name = name
|
||||
return
|
||||
|
||||
@property
|
||||
def force_calibrated(self) -> bool:
|
||||
"""True when at least one channel curve is loaded."""
|
||||
return self.calibration is not None and bool(self.calibration.channels)
|
||||
|
||||
@property
|
||||
def force_unit(self) -> str:
|
||||
return "N" if self.force_calibrated else "raw"
|
||||
|
||||
def set_calibration(
|
||||
self,
|
||||
cal: "cal_mod.Calibration | str | Path | None",
|
||||
) -> None:
|
||||
"""Attach or replace the active force calibration (live swap after save)."""
|
||||
if cal is None:
|
||||
self.calibration = None
|
||||
self._force_cal_sensor_name = None
|
||||
return
|
||||
if isinstance(cal, cal_mod.Calibration):
|
||||
self.calibration = cal
|
||||
return
|
||||
self.calibration = cal_mod.load(cal, strict=True)
|
||||
|
||||
def _apply_force_filter(self):
|
||||
raw = np.asarray(self._force_raw, dtype=np.float64)
|
||||
@@ -846,12 +883,11 @@ class Sensor:
|
||||
else:
|
||||
fn, fx, fy = float(raw[0]), float(raw[1]), float(raw[2])
|
||||
|
||||
# Apply sensors/<name>/force_calibration.json when present; otherwise keep raw.
|
||||
name = self._force_cal_sensor_name
|
||||
if name is not None:
|
||||
fn = float(calibrate_normal_force(fn, sensor_name=name))
|
||||
fx, fy = calibrate_shear_components(fx, fy, sensor_name=name)
|
||||
# Apply loaded force_calibration.json when present; otherwise keep raw.
|
||||
if self.calibration is not None:
|
||||
fn, fx, fy = self.calibration.convert(fn, fx, fy)
|
||||
fx, fy = float(fx), float(fy)
|
||||
fn = float(fn)
|
||||
|
||||
self.fnormal, self.fshearx, self.fsheary = fn, fx, fy
|
||||
|
||||
|
||||
+81
-146
@@ -3,10 +3,14 @@
|
||||
Batch ``lowpass_filter`` is the zero-phase Butterworth used by example charts.
|
||||
``LowpassFilter`` is a causal, stateful filter for per-frame ``Sensor`` outputs.
|
||||
|
||||
Force calibration resolution order (when register support arrives):
|
||||
1. Sensor register / EEPROM calibration blob
|
||||
2. ``sensors/<name>/force_calibration.json``
|
||||
3. Hardcoded linear defaults below
|
||||
Force calibration is owned by ``orisys.calibration``. These helpers remain as
|
||||
convenience wrappers:
|
||||
|
||||
1. Bundle JSON via ``calibration.load(sensor_type=...)``
|
||||
2. Hardcoded linear defaults for normal force when no file is present
|
||||
3. Shear helpers return raw when no curve is available
|
||||
|
||||
Prefer ``Sensor.calibration`` / ``read_info(FNORMAL, …)`` in new code.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
@@ -26,20 +30,15 @@ DEFAULT_LOWPASS_ORDER = 2
|
||||
|
||||
|
||||
def _parse_calibration_point(point) -> tuple[float, float]:
|
||||
if isinstance(point, dict):
|
||||
raw = point.get("fnormal", point.get("raw", point.get("x")))
|
||||
force_n = point.get("force_n", point.get("n", point.get("y")))
|
||||
if raw is None or force_n is None:
|
||||
raise ValueError(f"invalid calibration point: {point!r}")
|
||||
return float(raw), float(force_n)
|
||||
if isinstance(point, (list, tuple)) and len(point) >= 2:
|
||||
return float(point[0]), float(point[1])
|
||||
raise ValueError(f"invalid calibration point: {point!r}")
|
||||
"""Legacy helper; prefer ``orisys.calibration``."""
|
||||
from .calibration import _parse_point
|
||||
|
||||
return _parse_point(point)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ForceChannelCalibration:
|
||||
"""One force channel: linear coeffs and/or piecewise-linear breakpoints."""
|
||||
"""Compatibility wrapper around ``calibration.ChannelCurve`` convert math."""
|
||||
|
||||
method: str = "linear"
|
||||
slope: float | None = None
|
||||
@@ -63,75 +62,40 @@ class ForceChannelCalibration:
|
||||
unit: str = "N",
|
||||
default_method: str = "piecewise_linear",
|
||||
) -> ForceChannelCalibration | None:
|
||||
if not isinstance(block, dict):
|
||||
return None
|
||||
from .calibration import _channel_from_block
|
||||
|
||||
method = str(block.get("method", block.get("model", default_method))).lower()
|
||||
if method in ("linear", "affine"):
|
||||
slope = block.get("slope")
|
||||
offset = block.get("offset")
|
||||
if slope is None or offset is None:
|
||||
return None
|
||||
return cls.linear(float(slope), float(offset), unit=str(block.get("unit", unit)))
|
||||
|
||||
if method != "piecewise_linear":
|
||||
curve = _channel_from_block(
|
||||
"fn", block, unit=unit, default_method=default_method
|
||||
)
|
||||
if curve is None:
|
||||
return None
|
||||
|
||||
raw_points = block.get("points", [])
|
||||
if not isinstance(raw_points, (list, tuple)) or len(raw_points) < 2:
|
||||
return None
|
||||
try:
|
||||
points = tuple(sorted(_parse_calibration_point(p) for p in raw_points))
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
if len(points) < 2:
|
||||
return None
|
||||
|
||||
return cls(
|
||||
method="piecewise_linear",
|
||||
points=points,
|
||||
extrapolate=bool(block.get("extrapolate", True)),
|
||||
clamp_output_min=block.get("clamp_output_min", 0.0),
|
||||
input_mode=str(block.get("input", "magnitude")).lower(),
|
||||
signed=bool(block.get("signed", False)),
|
||||
unit=str(block.get("unit", unit)),
|
||||
method=curve.method,
|
||||
slope=curve.slope,
|
||||
offset=curve.offset,
|
||||
points=curve.points,
|
||||
extrapolate=curve.extrapolate,
|
||||
clamp_output_min=curve.clamp_output_min,
|
||||
input_mode=curve.input_mode,
|
||||
signed=curve.signed,
|
||||
unit=curve.unit,
|
||||
)
|
||||
|
||||
def convert(self, raw: float) -> float:
|
||||
raw = float(raw)
|
||||
if self.method == "linear":
|
||||
slope = NORMAL_FORCE_CAL_SLOPE if self.slope is None else float(self.slope)
|
||||
offset = NORMAL_FORCE_CAL_OFFSET if self.offset is None else float(self.offset)
|
||||
return slope * raw + offset
|
||||
from .calibration import ChannelCurve
|
||||
|
||||
points = self.points
|
||||
if len(points) < 2:
|
||||
return raw
|
||||
|
||||
if raw <= points[0][0]:
|
||||
segment = (points[0], points[1])
|
||||
if not self.extrapolate:
|
||||
return self._clamp(points[0][1])
|
||||
elif raw >= points[-1][0]:
|
||||
segment = (points[-2], points[-1])
|
||||
if not self.extrapolate:
|
||||
return self._clamp(points[-1][1])
|
||||
else:
|
||||
segment = None
|
||||
for left, right in zip(points, points[1:]):
|
||||
if left[0] <= raw <= right[0]:
|
||||
segment = (left, right)
|
||||
break
|
||||
if segment is None:
|
||||
return self._clamp(points[-1][1])
|
||||
|
||||
(raw0, n0), (raw1, n1) = segment
|
||||
if raw1 == raw0:
|
||||
value = n0
|
||||
else:
|
||||
ratio = (raw - raw0) / (raw1 - raw0)
|
||||
value = n0 + ratio * (n1 - n0)
|
||||
return self._clamp(value)
|
||||
return ChannelCurve(
|
||||
id="fn",
|
||||
method=self.method,
|
||||
points=self.points,
|
||||
slope=self.slope if self.slope is not None else NORMAL_FORCE_CAL_SLOPE,
|
||||
offset=self.offset if self.offset is not None else NORMAL_FORCE_CAL_OFFSET,
|
||||
signed=self.signed,
|
||||
extrapolate=self.extrapolate,
|
||||
clamp_output_min=self.clamp_output_min,
|
||||
input_mode=self.input_mode,
|
||||
unit=self.unit,
|
||||
).convert(raw)
|
||||
|
||||
def convert_component(self, raw: float) -> float:
|
||||
if not self.signed:
|
||||
@@ -140,11 +104,6 @@ class ForceChannelCalibration:
|
||||
sign = -1.0 if raw < 0.0 else 1.0
|
||||
return sign * self.convert(abs(raw))
|
||||
|
||||
def _clamp(self, value: float) -> float:
|
||||
if self.clamp_output_min is not None:
|
||||
return max(float(self.clamp_output_min), float(value))
|
||||
return float(value)
|
||||
|
||||
|
||||
_DEFAULT_NORMAL = ForceChannelCalibration.linear(
|
||||
NORMAL_FORCE_CAL_SLOPE,
|
||||
@@ -152,57 +111,41 @@ _DEFAULT_NORMAL = ForceChannelCalibration.linear(
|
||||
)
|
||||
|
||||
|
||||
def _load_force_calibration_data(sensor_name: str | None) -> dict | None:
|
||||
def _load_calibration(sensor_name: str | None):
|
||||
if not sensor_name:
|
||||
return None
|
||||
try:
|
||||
from .configs import load_force_calibration
|
||||
from .calibration import load
|
||||
|
||||
data = load_force_calibration(sensor_name)
|
||||
return load(sensor_type=sensor_name)
|
||||
except Exception:
|
||||
return None
|
||||
return data if isinstance(data, dict) else None
|
||||
|
||||
|
||||
def _channel_from_data(data: dict | None, channel: str) -> ForceChannelCalibration | None:
|
||||
if not isinstance(data, dict):
|
||||
return None
|
||||
|
||||
unit = str(data.get("unit", "N"))
|
||||
channels = data.get("channels")
|
||||
if isinstance(channels, dict):
|
||||
block = channels.get(channel)
|
||||
cal = ForceChannelCalibration.from_channel_dict(block, unit=unit)
|
||||
if cal is not None:
|
||||
return cal
|
||||
|
||||
# Legacy single-channel file: {"normal_force": {"model": "linear", ...}}
|
||||
if channel == "normal":
|
||||
block = data.get("normal_force", data)
|
||||
cal = ForceChannelCalibration.from_channel_dict(
|
||||
block, unit=unit, default_method="linear"
|
||||
)
|
||||
if cal is not None:
|
||||
return cal
|
||||
if isinstance(block, dict):
|
||||
slope = block.get("slope")
|
||||
offset = block.get("offset")
|
||||
if slope is not None and offset is not None:
|
||||
return ForceChannelCalibration.linear(
|
||||
float(slope),
|
||||
float(offset),
|
||||
unit=str(block.get("unit", unit)),
|
||||
)
|
||||
return None
|
||||
|
||||
|
||||
def _normal_calibration(sensor_name: str | None = None) -> ForceChannelCalibration:
|
||||
cal = _channel_from_data(_load_force_calibration_data(sensor_name), "normal")
|
||||
return cal if cal is not None else _DEFAULT_NORMAL
|
||||
cal = _load_calibration(sensor_name)
|
||||
if cal is not None:
|
||||
curve = cal.get("fn")
|
||||
if curve is not None:
|
||||
return ForceChannelCalibration(
|
||||
method=curve.method,
|
||||
slope=curve.slope,
|
||||
offset=curve.offset,
|
||||
points=curve.points,
|
||||
extrapolate=curve.extrapolate,
|
||||
clamp_output_min=curve.clamp_output_min,
|
||||
input_mode=curve.input_mode,
|
||||
signed=curve.signed,
|
||||
unit=curve.unit,
|
||||
)
|
||||
return _DEFAULT_NORMAL
|
||||
|
||||
|
||||
def _shear_calibration(sensor_name: str | None = None) -> ForceChannelCalibration | None:
|
||||
return _channel_from_data(_load_force_calibration_data(sensor_name), "shear")
|
||||
def _shear_curve(sensor_name: str | None, channel: str):
|
||||
cal = _load_calibration(sensor_name)
|
||||
if cal is None:
|
||||
return None
|
||||
return cal.get(channel)
|
||||
|
||||
|
||||
def _load_normal_force_coeffs(sensor_name: str | None = None) -> tuple[float, float]:
|
||||
@@ -225,12 +168,20 @@ def calibrate_normal_force(raw_value: float, *, sensor_name: str | None = None)
|
||||
def calibrate_shear_force(raw_value: float, *, sensor_name: str | None = None) -> float:
|
||||
"""Convert a raw shear magnitude to Newtons using the shear channel curve.
|
||||
|
||||
Returns ``float(raw_value)`` unchanged when no shear calibration is available.
|
||||
Prefers the average of FX/FY magnitude maps when present.
|
||||
Returns ``float(raw_value)`` when unavailable.
|
||||
"""
|
||||
cal = _shear_calibration(sensor_name)
|
||||
if cal is None:
|
||||
return float(raw_value)
|
||||
return cal.convert(raw_value)
|
||||
cal_x = _shear_curve(sensor_name, "fx")
|
||||
cal_y = _shear_curve(sensor_name, "fy")
|
||||
if cal_x is not None and cal_y is not None:
|
||||
return 0.5 * (
|
||||
cal_x.convert(abs(float(raw_value))) + cal_y.convert(abs(float(raw_value)))
|
||||
)
|
||||
if cal_x is not None:
|
||||
return cal_x.convert(abs(float(raw_value)))
|
||||
if cal_y is not None:
|
||||
return cal_y.convert(abs(float(raw_value)))
|
||||
return float(raw_value)
|
||||
|
||||
|
||||
def calibrate_shear_components(
|
||||
@@ -239,27 +190,11 @@ def calibrate_shear_components(
|
||||
*,
|
||||
sensor_name: str | None = None,
|
||||
) -> tuple[float, float]:
|
||||
"""Convert raw shear components to Newtons.
|
||||
|
||||
When the shear channel uses ``input: components`` and ``signed: true``, each
|
||||
axis is calibrated on absolute magnitude then re-signed. Otherwise the
|
||||
magnitude is calibrated and components are scaled proportionally.
|
||||
"""
|
||||
sx = float(shear_x)
|
||||
sy = float(shear_y)
|
||||
cal = _shear_calibration(sensor_name)
|
||||
"""Convert raw shear components to Newtons via ``calibration.load`` curves."""
|
||||
cal = _load_calibration(sensor_name)
|
||||
if cal is None:
|
||||
return sx, sy
|
||||
|
||||
if cal.input_mode == "components":
|
||||
return cal.convert_component(sx), cal.convert_component(sy)
|
||||
|
||||
magnitude = (sx * sx + sy * sy) ** 0.5
|
||||
if magnitude <= 0.0:
|
||||
return 0.0, 0.0
|
||||
calibrated = cal.convert(magnitude)
|
||||
scale = calibrated / magnitude
|
||||
return sx * scale, sy * scale
|
||||
return float(shear_x), float(shear_y)
|
||||
return cal.convert(0.0, float(shear_x), float(shear_y))[1:]
|
||||
|
||||
|
||||
def _nyquist_hz(sample_rate_hz: float) -> float:
|
||||
|
||||
Reference in New Issue
Block a user