from dataclasses import dataclass import time from .filters import ExponentialFilter def shear_magnitude(sample): return (float(sample["fshearx"]) ** 2 + float(sample["fsheary"]) ** 2) ** 0.5 def _component_shear_enabled(converter): return getattr(converter, "input_mode", "magnitude") == "components" @dataclass class ForceBaseline: left_normal: float = 0.0 right_normal: float = 0.0 left_shear: float = 0.0 right_shear: float = 0.0 left_shear_x: float = 0.0 left_shear_y: float = 0.0 right_shear_x: float = 0.0 right_shear_y: float = 0.0 @dataclass class ForceFeedback: left_normal: float right_normal: float min_normal: float max_normal: float normal_diff: float left_shear: float right_shear: float max_shear: float shear_diff: float left_shear_x: float = 0.0 left_shear_y: float = 0.0 right_shear_x: float = 0.0 right_shear_y: float = 0.0 class TactileFeedbackProcessor: def __init__(self, config, normal_converter, shear_converter): self.config = config self.normal_converter = normal_converter self.shear_converter = shear_converter self.baseline = ForceBaseline() self.reset_filters() def reset_filters(self): self.left_normal_filter = ExponentialFilter(self.config.filter_alpha) self.right_normal_filter = ExponentialFilter(self.config.filter_alpha) self.left_shear_filter = ExponentialFilter(self.config.shear_filter_alpha) self.right_shear_filter = ExponentialFilter(self.config.shear_filter_alpha) self.left_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha) self.left_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha) self.right_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha) self.right_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha) def _convert_shear(self, sample): if _component_shear_enabled(self.shear_converter): shear_x = self.shear_converter.convert_component(sample["fshearx"]) shear_y = self.shear_converter.convert_component(sample["fsheary"]) return (shear_x ** 2 + shear_y ** 2) ** 0.5, shear_x, shear_y return self.shear_converter.convert(shear_magnitude(sample)), 0.0, 0.0 def calibrate_baseline(self, reader): seconds = self.config.baseline_seconds if seconds <= 0: self.baseline = ForceBaseline() return self.baseline left_values = [] right_values = [] left_shear_values = [] right_shear_values = [] left_shear_x_values = [] left_shear_y_values = [] right_shear_x_values = [] right_shear_y_values = [] deadline = time.perf_counter() + seconds while time.perf_counter() < deadline: left, right = reader.get_samples(timeout=0.2) if left is not None: left_values.append(self.normal_converter.convert(left["fnormal"])) shear, shear_x, shear_y = self._convert_shear(left) left_shear_values.append(shear) left_shear_x_values.append(shear_x) left_shear_y_values.append(shear_y) if right is not None: right_values.append(self.normal_converter.convert(right["fnormal"])) shear, shear_x, shear_y = self._convert_shear(right) right_shear_values.append(shear) right_shear_x_values.append(shear_x) right_shear_y_values.append(shear_y) time.sleep(0.01) self.baseline = ForceBaseline( left_normal=sum(left_values) / len(left_values) if left_values else 0.0, right_normal=sum(right_values) / len(right_values) if right_values else 0.0, left_shear=sum(left_shear_values) / len(left_shear_values) if left_shear_values else 0.0, right_shear=sum(right_shear_values) / len(right_shear_values) if right_shear_values else 0.0, left_shear_x=sum(left_shear_x_values) / len(left_shear_x_values) if left_shear_x_values else 0.0, left_shear_y=sum(left_shear_y_values) / len(left_shear_y_values) if left_shear_y_values else 0.0, right_shear_x=sum(right_shear_x_values) / len(right_shear_x_values) if right_shear_x_values else 0.0, right_shear_y=sum(right_shear_y_values) / len(right_shear_y_values) if right_shear_y_values else 0.0, ) return self.baseline def read(self, reader, timeout=0.3): left, right = reader.get_samples(timeout=timeout) if left is None or right is None: return None left_force = max( 0.0, self.normal_converter.convert(left["fnormal"]) - self.baseline.left_normal, ) right_force = max( 0.0, self.normal_converter.convert(right["fnormal"]) - self.baseline.right_normal, ) if _component_shear_enabled(self.shear_converter): _, raw_left_shear_x, raw_left_shear_y = self._convert_shear(left) _, raw_right_shear_x, raw_right_shear_y = self._convert_shear(right) left_shear_x = raw_left_shear_x - self.baseline.left_shear_x left_shear_y = raw_left_shear_y - self.baseline.left_shear_y right_shear_x = raw_right_shear_x - self.baseline.right_shear_x right_shear_y = raw_right_shear_y - self.baseline.right_shear_y left_shear = (left_shear_x ** 2 + left_shear_y ** 2) ** 0.5 right_shear = (right_shear_x ** 2 + right_shear_y ** 2) ** 0.5 else: left_shear_x = left_shear_y = right_shear_x = right_shear_y = 0.0 left_shear = max( 0.0, self.shear_converter.convert(shear_magnitude(left)) - self.baseline.left_shear, ) right_shear = max( 0.0, self.shear_converter.convert(shear_magnitude(right)) - self.baseline.right_shear, ) left_normal = self.left_normal_filter.update(left_force) right_normal = self.right_normal_filter.update(right_force) left_shear_filtered = self.left_shear_filter.update(left_shear) right_shear_filtered = self.right_shear_filter.update(right_shear) left_shear_x_filtered = self.left_shear_x_filter.update(left_shear_x) left_shear_y_filtered = self.left_shear_y_filter.update(left_shear_y) right_shear_x_filtered = self.right_shear_x_filter.update(right_shear_x) right_shear_y_filtered = self.right_shear_y_filter.update(right_shear_y) return ForceFeedback( left_normal=left_normal, right_normal=right_normal, min_normal=min(left_normal, right_normal), max_normal=max(left_normal, right_normal), normal_diff=left_normal - right_normal, left_shear=left_shear_filtered, right_shear=right_shear_filtered, max_shear=max(left_shear_filtered, right_shear_filtered), shear_diff=left_shear_filtered - right_shear_filtered, left_shear_x=left_shear_x_filtered, left_shear_y=left_shear_y_filtered, right_shear_x=right_shear_x_filtered, right_shear_y=right_shear_y_filtered, )