ykcheungab
265 lines
9.6 KiB
Python
265 lines
9.6 KiB
Python
"""Tests for SDK signal helpers and Sensor.configure runtime settings."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import numpy as np
|
|
import pytest
|
|
from scipy.signal import butter, filtfilt
|
|
|
|
from orisys import Sensor
|
|
from orisys.signal_processing import (
|
|
NORMAL_FORCE_CAL_OFFSET,
|
|
NORMAL_FORCE_CAL_SLOPE,
|
|
LowpassFilter,
|
|
calibrate_normal_force,
|
|
calibrate_shear_components,
|
|
calibrate_shear_force,
|
|
lowpass_filter,
|
|
)
|
|
|
|
|
|
def _legacy_viewer_lowpass(data, cutoff_hz, sample_rate_hz=30.0, order=2):
|
|
"""Reference copy of the previous viewer2d Butterworth + filtfilt path."""
|
|
y = np.asarray(data, dtype=np.float64)
|
|
if cutoff_hz == 0:
|
|
return y.copy()
|
|
if y.size == 0:
|
|
return y
|
|
nyquist = 0.5 * sample_rate_hz
|
|
b, a = butter(order, cutoff_hz / nyquist, btype="low")
|
|
padlen = 3 * (max(len(b), len(a)) - 1)
|
|
if y.size <= padlen:
|
|
return y.copy()
|
|
return filtfilt(b, a, y)
|
|
|
|
|
|
def test_batch_lowpass_matches_legacy_viewer_filter():
|
|
rng = np.random.default_rng(0)
|
|
t = np.arange(180, dtype=np.float64) / 30.0
|
|
signal = np.sin(2 * np.pi * 1.0 * t) + 0.4 * rng.normal(size=t.size)
|
|
cutoff = 5.0
|
|
actual = lowpass_filter(signal, cutoff_hz=cutoff, sample_rate_hz=30.0, order=2)
|
|
expected = _legacy_viewer_lowpass(signal, cutoff_hz=cutoff, sample_rate_hz=30.0, order=2)
|
|
np.testing.assert_allclose(actual, expected, rtol=1e-10, atol=1e-10)
|
|
|
|
|
|
def test_batch_lowpass_cutoff_zero_is_noop():
|
|
data = np.array([1.0, 2.0, 3.0])
|
|
out = lowpass_filter(data, cutoff_hz=0)
|
|
np.testing.assert_array_equal(out, data)
|
|
assert out is not data
|
|
|
|
|
|
def test_stateful_lowpass_reduces_high_frequency_noise():
|
|
rng = np.random.default_rng(1)
|
|
n = 240
|
|
t = np.arange(n, dtype=np.float64) / 30.0
|
|
clean = np.sin(2 * np.pi * 0.5 * t)
|
|
noisy = clean + 0.8 * rng.normal(size=n)
|
|
|
|
filt = LowpassFilter(cutoff_hz=3.0, sample_rate_hz=30.0, order=2, n_channels=1)
|
|
filtered = np.array([float(filt.process([v])[0]) for v in noisy])
|
|
|
|
assert np.isfinite(filtered).all()
|
|
noisy_hf = float(np.std(np.diff(noisy)))
|
|
filtered_hf = float(np.std(np.diff(filtered)))
|
|
assert filtered_hf < 0.5 * noisy_hf
|
|
# Steady-state init on the first sample should not start at zero.
|
|
assert filtered[0] == pytest.approx(noisy[0])
|
|
|
|
|
|
def test_calibrate_normal_force_preserves_linear_curve():
|
|
raw = 12345.0
|
|
expected = NORMAL_FORCE_CAL_SLOPE * raw + NORMAL_FORCE_CAL_OFFSET
|
|
assert calibrate_normal_force(raw) == pytest.approx(expected)
|
|
assert calibrate_normal_force(0.0) == pytest.approx(NORMAL_FORCE_CAL_OFFSET)
|
|
|
|
|
|
def test_calibrate_normal_force_mini_uses_legacy_linear_file():
|
|
raw = 10000.0
|
|
expected = NORMAL_FORCE_CAL_SLOPE * raw + NORMAL_FORCE_CAL_OFFSET
|
|
assert calibrate_normal_force(raw, sensor_name="mini") == pytest.approx(expected)
|
|
|
|
|
|
def test_calibrate_normal_force_box_uses_piecewise_curve():
|
|
assert calibrate_normal_force(10000.0, sensor_name="box") == pytest.approx(0.377)
|
|
assert calibrate_normal_force(30000.0, sensor_name="box") == pytest.approx(1.377)
|
|
# Mid-segment interpolation between 10000→0.377 and 30000→1.377
|
|
mid = calibrate_normal_force(20000.0, sensor_name="box")
|
|
assert mid == pytest.approx(0.877)
|
|
# Below first breakpoint with extrapolate=true
|
|
assert calibrate_normal_force(0.0, sensor_name="box") == pytest.approx(0.0)
|
|
|
|
|
|
def test_calibrate_shear_force_box_piecewise():
|
|
assert calibrate_shear_force(1000.0, sensor_name="box") == pytest.approx(0.0)
|
|
assert calibrate_shear_force(36000.0, sensor_name="box") == pytest.approx(1.19)
|
|
assert calibrate_shear_force(68000.0, sensor_name="box") == pytest.approx(2.06)
|
|
|
|
|
|
def test_calibrate_shear_components_box_signed():
|
|
sx, sy = calibrate_shear_components(36000.0, -36000.0, sensor_name="box")
|
|
assert sx == pytest.approx(1.19)
|
|
assert sy == pytest.approx(-1.19)
|
|
|
|
|
|
def test_calibrate_shear_missing_file_returns_raw():
|
|
assert calibrate_shear_force(1234.5, sensor_name="no-such-sensor") == pytest.approx(1234.5)
|
|
sx, sy = calibrate_shear_components(1.0, -2.0, sensor_name="no-such-sensor")
|
|
assert (sx, sy) == pytest.approx((1.0, -2.0))
|
|
|
|
|
|
def test_calibrate_normal_force_unknown_sensor_falls_back_to_defaults():
|
|
raw = 5000.0
|
|
expected = NORMAL_FORCE_CAL_SLOPE * raw + NORMAL_FORCE_CAL_OFFSET
|
|
assert calibrate_normal_force(raw, sensor_name="no-such-sensor") == pytest.approx(expected)
|
|
|
|
|
|
def _external_sensor():
|
|
return Sensor("external", config_name="box", verbose=False, backend="cpu")
|
|
|
|
|
|
def test_configure_lowpass_filters_read_info_and_keeps_raw():
|
|
sensor = _external_sensor()
|
|
try:
|
|
sensor._force_raw = (1000.0, -200.0, 50.0)
|
|
sensor._apply_force_filter()
|
|
fn, fx, fy = sensor.read_info(
|
|
sensor.info.FNORMAL,
|
|
sensor.info.FSHEARX,
|
|
sensor.info.FSHEARY,
|
|
)
|
|
fn_raw, fx_raw, fy_raw = sensor.read_info(
|
|
sensor.info.FNORMAL_RAW,
|
|
sensor.info.FSHEARX_RAW,
|
|
sensor.info.FSHEARY_RAW,
|
|
)
|
|
assert (fn_raw, fx_raw, fy_raw) == pytest.approx((1000.0, -200.0, 50.0))
|
|
assert fn == pytest.approx(calibrate_normal_force(1000.0, sensor_name="box"))
|
|
assert (fx, fy) == pytest.approx(
|
|
calibrate_shear_components(-200.0, 50.0, sensor_name="box")
|
|
)
|
|
|
|
sensor.configure(lowpass_cutoff_hz=5.0, lowpass_sample_rate_hz=30.0)
|
|
noisy = [(1000.0 + (200.0 if i % 2 else -200.0), -200.0, 50.0) for i in range(40)]
|
|
filtered_fn = []
|
|
raw_fn = []
|
|
for sample in noisy:
|
|
sensor._force_raw = sample
|
|
sensor._apply_force_filter()
|
|
filtered_fn.append(sensor.read_info(sensor.info.FNORMAL)[0])
|
|
raw_fn.append(sensor.read_info(sensor.info.FNORMAL_RAW)[0])
|
|
|
|
assert raw_fn[-1] == pytest.approx(noisy[-1][0])
|
|
assert filtered_fn[-1] != pytest.approx(raw_fn[-1])
|
|
assert float(np.std(np.diff(filtered_fn))) < float(np.std(np.diff(raw_fn)))
|
|
fx_raw, fy_raw = sensor.read_info(sensor.info.FSHEARX_RAW, sensor.info.FSHEARY_RAW)
|
|
assert fx_raw == pytest.approx(-200.0)
|
|
assert fy_raw == pytest.approx(50.0)
|
|
finally:
|
|
sensor.disconnect()
|
|
|
|
|
|
def test_configure_lowpass_disabled_or_zero_cutoff_returns_raw():
|
|
sensor = _external_sensor()
|
|
try:
|
|
sensor.configure(lowpass_cutoff_hz=5.0)
|
|
sensor._force_raw = (10.0, 20.0, 30.0)
|
|
sensor._apply_force_filter()
|
|
|
|
sensor.configure(lowpass_cutoff_hz=0)
|
|
sensor._force_raw = (111.0, 222.0, 333.0)
|
|
sensor._apply_force_filter()
|
|
fn, fx, fy = sensor.read_info(
|
|
sensor.info.FNORMAL,
|
|
sensor.info.FSHEARX,
|
|
sensor.info.FSHEARY,
|
|
)
|
|
assert fn == pytest.approx(calibrate_normal_force(111.0, sensor_name="box"))
|
|
assert (fx, fy) == pytest.approx(
|
|
calibrate_shear_components(222.0, 333.0, sensor_name="box")
|
|
)
|
|
|
|
sensor.configure(lowpass_cutoff_hz=5.0, lowpass_enabled=False)
|
|
sensor._force_raw = (7.0, 8.0, 9.0)
|
|
sensor._apply_force_filter()
|
|
fn, fx, fy = sensor.read_info(
|
|
sensor.info.FNORMAL,
|
|
sensor.info.FSHEARX,
|
|
sensor.info.FSHEARY,
|
|
)
|
|
fn_raw = sensor.read_info(sensor.info.FNORMAL_RAW)[0]
|
|
assert fn == pytest.approx(calibrate_normal_force(7.0, sensor_name="box"))
|
|
assert (fx, fy) == pytest.approx(
|
|
calibrate_shear_components(8.0, 9.0, sensor_name="box")
|
|
)
|
|
assert fn_raw == pytest.approx(7.0)
|
|
finally:
|
|
sensor.disconnect()
|
|
|
|
|
|
def test_box_read_info_applies_force_calibration_json():
|
|
sensor = _external_sensor()
|
|
try:
|
|
assert sensor._force_cal_sensor_name == "box"
|
|
sensor._force_raw = (10000.0, 36000.0, -36000.0)
|
|
sensor._apply_force_filter()
|
|
fn, fx, fy = sensor.read_info(
|
|
sensor.info.FNORMAL,
|
|
sensor.info.FSHEARX,
|
|
sensor.info.FSHEARY,
|
|
)
|
|
fn_raw, fx_raw, fy_raw = sensor.read_info(
|
|
sensor.info.FNORMAL_RAW,
|
|
sensor.info.FSHEARX_RAW,
|
|
sensor.info.FSHEARY_RAW,
|
|
)
|
|
assert (fn_raw, fx_raw, fy_raw) == pytest.approx((10000.0, 36000.0, -36000.0))
|
|
assert fn == pytest.approx(0.377)
|
|
assert fx == pytest.approx(1.19)
|
|
assert fy == pytest.approx(-1.19)
|
|
finally:
|
|
sensor.disconnect()
|
|
|
|
|
|
def test_read_info_force_stays_raw_without_calibration_json(monkeypatch):
|
|
monkeypatch.setattr(
|
|
"orisys.sensor.configs.load_force_calibration",
|
|
lambda _name: None,
|
|
)
|
|
sensor = Sensor("external", config_name="box", verbose=False, backend="cpu")
|
|
try:
|
|
assert sensor._force_cal_sensor_name is None
|
|
sensor._force_raw = (111.0, -22.0, 33.0)
|
|
sensor._apply_force_filter()
|
|
fn, fx, fy = sensor.read_info(
|
|
sensor.info.FNORMAL,
|
|
sensor.info.FSHEARX,
|
|
sensor.info.FSHEARY,
|
|
)
|
|
assert (fn, fx, fy) == pytest.approx((111.0, -22.0, 33.0))
|
|
finally:
|
|
sensor.disconnect()
|
|
|
|
|
|
def test_configure_hhd_downsample_invalidates_solver_cache():
|
|
sensor = _external_sensor()
|
|
try:
|
|
sensor._hhd_downsample = 1
|
|
sensor._Sensor__hhd = object()
|
|
sensor._hhd_grid_shape = (40, 40)
|
|
sensor.valid_mask = np.ones((90, 90), dtype=bool)
|
|
sensor.valid_mask_hhd = sensor.valid_mask
|
|
|
|
sensor.configure(hhd_downsample=3)
|
|
assert sensor._hhd_downsample == 3
|
|
assert sensor._Sensor__hhd is None
|
|
assert sensor._hhd_grid_shape is None
|
|
assert sensor.valid_mask_hhd is not None
|
|
assert sensor.valid_mask_hhd.shape == (30, 30)
|
|
|
|
with pytest.raises(ValueError):
|
|
sensor.configure(hhd_downsample="nope")
|
|
finally:
|
|
sensor.disconnect()
|