v 2.1

gripper_control_02/controller.py

@@ -127,12 +127,19 @@ class SideTriggerGripController:
         trigger_clear_since = None
         trigger_armed = False
         last_trigger = False
+        open_wait_samples = []
+        open_wait_stable_values = {}
         force_pct = int(clamp(config.open_force, config.force_min, config.force_max))
         adaptive_target_force = int(clamp(config.hold_force, config.force_min, config.force_max))
         release_armed = False
         hold_pos = None
         reference = None
 
+        def reset_open_wait_stability():
+            nonlocal open_wait_samples, open_wait_stable_values
+            open_wait_samples = []
+            open_wait_stable_values = {}
+
         while not self._stop_event.is_set():
             tick_start = time.perf_counter()
             now = time.perf_counter()
@@ -156,6 +163,7 @@ class SideTriggerGripController:
                 recover_position_since = None
                 trigger_clear_since = None
                 trigger_armed = False
+                reset_open_wait_stability()
                 release_armed = False
                 hold_pos = None
                 reference = None
@@ -177,6 +185,7 @@ class SideTriggerGripController:
                 recover_position_since = None
                 trigger_clear_since = None
                 trigger_armed = False
+                reset_open_wait_stability()
                 release_armed = False
                 hold_pos = None
                 reference = None
@@ -194,6 +203,7 @@ class SideTriggerGripController:
                 last_force_ramp_time = now
                 trigger_clear_since = None
                 trigger_armed = False
+                reset_open_wait_stability()
                 release_armed = False
                 reference = None
                 manual_action = "manual-hold"
@@ -203,6 +213,7 @@ class SideTriggerGripController:
                     state_start = now
                     trigger_clear_since = None
                     trigger_armed = False
+                    reset_open_wait_stability()
                     release_armed = False
                     hold_pos = None
                     reference = None
@@ -239,21 +250,21 @@ class SideTriggerGripController:
             if manual_action is not None:
                 pass
             elif state == "open_wait":
-                trigger_edge = trigger and not last_trigger
+                open_wait_sample = self._open_wait_sample(feedback)
+                self._append_open_wait_sample(open_wait_samples, open_wait_sample)
+                stable_values = self._open_wait_stable_values(open_wait_samples)
+                trigger = self._open_wait_delta_triggered(
+                    open_wait_sample,
+                    open_wait_stable_values,
+                )
                 if not trigger:
-                    if trigger_clear_since is None:
-                        trigger_clear_since = now
-                        action = "open-wait-trigger-clearing"
-                    elif now - trigger_clear_since >= config.trigger_clear_stable_seconds:
-                        trigger_armed = True
-                        action = "open-wait-armed"
-                    else:
-                        action = "open-wait-trigger-clearing"
-                elif not trigger_armed:
-                    trigger_clear_since = None
-                    action = "open-wait-clear-trigger"
-                elif not trigger_edge:
-                    action = "open-wait-trigger-held"
+                    open_wait_stable_values.update(stable_values)
+
+                trigger_armed = bool(open_wait_stable_values)
+                if not trigger_armed:
+                    action = "open-wait-stabilizing"
+                elif not trigger:
+                    action = "open-wait-armed"
                 else:
                     force_pct = int(clamp(
                         config.close_force,
@@ -271,6 +282,7 @@ class SideTriggerGripController:
                     recover_position_since = None
                     trigger_clear_since = None
                     trigger_armed = False
+                    reset_open_wait_stability()
                     adaptive_target_force = self._adaptive_force_target(feedback)
                     release_armed = False
                     hold_pos = None
@@ -308,6 +320,7 @@ class SideTriggerGripController:
                     last_open_position_ready = False
                     recover_quiet_since = None
                     recover_position_since = None
+                    reset_open_wait_stability()
                     release_armed = False
                     hold_pos = None
                     reference = None
@@ -404,6 +417,7 @@ class SideTriggerGripController:
                         last_open_position_ready = False
                         recover_quiet_since = None
                         recover_position_since = None
+                        reset_open_wait_stability()
                         release_armed = False
                         hold_pos = None
                         reference = None
@@ -442,6 +456,7 @@ class SideTriggerGripController:
                         recover_position_since = None
                         trigger_clear_since = None
                         trigger_armed = True
+                        reset_open_wait_stability()
                         last_trigger = trigger
                         release_armed = False
                         hold_pos = None
@@ -464,6 +479,7 @@ class SideTriggerGripController:
                         recover_position_since = None
                         trigger_clear_since = None
                         trigger_armed = True
+                        reset_open_wait_stability()
                         last_trigger = trigger
                         release_armed = False
                         hold_pos = None
@@ -526,6 +542,51 @@ class SideTriggerGripController:
             or feedback.max_shear >= self.config.trigger_shear_force
         )
 
+    def _open_wait_sample(self, feedback):
+        return {
+            "left_normal": feedback.left_normal,
+            "right_normal": feedback.right_normal,
+            "left_shear": feedback.left_shear,
+            "right_shear": feedback.right_shear,
+        }
+
+    def _append_open_wait_sample(self, samples, sample):
+        samples.append(sample)
+        stable_samples = max(2, int(self.config.open_wait_stable_samples))
+        if len(samples) > stable_samples:
+            del samples[0]
+
+    def _open_wait_stable_values(self, samples):
+        stable_samples = max(2, int(self.config.open_wait_stable_samples))
+        if len(samples) < stable_samples:
+            return {}
+
+        stable = {}
+        first = samples[0]
+        last = samples[-1]
+        for key in first:
+            values = [sample[key] for sample in samples]
+            value_range = max(values) - min(values)
+            trend = abs(last[key] - first[key])
+            if (
+                value_range <= self.config.open_wait_stable_range_n
+                and trend <= self.config.open_wait_stable_trend_n
+            ):
+                stable[key] = sum(values) / len(values)
+        return stable
+
+    def _open_wait_delta_triggered(self, sample, stable_values):
+        thresholds = {
+            "left_normal": self.config.trigger_normal_force,
+            "right_normal": self.config.trigger_normal_force,
+            "left_shear": self.config.trigger_shear_force,
+            "right_shear": self.config.trigger_shear_force,
+        }
+        for key, stable_value in stable_values.items():
+            if sample[key] - stable_value >= thresholds[key]:
+                return True
+        return False
+
     def _grip_contact(self, feedback):
         left_contact = feedback.left_normal >= self.config.grip_normal_force
         right_contact = feedback.right_normal >= self.config.grip_normal_force