init

gripper_control_02/README.md

@@ -0,0 +1,91 @@
+# Gripper Control 02
+
+夹爪02是新的展示程序，和原来的定时开合逻辑分开。它不是定时循环开合；程序只是常驻监听单侧触发，每次触发后执行一次“闭合夹住 -> 监测变化 -> 直接张开”的流程。
+
+## 展示流程
+
+1. 程序启动后，夹爪先张开。
+2. 程序等待任意一侧触觉传感器感受到法向力或切向力；进入等待后会先记录张开时的安静参考值。
+3. 展示时，把物体从某个侧边扫一下，触发单侧力变化。
+4. 触发后夹爪开始闭合，你把物体移动到中间。
+5. 闭合过程中检测到物体后，切到低力并保持当前位置。
+6. 像 01 一样进入 `gripping`，慢慢加力到 `HOLD_FORCE`。
+7. 进入 `hold_check` 后记录夹持参考力。
+8. 后续法向力或切向力变化超过阈值，夹爪直接张开到 `OPEN_POS`。
+9. 张开后进入 `open_recover`，保持张开并等待传感器力清空。
+10. 法向和切向读数都低于恢复阈值并稳定后，才回到 `open_wait` 等待下一次单侧扫过触发。
+
+## 运行
+
+从项目根目录运行：
+
+```powershell
+conda activate py311
+python -m gripper_control_02.main
+```
+
+只看逻辑、不打开串口：
+
+```powershell
+python -m gripper_control_02.main --dry-run
+```
+
+默认配置文件：
+
+```text
+gripper_control_02\config\gripper_demo_02.py
+```
+
+## 关键配置
+
+- `TRIGGER_NORMAL_FORCE`: 单侧法向力触发阈值。
+- `TRIGGER_SHEAR_FORCE`: 单侧切向力触发阈值。
+- `TRIGGER_NORMAL_CHANGE`: 单侧法向力相对张开参考值的变化触发阈值。
+- `TRIGGER_SHEAR_CHANGE`: 单侧切向力相对张开参考值的变化触发阈值。
+- `GRIP_NORMAL_FORCE`: 夹住时左右接触阈值。
+- `GRIP_REQUIRES_BOTH`: 是否必须左右都接触才算夹住。
+- `GRIP_START_FORCE`: 检测到物体后先切到的低力。
+- `FORCE_RAMP_STEP`: 慢慢加力时每次增加多少 force_pct。
+- `FORCE_RAMP_INTERVAL`: 慢慢加力时两次加力的间隔。
+- `HOLD_FORCE`: 慢慢加力到这个值后进入 `hold_check`。
+- `GRIP_SETTLE_SECONDS`: 夹住后等待稳定多久再记录参考力。
+- `RELEASE_NORMAL_CHANGE`: 夹住后法向力变化超过它就松开。
+- `RELEASE_SHEAR_CHANGE`: 夹住后切向力变化超过它就松开。
+- `RELEASE_CONTACT_LOST_NORMAL_FORCE`: 夹住后判断接触丢失的最大法向力。
+- `RELEASE_CONTACT_LOST_SHEAR_FORCE`: 夹住后判断接触丢失的最大切向力。
+- `RELEASE_CONTACT_LOST_SECONDS`: 接触丢失持续多久后松开。
+- `REARM_SECONDS`: 张开后最短等待时间，防止刚松开马上又误触发。
+- `RECOVER_NORMAL_FORCE`: 张开恢复时允许重新触发的最大法向残余力。
+- `RECOVER_SHEAR_FORCE`: 张开恢复时允许重新触发的最大切向残余力。
+- `RECOVER_STABLE_SECONDS`: 力清空后需要连续稳定多久，才重新等待触发。
+
+## 日志字段
+
+- `L/R`: 左右法向力，单位 N。
+- `maxN`: 左右法向力最大值，单位 N。
+- `shear`: 左右切向力最大值，单位 N。
+- `trigger`: 是否满足单侧触发条件。
+- `grip_contact`: 是否满足夹住接触条件。
+- `dN`: 夹住后当前法向力相对参考值的最大变化，单位 N。
+- `dShear`: 夹住后当前切向力相对参考值的变化，单位 N。
+- `state`: 当前状态。
+- `hold_pos`: 夹住后保持的位置。
+- `force_pct`: 当前夹爪目标力百分比。
+- `action`: 当前动作。
+
+## 状态说明
+
+- `open_wait`: 夹爪张开，等待任意单侧触发。
+- `closing`: 单侧触发后正在闭合，等待检测到物体。
+- `gripping`: 检测到物体后低力保持当前位置，并慢慢加力。
+- `hold_check`: 已夹住并记录参考力，监测法向/切向变化；变化超过阈值会直接张开。
+- `open_recover`: 已经张开到初始位置，正在等待法向/切向残余力清空；这个状态不会闭合夹爪。
+
+## 常见动作说明
+
+- `open-wait-armed`: 已经记录张开状态参考值，下一次单侧力变化可以触发闭合。
+- `force-change-release-open`: 检测到夹住后的法向或切向变化，已经发出张开命令。
+- `contact-lost-release-open`: 夹住后接触力消失了一小段时间，已经发出张开命令。
+- `open-recover-wait-force-clear`: 夹爪保持张开，但传感器还有残余力，不允许重新闭合。
+- `open-recover-stabilizing`: 力已经低于恢复阈值，正在等待稳定时间。
+- `open-ready`: 力已经清空并稳定，重新进入等待下一次扫过触发。

gripper_control_02/__init__.py

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

gripper_control_02/config/__init__.py

@@ -0,0 +1,2 @@
+"""Demo 02 Python configs."""
+

gripper_control_02/config/gripper_demo_02.py

@@ -0,0 +1,212 @@
+"""夹爪02展示配置。
+
+展示逻辑：
+1. 程序启动后夹爪默认张开。
+2. 等待任意一侧触觉传感器感受到力。
+3. 一侧触发后，夹爪开始闭合；你把物体移动到中间。
+4. 闭合时检测到物体后，像 01 一样低力保持当前位置并慢慢加力夹住。
+5. 夹住后法向力或切向力变化超过阈值，直接张开到 OPEN_POS。
+6. 张开后保持初始张开状态，等力清空并稳定后才重新等待单侧触发；不会自己定时开合。
+"""
+
+
+# 左右触觉传感器视频源。
+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
+
+
+# 夹爪串口配置。
+PORT = "COM5"
+SLAVE_ID = 1
+BAUDRATE = 115200
+SERIAL_TIMEOUT = 0.2
+
+
+# 夹爪位置。
+OPEN_POS = 0
+CLOSE_POS = 9000
+
+# 夹爪运动参数。展示时可以速度稍快一点。
+SPEED = 25
+ACCEL = 80
+DECEL = 80
+
+
+# 夹爪力度百分比。
+OPEN_FORCE = 15           # 张开时使用的力。
+CLOSE_FORCE = 15          # 单侧触发后闭合时使用的小力。
+GRIP_START_FORCE = 10     # 闭合检测到物体后，先切到这个低力保持。
+HOLD_FORCE = 15           # 慢慢加力到这个值后进入 hold_check。
+FORCE_MIN = 10
+FORCE_MAX = 30
+
+
+# 控制循环频率。展示时调高一点，单侧快速扫过不容易漏检。
+CONTROL_HZ = 30.0
+
+
+# 启动时采集空载基线的时间。
+BASELINE_SECONDS = 1.0
+
+# 滤波系数，越大响应越快，越小越平滑。
+FILTER_ALPHA = 0.55
+SHEAR_FILTER_ALPHA = 0.65
+
+
+# 等待触发：任意一侧超过这些阈值，就开始闭合。
+TRIGGER_NORMAL_FORCE = 0.04  # 单侧法向力触发阈值，单位 N。
+TRIGGER_SHEAR_FORCE = 0.04   # 单侧切向力触发阈值，单位 N。
+
+# 等待触发：如果已经记录了张开时的安静参考值，任意一侧变化超过这些阈值也会闭合。
+# 这样现场轻扫一下也能触发，不完全依赖绝对力超过 TRIGGER_*_FORCE。
+TRIGGER_NORMAL_CHANGE = 0.04  # 单侧法向力相对张开参考值的变化触发阈值，单位 N。
+TRIGGER_SHEAR_CHANGE = 0.04   # 单侧切向力相对张开参考值的变化触发阈值，单位 N。
+
+
+# 闭合夹取：物体移动到中间后，左右两边都超过该阈值就认为夹住。
+GRIP_NORMAL_FORCE = 0.05     # 双侧接触阈值，单位 N。
+GRIP_REQUIRES_BOTH = True    # True 表示必须左右两侧都接触才算夹住。
+CLOSE_TIMEOUT_SECONDS = 5.0  # 触发闭合后这么久还没夹住，就重新张开等待。
+
+
+# 检测到物体后慢慢加力，类似 01 的 gripping 状态。
+FORCE_RAMP_STEP = 1
+FORCE_RAMP_INTERVAL = 0.2
+GRIP_SETTLE_SECONDS = 0.3    # 加到 HOLD_FORCE 后再稳定这么久，进入 hold_check。
+
+# 松开触发阈值。夹住后，法向或切向变化超过其中任意一个，就张开。
+RELEASE_NORMAL_CHANGE = 0.03  # 左/右法向力相对夹住参考值的变化阈值，单位 N。
+RELEASE_SHEAR_CHANGE = 0.03   # 左/右切向力相对夹住参考值的变化阈值，单位 N。
+
+# 夹住后如果物体被拿走，法向和切向都降到这些阈值以下并持续一小段时间，也直接张开。
+RELEASE_CONTACT_LOST_NORMAL_FORCE = 0.025  # 判断接触丢失的最大法向力，单位 N。
+RELEASE_CONTACT_LOST_SHEAR_FORCE = 0.025   # 判断接触丢失的最大切向力，单位 N。
+RELEASE_CONTACT_LOST_SECONDS = 0.15        # 接触丢失持续这么久后张开。
+
+# 松开后至少等待多久重新进入等待触发，避免刚张开时马上再次触发。
+REARM_SECONDS = 0.5
+
+# 松开/超时张开后，必须等传感器读数低于这些阈值并稳定，才允许下一次触发。
+# 这几个值应略小于 TRIGGER_NORMAL_FORCE / TRIGGER_SHEAR_FORCE。
+RECOVER_NORMAL_FORCE = 0.035  # 张开恢复时允许重新触发的最大法向残余力，单位 N。
+RECOVER_SHEAR_FORCE = 0.035   # 张开恢复时允许重新触发的最大切向残余力，单位 N。
+RECOVER_STABLE_SECONDS = 0.25 # 力清空后需要连续稳定这么久，才回到 open_wait。
+
+
+# 如果读取当前位置失败，保持当前位置时使用这个备用位置；None 表示不使用。
+HOLD_FALLBACK_POS = None
+
+
+# SDK 行为开关。
+TRIGGER_FORCE_UPDATE = False
+OPEN_AT_END = True
+DRY_RUN = False
+
+
+# 法向力标定：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},
+        {"fnormal": 10000.0, "force_n": 0.377},
+        {"fnormal": 30000.0, "force_n": 1.377},
+        {"fnormal": 62000.0, "force_n": 2.377},
+    ],
+}
+
+
+# 切向力标定：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},
+    ],
+}
+
+
+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,
+    "speed": SPEED,
+    "accel": ACCEL,
+    "decel": DECEL,
+
+    "open_force": OPEN_FORCE,
+    "close_force": CLOSE_FORCE,
+    "grip_start_force": GRIP_START_FORCE,
+    "hold_force": HOLD_FORCE,
+    "initial_force": OPEN_FORCE,
+    "force_min": FORCE_MIN,
+    "force_max": FORCE_MAX,
+
+    "control_hz": CONTROL_HZ,
+    "baseline_seconds": BASELINE_SECONDS,
+    "filter_alpha": FILTER_ALPHA,
+    "shear_filter_alpha": SHEAR_FILTER_ALPHA,
+
+    "trigger_normal_force": TRIGGER_NORMAL_FORCE,
+    "trigger_shear_force": TRIGGER_SHEAR_FORCE,
+    "trigger_normal_change": TRIGGER_NORMAL_CHANGE,
+    "trigger_shear_change": TRIGGER_SHEAR_CHANGE,
+    "grip_normal_force": GRIP_NORMAL_FORCE,
+    "grip_requires_both": GRIP_REQUIRES_BOTH,
+    "close_timeout_seconds": CLOSE_TIMEOUT_SECONDS,
+    "force_ramp_step": FORCE_RAMP_STEP,
+    "force_ramp_interval": FORCE_RAMP_INTERVAL,
+    "grip_settle_seconds": GRIP_SETTLE_SECONDS,
+    "release_normal_change": RELEASE_NORMAL_CHANGE,
+    "release_shear_change": RELEASE_SHEAR_CHANGE,
+    "release_contact_lost_normal_force": RELEASE_CONTACT_LOST_NORMAL_FORCE,
+    "release_contact_lost_shear_force": RELEASE_CONTACT_LOST_SHEAR_FORCE,
+    "release_contact_lost_seconds": RELEASE_CONTACT_LOST_SECONDS,
+    "rearm_seconds": REARM_SECONDS,
+    "recover_normal_force": RECOVER_NORMAL_FORCE,
+    "recover_shear_force": RECOVER_SHEAR_FORCE,
+    "recover_stable_seconds": RECOVER_STABLE_SECONDS,
+
+    "hold_fallback_pos": HOLD_FALLBACK_POS,
+    "trigger_force_update": TRIGGER_FORCE_UPDATE,
+    "open_at_end": OPEN_AT_END,
+    "dry_run": DRY_RUN,
+
+    "normal_force_calibration": NORMAL_FORCE_CALIBRATION,
+    "shear_force_calibration": SHEAR_FORCE_CALIBRATION,
+}

gripper_control_02/controller.py

@@ -0,0 +1,304 @@
+import time
+
+from gripper_control.filters import clamp
+
+
+class SideTriggerGripController:
+    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):
+        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}"
+        )
+
+    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
+        recover_quiet_since = None
+        force_pct = int(clamp(config.open_force, config.force_min, config.force_max))
+        hold_pos = None
+        reference = None
+
+        while True:
+            tick_start = time.perf_counter()
+            now = time.perf_counter()
+
+            feedback = self.feedback_processor.read(self.reader, timeout=0.3)
+            if feedback is None:
+                print(f"state={state} action=waiting-for-sensor-samples")
+                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 = "wait"
+
+            if state == "open_wait":
+                if trigger:
+                    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
+                    recover_quiet_since = None
+                    hold_pos = None
+                    reference = None
+                    action = "side-trigger-close"
+                else:
+                    action = "open-wait-touch"
+
+            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
+                    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
+                    recover_quiet_since = None
+                    hold_pos = None
+                    reference = None
+                    action = "close-timeout-open"
+                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
+                    action = "hold-reference-armed"
+                else:
+                    action = "grip-hold"
+
+            elif state == "hold_check":
+                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")
+                    self.feedback_processor.reset_filters()
+                    state = "open_recover"
+                    state_start = now
+                    recover_quiet_since = None
+                    hold_pos = None
+                    reference = None
+                    action = "force-change-release-open"
+                else:
+                    action = "hold-check"
+
+            elif state == "open_recover":
+                if self._recover_quiet(feedback):
+                    if recover_quiet_since is None:
+                        recover_quiet_since = now
+                        action = "open-recover-quiet"
+                    elif (
+                        now - recover_quiet_since >= config.recover_stable_seconds
+                        and now - state_start >= config.rearm_seconds
+                    ):
+                        state = "open_wait"
+                        state_start = now
+                        recover_quiet_since = None
+                        hold_pos = None
+                        reference = None
+                        action = "open-ready"
+                    else:
+                        action = "open-recover-stabilizing"
+                else:
+                    recover_quiet_since = None
+                    action = "open-recover-wait-force-clear"
+
+            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,
+                action=action,
+            )
+
+            sleep_time = interval - (time.perf_counter() - tick_start)
+            if sleep_time > 0:
+                time.sleep(sleep_time)
+
+    def _side_triggered(self, feedback, reference=None):
+        if reference is not None:
+            normal_change, shear_change = self._release_changes(feedback, reference)
+            if (
+                normal_change >= self.config.trigger_normal_change
+                or shear_change >= self.config.trigger_shear_change
+            ):
+                return True
+
+        return (
+            feedback.max_normal >= self.config.trigger_normal_force
+            or feedback.max_shear >= self.config.trigger_shear_force
+        )
+
+    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 _make_reference(self, feedback):
+        return {
+            "left_normal": feedback.left_normal,
+            "right_normal": feedback.right_normal,
+            "max_normal": feedback.max_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"]),
+            abs(feedback.max_normal - reference["max_normal"]),
+        )
+        shear_change = max(
+            abs(feedback.left_shear - reference["left_shear"]),
+            abs(feedback.right_shear - reference["right_shear"]),
+            abs(feedback.max_shear - reference["max_shear"]),
+        )
+        return normal_change, shear_change
+
+    def _release_contact_lost(self, feedback):
+        return (
+            feedback.max_normal <= self.config.release_contact_lost_normal_force
+            and feedback.max_shear <= self.config.release_contact_lost_shear_force
+        )
+
+    def _log_tick(
+        self,
+        feedback,
+        state,
+        trigger,
+        grip_contact,
+        normal_change,
+        shear_change,
+        hold_pos,
+        force_pct,
+        action,
+    ):
+        print(
+            f"L={feedback.left_normal:7.3f}{self.normal_unit} "
+            f"R={feedback.right_normal:7.3f}{self.normal_unit} "
+            f"maxN={feedback.max_normal:7.3f}{self.normal_unit} "
+            f"shear={feedback.max_shear:7.3f}{self.shear_unit} "
+            f"trigger={int(trigger)} grip_contact={int(grip_contact)} "
+            f"dN={normal_change:7.3f}{self.normal_unit} "
+            f"dShear={shear_change:7.3f}{self.shear_unit} "
+            f"state={state} hold_pos={hold_pos} "
+            f"force_pct={force_pct:3d} action={action}"
+        )

gripper_control_02/main.py

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

gripper_control_02/settings.py

@@ -0,0 +1,172 @@
+import argparse
+import copy
+from dataclasses import dataclass, field
+
+from gripper_control.calibration import (
+    DEFAULT_NORMAL_FORCE_CALIBRATION,
+    DEFAULT_SHEAR_FORCE_CALIBRATION,
+)
+from gripper_control.config import load_control_config
+from gripper_control.paths import REPO_ROOT
+
+
+DEFAULT_CONTROL_CONFIG = str(
+    REPO_ROOT / "gripper_control_02" / "config" / "gripper_demo_02.py"
+)
+
+
+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: 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 = "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
+
+    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 = 30
+
+    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_normal_change: float = 0.02
+    trigger_shear_change: float = 0.02
+    grip_normal_force: float = 0.05
+    grip_requires_both: bool = True
+    close_timeout_seconds: float = 5.0
+    force_ramp_step: int = 1
+    force_ramp_interval: float = 0.2
+    grip_settle_seconds: float = 0.3
+    release_normal_change: float = 0.03
+    release_shear_change: float = 0.03
+    release_contact_lost_normal_force: float = 0.025
+    release_contact_lost_shear_force: float = 0.025
+    release_contact_lost_seconds: float = 0.15
+    rearm_seconds: float = 0.5
+    recover_normal_force: float = 0.035
+    recover_shear_force: float = 0.035
+    recover_stable_seconds: float = 0.25
+
+    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", 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("--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("--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-normal-change", type=float, default=get("trigger_normal_change", cls.trigger_normal_change))
+        add("--trigger-shear-change", type=float, default=get("trigger_shear_change", cls.trigger_shear_change))
+        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("--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("--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("--release-contact-lost-normal-force", type=float, default=get("release_contact_lost_normal_force", cls.release_contact_lost_normal_force))
+        add("--release-contact-lost-shear-force", type=float, default=get("release_contact_lost_shear_force", cls.release_contact_lost_shear_force))
+        add("--release-contact-lost-seconds", type=float, default=get("release_contact_lost_seconds", cls.release_contact_lost_seconds))
+        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("--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["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)