cheng
114 lines
4.0 KiB
Python
114 lines
4.0 KiB
Python
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
|
|
|
|
|
|
@dataclass
|
|
class ForceBaseline:
|
|
left_normal: float = 0.0
|
|
right_normal: float = 0.0
|
|
left_shear: float = 0.0
|
|
right_shear: 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
|
|
|
|
|
|
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)
|
|
|
|
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 = []
|
|
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"]))
|
|
left_shear_values.append(self.shear_converter.convert(shear_magnitude(left)))
|
|
if right is not None:
|
|
right_values.append(self.normal_converter.convert(right["fnormal"]))
|
|
right_shear_values.append(self.shear_converter.convert(shear_magnitude(right)))
|
|
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,
|
|
)
|
|
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,
|
|
)
|
|
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)
|
|
|
|
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,
|
|
)
|
|
|