完善释放控制：稳定/迅速行为预设、力曲线夹稳门控与会话记录。

Co-authored-by: Cursor <cursoragent@cursor.com>

gripper/control/__init__.py

@@ -1,5 +1,11 @@
 from .controller import SideTriggerGripController
 from .feedback import TactileFeedbackProcessor
+from .actuator import ActuatorWorker, GripCommand
 
 
-__all__ = ["SideTriggerGripController", "TactileFeedbackProcessor"]
+__all__ = [
+    "SideTriggerGripController",
+    "TactileFeedbackProcessor",
+    "ActuatorWorker",
+    "GripCommand",
+]

gripper/control/actuator.py

@@ -0,0 +1,362 @@
+"""Serial-owning actuator worker: command queue + position cache."""
+
+from __future__ import annotations
+
+import threading
+import time
+from collections import deque
+from dataclasses import dataclass
+from typing import Optional
+
+
+@dataclass(frozen=True)
+class GripCommand:
+    kind: str
+    force_pct: Optional[int] = None
+    position: Optional[int] = None
+    label: str = ""
+    speed_pct: Optional[int] = None
+    accel: Optional[int] = None
+    decel: Optional[int] = None
+
+
+_MOTION_KINDS = frozenset({"open", "close", "move", "hold"})
+
+
+class ActuatorWorker:
+    """Runs all GripperClient I/O on one thread; control loop only submits."""
+
+    def __init__(self, gripper, position_poll_hz: float = 10.0):
+        self.gripper = gripper
+        self.position_poll_hz = max(1.0, float(position_poll_hz))
+        self._stop_event = threading.Event()
+        self._started = threading.Event()
+        self._start_error: Optional[BaseException] = None
+        self._thread: Optional[threading.Thread] = None
+
+        self._cmd_lock = threading.Lock()
+        self._commands: deque[GripCommand] = deque()
+        self._active_command: Optional[GripCommand] = None
+        self._motion_completed_at: Optional[float] = None
+        self._wake = threading.Event()
+        self._busy = threading.Event()
+
+        self._state_lock = threading.Lock()
+        self._position: Optional[int] = None
+        self._hold_pos: Optional[int] = None
+        self._last_error: Optional[str] = None
+        self._cmd_count = 0
+        self._cmd_total_s = 0.0
+        self._cmd_max_s = 0.0
+        self._pos_count = 0
+        self._pos_total_s = 0.0
+        self._pos_max_s = 0.0
+        self._perf_last_log = 0.0
+
+    @property
+    def position(self) -> Optional[int]:
+        with self._state_lock:
+            return self._position
+
+    @property
+    def hold_pos(self) -> Optional[int]:
+        with self._state_lock:
+            return self._hold_pos
+
+    @property
+    def last_error(self) -> Optional[str]:
+        with self._state_lock:
+            return self._last_error
+
+    @property
+    def motion_status(self):
+        """Atomic queue/in-flight snapshot and actual I/O completion time."""
+        with self._cmd_lock:
+            return bool(self._commands) or self._active_command is not None, self._motion_completed_at
+
+    def start(self, timeout: float = 10.0) -> None:
+        if self._thread is not None and self._thread.is_alive():
+            return
+        self._stop_event.clear()
+        self._started.clear()
+        self._start_error = None
+        self._thread = threading.Thread(
+            target=self._run,
+            name="gripper-actuator",
+            daemon=True,
+        )
+        self._thread.start()
+        if not self._started.wait(timeout=timeout):
+            raise TimeoutError("actuator worker failed to start in time")
+        if self._start_error is not None:
+            raise RuntimeError(f"actuator worker failed to connect: {self._start_error}")
+
+    def stop(self, timeout: float = 3.0) -> None:
+        self._stop_event.set()
+        self._wake.set()
+        thread = self._thread
+        if thread is not None:
+            thread.join(timeout=timeout)
+        self._thread = None
+
+    def submit(self, command: GripCommand) -> None:
+        with self._cmd_lock:
+            if command.kind in _MOTION_KINDS:
+                self._commands = deque(
+                    item for item in self._commands
+                    if item.kind not in _MOTION_KINDS and item.kind != "set_force"
+                )
+            elif command.kind == "set_force":
+                self._commands = deque(
+                    item for item in self._commands if item.kind != "set_force"
+                )
+            self._commands.append(command)
+        self._wake.set()
+
+    def open(
+        self,
+        force_pct: int,
+        label: str = "open",
+        speed_pct=None,
+        accel=None,
+        decel=None,
+    ) -> None:
+        self.submit(
+            GripCommand(
+                kind="open",
+                force_pct=int(force_pct),
+                label=label,
+                speed_pct=speed_pct,
+                accel=accel,
+                decel=decel,
+            )
+        )
+
+    def close(
+        self,
+        force_pct: int,
+        label: str = "close",
+        speed_pct=None,
+        accel=None,
+        decel=None,
+    ) -> None:
+        self.submit(
+            GripCommand(
+                kind="close",
+                force_pct=int(force_pct),
+                label=label,
+                speed_pct=speed_pct,
+                accel=accel,
+                decel=decel,
+            )
+        )
+
+    def move(
+        self,
+        position: int,
+        force_pct: int,
+        label: str = "move",
+        speed_pct=None,
+        accel=None,
+        decel=None,
+    ) -> None:
+        self.submit(
+            GripCommand(
+                kind="move",
+                position=int(position),
+                force_pct=int(force_pct),
+                label=label,
+                speed_pct=speed_pct,
+                accel=accel,
+                decel=decel,
+            )
+        )
+
+    def set_force(self, force_pct: int) -> None:
+        self.submit(GripCommand(kind="set_force", force_pct=int(force_pct)))
+
+    def hold(self, force_pct: int, speed_pct=None, accel=None, decel=None) -> None:
+        self.submit(
+            GripCommand(
+                kind="hold",
+                force_pct=int(force_pct),
+                speed_pct=speed_pct,
+                accel=accel,
+                decel=decel,
+                label="clamp-hold",
+            )
+        )
+
+    def final_open_if_needed(self) -> None:
+        self.submit(GripCommand(kind="final_open"))
+        self.wait_idle(timeout=5.0)
+
+    def wait_idle(self, timeout: float = 5.0) -> bool:
+        deadline = time.perf_counter() + timeout
+        while time.perf_counter() < deadline:
+            pending, _ = self.motion_status
+            if not pending:
+                return True
+            time.sleep(0.01)
+        return False
+
+    def _pop_command(self) -> Optional[GripCommand]:
+        with self._cmd_lock:
+            if not self._commands:
+                return None
+            self._active_command = self._commands.popleft()
+            return self._active_command
+
+    def _set_error(self, message: str) -> None:
+        with self._state_lock:
+            self._last_error = message
+
+    def _update_position(self, position: Optional[int]) -> None:
+        with self._state_lock:
+            if position is not None:
+                self._position = int(position)
+
+    def _run(self) -> None:
+        try:
+            self.gripper.connect()
+        except BaseException as exc:
+            self._start_error = exc
+            self._started.set()
+            return
+
+        self._started.set()
+        poll_interval = 1.0 / self.position_poll_hz
+        next_poll = time.perf_counter()
+        self._perf_last_log = time.perf_counter()
+
+        # Prime position cache once after connect.
+        self._poll_position()
+
+        while not self._stop_event.is_set():
+            cmd = self._pop_command()
+            if cmd is not None:
+                self._busy.set()
+                try:
+                    self._execute(cmd)
+                finally:
+                    with self._cmd_lock:
+                        self._motion_completed_at = time.perf_counter()
+                        self._active_command = None
+                        still_pending = bool(self._commands)
+                    if not still_pending:
+                        self._busy.clear()
+                continue
+
+            self._busy.clear()
+            now = time.perf_counter()
+            if now >= next_poll:
+                self._poll_position()
+                next_poll = now + poll_interval
+                self._maybe_log_perf(now)
+                continue
+
+            wait_s = min(0.02, max(0.0, next_poll - now))
+            self._wake.wait(timeout=wait_s)
+            self._wake.clear()
+
+    def _execute(self, cmd: GripCommand) -> None:
+        t0 = time.perf_counter()
+        try:
+            if cmd.kind == "open":
+                self.gripper.open(
+                    cmd.force_pct,
+                    cmd.label or "open",
+                    speed_pct=cmd.speed_pct,
+                    accel=cmd.accel,
+                    decel=cmd.decel,
+                )
+                self._update_position(self.gripper.last_commanded_position)
+            elif cmd.kind == "close":
+                self.gripper.close(
+                    cmd.force_pct,
+                    cmd.label or "close",
+                    speed_pct=cmd.speed_pct,
+                    accel=cmd.accel,
+                    decel=cmd.decel,
+                )
+                self._update_position(self.gripper.last_commanded_position)
+            elif cmd.kind == "move":
+                self.gripper.move(
+                    cmd.position,
+                    cmd.force_pct,
+                    cmd.label or "move",
+                    speed_pct=cmd.speed_pct,
+                    accel=cmd.accel,
+                    decel=cmd.decel,
+                )
+                self._update_position(cmd.position)
+            elif cmd.kind == "set_force":
+                self.gripper.set_force(cmd.force_pct)
+            elif cmd.kind == "hold":
+                hold_pos = self.gripper.hold_current_position(
+                    cmd.force_pct,
+                    speed_pct=cmd.speed_pct,
+                    accel=cmd.accel,
+                    decel=cmd.decel,
+                )
+                with self._state_lock:
+                    self._hold_pos = hold_pos
+                    if hold_pos is not None:
+                        self._position = int(hold_pos)
+            elif cmd.kind == "final_open":
+                self.gripper.final_open_if_needed()
+                self._update_position(self.gripper.last_commanded_position)
+            else:
+                self._set_error(f"unknown command kind: {cmd.kind}")
+        except Exception as exc:
+            self._set_error(f"{cmd.kind} failed: {exc}")
+            print(f"[actuator] {cmd.kind} failed: {exc}")
+        finally:
+            dt = time.perf_counter() - t0
+            with self._state_lock:
+                self._cmd_count += 1
+                self._cmd_total_s += dt
+                self._cmd_max_s = max(self._cmd_max_s, dt)
+
+    def _poll_position(self) -> None:
+        t0 = time.perf_counter()
+        try:
+            position = self.gripper.read_position()
+            self._update_position(position)
+        except Exception as exc:
+            self._set_error(f"read_position failed: {exc}")
+        finally:
+            dt = time.perf_counter() - t0
+            with self._state_lock:
+                self._pos_count += 1
+                self._pos_total_s += dt
+                self._pos_max_s = max(self._pos_max_s, dt)
+
+    def _maybe_log_perf(self, now: float) -> None:
+        if now - self._perf_last_log < 1.0:
+            return
+        with self._state_lock:
+            cmd_count = self._cmd_count
+            cmd_total = self._cmd_total_s
+            cmd_max = self._cmd_max_s
+            pos_count = self._pos_count
+            pos_total = self._pos_total_s
+            pos_max = self._pos_max_s
+            position = self._position
+            self._cmd_count = 0
+            self._cmd_total_s = 0.0
+            self._cmd_max_s = 0.0
+            self._pos_count = 0
+            self._pos_total_s = 0.0
+            self._pos_max_s = 0.0
+        self._perf_last_log = now
+        if cmd_count == 0 and pos_count == 0:
+            return
+        cmd_avg = (cmd_total / cmd_count * 1000.0) if cmd_count else 0.0
+        pos_avg = (pos_total / pos_count * 1000.0) if pos_count else 0.0
+        print(
+            "[perf] actuator "
+            f"cmd={cmd_count} avg={cmd_avg:.2f}ms max={cmd_max * 1000.0:.2f}ms "
+            f"pos_read={pos_count} avg={pos_avg:.2f}ms max={pos_max * 1000.0:.2f}ms "
+            f"pos={position}"
+        )

gripper/control/controller.py

@@ -1,8 +1,14 @@
 import time
 import threading
+import json
 from collections import deque
+from dataclasses import asdict
 
 from ..common.filters import clamp
+from ..common.paths import REPO_ROOT
+from .actuator import ActuatorWorker
+from .release_watch import ReleaseWatch
+from .session_recorder import SessionRecorder
 
 
 class SideTriggerGripController:
@@ -21,11 +27,20 @@ class SideTriggerGripController:
         self.normal_unit = feedback_processor.normal_converter.unit
         self.shear_unit = feedback_processor.shear_converter.unit
         self.status_callback = status_callback
+        self.actuator = None
+        self.release_watch = ReleaseWatch(config)
+        self._release_decision = None
+        self._force_trace = deque(maxlen=120)
+        self.release_trace_path = REPO_ROOT / "logs" / "release-last.json"
+        self.session_recorder = SessionRecorder(REPO_ROOT / "logs" / "sessions")
         self._stop_event = threading.Event()
         self._manual_lock = threading.Lock()
         self._manual_commands = deque()
         self._mode_lock = threading.Lock()
         self._control_mode = "smart"
+        self._last_log_time = 0.0
+        self._log_interval_s = 0.2
+        self._last_log_key = None
 
     def stop(self):
         self._stop_event.set()
@@ -62,6 +77,59 @@ class SideTriggerGripController:
         with self._mode_lock:
             self._control_mode = mode
 
+    def set_grip_behavior_mode(self, mode):
+        """Apply rapid/stable preset onto live config and reset release watch."""
+        applied = self.config.apply_grip_behavior_mode(mode)
+        # Filters should match the new profile immediately.
+        try:
+            self.feedback_processor.reset_filters()
+        except Exception:
+            pass
+        try:
+            self.release_watch.reset()
+        except Exception:
+            pass
+        print(
+            f"[behavior] grip_behavior_mode={applied} "
+            f"min_hold={self.config.release_min_hold_seconds:.2f}s "
+            f"side_touch={self.config.release_side_touch_change:.3f} "
+            f"confirm={self.config.release_confirm_samples} "
+            f"curve_hist={getattr(self.config, 'release_curve_history', 6)} "
+            f"slope_settle={getattr(self.config, 'release_require_slope_settle', False)}"
+        )
+        return applied
+
+    def start_session_recording(self, note=""):
+        meta = {
+            "note": note,
+            "grip_behavior_mode": getattr(self.config, "grip_behavior_mode", None),
+            "control_mode": self._get_control_mode(),
+            "config": {
+                key: getattr(self.config, key)
+                for key in dir(self.config)
+                if key.startswith("release_")
+                or key in {
+                    "control_hz",
+                    "filter_alpha",
+                    "shear_filter_alpha",
+                    "trigger_normal_force",
+                    "trigger_shear_force",
+                    "grip_normal_force",
+                    "grip_requires_both",
+                    "grip_behavior_mode",
+                    "adaptive_grip_enabled",
+                }
+            },
+        }
+        return self.session_recorder.start(meta=meta)
+
+    def stop_session_recording(self):
+        return self.session_recorder.stop()
+
+    @property
+    def session_recording(self):
+        return self.session_recorder.enabled
+
     def _get_control_mode(self):
         with self._mode_lock:
             return self._control_mode
@@ -75,16 +143,18 @@ class SideTriggerGripController:
     def run(self):
         self._stop_event.clear()
         self.reader.start()
+        self.actuator = ActuatorWorker(self.gripper, position_poll_hz=10.0)
 
         try:
-            self.gripper.connect()
+            self.actuator.start()
             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")
+            print("control loop uses async actuator thread (serial I/O off control tick)")
+            self.actuator.open(open_force, "startup-open")
 
             self._print_thresholds()
             print(
@@ -104,7 +174,16 @@ class SideTriggerGripController:
         except KeyboardInterrupt:
             print("\nStopping demo02.")
         finally:
-            self.gripper.final_open_if_needed()
+            try:
+                self.stop_session_recording()
+            except Exception:
+                pass
+            if self.actuator is not None:
+                try:
+                    self.actuator.final_open_if_needed()
+                finally:
+                    self.actuator.stop()
+                self.actuator = None
             self.reader.stop()
 
     def _print_thresholds(self):
@@ -117,8 +196,11 @@ class SideTriggerGripController:
             "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}"
+            f"release_abs_n={self.config.release_normal_change:.3f}{self.normal_unit}, "
+            f"release_abs_s={self.config.release_shear_change:.3f}{self.shear_unit}, "
+            f"release_rate_n={getattr(self.config, 'release_normal_rate', 1.8):.2f}/s, "
+            f"release_rate_s={getattr(self.config, 'release_shear_rate', 1.8):.2f}/s, "
+            f"slow_rate={getattr(self.config, 'release_slow_rate', 0.35):.2f}/s"
         )
         print(
             "Adaptive grip: "
@@ -128,7 +210,10 @@ class SideTriggerGripController:
             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"
+            f"self_force_cooldown={self.config.release_arm_delay_seconds:.2f}s, "
+            f"min_hold={getattr(self.config, 'release_min_hold_seconds', 0.28):.2f}s, "
+            f"curve_settle={self.config.release_settle_seconds:.2f}s, "
+            f"release_confirm={getattr(self.config, 'release_confirm_samples', 2)}"
         )
         print(
             "Release open: "
@@ -136,6 +221,7 @@ class SideTriggerGripController:
             f"accel={self.config.release_open_accel}, "
             f"decel={self.config.release_open_decel}"
         )
+        print("Release: side-touch watch starts at contact; normal clamp protection never waits for global quiet")
         print(
             "Recover: "
             f"force_normal={self.config.recover_normal_force:.3f}{self.normal_unit}, "
@@ -157,66 +243,79 @@ class SideTriggerGripController:
         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
+        release_watch = self.release_watch
+        release_watch.reset()
+        recent_feedback = deque(maxlen=6)
+        recent_pair_stamps = None
         hold_pos = None
-        reference = None
-        current_position = None
-        last_position_status_time = 0.0
-        position_perf = {
-            "last_log": time.perf_counter(),
-            "count": 0,
-            "total": 0.0,
-            "max": 0.0,
-        }
-
-        def update_current_position(now, force=False):
-            nonlocal current_position, last_position_status_time, position_perf
-            if force or now - last_position_status_time >= 0.1:
-                last_position_status_time = now
-                t0 = time.perf_counter()
-                current_position = self.gripper.read_position()
-                dt = time.perf_counter() - t0
-                position_perf["count"] += 1
-                position_perf["total"] += dt
-                position_perf["max"] = max(position_perf["max"], dt)
-                if now - position_perf["last_log"] >= 1.0:
-                    avg_ms = (
-                        position_perf["total"] / position_perf["count"] * 1000.0
-                        if position_perf["count"]
-                        else 0.0
-                    )
-                    print(
-                        "[perf] position_read "
-                        f"count={position_perf['count']} "
-                        f"avg={avg_ms:.2f}ms "
-                        f"max={position_perf['max'] * 1000.0:.2f}ms "
-                        f"pos={current_position}"
-                    )
-                    position_perf = {
-                        "last_log": now,
-                        "count": 0,
-                        "total": 0.0,
-                        "max": 0.0,
-                    }
-            return current_position
+        actuator = self.actuator
 
         def reset_open_wait_stability():
             nonlocal open_wait_samples, open_wait_stable_values
             open_wait_samples = []
             open_wait_stable_values = {}
 
+        def current_position():
+            return None if actuator is None else actuator.position
+
+        def refresh_hold_pos():
+            nonlocal hold_pos
+            if actuator is None:
+                return hold_pos
+            cached = actuator.hold_pos
+            if cached is not None:
+                hold_pos = cached
+            return hold_pos
+
+        def reset_release_watch():
+            release_watch.reset()
+
+        def try_release_watch(now_ts, feedback_now):
+            # The release FSM returns effects; only this adapter submits I/O.
+            nonlocal state, state_start, force_pct, hold_pos
+            nonlocal last_open_position_check_time, last_open_position
+            nonlocal last_open_position_ready, recover_quiet_since, recover_position_since
+            pending, completed_at = actuator.motion_status
+            decision = release_watch.step(
+                now_ts, feedback_now,
+                motion_pending=pending, motion_completed_at=completed_at,
+            )
+            self._release_decision = decision
+            action = decision.action
+            if decision.release:
+                force_pct = int(clamp(config.open_force, config.force_min, config.force_max))
+                actuator.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_ts
+                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()
+                reset_release_watch()
+                hold_pos = None
+                action = f"force-change-release-open({decision.reason})"
+            return decision.release, action, decision.normal_change, decision.shear_change
+
         while not self._stop_event.is_set():
             tick_start = time.perf_counter()
             now = time.perf_counter()
             manual_action = None
+            self._release_decision = None
             control_mode = self._get_control_mode()
+            if control_mode == "manual":
+                release_watch.suspend()
 
             manual_payload = self._pop_manual_command()
             if isinstance(manual_payload, dict):
@@ -235,7 +334,7 @@ class SideTriggerGripController:
                     config.force_min,
                     config.force_max,
                 ))
-                self.gripper.open(force_pct, "manual-open", speed_pct=manual_speed_pct)
+                actuator.open(force_pct, "manual-open", speed_pct=manual_speed_pct)
                 self.feedback_processor.reset_filters()
                 state = "open_recover"
                 state_start = now
@@ -244,12 +343,10 @@ class SideTriggerGripController:
                 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
+                reset_release_watch()
                 hold_pos = None
-                reference = None
                 manual_action = "manual-open"
             elif manual_command == "close":
                 force_pct = int(clamp(
@@ -257,7 +354,7 @@ class SideTriggerGripController:
                     config.force_min,
                     config.force_max,
                 ))
-                self.gripper.close(force_pct, "manual-close", speed_pct=manual_speed_pct)
+                actuator.close(force_pct, "manual-close", speed_pct=manual_speed_pct)
                 state = "manual_close"
                 state_start = now
                 last_force_ramp_time = now
@@ -266,12 +363,10 @@ class SideTriggerGripController:
                 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
+                reset_release_watch()
                 hold_pos = None
-                reference = None
                 manual_action = "manual-close"
             elif manual_command == "hold":
                 force_pct = int(clamp(
@@ -279,30 +374,24 @@ class SideTriggerGripController:
                     config.force_min,
                     config.force_max,
                 ))
-                self.gripper.set_force(force_pct)
-                hold_pos = self.gripper.hold_current_position(
-                    force_pct,
-                    speed_pct=manual_speed_pct,
-                )
+                actuator.set_force(force_pct)
+                actuator.hold(force_pct, speed_pct=manual_speed_pct)
+                hold_pos = refresh_hold_pos() or current_position()
                 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
+                reset_release_watch()
                 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
+                    reset_release_watch()
                     hold_pos = None
-                    reference = None
                 manual_action = "manual-release"
             elif manual_command == "move_position":
                 target_min = min(int(config.open_pos), int(config.close_pos))
@@ -317,7 +406,7 @@ class SideTriggerGripController:
                     config.force_min,
                     config.force_max,
                 ))
-                self.gripper.move(
+                actuator.move(
                     target_position,
                     force_pct,
                     "manual-move-position",
@@ -331,36 +420,37 @@ class SideTriggerGripController:
                 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
+                reset_release_watch()
                 hold_pos = None
-                reference = None
-                current_position = target_position
                 manual_action = "manual-move-position"
             elif manual_command == "restart_left_sensor":
                 if self.reader.restart_sensor(0):
+                    release_watch.suspend()
                     reset_open_wait_stability()
-                    trigger_clear_since = None
                     trigger_armed = False
                     manual_action = "left-sensor-restart"
                 else:
                     manual_action = "left-sensor-restart-failed"
             elif manual_command == "restart_right_sensor":
                 if self.reader.restart_sensor(1):
+                    release_watch.suspend()
                     reset_open_wait_stability()
-                    trigger_clear_since = None
                     trigger_armed = False
                     manual_action = "right-sensor-restart"
                 else:
                     manual_action = "right-sensor-restart-failed"
 
-            feedback = self.feedback_processor.read(self.reader, timeout=0.3)
+            # Non-blocking sample read: never stall the FSM on sensor wait.
+            feedback = self.feedback_processor.read(self.reader, timeout=0.0)
             if feedback is None:
+                release_watch.suspend()
+                recent_feedback.clear()
+                recent_pair_stamps = None
                 action = manual_action or "waiting-for-sensor-samples"
-                position_for_status = update_current_position(now)
-                print(f"state={state} action={action}")
+                position_for_status = current_position()
+                self._log_sparse(f"state={state} action={action}", now)
                 self._emit_status(
                     {
                         "timestamp": time.time(),
@@ -369,7 +459,9 @@ class SideTriggerGripController:
                         "force_pct": force_pct,
                         "adaptive_target_force": adaptive_target_force,
                         "adaptive_grip_enabled": config.adaptive_grip_enabled,
-                        "release_armed": release_armed,
+                        "release_armed": release_watch.armed,
+                        "release_phase": release_watch.phase.value,
+                        "release_reason": "sample-gap",
                         "manual_hold_active": state == "manual_hold",
                         "control_mode": control_mode,
                         "speed_pct": config.speed,
@@ -380,9 +472,19 @@ class SideTriggerGripController:
                         "shear_unit": self.shear_unit,
                     }
                 )
-                time.sleep(0.05)
+                sleep_time = interval - (time.perf_counter() - tick_start)
+                if sleep_time > 0:
+                    time.sleep(sleep_time)
                 continue
 
+            pair_stamps = (feedback.left_timestamp, feedback.right_timestamp)
+            if (recent_pair_stamps is None or None in pair_stamps
+                    or all(a != b for a, b in zip(pair_stamps, recent_pair_stamps))):
+                if recent_feedback and now - recent_feedback[-1][0] > config.release_sample_timeout_seconds:
+                    recent_feedback.clear()
+                recent_feedback.append((now, feedback))
+                recent_pair_stamps = pair_stamps
+
             trigger = self._side_triggered(feedback)
             grip_contact = self._grip_contact(feedback)
             normal_change = 0.0
@@ -397,14 +499,17 @@ class SideTriggerGripController:
                 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(
+                delta_trigger = self._open_wait_delta_triggered(
                     open_wait_sample,
                     open_wait_stable_values,
                 )
-                if not trigger:
-                    open_wait_stable_values.update(stable_values)
+                # Lock each channel's first stable reference. Continuously
+                # replacing it makes the reference follow a slow touch.
+                for key, value in stable_values.items():
+                    open_wait_stable_values.setdefault(key, value)
 
                 trigger_armed = bool(open_wait_stable_values)
+                trigger = trigger_armed and delta_trigger
                 if not trigger_armed:
                     action = "open-wait-stabilizing"
                 elif not trigger:
@@ -415,7 +520,7 @@ class SideTriggerGripController:
                         config.force_min,
                         config.force_max,
                     ))
-                    self.gripper.close(force_pct, "side-trigger-close")
+                    actuator.close(force_pct, "side-trigger-close")
                     state = "closing"
                     state_start = now
                     last_force_ramp_time = now
@@ -424,13 +529,11 @@ class SideTriggerGripController:
                     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
+                    reset_release_watch()
                     hold_pos = None
-                    reference = None
                     action = "side-trigger-close"
 
             elif state == "closing":
@@ -440,14 +543,14 @@ class SideTriggerGripController:
                         config.force_min,
                         config.force_max,
                     ))
-                    self.gripper.set_force(force_pct)
-                    hold_pos = self.gripper.hold_current_position(force_pct)
+                    actuator.set_force(force_pct)
+                    actuator.hold(force_pct)
+                    hold_pos = refresh_hold_pos() or current_position()
                     state = "gripping"
                     state_start = now
                     last_force_ramp_time = now
                     adaptive_target_force = self._adaptive_force_target(feedback)
-                    release_armed = False
-                    reference = None
+                    release_watch.contact(now, feedback, history=recent_feedback)
                     action = "object-detected-low-force"
                 elif now - state_start >= config.close_timeout_seconds:
                     force_pct = int(clamp(
@@ -455,7 +558,7 @@ class SideTriggerGripController:
                         config.force_min,
                         config.force_max,
                     ))
-                    self.gripper.open(force_pct, "close-timeout-open")
+                    actuator.open(force_pct, "close-timeout-open")
                     self.feedback_processor.reset_filters()
                     state = "open_recover"
                     state_start = now
@@ -465,24 +568,31 @@ class SideTriggerGripController:
                     recover_quiet_since = None
                     recover_position_since = None
                     reset_open_wait_stability()
-                    release_armed = False
+                    reset_release_watch()
                     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")
+                        actuator.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
+                refresh_hold_pos()
+                released, release_action, normal_change, shear_change = try_release_watch(
+                    now,
+                    feedback,
+                )
+                if released:
+                    action = release_action
+                elif (
+                    self._release_decision.allow_force_update
+                    and now - last_force_ramp_time >= config.force_ramp_interval
                     and force_pct < config.hold_force
                 ):
                     force_pct = int(clamp(
@@ -490,32 +600,37 @@ class SideTriggerGripController:
                         config.force_min,
                         config.hold_force,
                     ))
-                    self.gripper.set_force(force_pct)
+                    actuator.set_force(force_pct)
                     last_force_ramp_time = now
-                    action = "grip-ramp-up"
+                    action = f"{release_action}; grip-ramp-up"
                 elif (
                     force_pct >= config.hold_force
                     and now - state_start >= config.grip_settle_seconds
                 ):
-                    reference = self._make_reference(feedback)
+                    # Keep release watch continuous; do not re-lock on enter.
                     state = "hold_check"
                     state_start = now
                     adaptive_target_force = self._adaptive_force_target(feedback)
-                    release_armed = False
-                    action = "hold-reference-armed"
+                    action = release_action or "enter-hold-check"
                 else:
-                    action = "grip-hold"
+                    action = release_action or "grip-hold"
 
             elif state == "hold_check":
+                refresh_hold_pos()
                 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
                 )
 
+                # Always inspect external force first, even while the target
+                # asks for more clamp force. Confirmation takes priority.
+                released, release_action, normal_change, shear_change = try_release_watch(now, feedback)
+                action = release_action
                 if (
-                    should_adapt_force
+                    not released
+                    and self._release_decision.allow_force_update
+                    and should_adapt_force
                     and now - last_force_ramp_time >= config.adaptive_force_interval
                 ):
                     force_pct = int(clamp(
@@ -523,59 +638,9 @@ class SideTriggerGripController:
                         config.force_min,
                         adaptive_target_force,
                     ))
-                    self.gripper.set_force(force_pct)
+                    actuator.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"
+                    action = f"{release_action}; adaptive-grip-ramp"
 
             elif state == "open_recover":
                 current_pos = last_open_position
@@ -586,7 +651,8 @@ class SideTriggerGripController:
                     >= config.open_position_check_interval
                 ):
                     last_open_position_check_time = now
-                    current_pos = self.gripper.read_position()
+                    # Position is polled by actuator thread; FSM only reads cache.
+                    current_pos = current_position()
                     position_ready = self._open_position_ready(current_pos)
                     last_open_position = current_pos
                     last_open_position_ready = position_ready
@@ -606,13 +672,10 @@ class SideTriggerGripController:
                         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
+                        reset_release_watch()
                         hold_pos = None
-                        reference = None
                         action = f"open-ready-position pos={current_pos}"
                     else:
                         action = f"open-recover-position-stabilizing pos={current_pos}"
@@ -629,13 +692,10 @@ class SideTriggerGripController:
                         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
+                        reset_release_watch()
                         hold_pos = None
-                        reference = None
                         action = "open-ready"
                     else:
                         action = "open-recover-stabilizing"
@@ -645,6 +705,7 @@ class SideTriggerGripController:
                     action = "open-recover-wait-force-clear"
 
             elif state == "manual_hold":
+                refresh_hold_pos()
                 action = "manual-hold"
 
             elif state == "manual_close":
@@ -653,6 +714,23 @@ class SideTriggerGripController:
             elif state == "manual_position":
                 action = "manual-position"
 
+            self._record_force_trace(now, feedback, state, force_pct, action)
+            self._record_session_tick(
+                now=now,
+                feedback=feedback,
+                state=state,
+                force_pct=force_pct,
+                adaptive_target_force=adaptive_target_force,
+                action=action,
+                trigger=trigger,
+                trigger_armed=trigger_armed,
+                grip_contact=grip_contact,
+                normal_change=normal_change,
+                shear_change=shear_change,
+                control_mode=control_mode,
+                gripper_position=current_position(),
+                hold_pos=hold_pos,
+            )
             self._log_tick(
                 feedback=feedback,
                 state=state,
@@ -663,11 +741,12 @@ class SideTriggerGripController:
                 hold_pos=hold_pos,
                 force_pct=force_pct,
                 adaptive_target_force=adaptive_target_force,
-                release_armed=release_armed,
+                release_armed=release_watch.armed,
                 action=action,
                 trigger_armed=trigger_armed,
+                now=now,
             )
-            position_for_status = update_current_position(now)
+            position_for_status = current_position()
             self._emit_status(
                 self._make_status(
                     feedback=feedback,
@@ -679,14 +758,13 @@ class SideTriggerGripController:
                     hold_pos=hold_pos,
                     force_pct=force_pct,
                     adaptive_target_force=adaptive_target_force,
-                    release_armed=release_armed,
+                    release_armed=release_watch.armed,
                     action=action,
                     trigger_armed=trigger_armed,
                     control_mode=control_mode,
                     gripper_position=position_for_status,
                 )
             )
-            last_trigger = trigger
 
             sleep_time = interval - (time.perf_counter() - tick_start)
             if sleep_time > 0:
@@ -796,29 +874,118 @@ class SideTriggerGripController:
         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 _log_sparse(self, message, now=None):
+        now = time.perf_counter() if now is None else now
+        if now - self._last_log_time < self._log_interval_s:
+            return
+        self._last_log_time = now
+        print(message)
 
-    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 _record_force_trace(self, now, feedback, state, force_pct, action):
+        """Keep the actual pre-release curve, including cached-frame timestamps.
+
+        File I/O happens only after a release command has already been submitted.
+        The bounded snapshot replaces the previous event, even in source runs.
+        """
+        decision = self._release_decision
+        self._force_trace.append({
+            "time": now, "state": state, "force_pct": force_pct, "action": action,
+            "feedback": asdict(feedback),
+            "release": None if decision is None else asdict(decision),
+        })
+        if decision is None or not decision.release:
+            return
+        path = self.release_trace_path
+        try:
+            path.parent.mkdir(parents=True, exist_ok=True)
+            payload = {
+                "captured_at": time.time(),
+                "config": {key: value for key, value in vars(self.config).items()
+                           if key.startswith("release_") or key in {
+                               "control_hz", "filter_alpha", "shear_filter_alpha"}},
+                "frames": list(self._force_trace),
+            }
+            temporary = path.with_suffix(".tmp")
+            temporary.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8")
+            temporary.replace(path)
+            print(f"[release-trace] {path}")
+        except (OSError, TypeError, ValueError) as exc:
+            print(f"[release-trace] save failed: {exc}")
+
+    def _record_session_tick(
+        self,
+        now,
+        feedback,
+        state,
+        force_pct,
+        adaptive_target_force,
+        action,
+        trigger,
+        trigger_armed,
+        grip_contact,
+        normal_change,
+        shear_change,
+        control_mode,
+        gripper_position,
+        hold_pos,
+    ):
+        if not self.session_recorder.enabled:
+            return
+        decision = self._release_decision
+        pending = completed_at = None
+        if self.actuator is not None:
+            try:
+                pending, completed_at = self.actuator.motion_status
+            except Exception:
+                pending, completed_at = None, None
+
+        left_sample = 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
+
+        def _raw_force(sample):
+            if not isinstance(sample, dict):
+                return None
+            return {
+                "fnormal": sample.get("fnormal"),
+                "fshearx": sample.get("fshearx"),
+                "fsheary": sample.get("fsheary"),
+                "fps": sample.get("fps"),
+                "timestamp": sample.get("timestamp"),
+            }
+
+        self.session_recorder.record({
+            "t": now,
+            "wall": time.time(),
+            "state": state,
+            "action": action,
+            "control_mode": control_mode,
+            "grip_behavior_mode": getattr(self.config, "grip_behavior_mode", None),
+            "force_pct": force_pct,
+            "adaptive_target_force": adaptive_target_force,
+            "hold_pos": hold_pos,
+            "gripper_position": gripper_position,
+            "trigger": bool(trigger),
+            "trigger_armed": bool(trigger_armed),
+            "grip_contact": bool(grip_contact),
+            "normal_change": normal_change,
+            "shear_change": shear_change,
+            "feedback": asdict(feedback),
+            "raw_left": _raw_force(left_sample),
+            "raw_right": _raw_force(right_sample),
+            "release": None if decision is None else asdict(decision),
+            "release_phase": (
+                None if decision is None else getattr(decision.phase, "value", decision.phase)
+            ),
+            "release_armed": bool(self.release_watch.armed),
+            "side_watch": bool(self.release_watch.side_watch_active),
+            "motion_pending": pending,
+            "motion_completed_at": completed_at,
+        })
 
     def _log_tick(
         self,
@@ -834,7 +1001,21 @@ class SideTriggerGripController:
         release_armed,
         action,
         trigger_armed,
+        now=None,
     ):
+        now = time.perf_counter() if now is None else now
+        decision = self._release_decision
+        release_reason = decision.reason if decision is not None else ""
+        log_key = (state, release_reason, action.split(" ", 1)[0])
+        if (release_reason == "waiting-sample" and self._last_log_key is not None
+                and self._last_log_key[0] == state):
+            # Camera/control rates differ: cached ticks are not state changes.
+            log_key = self._last_log_key
+        if log_key == self._last_log_key and now - self._last_log_time < self._log_interval_s:
+            return
+        self._last_log_key = log_key
+        self._last_log_time = now
+
         left_fps = 0.0
         right_fps = 0.0
         try:
@@ -858,6 +1039,7 @@ class SideTriggerGripController:
             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}"
+            f" side_watch={int(self.release_watch.side_watch_active)}"
         )
 
     def _make_status(
@@ -878,7 +1060,7 @@ class SideTriggerGripController:
         gripper_position=None,
     ):
         speed_pct = self.config.speed
-        if action == "force-change-release-open":
+        if action.startswith("force-change-release-open"):
             speed_pct = self.config.release_open_speed
 
         left_sample = None
@@ -917,8 +1099,16 @@ class SideTriggerGripController:
             "adaptive_target_force": adaptive_target_force,
             "adaptive_grip_enabled": self.config.adaptive_grip_enabled,
             "release_armed": bool(release_armed),
+            "release_phase": self._release_decision.phase.value if self._release_decision else self.release_watch.phase.value,
+            "release_side_watch": self.release_watch.side_watch_active,
+            "release_reason": self._release_decision.reason if self._release_decision else "",
+            "release_normal_rate": self._release_decision.normal_rate if self._release_decision else 0.0,
+            "release_shear_rate": self._release_decision.shear_rate if self._release_decision else 0.0,
+            "release_curve_motion": self._release_decision.curve_motion if self._release_decision else "",
+            "release_channel": self._release_decision.channel if self._release_decision else "",
             "manual_hold_active": state == "manual_hold",
             "control_mode": control_mode,
+            "grip_behavior_mode": getattr(self.config, "grip_behavior_mode", "stable"),
             "speed_pct": speed_pct,
             "gripper_position": gripper_position,
             "open_pos": self.config.open_pos,

gripper/control/feedback.py

@@ -39,6 +39,8 @@ class ForceFeedback:
     left_shear_y: float = 0.0
     right_shear_x: float = 0.0
     right_shear_y: float = 0.0
+    left_timestamp: float | None = None
+    right_timestamp: float | None = None
 
 
 class TactileFeedbackProcessor:
@@ -50,6 +52,7 @@ class TactileFeedbackProcessor:
         self.reset_filters()
 
     def reset_filters(self):
+        self._sample_timestamps = {}
         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)
@@ -59,6 +62,14 @@ class TactileFeedbackProcessor:
         self.right_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha)
         self.right_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha)
 
+    def _filter_sample(self, key, value, timestamp):
+        """A cached camera frame must not advance the filter a second time."""
+        filter_ = getattr(self, f"{key}_filter")
+        if timestamp is None or self._sample_timestamps.get(key) != timestamp:
+            self._sample_timestamps[key] = timestamp
+            return filter_.update(value)
+        return filter_.value
+
     def _convert_shear(self, sample):
         if _component_shear_enabled(self.shear_converter):
             shear_x = self.shear_converter.convert_component(sample["fshearx"])
@@ -142,14 +153,15 @@ class TactileFeedbackProcessor:
                 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)
+        left_timestamp, right_timestamp = left.get("timestamp"), right.get("timestamp")
+        left_normal = self._filter_sample("left_normal", left_force, left_timestamp)
+        right_normal = self._filter_sample("right_normal", right_force, right_timestamp)
+        left_shear_filtered = self._filter_sample("left_shear", left_shear, left_timestamp)
+        right_shear_filtered = self._filter_sample("right_shear", right_shear, right_timestamp)
+        left_shear_x_filtered = self._filter_sample("left_shear_x", left_shear_x, left_timestamp)
+        left_shear_y_filtered = self._filter_sample("left_shear_y", left_shear_y, left_timestamp)
+        right_shear_x_filtered = self._filter_sample("right_shear_x", right_shear_x, right_timestamp)
+        right_shear_y_filtered = self._filter_sample("right_shear_y", right_shear_y, right_timestamp)
 
         return ForceFeedback(
             left_normal=left_normal,
@@ -165,4 +177,6 @@ class TactileFeedbackProcessor:
             left_shear_y=left_shear_y_filtered,
             right_shear_x=right_shear_x_filtered,
             right_shear_y=right_shear_y_filtered,
+            left_timestamp=left_timestamp,
+            right_timestamp=right_timestamp,
         )

gripper/control/force_curve.py

@@ -0,0 +1,211 @@
+"""Recognize a new force onset, rather than accumulated settling movement."""
+
+from collections import deque
+from dataclasses import dataclass, replace
+import math
+from statistics import median
+
+
+@dataclass(frozen=True)
+class _CurveModel:
+    time: float
+    scale: float
+    coefficients: tuple
+    covariance: tuple
+    noise: float
+
+    def terms_at(self, stamp):
+        x = (stamp - self.time) / self.scale
+        return tuple(x ** i for i in range(len(self.coefficients)))
+
+    def predict(self, stamp):
+        return sum(c * x for c, x in zip(self.coefficients, self.terms_at(stamp)))
+
+    def uncertainty(self, stamp):
+        terms = self.terms_at(stamp)
+        variance = sum(a * b * self.covariance[i][j]
+                       for i, a in enumerate(terms) for j, b in enumerate(terms))
+        # Include the new sample's noise, and widen the corridor with forecast
+        # distance. A tiny fitting error must not grow into release evidence.
+        return 4.0 * self.noise * math.sqrt(1.0 + max(0.0, variance))
+
+    def change_uncertainty(self, before, after):
+        terms = tuple(b - a for a, b in zip(self.terms_at(before), self.terms_at(after)))
+        variance = sum(a * b * self.covariance[i][j]
+                       for i, a in enumerate(terms) for j, b in enumerate(terms))
+        return 4.0 * self.noise * math.sqrt(2.0 + max(0.0, variance))
+
+    @property
+    def slope(self):
+        """Physical dy/dt from the linear term of the scaled polynomial."""
+        if len(self.coefficients) < 2 or self.scale <= 0:
+            return 0.0
+        return self.coefficients[1] / self.scale
+
+
+def _fit_curve(history, degree, noise_floor):
+    """Small least-squares fit with its prediction uncertainty (no I/O)."""
+    stamp = history[-1][0]
+    scale = max(stamp - history[0][0], 1e-6)
+    rows = [tuple(((t - stamp) / scale) ** i for i in range(degree + 1))
+            for t, _ in history]
+    size = degree + 1
+    matrix = [[sum(row[i] * row[j] for row in rows) for j in range(size)]
+              + [float(i == j) for j in range(size)] for i in range(size)]
+    # Invert the at-most 3x3 normal matrix. Sensor times have already been
+    # checked for duplicates/restarts before samples enter the history.
+    for column in range(size):
+        pivot = max(range(column, size), key=lambda row: abs(matrix[row][column]))
+        matrix[column], matrix[pivot] = matrix[pivot], matrix[column]
+        divisor = matrix[column][column]
+        if abs(divisor) < 1e-12:
+            return _fit_curve(history, degree - 1, noise_floor)
+        matrix[column] = [value / divisor for value in matrix[column]]
+        for row in range(size):
+            if row != column:
+                multiplier = matrix[row][column]
+                matrix[row] = [a - multiplier * b for a, b in zip(matrix[row], matrix[column])]
+    covariance = tuple(tuple(row[size:]) for row in matrix)
+    target = [sum(row[i] * value for row, (_, value) in zip(rows, history)) for i in range(size)]
+    coefficients = tuple(sum(a * b for a, b in zip(row, target)) for row in covariance)
+    error = sum((value - sum(c * x for c, x in zip(coefficients, row))) ** 2
+                for row, (_, value) in zip(rows, history))
+    noise = max(noise_floor, math.sqrt(error / max(1, len(history) - size)))
+    return _CurveModel(stamp, scale, coefficients, covariance, noise)
+
+
+@dataclass(frozen=True)
+class ForceOnset:
+    time: float
+    origin: float
+    direction: float
+    innovation: float
+    model: _CurveModel
+
+    def baseline_at(self, stamp):
+        return self.model.predict(stamp)
+
+    def significant_at(self, stamp, value):
+        return abs(value - self.baseline_at(stamp)) > self.model.uncertainty(stamp)
+
+
+class ForceCurve:
+    """Local polynomial continuation plus signed slope/turn classification.
+
+    A short recent window still drives onset detection. A longer retained history
+    estimates the settling slope (grip force ramp decaying toward zero).
+    """
+
+    def __init__(self, config):
+        self.config = config
+        history = max(6, int(getattr(config, "release_curve_history", 6) or 6))
+        self.history = deque(maxlen=history)
+        self._noise_history = deque(maxlen=6)
+        self.onset = None
+        self.rate = 0.0
+        self.delta = 0.0
+        self.innovation = 0.0
+        self.motion = "curve_follow"
+        self.fit_slope = 0.0
+
+    def discard_onset(self):
+        self.onset = None
+
+    def _onset_history(self):
+        """Keep onset detection local even when the retained history is long."""
+        onset_points = max(3, min(6, len(self.history)))
+        if len(self.history) <= onset_points:
+            return list(self.history)
+        return list(self.history)[-onset_points:]
+
+    def window_slope(self, now=None, window_s=None):
+        """Linear least-squares slope over a longer settling window (N/s)."""
+        if len(self.history) < 2:
+            return 0.0
+        window_s = float(
+            getattr(self.config, "release_slope_window_seconds", 0.8)
+            if window_s is None
+            else window_s
+        )
+        now = self.history[-1][0] if now is None else now
+        cutoff = now - max(0.05, window_s)
+        points = [(t, v) for t, v in self.history if t >= cutoff]
+        if len(points) < 2:
+            points = list(self.history)
+        floor = min(self.config.release_min_change, self.config.release_side_touch_change)
+        model = _fit_curve(points, min(1, len(points) - 1), max(1e-6, floor * 0.025))
+        return model.slope
+
+    def observe(self, stamp, value):
+        if self.history:
+            if stamp == self.history[-1][0]:
+                return
+            if stamp < self.history[-1][0]:
+                self.history.clear()
+                self._noise_history.clear()
+                self.onset = None
+                self.rate = self.delta = self.innovation = 0.0
+                self.fit_slope = 0.0
+        self.motion = "curve_follow"
+        if not self.history:
+            self.history.append((stamp, value))
+            return
+        previous_time, previous_value = self.history[-1]
+        dt = stamp - previous_time
+        self.delta = value - previous_value
+        self.rate = self.delta / dt
+        previous_rate = 0.0
+        prior_rates = []
+        if len(self.history) >= 2:
+            older_time, older_value = self.history[-2]
+            previous_rate = (previous_value - older_value) / (previous_time - older_time)
+            prior_rates.append(previous_rate)
+            if len(self.history) >= 3:
+                oldest_time, oldest_value = self.history[-3]
+                older_rate = (older_value - oldest_value) / (older_time - oldest_time)
+                prior_rates.append(older_rate)
+        floor = min(self.config.release_min_change, self.config.release_side_touch_change)
+        noise_floor = floor * 0.025
+        onset_history = self._onset_history()
+        model = _fit_curve(onset_history, min(2, len(onset_history) - 1), noise_floor)
+        self._noise_history.append(model.noise)
+        model = replace(model, noise=max(model.noise, median(self._noise_history)))
+        self.innovation = value - model.predict(stamp)
+        # Append first so the settling slope includes the new sample.
+        self.history.append((stamp, value))
+        self.fit_slope = self.window_slope(stamp)
+        # A sign reversal of comparable speed is a turn, not another impulse.
+        turning = (previous_rate * self.rate < 0
+                   and abs(self.rate) <= 1.5 * max((abs(r) for r in prior_rates), default=0.0))
+        decelerating = (previous_rate * self.rate >= 0
+                        and abs(self.rate) <= abs(previous_rate) + 1e-9)
+        onset = self.onset
+        if onset is not None:
+            if self.delta * onset.direction < -floor * 0.1:
+                self.onset = None
+                turning = True
+            elif not onset.significant_at(stamp, value):
+                # Losing evidence before reaching the amplitude threshold is
+                # not permission to accumulate later, unrelated curve travel.
+                self.onset = None
+            elif ((value - onset.origin) * onset.direction < floor
+                  and self.delta * onset.direction < floor * 0.1):
+                # A sub-threshold onset which has already flattened is over.
+                # Do not let the forecast alone grow a small bump into a later
+                # candidate while the real signal resumes its natural curve.
+                self.onset = None
+            elif stamp - onset.time > self.config.release_touch_window_seconds:
+                self.onset = None
+        if turning:
+            self.motion = "settling_turn"
+        elif decelerating and abs(previous_rate) > 1e-9:
+            self.motion = "settling_flatten"
+
+        if (self.onset is None and not turning and not decelerating
+                and abs(self.innovation) + 1e-9 >= floor * 0.2
+                and abs(self.innovation) > model.uncertainty(stamp)
+                and self.innovation * self.delta > 0):
+            self.onset = ForceOnset(stamp, previous_value,
+                                    1.0 if self.innovation > 0 else -1.0, self.innovation,
+                                    model)
+            self.motion = "new_onset"

gripper/control/release_watch.py

@@ -0,0 +1,598 @@
+"""Release FSM. No clock, serial I/O or callbacks: step() returns a decision."""
+
+from dataclasses import dataclass
+from enum import Enum
+import math
+
+from .force_curve import ForceCurve, ForceOnset
+
+
+CHANNELS = ("left_normal", "right_normal", "left_shear", "right_shear")
+
+
+class ReleasePhase(str, Enum):
+    DISARMED = "disarmed"
+    GRIP_SETTLE = "grip_settle"
+    SELF_SETTLE = "self_settle"
+    WATCHING = "watching"
+    CONFIRMING = "confirming"
+    RELEASED = "released"
+
+
+@dataclass(frozen=True)
+class ReleaseDecision:
+    phase: ReleasePhase
+    reason: str
+    release: bool = False
+    allow_force_update: bool = False
+    normal_change: float = 0.0
+    shear_change: float = 0.0
+    normal_rate: float = 0.0
+    shear_rate: float = 0.0
+    confirm_count: int = 0
+    confirm_needed: int = 0
+    channel: str = ""
+    innovation: float = 0.0
+    curve_motion: str = ""
+
+    @property
+    def action(self):
+        return (
+            f"release-{self.reason} phase={self.phase.value} "
+            f"confirm={self.confirm_count}/{self.confirm_needed} "
+            f"rN={self.normal_rate:.2f} rS={self.shear_rate:.2f}"
+            f" channel={self.channel or '-'} innovation={self.innovation:.4f} curve={self.curve_motion or '-'}"
+        )
+
+
+@dataclass
+class _Candidate:
+    kind: str
+    channel: str
+    origin: float
+    direction: float
+    peak: float
+    started_at: float
+    needed: int
+    floor: float
+    count: int = 1
+    onset: ForceOnset | None = None
+    last_displacement: float = 0.0
+    last_sample_time: float = 0.0
+
+
+@dataclass
+class _Rebound:
+    origin: float
+    direction: float
+    remaining: float
+
+
+def _returning_tail(tails, key, value, floor):
+    """Suppress only a rejected pulse returning towards its original baseline.
+
+    This is channel-local and ends on return or renewed movement. It does not
+    wait for unrelated channels to become quiet or impose a new hold timer.
+    """
+    tail = tails.get(key)
+    if tail is None:
+        return False
+    remaining = (value - tail.origin) * tail.direction
+    if abs(remaining) <= floor * 0.5:
+        del tails[key]
+        return True
+    if remaining < 0 or remaining > tail.remaining + floor * 0.25:
+        del tails[key]
+        return False
+    tail.remaining = min(tail.remaining, remaining)
+    return True
+
+
+class _SideTouch:
+    """Observe a new lateral touch even during clamp protection.
+
+    Bilateral common-mode force is cancelled. A vote needs new frames from
+    both sides so asynchronous camera updates cannot fabricate a difference.
+    Normal force stays masked during an actual force command and its tail;
+    differential shear remains available without a contact timer.
+    """
+
+    def __init__(self, config):
+        self.config = config
+        self.reset()
+
+    def reset(self):
+        self.last_pair = None
+        self.seen = set()
+        self.candidate = None
+        self.recover_until = 0.0
+        self.tails = {}
+        self.curves = {index: ForceCurve(self.config) for index in (1, 2)}
+
+    def seed(self, now, values):
+        self.last_pair = (now, values["left_normal"] - values["right_normal"],
+                          values["left_shear"] - values["right_shear"])
+        for index in (1, 2):
+            if not self.curves[index].history:
+                self.curves[index].observe(now, self.last_pair[index])
+
+    def prime(self, now, values):
+        for index, field in ((1, "normal"), (2, "shear")):
+            self.curves[index].observe(now, values[f"left_{field}"] - values[f"right_{field}"])
+            self.curves[index].discard_onset()
+
+    def step(self, now, values, fresh, enabled, normal_masked, sample_time,
+             onset_allowed=True):
+        self.seen.update(fresh)
+        if not all(key in self.seen for key in CHANNELS):
+            return None
+        self.seen.clear()
+        sample = (sample_time, values["left_normal"] - values["right_normal"],
+                  values["left_shear"] - values["right_shear"])
+        for index in (1, 2):
+            self.curves[index].observe(sample_time, sample[index])
+        if normal_masked:
+            self.curves[1].discard_onset()
+        if not enabled or now < self.recover_until:
+            self.seed(sample_time, values)
+            self.candidate = None
+            for curve in self.curves.values():
+                curve.discard_onset()
+            return None
+        self.last_pair = sample
+        # Stable grip: keep learning the long ramp curve, but do not arm side
+        # touch while the settling slope is still decaying toward zero.
+        if not onset_allowed:
+            self.candidate = None
+            for curve in self.curves.values():
+                curve.discard_onset()
+            return None
+        # Drop leftover candidates that the profile has disabled.
+        if self.candidate is not None:
+            if (self.candidate.channel == "side_shear"
+                    and not getattr(self.config, "release_side_shear_enabled", True)):
+                self.candidate = None
+                self.curves[2].discard_onset()
+            elif (self.candidate.channel == "side_normal"
+                  and not getattr(self.config, "release_side_normal_enabled", True)):
+                self.candidate = None
+                self.curves[1].discard_onset()
+        returning = {index for index in (1, 2) if _returning_tail(
+            self.tails, index, sample[index], self.config.release_side_touch_change)}
+        if returning:
+            for index in returning:
+                self.curves[index].discard_onset()
+        candidate = self.candidate
+        if candidate is not None:
+            index = 1 if candidate.channel == "side_normal" else 2
+            baseline = candidate.onset.baseline_at(sample_time)
+            delta = (sample[index] - baseline) * candidate.direction
+            return_margin = max(self.config.release_min_change * 0.1,
+                                candidate.onset.model.change_uncertainty(candidate.last_sample_time, sample_time))
+            turning = delta < candidate.last_displacement - return_margin
+            if (now - candidate.started_at > self.config.release_confirm_window_seconds
+                    or delta + 1e-9 < max(candidate.floor, 0.6 * candidate.peak)
+                    or not candidate.onset.significant_at(sample_time, sample[index])
+                    or (normal_masked and index == 1) or turning):
+                self.candidate = None
+                self.tails[index] = _Rebound(baseline, candidate.direction, delta)
+                self.recover_until = now + self.config.release_settle_seconds
+                self.seed(sample_time, values)
+                self.curves[index].discard_onset()
+                return "settling-turn" if turning else "rejected"
+            candidate.peak = max(candidate.peak, delta)
+            candidate.last_displacement = delta
+            candidate.last_sample_time = sample_time
+            candidate.count += 1
+        else:
+            shear_ok = getattr(self.config, "release_side_shear_enabled", True)
+            normal_ok = getattr(self.config, "release_side_normal_enabled", True)
+            channels = []
+            if shear_ok:
+                channels.append((2, "side_shear"))
+            if normal_ok:
+                channels.append((1, "side_normal"))
+            for index, channel in channels:
+                if index in returning or (normal_masked and index == 1):
+                    continue
+                onset = self.curves[index].onset
+                if onset is None:
+                    continue
+                delta = sample[index] - onset.baseline_at(sample_time)
+                if (abs(delta) + 1e-9 >= self.config.release_side_touch_change
+                        and (sample[index] - onset.origin) * onset.direction + 1e-9 >= self.config.release_side_touch_change
+                        and onset.significant_at(sample_time, sample[index])
+                        and delta * onset.direction > 0):
+                    candidate = _Candidate("side_touch", channel, onset.origin,
+                                           1.0 if delta > 0 else -1.0, abs(delta), now,
+                                           max(2, self.config.release_confirm_samples),
+                                           self.config.release_side_touch_change,
+                                           onset=onset, last_displacement=abs(delta), last_sample_time=sample_time)
+                    self.candidate = candidate
+                    break
+        return candidate
+
+
+class ReleaseWatch:
+    """Immediate side touch + bounded clamp protection + persistent displacement.
+
+    A force command masks normal changes through execution and a short tail.
+    Lateral shear remains observable; global quiet never gates release.
+    New force commands require an observation window, so repeated adaptive
+    commands cannot continuously re-arm a timer or erase release evidence.
+    Sensor timestamps are optional for replay; production supplies both sides.
+    """
+
+    def __init__(self, config):
+        self.config = config
+        self._side_touch = _SideTouch(config)
+        self.reset()
+
+    def reset(self):
+        self.phase = ReleasePhase.DISARMED
+        self._contact_at = None
+        self._grip_settled = False
+        self._slope_settle_since = None
+        self._plateau_latched = False
+        self._reference = {}
+        self._previous = {}
+        self._recovery_until = 0.0
+        self._settle_seen = set()
+        self._tails = {}
+        self._watch_since = None
+        self._candidate = None
+        self._last_fresh = {}
+        self._rates = {}
+        self._curves = {key: ForceCurve(self.config) for key in CHANNELS}
+        self._side_touch.reset()
+
+    @property
+    def armed(self):
+        return (self.phase in {ReleasePhase.WATCHING, ReleasePhase.CONFIRMING}
+                or self._side_touch.candidate is not None)
+
+    @property
+    def side_watch_active(self):
+        return (self.phase not in {ReleasePhase.DISARMED, ReleasePhase.RELEASED}
+                and self._side_touch.last_pair is not None and len(self._previous) == len(CHANNELS))
+
+    def contact(self, now, feedback=None, history=()):
+        self.reset()
+        self._contact_at = now
+        self.phase = ReleasePhase.GRIP_SETTLE
+        for frame_time, frame in history:
+            values = {key: float(getattr(frame, key)) for key in CHANNELS}
+            for key, value in values.items():
+                stamp = getattr(frame, f"{key.split('_', 1)[0]}_timestamp", None)
+                self._curves[key].observe(frame_time if stamp is None else stamp, value)
+                self._curves[key].discard_onset()
+            self._side_touch.prime(self._pair_time(frame_time, frame), values)
+        if feedback is not None:
+            self._side_touch.seed(self._pair_time(now, feedback), {key: float(getattr(feedback, key)) for key in CHANNELS})
+            for key in CHANNELS:
+                stamp = getattr(feedback, f"{key.split('_', 1)[0]}_timestamp", None)
+                self._curves[key].observe(now if stamp is None else stamp, float(getattr(feedback, key)))
+                self._curves[key].discard_onset()
+
+    @staticmethod
+    def _pair_time(now, feedback):
+        stamps = (getattr(feedback, "left_timestamp", None), getattr(feedback, "right_timestamp", None))
+        return now if None in stamps else max(stamps)
+
+    def suspend(self):
+        """Discard evidence during missing feedback, sensor restart or manual mode."""
+        if self.phase in {ReleasePhase.DISARMED, ReleasePhase.RELEASED}:
+            return
+        self._protect("sample-gap", {})
+        self._side_touch.reset()
+        self._reference.clear()
+        self._previous.clear()
+        self._rates.clear()
+        self._last_fresh.clear()
+        self._tails.clear()
+        self._curves = {key: ForceCurve(self.config) for key in CHANNELS}
+
+    def _normal_slopes_live_ok(self, now):
+        """Live check: normal long-window slopes are near zero long enough."""
+        if not getattr(self.config, "release_require_slope_settle", False):
+            return True
+        threshold = max(0.0, float(getattr(self.config, "release_slope_settle_rate", 0.2) or 0.2))
+        min_points = max(3, int(getattr(self.config, "release_slope_min_points", 8) or 8))
+        hold = max(0.0, float(getattr(self.config, "release_slope_hold_seconds", 0.0) or 0.0))
+        normal_only = getattr(self.config, "release_settle_normal_only", True)
+        keys = CHANNELS[:2] if normal_only else CHANNELS
+        for key in keys:
+            curve = self._curves[key]
+            if len(curve.history) < min_points or abs(curve.fit_slope) > threshold:
+                self._slope_settle_since = None
+                return False
+        if self._slope_settle_since is None:
+            self._slope_settle_since = now
+        if hold > 0 and now - self._slope_settle_since < hold:
+            return False
+        return True
+
+    def _force_settled(self, now):
+        """Release-armed after the first relative normal plateau (latched).
+
+        The live slope near zero is only required to *enter* the armed state.
+        Once latched, further adaptive 加力 or an intentional open push may make
+        the slope non-zero again — that must not revoke release permission, or
+        the open gesture is mistaken for "still settling".
+        """
+        if not getattr(self.config, "release_require_slope_settle", False):
+            self._plateau_latched = True
+            return True
+        if self._plateau_latched:
+            return True
+        if self._normal_slopes_live_ok(now):
+            self._plateau_latched = True
+            return True
+        return False
+
+    def _protect(self, reason, values, **metrics):
+        self.phase = (
+            ReleasePhase.SELF_SETTLE if self._grip_settled
+            else ReleasePhase.GRIP_SETTLE
+        )
+        self._reference.update(values)
+        self._candidate = None
+        self._watch_since = None
+        self._settle_seen.clear()
+        for curve in self._curves.values():
+            curve.discard_onset()
+        return ReleaseDecision(self.phase, reason, **metrics)
+
+    def step(self, now, feedback, *, motion_pending=False, motion_completed_at=None):
+        if self.phase in {ReleasePhase.DISARMED, ReleasePhase.RELEASED}:
+            return ReleaseDecision(self.phase, self.phase.value)
+
+        cfg = self.config
+        timeout = max(0.001, cfg.release_sample_timeout_seconds)
+        values = {key: float(getattr(feedback, key)) for key in CHANNELS}
+        if not all(math.isfinite(value) for value in values.values()):
+            self._previous.clear()
+            self._rates.clear()
+            self._side_touch.reset()
+            self._curves = {key: ForceCurve(cfg) for key in CHANNELS}
+            self._recovery_until = now + cfg.release_settle_seconds
+            return self._protect("invalid-sample", {})
+
+        rates, fresh = {}, set()
+        discontinuity = False
+        for key, value in values.items():
+            side = key.split("_", 1)[0]
+            stamp = getattr(feedback, f"{side}_timestamp", None)
+            stamp = now if stamp is None else stamp
+            previous = self._previous.get(key)
+            if previous is not None and stamp == previous[0]:
+                continue
+            fresh.add(key)
+            self._last_fresh[key] = now
+            if previous is not None:
+                dt = stamp - previous[0]
+                if 0 < dt <= timeout:
+                    rates[key] = (value - previous[1]) / dt
+                else:
+                    discontinuity = True
+            self._previous[key] = (stamp, value)
+        self._rates.update(rates)
+
+        normal_rate = max((abs(rates.get(k, 0.0)) for k in CHANNELS[:2]), default=0.0)
+        shear_rate = max((abs(rates.get(k, 0.0)) for k in CHANNELS[2:]), default=0.0)
+        normal_change = max(abs(values[k] - self._reference.get(k, values[k])) for k in CHANNELS[:2])
+        shear_change = max(abs(values[k] - self._reference.get(k, values[k])) for k in CHANNELS[2:])
+        metrics = dict(normal_change=normal_change, shear_change=shear_change,
+                       normal_rate=normal_rate, shear_rate=shear_rate)
+        stale = any(now - self._last_fresh.get(k, now) > timeout for k in CHANNELS)
+        if stale or discontinuity:
+            self._side_touch.reset()
+            self._curves = {key: ForceCurve(cfg) for key in CHANNELS}
+            self._recovery_until = now + cfg.release_settle_seconds
+            return self._protect("sample-gap", values, **metrics)
+
+        for key in fresh:
+            self._curves[key].observe(self._previous[key][0], values[key])
+        motions = {curve.motion for curve in self._curves.values()}
+        metrics["curve_motion"] = ("new_onset" if "new_onset" in motions else
+                                   "settling_turn" if "settling_turn" in motions else
+                                   "settling_flatten" if "settling_flatten" in motions else "curve_follow")
+
+        self_tail = (
+            motion_completed_at is not None
+            and now - motion_completed_at < max(0.0, cfg.release_arm_delay_seconds)
+        )
+        contact_guard = now - self._contact_at < max(0.0, cfg.release_min_hold_seconds)
+        protected = motion_pending or self_tail or contact_guard or self.phase in {
+            ReleasePhase.GRIP_SETTLE, ReleasePhase.SELF_SETTLE,
+        }
+        force_settled = self._force_settled(now)
+        if not force_settled:
+            metrics["curve_motion"] = "force_settling"
+        side = self._side_touch.step(now, values, fresh,
+                                     enabled=protected or self._side_touch.candidate is not None,
+                                     normal_masked=motion_pending or self_tail,
+                                     sample_time=self._pair_time(now, feedback),
+                                     onset_allowed=force_settled)
+        if side in ("rejected", "settling-turn"):
+            reason = "settling-turn" if side == "settling-turn" else "transient-rejected"
+            return self._cancel(now, values, reason, metrics, reset_side=False)
+        if side is not None:
+            self._candidate = None
+            released = side.count >= side.needed
+            if released:
+                self.phase = ReleasePhase.RELEASED
+            return ReleaseDecision(ReleasePhase.RELEASED if released else ReleasePhase.CONFIRMING,
+                                   "side_touch", release=released, confirm_count=side.count,
+                                   confirm_needed=side.needed, channel=side.channel,
+                                   innovation=side.onset.innovation,
+                                   **dict(metrics, curve_motion="onset_confirm"))
+
+        slope_gate = getattr(cfg, "release_require_slope_settle", False)
+        # Contact min-hold always clamps. Self-motion clamps until the first
+        # normal plateau is latched (stable) — or always when slope gate is off.
+        if contact_guard or ((motion_pending or self_tail) and (not slope_gate or not force_settled)):
+            reason = ("force-settling" if slope_gate and not force_settled
+                      else "self-settle" if self._grip_settled else "grip-settle")
+            return self._protect(reason, values, **metrics)
+        if slope_gate and force_settled and (motion_pending or self_tail):
+            # Adaptive 加力 after plateau: keep watching for intentional open.
+            if self.phase in {ReleasePhase.GRIP_SETTLE, ReleasePhase.SELF_SETTLE}:
+                self._grip_settled = True
+                self.phase = ReleasePhase.WATCHING
+                if self._watch_since is None:
+                    self._watch_since = now
+                for curve in self._curves.values():
+                    curve.discard_onset()
+
+        if not fresh:
+            return ReleaseDecision(self.phase, "waiting-sample", **metrics)
+
+        # Quiet is diagnostic only. No global "all four rates quiet" arming
+        # gate: even small continuous camera noise can keep that gate shut.
+        quiet = (
+            len(self._rates) == len(CHANNELS)
+            and max(abs(rate) for rate in self._rates.values()) <= cfg.release_slow_rate
+        )
+        if self.phase in {ReleasePhase.GRIP_SETTLE, ReleasePhase.SELF_SETTLE}:
+            self._settle_seen.update(fresh)
+            self._reference.update(values)
+            if now < self._recovery_until or len(self._settle_seen) < len(CHANNELS):
+                reason = "self-settle" if self._grip_settled else "grip-settle"
+                return ReleaseDecision(self.phase, reason, **metrics)
+            # Stable mode waits until the long-history slope has decayed near
+            # zero before ordinary watching / release paths arm.
+            if not force_settled:
+                return ReleaseDecision(self.phase, "force-settling", **metrics)
+            self._grip_settled = True
+            self.phase = ReleasePhase.WATCHING
+            self._watch_since = now
+            for curve in self._curves.values():
+                curve.discard_onset()
+            return ReleaseDecision(self.phase, "ready", **metrics)
+        returning = {key for key in fresh if _returning_tail(
+            self._tails, key, values[key], cfg.release_min_change)}
+        for key in returning:
+            self._reference[key] = values[key]
+            self._curves[key].discard_onset()
+
+        # Drop watching candidates on channel families the profile disabled.
+        if (self._candidate is not None
+                and self._candidate.channel.endswith("normal")
+                and not getattr(cfg, "release_normal_onset_enabled", True)):
+            self._candidate = None
+            if self._grip_settled:
+                self.phase = ReleasePhase.WATCHING
+        if (self._candidate is not None
+                and self._candidate.channel.endswith("shear")
+                and not getattr(cfg, "release_shear_onset_enabled", True)):
+            self._candidate = None
+            if self._grip_settled:
+                self.phase = ReleasePhase.WATCHING
+
+        candidate = self._candidate
+        if candidate is not None:
+            if now - candidate.started_at > cfg.release_confirm_window_seconds:
+                return self._cancel(now, values, "confirm-expired", metrics)
+            if candidate.channel not in fresh:
+                return ReleaseDecision(self.phase, "waiting-sample", **metrics)
+            stamp = self._previous[candidate.channel][0]
+            displacement = (values[candidate.channel] - candidate.onset.baseline_at(stamp)) * candidate.direction
+            return_margin = max(cfg.release_min_change * 0.1,
+                                candidate.onset.model.change_uncertainty(candidate.last_sample_time, stamp))
+            if displacement < candidate.last_displacement - return_margin:
+                return self._cancel(now, values, "settling-turn", metrics)
+            # A spike returning to baseline, reversal, or channel swap is not a
+            # second vote. A step that stays displaced is, even with dF/dt=0.
+            if displacement + 1e-9 < max(candidate.floor, 0.6 * candidate.peak):
+                return self._cancel(now, values, "transient-rejected", metrics)
+            if not candidate.onset.significant_at(stamp, values[candidate.channel]):
+                return self._cancel(now, values, "transient-rejected", metrics)
+            candidate.peak = max(candidate.peak, displacement)
+            candidate.last_displacement = displacement
+            candidate.last_sample_time = stamp
+            candidate.count += 1
+        else:
+            choices = []
+            for key in fresh:
+                if key in returning:
+                    continue
+                is_normal = key.endswith("normal")
+                if is_normal and not getattr(cfg, "release_normal_onset_enabled", True):
+                    continue
+                if (not is_normal
+                        and not getattr(cfg, "release_shear_onset_enabled", True)):
+                    continue
+                onset = self._curves[key].onset
+                if onset is None or not onset.significant_at(self._previous[key][0], values[key]):
+                    continue
+                rate_limit = cfg.release_normal_rate if is_normal else cfg.release_shear_rate
+                weak_limit = cfg.release_normal_change if is_normal else cfg.release_shear_change
+                delta = values[key] - onset.baseline_at(self._previous[key][0])
+                rate = rates.get(key, 0.0)
+                kind = None
+                origin = onset.origin
+                measured = (values[key] - origin) * onset.direction
+                if (abs(rate) >= rate_limit and abs(delta) >= cfg.release_min_change
+                        and measured + 1e-9 >= cfg.release_min_change and rate * delta > 0):
+                    kind, needed, floor = "sudden", max(2, cfg.release_confirm_samples), cfg.release_min_change
+                elif (abs(delta) + 1e-9 >= cfg.release_min_change
+                      and measured + 1e-9 >= cfg.release_min_change and delta * onset.direction > 0):
+                    kind, needed, floor = "touch", max(2, cfg.release_confirm_samples), cfg.release_min_change
+                elif abs(delta) >= weak_limit and measured >= weak_limit:
+                    kind = "slow_external"
+                    needed = max(2, cfg.release_confirm_samples, cfg.release_slow_confirm_samples)
+                    floor = weak_limit
+                if kind:
+                    choices.append((kind == "sudden", abs(delta), key, kind, needed, floor, delta, origin, onset))
+            if choices:
+                _, _, key, kind, needed, floor, delta, origin, onset = max(choices)
+                candidate = _Candidate(kind, key, origin,
+                                       1.0 if delta > 0 else -1.0, abs(delta), now, needed, floor,
+                                       onset=onset, last_displacement=abs(delta),
+                                       last_sample_time=self._previous[key][0])
+                self._candidate = candidate
+                self.phase = ReleasePhase.CONFIRMING
+
+        if candidate is not None:
+            released = candidate.count >= candidate.needed
+            if released:
+                self.phase = ReleasePhase.RELEASED
+            return ReleaseDecision(self.phase, candidate.kind, release=released,
+                                   confirm_count=candidate.count,
+                                   confirm_needed=candidate.needed, channel=candidate.channel,
+                                   innovation=candidate.onset.innovation,
+                                   **dict(metrics, curve_motion="onset_confirm"))
+
+        # Follow curve continuation, including large smooth turns. Displacement
+        # without a new onset must never become a weak-path release later.
+        for key in fresh:
+            if self._curves[key].onset is None:
+                self._reference[key] = values[key]
+        observation = max(0.1, max(2, cfg.release_confirm_samples) / max(1.0, cfg.control_hz))
+        # Ordinary noise must not prevent clamp force from reaching its target.
+        # Meaningful coherent movement reserves the next frame for release.
+        touch_pending = any(
+            self._curves[key].onset is not None
+            and abs(values[key] - self._curves[key].onset.baseline_at(self._previous[key][0])) >= cfg.release_min_change * 0.5
+            for key in fresh
+        )
+        allow_force = (not touch_pending and self._watch_since is not None
+                       and now - self._watch_since >= observation)
+        reason = "settling-turn" if metrics["curve_motion"] == "settling_turn" else "slow_internal" if quiet else "watch"
+        return ReleaseDecision(self.phase, reason,
+                               allow_force_update=allow_force, **metrics)
+
+    def _cancel(self, now, values, reason, metrics, *, reset_side=True):
+        # Wait for the rejected pulse's tail; otherwise its falling edge can
+        # become a fresh negative-direction candidate and release on rebound.
+        self._recovery_until = now + self.config.release_settle_seconds
+        if self._candidate is not None:
+            candidate = self._candidate
+            baseline = candidate.onset.baseline_at(self._previous[candidate.channel][0])
+            remaining = (values[candidate.channel] - baseline) * candidate.direction
+            self._tails[candidate.channel] = _Rebound(baseline, candidate.direction, remaining)
+        if reset_side:
+            self._side_touch.reset()
+            self._side_touch.recover_until = self._recovery_until
+        return self._protect(reason, values, **metrics)

gripper/control/session_recorder.py

@@ -0,0 +1,124 @@
+"""Session recorder for grip/release tuning.
+
+Writes one JSON object per control tick (JSONL). Enable from UI or config; keep
+I/O off the hot path when disabled.
+"""
+
+from __future__ import annotations
+
+import json
+import threading
+import time
+from dataclasses import asdict, is_dataclass
+from datetime import datetime
+from pathlib import Path
+
+
+def _jsonable(value):
+    if value is None or isinstance(value, (bool, int, float, str)):
+        return value
+    if is_dataclass(value):
+        return {key: _jsonable(item) for key, item in asdict(value).items()}
+    if isinstance(value, dict):
+        return {str(key): _jsonable(item) for key, item in value.items()}
+    if isinstance(value, (list, tuple)):
+        return [_jsonable(item) for item in value]
+    if hasattr(value, "value") and not callable(value):
+        # Enum-like
+        try:
+            return value.value
+        except Exception:
+            pass
+    return str(value)
+
+
+class SessionRecorder:
+    def __init__(self, root: Path):
+        self.root = Path(root)
+        self._lock = threading.Lock()
+        self._enabled = False
+        self._path = None
+        self._file = None
+        self._frames = 0
+        self._started_at = None
+        self._meta = {}
+
+    @property
+    def enabled(self) -> bool:
+        with self._lock:
+            return self._enabled
+
+    @property
+    def path(self):
+        with self._lock:
+            return self._path
+
+    @property
+    def frames(self) -> int:
+        with self._lock:
+            return self._frames
+
+    def start(self, meta=None) -> Path:
+        with self._lock:
+            if self._enabled:
+                return self._path
+            self.root.mkdir(parents=True, exist_ok=True)
+            stamp = datetime.now().strftime("%Y%m%d-%H%M%S")
+            path = self.root / f"session-{stamp}.jsonl"
+            handle = path.open("w", encoding="utf-8")
+            header = {
+                "type": "session_header",
+                "started_at": time.time(),
+                "started_at_iso": datetime.now().isoformat(timespec="seconds"),
+                "meta": _jsonable(meta or {}),
+            }
+            handle.write(json.dumps(header, ensure_ascii=False) + "\n")
+            handle.flush()
+            self._file = handle
+            self._path = path
+            self._enabled = True
+            self._frames = 0
+            self._started_at = time.perf_counter()
+            self._meta = dict(meta or {})
+            print(f"[session-record] start {path}")
+            return path
+
+    def stop(self) -> Path | None:
+        with self._lock:
+            if not self._enabled:
+                return None
+            path = self._path
+            frames = self._frames
+            started = self._started_at
+            try:
+                footer = {
+                    "type": "session_footer",
+                    "stopped_at": time.time(),
+                    "frames": frames,
+                    "duration_s": (
+                        None if started is None else time.perf_counter() - started
+                    ),
+                }
+                if self._file is not None:
+                    self._file.write(json.dumps(footer, ensure_ascii=False) + "\n")
+                    self._file.flush()
+                    self._file.close()
+            finally:
+                self._file = None
+                self._enabled = False
+                self._path = None
+                self._frames = 0
+                self._started_at = None
+            print(f"[session-record] stop {path} frames={frames}")
+            return path
+
+    def record(self, frame: dict) -> None:
+        with self._lock:
+            if not self._enabled or self._file is None:
+                return
+            payload = {"type": "tick", **_jsonable(frame)}
+            self._file.write(json.dumps(payload, ensure_ascii=False) + "\n")
+            self._frames += 1
+            # Flush periodically so a crash still keeps most of the capture.
+            if self._frames % 30 == 0:
+                self._file.flush()