included low pass filter in package and add to plot_data example

src/orisys/sensor.py

@@ -23,6 +23,7 @@ from . import configs
 from .get_depth import Contact3D
 from .backend_availability import (check_cupy_available, check_opencl_available)
 from .frame_processors import create_frame_processor
+from .signal_processing import DEFAULT_SAMPLE_RATE_HZ, LowpassFilter
 
 class Sensor:
     """
@@ -46,6 +47,9 @@ class Sensor:
         FNORMAL = auto()
         FSHEARX = auto()
         FSHEARY = auto()
+        FNORMAL_RAW = auto()
+        FSHEARX_RAW = auto()
+        FSHEARY_RAW = auto()
         CENTROID = auto()
         CONTOUR = auto()
         DEPTH = auto()
@@ -126,6 +130,17 @@ class Sensor:
         self.vshear = self.vtemplate
         self.n_boxes = None
         self.fnormal = self.fshearx = self.fsheary = self.ftor = 0.0
+        self._force_raw = (0.0, 0.0, 0.0)
+        self._lowpass_enabled = False
+        self._lowpass_cutoff_hz = 0.0
+        self._lowpass_sample_rate_hz = float(DEFAULT_SAMPLE_RATE_HZ)
+        self._lowpass_order = 2
+        self._force_filter = LowpassFilter(
+            cutoff_hz=self._lowpass_cutoff_hz,
+            sample_rate_hz=self._lowpass_sample_rate_hz,
+            order=self._lowpass_order,
+            n_channels=3,
+        )
 
         # Contact Data Variables (public)
         self.centroid = (0, 0)
@@ -529,7 +544,10 @@ class Sensor:
             self._hhd_grid_shape = ds_shape
 
         _, forces = self.__hhd.get_force(flow_ds, valid_mask=mask_ds)
-        self.fnormal, self.fshearx, self.fsheary, self.ftor = forces
+        fn, fx, fy, ftor = forces
+        self._force_raw = (float(fn), float(fx), float(fy))
+        self.ftor = float(ftor)
+        self._apply_force_filter()
 
         vnormal_ds = self.__hhd.get_normal()
         vshear_ds = self.__hhd.get_shear()
@@ -631,6 +649,9 @@ class Sensor:
             self.info.FNORMAL: lambda: self.fnormal,
             self.info.FSHEARX: lambda: self.fshearx,
             self.info.FSHEARY: lambda: self.fsheary,
+            self.info.FNORMAL_RAW: lambda: self._force_raw[0],
+            self.info.FSHEARX_RAW: lambda: self._force_raw[1],
+            self.info.FSHEARY_RAW: lambda: self._force_raw[2],
             self.info.FPS: lambda: self._fps,
             self.info.TIMESTAMP: lambda: time.strftime('%H:%M:%S'),
             # Return copies of arrays to prevent external modification
@@ -649,8 +670,9 @@ class Sensor:
         return tuple(mapping.get(arg, lambda: None)() for arg in args if isinstance(arg, self.info))
 
     def reset(self):
-        """Reset optical flow tracker."""
+        """Reset optical flow tracker and force-filter state."""
         self.__tracker.reset()
+        self._force_filter.reset()
         if self.img is not None:
             self.initial_img = self.img.copy()
 
@@ -679,7 +701,88 @@ class Sensor:
     # =========================================================================
     # Public API - Configuration & Calibration
     # =========================================================================
-    
+
+    def configure(
+        self,
+        *,
+        lowpass_cutoff_hz=None,
+        lowpass_sample_rate_hz=None,
+        lowpass_order=None,
+        lowpass_enabled=None,
+        hhd_downsample=None,
+    ):
+        """Update runtime-safe settings without reconnecting the camera.
+
+        Parameters
+        ----------
+        lowpass_cutoff_hz : float, optional
+            Force low-pass cutoff in Hz. ``0`` disables filtering. A positive
+            value enables filtering unless ``lowpass_enabled=False``.
+        lowpass_sample_rate_hz : float, optional
+            Assumed force sample rate for the Butterworth design (default 30).
+        lowpass_order : int, optional
+            Butterworth order (default 2).
+        lowpass_enabled : bool, optional
+            Explicit on/off for the causal force filter.
+        hhd_downsample : int, optional
+            Integer ``>= 1``. Rebuilds the HHD solver on the next frame.
+        """
+        filter_changed = False
+        if lowpass_cutoff_hz is not None:
+            self._lowpass_cutoff_hz = float(lowpass_cutoff_hz)
+            filter_changed = True
+            if lowpass_enabled is None:
+                self._lowpass_enabled = self._lowpass_cutoff_hz > 0
+        if lowpass_sample_rate_hz is not None:
+            rate = float(lowpass_sample_rate_hz)
+            if rate <= 0:
+                raise ValueError("lowpass_sample_rate_hz must be > 0")
+            self._lowpass_sample_rate_hz = rate
+            filter_changed = True
+        if lowpass_order is not None:
+            order = int(lowpass_order)
+            if order < 1:
+                raise ValueError("lowpass_order must be >= 1")
+            self._lowpass_order = order
+            filter_changed = True
+        if lowpass_enabled is not None:
+            self._lowpass_enabled = bool(lowpass_enabled)
+            filter_changed = True
+
+        if filter_changed:
+            cutoff = self._lowpass_cutoff_hz if self._lowpass_enabled else 0.0
+            self._force_filter.configure(
+                cutoff_hz=cutoff,
+                sample_rate_hz=self._lowpass_sample_rate_hz,
+                order=self._lowpass_order,
+            )
+            if self.compute_deformation_valid:
+                self._apply_force_filter()
+
+        if hhd_downsample is not None:
+            try:
+                k = max(1, int(hhd_downsample))
+            except (TypeError, ValueError) as exc:
+                raise ValueError("hhd_downsample must be an integer >= 1") from exc
+            if k != self._hhd_downsample:
+                self._hhd_downsample = k
+                self.__hhd = None
+                self._hhd_grid_shape = None
+                self._update_hhd_mask_cache()
+        return self
+
+    def _apply_force_filter(self):
+        raw = np.asarray(self._force_raw, dtype=np.float64)
+        if self._lowpass_enabled and self._lowpass_cutoff_hz > 0:
+            filtered = self._force_filter.process(raw)
+            self.fnormal, self.fshearx, self.fsheary = (float(v) for v in filtered)
+        else:
+            self.fnormal, self.fshearx, self.fsheary = (
+                float(raw[0]),
+                float(raw[1]),
+                float(raw[2]),
+            )
+
     # def export_config(self, path="./config/exported_config.json"):
     #     """Return current configuration dict."""
     #     configs.export_config(self._config, path)