added calibration data handling functions
This commit is contained in:
ykcheungab
+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