diff --git a/config/compound_eye.png b/config/compound_eye.png index 558b9cc..74499d8 100644 Binary files a/config/compound_eye.png and b/config/compound_eye.png differ diff --git a/config/gripper.json b/config/gripper.json index d6fe0e8..4c1969c 100644 --- a/config/gripper.json +++ b/config/gripper.json @@ -12,16 +12,16 @@ "serial_timeout": 0.2, "open_pos": 0, "close_pos": 9000, - "speed": 35, + "speed": 30, "accel": 70, "decel": 70, "release_open_speed": 55, "release_open_accel": 70, "release_open_decel": 70, "open_force": 25, - "close_force": 33, - "grip_start_force": 25, - "hold_force": 33, + "close_force": 70, + "grip_start_force": 70, + "hold_force": 70, "initial_force": 25, "force_min": 20, "force_max": 100, @@ -40,52 +40,52 @@ "grip_requires_both": true, "close_timeout_seconds": 3.5, "closing_command_interval": 0.2, - "force_ramp_step": 1, - "force_ramp_interval": 0.1, + "force_ramp_step": 5, + "force_ramp_interval": 0.03, "grip_settle_seconds": 0.2, - "grip_behavior_mode": "sensitive", + "grip_behavior_mode": "stable", "adaptive_grip_enabled": true, "adaptive_shear_start_force": 0.1, "adaptive_shear_full_force": 0.6, - "adaptive_force_min": 33, - "adaptive_force_max": 80, + "adaptive_force_min": 70, + "adaptive_force_max": 70, "adaptive_force_step": 5, - "adaptive_force_interval": 0.08, - "release_arm_delay_seconds": 0.28, - "release_min_hold_seconds": 0.65, - "release_settle_seconds": 0.12, - "release_min_change": 0.08, - "release_touch_window_seconds": 0.14, - "release_side_touch_change": 0.12, - "release_confirm_window_seconds": 0.14, - "release_sample_timeout_seconds": 0.2, - "release_confirm_samples": 4, - "release_slow_confirm_samples": 8, - "release_normal_rate": 2.6, - "release_shear_rate": 2.4, - "release_slow_rate": 0.22, - "release_normal_change": 0.42, - "release_shear_change": 0.35, - "release_curve_history": 30, - "release_slope_window_seconds": 0.8, - "release_slope_settle_rate": 0.22, - "release_slope_min_points": 10, - "release_require_slope_settle": true, - "release_slope_hold_seconds": 0.45, + "adaptive_force_interval": 0.03, + "release_arm_delay_seconds": 0.55, + "release_min_hold_seconds": 2.0, + "release_settle_seconds": 0.22, + "release_min_change": 0.12, + "release_touch_window_seconds": 0.22, + "release_side_touch_change": 0.24, + "release_confirm_window_seconds": 0.28, + "release_sample_timeout_seconds": 0.3, + "release_confirm_samples": 8, + "release_slow_confirm_samples": 12, + "release_normal_rate": 4.0, + "release_shear_rate": 3.6, + "release_slow_rate": 0.14, + "release_normal_change": 0.7, + "release_shear_change": 0.6, + "release_curve_history": 42, + "release_slope_window_seconds": 1.2, + "release_slope_settle_rate": 0.12, + "release_slope_min_points": 14, + "release_require_slope_settle": false, + "release_slope_hold_seconds": 0.9, "release_settle_normal_only": true, - "release_shear_oscillation_open": true, - "release_shear_oscillation_window_seconds": 0.4, - "release_shear_oscillation_min_flips": 3, - "release_shear_oscillation_amplitude": 0.15, - "release_shear_oscillation_confirm_samples": 2, + "release_shear_oscillation_open": false, + "release_shear_oscillation_window_seconds": 0.6, + "release_shear_oscillation_min_flips": 5, + "release_shear_oscillation_amplitude": 0.3, + "release_shear_oscillation_confirm_samples": 4, "release_side_shear_enabled": true, - "release_side_normal_enabled": true, + "release_side_normal_enabled": false, "release_shear_onset_enabled": true, - "release_normal_onset_enabled": true, - "rearm_seconds": 0.55, + "release_normal_onset_enabled": false, + "rearm_seconds": 0.9, "recover_normal_force": 0.02, "recover_shear_force": 0.02, - "recover_stable_seconds": 0.55, + "recover_stable_seconds": 0.9, "open_position_recover_enabled": true, "open_position_tolerance": 100, "open_position_stable_seconds": 0.1, @@ -140,10 +140,10 @@ } ] }, - "release_guard_fast_paths": false, - "grip_contact_hold_position": true, - "release_shear_axes": "", - "release_shear_displacement": 0.0, + "release_guard_fast_paths": true, + "grip_contact_hold_position": false, + "release_shear_axes": "y", + "release_shear_displacement": 0.6, "release_shear_displacement_samples": 6, - "release_object_lost_open": false + "release_object_lost_open": true } diff --git a/gripper/config/behavior_profiles.py b/gripper/config/behavior_profiles.py index 616c20f..b228b49 100644 --- a/gripper/config/behavior_profiles.py +++ b/gripper/config/behavior_profiles.py @@ -28,6 +28,7 @@ GRIP_TRIGGER_KEYS = ( # Keys rewritten by the behavior mode. Trigger params are not included — # they stay locked to the sensitive profile via GRIP_TRIGGER_KEYS. GRIP_BEHAVIOR_KEYS = ( + "speed", "grip_contact_hold_position", "close_force", "grip_start_force", @@ -123,6 +124,7 @@ def _constant_force(pct): _SENSITIVE_PROFILE = { **_SENSITIVE_TRIGGER, **_ADAPTIVE_FORCE, + "speed": 55, "release_guard_fast_paths": False, "release_shear_axes": "", "release_shear_displacement": 0.0, @@ -169,6 +171,7 @@ _SENSITIVE_PROFILE = { _STABLE_PROFILE = { **_constant_force(70), + "speed": 30, "release_guard_fast_paths": True, # Release looks at Y shear only, regardless of the global control axes. "release_shear_axes": "y", diff --git a/gripper/control/controller.py b/gripper/control/controller.py index b63b0f9..bc3c6a1 100644 --- a/gripper/control/controller.py +++ b/gripper/control/controller.py @@ -7,7 +7,8 @@ from dataclasses import asdict, replace from ..common.filters import clamp from ..common.paths import REPO_ROOT from .actuator import ActuatorWorker -from .release_watch import ReleasePhase, ReleaseWatch +from .release_dispatch import ModeReleaseWatch +from .release_watch import ReleasePhase from .session_recorder import SessionRecorder @@ -28,7 +29,7 @@ class SideTriggerGripController: self.shear_unit = feedback_processor.shear_converter.unit self.status_callback = status_callback self.actuator = None - self.release_watch = ReleaseWatch(config) + self.release_watch = ModeReleaseWatch(config) self._release_decision = None self._force_trace = deque(maxlen=120) self.release_trace_path = REPO_ROOT / "logs" / "release-last.json" diff --git a/gripper/control/release_dispatch.py b/gripper/control/release_dispatch.py new file mode 100644 index 0000000..a5fcf1f --- /dev/null +++ b/gripper/control/release_dispatch.py @@ -0,0 +1,27 @@ +"""Route release decisions to the FSM owned by the current grip behavior mode. + +sensitive -> release_watch_sensitive (frozen copy of the tuned sensitive logic) +stable / firm -> release_watch (constant clamp, fitted Y displacement) +""" + +from . import release_watch as held_release +from . import release_watch_sensitive as sensitive_release + + +class ModeReleaseWatch: + def __init__(self, config): + self.config = config + self._sensitive = sensitive_release.ReleaseWatch(config) + self._held = held_release.ReleaseWatch(config) + + @property + def active(self): + mode = getattr(self.config, "grip_behavior_mode", "sensitive") + return self._sensitive if mode == "sensitive" else self._held + + def reset(self): + self._sensitive.reset() + self._held.reset() + + def __getattr__(self, name): + return getattr(self.active, name) diff --git a/gripper/control/release_watch_sensitive.py b/gripper/control/release_watch_sensitive.py new file mode 100644 index 0000000..4ecae7b --- /dev/null +++ b/gripper/control/release_watch_sensitive.py @@ -0,0 +1,675 @@ +"""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._osc_count = 0 + self._osc_channel = "" + 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 _shear_oscillation(self, now): + """Detect repeated shear direction flips with enough peak-to-peak swing. + + Intentional open/close often shows as left/right shear oscillating while + normals stay flat and the grip has not yet latched a plateau. Returns + the strongest oscillating shear channel name, or "". + """ + if not getattr(self.config, "release_shear_oscillation_open", False): + return "" + if not getattr(self.config, "release_shear_onset_enabled", True): + return "" + window = max(0.12, float(getattr(self.config, "release_shear_oscillation_window_seconds", 0.4) or 0.4)) + min_flips = max(2, int(getattr(self.config, "release_shear_oscillation_min_flips", 3) or 3)) + amplitude = max(0.04, float(getattr(self.config, "release_shear_oscillation_amplitude", 0.15) or 0.15)) + step_floor = max(0.02, amplitude * 0.2) + best_key, best_flips, best_span = "", 0, 0.0 + cutoff = now - window + for key in CHANNELS[2:]: + points = [(t, v) for t, v in self._curves[key].history if t >= cutoff] + if len(points) < min_flips + 2: + continue + values = [v for _, v in points] + span = max(values) - min(values) + if span + 1e-9 < amplitude: + continue + flips = 0 + prev_sign = 0 + for i in range(1, len(points)): + delta = points[i][1] - points[i - 1][1] + if abs(delta) < step_floor: + continue + sign = 1 if delta > 0 else -1 + if prev_sign and sign != prev_sign: + flips += 1 + prev_sign = sign + if flips >= min_flips and (flips > best_flips or (flips == best_flips and span > best_span)): + best_key, best_flips, best_span = key, flips, span + return best_key + + def _shear_oscillation_release(self, now, metrics): + """Confirm oscillating shear and release without waiting for plateau.""" + channel = self._shear_oscillation(now) + if not channel: + self._osc_count = 0 + self._osc_channel = "" + return None + needed = max(2, int(getattr(self.config, "release_shear_oscillation_confirm_samples", 2) or 2)) + if self._osc_channel != channel: + self._osc_channel = channel + self._osc_count = 1 + else: + self._osc_count += 1 + released = self._osc_count >= needed + if released: + self.phase = ReleasePhase.RELEASED + else: + self.phase = ReleasePhase.CONFIRMING + self._candidate = None + return ReleaseDecision( + self.phase, + "shear_oscillation", + release=released, + confirm_count=self._osc_count, + confirm_needed=needed, + channel=channel, + **dict(metrics, curve_motion="shear_oscillation"), + ) + + 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._osc_count = 0 + self._osc_channel = "" + 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") + + # Shear oscillation = intentional open/close: bypass plateau / settle wait. + osc_decision = self._shear_oscillation_release(now, metrics) + if osc_decision is not None: + return osc_decision + + 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) diff --git a/gripper/ui/visualizer.py b/gripper/ui/visualizer.py index 1faa97a..0740b33 100644 --- a/gripper/ui/visualizer.py +++ b/gripper/ui/visualizer.py @@ -98,6 +98,7 @@ else: from ..app import build_controller from ..common.paths import PACKAGE_DIR, REPO_ROOT +from ..config.behavior_profiles import grip_behavior_profile, normalize_grip_behavior_mode from ..sensors.camera_discovery import ( default_sensor_camera_sources, list_camera_ids, @@ -3114,6 +3115,7 @@ class VisualSettingsDialog(QDialog): updated.get("slave_id", 1), ) applied_live = self.viewer.apply_runtime_config(updated) + self.viewer._sync_mode_controls() if applied_live: tip = "配置已写入，并已对当前运行中的控制即时生效。" else: @@ -3729,6 +3731,36 @@ class GripperDemo02Viewer(QMainWindow): position_max = position_min + 1 return open_pos, close_pos, position_min, position_max + def _sync_mode_controls(self, mode=None): + """Speed / max-force sliders follow the active grip behavior mode.""" + controller = getattr(self, "controller", None) + source = getattr(controller, "config", None) + if source is None: + try: + with open(self._control_config_path(), "r", encoding="utf-8") as f: + source = json.load(f) + except Exception: + source = {} + if mode is None: + mode = (source.get("grip_behavior_mode") if isinstance(source, dict) + else getattr(source, "grip_behavior_mode", None)) + mode = normalize_grip_behavior_mode(mode) + if not source or (isinstance(source, dict) + and source.get("grip_behavior_mode") not in (None, mode)): + source = grip_behavior_profile(mode) + get = source.get if isinstance(source, dict) else (lambda k, d=None: getattr(source, k, d)) + values = {"speed_control": get("speed"), "force_control": get("close_force")} + # Only on change, so a manual slider drag is not snapped back each tick. + key = (mode, tuple(values.values())) + if key == self._mode_controls_key: + return + self._mode_controls_key = key + for control_name, value in values.items(): + control = getattr(self, control_name, None) + if control is not None and value is not None: + # Signals stay blocked so this does not echo into the live config. + control.set_value(value, force=True) + def _load_manual_control_config(self): speed = 25 force = 10 @@ -3771,6 +3803,7 @@ class GripperDemo02Viewer(QMainWindow): self.action_label.setObjectName("StatusSub") status_layout.addWidget(self.status_chip) status_layout.addWidget(self.action_label) + self._mode_controls_key = None self.start_button = TopIconButton("start", "启动控制程序") self.start_button.clicked.connect(self.start_control) self.stop_button = TopIconButton("stop", "停止控制程序") @@ -4465,6 +4498,7 @@ class GripperDemo02Viewer(QMainWindow): self.action_label.setText( shorten_status_text(f"action={label_with_cn(action, ACTION_TEXT)}", 28) ) + self._sync_mode_controls(status.get("grip_behavior_mode")) self.manual_hold_button.setText( "释放" if bool(status.get("manual_hold_active")) else "保持" )