v 2.1

2026-06-09 16:25:40 +08:00
parent c700f95fb0
commit 9ef1c8782a
13 changed files with 173 additions and 17 deletions
@@ -73,6 +73,9 @@ SHEAR_FILTER_ALPHA = 0.65
 TRIGGER_NORMAL_FORCE = 0.1  # 单侧法向力触发阈值，单位 N。
 TRIGGER_SHEAR_FORCE = 0.1   # 单侧切向力触发阈值，单位 N。
 TRIGGER_CLEAR_STABLE_SECONDS = 0.2  # 张开恢复后，必须先看到触发力清掉并稳定这么久，才允许下一次扫过触发。
+OPEN_WAIT_STABLE_SAMPLES = 12       # 张开等待时，用多少个近期样本判断当前稳定基准。
+OPEN_WAIT_STABLE_RANGE_N = 0.03     # 近期样本最大-最小值小于该值，认为该通道趋于稳定，单位 N。
+OPEN_WAIT_STABLE_TREND_N = 0.015    # 近期样本首尾变化小于该值，认为该通道无明显漂移，单位 N。


 # 闭合夹取：物体移动到中间后，左右两边都超过该阈值就认为夹住。
@@ -202,6 +205,9 @@ CONFIG = {
    "trigger_normal_force": TRIGGER_NORMAL_FORCE,
    "trigger_shear_force": TRIGGER_SHEAR_FORCE,
    "trigger_clear_stable_seconds": TRIGGER_CLEAR_STABLE_SECONDS,
+    "open_wait_stable_samples": OPEN_WAIT_STABLE_SAMPLES,
+    "open_wait_stable_range_n": OPEN_WAIT_STABLE_RANGE_N,
+    "open_wait_stable_trend_n": OPEN_WAIT_STABLE_TREND_N,
    "grip_normal_force": GRIP_NORMAL_FORCE,
    "grip_requires_both": GRIP_REQUIRES_BOTH,
    "close_timeout_seconds": CLOSE_TIMEOUT_SECONDS,
@@ -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
+                    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:
-                        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"
                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
@@ -63,6 +63,9 @@ class Demo02Config:
    trigger_normal_force: float = 0.04
    trigger_shear_force: float = 0.04
    trigger_clear_stable_seconds: float = 0.2
+    open_wait_stable_samples: int = 12
+    open_wait_stable_range_n: float = 0.03
+    open_wait_stable_trend_n: float = 0.015
    grip_normal_force: float = 0.05
    grip_requires_both: bool = True
    close_timeout_seconds: float = 5.0
@@ -151,6 +154,9 @@ class Demo02Config:
        add("--trigger-normal-force", type=float, default=get("trigger_normal_force", cls.trigger_normal_force))
        add("--trigger-shear-force", type=float, default=get("trigger_shear_force", cls.trigger_shear_force))
        add("--trigger-clear-stable-seconds", type=float, default=get("trigger_clear_stable_seconds", cls.trigger_clear_stable_seconds))
+        add("--open-wait-stable-samples", type=int, default=get("open_wait_stable_samples", cls.open_wait_stable_samples))
+        add("--open-wait-stable-range-n", type=float, default=get("open_wait_stable_range_n", cls.open_wait_stable_range_n))
+        add("--open-wait-stable-trend-n", type=float, default=get("open_wait_stable_trend_n", cls.open_wait_stable_trend_n))
        add("--grip-normal-force", type=float, default=get("grip_normal_force", cls.grip_normal_force))
        add("--grip-requires-both", dest="grip_requires_both", action="store_true", default=get("grip_requires_both", cls.grip_requires_both))
        add("--allow-single-side-grip", dest="grip_requires_both", action="store_false")
@@ -97,6 +97,62 @@ def fmt(value, unit="", width=0, precision=3):
    return f"{text}{unit}"


+STATE_TEXT = {
+    "open_wait": "等待触发",
+    "closing": "闭合中",
+    "gripping": "夹持加力",
+    "hold_check": "夹持检测",
+    "open_recover": "张开恢复",
+    "manual_close": "手动闭合",
+    "manual_hold": "手动保持",
+    "error": "错误",
+}
+
+ACTION_TEXT = {
+    "wait": "等待",
+    "waiting-for-sensor-samples": "等待传感器数据",
+    "open-wait-stabilizing": "等待稳定基准",
+    "open-wait-armed": "已就绪",
+    "side-trigger-close": "侧边触发闭合",
+    "closing-to-grip": "闭合寻找物体",
+    "object-detected-low-force": "检测到物体",
+    "close-timeout-open": "闭合超时张开",
+    "grip-ramp-up": "夹持加力",
+    "grip-hold": "夹持保持",
+    "hold-reference-armed": "记录夹持基准",
+    "adaptive-grip-ramp": "自适应加力",
+    "adaptive-grip-wait": "等待自适应加力",
+    "release-arm-delay": "释放检测延时",
+    "release-reference-armed": "释放检测就绪",
+    "hold-check": "保持检测",
+    "force-change-release-open": "力变化张开",
+    "open-recover-position": "张开位置恢复",
+    "open-recover-position-stabilizing": "张开位置稳定中",
+    "open-ready-position": "张开到位",
+    "open-recover-quiet": "力稳定恢复",
+    "open-recover-stabilizing": "恢复稳定中",
+    "open-recover-wait-force-clear": "等待力恢复",
+    "open-ready": "恢复完成",
+    "manual-open": "手动张开",
+    "manual-close": "手动闭合",
+    "manual-hold": "手动保持",
+    "manual-release": "手动释放",
+}
+
+
+def label_with_cn(value, mapping):
+    value = str(value or "--")
+    cn = mapping.get(value)
+    if cn is None:
+        for prefix, text in mapping.items():
+            if value.startswith(f"{prefix} "):
+                cn = text
+                break
+    if cn is None:
+        return value
+    return f"{value}（{cn}）"
+
+
 STYLE = """
 QMainWindow {
    background-color: #000000;
@@ -140,6 +196,22 @@ QLabel#StatusChip {
    font-weight: 600;
    padding: 8px 12px;
 }
+QFrame#StatusPanel {
+    background-color: #101010;
+    border: 1px solid #303030;
+    border-radius: 4px;
+}
+QLabel#StatusMain {
+    color: #00e5ff;
+    font-family: "Cascadia Code", "Consolas";
+    font-size: 17px;
+    font-weight: 600;
+}
+QLabel#StatusSub {
+    color: #9a9a9a;
+    font-family: "Cascadia Code", "Consolas";
+    font-size: 12px;
+}
 QLabel#ActionChip {
    background-color: #0b0b0b;
    border: 1px solid #262626;
@@ -332,8 +404,17 @@ class GripperDemo02Viewer(QMainWindow):
        top.setSpacing(8)
        title = QLabel("Orisys 夹爪")
        title.setObjectName("Title")
+        self.status_panel = QFrame()
+        self.status_panel.setObjectName("StatusPanel")
+        status_layout = QVBoxLayout(self.status_panel)
+        status_layout.setContentsMargins(10, 6, 10, 6)
+        status_layout.setSpacing(1)
        self.status_chip = QLabel("state=--")
-        self.status_chip.setObjectName("StatusChip")
+        self.status_chip.setObjectName("StatusMain")
+        self.action_label = QLabel("action=--")
+        self.action_label.setObjectName("StatusSub")
+        status_layout.addWidget(self.status_chip)
+        status_layout.addWidget(self.action_label)
        self.start_button = AnimatedButton("启动")
        self.start_button.setObjectName("StartButton")
        self.start_button.clicked.connect(self.start_control)
@@ -355,7 +436,7 @@ class GripperDemo02Viewer(QMainWindow):
        self.manual_hold_button.clicked.connect(self.toggle_manual_hold)
        self._set_button_state("idle")
        top.addWidget(title)
-        top.addWidget(self.status_chip)
+        top.addWidget(self.status_panel)
        top.addStretch(1)
        top.addWidget(self.start_button)
        top.addWidget(self.stop_button)
@@ -557,9 +638,11 @@ class GripperDemo02Viewer(QMainWindow):
            self._set_button_state("running")

        state = status.get("state", "--")
+        action = status.get("action", "--")
        unit = status.get("normal_unit", "N")
        shear_unit = status.get("shear_unit", "N")
-        self.status_chip.setText(f"state={state}")
+        self.status_chip.setText(f"state={label_with_cn(state, STATE_TEXT)}")
+        self.action_label.setText(f"action={label_with_cn(action, ACTION_TEXT)}")
        self.manual_hold_button.setText(
            "释放" if bool(status.get("manual_hold_active")) else "保持"
        )