included low pass filter in package and add to plot_data example

2026-08-17 22:11:17 +08:00
parent 9a69f01278
commit 2e09c9360e
50 changed files with 1055 additions and 1873 deletions
@@ -0,0 +1,163 @@
+"""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,
+    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 _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,
+        )
+        assert (fn, fx, fy) == pytest.approx((1000.0, -200.0, 50.0))
+
+        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, fx, fy) == pytest.approx((111.0, 222.0, 333.0))
+
+        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, fx, fy) == pytest.approx((7.0, 8.0, 9.0))
+        assert fn_raw == pytest.approx(7.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()