init

config/ddjx01.json

@@ -0,0 +1,44 @@
+{
+  "name": "prototype",
+  "version": "1.0",
+  "camera": {
+    "resolution_raw": [1920, 1080],
+    "fps": 30,
+    "units": 15,
+    "units_used": 9, 
+    "resolution_stitch": [365, 365],
+    "stitch_res_list": [365,263,123,63]
+  },
+  "stitcher": {
+    "min_area_frac": 0.01,
+    "max_area_frac": 0.05,
+    "max_aspect_ratio": 50,
+    "index": [1,2,3,6,7,8,11,12,13],
+    "margin": 10,
+    "nominal_ov_pos": 100,
+    "n": [3,3],
+    "row_tol": 0.6,
+    "box_size": [250, 250]
+  },
+  "tracker": {
+    "adaptive": true,
+    "lower_threshold": 12.0,
+    "upper_threshold": 18.0,
+    "loop_closure_interval": 1,
+    "algorithm": "dis",
+    "dis_preset": "medium",
+    "dis_mode": 3,
+    "algorithm_list": ["raft", "dis"],
+    "hhd_downsample": 3
+  },
+  "contact": {
+    "min_contour_area": 3000,
+    "flow_threshold":5,
+    "div_threshold":0.05,
+    "expansion_threshold": 0.01,
+    "kernel_size": [5,5],
+    "denoise_sigma": 12
+  }
+}
+
+

config/gripper.json

@@ -0,0 +1,113 @@
+{
+  "video1": "0",
+  "video2": "1",
+  "sensor_config1": "./config/ddjx01.json",
+  "sensor_config2": "./config/ddjx01.json",
+  "sensor_fps": 30.0,
+  "motion_threshold": 0.0,
+  "sensor_cpu": false,
+  "port": "COM5",
+  "slave_id": 1,
+  "baudrate": 115200,
+  "serial_timeout": 0.2,
+  "open_pos": 0,
+  "close_pos": 9000,
+  "speed": 25,
+  "accel": 80,
+  "decel": 80,
+  "release_open_speed": 50,
+  "release_open_accel": 120,
+  "release_open_decel": 120,
+  "open_force": 15,
+  "close_force": 15,
+  "grip_start_force": 10,
+  "hold_force": 15,
+  "initial_force": 15,
+  "force_min": 10,
+  "force_max": 80,
+  "control_hz": 30.0,
+  "baseline_seconds": 1.0,
+  "filter_alpha": 0.55,
+  "shear_filter_alpha": 0.65,
+  "trigger_normal_force": 0.1,
+  "trigger_shear_force": 0.1,
+  "trigger_clear_stable_seconds": 0.2,
+  "open_wait_stable_samples": 12,
+  "open_wait_stable_range_n": 0.03,
+  "open_wait_stable_trend_n": 0.015,
+  "grip_normal_force": 0.2,
+  "grip_requires_both": true,
+  "close_timeout_seconds": 5.0,
+  "closing_command_interval": 0.2,
+  "force_ramp_step": 1,
+  "force_ramp_interval": 0.1,
+  "grip_settle_seconds": 0.1,
+  "adaptive_grip_enabled": true,
+  "adaptive_shear_start_force": 0.1,
+  "adaptive_shear_full_force": 0.6,
+  "adaptive_force_min": 15,
+  "adaptive_force_max": 45,
+  "adaptive_force_step": 1,
+  "adaptive_force_interval": 0.05,
+  "release_arm_delay_seconds": 0.8,
+  "release_normal_change": 0.2,
+  "release_shear_change": 0.2,
+  "rearm_seconds": 0.5,
+  "recover_normal_force": 0.02,
+  "recover_shear_force": 0.02,
+  "recover_stable_seconds": 0.4,
+  "open_position_recover_enabled": true,
+  "open_position_tolerance": 100,
+  "open_position_stable_seconds": 0.1,
+  "open_position_check_interval": 0.1,
+  "hold_fallback_pos": null,
+  "trigger_force_update": false,
+  "open_at_end": true,
+  "dry_run": false,
+  "normal_force_calibration": {
+    "enabled": true,
+    "method": "piecewise_linear",
+    "extrapolate": true,
+    "clamp_output_min": 0.0,
+    "points": [
+      {
+        "fnormal": 1000.0,
+        "force_n": 0.0
+      },
+      {
+        "fnormal": 10000.0,
+        "force_n": 0.377
+      },
+      {
+        "fnormal": 30000.0,
+        "force_n": 1.377
+      },
+      {
+        "fnormal": 62000.0,
+        "force_n": 2.377
+      }
+    ]
+  },
+  "shear_force_calibration": {
+    "enabled": true,
+    "method": "piecewise_linear",
+    "input": "components",
+    "signed": true,
+    "extrapolate": true,
+    "clamp_output_min": 0.0,
+    "points": [
+      {
+        "raw": 1000.0,
+        "force_n": 0.0
+      },
+      {
+        "raw": 36000.0,
+        "force_n": 1.19
+      },
+      {
+        "raw": 68000.0,
+        "force_n": 2.06
+      }
+    ]
+  }
+}

gripper/README.md

@@ -0,0 +1,147 @@
+# Gripper
+
+夹爪02是新的展示程序，和原来的定时开合逻辑分开。它不是定时循环开合；程序只是常驻监听单侧触发，每次触发后执行一次“闭合夹住 -> 监测变化 -> 直接张开”的流程。
+
+## 展示流程
+
+1. 程序启动后，夹爪先张开。
+2. 程序等待任意一侧触觉传感器感受到法向力或切向力。
+3. 展示时，把物体从某个侧边扫一下，触发单侧力变化。
+4. 触发后夹爪开始闭合，你把物体移动到中间。
+5. 闭合过程中检测到物体后，切到低力并保持当前位置。
+6. 像 01 一样进入 `gripping`，先慢慢加力到基础 `HOLD_FORCE`。
+7. 进入 `hold_check` 后，根据切向力估计物体负载；切向力越大，目标夹紧力越高，并按小步进继续补力。
+8. 自适应补力停止并稳定 `RELEASE_ARM_DELAY_SECONDS` 后，重新记录释放参考力，才开始检测人取物松开。
+9. 后续法向力或切向力变化超过阈值，夹爪直接张开到 `OPEN_POS`。
+10. 张开后进入 `open_recover`，保持张开并等待恢复条件。
+11. 夹爪回到 `OPEN_POS` 初始点位，或法向/切向读数都低于恢复阈值并稳定后，继续休息至少 `REARM_SECONDS`。
+12. 休息结束后才回到 `open_wait` 并重新武装；休息期间不管有没有物体扫过，都不会闭合。
+
+## 运行
+
+从项目根目录运行：
+
+```powershell
+conda activate py311
+python -m gripper.main
+```
+
+只看逻辑、不打开串口：
+
+```powershell
+python -m gripper.main --dry-run
+```
+
+PyQt 可视化展示：
+
+```powershell
+python -m gripper.visualizer
+```
+
+窗口模式运行：
+
+```powershell
+python -m gripper.visualizer --windowed
+```
+
+兼容入口：
+
+```powershell
+python -m gripper.visualizer
+```
+
+默认配置文件：
+
+```text
+config\gripper_demo_02.json
+```
+
+## 关键配置
+
+- `trigger_normal_force`: 单侧法向力触发阈值。
+- `trigger_shear_force`: 单侧切向力触发阈值。
+- `trigger_clear_stable_seconds`: 张开恢复后，触发力必须清掉并稳定多久，才允许下一次扫过触发。
+- `grip_normal_force`: 夹住时左右接触阈值。
+- `grip_requires_both`: 是否必须左右都接触才算夹住。
+- `grip_start_force`: 检测到物体后先切到的低力。
+- `force_ramp_step`: 慢慢加力时每次增加多少 force_pct。
+- `force_ramp_interval`: 慢慢加力时两次加力的间隔。
+- `hold_force`: 慢慢加力到这个值后进入 `hold_check`。
+- `grip_settle_seconds`: 夹住后等待稳定多久再记录参考力。
+- `adaptive_grip_enabled`: 是否启用按切向力估计负载并自适应补夹紧力。
+- `adaptive_shear_start_force`: 切向力低于这个值时，认为是轻物体，只使用最小目标力。
+- `adaptive_shear_full_force`: 切向力达到这个值时，认为负载较大，目标力加到最大目标力。
+- `adaptive_force_min`: 自适应夹持的最小目标力。
+- `adaptive_force_max`: 自适应夹持的最大目标力；重物夹不住优先加这个，但不要超过 `force_max`。
+- `adaptive_force_step`: 自适应补力时每次增加多少 `force_pct`。
+- `adaptive_force_interval`: 自适应补力两次之间的间隔，越大越柔和。
+- `release_arm_delay_seconds`: 自适应补力停止后，等待多久再启用“人取物松开”检测。
+- `release_normal_change`: 夹住后法向力变化超过它就松开。
+- `release_shear_change`: 夹住后切向力变化超过它就松开。
+- `release_open_speed`: 检测到松开后，张开到 `open_pos` 使用的速度。
+- `release_open_accel`: release 张开的加速度。
+- `release_open_decel`: release 张开的减速度。
+- `rearm_seconds`: 夹爪回到 `open_pos` 或恢复条件成立后，至少休息多久才重新等待触发；休息期间扫过也不会闭合。
+- `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` 中读取夹爪当前位置的间隔。
+- `normal_force_calibration`: 法向力标定，配置在 `config\gripper_demo_02.json`。
+- `shear_force_calibration`: 切向力标定，配置在 `config\gripper_demo_02.json`。
+
+## 日志字段
+
+- `L/R`: 左右法向力，单位 N。
+- `maxN`: 左右法向力最大值，单位 N。
+- `shear`: 左右切向力最大值，单位 N。
+- `trigger`: 是否满足单侧触发条件。
+- `armed`: 是否已经重新武装；只有 `trigger=1 armed=1` 且是新的一次触发上升沿才会闭合。
+- `grip_contact`: 是否满足夹住接触条件。
+- `dN`: 夹住后当前法向力相对参考值的最大变化，单位 N。
+- `dShear`: 夹住后当前切向力相对参考值的变化，单位 N。
+- `state`: 当前状态。
+- `hold_pos`: 夹住后保持的位置。
+- `force_pct`: 当前夹爪目标力百分比。
+- `target`: 当前根据切向力计算出来的自适应目标力。
+- `release_armed`: 是否已经启用释放检测；`0` 表示还在补力/等待稳定，`1` 表示法向或切向变化会触发张开。
+- `action`: 当前动作。
+
+## 可视化展示
+
+- 左右两边的光流箭头图。
+- 左右两边的光流幅值图。
+- 左右法向力、左切向力、右切向力。
+- 当前 `state`、`action`、`trigger/armed`、`grip_contact`。
+- 当前 `force_pct`、自适应目标力、速度 `speed_pct`、`hold_pos`。
+- 顶部有 `重启` 按钮，会先停止当前控制线程，释放相机与夹爪后再重新启动。
+- 当前 `release_armed`，用于判断系统是在补夹紧力，还是已经开始检测人取物松开。
+- 折线图带图例，默认固定 y 轴为 `0~3 N`，范围在 `visualizer.py` 顶部的 `PLOT_FORCE_Y_MIN_N / PLOT_FORCE_Y_MAX_N` 调整。
+- 左右传感器 FPS、接触标志、光流均值/最大值。
+
+## 状态说明
+
+- `open_wait`: 夹爪张开，等待任意单侧触发。
+- `closing`: 单侧触发后正在闭合，等待检测到物体。
+- `gripping`: 检测到物体后低力保持当前位置，并慢慢加力。
+- `hold_check`: 已夹住；先根据切向力自适应补夹紧力，补力稳定后再监测法向/切向变化；变化超过阈值会直接张开。
+- `open_recover`: 已经发出张开命令，正在等待回到初始点位或等待法向/切向残余力清空；这个状态不会闭合夹爪。
+
+## 常见动作说明
+
+- `force-change-release-open`: 检测到夹住后的法向或切向变化，已经发出张开命令。
+- `adaptive-grip-ramp`: 切向力显示负载较大，正在按小步进增加夹紧力。
+- `adaptive-grip-wait`: 已经需要更高目标力，但还没到下一次加力时间。
+- `release-arm-delay`: 补力刚停止，正在等待力稳定，暂时不触发松开。
+- `release-reference-armed`: 已重新记录释放参考力，后续法向/切向变化会触发张开。
+- `open-recover-position`: 夹爪当前位置已经接近 `OPEN_POS`，正在等待到位稳定。
+- `open-ready-position`: 夹爪已经回到初始点位，并且休息时间结束，重新进入等待下一次扫过触发；此时会直接武装，但残余触发保持不变不会自动闭合。
+- `open-wait-clear-trigger`: 已经回到等待状态，但当前力还超过触发阈值，暂时不会闭合。
+- `open-wait-trigger-clearing`: 触发力已经低于阈值，正在等待清零稳定时间。
+- `open-wait-armed`: 触发已经清掉并稳定，下一次扫过可以闭合。
+- `open-wait-trigger-held`: 已经武装，但当前触发不是新上升沿，等待力先降下去再下一次扫过。
+- `open-recover-wait-force-clear`: 夹爪保持张开，但传感器还有残余力，不允许重新闭合。
+- `open-recover-stabilizing`: 力已经低于恢复阈值，正在等待稳定和休息时间结束。
+- `open-ready`: 力已经清空并稳定，并且休息时间结束，重新进入等待下一次扫过触发。

gripper/__init__.py

@@ -0,0 +1,2 @@
+"""Demo 02 gripper control: side-touch trigger, grip, and instant release."""
+

gripper/calibration.py

@@ -0,0 +1,103 @@
+from dataclasses import dataclass
+
+
+DEFAULT_NORMAL_FORCE_CALIBRATION = {
+    "enabled": False,
+    "points": [],
+}
+
+DEFAULT_SHEAR_FORCE_CALIBRATION = {
+    "enabled": False,
+    "points": [],
+}
+
+
+def parse_calibration_point(point):
+    if isinstance(point, dict):
+        raw = point.get("fnormal", point.get("raw", point.get("x")))
+        force_n = point.get("force_n", point.get("n", point.get("y")))
+        return float(raw), float(force_n)
+    if isinstance(point, (list, tuple)) and len(point) >= 2:
+        return float(point[0]), float(point[1])
+    raise ValueError(f"invalid calibration point: {point!r}")
+
+
+@dataclass(frozen=True)
+class ForceConverter:
+    points: tuple
+    unit: str = "N"
+    enabled: bool = True
+    extrapolate: bool = True
+    clamp_output_min: float | None = 0.0
+    input_mode: str = "magnitude"
+    signed: bool = False
+
+    @classmethod
+    def from_config(cls, calibration, config_name):
+        if calibration is None:
+            calibration = {"enabled": False}
+        if not calibration.get("enabled", True):
+            return cls(points=(), unit="raw", enabled=False)
+
+        method = str(calibration.get("method", "piecewise_linear")).lower()
+        if method != "piecewise_linear":
+            raise ValueError(f"unsupported {config_name} method: {method}")
+
+        points = tuple(sorted(
+            parse_calibration_point(point)
+            for point in calibration.get("points", [])
+        ))
+        if len(points) < 2:
+            raise ValueError(f"{config_name}.points must contain at least two points")
+
+        return cls(
+            points=points,
+            enabled=True,
+            extrapolate=bool(calibration.get("extrapolate", True)),
+            clamp_output_min=calibration.get("clamp_output_min", 0.0),
+            input_mode=str(calibration.get("input", "magnitude")).lower(),
+            signed=bool(calibration.get("signed", False)),
+        )
+
+    def convert(self, raw):
+        if not self.enabled:
+            return float(raw)
+
+        raw = float(raw)
+        points = self.points
+        if raw <= points[0][0]:
+            segment = (points[0], points[1])
+            if not self.extrapolate:
+                return self._clamp(points[0][1])
+        elif raw >= points[-1][0]:
+            segment = (points[-2], points[-1])
+            if not self.extrapolate:
+                return self._clamp(points[-1][1])
+        else:
+            segment = None
+            for left, right in zip(points, points[1:]):
+                if left[0] <= raw <= right[0]:
+                    segment = (left, right)
+                    break
+            if segment is None:
+                return self._clamp(points[-1][1])
+
+        (raw0, n0), (raw1, n1) = segment
+        if raw1 == raw0:
+            value = n0
+        else:
+            ratio = (raw - raw0) / (raw1 - raw0)
+            value = n0 + ratio * (n1 - n0)
+        return self._clamp(value)
+
+    def convert_component(self, raw):
+        if not self.signed:
+            return self.convert(raw)
+        raw = float(raw)
+        sign = -1.0 if raw < 0.0 else 1.0
+        return sign * self.convert(abs(raw))
+
+    def _clamp(self, value):
+        if self.clamp_output_min is not None:
+            value = max(float(self.clamp_output_min), value)
+        return value

gripper/config_loader.py

@@ -0,0 +1,21 @@
+import json
+
+
+def load_control_config(path):
+    return load_json_config(path)
+
+
+def load_json_config(path):
+    try:
+        with open(path, "r", encoding="utf-8") as f:
+            config = json.load(f)
+    except FileNotFoundError:
+        print(f"[Info] control config not found: {path}; using CLI/default values")
+        return {}
+    except Exception as exc:
+        print(f"[Error] failed to read control config {path}: {exc}; using CLI/default values")
+        return {}
+    if not isinstance(config, dict):
+        print(f"[Error] control config {path} must be a JSON object; using CLI/default values")
+        return {}
+    return config

gripper/controller.py

@@ -0,0 +1,771 @@
+import time
+import threading
+
+from .filters import clamp
+
+
+class SideTriggerGripController:
+    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()
+        self._manual_lock = threading.Lock()
+        self._manual_command = None
+
+    def stop(self):
+        self._stop_event.set()
+
+    def request_manual_command(self, command):
+        if command not in {"open", "close", "hold", "release"}:
+            raise ValueError(f"unsupported manual command: {command}")
+        with self._manual_lock:
+            self._manual_command = command
+
+    def _pop_manual_command(self):
+        with self._manual_lock:
+            command = self._manual_command
+            self._manual_command = None
+        return command
+
+    def run(self):
+        self._stop_event.clear()
+        self.reader.start()
+
+        try:
+            self.gripper.connect()
+            open_force = int(clamp(
+                self.config.open_force,
+                self.config.force_min,
+                self.config.force_max,
+            ))
+            print("demo02 start: open gripper and wait for side touch")
+            self.gripper.open(open_force, "startup-open")
+
+            self._print_thresholds()
+            print(
+                f"Calibrating baseline for {self.config.baseline_seconds:.2f}s. "
+                "Keep sensors unloaded."
+            )
+            baseline = self.feedback_processor.calibrate_baseline(self.reader)
+            print(
+                f"baseline normal: L={baseline.left_normal:.4f}{self.normal_unit} "
+                f"R={baseline.right_normal:.4f}{self.normal_unit}; "
+                f"shear: L={baseline.left_shear:.4f}{self.shear_unit} "
+                f"R={baseline.right_shear:.4f}{self.shear_unit}"
+            )
+
+            self._run_event_monitor()
+
+        except KeyboardInterrupt:
+            print("\nStopping demo02.")
+        finally:
+            self.gripper.final_open_if_needed()
+            self.reader.stop()
+
+    def _print_thresholds(self):
+        print(
+            "Trigger thresholds: "
+            f"normal={self.config.trigger_normal_force:.3f}{self.normal_unit}, "
+            f"shear={self.config.trigger_shear_force:.3f}{self.shear_unit}"
+        )
+        print(
+            "Grip/release thresholds: "
+            f"grip_normal={self.config.grip_normal_force:.3f}{self.normal_unit}, "
+            f"requires_both={int(self.config.grip_requires_both)}, "
+            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(
+            "Adaptive grip: "
+            f"enabled={int(self.config.adaptive_grip_enabled)}, "
+            f"shear_start={self.config.adaptive_shear_start_force:.3f}{self.shear_unit}, "
+            f"shear_full={self.config.adaptive_shear_full_force:.3f}{self.shear_unit}, "
+            f"force={self.config.adaptive_force_min}-{self.config.adaptive_force_max}, "
+            f"step={self.config.adaptive_force_step}, "
+            f"interval={self.config.adaptive_force_interval:.2f}s, "
+            f"release_arm_delay={self.config.release_arm_delay_seconds:.2f}s"
+        )
+        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
+        interval = 1.0 / config.control_hz if config.control_hz > 0 else 0.05
+
+        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
+        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()
+            manual_action = None
+
+            manual_command = self._pop_manual_command()
+            if manual_command == "open":
+                force_pct = int(clamp(
+                    config.open_force,
+                    config.force_min,
+                    config.force_max,
+                ))
+                self.gripper.open(force_pct, "manual-open")
+                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
+                trigger_clear_since = None
+                trigger_armed = False
+                reset_open_wait_stability()
+                release_armed = False
+                hold_pos = None
+                reference = None
+                manual_action = "manual-open"
+            elif manual_command == "close":
+                force_pct = int(clamp(
+                    config.close_force,
+                    config.force_min,
+                    config.force_max,
+                ))
+                self.gripper.close(force_pct, "manual-close")
+                state = "manual_close"
+                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
+                reset_open_wait_stability()
+                release_armed = False
+                hold_pos = None
+                reference = None
+                manual_action = "manual-close"
+            elif manual_command == "hold":
+                force_pct = int(clamp(
+                    config.hold_force,
+                    config.force_min,
+                    config.force_max,
+                ))
+                self.gripper.set_force(force_pct)
+                hold_pos = self.gripper.hold_current_position(force_pct)
+                state = "manual_hold"
+                state_start = now
+                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"
+            elif manual_command == "release":
+                if state == "manual_hold":
+                    state = "open_wait"
+                    state_start = now
+                    trigger_clear_since = None
+                    trigger_armed = False
+                    reset_open_wait_stability()
+                    release_armed = False
+                    hold_pos = None
+                    reference = None
+                manual_action = "manual-release"
+
+            feedback = self.feedback_processor.read(self.reader, timeout=0.3)
+            if feedback is None:
+                action = manual_action or "waiting-for-sensor-samples"
+                print(f"state={state} action={action}")
+                self._emit_status(
+                    {
+                        "timestamp": time.time(),
+                        "state": state,
+                        "action": action,
+                        "force_pct": force_pct,
+                        "adaptive_target_force": adaptive_target_force,
+                        "adaptive_grip_enabled": config.adaptive_grip_enabled,
+                        "release_armed": release_armed,
+                        "manual_hold_active": state == "manual_hold",
+                        "speed_pct": config.speed,
+                        "normal_unit": self.normal_unit,
+                        "shear_unit": self.shear_unit,
+                    }
+                )
+                time.sleep(0.05)
+                continue
+
+            trigger = self._side_triggered(feedback)
+            grip_contact = self._grip_contact(feedback)
+            normal_change = 0.0
+            shear_change = 0.0
+            action = manual_action or "wait"
+
+            if manual_action is not None:
+                pass
+            elif state == "open_wait":
+                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:
+                    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,
+                        config.force_min,
+                        config.force_max,
+                    ))
+                    self.gripper.close(force_pct, "side-trigger-close")
+                    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
+                    reset_open_wait_stability()
+                    adaptive_target_force = self._adaptive_force_target(feedback)
+                    release_armed = False
+                    hold_pos = None
+                    reference = None
+                    action = "side-trigger-close"
+
+            elif state == "closing":
+                if grip_contact:
+                    force_pct = int(clamp(
+                        config.grip_start_force,
+                        config.force_min,
+                        config.force_max,
+                    ))
+                    self.gripper.set_force(force_pct)
+                    hold_pos = self.gripper.hold_current_position(force_pct)
+                    state = "gripping"
+                    state_start = now
+                    last_force_ramp_time = now
+                    adaptive_target_force = self._adaptive_force_target(feedback)
+                    release_armed = False
+                    reference = None
+                    action = "object-detected-low-force"
+                elif now - state_start >= config.close_timeout_seconds:
+                    force_pct = int(clamp(
+                        config.open_force,
+                        config.force_min,
+                        config.force_max,
+                    ))
+                    self.gripper.open(force_pct, "close-timeout-open")
+                    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
+                    reset_open_wait_stability()
+                    release_armed = False
+                    hold_pos = None
+                    reference = None
+                    action = "close-timeout-open"
+                else:
+                    if (
+                        config.closing_command_interval > 0
+                        and now - last_force_ramp_time >= config.closing_command_interval
+                    ):
+                        self.gripper.close(force_pct, "closing-continue")
+                        last_force_ramp_time = now
+                        action = "closing-continue"
+                    else:
+                        action = "closing-to-grip"
+
+            elif state == "gripping":
+                if (
+                    now - last_force_ramp_time >= config.force_ramp_interval
+                    and force_pct < config.hold_force
+                ):
+                    force_pct = int(clamp(
+                        force_pct + config.force_ramp_step,
+                        config.force_min,
+                        config.hold_force,
+                    ))
+                    self.gripper.set_force(force_pct)
+                    last_force_ramp_time = now
+                    action = "grip-ramp-up"
+                elif (
+                    force_pct >= config.hold_force
+                    and now - state_start >= config.grip_settle_seconds
+                ):
+                    reference = self._make_reference(feedback)
+                    state = "hold_check"
+                    state_start = now
+                    adaptive_target_force = self._adaptive_force_target(feedback)
+                    release_armed = False
+                    action = "hold-reference-armed"
+                else:
+                    action = "grip-hold"
+
+            elif state == "hold_check":
+                adaptive_target_force = self._adaptive_force_target(feedback)
+                should_adapt_force = (
+                    config.adaptive_grip_enabled
+                    and not release_armed
+                    and force_pct < adaptive_target_force
+                )
+
+                if (
+                    should_adapt_force
+                    and now - last_force_ramp_time >= config.adaptive_force_interval
+                ):
+                    force_pct = int(clamp(
+                        force_pct + config.adaptive_force_step,
+                        config.force_min,
+                        adaptive_target_force,
+                    ))
+                    self.gripper.set_force(force_pct)
+                    last_force_ramp_time = now
+                    state_start = now
+                    reference = self._make_reference(feedback)
+                    release_armed = False
+                    action = "adaptive-grip-ramp"
+                elif should_adapt_force:
+                    release_armed = False
+                    action = "adaptive-grip-wait"
+                elif (
+                    not release_armed
+                    and now - max(state_start, last_force_ramp_time)
+                    < config.release_arm_delay_seconds
+                ):
+                    action = "release-arm-delay"
+                elif not release_armed:
+                    reference = self._make_reference(feedback)
+                    release_armed = True
+                    action = "release-reference-armed"
+                else:
+                    normal_change, shear_change = self._release_changes(feedback, reference)
+                    should_release = (
+                        normal_change >= config.release_normal_change
+                        or shear_change >= config.release_shear_change
+                    )
+                    if should_release:
+                        force_pct = int(clamp(
+                            config.open_force,
+                            config.force_min,
+                            config.force_max,
+                        ))
+                        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
+                        reset_open_wait_stability()
+                        release_armed = False
+                        hold_pos = None
+                        reference = None
+                        action = "force-change-release-open"
+                    else:
+                        action = "hold-check"
+
+            elif state == "open_recover":
+                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
+                        >= max(
+                            config.open_position_stable_seconds,
+                            config.rearm_seconds,
+                        )
+                    ):
+                        state = "open_wait"
+                        state_start = now
+                        recover_quiet_since = None
+                        recover_position_since = None
+                        trigger_clear_since = None
+                        trigger_armed = True
+                        reset_open_wait_stability()
+                        last_trigger = trigger
+                        release_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"
+                    elif (
+                        now - recover_quiet_since
+                        >= max(config.recover_stable_seconds, config.rearm_seconds)
+                    ):
+                        state = "open_wait"
+                        state_start = now
+                        recover_quiet_since = None
+                        recover_position_since = None
+                        trigger_clear_since = None
+                        trigger_armed = True
+                        reset_open_wait_stability()
+                        last_trigger = trigger
+                        release_armed = False
+                        hold_pos = None
+                        reference = None
+                        action = "open-ready"
+                    else:
+                        action = "open-recover-stabilizing"
+                else:
+                    recover_quiet_since = None
+                    recover_position_since = None
+                    action = "open-recover-wait-force-clear"
+
+            elif state == "manual_hold":
+                action = "manual-hold"
+
+            elif state == "manual_close":
+                action = "manual-close"
+
+            self._log_tick(
+                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,
+                adaptive_target_force=adaptive_target_force,
+                release_armed=release_armed,
+                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,
+                    adaptive_target_force=adaptive_target_force,
+                    release_armed=release_armed,
+                    action=action,
+                    trigger_armed=trigger_armed,
+                )
+            )
+            last_trigger = trigger
+
+            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
+            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
+        if self.config.grip_requires_both:
+            return left_contact and right_contact
+        return left_contact or right_contact
+
+    def _recover_quiet(self, feedback):
+        return (
+            feedback.max_normal <= self.config.recover_normal_force
+            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 _adaptive_force_target(self, feedback):
+        config = self.config
+        if not config.adaptive_grip_enabled:
+            return int(clamp(config.hold_force, config.force_min, config.force_max))
+
+        force_min = int(clamp(
+            config.adaptive_force_min,
+            config.force_min,
+            config.force_max,
+        ))
+        force_max = int(clamp(
+            config.adaptive_force_max,
+            config.force_min,
+            config.force_max,
+        ))
+        if force_max < force_min:
+            force_max = force_min
+
+        shear_start = max(0.0, float(config.adaptive_shear_start_force))
+        shear_full = max(shear_start + 1e-6, float(config.adaptive_shear_full_force))
+        shear = max(0.0, float(feedback.max_shear))
+        if shear <= shear_start:
+            return force_min
+        if shear >= shear_full:
+            return force_max
+
+        ratio = (shear - shear_start) / (shear_full - shear_start)
+        target = round(force_min + ratio * (force_max - force_min))
+        return int(clamp(target, force_min, force_max))
+
+    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,
+        }
+
+    def _release_changes(self, feedback, reference):
+        if reference is None:
+            return 0.0, 0.0
+        normal_change = max(
+            abs(feedback.left_normal - reference["left_normal"]),
+            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(
+        self,
+        feedback,
+        state,
+        trigger,
+        grip_contact,
+        normal_change,
+        shear_change,
+        hold_pos,
+        force_pct,
+        adaptive_target_force,
+        release_armed,
+        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)} 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} target={adaptive_target_force:3d} "
+            f"release_armed={int(release_armed)} action={action}"
+        )
+
+    def _make_status(
+        self,
+        feedback,
+        state,
+        trigger,
+        grip_contact,
+        normal_change,
+        shear_change,
+        hold_pos,
+        force_pct,
+        adaptive_target_force,
+        release_armed,
+        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,
+            "left_shear_x": feedback.left_shear_x,
+            "left_shear_y": feedback.left_shear_y,
+            "right_shear_x": feedback.right_shear_x,
+            "right_shear_y": feedback.right_shear_y,
+            "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,
+            "adaptive_target_force": adaptive_target_force,
+            "adaptive_grip_enabled": self.config.adaptive_grip_enabled,
+            "release_armed": bool(release_armed),
+            "manual_hold_active": state == "manual_hold",
+            "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/feedback.py

@@ -0,0 +1,168 @@
+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
+
+
+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
+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
+    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:
+    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)
+        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
+        if seconds <= 0:
+            self.baseline = ForceBaseline()
+            return self.baseline
+
+        left_values = []
+        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"]))
+                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"]))
+                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(
+            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,
+            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
+
+    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,
+        )
+        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,
+            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,
+            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,
+        )

gripper/filters.py

@@ -0,0 +1,23 @@
+class ExponentialFilter:
+    def __init__(self, alpha):
+        self.alpha = max(0.0, min(1.0, float(alpha)))
+        self.value = 0.0
+        self.initialized = False
+
+    def update(self, sample):
+        sample = float(sample)
+        if not self.initialized:
+            self.value = sample
+            self.initialized = True
+        else:
+            self.value = self.alpha * sample + (1.0 - self.alpha) * self.value
+        return self.value
+
+    def reset(self):
+        self.value = 0.0
+        self.initialized = False
+
+
+def clamp(value, min_value, max_value):
+    return max(min_value, min(max_value, value))
+

gripper/force_reader.py

@@ -0,0 +1,456 @@
+import os
+os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
+
+import argparse
+import inspect
+import multiprocessing as mp
+import queue
+import time
+from multiprocessing import Process, Queue
+
+import cv2
+import numpy as np
+
+from .paths import ensure_project_paths
+from .rotation import (
+    DEFAULT_ROTATION1,
+    DEFAULT_ROTATION2,
+    normalize_rotation_config,
+    write_temp_rotation_config,
+)
+
+ensure_project_paths()
+
+
+def parse_video_source(value):
+    if isinstance(value, str):
+        value = value.strip()
+        if value.isdigit():
+            return int(value)
+    return value
+
+
+def resolve_rotation_config_path(value, label):
+    if not isinstance(value, dict):
+        return value
+    return write_temp_rotation_config(value, label)
+
+
+def build_sensor(orisys_module, vid_src, config_name, rotation_config, cuda, isstitch, backend, sensor_id):
+    candidates = {
+        "isstitch": isstitch,
+        "cuda": cuda,
+        "verbose": False,
+        "config_name": config_name,
+        "backend": backend,
+        "rotation_config": rotation_config,
+    }
+
+    try:
+        params = inspect.signature(orisys_module.Sensor).parameters
+    except (TypeError, ValueError):
+        params = {}
+
+    accepts_kwargs = any(
+        param.kind == inspect.Parameter.VAR_KEYWORD
+        for param in params.values()
+    )
+    if accepts_kwargs:
+        kwargs = candidates
+    else:
+        kwargs = {
+            key: value
+            for key, value in candidates.items()
+            if key in params
+        }
+
+    if "rotation_config_path" in params:
+        kwargs["rotation_config_path"] = resolve_rotation_config_path(
+            rotation_config,
+            f"sensor_{sensor_id}",
+        )
+
+    return orisys_module.Sensor(vid_src, **kwargs)
+
+
+def _put_latest(result_queue, data):
+    try:
+        result_queue.put_nowait(data)
+        return
+    except queue.Full:
+        pass
+
+    try:
+        result_queue.get_nowait()
+    except Exception:
+        pass
+
+    try:
+        result_queue.put_nowait(data)
+    except queue.Full:
+        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,
+    config_name,
+    rotation_config,
+    result_queue,
+    stop_event,
+    target_fps,
+    cuda,
+    isstitch,
+    backend,
+    motion_threshold,
+    include_visuals,
+    visual_size,
+):
+    ensure_project_paths()
+    import orisys
+
+    sensor = None
+    try:
+        sensor = build_sensor(
+            orisys,
+            vid_src,
+            config_name,
+            rotation_config,
+            cuda,
+            isstitch,
+            backend,
+            sensor_id,
+        )
+
+        frame_interval = 1.0 / target_fps if target_fps and target_fps > 0 else 0.0
+
+        while not stop_event.is_set():
+            t_start = time.perf_counter()
+            img = sensor.get_img()
+            if img is None:
+                time.sleep(0.01)
+                continue
+
+            is_contact = sensor.compute_deformation(
+                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
+
+            data = {
+                "sensor_id": sensor_id,
+                "fnormal": float(fnormal),
+                "fshearx": float(fshearx),
+                "fsheary": float(fsheary),
+                "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)
+                if sleep_time > 0.001:
+                    time.sleep(sleep_time)
+
+    except Exception as exc:
+        _put_latest(
+            result_queue,
+            {
+                "sensor_id": sensor_id,
+                "error": str(exc),
+                "timestamp": time.time(),
+            },
+        )
+    finally:
+        if sensor is not None:
+            try:
+                sensor.disconnect()
+            except Exception:
+                pass
+
+
+class TwoSensorForceReader:
+    def __init__(
+        self,
+        video1=0,
+        video2=1,
+        config1="./config/ddjx01.json",
+        config2="./config/ddjx01.json",
+        rotation1=None,
+        rotation2=None,
+        target_fps=30,
+        cuda=True,
+        isstitch=True,
+        backend="auto",
+        motion_threshold=0,
+        queue_size=5,
+        include_visuals=False,
+        visual_size=320,
+    ):
+        self.sensor_configs = [
+            {
+                "sensor_id": 0,
+                "vid_src": parse_video_source(video1),
+                "config_name": config1,
+                "rotation_config": normalize_rotation_config(rotation1, DEFAULT_ROTATION1),
+            },
+            {
+                "sensor_id": 1,
+                "vid_src": parse_video_source(video2),
+                "config_name": config2,
+                "rotation_config": normalize_rotation_config(rotation2, DEFAULT_ROTATION2),
+            },
+        ]
+        self.target_fps = target_fps
+        self.cuda = cuda
+        self.isstitch = isstitch
+        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 = []
+        self.processes = []
+        self.latest = [None, None]
+
+    def start(self):
+        if self.processes:
+            return
+
+        self.result_queues = [Queue(maxsize=self.queue_size) for _ in self.sensor_configs]
+        self.stop_events = [mp.Event() for _ in self.sensor_configs]
+        self.latest = [None] * len(self.sensor_configs)
+
+        for idx, config in enumerate(self.sensor_configs):
+            process = Process(
+                target=_sensor_force_worker,
+                args=(
+                    config["sensor_id"],
+                    config["vid_src"],
+                    config["config_name"],
+                    config["rotation_config"],
+                    self.result_queues[idx],
+                    self.stop_events[idx],
+                    self.target_fps,
+                    self.cuda,
+                    self.isstitch,
+                    self.backend,
+                    self.motion_threshold,
+                    self.include_visuals,
+                    self.visual_size,
+                ),
+            )
+            process.start()
+            self.processes.append(process)
+
+    def update(self):
+        for idx, result_queue in enumerate(self.result_queues):
+            data = None
+            while True:
+                try:
+                    data = result_queue.get_nowait()
+                except queue.Empty:
+                    break
+                except Exception:
+                    break
+
+            if data is not None:
+                if "error" in data:
+                    raise RuntimeError(
+                        f"sensor {data.get('sensor_id', idx)} failed: {data['error']}"
+                    )
+                self.latest[idx] = data
+
+        return self.latest
+
+    def get_samples(self, timeout=0.0):
+        deadline = time.perf_counter() + max(0.0, float(timeout))
+        while True:
+            self.update()
+            if all(sample is not None for sample in self.latest):
+                return self.latest[0], self.latest[1]
+            if time.perf_counter() >= deadline:
+                return self.latest[0], self.latest[1]
+            time.sleep(0.005)
+
+    def get_forces(self, timeout=0.0):
+        left, right = self.get_samples(timeout=timeout)
+        left_force = None if left is None else left["fnormal"]
+        right_force = None if right is None else right["fnormal"]
+        return left_force, right_force
+
+    def stop(self):
+        for event in self.stop_events:
+            event.set()
+
+        for process in self.processes:
+            process.join(timeout=2.0)
+            if process.is_alive():
+                process.terminate()
+                process.join(timeout=1.0)
+
+        self.processes = []
+        self.stop_events = []
+        self.result_queues = []
+
+    def __enter__(self):
+        self.start()
+        return self
+
+    def __exit__(self, exc_type, exc, traceback):
+        self.stop()
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--video1", "-v1", type=str, default="0")
+    parser.add_argument("--video2", "-v2", type=str, default="1")
+    parser.add_argument("--config1", type=str, default="./config/ddjx01.json")
+    parser.add_argument("--config2", type=str, default="./config/ddjx01.json")
+    parser.add_argument("--rotation1", "-r1", type=str, default=None)
+    parser.add_argument("--rotation2", "-r2", type=str, default=None)
+    parser.add_argument("--target-fps", type=float, default=30)
+    parser.add_argument("--motion-threshold", type=float, default=0)
+    parser.add_argument("--cpu", action="store_true")
+    args = parser.parse_args()
+
+    reader = TwoSensorForceReader(
+        video1=args.video1,
+        video2=args.video2,
+        config1=args.config1,
+        config2=args.config2,
+        rotation1=args.rotation1,
+        rotation2=args.rotation2,
+        target_fps=args.target_fps,
+        cuda=not args.cpu,
+        motion_threshold=args.motion_threshold,
+    )
+
+    print("Starting two-sensor force reader. Press Ctrl+C to stop.")
+    reader.start()
+    try:
+        while True:
+            left_sample, right_sample = reader.get_samples(timeout=1.0)
+            left_force, right_force = reader.get_forces()
+            left_fps = 0.0 if left_sample is None else left_sample["fps"]
+            right_fps = 0.0 if right_sample is None else right_sample["fps"]
+            left_contact = False if left_sample is None else left_sample["is_contact"]
+            right_contact = False if right_sample is None else right_sample["is_contact"]
+
+            print(
+                "left={:>10} right={:>10} fps=({:5.1f}, {:5.1f}) contact=({}, {})".format(
+                    "None" if left_force is None else f"{left_force:.4f}",
+                    "None" if right_force is None else f"{right_force:.4f}",
+                    left_fps,
+                    right_fps,
+                    int(left_contact),
+                    int(right_contact),
+                )
+            )
+            time.sleep(0.05)
+    except KeyboardInterrupt:
+        print("\nStopping.")
+    finally:
+        reader.stop()
+
+
+if __name__ == "__main__":
+    mp.freeze_support()
+    main()

gripper/hardware.py

@@ -0,0 +1,119 @@
+from .filters import clamp
+from .paths import ensure_project_paths
+
+
+class GripperClient:
+    def __init__(self, config):
+        self.config = config
+        self.motor = None
+
+    def connect(self):
+        if self.config.dry_run:
+            return self
+
+        ensure_project_paths()
+        from changingtek_p_rtu_Servo import MotorController
+
+        self.motor = MotorController(
+            self.config.port,
+            self.config.slave_id,
+            baudrate=self.config.baudrate,
+            timeout=self.config.serial_timeout,
+        )
+        return self
+
+    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={speed_pct}, force={force_pct}, "
+                f"accel={accel}, decel={decel}"
+            )
+            return
+        if self.motor is None:
+            raise RuntimeError(
+                "gripper motor is not connected; check serial port, conda environment, "
+                "and changingtek_p_rtu_Servo.py"
+            )
+
+        self.motor.temp_move(
+            position_mm=int(position),
+            speed_pct=int(speed_pct),
+            force_pct=int(force_pct),
+            accel=int(accel),
+            decel=int(decel),
+            trigger=True,
+        )
+
+    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)
+
+    def set_force(self, force_pct):
+        config = self.config
+        force_pct = int(clamp(force_pct, config.force_min, config.force_max))
+        if config.dry_run:
+            print(f"[dry-run] set_target_force {force_pct}")
+            return force_pct
+
+        self.motor.set_target_force(force_pct)
+        if config.trigger_force_update:
+            self.motor.trigger_motion()
+        return force_pct
+
+    def read_position(self):
+        if self.config.dry_run:
+            return None
+        try:
+            return self.motor.read_real_position()
+        except Exception as exc:
+            print(f"read_real_position failed: {exc}")
+            return None
+
+    def hold_current_position(self, force_pct):
+        current_pos = self.read_position()
+        if current_pos is None:
+            current_pos = self.config.hold_fallback_pos
+
+        if current_pos is None:
+            self.set_force(force_pct)
+            self._stop_by_speed()
+            return None
+
+        self.move(int(current_pos), force_pct, "clamp-hold")
+        return int(current_pos)
+
+    def final_open_if_needed(self):
+        if not self.config.open_at_end:
+            return
+        if not self.config.dry_run and self.motor is None:
+            return
+        final_force = int(clamp(
+            self.config.initial_force,
+            self.config.force_min,
+            self.config.force_max,
+        ))
+        self.open(final_force, "open-at-end")
+
+    def _stop_by_speed(self):
+        if self.config.dry_run:
+            return
+        try:
+            self.motor.set_target_speed(0)
+            self.motor.trigger_motion()
+        except Exception as exc:
+            print(f"stop by speed=0 failed: {exc}")

gripper/main.py

@@ -0,0 +1,62 @@
+import multiprocessing as mp
+
+from .calibration import ForceConverter
+from .feedback import TactileFeedbackProcessor
+from .force_reader import TwoSensorForceReader
+from .hardware import GripperClient
+from .paths import ensure_project_paths
+
+from .controller import SideTriggerGripController
+from .settings import Demo02Config
+
+
+def build_controller(
+    argv=None,
+    status_callback=None,
+    include_visuals=False,
+    visual_size=320,
+):
+    ensure_project_paths()
+    config = Demo02Config.from_args(argv)
+
+    normal_converter = ForceConverter.from_config(
+        config.normal_force_calibration,
+        "normal_force_calibration",
+    )
+    shear_converter = ForceConverter.from_config(
+        config.shear_force_calibration,
+        "shear_force_calibration",
+    )
+
+    reader = TwoSensorForceReader(
+        video1=config.video1,
+        video2=config.video2,
+        config1=config.sensor_config1,
+        config2=config.sensor_config2,
+        rotation1=config.rotation1,
+        rotation2=config.rotation2,
+        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,
+        status_callback=status_callback,
+    )
+
+
+def main(argv=None):
+    controller = build_controller(argv)
+    controller.run()
+
+
+if __name__ == "__main__":
+    mp.freeze_support()
+    main()

gripper/paths.py

@@ -0,0 +1,16 @@
+from pathlib import Path
+import sys
+
+
+PACKAGE_DIR = Path(__file__).resolve().parent
+REPO_ROOT = PACKAGE_DIR.parent
+SDK_DIR = REPO_ROOT / "sdk"
+SRC_DIR = REPO_ROOT / "src"
+
+
+def ensure_project_paths():
+    for path in (SDK_DIR, SRC_DIR, REPO_ROOT):
+        path_text = str(path)
+        if path_text not in sys.path:
+            sys.path.insert(0, path_text)
+

gripper/rotation.py

@@ -0,0 +1,38 @@
+import copy
+import json
+import os
+import tempfile
+
+
+DEFAULT_ROTATION1 = {
+    "rotation_angle": 90,
+    "flip_x": True,
+    "flip_y": False,
+}
+
+DEFAULT_ROTATION2 = {
+    "rotation_angle": 90,
+    "flip_x": False,
+    "flip_y": False,
+}
+
+
+def normalize_rotation_config(value, default_config):
+    if value is None:
+        return copy.deepcopy(default_config)
+    if isinstance(value, dict):
+        return copy.deepcopy(value)
+    if isinstance(value, str):
+        text = value.strip()
+        if not text or text.lower() == "default":
+            return copy.deepcopy(default_config)
+        if text.startswith("{"):
+            return json.loads(text)
+    return value
+
+
+def write_temp_rotation_config(config, label):
+    path = os.path.join(tempfile.gettempdir(), f"gripper_{label}_rotation.json")
+    with open(path, "w", encoding="utf-8") as f:
+        json.dump(config, f, ensure_ascii=False, indent=2)
+    return path

gripper/settings.py

@@ -0,0 +1,211 @@
+import argparse
+import copy
+from dataclasses import dataclass, field
+
+from .calibration import (
+    DEFAULT_NORMAL_FORCE_CALIBRATION,
+    DEFAULT_SHEAR_FORCE_CALIBRATION,
+)
+from .config_loader import load_control_config
+from .paths import REPO_ROOT
+from .rotation import (
+    DEFAULT_ROTATION1,
+    DEFAULT_ROTATION2,
+    normalize_rotation_config,
+)
+
+
+DEFAULT_CONTROL_CONFIG = str(
+    REPO_ROOT / "config" / "gripper.json"
+)
+
+
+def _config_default(config, key, default):
+    return config.get(key, config.get(key.replace("_", "-"), default))
+
+
+@dataclass
+class Demo02Config:
+    control_config: str = DEFAULT_CONTROL_CONFIG
+
+    video1: str = "0"
+    video2: str = "1"
+    rotation1: dict | str = field(default_factory=lambda: copy.deepcopy(DEFAULT_ROTATION1))
+    rotation2: dict | str = field(default_factory=lambda: copy.deepcopy(DEFAULT_ROTATION2))
+    sensor_config1: str = "./config/ddjx01.json"
+    sensor_config2: str = "./config/ddjx01.json"
+    sensor_fps: float = 30.0
+    motion_threshold: float = 0.0
+    sensor_cpu: bool = False
+
+    port: str = "COM5"
+    slave_id: int = 1
+    baudrate: int = 115200
+    serial_timeout: float = 0.2
+
+    open_pos: int = 0
+    close_pos: int = 9000
+    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
+    grip_start_force: int = 10
+    hold_force: int = 15
+    initial_force: int = 10
+    force_min: int = 10
+    force_max: int = 60
+
+    control_hz: float = 30.0
+    baseline_seconds: float = 1.0
+    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
+    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
+    closing_command_interval: float = 0.2
+    force_ramp_step: int = 1
+    force_ramp_interval: float = 0.1
+    grip_settle_seconds: float = 0.1
+    adaptive_grip_enabled: bool = True
+    adaptive_shear_start_force: float = 0.10
+    adaptive_shear_full_force: float = 0.60
+    adaptive_force_min: int = 15
+    adaptive_force_max: int = 45
+    adaptive_force_step: int = 1
+    adaptive_force_interval: float = 0.15
+    release_arm_delay_seconds: float = 0.8
+    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
+    open_at_end: bool = True
+    dry_run: bool = False
+
+    normal_force_calibration: dict = field(
+        default_factory=lambda: copy.deepcopy(DEFAULT_NORMAL_FORCE_CALIBRATION)
+    )
+    shear_force_calibration: dict = field(
+        default_factory=lambda: copy.deepcopy(DEFAULT_SHEAR_FORCE_CALIBRATION)
+    )
+
+    @classmethod
+    def from_args(cls, argv=None):
+        bootstrap = argparse.ArgumentParser(add_help=False)
+        bootstrap.add_argument("--control-config", default=DEFAULT_CONTROL_CONFIG)
+        bootstrap_args, _ = bootstrap.parse_known_args(argv)
+        config_data = load_control_config(bootstrap_args.control_config)
+
+        parser = argparse.ArgumentParser()
+        add = parser.add_argument
+        get = lambda key, default: _config_default(config_data, key, default)
+
+        add("--control-config", type=str, default=bootstrap_args.control_config)
+        add("--video1", "-v1", type=str, default=get("video1", cls.video1))
+        add("--video2", "-v2", type=str, default=get("video2", cls.video2))
+        add("--rotation1", "-r1", type=str, default=get("rotation1", copy.deepcopy(DEFAULT_ROTATION1)))
+        add("--rotation2", "-r2", type=str, default=get("rotation2", copy.deepcopy(DEFAULT_ROTATION2)))
+        add("--sensor-config1", type=str, default=get("sensor_config1", cls.sensor_config1))
+        add("--sensor-config2", type=str, default=get("sensor_config2", cls.sensor_config2))
+        add("--sensor-fps", type=float, default=get("sensor_fps", cls.sensor_fps))
+        add("--motion-threshold", type=float, default=get("motion_threshold", cls.motion_threshold))
+        add("--sensor-cpu", action="store_true", default=get("sensor_cpu", cls.sensor_cpu))
+
+        add("--port", type=str, default=get("port", cls.port))
+        add("--slave-id", type=int, default=get("slave_id", cls.slave_id))
+        add("--baudrate", type=int, default=get("baudrate", cls.baudrate))
+        add("--serial-timeout", type=float, default=get("serial_timeout", cls.serial_timeout))
+
+        add("--open-pos", type=int, default=get("open_pos", cls.open_pos))
+        add("--close-pos", type=int, default=get("close_pos", cls.close_pos))
+        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))
+        add("--grip-start-force", type=int, default=get("grip_start_force", cls.grip_start_force))
+        add("--hold-force", type=int, default=get("hold_force", cls.hold_force))
+        add("--force-min", type=int, default=get("force_min", cls.force_min))
+        add("--force-max", type=int, default=get("force_max", cls.force_max))
+
+        add("--control-hz", type=float, default=get("control_hz", cls.control_hz))
+        add("--baseline-seconds", type=float, default=get("baseline_seconds", cls.baseline_seconds))
+        add("--filter-alpha", type=float, default=get("filter_alpha", cls.filter_alpha))
+        add("--shear-filter-alpha", type=float, default=get("shear_filter_alpha", cls.shear_filter_alpha))
+
+        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")
+        add("--close-timeout-seconds", type=float, default=get("close_timeout_seconds", cls.close_timeout_seconds))
+        add("--closing-command-interval", type=float, default=get("closing_command_interval", cls.closing_command_interval))
+        add("--force-ramp-step", type=int, default=get("force_ramp_step", cls.force_ramp_step))
+        add("--force-ramp-interval", type=float, default=get("force_ramp_interval", cls.force_ramp_interval))
+        add("--grip-settle-seconds", type=float, default=get("grip_settle_seconds", cls.grip_settle_seconds))
+        add("--enable-adaptive-grip", dest="adaptive_grip_enabled", action="store_true", default=get("adaptive_grip_enabled", cls.adaptive_grip_enabled))
+        add("--disable-adaptive-grip", dest="adaptive_grip_enabled", action="store_false")
+        add("--adaptive-shear-start-force", type=float, default=get("adaptive_shear_start_force", cls.adaptive_shear_start_force))
+        add("--adaptive-shear-full-force", type=float, default=get("adaptive_shear_full_force", cls.adaptive_shear_full_force))
+        add("--adaptive-force-min", type=int, default=get("adaptive_force_min", cls.adaptive_force_min))
+        add("--adaptive-force-max", type=int, default=get("adaptive_force_max", cls.adaptive_force_max))
+        add("--adaptive-force-step", type=int, default=get("adaptive_force_step", cls.adaptive_force_step))
+        add("--adaptive-force-interval", type=float, default=get("adaptive_force_interval", cls.adaptive_force_interval))
+        add("--release-arm-delay-seconds", type=float, default=get("release_arm_delay_seconds", cls.release_arm_delay_seconds))
+        add("--release-normal-change", type=float, default=get("release_normal_change", cls.release_normal_change))
+        add("--release-shear-change", type=float, default=get("release_shear_change", cls.release_shear_change))
+        add("--rearm-seconds", type=float, default=get("rearm_seconds", cls.rearm_seconds))
+        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))
+        add("--open-at-end", action="store_true", default=get("open_at_end", cls.open_at_end))
+        add("--dry-run", action="store_true", default=get("dry_run", cls.dry_run))
+
+        args = parser.parse_args(argv)
+        values = vars(args)
+        values["initial_force"] = values["open_force"]
+        values["rotation1"] = normalize_rotation_config(values["rotation1"], DEFAULT_ROTATION1)
+        values["rotation2"] = normalize_rotation_config(values["rotation2"], DEFAULT_ROTATION2)
+        values["normal_force_calibration"] = copy.deepcopy(
+            config_data.get("normal_force_calibration", DEFAULT_NORMAL_FORCE_CALIBRATION)
+        )
+        values["shear_force_calibration"] = copy.deepcopy(
+            config_data.get("shear_force_calibration", DEFAULT_SHEAR_FORCE_CALIBRATION)
+        )
+        return cls(**values)

gripper/visualizer.py

@@ -0,0 +1,877 @@
+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 QEasingCurve, QPropertyAnimation, Qt, QTimer
+    from PyQt5.QtGui import QFont, QImage, QPixmap
+    from PyQt5.QtWidgets import (
+        QApplication,
+        QFrame,
+        QGraphicsOpacityEffect,
+        QGridLayout,
+        QHBoxLayout,
+        QLabel,
+        QMainWindow,
+        QPushButton,
+        QSizePolicy,
+        QSplitter,
+        QVBoxLayout,
+        QWidget,
+    )
+except Exception as exc:
+    _QT_IMPORT_ERROR = exc
+    Qt = QTimer = QPropertyAnimation = QEasingCurve = QFont = QImage = QPixmap = QApplication = None
+    QGridLayout = QHBoxLayout = QLabel = QPushButton = QSizePolicy = QSplitter = None
+    QVBoxLayout = QWidget = None
+    QFrame = QGraphicsOpacityEffect = 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
+PLOT_FORCE_Y_MIN_N = 0.0
+PLOT_FORCE_Y_MAX_N = 3.0
+PLOT_SHEAR_Y_MIN_N = -3.0
+PLOT_SHEAR_Y_MAX_N = 3.0
+FORCE_DISPLAY_ZERO_THRESHOLD_N = 0.05
+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 apply_visual_transform(img, transform):
+    if img is None:
+        return None
+    angle = int(transform.get("rotation_angle", 0)) % 360
+    if angle == 90:
+        img = cv2.rotate(img, cv2.ROTATE_90_CLOCKWISE)
+    elif angle == 180:
+        img = cv2.rotate(img, cv2.ROTATE_180)
+    elif angle == 270:
+        img = cv2.rotate(img, cv2.ROTATE_90_COUNTERCLOCKWISE)
+
+    flip_x = bool(transform.get("flip_x"))
+    flip_y = bool(transform.get("flip_y"))
+    if flip_x and flip_y:
+        img = cv2.flip(img, -1)
+    elif flip_x:
+        img = cv2.flip(img, 1)
+    elif flip_y:
+        img = cv2.flip(img, 0)
+    return img
+
+
+def force_display_value(value):
+    value = float(value)
+    if abs(value) < FORCE_DISPLAY_ZERO_THRESHOLD_N:
+        return 0.0
+    return value
+
+
+def fmt(value, unit="", width=0, precision=3):
+    if value is None:
+        text = "--"
+    else:
+        value = force_display_value(value)
+        text = "0" if value == 0.0 else f"{value:.{precision}f}"
+    if width:
+        text = text.rjust(width)
+    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-continue": "继续闭合",
+    "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;
+}
+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;
+}
+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;
+    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: #171717;
+    color: #d8d8d8;
+    border: 1px solid #363636;
+    border-radius: 4px;
+    padding: 7px 16px;
+    font-size: 13px;
+}
+QPushButton:hover {
+    background-color: #242424;
+    color: #ffffff;
+    border: 1px solid #5a5a5a;
+}
+QPushButton:pressed {
+    background-color: #0f0f0f;
+    border: 1px solid #8a8a8a;
+    padding-top: 8px;
+    padding-bottom: 6px;
+}
+QPushButton:disabled {
+    background-color: #101010;
+    color: #606060;
+    border: 1px solid #242424;
+}
+QPushButton#StartButton {
+    background-color: #171717;
+    color: #d8d8d8;
+    border: 1px solid #363636;
+}
+QPushButton#StopButton {
+    background-color: #171717;
+    color: #d8d8d8;
+    border: 1px solid #363636;
+}
+QPushButton#RestartButton {
+    background-color: #171717;
+    color: #d8d8d8;
+    border: 1px solid #363636;
+}
+QPushButton#ManualButton {
+    background-color: #171717;
+    color: #d8d8d8;
+    border: 1px solid #363636;
+}
+QPushButton#HoldButton {
+    background-color: #171717;
+    color: #d8d8d8;
+    border: 1px solid #363636;
+}
+QSplitter::handle {
+    background-color: #151515;
+}
+QSplitter::handle:hover {
+    background-color: #2a2a2a;
+}
+QSplitter::handle:horizontal {
+    width: 8px;
+}
+QSplitter::handle:vertical {
+    height: 8px;
+}
+"""
+
+
+if QPushButton is not None:
+    class AnimatedButton(QPushButton):
+        def __init__(self, text=""):
+            super().__init__(text)
+            self._opacity = QGraphicsOpacityEffect(self)
+            self._opacity.setOpacity(1.0)
+            self.setGraphicsEffect(self._opacity)
+            self._animation = QPropertyAnimation(self._opacity, b"opacity", self)
+            self._animation.setEasingCurve(QEasingCurve.OutCubic)
+
+        def _animate_opacity(self, target, duration):
+            self._animation.stop()
+            self._animation.setDuration(duration)
+            self._animation.setStartValue(self._opacity.opacity())
+            self._animation.setEndValue(target)
+            self._animation.start()
+
+        def mousePressEvent(self, event):
+            if self.isEnabled():
+                self._animate_opacity(0.72, 70)
+            super().mousePressEvent(event)
+
+        def mouseReleaseEvent(self, event):
+            if self.isEnabled():
+                self._animate_opacity(1.0, 140)
+            super().mouseReleaseEvent(event)
+else:
+    AnimatedButton = None
+
+
+class MetricBox(QFrame):
+    def __init__(self, title, value="--"):
+        super().__init__()
+        self.setObjectName("Panel")
+        layout = QVBoxLayout(self)
+        layout.setContentsMargins(10, 8, 10, 8)
+        layout.setSpacing(4)
+        self.title_label = QLabel(title)
+        self.title_label.setObjectName("MetricLabel")
+        self.value_label = QLabel(value)
+        self.value_label.setObjectName("MetricValue")
+        layout.addWidget(self.title_label)
+        layout.addWidget(self.value_label)
+
+    def set_value(self, value, sub=None):
+        self.value_label.setText(value)
+
+
+class ImageBox(QFrame):
+    def __init__(self, title, on_rotate=None, on_flip_x=None, on_flip_y=None, on_reset=None):
+        super().__init__()
+        self.title = title
+        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)
+        controls = QHBoxLayout()
+        controls.setSpacing(6)
+        self.rotate_button = AnimatedButton("旋转")
+        self.flip_x_button = AnimatedButton("水平")
+        self.flip_y_button = AnimatedButton("垂直")
+        self.reset_button = AnimatedButton("重置")
+        for button, callback in (
+            (self.rotate_button, on_rotate),
+            (self.flip_x_button, on_flip_x),
+            (self.flip_y_button, on_flip_y),
+            (self.reset_button, on_reset),
+        ):
+            button.setObjectName("ManualButton")
+            if callback is not None:
+                button.clicked.connect(callback)
+            controls.addWidget(button)
+        layout.addLayout(controls)
+
+    def set_image(self, img):
+        self.image_label.setPixmap(cv2_to_pixmap(img))
+
+    def set_transform_label(self, transform):
+        angle = int(transform.get("rotation_angle", 0)) % 360
+        flip_x = "X1" if transform.get("flip_x") else "X0"
+        flip_y = "Y1" if transform.get("flip_y") else "Y0"
+        self.title_label.setText(f"{self.title}  {angle}deg {flip_x} {flip_y}")
+
+
+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.restart_pending = False
+        self.button_state = "idle"
+        self.start_time = time.time()
+        self.force_history = []
+        self.argv = argv
+        self.visual_transforms = {
+            "left": {"rotation_angle": 0, "flip_x": False, "flip_y": False},
+            "right": {"rotation_angle": 0, "flip_x": False, "flip_y": False},
+        }
+
+        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("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("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)
+        self.stop_button = AnimatedButton("停止")
+        self.stop_button.setObjectName("StopButton")
+        self.stop_button.clicked.connect(self.stop_control)
+        self.stop_button.setEnabled(False)
+        self.restart_button = AnimatedButton("重启")
+        self.restart_button.setObjectName("RestartButton")
+        self.restart_button.clicked.connect(self.restart_control)
+        self.manual_open_button = AnimatedButton("夹爪张开")
+        self.manual_open_button.setObjectName("ManualButton")
+        self.manual_open_button.clicked.connect(lambda: self.send_manual_command("open"))
+        self.manual_close_button = AnimatedButton("夹爪闭合")
+        self.manual_close_button.setObjectName("ManualButton")
+        self.manual_close_button.clicked.connect(lambda: self.send_manual_command("close"))
+        self.manual_hold_button = AnimatedButton("保持")
+        self.manual_hold_button.setObjectName("HoldButton")
+        self.manual_hold_button.clicked.connect(self.toggle_manual_hold)
+        self._set_button_state("idle")
+        top.addWidget(title)
+        top.addWidget(self.status_panel)
+        top.addStretch(1)
+        top.addWidget(self.start_button)
+        top.addWidget(self.stop_button)
+        top.addWidget(self.restart_button)
+        top.addWidget(self.manual_open_button)
+        top.addWidget(self.manual_close_button)
+        top.addWidget(self.manual_hold_button)
+        outer.addLayout(top)
+
+        main_splitter = QSplitter(Qt.Horizontal)
+        main_splitter.setChildrenCollapsible(False)
+        outer.addWidget(main_splitter, 1)
+
+        flow_splitter = QSplitter(Qt.Vertical)
+        flow_splitter.setChildrenCollapsible(False)
+        self.left_flow = ImageBox(
+            "左侧光流",
+            on_rotate=lambda: self.adjust_visual_transform("left", "rotate"),
+            on_flip_x=lambda: self.adjust_visual_transform("left", "flip_x"),
+            on_flip_y=lambda: self.adjust_visual_transform("left", "flip_y"),
+            on_reset=lambda: self.adjust_visual_transform("left", "reset"),
+        )
+        self.right_flow = ImageBox(
+            "右侧光流",
+            on_rotate=lambda: self.adjust_visual_transform("right", "rotate"),
+            on_flip_x=lambda: self.adjust_visual_transform("right", "flip_x"),
+            on_flip_y=lambda: self.adjust_visual_transform("right", "flip_y"),
+            on_reset=lambda: self.adjust_visual_transform("right", "reset"),
+        )
+        self.left_flow.set_transform_label(self.visual_transforms["left"])
+        self.right_flow.set_transform_label(self.visual_transforms["right"])
+        flow_splitter.addWidget(self.left_flow)
+        flow_splitter.addWidget(self.right_flow)
+        flow_splitter.setStretchFactor(0, 1)
+        flow_splitter.setStretchFactor(1, 1)
+        flow_splitter.setSizes([360, 360])
+        main_splitter.addWidget(flow_splitter)
+
+        side_widget = QWidget()
+        side = QVBoxLayout(side_widget)
+        side.setContentsMargins(0, 0, 0, 0)
+        side.setSpacing(8)
+        main_splitter.addWidget(side_widget)
+        main_splitter.setStretchFactor(0, 2)
+        main_splitter.setStretchFactor(1, 4)
+        main_splitter.setSizes([520, 980])
+
+        metrics_grid = QGridLayout()
+        metrics_grid.setSpacing(8)
+        side.addLayout(metrics_grid)
+        self.left_force = MetricBox("左法向力", "--")
+        self.right_force = MetricBox("右法向力", "--")
+        self.left_shear_box = MetricBox("左切向力", "--")
+        self.right_shear_box = MetricBox("右切向力", "--")
+        self.force_pct_box = MetricBox("夹爪当前力比例（%）", "--")
+        self.target_force_box = MetricBox("夹爪目标力比例（%）", "--")
+        self.speed_box = MetricBox("速度", "--")
+        self.trigger_box = MetricBox("是否触发/系统是否允许触发", "--")
+        self.contact_box = MetricBox("接触", "--")
+        self.release_box = MetricBox("释放检测", "--")
+        boxes = [
+            self.left_force,
+            self.right_force,
+            self.left_shear_box,
+            self.right_shear_box,
+            self.force_pct_box,
+            self.target_force_box,
+            self.speed_box,
+            self.trigger_box,
+            self.contact_box,
+            self.release_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)
+
+    def _configure_plot(self):
+        self.plots = []
+        self.curves = {}
+        plot_specs = [
+            ("left_normal", "左法向力", "#00E5FF", PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N),
+            ("right_normal", "右法向力", "#ffb000", PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N),
+            ("left_shear_x", "左切向 X", "#61d394", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N),
+            ("left_shear_y", "左切向 Y", "#f07178", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N),
+            ("right_shear_x", "右切向 X", "#8ab4f8", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N),
+            ("right_shear_y", "右切向 Y", "#d19a66", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N),
+        ]
+        for idx, (key, title, color, y_min, y_max) in enumerate(plot_specs):
+            row = idx // 2
+            col = idx % 2
+            plot = self.plot_widget.addPlot(row=row, col=col)
+            plot.setTitle(title, color="#999999", size="11pt")
+            plot.titleLabel.setFont(QFont("SimHei", 11))
+            plot.showAxis("top", False)
+            plot.showAxis("right", False)
+            plot.showGrid(x=False, y=True, alpha=0.12)
+            plot.setLabel("bottom", "时间", units="s")
+            plot.setLabel("left", "力", units="N")
+            plot.getAxis("bottom").setTickSpacing(major=1.0, minor=1.0)
+            plot.setYRange(y_min, y_max, padding=0)
+            plot.setLimits(yMin=y_min, yMax=y_max)
+            plot.enableAutoRange(axis=pg.ViewBox.YAxis, enable=False)
+            self.plots.append(plot)
+            self.curves[key] = plot.plot(pen=pg.mkPen(color, width=2))
+
+    def _set_button_state(self, state):
+        labels = {
+            "idle": ("启动", "停止", "重启"),
+            "starting": ("启动中", "停止", "重启"),
+            "running": ("启动", "停止", "重启"),
+            "stopping": ("启动", "停止中", "重启"),
+            "restarting": ("启动", "停止", "重启中"),
+        }
+        enabled = {
+            "idle": (True, False, True),
+            "starting": (False, True, False),
+            "running": (False, True, True),
+            "stopping": (False, False, False),
+            "restarting": (False, False, False),
+        }
+        self.button_state = state
+        start_text, stop_text, restart_text = labels[state]
+        start_enabled, stop_enabled, restart_enabled = enabled[state]
+        self.start_button.setText(start_text)
+        self.stop_button.setText(stop_text)
+        self.restart_button.setText(restart_text)
+        self.start_button.setEnabled(start_enabled)
+        self.stop_button.setEnabled(stop_enabled)
+        self.restart_button.setEnabled(restart_enabled)
+        manual_enabled = state == "running"
+        self.manual_open_button.setEnabled(manual_enabled)
+        self.manual_close_button.setEnabled(manual_enabled)
+        self.manual_hold_button.setEnabled(manual_enabled)
+
+    def adjust_visual_transform(self, side, action):
+        transform = self.visual_transforms[side]
+        if action == "rotate":
+            transform["rotation_angle"] = (int(transform["rotation_angle"]) + 90) % 360
+        elif action == "flip_x":
+            transform["flip_x"] = not bool(transform["flip_x"])
+        elif action == "flip_y":
+            transform["flip_y"] = not bool(transform["flip_y"])
+        elif action == "reset":
+            transform.update({"rotation_angle": 0, "flip_x": False, "flip_y": False})
+
+        if side == "left":
+            self.left_flow.set_transform_label(transform)
+        else:
+            self.right_flow.set_transform_label(transform)
+
+    def send_manual_command(self, command):
+        if self.controller is None or self.control_thread is None or not self.control_thread.is_alive():
+            return
+        try:
+            self.controller.request_manual_command(command)
+        except Exception as exc:
+            print(f"manual command failed: {exc}")
+
+    def toggle_manual_hold(self):
+        command = "release" if self.manual_hold_button.text() == "释放" else "hold"
+        self.send_manual_command(command)
+
+    def start_control(self, from_restart=False):
+        if self.control_thread is not None and self.control_thread.is_alive():
+            return
+        self.force_history.clear()
+        self.start_time = time.time()
+        self._set_button_state("restarting" if from_restart else "starting")
+        self.controller_finished = False
+
+        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._set_button_state("stopping")
+
+    def restart_control(self):
+        if self.control_thread is not None and self.control_thread.is_alive():
+            self.restart_pending = True
+            self._set_button_state("restarting")
+            if self.controller is not None:
+                self.controller.stop()
+            return
+        self.restart_pending = False
+        self.start_control(from_restart=True)
+
+    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()
+        if (
+            self.control_thread is not None
+            and self.control_thread.is_alive()
+            and not self.restart_pending
+            and self.button_state in ("starting", "restarting")
+        ):
+            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={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 "保持"
+        )
+
+        self.left_force.set_value(
+            fmt(status.get("left_normal"), unit),
+        )
+        self.right_force.set_value(
+            fmt(status.get("right_normal"), unit),
+        )
+        self.left_shear_box.set_value(
+            fmt(status.get("left_shear"), shear_unit),
+        )
+        self.right_shear_box.set_value(
+            fmt(status.get("right_shear"), shear_unit),
+        )
+        self.force_pct_box.set_value(
+            str(status.get("force_pct", "--")),
+        )
+        self.target_force_box.set_value(
+            str(status.get("adaptive_target_force", "--")),
+        )
+        self.speed_box.set_value(
+            str(status.get("speed_pct", "--")),
+        )
+        self.trigger_box.set_value(
+            f"{int(bool(status.get('trigger')))} / {int(bool(status.get('trigger_armed')))}",
+        )
+        self.contact_box.set_value(
+            str(int(bool(status.get("grip_contact")))),
+        )
+        self.release_box.set_value(
+            str(int(bool(status.get("release_armed")))),
+        )
+
+        left_sample = status.get("left_sample") or {}
+        right_sample = status.get("right_sample") or {}
+        self.left_flow.set_image(
+            apply_visual_transform(left_sample.get("flow_view"), self.visual_transforms["left"])
+        )
+        self.right_flow.set_image(
+            apply_visual_transform(right_sample.get("flow_view"), self.visual_transforms["right"])
+        )
+
+        elapsed = time.time() - self.start_time
+        self.force_history.append(
+            (
+                elapsed,
+                force_display_value(status.get("left_normal", 0.0) or 0.0),
+                force_display_value(status.get("right_normal", 0.0) or 0.0),
+                force_display_value(status.get("left_shear_x", 0.0) or 0.0),
+                force_display_value(status.get("left_shear_y", 0.0) or 0.0),
+                force_display_value(status.get("right_shear_x", 0.0) or 0.0),
+                force_display_value(status.get("right_shear_y", 0.0) or 0.0),
+            )
+        )
+        if len(self.force_history) > PLOT_POINTS:
+            self.force_history = self.force_history[-PLOT_POINTS:]
+        self._update_plot()
+
+    def _update_plot(self):
+        if not self.force_history:
+            return
+        data = np.asarray(self.force_history, dtype=float)
+        x = data[:, 0]
+        keys = [
+            "left_normal",
+            "right_normal",
+            "left_shear_x",
+            "left_shear_y",
+            "right_shear_x",
+            "right_shear_y",
+        ]
+        for idx, key in enumerate(keys, start=1):
+            self.curves[key].setData(x, data[:, idx])
+        if len(x) >= 2:
+            x_max = max(6.0, float(np.ceil(x[-1])))
+            x_min = max(0.0, x_max - 6.0)
+            for plot in self.plots:
+                plot.setXRange(x_min, x_max, padding=0)
+
+    def _update_thread_buttons(self):
+        if not self.controller_finished:
+            return
+        self.controller_finished = False
+        if self.control_thread is not None:
+            self.control_thread.join(timeout=0.1)
+            self.control_thread = None
+        if self.restart_pending:
+            self.restart_pending = False
+            self.start_control(from_restart=True)
+            return
+        self._set_button_state("idle")
+
+    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.restart_pending = False
+        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:]))

sdk/changingtek_p_rtu_Servo.py

@@ -0,0 +1,284 @@
+"""
+CTAG2F90D（Servo）控制器SDK (基于Modbus RTU的RS-485通信)
+---------------------------------------------
+一个轻量级的Python SDK，用于通过minimalmodbus库控制电机，
+具有线程安全、清晰的寄存器访问方式和实时数据监控功能。
+
+依赖安装:
+    pip install minimalmodbus pyserial
+
+作者: 知行机器人
+"""
+
+import time
+import threading
+import minimalmodbus
+import serial
+
+# -----------------------------
+# 寄存器映射 (保持寄存器，功能码0x03/0x06)
+# -----------------------------
+# 写入寄存器 (目标值/控制)
+REG_TARGET_POS_HIGH       = 0x0102  # 目标位置（高16位）
+REG_TARGET_POS_LOW        = 0x0103  # 目标位置（低16位）
+REG_TARGET_SPEED          = 0x0104  # 目标速度
+REG_TARGET_FORCE          = 0x0105  # 目标力/力矩
+REG_TARGET_ACCELERATION   = 0x0106  # 目标加速度
+REG_TARGET_DECELERATION   = 0x0107  # 目标减速度
+REG_MOTION_TRIGGER        = 0x0108  # 运动触发（0: 空闲, 1: 触发）
+
+# 读取寄存器 (反馈/状态)
+REG_REAL_POS_HIGH         = 0x0418  # 实时位置（高16位）
+REG_REAL_POS_LOW          = 0x0419  # 实时位置（低16位）
+REG_REAL_SPEED            = 0x041A  # 实时速度反馈
+REG_REAL_CURRENT          = 0x041B  # 实时电流反馈
+
+
+class MotorController:
+    def __init__(self, port: str, slave_id: int = 1, baudrate: int = 115200, timeout: float = 1.0):
+        """初始化与电机控制器的Modbus RTU通信
+
+        参数:
+            port: 串口端口（例如，Windows上为'COM3'，Linux上为'/dev/ttyUSB0'）
+            slave_id: Modbus从机地址（默认: 1）
+            baudrate: 串口波特率（默认: 115200）
+            timeout: 读写超时时间（秒，默认: 1.0）
+        """
+        self.instrument = minimalmodbus.Instrument(port, slave_id)
+        self.instrument.serial.baudrate = baudrate
+        self.instrument.serial.bytesize = 8
+        self.instrument.serial.parity = serial.PARITY_NONE
+        self.instrument.serial.stopbits = 1
+        self.instrument.serial.timeout = timeout
+        self.instrument.mode = minimalmodbus.MODE_RTU
+        self._lock = threading.Lock()  # 线程锁，确保串口操作线程安全
+        self._monitor_running = False  # 监控线程运行标志
+        self._monitor_thread = None    # 监控线程对象
+
+    # -----------------------------
+    # 低级寄存器操作工具方法
+    # -----------------------------
+    def _write_register(self, addr: int, value: int) -> None:
+        """写入单个16位保持寄存器（Modbus功能码0x06）
+
+        参数:
+            addr: 寄存器地址
+            value: 要写入的值
+        """
+        with self._lock:
+            self.instrument.write_register(addr, value, functioncode=6)
+
+    def _read_register(self, addr: int) -> int:
+        """读取单个16位保持寄存器（Modbus功能码0x03）
+
+        参数:
+            addr: 寄存器地址
+
+        返回:
+            寄存器的值
+        """
+        with self._lock:
+            return self.instrument.read_register(addr, functioncode=3)
+
+    def _write_registers(self, addr: int, values: list) -> None:
+        """写入多个16位保持寄存器（Modbus功能码0x10）
+
+        参数:
+            addr: 起始寄存器地址
+            values: 要写入的值列表
+        """
+        with self._lock:
+            self.instrument.write_registers(addr, values)
+
+    def _read_registers(self, addr: int, count: int) -> list:
+        """读取多个16位保持寄存器（Modbus功能码0x03）
+
+        参数:
+            addr: 起始寄存器地址
+            count: 要读取的寄存器数量
+
+        返回:
+            寄存器值的列表
+        """
+        with self._lock:
+            return self.instrument.read_registers(addr, count, functioncode=3)
+
+    # -----------------------------
+    # 目标值/控制写入操作
+    # -----------------------------
+    def set_target_position(self, position: int) -> None:
+        """设置目标位置（组合高低16位寄存器）
+
+        参数:
+            position: 目标位置值
+        """
+        hi = (position >> 16) & 0xFFFF  # 提取高16位
+        lo = position & 0xFFFF          # 提取低16位
+        self._write_registers(REG_TARGET_POS_HIGH, [hi, lo])
+
+    def set_target_speed(self, speed: int) -> None:
+        """设置目标速度
+
+        参数:
+            speed: 目标速度值
+        """
+        self._write_register(REG_TARGET_SPEED, speed)
+
+    def set_target_force(self, force: int) -> None:
+        """设置目标力/力矩
+
+        参数:
+            force: 目标力/力矩值
+        """
+        self._write_register(REG_TARGET_FORCE, force)
+
+    def set_target_acceleration(self, acceleration: int) -> None:
+        """设置目标加速度
+
+        参数:
+            acceleration: 目标加速度值
+        """
+        self._write_register(REG_TARGET_ACCELERATION, acceleration)
+
+    def set_target_deceleration(self, deceleration: int) -> None:
+        """设置目标减速度
+
+        参数:
+            deceleration: 目标减速度值
+        """
+        self._write_register(REG_TARGET_DECELERATION, deceleration)
+
+    def trigger_motion(self) -> None:
+        """使用已配置的目标参数触发运动"""
+        self._write_register(REG_MOTION_TRIGGER, 1)
+
+    def temp_move(self, position_mm: int, speed_pct: int, force_pct: int,
+                  accel: int, decel: int, trigger: bool = True) -> None:
+        """临时运动控制方法，一次性设置所有运动参数并可选触发运动
+
+        参数:
+            position_mm: 目标位置(毫米)
+            speed_pct: 速度百分比
+            force_pct: 力/力矩百分比
+            accel: 加速度值
+            decel: 减速度值
+            trigger: 是否立即触发运动，默认为True
+        """
+        self.set_target_position(position_mm)
+        self.set_target_speed(speed_pct)
+        self.set_target_force(force_pct)
+        self.set_target_acceleration(accel)
+        self.set_target_deceleration(decel)
+
+        if trigger:
+            self.trigger_motion()
+            print(f"已触发运动到位置 {position_mm}，速度 {speed_pct}%，力 {force_pct}%")
+
+    # -----------------------------
+    # 反馈/状态读取操作
+    # -----------------------------
+    def read_real_position(self) -> int:
+        """读取实时位置（组合高低16位寄存器）
+
+        返回:
+            组合后的32位实时位置（处理有符号值）
+        """
+        regs = self._read_registers(REG_REAL_POS_HIGH, 2)
+        hi, lo = regs[0], regs[1]
+        combined = (hi << 16) | lo
+        # 处理32位有符号整数
+        if combined & 0x80000000:  # 最高位为1表示负数
+            combined = combined - 0x100000000
+        return combined
+
+    def read_real_speed(self) -> int:
+        """读取实时速度反馈
+
+        返回:
+            实时速度值
+        """
+        return self._read_register(REG_REAL_SPEED)
+
+    def read_real_current(self) -> int:
+        """读取实时电流反馈
+
+        返回:
+            实时电流值
+        """
+        return self._read_register(REG_REAL_CURRENT)
+
+    # -----------------------------
+    # 实时监控线程
+    # -----------------------------
+    def _monitor_loop(self, interval: float = 0.5) -> None:
+        """实时数据监控的后台循环"""
+        while self._monitor_running:
+            try:
+                pos = self.read_real_position()
+                speed = self.read_real_speed()
+                current = self.read_real_current()
+                print(f"[实时数据] 位置: {pos:6d}, 速度: {speed:3d}, 电流: {current:3d}")
+            except Exception as e:
+                print(f"监控错误: {e}")
+            time.sleep(interval)
+
+    def start_monitoring(self, interval: float = 0.01) -> None:
+        """启动实时数据监控线程
+
+        参数:
+            interval: 监控间隔（秒，默认: 0.01）
+        """
+        if not self._monitor_running:
+            self._monitor_running = True
+            self._monitor_thread = threading.Thread(
+                target=self._monitor_loop,
+                args=(interval,)
+            )
+            self._monitor_thread.daemon = True  # 守护线程，主程序退出时自动结束
+            self._monitor_thread.start()
+            print("实时监控已启动。")
+
+    def stop_monitoring(self) -> None:
+        """停止实时数据监控线程"""
+        self._monitor_running = False
+        if self._monitor_thread:
+            self._monitor_thread.join(timeout=1.0)  # 等待线程结束
+        print("实时监控已停止。")
+
+
+if __name__ == "__main__":
+    # 示例用法（根据实际设备调整端口/从机ID）
+    PORT = "COM5"
+    SLAVE_ID = 1
+    sdk = MotorController(PORT, SLAVE_ID, baudrate=115200, timeout=1.0)
+
+    try:
+        # 启动实时监控
+        sdk.start_monitoring(interval=0.5)
+        
+        # 循环控制：5次往复运动
+        # 假设 9000 为闭合位置，0 为张开位置
+        # 最后一次循环结束后，确保处于张开位置 (0)
+        for i in range(5):
+            print(f"\n--- 第 {i+1}/5 次循环 ---")
+            
+            # 阶段1: 运动到位置 9000 (闭合?)
+            sdk.temp_move(position_mm=9000, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True)
+            time.sleep(5)  # 等待运动完成
+            
+            # 阶段2: 运动到位置 0 (张开?)
+            sdk.temp_move(position_mm=0, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True)
+            time.sleep(5)  # 等待运动完成
+            
+        print("\n循环结束，确认最终状态为张开 (位置 0)...")
+        # 再次发送指令确保最终状态为 0 (虽然循环最后一步已经是 0，但这保证了"最后要张开"的要求)
+        sdk.temp_move(position_mm=0, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True)
+        time.sleep(1)
+
+    except KeyboardInterrupt:
+        print("\n程序被用户中断。")
+    except Exception as e:
+        print(f"执行过程中出错: {e}")
+    finally:
+        sdk.stop_monitoring()
+        print("程序已完成。")

start_gripper.bat

@@ -0,0 +1,11 @@
+@echo off
+setlocal
+cd /d "%~dp0"
+call conda activate py311
+if errorlevel 1 (
+    echo Failed to activate conda environment: py311
+    pause
+    exit /b 1
+)
+python -m gripper.visualizer %*
+pause