orisys/test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

v 1.2

Browse Source
This commit is contained in:
cheng
2026-06-09 14:29:44 +08:00
parent 74d2bc4853
commit 1f1ee6d8fe
16 changed files with 277 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gripper_control/feedback.py
+66 -11
View File
@@ -8,12 +8,20 @@ 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
@@ -27,6 +35,10 @@ class ForceFeedback:
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:
@@ -42,6 +54,17 @@ class TactileFeedbackProcessor:
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
@@ -53,15 +76,25 @@ class TactileFeedbackProcessor:
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"]))
left_shear_values.append(self.shear_converter.convert(shear_magnitude(left)))
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"]))
right_shear_values.append(self.shear_converter.convert(shear_magnitude(right)))
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(
@@ -69,6 +102,10 @@ class TactileFeedbackProcessor:
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
@@ -85,19 +122,34 @@ class TactileFeedbackProcessor:
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,
)
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,
@@ -109,5 +161,8 @@ class TactileFeedbackProcessor:
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,
)