init

gripper_control/README.md

@@ -0,0 +1,118 @@
+# Gripper Control
+
+当前实际控制链：
+
+1. `gripper_control/main.py`
+   程序入口，负责组装配置、触觉读取、夹爪硬件和控制器。
+
+2. `gripper_control/config.py`
+   读取 Python 配置文件和命令行参数，生成 `ControlConfig`。
+
+3. `gripper_control/force_reader.py`
+   启动左右两个触觉传感器进程，读取 `FNORMAL`、`FSHEARX`、`FSHEARY`。
+
+4. `gripper_control/calibration.py`
+   把原始力值映射成 N。
+
+5. `gripper_control/feedback.py`
+   做基线扣除、滤波，输出控制器需要的力反馈。
+
+6. `gripper_control/hardware.py`
+   封装夹爪 SDK 的 `temp_move`、`set_target_force`、`read_real_position`。
+
+7. `gripper_control/controller.py`
+   定时开合、检测物体、慢慢加力、稳定夹持、空夹恢复、人取物松手的状态机。
+
+## 运行
+
+从项目根目录运行：
+
+```powershell
+conda activate py311
+python -m gripper_control.main
+```
+
+旧入口仍然可用：
+
+```powershell
+python examples\gripper_timed_cycle_control.py
+```
+
+默认配置文件：
+
+```text
+gripper_control\config\gripper_timed_cycle_control.py
+```
+
+只测试逻辑、不打开串口：
+
+```powershell
+python -m gripper_control.main --dry-run
+```
+
+## 日志字段
+
+程序运行时主要有两类输出：周期状态日志和事件日志。
+
+### 周期状态日志
+
+示例：
+
+```text
+t=38.83s L=0.024N R=0.067N min=0.024N max=0.067N diff=-0.043N both=0 object=0 grip_contact=0 empty_for=0.4s shear=0.113N d_shear=-0.013N shear_stable=1 state=hold_check gripped=1 hold_pos=9000 force_pct=15 action=hold-check
+```
+
+字段说明：
+
+- `t`: 当前 close phase 已经运行的时间，单位秒。
+- `L`: 左侧触觉传感器标定后的法向力，单位 N。
+- `R`: 右侧触觉传感器标定后的法向力，单位 N。
+- `min`: 左右两侧法向力中的较小值，单位 N。
+- `max`: 左右两侧法向力中的较大值，单位 N。
+- `diff`: `L - R`，左右法向力差值，单位 N；负数表示右侧更大。
+- `both`: 左右两侧是否都超过 `BOTH_CONTACT_FORCE`；`1` 是，`0` 否。
+- `object`: 是否确认检测到物体；默认需要达到 `OBJECT_FORCE` 且 `both=1`。
+- `grip_contact`: 夹持后是否仍然认为有接触；默认等于 `both`。
+- `empty_for`: 夹持/保持状态下，接触连续消失的时间；达到 `EMPTY_RELEASE_SECONDS` 后恢复定时开合。
+- `shear`: 标定后的切向力，单位 N；由 `sqrt(FSHEARX^2 + FSHEARY^2)` 映射得到。
+- `d_shear`: 本次循环相对上一次循环的切向力变化量，单位 N；负数表示切向力下降。
+- `shear_stable`: 切向力是否进入稳定计时；`1` 表示已经超过 `SHEAR_HOLD_FORCE` 且变化小于 `SHEAR_STABLE_DELTA`。
+- `state`: 控制器当前状态。
+- `gripped`: 是否已经进入过夹持状态；`1` 是，`0` 否。
+- `hold_pos`: 夹爪保持位置；通常是检测到物体时读取到的当前位置。
+- `force_pct`: 当前发送给夹爪的目标力百分比。
+- `action`: 当前这一步执行或等待的动作。
+
+### 事件日志
+
+示例：
+
+```text
+L=0.013N R=0.223N min=0.013N max=0.223N force_pct=10 action=empty-grip-resume-cycle
+```
+
+这类日志只在关键事件发生时打印，所以字段更少。
+
+- `L/R/min/max`: 事件发生时的左右法向力和最小/最大法向力，单位 N。
+- `force_pct`: 事件发生后设置的夹爪目标力百分比。
+- `action=empty-grip-resume-cycle`: 已经进入夹持/保持状态，但接触连续消失超过 `EMPTY_RELEASE_SECONDS`，程序判断为空夹，退出当前夹持状态，回到定时开合循环。下一轮会先执行 open phase。
+
+### 常见状态
+
+- `closing`: 正在按定时循环闭合，还没确认检测到物体。
+- `gripping`: 已检测到物体，夹爪保持当前位置，并从低力慢慢加力。
+- `hold_check`: 切向力已经达到最低要求并稳定，夹爪进入保持/人取物检测状态。
+
+### 常见 action
+
+- `closing`: 继续闭合等待物体。
+- `close-continue`: 未检测到物体，重复发送闭合命令。
+- `object-detected-low-force`: 检测到物体，停止定时开合，并把夹爪力切到 `GRIP_START_FORCE`。
+- `grip-ramp-up`: 夹住后慢慢增加 `force_pct`。
+- `wait-shear-stable`: 切向力已超过 `SHEAR_HOLD_FORCE`，正在等待稳定。
+- `shear-stable-hold`: 切向力稳定时间达到 `SHEAR_STABLE_SECONDS`，进入 `hold_check`。
+- `hold-check`: 保持夹住，等待人取物触发。
+- `hold-check-armed`: 进入 `hold_check` 后已经过了 `HUMAN_HOLD_SECONDS`，开始监测人取物。
+- `human-release-open`: 检测到人取物造成的切向力变化，夹爪张开。
+- `empty-grip-resume-cycle`: 夹持后接触消失，判定为空夹，回到定时开合。
+- `emergency-open`: 超过 `EMERGENCY_TOUCH_FORCE` 且启用 emergency open 后，夹爪张开。

gripper_control/__init__.py

@@ -0,0 +1,2 @@
+"""Class-based gripper/tactile closed-loop control package."""
+

gripper_control/calibration.py

@@ -0,0 +1,111 @@
+from dataclasses import dataclass
+
+
+DEFAULT_FORCE_CALIBRATION_POINTS = [
+    {"raw": 1000.0, "force_n": 0.0},
+    {"raw": 10000.0, "force_n": 0.377},
+    {"raw": 30000.0, "force_n": 1.377},
+    {"raw": 62000.0, "force_n": 2.377},
+]
+
+DEFAULT_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},
+    ],
+}
+
+DEFAULT_SHEAR_FORCE_CALIBRATION = {
+    "enabled": True,
+    "method": "piecewise_linear",
+    "extrapolate": True,
+    "clamp_output_min": 0.0,
+    "points": DEFAULT_FORCE_CALIBRATION_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
+
+    @classmethod
+    def from_config(cls, calibration, config_name):
+        if calibration is None:
+            calibration = DEFAULT_NORMAL_FORCE_CALIBRATION
+        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),
+        )
+
+    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 _clamp(self, value):
+        if self.clamp_output_min is not None:
+            value = max(float(self.clamp_output_min), value)
+        return value
+

gripper_control/config.py

@@ -0,0 +1,222 @@
+import argparse
+import copy
+import importlib.util
+import json
+from dataclasses import dataclass, field
+from pathlib import Path
+
+from .calibration import (
+    DEFAULT_NORMAL_FORCE_CALIBRATION,
+    DEFAULT_SHEAR_FORCE_CALIBRATION,
+)
+from .paths import PACKAGE_DIR
+
+
+DEFAULT_CONTROL_CONFIG = str(PACKAGE_DIR / "config" / "gripper_timed_cycle_control.py")
+
+
+def load_control_config(path):
+    suffix = Path(path).suffix.lower()
+    if suffix == ".py":
+        return load_python_config(path)
+    return load_json_config(path)
+
+
+def load_python_config(path):
+    try:
+        spec = importlib.util.spec_from_file_location("gripper_user_config", path)
+        if spec is None or spec.loader is None:
+            raise RuntimeError("cannot create import spec")
+        module = importlib.util.module_from_spec(spec)
+        spec.loader.exec_module(module)
+    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 {}
+
+    config = getattr(module, "CONFIG", None)
+    if config is None:
+        config = {
+            key.lower(): value
+            for key, value in vars(module).items()
+            if key.isupper()
+        }
+    if not isinstance(config, dict):
+        print(f"[Error] Python config {path} must define CONFIG as a dict; using CLI/default values")
+        return {}
+    return config
+
+
+def load_json_config(path):
+    try:
+        with open(path, "r", encoding="utf-8") as f:
+            return 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 {}
+
+
+def _config_default(config, key, default):
+    return config.get(key, config.get(key.replace("_", "-"), default))
+
+
+@dataclass
+class ControlConfig:
+    control_config: str = DEFAULT_CONTROL_CONFIG
+
+    video1: str = "0"
+    video2: str = "1"
+    rotation1: str = "./config/rotation_config_0.json"
+    rotation2: str = "./config/rotation_config_1.json"
+    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 = "COM3"
+    slave_id: int = 1
+    baudrate: int = 115200
+    serial_timeout: float = 0.2
+
+    open_pos: int = 0
+    close_pos: int = 9000
+    open_seconds: float = 3.0
+    close_seconds: float = 5.0
+    cycles: int = 0
+    open_at_end: bool = False
+
+    speed: int = 20
+    accel: int = 60
+    decel: int = 60
+
+    initial_force: int = 10
+    grip_start_force: int = 10
+    force_min: int = 10
+    force_max: int = 60
+    control_hz: float = 10.0
+
+    both_contact_force: float = 0.05
+    object_force: float = 0.08
+    object_requires_both_contact: bool = True
+    empty_release_seconds: float = 1.0
+    close_command_interval: float = 0.5
+    hold_fallback_pos: int | None = None
+
+    force_ramp_step: int = 1
+    force_ramp_interval: float = 0.3
+    shear_hold_force: float = 0.05
+    shear_stable_delta: float = 0.02
+    shear_stable_seconds: float = 0.5
+    human_hold_seconds: float = 1.0
+    release_shear_change: float = 0.5
+
+    emergency_touch_force: float = 2.5
+    filter_alpha: float = 0.25
+    shear_filter_alpha: float = 0.35
+    baseline_seconds: float = 1.0
+
+    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)
+    )
+
+    trigger_force_update: bool = False
+    enable_human_release: bool = True
+    enable_emergency_open: bool = False
+    dry_run: bool = False
+
+    @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", cls.rotation1))
+        add("--rotation2", "-r2", type=str, default=get("rotation2", cls.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("--open-seconds", type=float, default=get("open_seconds", cls.open_seconds))
+        add("--close-seconds", type=float, default=get("close_seconds", cls.close_seconds))
+        add("--cycles", type=int, default=get("cycles", cls.cycles))
+        add("--open-at-end", action="store_true", default=get("open_at_end", cls.open_at_end))
+
+        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("--initial-force", type=int, default=get("initial_force", cls.initial_force))
+        add("--grip-start-force", type=int, default=get("grip_start_force", cls.grip_start_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("--both-contact-force", type=float, default=get("both_contact_force", cls.both_contact_force))
+        add("--object-force", "--clamp-force", dest="object_force", type=float, default=get("object_force", cls.object_force))
+        add("--object-requires-both-contact", dest="object_requires_both_contact", action="store_true", default=get("object_requires_both_contact", cls.object_requires_both_contact))
+        add("--allow-single-side-object", dest="object_requires_both_contact", action="store_false")
+        add("--empty-release-seconds", type=float, default=get("empty_release_seconds", cls.empty_release_seconds))
+        add("--close-command-interval", type=float, default=get("close_command_interval", cls.close_command_interval))
+        add("--hold-fallback-pos", type=int, default=get("hold_fallback_pos", cls.hold_fallback_pos))
+
+        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("--shear-hold-force", type=float, default=get("shear_hold_force", cls.shear_hold_force))
+        add("--shear-stable-delta", type=float, default=get("shear_stable_delta", cls.shear_stable_delta))
+        add("--shear-stable-seconds", type=float, default=get("shear_stable_seconds", cls.shear_stable_seconds))
+        add("--human-hold-seconds", type=float, default=get("human_hold_seconds", cls.human_hold_seconds))
+        add("--release-shear-change", type=float, default=get("release_shear_change", cls.release_shear_change))
+        add("--emergency-touch-force", type=float, default=get("emergency_touch_force", cls.emergency_touch_force))
+        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("--baseline-seconds", type=float, default=get("baseline_seconds", cls.baseline_seconds))
+
+        add("--target-touch-force", type=float, default=1500.0, help=argparse.SUPPRESS)
+        add("--deadband", type=float, default=80.0, help=argparse.SUPPRESS)
+        add("--step-up", type=int, default=1, help=argparse.SUPPRESS)
+        add("--step-down", type=int, default=2, help=argparse.SUPPRESS)
+        add("--shear-spike-threshold", type=float, default=0.05, help=argparse.SUPPRESS)
+
+        add("--trigger-force-update", action="store_true", default=get("trigger_force_update", cls.trigger_force_update))
+        add("--enable-human-release", dest="enable_human_release", action="store_true", default=get("enable_human_release", cls.enable_human_release))
+        add("--disable-human-release", dest="enable_human_release", action="store_false")
+        add("--enable-emergency-open", action="store_true", default=get("enable_emergency_open", cls.enable_emergency_open))
+        add("--dry-run", action="store_true", default=get("dry_run", cls.dry_run))
+
+        args = parser.parse_args(argv)
+        values = vars(args)
+        for legacy_key in ("target_touch_force", "deadband", "step_up", "step_down", "shear_spike_threshold"):
+            values.pop(legacy_key, None)
+        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_control/config/gripper_timed_cycle_control.py

@@ -0,0 +1,195 @@
+"""夹爪定时开合 + 双触觉反馈控制配置。
+
+这个文件就是当前默认配置。可以直接改数值，`#` 后面是 Python 注释。
+运行入口：
+    python -m gripper_control.main
+"""
+
+
+# 左右触觉传感器视频源；纯数字字符串表示摄像头编号，也可以写视频文件路径。
+VIDEO1 = "0"
+VIDEO2 = "1"
+
+# 左右触觉传感器方向配置。
+ROTATION1 = "./config/rotation_config_0.json"
+ROTATION2 = "./config/rotation_config_1.json"
+
+# 左右触觉传感器 SDK 配置。
+SENSOR_CONFIG1 = "./config/ddjx01.json"
+SENSOR_CONFIG2 = "./config/ddjx01.json"
+
+# 触觉处理帧率和运动检测阈值。
+SENSOR_FPS = 30.0
+MOTION_THRESHOLD = 0.0
+SENSOR_CPU = False  # True 表示禁用 CUDA。
+
+
+# 夹爪串口配置。
+PORT = "COM5"
+SLAVE_ID = 1
+BAUDRATE = 115200
+SERIAL_TIMEOUT = 0.2
+
+
+# 夹爪开合位置和定时循环。
+OPEN_POS = 0
+CLOSE_POS = 9000
+OPEN_SECONDS = 3.0       # 未夹住物体时，张开阶段持续时间。
+CLOSE_SECONDS = 5.0      # 未夹住物体时，闭合尝试持续时间。
+CYCLES = 0               # 0 表示一直循环，直到夹住物体或手动停止。
+OPEN_AT_END = False      # 程序退出时是否发送一次张开命令。
+
+
+# 夹爪运动参数。
+SPEED = 15
+ACCEL = 60
+DECEL = 60
+
+
+# 夹爪力度参数，单位是夹爪 SDK 的 force_pct 百分比。
+INITIAL_FORCE = 15       # 定时闭合时用小力，避免空夹/误触发时用大力。
+GRIP_START_FORCE = 15    # 检测到物体后立刻切回这个低力，再慢慢加力。
+FORCE_MIN = 10
+FORCE_MAX = 30
+CONTROL_HZ = 10.0
+
+
+# 法向力阈值，单位 N。程序会先做标定映射，再做判断。
+BOTH_CONTACT_FORCE = 0.05          # 左右两边都超过它时 both=1。
+OBJECT_FORCE = 0.08                # 检测到物体的法向力阈值。
+OBJECT_REQUIRES_BOTH_CONTACT = True  # True 可避免空夹时单侧噪声误触发。
+EMPTY_RELEASE_SECONDS = 1.0        # 夹持后接触消失这么久，判定为空夹并恢复定时开合。
+
+
+# 未检测到物体时，重复发送闭合命令的间隔；0 表示不重复发送。
+CLOSE_COMMAND_INTERVAL = 0.5
+
+# 读取当前夹爪位置失败时的备用保持位置；None 表示不使用备用位置。
+HOLD_FALLBACK_POS = None
+
+
+# 检测到物体后慢慢加力。
+FORCE_RAMP_STEP = 1
+FORCE_RAMP_INTERVAL = 0.3
+
+
+# 切向力判断，单位 N。当前切向力标定暂时沿用 N_.jpg，后续有切向标定后替换。
+SHEAR_HOLD_FORCE = 0.05       # 切向力超过它后，停止继续加力，开始等稳定。
+SHEAR_STABLE_DELTA = 0.02     # 相邻控制周期变化小于等于它，认为这一拍稳定。
+SHEAR_STABLE_SECONDS = 0.5    # 连续稳定这么久后进入 hold_check。
+HUMAN_HOLD_SECONDS = 1.0      # 进入 hold_check 后先稳定保持这么久，再判断人取物。
+RELEASE_SHEAR_CHANGE = 0.5    # hold_check 后切向力变化超过它，认为人在取物并松开。
+
+
+# 高法向力保护，单位 N。
+EMERGENCY_TOUCH_FORCE = 2.5
+
+
+# 滤波和启动基线。
+FILTER_ALPHA = 0.25
+SHEAR_FILTER_ALPHA = 0.35
+BASELINE_SECONDS = 1.0
+
+
+# 法向力标定：FNORMAL 原始值 -> N。
+NORMAL_FORCE_CALIBRATION = {
+    "enabled": True,
+    "method": "piecewise_linear",
+    "extrapolate": True,
+    "clamp_output_min": 0.0,
+    "points": [
+        {"fnormal": 1000.0, "force_n": 0.0},       # 0 N
+        {"fnormal": 10000.0, "force_n": 0.377},    # 0.377 N
+        {"fnormal": 30000.0, "force_n": 1.377},    # 1.377 N
+        {"fnormal": 62000.0, "force_n": 2.377},    # 2.377 N
+    ],
+}
+
+
+# 切向力标定：sqrt(FSHEARX^2 + FSHEARY^2) 原始幅值 -> N。
+# 注意：这里目前临时沿用 N_.jpg 的法向力标定点。
+# 后续如果你做了切向力标定，只需要替换 points。
+SHEAR_FORCE_CALIBRATION = {
+    "enabled": True,
+    "method": "piecewise_linear",
+    "extrapolate": True,
+    "clamp_output_min": 0.0,
+    "points": [
+        {"raw": 1000.0, "force_n": 0.0},
+        {"raw": 10000.0, "force_n": 0.377},
+        {"raw": 30000.0, "force_n": 1.377},
+        {"raw": 62000.0, "force_n": 2.377},
+    ],
+}
+
+
+# 夹爪 SDK 细节开关。
+TRIGGER_FORCE_UPDATE = False  # 如果 set_target_force 后力度不生效，可改 True。
+ENABLE_HUMAN_RELEASE = True   # True 表示人拿物体时，切向力变化会触发松开。
+ENABLE_EMERGENCY_OPEN = False # True 表示超过 EMERGENCY_TOUCH_FORCE 时自动张开。
+DRY_RUN = False               # True 只打印命令，不打开串口。
+
+
+# 运行代码最终读取这个字典。
+CONFIG = {
+    "video1": VIDEO1,
+    "video2": VIDEO2,
+    "rotation1": ROTATION1,
+    "rotation2": ROTATION2,
+    "sensor_config1": SENSOR_CONFIG1,
+    "sensor_config2": SENSOR_CONFIG2,
+    "sensor_fps": SENSOR_FPS,
+    "motion_threshold": MOTION_THRESHOLD,
+    "sensor_cpu": SENSOR_CPU,
+
+    "port": PORT,
+    "slave_id": SLAVE_ID,
+    "baudrate": BAUDRATE,
+    "serial_timeout": SERIAL_TIMEOUT,
+
+    "open_pos": OPEN_POS,
+    "close_pos": CLOSE_POS,
+    "open_seconds": OPEN_SECONDS,
+    "close_seconds": CLOSE_SECONDS,
+    "cycles": CYCLES,
+    "open_at_end": OPEN_AT_END,
+
+    "speed": SPEED,
+    "accel": ACCEL,
+    "decel": DECEL,
+
+    "initial_force": INITIAL_FORCE,
+    "grip_start_force": GRIP_START_FORCE,
+    "force_min": FORCE_MIN,
+    "force_max": FORCE_MAX,
+    "control_hz": CONTROL_HZ,
+
+    "both_contact_force": BOTH_CONTACT_FORCE,
+    "object_force": OBJECT_FORCE,
+    "object_requires_both_contact": OBJECT_REQUIRES_BOTH_CONTACT,
+    "empty_release_seconds": EMPTY_RELEASE_SECONDS,
+    "close_command_interval": CLOSE_COMMAND_INTERVAL,
+    "hold_fallback_pos": HOLD_FALLBACK_POS,
+
+    "force_ramp_step": FORCE_RAMP_STEP,
+    "force_ramp_interval": FORCE_RAMP_INTERVAL,
+    "shear_hold_force": SHEAR_HOLD_FORCE,
+    "shear_stable_delta": SHEAR_STABLE_DELTA,
+    "shear_stable_seconds": SHEAR_STABLE_SECONDS,
+    "human_hold_seconds": HUMAN_HOLD_SECONDS,
+    "release_shear_change": RELEASE_SHEAR_CHANGE,
+
+    "emergency_touch_force": EMERGENCY_TOUCH_FORCE,
+    "filter_alpha": FILTER_ALPHA,
+    "shear_filter_alpha": SHEAR_FILTER_ALPHA,
+    "baseline_seconds": BASELINE_SECONDS,
+
+    "normal_force_calibration": NORMAL_FORCE_CALIBRATION,
+    "shear_force_calibration": SHEAR_FORCE_CALIBRATION,
+
+    "trigger_force_update": TRIGGER_FORCE_UPDATE,
+    "enable_human_release": ENABLE_HUMAN_RELEASE,
+    "enable_emergency_open": ENABLE_EMERGENCY_OPEN,
+    "dry_run": DRY_RUN,
+}
+

gripper_control/controller.py

@@ -0,0 +1,326 @@
+import time
+
+from .filters import clamp
+
+
+class TimedCycleGripperController:
+    def __init__(self, config, reader, gripper, feedback_processor):
+        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
+
+    def run(self):
+        config = self.config
+        self.reader.start()
+
+        try:
+            self._print_thresholds()
+            print(
+                f"Calibrating baseline for {config.baseline_seconds:.2f}s. "
+                "Keep sensors unloaded."
+            )
+            baseline = self.feedback_processor.calibrate_baseline(self.reader)
+            print(
+                f"baseline normal: left={baseline.left_normal:.4f}{self.normal_unit}, "
+                f"right={baseline.right_normal:.4f}{self.normal_unit}; "
+                f"shear: left={baseline.left_shear:.4f}{self.shear_unit}, "
+                f"right={baseline.right_shear:.4f}{self.shear_unit}"
+            )
+
+            self.gripper.connect()
+
+            cycle_index = 1
+            while config.cycles <= 0 or cycle_index <= config.cycles:
+                open_force = int(clamp(
+                    config.initial_force,
+                    config.force_min,
+                    config.force_max,
+                ))
+                self._run_open_phase(open_force, cycle_index)
+                result = self._run_close_phase(cycle_index)
+
+                if result in ("emergency", "released"):
+                    time.sleep(config.open_seconds)
+                if result == "gripped":
+                    print("object is gripped; timed open/close loop stopped")
+                    break
+                cycle_index += 1
+
+        except KeyboardInterrupt:
+            print("\nStopping timed cycle control.")
+        finally:
+            self.gripper.final_open_if_needed()
+            self.reader.stop()
+
+    def _print_thresholds(self):
+        config = self.config
+        print(
+            "Normal force thresholds: "
+            f"both_contact={config.both_contact_force:.3f}{self.normal_unit}, "
+            f"object={config.object_force:.3f}{self.normal_unit}, "
+            f"emergency={config.emergency_touch_force:.3f}{self.normal_unit}, "
+            f"requires_both={int(config.object_requires_both_contact)}, "
+            f"empty_release={config.empty_release_seconds:.2f}s"
+        )
+        print(
+            "Shear force thresholds: "
+            f"hold_min={config.shear_hold_force:.3f}{self.shear_unit}, "
+            f"stable_delta={config.shear_stable_delta:.3f}{self.shear_unit}, "
+            f"stable_seconds={config.shear_stable_seconds:.2f}s, "
+            f"release_change={config.release_shear_change:.3f}{self.shear_unit}"
+        )
+
+    def _run_open_phase(self, force_pct, cycle_index):
+        print(f"\ncycle {cycle_index}: open phase")
+        self.gripper.open(force_pct, "open")
+
+        deadline = time.perf_counter() + self.config.open_seconds
+        while time.perf_counter() < deadline:
+            self.reader.update()
+            time.sleep(0.02)
+
+    def _run_close_phase(self, cycle_index):
+        config = self.config
+        print(f"cycle {cycle_index}: close phase")
+        self.feedback_processor.reset_filters()
+
+        force_pct = int(clamp(config.initial_force, config.force_min, config.force_max))
+        self.gripper.close(force_pct, "close")
+
+        interval = 1.0 / config.control_hz if config.control_hz > 0 else 0.1
+        phase_start = time.perf_counter()
+        last_close_command_time = phase_start
+        last_force_ramp_time = phase_start
+        state = "closing"
+        hold_pos = None
+        previous_shear = None
+        hold_start_time = None
+        hold_shear_reference = None
+        release_monitor_armed = False
+        shear_stable_since = None
+        empty_since = None
+        gripped = False
+        deadline = time.perf_counter() + config.close_seconds
+
+        while True:
+            tick_start = time.perf_counter()
+            now = time.perf_counter()
+            if state == "closing" and now >= deadline:
+                print("close phase finished without object; continue timed open/close cycle")
+                return "no_object"
+
+            feedback = self.feedback_processor.read(self.reader, timeout=0.3)
+            if feedback is None:
+                phase_time = time.perf_counter() - phase_start
+                print(f"t={phase_time:5.2f}s action=timed-close waiting for sensor samples")
+                time.sleep(0.05)
+                continue
+
+            has_previous_shear = previous_shear is not None
+            shear_delta = 0.0 if not has_previous_shear else feedback.max_shear - previous_shear
+            shear_abs_delta = abs(shear_delta)
+            previous_shear = feedback.max_shear
+
+            both_contact = (
+                feedback.left_normal >= config.both_contact_force
+                and feedback.right_normal >= config.both_contact_force
+            )
+            normal_force_detected = feedback.max_normal >= config.object_force
+            contact_confirmed = (
+                normal_force_detected
+                and (both_contact or not config.object_requires_both_contact)
+            )
+            object_detected = contact_confirmed
+            grip_contact = (
+                both_contact
+                if config.object_requires_both_contact
+                else feedback.max_normal >= config.both_contact_force
+            )
+
+            if state in ("gripping", "hold_check") and not grip_contact:
+                if empty_since is None:
+                    empty_since = now
+                elif now - empty_since >= config.empty_release_seconds:
+                    force_pct = self.gripper.set_force(config.force_min)
+                    print(
+                        f"L={feedback.left_normal:8.3f}{self.normal_unit} "
+                        f"R={feedback.right_normal:8.3f}{self.normal_unit} "
+                        f"min={feedback.min_normal:8.3f}{self.normal_unit} "
+                        f"max={feedback.max_normal:8.3f}{self.normal_unit} "
+                        f"force_pct={force_pct:3d} action=empty-grip-resume-cycle"
+                    )
+                    return "no_object"
+            else:
+                empty_since = None
+
+            if feedback.max_normal >= config.emergency_touch_force:
+                if config.enable_emergency_open:
+                    force_pct = self.gripper.set_force(config.force_min)
+                    self.gripper.open(force_pct, "emergency-open")
+                    print(
+                        f"L={feedback.left_normal:8.3f}{self.normal_unit} "
+                        f"R={feedback.right_normal:8.3f}{self.normal_unit} "
+                        f"max={feedback.max_normal:8.3f}{self.normal_unit} "
+                        f"force_pct={force_pct:3d} action=emergency-open"
+                    )
+                    return "emergency"
+                hold_pos = self.gripper.hold_current_position(force_pct)
+                state = "hold_check"
+                gripped = True
+                if hold_start_time is None:
+                    hold_start_time = now
+                action = "overforce-hold"
+
+            if state == "closing":
+                if contact_confirmed:
+                    state = "gripping"
+                    gripped = True
+                    force_pct = int(clamp(
+                        config.grip_start_force,
+                        config.force_min,
+                        config.force_max,
+                    ))
+                    force_pct = self.gripper.set_force(force_pct)
+                    hold_pos = self.gripper.hold_current_position(force_pct)
+                    last_force_ramp_time = now
+                    shear_stable_since = None
+                    action = "object-detected-low-force"
+                    print(
+                        "object detected; timed open/close cycle stopped, "
+                        f"grip force reset to {force_pct}"
+                    )
+                else:
+                    action = "closing"
+                    if (
+                        config.close_command_interval > 0
+                        and now - last_close_command_time >= config.close_command_interval
+                    ):
+                        self.gripper.close(force_pct, "close-continue")
+                        last_close_command_time = now
+
+            elif state == "gripping":
+                shear_is_high_enough = feedback.max_shear >= config.shear_hold_force
+                shear_is_stable = (
+                    shear_is_high_enough
+                    and has_previous_shear
+                    and shear_abs_delta <= config.shear_stable_delta
+                )
+
+                if shear_is_stable:
+                    if shear_stable_since is None:
+                        shear_stable_since = now
+                else:
+                    shear_stable_since = None
+
+                if (
+                    shear_stable_since is not None
+                    and now - shear_stable_since >= config.shear_stable_seconds
+                ):
+                    state = "hold_check"
+                    hold_start_time = now
+                    hold_shear_reference = None
+                    release_monitor_armed = False
+                    hold_pos = self.gripper.hold_current_position(force_pct)
+                    action = "shear-stable-hold"
+                elif shear_is_high_enough:
+                    action = "wait-shear-stable"
+                elif (
+                    now - last_force_ramp_time >= config.force_ramp_interval
+                    and force_pct < config.force_max
+                ):
+                    force_pct = int(clamp(
+                        force_pct + config.force_ramp_step,
+                        config.force_min,
+                        config.force_max,
+                    ))
+                    force_pct = self.gripper.set_force(force_pct)
+                    last_force_ramp_time = now
+                    action = "grip-ramp-up"
+                else:
+                    action = "grip-hold"
+
+            elif state == "hold_check":
+                hold_elapsed = now - hold_start_time
+                if not release_monitor_armed and hold_elapsed >= config.human_hold_seconds:
+                    hold_shear_reference = feedback.max_shear
+                    release_monitor_armed = True
+                    action = "hold-check-armed"
+                elif release_monitor_armed:
+                    hold_shear_change = abs(feedback.max_shear - hold_shear_reference)
+                    if hold_shear_change >= config.release_shear_change:
+                        if config.enable_human_release:
+                            force_pct = self.gripper.set_force(config.force_min)
+                            self.gripper.open(force_pct, "human-release-open")
+                            print(
+                                f"L={feedback.left_normal:8.3f}{self.normal_unit} "
+                                f"R={feedback.right_normal:8.3f}{self.normal_unit} "
+                                f"shear={feedback.max_shear:8.3f}{self.shear_unit} "
+                                f"hold_change={hold_shear_change:8.3f}{self.shear_unit} "
+                                f"force_pct={force_pct:3d} action=human-release-open"
+                            )
+                            return "released"
+                        hold_shear_reference = feedback.max_shear
+                        action = "hold-check-change-ignored"
+                    else:
+                        action = "hold-check"
+                else:
+                    action = "hold-check"
+
+            self._log_tick(
+                phase_start,
+                feedback,
+                shear_delta,
+                both_contact,
+                object_detected,
+                grip_contact,
+                empty_since,
+                shear_stable_since,
+                state,
+                gripped,
+                hold_pos,
+                force_pct,
+                action,
+            )
+
+            sleep_time = interval - (time.perf_counter() - tick_start)
+            if sleep_time > 0:
+                time.sleep(sleep_time)
+
+    def _log_tick(
+        self,
+        phase_start,
+        feedback,
+        shear_delta,
+        both_contact,
+        object_detected,
+        grip_contact,
+        empty_since,
+        shear_stable_since,
+        state,
+        gripped,
+        hold_pos,
+        force_pct,
+        action,
+    ):
+        now = time.perf_counter()
+        phase_time = now - phase_start
+        print(
+            f"t={phase_time:5.2f}s "
+            f"L={feedback.left_normal:8.3f}{self.normal_unit} "
+            f"R={feedback.right_normal:8.3f}{self.normal_unit} "
+            f"min={feedback.min_normal:8.3f}{self.normal_unit} "
+            f"max={feedback.max_normal:8.3f}{self.normal_unit} "
+            f"diff={feedback.normal_diff:8.3f}{self.normal_unit} "
+            f"both={int(both_contact)} object={int(object_detected)} "
+            f"grip_contact={int(grip_contact)} "
+            f"empty_for={0.0 if empty_since is None else now - empty_since:4.1f}s "
+            f"shear={feedback.max_shear:8.3f}{self.shear_unit} "
+            f"d_shear={shear_delta:8.3f}{self.shear_unit} "
+            f"shear_stable={int(shear_stable_since is not None)} "
+            f"state={state} gripped={int(gripped)} hold_pos={hold_pos} "
+            f"force_pct={force_pct:3d} action={action}"
+        )
+

gripper_control/feedback.py

@@ -0,0 +1,113 @@
+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
+
+
+@dataclass
+class ForceBaseline:
+    left_normal: float = 0.0
+    right_normal: float = 0.0
+    left_shear: float = 0.0
+    right_shear: 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
+
+
+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)
+
+    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 = []
+        deadline = time.perf_counter() + seconds
+        while time.perf_counter() < deadline:
+            left, right = reader.get_samples(timeout=0.2)
+            if left is not None:
+                left_values.append(self.normal_converter.convert(left["fnormal"]))
+                left_shear_values.append(self.shear_converter.convert(shear_magnitude(left)))
+            if right is not None:
+                right_values.append(self.normal_converter.convert(right["fnormal"]))
+                right_shear_values.append(self.shear_converter.convert(shear_magnitude(right)))
+            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,
+        )
+        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,
+        )
+        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)
+
+        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,
+        )
+

gripper_control/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_control/force_reader.py

@@ -0,0 +1,307 @@
+import os
+os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
+
+import argparse
+import multiprocessing as mp
+import queue
+import time
+from multiprocessing import Process, Queue
+
+from .paths import ensure_project_paths
+
+ensure_project_paths()
+
+
+def parse_video_source(value):
+    if isinstance(value, str):
+        value = value.strip()
+        if value.isdigit():
+            return int(value)
+    return value
+
+
+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 _sensor_force_worker(
+    sensor_id,
+    vid_src,
+    config_name,
+    rotation_config_path,
+    result_queue,
+    stop_event,
+    target_fps,
+    cuda,
+    isstitch,
+    backend,
+    motion_threshold,
+):
+    ensure_project_paths()
+    import orisys
+
+    sensor = None
+    try:
+        sensor = orisys.Sensor(
+            vid_src,
+            isstitch=isstitch,
+            cuda=cuda,
+            verbose=False,
+            config_name=config_name,
+            backend=backend,
+            rotation_config_path=rotation_config_path,
+        )
+
+        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,
+            )
+            fps, fnormal, fshearx, fsheary = sensor.read_info(
+                sensor.info.FPS,
+                sensor.info.FNORMAL,
+                sensor.info.FSHEARX,
+                sensor.info.FSHEARY,
+            )
+
+            _put_latest(
+                result_queue,
+                {
+                    "sensor_id": sensor_id,
+                    "fnormal": float(fnormal),
+                    "fshearx": float(fshearx),
+                    "fsheary": float(fsheary),
+                    "fps": float(fps),
+                    "is_contact": bool(is_contact),
+                    "timestamp": time.time(),
+                },
+            )
+
+            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="./config/rotation_config_0.json",
+        rotation2="./config/rotation_config_1.json",
+        target_fps=30,
+        cuda=True,
+        isstitch=True,
+        backend="auto",
+        motion_threshold=0,
+        queue_size=5,
+    ):
+        self.sensor_configs = [
+            {
+                "sensor_id": 0,
+                "vid_src": parse_video_source(video1),
+                "config_name": config1,
+                "rotation_config_path": rotation1,
+            },
+            {
+                "sensor_id": 1,
+                "vid_src": parse_video_source(video2),
+                "config_name": config2,
+                "rotation_config_path": 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.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_path"],
+                    self.result_queues[idx],
+                    self.stop_events[idx],
+                    self.target_fps,
+                    self.cuda,
+                    self.isstitch,
+                    self.backend,
+                    self.motion_threshold,
+                ),
+            )
+            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="./config/rotation_config_0.json")
+    parser.add_argument("--rotation2", "-r2", type=str, default="./config/rotation_config_1.json")
+    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_control/hardware.py

@@ -0,0 +1,102 @@
+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):
+        config = self.config
+        if config.dry_run:
+            print(
+                f"[dry-run] {label}: temp_move "
+                f"position={position}, speed={config.speed}, force={force_pct}, "
+                f"accel={config.accel}, decel={config.decel}"
+            )
+            return
+
+        self.motor.temp_move(
+            position_mm=int(position),
+            speed_pct=int(config.speed),
+            force_pct=int(force_pct),
+            accel=int(config.accel),
+            decel=int(config.decel),
+            trigger=True,
+        )
+
+    def open(self, force_pct, label="open"):
+        self.move(self.config.open_pos, force_pct, label)
+
+    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
+        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_control/main.py

@@ -0,0 +1,49 @@
+import multiprocessing as mp
+
+from .calibration import ForceConverter
+from .config import ControlConfig
+from .controller import TimedCycleGripperController
+from .feedback import TactileFeedbackProcessor
+from .force_reader import TwoSensorForceReader
+from .hardware import GripperClient
+from .paths import ensure_project_paths
+
+
+def build_controller(argv=None):
+    ensure_project_paths()
+    config = ControlConfig.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,
+    )
+    gripper = GripperClient(config)
+    feedback = TactileFeedbackProcessor(config, normal_converter, shear_converter)
+    return TimedCycleGripperController(config, reader, gripper, feedback)
+
+
+def main(argv=None):
+    controller = build_controller(argv)
+    controller.run()
+
+
+if __name__ == "__main__":
+    mp.freeze_support()
+    main()
+

gripper_control/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)
+