qt第一版可视化

examples/gripper_demo_02_viewer.py

@@ -0,0 +1,17 @@
+"""Compatibility entry point for gripper demo 02 PyQt viewer."""
+
+import multiprocessing as mp
+import sys
+from pathlib import Path
+
+
+REPO_ROOT = Path(__file__).resolve().parents[1]
+if str(REPO_ROOT) not in sys.path:
+    sys.path.insert(0, str(REPO_ROOT))
+
+from gripper_control_02.visualizer import main
+
+
+if __name__ == "__main__":
+    mp.freeze_support()
+    raise SystemExit(main(sys.argv[1:]))

gripper_control/force_reader.py

@@ -7,6 +7,9 @@ import queue
 import time
 from multiprocessing import Process, Queue
 
+import cv2
+import numpy as np
+
 from .paths import ensure_project_paths
 
 ensure_project_paths()
@@ -38,6 +41,69 @@ def _put_latest(result_queue, data):
         pass
 
 
+def _draw_magnitude_map(flow, visual_size, threshold=1.5):
+    magnitude = np.sqrt(flow[:, :, 0] ** 2 + flow[:, :, 1] ** 2)
+    h, w = magnitude.shape
+    if h <= 0 or w <= 0:
+        return None
+
+    downsample_factor = 10
+    magnitude_downsampled = magnitude[::downsample_factor, ::downsample_factor]
+    magnitude_smoothed = cv2.resize(
+        magnitude_downsampled,
+        (w, h),
+        interpolation=cv2.INTER_CUBIC,
+    )
+
+    magnitude_thresholded = magnitude_smoothed.copy()
+    magnitude_thresholded[magnitude_smoothed < threshold] = 0
+
+    mag_min = float(magnitude_thresholded.min())
+    mag_max = float(magnitude_thresholded.max())
+    if mag_max > mag_min:
+        magnitude_norm = (
+            (magnitude_thresholded - mag_min) / (mag_max - mag_min) * 255
+        ).astype(np.uint8)
+    else:
+        magnitude_norm = np.zeros_like(magnitude_thresholded, dtype=np.uint8)
+
+    magnitude_colored = cv2.applyColorMap(magnitude_norm, cv2.COLORMAP_JET)
+    return cv2.resize(
+        magnitude_colored,
+        (visual_size, visual_size),
+        interpolation=cv2.INTER_CUBIC,
+    )
+
+
+def _make_visuals(orisys_module, img, flow, visual_size):
+    if img is None or flow is None or np.asarray(flow).size == 0:
+        return None, None, 0.0, 0.0
+
+    if len(img.shape) == 2:
+        img_bgr = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
+    else:
+        img_bgr = img
+
+    h, w = img_bgr.shape[:2]
+    bg = np.empty((h, w, 3), dtype=img_bgr.dtype)
+    bg[:, :, 0] = 22
+    bg[:, :, 1] = 16
+    bg[:, :, 2] = 11
+
+    arrows = orisys_module.util.draw_arrows(
+        bg,
+        flow,
+        threshold=2,
+        grid_spacing=20,
+        arrow_scale=0.5,
+        below_threshold_color=(160, 160, 160),
+    )
+    arrows = cv2.resize(arrows, (visual_size, visual_size), interpolation=cv2.INTER_CUBIC)
+    magnitude = _draw_magnitude_map(flow, visual_size)
+    flow_mag = np.sqrt(flow[:, :, 0] ** 2 + flow[:, :, 1] ** 2)
+    return arrows, magnitude, float(flow_mag.mean()), float(flow_mag.max())
+
+
 def _sensor_force_worker(
     sensor_id,
     vid_src,
@@ -50,6 +116,8 @@ def _sensor_force_worker(
     isstitch,
     backend,
     motion_threshold,
+    include_visuals,
+    visual_size,
 ):
     ensure_project_paths()
     import orisys
@@ -79,16 +147,34 @@ def _sensor_force_worker(
                 check_motion=True,
                 threshold=motion_threshold,
             )
+            if include_visuals:
+                fps, fnormal, fshearx, fsheary, flow, img_view = sensor.read_info(
+                    sensor.info.FPS,
+                    sensor.info.FNORMAL,
+                    sensor.info.FSHEARX,
+                    sensor.info.FSHEARY,
+                    sensor.info.VRAW,
+                    sensor.info.IMG,
+                )
+                flow_view, magnitude_view, flow_mean, flow_max = _make_visuals(
+                    orisys,
+                    img_view,
+                    flow,
+                    int(visual_size),
+                )
+            else:
                 fps, fnormal, fshearx, fsheary = sensor.read_info(
                     sensor.info.FPS,
                     sensor.info.FNORMAL,
                     sensor.info.FSHEARX,
                     sensor.info.FSHEARY,
                 )
+                flow_view = None
+                magnitude_view = None
+                flow_mean = 0.0
+                flow_max = 0.0
 
-            _put_latest(
-                result_queue,
-                {
+            data = {
                 "sensor_id": sensor_id,
                 "fnormal": float(fnormal),
                 "fshearx": float(fshearx),
@@ -96,8 +182,14 @@ def _sensor_force_worker(
                 "fps": float(fps),
                 "is_contact": bool(is_contact),
                 "timestamp": time.time(),
-                },
-            )
+                "flow_mean": flow_mean,
+                "flow_max": flow_max,
+            }
+            if include_visuals:
+                data["flow_view"] = flow_view
+                data["magnitude_view"] = magnitude_view
+
+            _put_latest(result_queue, data)
 
             if frame_interval > 0:
                 sleep_time = frame_interval - (time.perf_counter() - t_start)
@@ -136,6 +228,8 @@ class TwoSensorForceReader:
         backend="auto",
         motion_threshold=0,
         queue_size=5,
+        include_visuals=False,
+        visual_size=320,
     ):
         self.sensor_configs = [
             {
@@ -157,6 +251,8 @@ class TwoSensorForceReader:
         self.backend = backend
         self.motion_threshold = motion_threshold
         self.queue_size = queue_size
+        self.include_visuals = include_visuals
+        self.visual_size = visual_size
 
         self.result_queues = []
         self.stop_events = []
@@ -186,6 +282,8 @@ class TwoSensorForceReader:
                     self.isstitch,
                     self.backend,
                     self.motion_threshold,
+                    self.include_visuals,
+                    self.visual_size,
                 ),
             )
             process.start()

gripper_control/hardware.py

@@ -22,27 +22,38 @@ class GripperClient:
         )
         return self
 
-    def move(self, position, force_pct, label):
+    def move(self, position, force_pct, label, speed_pct=None, accel=None, decel=None):
         config = self.config
+        speed_pct = config.speed if speed_pct is None else speed_pct
+        accel = config.accel if accel is None else accel
+        decel = config.decel if decel is None else decel
+
         if config.dry_run:
             print(
                 f"[dry-run] {label}: temp_move "
-                f"position={position}, speed={config.speed}, force={force_pct}, "
-                f"accel={config.accel}, decel={config.decel}"
+                f"position={position}, speed={speed_pct}, force={force_pct}, "
+                f"accel={accel}, decel={decel}"
             )
             return
 
         self.motor.temp_move(
             position_mm=int(position),
-            speed_pct=int(config.speed),
+            speed_pct=int(speed_pct),
             force_pct=int(force_pct),
-            accel=int(config.accel),
-            decel=int(config.decel),
+            accel=int(accel),
+            decel=int(decel),
             trigger=True,
         )
 
-    def open(self, force_pct, label="open"):
-        self.move(self.config.open_pos, force_pct, label)
+    def open(self, force_pct, label="open", speed_pct=None, accel=None, decel=None):
+        self.move(
+            self.config.open_pos,
+            force_pct,
+            label,
+            speed_pct=speed_pct,
+            accel=accel,
+            decel=decel,
+        )
 
     def close(self, force_pct, label="close"):
         self.move(self.config.close_pos, force_pct, label)
@@ -99,4 +110,3 @@ class GripperClient:
             self.motor.trigger_motion()
         except Exception as exc:
             print(f"stop by speed=0 failed: {exc}")
-

gripper_control_02/README.md

@@ -12,8 +12,9 @@
 6. 像 01 一样进入 `gripping`，慢慢加力到 `HOLD_FORCE`。
 7. 进入 `hold_check` 后记录夹持参考力。
 8. 后续法向力或切向力变化超过阈值，夹爪直接张开到 `OPEN_POS`。
-9. 张开后进入 `open_recover`，保持张开并等待传感器力清空。
-10. 法向和切向读数都低于恢复阈值并稳定后，才回到 `open_wait` 等待下一次单侧扫过触发。
+9. 张开后进入 `open_recover`，保持张开并等待恢复条件。
+10. 夹爪回到 `OPEN_POS` 初始点位，或法向/切向读数都低于恢复阈值并稳定后，回到 `open_wait`。
+11. 回到 `open_wait` 后，程序必须先看到触发力清掉并稳定，才会重新武装；下一次扫过才允许闭合。
 
 ## 运行
 
@@ -30,6 +31,24 @@ python -m gripper_control_02.main
 python -m gripper_control_02.main --dry-run
 ```
 
+PyQt 可视化展示：
+
+```powershell
+python -m gripper_control_02.visualizer
+```
+
+窗口模式运行：
+
+```powershell
+python -m gripper_control_02.visualizer --windowed
+```
+
+兼容入口：
+
+```powershell
+python examples\gripper_demo_02_viewer.py
+```
+
 默认配置文件：
 
 ```text
@@ -40,6 +59,7 @@ gripper_control_02\config\gripper_demo_02.py
 
 - `TRIGGER_NORMAL_FORCE`: 单侧法向力触发阈值。
 - `TRIGGER_SHEAR_FORCE`: 单侧切向力触发阈值。
+- `TRIGGER_CLEAR_STABLE_SECONDS`: 张开恢复后，触发力必须清掉并稳定多久，才允许下一次扫过触发。
 - `GRIP_NORMAL_FORCE`: 夹住时左右接触阈值。
 - `GRIP_REQUIRES_BOTH`: 是否必须左右都接触才算夹住。
 - `GRIP_START_FORCE`: 检测到物体后先切到的低力。
@@ -49,10 +69,17 @@ gripper_control_02\config\gripper_demo_02.py
 - `GRIP_SETTLE_SECONDS`: 夹住后等待稳定多久再记录参考力。
 - `RELEASE_NORMAL_CHANGE`: 夹住后法向力变化超过它就松开。
 - `RELEASE_SHEAR_CHANGE`: 夹住后切向力变化超过它就松开。
+- `RELEASE_OPEN_SPEED`: 检测到松开后，张开到 `OPEN_POS` 使用的速度。
+- `RELEASE_OPEN_ACCEL`: release 张开的加速度。
+- `RELEASE_OPEN_DECEL`: release 张开的减速度。
 - `REARM_SECONDS`: 张开后最短等待时间，防止刚松开马上又误触发。
 - `RECOVER_NORMAL_FORCE`: 张开恢复时允许重新触发的最大法向残余力。
 - `RECOVER_SHEAR_FORCE`: 张开恢复时允许重新触发的最大切向残余力。
 - `RECOVER_STABLE_SECONDS`: 力清空后需要连续稳定多久，才重新等待触发。
+- `OPEN_POSITION_RECOVER_ENABLED`: 是否允许夹爪回到 `OPEN_POS` 后直接恢复等待。
+- `OPEN_POSITION_TOLERANCE`: 当前点位和 `OPEN_POS` 差值小于它，就认为已经回到初始点位。
+- `OPEN_POSITION_STABLE_SECONDS`: 回到初始点位后需要稳定多久，才重新等待触发。
+- `OPEN_POSITION_CHECK_INTERVAL`: `open_recover` 中读取夹爪当前位置的间隔。
 
 ## 日志字段
 
@@ -60,6 +87,7 @@ gripper_control_02\config\gripper_demo_02.py
 - `maxN`: 左右法向力最大值，单位 N。
 - `shear`: 左右切向力最大值，单位 N。
 - `trigger`: 是否满足单侧触发条件。
+- `armed`: 是否已经重新武装；只有 `trigger=1 armed=1` 才会闭合。
 - `grip_contact`: 是否满足夹住接触条件。
 - `dN`: 夹住后当前法向力相对参考值的最大变化，单位 N。
 - `dShear`: 夹住后当前切向力相对参考值的变化，单位 N。
@@ -68,17 +96,31 @@ gripper_control_02\config\gripper_demo_02.py
 - `force_pct`: 当前夹爪目标力百分比。
 - `action`: 当前动作。
 
+## 可视化展示
+
+- 左右两边的光流箭头图。
+- 左右两边的光流幅值图。
+- 左右法向力、最大切向力、左右差值。
+- 当前 `state`、`action`、`trigger/armed`、`grip_contact`。
+- 当前 `force_pct`、速度 `speed_pct`、`hold_pos`。
+- 左右传感器 FPS、接触标志、光流均值/最大值。
+
 ## 状态说明
 
 - `open_wait`: 夹爪张开，等待任意单侧触发。
 - `closing`: 单侧触发后正在闭合，等待检测到物体。
 - `gripping`: 检测到物体后低力保持当前位置，并慢慢加力。
 - `hold_check`: 已夹住并记录参考力，监测法向/切向变化；变化超过阈值会直接张开。
-- `open_recover`: 已经张开到初始位置，正在等待法向/切向残余力清空；这个状态不会闭合夹爪。
+- `open_recover`: 已经发出张开命令，正在等待回到初始点位或等待法向/切向残余力清空；这个状态不会闭合夹爪。
 
 ## 常见动作说明
 
 - `force-change-release-open`: 检测到夹住后的法向或切向变化，已经发出张开命令。
+- `open-recover-position`: 夹爪当前位置已经接近 `OPEN_POS`，正在等待到位稳定。
+- `open-ready-position`: 夹爪已经回到初始点位，重新进入等待下一次扫过触发。
+- `open-wait-clear-trigger`: 已经回到等待状态，但当前力还超过触发阈值，暂时不会闭合。
+- `open-wait-trigger-clearing`: 触发力已经低于阈值，正在等待清零稳定时间。
+- `open-wait-armed`: 触发已经清掉并稳定，下一次扫过可以闭合。
 - `open-recover-wait-force-clear`: 夹爪保持张开，但传感器还有残余力，不允许重新闭合。
 - `open-recover-stabilizing`: 力已经低于恢复阈值，正在等待稳定时间。
 - `open-ready`: 力已经清空并稳定，重新进入等待下一次扫过触发。

gripper_control_02/config/gripper_demo_02.py

@@ -43,6 +43,9 @@ CLOSE_POS = 9000
 SPEED = 25
 ACCEL = 80
 DECEL = 80
+RELEASE_OPEN_SPEED = 50    # 检测到人取物/力变化后，张开使用的速度。
+RELEASE_OPEN_ACCEL = 120   # release 张开的加速度。
+RELEASE_OPEN_DECEL = 120   # release 张开的减速度。
 
 
 # 夹爪力度百分比。
@@ -54,21 +57,22 @@ FORCE_MIN = 10
 FORCE_MAX = 30
 
 
-# 控制循环频率。
-CONTROL_HZ = 20.0
+# 控制循环频率。越高检测释放越快。
+CONTROL_HZ = 30.0
 
 
 # 启动时采集空载基线的时间。
 BASELINE_SECONDS = 1.0
 
 # 滤波系数，越大响应越快，越小越平滑。
-FILTER_ALPHA = 0.35
-SHEAR_FILTER_ALPHA = 0.45
+FILTER_ALPHA = 0.55
+SHEAR_FILTER_ALPHA = 0.65
 
 
 # 等待触发：任意一侧超过这些阈值，就开始闭合。
-TRIGGER_NORMAL_FORCE = 0.04  # 单侧法向力触发阈值，单位 N。
-TRIGGER_SHEAR_FORCE = 0.04   # 单侧切向力触发阈值，单位 N。
+TRIGGER_NORMAL_FORCE = 0.1  # 单侧法向力触发阈值，单位 N。
+TRIGGER_SHEAR_FORCE = 0.1   # 单侧切向力触发阈值，单位 N。
+TRIGGER_CLEAR_STABLE_SECONDS = 0.2  # 张开恢复后，必须先看到触发力清掉并稳定这么久，才允许下一次扫过触发。
 
 
 # 闭合夹取：物体移动到中间后，左右两边都超过该阈值就认为夹住。
@@ -79,12 +83,12 @@ CLOSE_TIMEOUT_SECONDS = 5.0  # 触发闭合后这么久还没夹住，就重新
 
 # 检测到物体后慢慢加力，类似 01 的 gripping 状态。
 FORCE_RAMP_STEP = 1
-FORCE_RAMP_INTERVAL = 0.2
-GRIP_SETTLE_SECONDS = 0.3    # 加到 HOLD_FORCE 后再稳定这么久，进入 hold_check。
+FORCE_RAMP_INTERVAL = 0.1
+GRIP_SETTLE_SECONDS = 0.1    # 加到 HOLD_FORCE 后再稳定这么久，进入 hold_check。
 
 # 松开触发阈值。夹住后，法向或切向变化超过其中任意一个，就张开。
-RELEASE_NORMAL_CHANGE = 0.05  # 左/右法向力相对夹住参考值的变化阈值，单位 N。
-RELEASE_SHEAR_CHANGE = 0.05   # 切向力相对夹住参考值的变化阈值，单位 N。
+RELEASE_NORMAL_CHANGE = 0.2  # 左/右法向力相对夹住参考值的变化阈值，单位 N。
+RELEASE_SHEAR_CHANGE = 0.2   # 左/右切向力相对夹住参考值的变化阈值，单位 N。
 
 # 松开后至少等待多久重新进入等待触发，避免刚张开时马上再次触发。
 REARM_SECONDS = 0.5
@@ -95,6 +99,13 @@ RECOVER_NORMAL_FORCE = 0.02   # 张开恢复时允许重新触发的最大法向
 RECOVER_SHEAR_FORCE = 0.02    # 张开恢复时允许重新触发的最大切向残余力，单位 N。
 RECOVER_STABLE_SECONDS = 0.4  # 力清空后需要连续稳定这么久，才回到 open_wait。
 
+# 松开/超时张开后，如果夹爪已经回到 OPEN_POS 初始点位，也允许重新触发。
+# 这样传感器上还有一点残余力时，不会一直卡在 open_recover。
+OPEN_POSITION_RECOVER_ENABLED = True
+OPEN_POSITION_TOLERANCE = 100         # 当前位置和 OPEN_POS 的差值小于它，就认为回到初始点位。
+OPEN_POSITION_STABLE_SECONDS = 0.1    # 回到初始点位后稳定这么久，才回到 open_wait。
+OPEN_POSITION_CHECK_INTERVAL = 0.1    # open_recover 状态下读取夹爪位置的间隔，单位秒。
+
 
 # 如果读取当前位置失败，保持当前位置时使用这个备用位置；None 表示不使用。
 HOLD_FALLBACK_POS = None
@@ -158,6 +169,9 @@ CONFIG = {
     "speed": SPEED,
     "accel": ACCEL,
     "decel": DECEL,
+    "release_open_speed": RELEASE_OPEN_SPEED,
+    "release_open_accel": RELEASE_OPEN_ACCEL,
+    "release_open_decel": RELEASE_OPEN_DECEL,
 
     "open_force": OPEN_FORCE,
     "close_force": CLOSE_FORCE,
@@ -174,6 +188,7 @@ CONFIG = {
 
     "trigger_normal_force": TRIGGER_NORMAL_FORCE,
     "trigger_shear_force": TRIGGER_SHEAR_FORCE,
+    "trigger_clear_stable_seconds": TRIGGER_CLEAR_STABLE_SECONDS,
     "grip_normal_force": GRIP_NORMAL_FORCE,
     "grip_requires_both": GRIP_REQUIRES_BOTH,
     "close_timeout_seconds": CLOSE_TIMEOUT_SECONDS,
@@ -186,6 +201,10 @@ CONFIG = {
     "recover_normal_force": RECOVER_NORMAL_FORCE,
     "recover_shear_force": RECOVER_SHEAR_FORCE,
     "recover_stable_seconds": RECOVER_STABLE_SECONDS,
+    "open_position_recover_enabled": OPEN_POSITION_RECOVER_ENABLED,
+    "open_position_tolerance": OPEN_POSITION_TOLERANCE,
+    "open_position_stable_seconds": OPEN_POSITION_STABLE_SECONDS,
+    "open_position_check_interval": OPEN_POSITION_CHECK_INTERVAL,
 
     "hold_fallback_pos": HOLD_FALLBACK_POS,
     "trigger_force_update": TRIGGER_FORCE_UPDATE,

gripper_control_02/controller.py

@@ -1,18 +1,32 @@
 import time
+import threading
 
 from gripper_control.filters import clamp
 
 
 class SideTriggerGripController:
-    def __init__(self, config, reader, gripper, feedback_processor):
+    def __init__(
+        self,
+        config,
+        reader,
+        gripper,
+        feedback_processor,
+        status_callback=None,
+    ):
         self.config = config
         self.reader = reader
         self.gripper = gripper
         self.feedback_processor = feedback_processor
         self.normal_unit = feedback_processor.normal_converter.unit
         self.shear_unit = feedback_processor.shear_converter.unit
+        self.status_callback = status_callback
+        self._stop_event = threading.Event()
+
+    def stop(self):
+        self._stop_event.set()
 
     def run(self):
+        self._stop_event.clear()
         self.reader.start()
 
         try:
@@ -59,6 +73,20 @@ class SideTriggerGripController:
             f"release_normal_change={self.config.release_normal_change:.3f}{self.normal_unit}, "
             f"release_shear_change={self.config.release_shear_change:.3f}{self.shear_unit}"
         )
+        print(
+            "Release open: "
+            f"speed={self.config.release_open_speed}, "
+            f"accel={self.config.release_open_accel}, "
+            f"decel={self.config.release_open_decel}"
+        )
+        print(
+            "Recover: "
+            f"force_normal={self.config.recover_normal_force:.3f}{self.normal_unit}, "
+            f"force_shear={self.config.recover_shear_force:.3f}{self.shear_unit}, "
+            f"open_pos={self.config.open_pos}, "
+            f"pos_enabled={int(self.config.open_position_recover_enabled)}, "
+            f"pos_tol={self.config.open_position_tolerance}"
+        )
 
     def _run_event_monitor(self):
         config = self.config
@@ -67,18 +95,35 @@ class SideTriggerGripController:
         state = "open_wait"
         state_start = time.perf_counter()
         last_force_ramp_time = state_start
+        last_open_position_check_time = 0.0
+        last_open_position = None
+        last_open_position_ready = False
         recover_quiet_since = None
+        recover_position_since = None
+        trigger_clear_since = None
+        trigger_armed = False
         force_pct = int(clamp(config.open_force, config.force_min, config.force_max))
         hold_pos = None
         reference = None
 
-        while True:
+        while not self._stop_event.is_set():
             tick_start = time.perf_counter()
             now = time.perf_counter()
 
             feedback = self.feedback_processor.read(self.reader, timeout=0.3)
             if feedback is None:
                 print(f"state={state} action=waiting-for-sensor-samples")
+                self._emit_status(
+                    {
+                        "timestamp": time.time(),
+                        "state": state,
+                        "action": "waiting-for-sensor-samples",
+                        "force_pct": force_pct,
+                        "speed_pct": config.speed,
+                        "normal_unit": self.normal_unit,
+                        "shear_unit": self.shear_unit,
+                    }
+                )
                 time.sleep(0.05)
                 continue
 
@@ -89,7 +134,19 @@ class SideTriggerGripController:
             action = "wait"
 
             if state == "open_wait":
-                if trigger:
+                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"
+                else:
                     force_pct = int(clamp(
                         config.close_force,
                         config.force_min,
@@ -99,12 +156,16 @@ class SideTriggerGripController:
                     state = "closing"
                     state_start = now
                     last_force_ramp_time = now
+                    last_open_position_check_time = 0.0
+                    last_open_position = None
+                    last_open_position_ready = False
                     recover_quiet_since = None
+                    recover_position_since = None
+                    trigger_clear_since = None
+                    trigger_armed = False
                     hold_pos = None
                     reference = None
                     action = "side-trigger-close"
-                else:
-                    action = "open-wait-touch"
 
             elif state == "closing":
                 if grip_contact:
@@ -130,7 +191,11 @@ class SideTriggerGripController:
                     self.feedback_processor.reset_filters()
                     state = "open_recover"
                     state_start = now
+                    last_open_position_check_time = 0.0
+                    last_open_position = None
+                    last_open_position_ready = False
                     recover_quiet_since = None
+                    recover_position_since = None
                     hold_pos = None
                     reference = None
                     action = "close-timeout-open"
@@ -173,11 +238,21 @@ class SideTriggerGripController:
                         config.force_min,
                         config.force_max,
                     ))
-                    self.gripper.open(force_pct, "force-change-release-open")
+                    self.gripper.open(
+                        force_pct,
+                        "force-change-release-open",
+                        speed_pct=config.release_open_speed,
+                        accel=config.release_open_accel,
+                        decel=config.release_open_decel,
+                    )
                     self.feedback_processor.reset_filters()
                     state = "open_recover"
                     state_start = now
+                    last_open_position_check_time = 0.0
+                    last_open_position = None
+                    last_open_position_ready = False
                     recover_quiet_since = None
+                    recover_position_since = None
                     hold_pos = None
                     reference = None
                     action = "force-change-release-open"
@@ -185,7 +260,41 @@ class SideTriggerGripController:
                     action = "hold-check"
 
             elif state == "open_recover":
-                if self._recover_quiet(feedback):
+                current_pos = last_open_position
+                position_ready = last_open_position_ready
+                if (
+                    config.open_position_recover_enabled
+                    and now - last_open_position_check_time
+                    >= config.open_position_check_interval
+                ):
+                    last_open_position_check_time = now
+                    current_pos = self.gripper.read_position()
+                    position_ready = self._open_position_ready(current_pos)
+                    last_open_position = current_pos
+                    last_open_position_ready = position_ready
+
+                if position_ready:
+                    if recover_position_since is None:
+                        recover_position_since = now
+                        action = f"open-recover-position pos={current_pos}"
+                    elif (
+                        now - recover_position_since
+                        >= config.open_position_stable_seconds
+                        and now - state_start >= config.rearm_seconds
+                    ):
+                        state = "open_wait"
+                        state_start = now
+                        recover_quiet_since = None
+                        recover_position_since = None
+                        trigger_clear_since = None
+                        trigger_armed = False
+                        hold_pos = None
+                        reference = None
+                        action = f"open-ready-position pos={current_pos}"
+                    else:
+                        action = f"open-recover-position-stabilizing pos={current_pos}"
+                elif self._recover_quiet(feedback):
+                    recover_position_since = None
                     if recover_quiet_since is None:
                         recover_quiet_since = now
                         action = "open-recover-quiet"
@@ -196,6 +305,9 @@ class SideTriggerGripController:
                         state = "open_wait"
                         state_start = now
                         recover_quiet_since = None
+                        recover_position_since = None
+                        trigger_clear_since = None
+                        trigger_armed = False
                         hold_pos = None
                         reference = None
                         action = "open-ready"
@@ -203,6 +315,7 @@ class SideTriggerGripController:
                         action = "open-recover-stabilizing"
                 else:
                     recover_quiet_since = None
+                    recover_position_since = None
                     action = "open-recover-wait-force-clear"
 
             self._log_tick(
@@ -215,12 +328,29 @@ class SideTriggerGripController:
                 hold_pos=hold_pos,
                 force_pct=force_pct,
                 action=action,
+                trigger_armed=trigger_armed,
+            )
+            self._emit_status(
+                self._make_status(
+                    feedback=feedback,
+                    state=state,
+                    trigger=trigger,
+                    grip_contact=grip_contact,
+                    normal_change=normal_change,
+                    shear_change=shear_change,
+                    hold_pos=hold_pos,
+                    force_pct=force_pct,
+                    action=action,
+                    trigger_armed=trigger_armed,
+                )
             )
 
             sleep_time = interval - (time.perf_counter() - tick_start)
             if sleep_time > 0:
                 time.sleep(sleep_time)
 
+        print("demo02 control loop stopped.")
+
     def _side_triggered(self, feedback):
         return (
             feedback.max_normal >= self.config.trigger_normal_force
@@ -240,10 +370,20 @@ class SideTriggerGripController:
             and feedback.max_shear <= self.config.recover_shear_force
         )
 
+    def _open_position_ready(self, current_pos):
+        if current_pos is None:
+            return False
+        return (
+            abs(int(current_pos) - int(self.config.open_pos))
+            <= int(self.config.open_position_tolerance)
+        )
+
     def _make_reference(self, feedback):
         return {
             "left_normal": feedback.left_normal,
             "right_normal": feedback.right_normal,
+            "left_shear": feedback.left_shear,
+            "right_shear": feedback.right_shear,
             "max_shear": feedback.max_shear,
         }
 
@@ -255,6 +395,11 @@ class SideTriggerGripController:
             abs(feedback.right_normal - reference["right_normal"]),
         )
         shear_change = abs(feedback.max_shear - reference["max_shear"])
+        shear_change = max(
+            shear_change,
+            abs(feedback.left_shear - reference["left_shear"]),
+            abs(feedback.right_shear - reference["right_shear"]),
+        )
         return normal_change, shear_change
 
     def _log_tick(
@@ -268,15 +413,78 @@ class SideTriggerGripController:
         hold_pos,
         force_pct,
         action,
+        trigger_armed,
     ):
         print(
             f"L={feedback.left_normal:7.3f}{self.normal_unit} "
             f"R={feedback.right_normal:7.3f}{self.normal_unit} "
             f"maxN={feedback.max_normal:7.3f}{self.normal_unit} "
             f"shear={feedback.max_shear:7.3f}{self.shear_unit} "
-            f"trigger={int(trigger)} grip_contact={int(grip_contact)} "
+            f"trigger={int(trigger)} armed={int(trigger_armed)} "
+            f"grip_contact={int(grip_contact)} "
             f"dN={normal_change:7.3f}{self.normal_unit} "
             f"dShear={shear_change:7.3f}{self.shear_unit} "
             f"state={state} hold_pos={hold_pos} "
             f"force_pct={force_pct:3d} action={action}"
         )
+
+    def _make_status(
+        self,
+        feedback,
+        state,
+        trigger,
+        grip_contact,
+        normal_change,
+        shear_change,
+        hold_pos,
+        force_pct,
+        action,
+        trigger_armed,
+    ):
+        speed_pct = self.config.speed
+        if action == "force-change-release-open":
+            speed_pct = self.config.release_open_speed
+
+        left_sample = None
+        right_sample = None
+        try:
+            if len(self.reader.latest) >= 2:
+                left_sample = self.reader.latest[0]
+                right_sample = self.reader.latest[1]
+        except Exception:
+            pass
+
+        return {
+            "timestamp": time.time(),
+            "state": state,
+            "action": action,
+            "trigger": bool(trigger),
+            "trigger_armed": bool(trigger_armed),
+            "grip_contact": bool(grip_contact),
+            "left_normal": feedback.left_normal,
+            "right_normal": feedback.right_normal,
+            "min_normal": feedback.min_normal,
+            "max_normal": feedback.max_normal,
+            "normal_diff": feedback.normal_diff,
+            "left_shear": feedback.left_shear,
+            "right_shear": feedback.right_shear,
+            "max_shear": feedback.max_shear,
+            "shear_diff": feedback.shear_diff,
+            "normal_change": normal_change,
+            "shear_change": shear_change,
+            "hold_pos": hold_pos,
+            "force_pct": force_pct,
+            "speed_pct": speed_pct,
+            "normal_unit": self.normal_unit,
+            "shear_unit": self.shear_unit,
+            "left_sample": left_sample,
+            "right_sample": right_sample,
+        }
+
+    def _emit_status(self, status):
+        if self.status_callback is None:
+            return
+        try:
+            self.status_callback(status)
+        except Exception as exc:
+            print(f"status callback failed: {exc}")

gripper_control_02/main.py

@@ -10,7 +10,12 @@ from .controller import SideTriggerGripController
 from .settings import Demo02Config
 
 
-def build_controller(argv=None):
+def build_controller(
+    argv=None,
+    status_callback=None,
+    include_visuals=False,
+    visual_size=320,
+):
     ensure_project_paths()
     config = Demo02Config.from_args(argv)
 
@@ -33,10 +38,18 @@ def build_controller(argv=None):
         target_fps=config.sensor_fps,
         cuda=not config.sensor_cpu,
         motion_threshold=config.motion_threshold,
+        include_visuals=include_visuals,
+        visual_size=visual_size,
     )
     gripper = GripperClient(config)
     feedback = TactileFeedbackProcessor(config, normal_converter, shear_converter)
-    return SideTriggerGripController(config, reader, gripper, feedback)
+    return SideTriggerGripController(
+        config,
+        reader,
+        gripper,
+        feedback,
+        status_callback=status_callback,
+    )
 
 
 def main(argv=None):

gripper_control_02/settings.py

@@ -43,6 +43,9 @@ class Demo02Config:
     speed: int = 25
     accel: int = 80
     decel: int = 80
+    release_open_speed: int = 50
+    release_open_accel: int = 120
+    release_open_decel: int = 120
 
     open_force: int = 10
     close_force: int = 10
@@ -52,25 +55,30 @@ class Demo02Config:
     force_min: int = 10
     force_max: int = 30
 
-    control_hz: float = 20.0
+    control_hz: float = 30.0
     baseline_seconds: float = 1.0
-    filter_alpha: float = 0.35
-    shear_filter_alpha: float = 0.45
+    filter_alpha: float = 0.55
+    shear_filter_alpha: float = 0.65
 
     trigger_normal_force: float = 0.04
     trigger_shear_force: float = 0.04
+    trigger_clear_stable_seconds: float = 0.2
     grip_normal_force: float = 0.05
     grip_requires_both: bool = True
     close_timeout_seconds: float = 5.0
     force_ramp_step: int = 1
-    force_ramp_interval: float = 0.2
-    grip_settle_seconds: float = 0.3
-    release_normal_change: float = 0.05
-    release_shear_change: float = 0.05
+    force_ramp_interval: float = 0.1
+    grip_settle_seconds: float = 0.1
+    release_normal_change: float = 0.03
+    release_shear_change: float = 0.03
     rearm_seconds: float = 0.5
     recover_normal_force: float = 0.02
     recover_shear_force: float = 0.02
     recover_stable_seconds: float = 0.4
+    open_position_recover_enabled: bool = True
+    open_position_tolerance: int = 100
+    open_position_stable_seconds: float = 0.1
+    open_position_check_interval: float = 0.1
 
     hold_fallback_pos: int | None = None
     trigger_force_update: bool = False
@@ -116,6 +124,9 @@ class Demo02Config:
         add("--speed", type=int, default=get("speed", cls.speed))
         add("--accel", type=int, default=get("accel", cls.accel))
         add("--decel", type=int, default=get("decel", cls.decel))
+        add("--release-open-speed", type=int, default=get("release_open_speed", cls.release_open_speed))
+        add("--release-open-accel", type=int, default=get("release_open_accel", cls.release_open_accel))
+        add("--release-open-decel", type=int, default=get("release_open_decel", cls.release_open_decel))
 
         add("--open-force", type=int, default=get("open_force", cls.open_force))
         add("--close-force", type=int, default=get("close_force", cls.close_force))
@@ -131,6 +142,7 @@ 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("--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")
@@ -144,6 +156,11 @@ class Demo02Config:
         add("--recover-normal-force", type=float, default=get("recover_normal_force", cls.recover_normal_force))
         add("--recover-shear-force", type=float, default=get("recover_shear_force", cls.recover_shear_force))
         add("--recover-stable-seconds", type=float, default=get("recover_stable_seconds", cls.recover_stable_seconds))
+        add("--open-position-recover-enabled", action="store_true", default=get("open_position_recover_enabled", cls.open_position_recover_enabled))
+        add("--disable-open-position-recover", dest="open_position_recover_enabled", action="store_false")
+        add("--open-position-tolerance", type=int, default=get("open_position_tolerance", cls.open_position_tolerance))
+        add("--open-position-stable-seconds", type=float, default=get("open_position_stable_seconds", cls.open_position_stable_seconds))
+        add("--open-position-check-interval", type=float, default=get("open_position_check_interval", cls.open_position_check_interval))
 
         add("--hold-fallback-pos", type=int, default=get("hold_fallback_pos", cls.hold_fallback_pos))
         add("--trigger-force-update", action="store_true", default=get("trigger_force_update", cls.trigger_force_update))

gripper_control_02/visualizer.py

@@ -0,0 +1,562 @@
+import argparse
+import multiprocessing as mp
+import sys
+import threading
+import time
+
+import cv2
+import numpy as np
+
+
+def _fix_qt_plugin_path():
+    try:
+        from PyQt5.QtCore import QLibraryInfo
+
+        import os
+
+        os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = QLibraryInfo.location(
+            QLibraryInfo.PluginsPath
+        )
+    except Exception:
+        pass
+
+
+_fix_qt_plugin_path()
+
+_QT_IMPORT_ERROR = None
+try:
+    from PyQt5.QtCore import Qt, QTimer
+    from PyQt5.QtGui import QFont, QImage, QPixmap
+    from PyQt5.QtWidgets import (
+        QApplication,
+        QFrame,
+        QGridLayout,
+        QHBoxLayout,
+        QLabel,
+        QMainWindow,
+        QPushButton,
+        QSizePolicy,
+        QVBoxLayout,
+        QWidget,
+    )
+except Exception as exc:
+    _QT_IMPORT_ERROR = exc
+    Qt = QTimer = QFont = QImage = QPixmap = QApplication = None
+    QGridLayout = QHBoxLayout = QLabel = QPushButton = QSizePolicy = None
+    QVBoxLayout = QWidget = None
+    QFrame = QMainWindow = object
+
+try:
+    import pyqtgraph as pg
+except Exception as exc:
+    pg = None
+    _PYQTGRAPH_IMPORT_ERROR = exc
+else:
+    _PYQTGRAPH_IMPORT_ERROR = None
+
+from .main import build_controller
+
+
+PLOT_POINTS = 180
+CURVE_COLOR = "#00E5FF"
+
+
+def cv2_to_pixmap(img_bgr):
+    if img_bgr is None:
+        return QPixmap()
+    if len(img_bgr.shape) == 2:
+        img_bgr = cv2.cvtColor(img_bgr, cv2.COLOR_GRAY2BGR)
+    rgb = cv2.cvtColor(np.ascontiguousarray(img_bgr), cv2.COLOR_BGR2RGB)
+    h, w, ch = rgb.shape
+    qimg = QImage(rgb.data, w, h, ch * w, QImage.Format_RGB888).copy()
+    return QPixmap.fromImage(qimg)
+
+
+def fmt(value, unit="", width=0, precision=3):
+    if value is None:
+        text = "--"
+    else:
+        text = f"{float(value):.{precision}f}"
+    if width:
+        text = text.rjust(width)
+    return f"{text}{unit}"
+
+
+STYLE = """
+QMainWindow {
+    background-color: #000000;
+}
+QWidget {
+    background-color: #000000;
+    color: #9a9a9a;
+    font-family: "Microsoft YaHei", "SimHei";
+}
+QLabel#Title {
+    color: #d7d7d7;
+    font-size: 18px;
+    font-weight: 600;
+}
+QLabel#Subtle {
+    color: #777777;
+    font-size: 12px;
+}
+QLabel#MetricLabel {
+    color: #888888;
+    font-size: 12px;
+}
+QLabel#MetricValue {
+    color: #f2f2f2;
+    font-family: "Cascadia Code", "Consolas";
+    font-size: 22px;
+    font-weight: 600;
+}
+QLabel#SmallValue {
+    color: #d8d8d8;
+    font-family: "Cascadia Code", "Consolas";
+    font-size: 15px;
+}
+QLabel#StatusChip {
+    background-color: #101010;
+    border: 1px solid #303030;
+    border-radius: 4px;
+    color: #00e5ff;
+    font-family: "Cascadia Code", "Consolas";
+    font-size: 18px;
+    font-weight: 600;
+    padding: 8px 12px;
+}
+QLabel#ActionChip {
+    background-color: #0b0b0b;
+    border: 1px solid #262626;
+    border-radius: 4px;
+    color: #c8c8c8;
+    font-family: "Cascadia Code", "Consolas";
+    font-size: 13px;
+    padding: 7px 10px;
+}
+QLabel#ImagePanel {
+    background-color: #0b0b0b;
+    border: 1px solid #262626;
+    border-radius: 4px;
+}
+QFrame#Panel {
+    background-color: #050505;
+    border: 1px solid #222222;
+    border-radius: 4px;
+}
+QPushButton {
+    background-color: #1a1a1a;
+    color: #a8a8a8;
+    border: 1px solid #333333;
+    border-radius: 4px;
+    padding: 7px 16px;
+    font-size: 13px;
+}
+QPushButton:hover {
+    background-color: #262626;
+    color: #d4d4d4;
+    border: 1px solid #555555;
+}
+QPushButton#StartButton {
+    background-color: #0d2818;
+    color: #67d79a;
+    border: 1px solid #1a4d30;
+}
+QPushButton#StopButton {
+    background-color: #2b1010;
+    color: #f07070;
+    border: 1px solid #4d1a1a;
+}
+"""
+
+
+class MetricBox(QFrame):
+    def __init__(self, title, value="--", sub=""):
+        super().__init__()
+        self.setObjectName("Panel")
+        layout = QVBoxLayout(self)
+        layout.setContentsMargins(10, 8, 10, 8)
+        layout.setSpacing(3)
+        self.title_label = QLabel(title)
+        self.title_label.setObjectName("MetricLabel")
+        self.value_label = QLabel(value)
+        self.value_label.setObjectName("MetricValue")
+        self.sub_label = QLabel(sub)
+        self.sub_label.setObjectName("Subtle")
+        layout.addWidget(self.title_label)
+        layout.addWidget(self.value_label)
+        layout.addWidget(self.sub_label)
+
+    def set_value(self, value, sub=None):
+        self.value_label.setText(value)
+        if sub is not None:
+            self.sub_label.setText(sub)
+
+
+class ImageBox(QFrame):
+    def __init__(self, title):
+        super().__init__()
+        self.setObjectName("Panel")
+        layout = QVBoxLayout(self)
+        layout.setContentsMargins(8, 8, 8, 8)
+        layout.setSpacing(6)
+        self.title_label = QLabel(title)
+        self.title_label.setObjectName("MetricLabel")
+        self.image_label = QLabel()
+        self.image_label.setObjectName("ImagePanel")
+        self.image_label.setAlignment(Qt.AlignCenter)
+        self.image_label.setMinimumSize(180, 180)
+        self.image_label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
+        self.image_label.setScaledContents(True)
+        layout.addWidget(self.title_label)
+        layout.addWidget(self.image_label, 1)
+
+    def set_image(self, img):
+        self.image_label.setPixmap(cv2_to_pixmap(img))
+
+
+class GripperDemo02Viewer(QMainWindow):
+    def __init__(self, argv=None):
+        super().__init__()
+        if _QT_IMPORT_ERROR is not None:
+            raise RuntimeError(f"PyQt5 import failed: {_QT_IMPORT_ERROR}")
+        if pg is None:
+            raise RuntimeError(f"pyqtgraph import failed: {_PYQTGRAPH_IMPORT_ERROR}")
+
+        self.setWindowTitle("Gripper Demo 02")
+        self.setStyleSheet(STYLE)
+        self.controller = None
+        self.control_thread = None
+        self.latest_status = None
+        self.status_lock = threading.Lock()
+        self.controller_finished = False
+        self.start_time = time.time()
+        self.force_history = []
+        self.argv = argv
+
+        self._build_ui()
+        self._configure_plot()
+
+        self.timer = QTimer(self)
+        self.timer.timeout.connect(self._refresh)
+        self.timer.start(33)
+
+    def _build_ui(self):
+        root = QWidget()
+        self.setCentralWidget(root)
+        outer = QVBoxLayout(root)
+        outer.setContentsMargins(8, 8, 8, 8)
+        outer.setSpacing(8)
+
+        top = QHBoxLayout()
+        top.setSpacing(8)
+        title = QLabel("Gripper Demo 02")
+        title.setObjectName("Title")
+        self.status_chip = QLabel("state=--")
+        self.status_chip.setObjectName("StatusChip")
+        self.action_chip = QLabel("action=--")
+        self.action_chip.setObjectName("ActionChip")
+        self.start_button = QPushButton("启动")
+        self.start_button.setObjectName("StartButton")
+        self.start_button.clicked.connect(self.start_control)
+        self.stop_button = QPushButton("停止")
+        self.stop_button.setObjectName("StopButton")
+        self.stop_button.clicked.connect(self.stop_control)
+        self.stop_button.setEnabled(False)
+        top.addWidget(title)
+        top.addWidget(self.status_chip)
+        top.addWidget(self.action_chip, 1)
+        top.addWidget(self.start_button)
+        top.addWidget(self.stop_button)
+        outer.addLayout(top)
+
+        main = QGridLayout()
+        main.setSpacing(8)
+        outer.addLayout(main, 1)
+
+        self.left_flow = ImageBox("左侧光流")
+        self.left_mag = ImageBox("左侧幅值图")
+        self.right_flow = ImageBox("右侧光流")
+        self.right_mag = ImageBox("右侧幅值图")
+        main.addWidget(self.left_flow, 0, 0)
+        main.addWidget(self.left_mag, 0, 1)
+        main.addWidget(self.right_flow, 1, 0)
+        main.addWidget(self.right_mag, 1, 1)
+
+        side = QVBoxLayout()
+        side.setSpacing(8)
+        main.addLayout(side, 0, 2, 2, 1)
+
+        metrics_grid = QGridLayout()
+        metrics_grid.setSpacing(8)
+        side.addLayout(metrics_grid)
+        self.left_force = MetricBox("左法向力", "--")
+        self.right_force = MetricBox("右法向力", "--")
+        self.shear_force = MetricBox("最大切向力", "--")
+        self.force_diff = MetricBox("左右差值", "--")
+        self.force_pct_box = MetricBox("force_pct", "--")
+        self.speed_box = MetricBox("速度", "--")
+        self.trigger_box = MetricBox("触发/武装", "--")
+        self.contact_box = MetricBox("接触", "--")
+        boxes = [
+            self.left_force,
+            self.right_force,
+            self.shear_force,
+            self.force_diff,
+            self.force_pct_box,
+            self.speed_box,
+            self.trigger_box,
+            self.contact_box,
+        ]
+        for idx, box in enumerate(boxes):
+            metrics_grid.addWidget(box, idx // 2, idx % 2)
+
+        pg.setConfigOption("background", "#000000")
+        pg.setConfigOption("foreground", "#777777")
+        pg.setConfigOptions(antialias=True)
+        self.plot_widget = pg.GraphicsLayoutWidget()
+        self.plot_widget.setMinimumHeight(240)
+        side.addWidget(self.plot_widget, 1)
+
+        self.info_box = QFrame()
+        self.info_box.setObjectName("Panel")
+        info_layout = QVBoxLayout(self.info_box)
+        info_layout.setContentsMargins(10, 8, 10, 8)
+        self.info_label = QLabel("等待启动")
+        self.info_label.setObjectName("SmallValue")
+        self.info_label.setWordWrap(True)
+        info_layout.addWidget(self.info_label)
+        side.addWidget(self.info_box)
+
+        main.setColumnStretch(0, 2)
+        main.setColumnStretch(1, 2)
+        main.setColumnStretch(2, 3)
+        main.setRowStretch(0, 1)
+        main.setRowStretch(1, 1)
+
+    def _configure_plot(self):
+        self.plot = self.plot_widget.addPlot()
+        self.plot.setTitle("左右法向力与最大切向力", color="#999999", size="13pt")
+        self.plot.titleLabel.setFont(QFont("SimHei", 13))
+        self.plot.showAxis("top", False)
+        self.plot.showAxis("right", False)
+        self.plot.showGrid(x=False, y=True, alpha=0.12)
+        self.plot.setLabel("bottom", "时间", units="s")
+        self.plot.setLabel("left", "力", units="N")
+        self.left_curve = self.plot.plot(pen=pg.mkPen(CURVE_COLOR, width=2), name="L")
+        self.right_curve = self.plot.plot(pen=pg.mkPen("#ffb000", width=2), name="R")
+        self.shear_curve = self.plot.plot(pen=pg.mkPen("#7ddc8a", width=2), name="Shear")
+
+    def start_control(self):
+        if self.control_thread is not None and self.control_thread.is_alive():
+            return
+        self.force_history.clear()
+        self.start_time = time.time()
+        self.start_button.setEnabled(False)
+        self.stop_button.setEnabled(True)
+        self.controller_finished = False
+        self.info_label.setText("启动中：正在连接夹爪与两个触觉传感器")
+
+        self.controller = build_controller(
+            self.argv,
+            status_callback=self._on_status,
+            include_visuals=True,
+            visual_size=320,
+        )
+        self.control_thread = threading.Thread(
+            target=self._run_controller,
+            daemon=True,
+        )
+        self.control_thread.start()
+
+    def stop_control(self):
+        if self.controller is not None:
+            self.controller.stop()
+        self.stop_button.setEnabled(False)
+        self.info_label.setText("停止中：正在释放相机与夹爪")
+
+    def _run_controller(self):
+        try:
+            self.controller.run()
+        except Exception as exc:
+            self._on_status({"state": "error", "action": str(exc), "timestamp": time.time()})
+        finally:
+            self.controller_finished = True
+
+    def _on_status(self, status):
+        with self.status_lock:
+            self.latest_status = status
+
+    def _refresh(self):
+        with self.status_lock:
+            status = self.latest_status
+        if not status:
+            self._update_thread_buttons()
+            return
+        self._update_thread_buttons()
+
+        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.action_chip.setText(f"action={action}")
+
+        self.left_force.set_value(
+            fmt(status.get("left_normal"), unit),
+            f"raw FPS {self._sample_fps(status, 'left_sample')}",
+        )
+        self.right_force.set_value(
+            fmt(status.get("right_normal"), unit),
+            f"raw FPS {self._sample_fps(status, 'right_sample')}",
+        )
+        self.shear_force.set_value(
+            fmt(status.get("max_shear"), shear_unit),
+            f"dShear {fmt(status.get('shear_change'), shear_unit)}",
+        )
+        self.force_diff.set_value(
+            fmt(status.get("normal_diff"), unit),
+            f"dN {fmt(status.get('normal_change'), unit)}",
+        )
+        self.force_pct_box.set_value(
+            str(status.get("force_pct", "--")),
+            f"hold_pos={status.get('hold_pos')}",
+        )
+        self.speed_box.set_value(
+            str(status.get("speed_pct", "--")),
+            "speed_pct",
+        )
+        self.trigger_box.set_value(
+            f"{int(bool(status.get('trigger')))} / {int(bool(status.get('trigger_armed')))}",
+            "trigger / armed",
+        )
+        self.contact_box.set_value(
+            str(int(bool(status.get("grip_contact")))),
+            self._contact_text(status),
+        )
+
+        left_sample = status.get("left_sample") or {}
+        right_sample = status.get("right_sample") or {}
+        self.left_flow.set_image(left_sample.get("flow_view"))
+        self.left_mag.set_image(left_sample.get("magnitude_view"))
+        self.right_flow.set_image(right_sample.get("flow_view"))
+        self.right_mag.set_image(right_sample.get("magnitude_view"))
+
+        elapsed = time.time() - self.start_time
+        self.force_history.append(
+            (
+                elapsed,
+                float(status.get("left_normal", 0.0) or 0.0),
+                float(status.get("right_normal", 0.0) or 0.0),
+                float(status.get("max_shear", 0.0) or 0.0),
+            )
+        )
+        if len(self.force_history) > PLOT_POINTS:
+            self.force_history = self.force_history[-PLOT_POINTS:]
+        self._update_plot()
+
+        self.info_label.setText(
+            " | ".join(
+                [
+                    f"min={fmt(status.get('min_normal'), unit)}",
+                    f"max={fmt(status.get('max_normal'), unit)}",
+                    f"L_shear={fmt(status.get('left_shear'), shear_unit)}",
+                    f"R_shear={fmt(status.get('right_shear'), shear_unit)}",
+                    f"flow L/R={self._flow_text(left_sample)} / {self._flow_text(right_sample)}",
+                ]
+            )
+        )
+
+    def _update_plot(self):
+        if not self.force_history:
+            return
+        data = np.asarray(self.force_history, dtype=float)
+        x = data[:, 0]
+        self.left_curve.setData(x, data[:, 1])
+        self.right_curve.setData(x, data[:, 2])
+        self.shear_curve.setData(x, data[:, 3])
+        if len(x) >= 2:
+            self.plot.setXRange(max(0.0, x[-1] - 6.0), max(6.0, x[-1]), padding=0)
+
+    def _update_thread_buttons(self):
+        if not self.controller_finished:
+            return
+        self.controller_finished = False
+        self.start_button.setEnabled(True)
+        self.stop_button.setEnabled(False)
+        if self.controller is not None:
+            self.info_label.setText("已停止")
+
+    def _sample_fps(self, status, key):
+        sample = status.get(key) or {}
+        fps = sample.get("fps")
+        if fps is None:
+            return "--"
+        return f"{fps:.1f}"
+
+    def _contact_text(self, status):
+        left = status.get("left_sample") or {}
+        right = status.get("right_sample") or {}
+        return f"L={int(bool(left.get('is_contact')))} R={int(bool(right.get('is_contact')))}"
+
+    def _flow_text(self, sample):
+        if not sample:
+            return "--"
+        return f"{sample.get('flow_mean', 0.0):.2f}/{sample.get('flow_max', 0.0):.2f}"
+
+    def keyPressEvent(self, event):
+        if event.key() == Qt.Key_Q:
+            self.close()
+            return
+        if event.key() == Qt.Key_Escape:
+            if self.isFullScreen():
+                self.showNormal()
+            else:
+                self.showFullScreen()
+            return
+        super().keyPressEvent(event)
+
+    def closeEvent(self, event):
+        self.timer.stop()
+        self.stop_control()
+        if self.control_thread is not None:
+            self.control_thread.join(timeout=3.0)
+        event.accept()
+
+
+def main(argv=None):
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--windowed", action="store_true")
+    ui_args, controller_argv = parser.parse_known_args(argv)
+
+    if _QT_IMPORT_ERROR is not None:
+        print(
+            "PyQt5 is required for the visualizer. "
+            "Install it in the active environment, for example: pip install PyQt5 pyqtgraph"
+        )
+        print(f"Import error: {_QT_IMPORT_ERROR}")
+        return 1
+    if pg is None:
+        print(
+            "pyqtgraph is required for the visualizer. "
+            "Install it in the active environment, for example: pip install pyqtgraph"
+        )
+        print(f"Import error: {_PYQTGRAPH_IMPORT_ERROR}")
+        return 1
+
+    QApplication.setAttribute(Qt.AA_EnableHighDpiScaling, True)
+    QApplication.setAttribute(Qt.AA_UseHighDpiPixmaps, True)
+    app = QApplication(sys.argv[:1])
+    window = GripperDemo02Viewer(controller_argv)
+    if ui_args.windowed:
+        window.resize(1500, 900)
+        window.show()
+    else:
+        window.showFullScreen()
+    window.start_control()
+    return app.exec_()
+
+
+if __name__ == "__main__":
+    mp.freeze_support()
+    raise SystemExit(main(sys.argv[1:]))

readme.md

@@ -14,3 +14,14 @@ conda activate py311
 1. 某个单侧有东西扫过时开始闭合夹住
 2. 闭合的时候要是检测到有物体，就像01那样几个状态，夹住它，不同的是后续有法向力，切向力输入的时候就松开夹爪（不要卡顿），直接张开到最开始的位置。
 3. 这就是整套流程，后续单侧再检测到有东西扫过的时候再执行这一整个过程
+
+# 展示03
+现在把hold_check状态抽离出来，这里后续需要扩写，这里我夹住时 物体的力可能受到
+
+# 可视化展示 qt界面
+目前夹住程序还是ok，现在需要可视化展示，用pyqt写，
+1. 展示两边的光流 和 赋值图
+2. 展示两边作用力的大小
+3. 展示当前状态 state=open_wait这种
+4. force_pct与速度 展示
+5. 你看看还有其他什么可以展示的