diff --git a/config/compound_eye.png b/config/compound_eye.png index e019068..558b9cc 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 f13840a..d6fe0e8 100644 --- a/config/gripper.json +++ b/config/gripper.json @@ -12,7 +12,7 @@ "serial_timeout": 0.2, "open_pos": 0, "close_pos": 9000, - "speed": 55, + "speed": 35, "accel": 70, "decel": 70, "release_open_speed": 55, @@ -24,7 +24,7 @@ "hold_force": 33, "initial_force": 25, "force_min": 20, - "force_max": 99, + "force_max": 100, "control_hz": 30.0, "baseline_seconds": 1.0, "filter_alpha": 0.42, @@ -139,5 +139,11 @@ "force_n": 2.06 } ] - } + }, + "release_guard_fast_paths": false, + "grip_contact_hold_position": true, + "release_shear_axes": "", + "release_shear_displacement": 0.0, + "release_shear_displacement_samples": 6, + "release_object_lost_open": false } diff --git a/docs/RELEASE_CONTROL.zh-CN.md b/docs/RELEASE_CONTROL.zh-CN.md index ee5dee4..1442f36 100644 --- a/docs/RELEASE_CONTROL.zh-CN.md +++ b/docs/RELEASE_CONTROL.zh-CN.md @@ -2,14 +2,16 @@ ## 行为模式（灵敏 / 较稳定 / 十分稳定） -设置里可切换三套开合预设（`grip_behavior_mode`），切换后只改写**释放/开合**相关参数； -闭合触发（`trigger_*`、等待再武装稳定门槛）一律固定为灵敏档。 +设置里可切换三套预设（`grip_behavior_mode`），切换后改写**夹持力**与**释放/开合**相关参数； +闭合触发（`trigger_*`、等待再武装稳定门槛）一律固定为灵敏档，闭合速度各档相同。 -| 模式 | 含义 | -| --- | --- | -| `sensitive`（灵敏） | 当前默认调参，跟手、更容易开合 | -| `stable`（较稳定） | 明显提高释放门槛与确认，少误开 | -| `firm`（十分稳定） | 约高一个数量级释放门槛，需故意大力开合 | +| 模式 | 夹持力 | 释放 | +| --- | --- | --- | +| `sensitive`（灵敏） | 起步 25%，按切向负载自适应 33~80% | 跟手；振荡/侧触在夹紧期间也生效，法向也可张开 | +| `stable`（较稳定） | 闭合指令即 70%，接触后不锁位置（`grip_contact_hold_position=false`），继续以 70% 朝闭合方向顶住，不再加力 | 接触后 2 s 保护期内不触发任何张开（`release_min_hold_seconds=2.0`，不再额外等法向夹稳）；张开只看 Y 切向拟合偏移：保护期结束时记下两侧带符号 Y 的拟合值作基准，之后任一侧拟合值偏离基准 ≥ 0.6 N 连续 6 帧即张开（`release_shear_displacement=0.6`，不取绝对值、回头不取消，基准只在接近时缓慢跟随漂移）；法向、切向振荡和侧触/突变/触碰路径都不参与。夹持中夹爪走到闭合极限（物体已脱出）立即张开（`release_object_lost_open=true`，不受保护期限制） | +| `firm`（十分稳定） | 同较稳定档的恒力方式，但闭合指令即 100% | 与较稳定档相同的张开方式，保护期 2.5 s（`release_min_hold_seconds=2.5`），Y 拟合偏移阈值 0.8 N（`release_shear_displacement=0.8`）；同样支持物体脱出张开 | + +满力需要 `force_max` ≥ 100。 预设见 `gripper/config/behavior_profiles.py`。旧配置里的 `rapid` /「迅速开合」映射为 `sensitive`。 diff --git a/gripper/config/behavior_profiles.py b/gripper/config/behavior_profiles.py index f10c496..616c20f 100644 --- a/gripper/config/behavior_profiles.py +++ b/gripper/config/behavior_profiles.py @@ -1,13 +1,13 @@ -"""Grip behavior presets: sensitive / stable / firm (open/release only).""" +"""Grip behavior presets: sensitive / stable / firm (grip force + release).""" from __future__ import annotations import copy -# sensitive = 灵敏：跟手释放，当前默认调参 -# stable = 较稳定：原十分稳定档，明显更难误开 -# firm = 十分稳定：约高一个数量级门槛，故意开合才响应 +# sensitive = 灵敏：跟手释放，自适应夹持力 +# stable = 较稳定：接触即 70% 恒力夹持，2 s 保护期，Y 拟合偏移 0.6 N 张开 +# firm = 十分稳定：接触即 100% 恒力夹持，2.5 s 保护期，Y 拟合偏移 0.8 N 张开 GRIP_BEHAVIOR_MODES = ("sensitive", "stable", "firm") GRIP_BEHAVIOR_LABELS = { @@ -28,6 +28,21 @@ 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 = ( + "grip_contact_hold_position", + "close_force", + "grip_start_force", + "hold_force", + "force_ramp_step", + "force_ramp_interval", + "adaptive_force_min", + "adaptive_force_max", + "adaptive_force_step", + "adaptive_force_interval", + "release_guard_fast_paths", + "release_shear_axes", + "release_shear_displacement", + "release_shear_displacement_samples", + "release_object_lost_open", "release_open_speed", "release_open_accel", "release_open_decel", @@ -75,8 +90,44 @@ _SENSITIVE_TRIGGER = { "open_wait_stable_trend_n": 0.015, } +# Adaptive clamp: start light, climb with payload shear. +_ADAPTIVE_FORCE = { + "grip_contact_hold_position": True, + "close_force": 33, + "grip_start_force": 25, + "hold_force": 33, + "force_ramp_step": 1, + "force_ramp_interval": 0.1, + "adaptive_force_min": 33, + "adaptive_force_max": 80, + "adaptive_force_step": 5, + "adaptive_force_interval": 0.08, +} + +def _constant_force(pct): + """Close at *pct* and keep that close command squeezing after contact + (no position hold, no later ramp). Close speed stays the global speed.""" + return { + "grip_contact_hold_position": False, + "close_force": pct, + "grip_start_force": pct, + "hold_force": pct, + "force_ramp_step": 5, + "force_ramp_interval": 0.03, + "adaptive_force_min": pct, + "adaptive_force_max": pct, + "adaptive_force_step": 5, + "adaptive_force_interval": 0.03, + } + _SENSITIVE_PROFILE = { **_SENSITIVE_TRIGGER, + **_ADAPTIVE_FORCE, + "release_guard_fast_paths": False, + "release_shear_axes": "", + "release_shear_displacement": 0.0, + "release_shear_displacement_samples": 6, + "release_object_lost_open": False, "release_open_speed": 55, "release_open_accel": 70, "release_open_decel": 70, @@ -117,13 +168,24 @@ _SENSITIVE_PROFILE = { } _STABLE_PROFILE = { - "release_open_speed": 45, - "release_open_accel": 60, - "release_open_decel": 60, + **_constant_force(70), + "release_guard_fast_paths": True, + # Release looks at Y shear only, regardless of the global control axes. + "release_shear_axes": "y", + # Full clamp swings Y by several N and flips sign, so onset confirmation + # keeps cancelling; open on sustained fitted Y displacement instead. + "release_shear_displacement": 0.6, + "release_shear_displacement_samples": 6, + # The close command keeps pushing: reaching close_pos means the object is gone. + "release_object_lost_open": True, + "release_open_speed": 55, + "release_open_accel": 70, + "release_open_decel": 70, "release_arm_delay_seconds": 0.55, - "release_min_hold_seconds": 1.2, + # Protection: no release of any kind within 2s of contact. + "release_min_hold_seconds": 2.0, "release_settle_seconds": 0.22, - "release_min_change": 0.16, + "release_min_change": 0.12, "release_touch_window_seconds": 0.22, "release_side_touch_change": 0.24, "release_confirm_window_seconds": 0.28, @@ -139,61 +201,31 @@ _STABLE_PROFILE = { "release_slope_window_seconds": 1.2, "release_slope_settle_rate": 0.12, "release_slope_min_points": 14, - "release_require_slope_settle": True, + # Full constant clamp: the protection window is the only arming gate. + "release_require_slope_settle": False, "release_slope_hold_seconds": 0.9, "release_settle_normal_only": True, - "release_shear_oscillation_open": True, + # Open by fitted Y shear displacement only, no oscillation path. + "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, + # Normal only judges the clamp plateau; opening is by Y shear only. "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, + "release_normal_onset_enabled": False, "rearm_seconds": 0.9, "recover_stable_seconds": 0.9, } -# Roughly ~10x harder to trip open than sensitive; intentional open needs clear force. +# Same release scheme as stable, with a harder clamp and larger margins. _FIRM_PROFILE = { - "release_open_speed": 35, - "release_open_accel": 50, - "release_open_decel": 50, - "release_arm_delay_seconds": 1.2, - "release_min_hold_seconds": 3.0, - "release_settle_seconds": 0.5, - "release_min_change": 0.8, - "release_touch_window_seconds": 0.35, - "release_side_touch_change": 1.2, - "release_confirm_window_seconds": 0.5, - "release_sample_timeout_seconds": 0.4, - "release_confirm_samples": 12, - "release_slow_confirm_samples": 18, - "release_normal_rate": 20.0, - "release_shear_rate": 18.0, - "release_slow_rate": 0.08, - "release_normal_change": 4.0, - "release_shear_change": 3.5, - "release_curve_history": 60, - "release_slope_window_seconds": 2.0, - "release_slope_settle_rate": 0.06, - "release_slope_min_points": 20, - "release_require_slope_settle": True, - "release_slope_hold_seconds": 2.0, - "release_settle_normal_only": True, - # Keep path available, but require large rub/twist evidence. - "release_shear_oscillation_open": True, - "release_shear_oscillation_window_seconds": 0.8, - "release_shear_oscillation_min_flips": 6, - "release_shear_oscillation_amplitude": 1.5, - "release_shear_oscillation_confirm_samples": 6, - "release_side_shear_enabled": True, - "release_side_normal_enabled": True, - "release_shear_onset_enabled": True, - "release_normal_onset_enabled": True, - "rearm_seconds": 1.5, - "recover_stable_seconds": 1.5, + **_STABLE_PROFILE, + **_constant_force(100), + "release_min_hold_seconds": 2.5, + "release_shear_displacement": 0.8, } GRIP_BEHAVIOR_PROFILES = { diff --git a/gripper/config/settings.py b/gripper/config/settings.py index 7dde9c8..af41887 100644 --- a/gripper/config/settings.py +++ b/gripper/config/settings.py @@ -84,6 +84,9 @@ class Demo02Config: force_ramp_step: int = 1 force_ramp_interval: float = 0.1 grip_settle_seconds: float = 0.1 + # On contact: True = hold at the contact position; False = keep the close + # command squeezing toward close_pos at full commanded force. + grip_contact_hold_position: bool = True grip_behavior_mode: str = "sensitive" adaptive_grip_enabled: bool = True adaptive_shear_start_force: float = 0.10 @@ -119,6 +122,16 @@ class Demo02Config: release_shear_oscillation_min_flips: int = 3 release_shear_oscillation_amplitude: float = 0.15 release_shear_oscillation_confirm_samples: int = 2 + # Block oscillation/side-touch release during contact hold and servo motion. + release_guard_fast_paths: bool = False + # Shear axes for release only: "" follows shear_control_axes; "y" | "x" | "xy". + release_shear_axes: str = "" + # >0: open when curve-fitted signed Y shear leaves its post-protection + # baseline by this much (N) for N consecutive samples; replaces onset paths. + release_shear_displacement: float = 0.0 + release_shear_displacement_samples: int = 6 + # Open when the gripper travels to close_pos while holding (object gone). + release_object_lost_open: bool = False release_side_shear_enabled: bool = True release_side_normal_enabled: bool = True release_shear_onset_enabled: bool = True @@ -212,6 +225,9 @@ class Demo02Config: add("--force-ramp-step", type=int, default=get("force_ramp_step", cls.force_ramp_step)) add("--force-ramp-interval", type=float, default=get("force_ramp_interval", cls.force_ramp_interval)) add("--grip-settle-seconds", type=float, default=get("grip_settle_seconds", cls.grip_settle_seconds)) + add("--grip-contact-hold-position", dest="grip_contact_hold_position", action="store_true", + default=get("grip_contact_hold_position", cls.grip_contact_hold_position)) + add("--no-grip-contact-hold-position", dest="grip_contact_hold_position", action="store_false") add( "--grip-behavior-mode", type=str, @@ -265,6 +281,18 @@ class Demo02Config: default=get("release_shear_oscillation_amplitude", cls.release_shear_oscillation_amplitude)) add("--release-shear-oscillation-confirm-samples", type=int, default=get("release_shear_oscillation_confirm_samples", cls.release_shear_oscillation_confirm_samples)) + add("--release-guard-fast-paths", dest="release_guard_fast_paths", action="store_true", + default=get("release_guard_fast_paths", cls.release_guard_fast_paths)) + add("--no-release-guard-fast-paths", dest="release_guard_fast_paths", action="store_false") + add("--release-shear-axes", type=str, choices=["", "xy", "x", "y"], + default=str(get("release_shear_axes", cls.release_shear_axes) or "").strip().lower()) + add("--release-shear-displacement", type=float, + default=get("release_shear_displacement", cls.release_shear_displacement)) + add("--release-shear-displacement-samples", type=int, + default=get("release_shear_displacement_samples", cls.release_shear_displacement_samples)) + add("--release-object-lost-open", dest="release_object_lost_open", action="store_true", + default=get("release_object_lost_open", cls.release_object_lost_open)) + add("--no-release-object-lost-open", dest="release_object_lost_open", action="store_false") add("--release-side-shear-enabled", dest="release_side_shear_enabled", action="store_true", default=get("release_side_shear_enabled", cls.release_side_shear_enabled)) add("--no-release-side-shear", dest="release_side_shear_enabled", action="store_false") diff --git a/gripper/control/controller.py b/gripper/control/controller.py index 77b4eee..b63b0f9 100644 --- a/gripper/control/controller.py +++ b/gripper/control/controller.py @@ -2,12 +2,12 @@ import time import threading import json from collections import deque -from dataclasses import asdict +from dataclasses import asdict, replace from ..common.filters import clamp from ..common.paths import REPO_ROOT from .actuator import ActuatorWorker -from .release_watch import ReleaseWatch +from .release_watch import ReleasePhase, ReleaseWatch from .session_recorder import SessionRecorder @@ -91,6 +91,9 @@ class SideTriggerGripController: pass print( f"[behavior] grip_behavior_mode={applied} " + f"force=close{self.config.close_force}/start{self.config.grip_start_force}/" + f"hold{self.config.hold_force}/adaptive{self.config.adaptive_force_min}-{self.config.adaptive_force_max} " + f"guard={getattr(self.config, 'release_guard_fast_paths', False)} " 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} " @@ -119,6 +122,13 @@ class SideTriggerGripController: "grip_requires_both", "grip_behavior_mode", "adaptive_grip_enabled", + "close_force", + "grip_start_force", + "hold_force", + "grip_contact_hold_position", + "force_max", + "adaptive_force_min", + "adaptive_force_max", } }, } @@ -254,6 +264,7 @@ class SideTriggerGripController: recent_feedback = deque(maxlen=6) recent_pair_stamps = None hold_pos = None + grip_contact_pos = None actuator = self.actuator def reset_open_wait_stability(): @@ -276,6 +287,19 @@ class SideTriggerGripController: def reset_release_watch(): release_watch.reset() + def object_lost(): + # The close command keeps pushing: arriving at close_pos from a + # contact position well short of it means the fingers are empty. + if not getattr(config, "release_object_lost_open", False) or grip_contact_pos is None: + return False + position = current_position() + if position is None: + return False + tolerance = max(1, int(config.open_position_tolerance)) + close_pos = int(config.close_pos) + return (abs(position - close_pos) <= tolerance + and abs(grip_contact_pos - close_pos) > 2 * tolerance) + 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 @@ -286,6 +310,9 @@ class SideTriggerGripController: now_ts, feedback_now, motion_pending=pending, motion_completed_at=completed_at, ) + if not decision.release and object_lost(): + decision = replace(decision, phase=ReleasePhase.RELEASED, + reason="object-lost", release=True) self._release_decision = decision action = decision.action if decision.release: @@ -539,14 +566,22 @@ class SideTriggerGripController: elif state == "closing": if grip_contact: - force_pct = int(clamp( + start_force = int(clamp( config.grip_start_force, config.force_min, config.force_max, )) - actuator.set_force(force_pct) - actuator.hold(force_pct) - hold_pos = refresh_hold_pos() or current_position() + if getattr(config, "grip_contact_hold_position", True): + force_pct = start_force + actuator.set_force(force_pct) + actuator.hold(force_pct) + elif start_force != force_pct: + # Keep the close command pushing toward close_pos; a hold + # would retarget to the contact position and drop squeeze. + force_pct = start_force + actuator.close(force_pct, "contact-squeeze") + grip_contact_pos = current_position() + hold_pos = refresh_hold_pos() or grip_contact_pos state = "gripping" state_start = now last_force_ramp_time = now diff --git a/gripper/control/release_watch.py b/gripper/control/release_watch.py index 4ecae7b..89d8223 100644 --- a/gripper/control/release_watch.py +++ b/gripper/control/release_watch.py @@ -1,13 +1,101 @@ """Release FSM. No clock, serial I/O or callbacks: step() returns a decision.""" +from collections import deque from dataclasses import dataclass from enum import Enum import math -from .force_curve import ForceCurve, ForceOnset +from .feedback import combine_shear_components +from .force_curve import ForceCurve, ForceOnset, _fit_curve CHANNELS = ("left_normal", "right_normal", "left_shear", "right_shear") +_SHEAR_COMPONENTS = ("left_shear_x", "left_shear_y", "right_shear_x", "right_shear_y") + + +def channel_values(config, feedback): + """Release channels; shear may use its own axes independent of the trigger.""" + values = {key: float(getattr(feedback, key)) for key in CHANNELS} + axes = str(getattr(config, "release_shear_axes", "") or "").strip().lower() + components = [float(getattr(feedback, key, 0.0) or 0.0) for key in _SHEAR_COMPONENTS] + # Magnitude-mode calibration has no per-axis components; keep feedback shear. + if axes and any(components): + values["left_shear"] = combine_shear_components(components[0], components[1], axes) + values["right_shear"] = combine_shear_components(components[2], components[3], axes) + return values + + +def signed_shear_y(feedback, key): + """Signed Y for *key* ("left_shear"/"right_shear"); magnitude if no components.""" + side = key.split("_", 1)[0] + x = float(getattr(feedback, f"{side}_shear_x", 0.0) or 0.0) + y = float(getattr(feedback, f"{side}_shear_y", 0.0) or 0.0) + if x or y: + return y + return float(getattr(feedback, key)) + + +class _ShearDisplacement: + """Sustained signed-Y departure from the post-protection baseline. + + Each side is smoothed by a local least-squares line (no abs fold, so a + sign flip under full clamp is ordinary travel). A turn never cancels: + only falling back under the threshold resets the consecutive count. The + baseline follows slow drift while the fitted value stays near it. + """ + + KEYS = ("left_shear", "right_shear") + FIT_POINTS = 6 + DRIFT_GAIN = 0.02 + + def __init__(self, config): + self.config = config + self.reset() + + def reset(self): + self.interrupt() + self.baseline = {} + + def interrupt(self): + self.history = {key: deque(maxlen=self.FIT_POINTS) for key in self.KEYS} + self.fitted = {} + self.count = 0 + self.channel = "" + self.deviation = 0.0 + + def observe(self, key, stamp, value): + history = self.history[key] + if history and stamp <= history[-1][0]: + if stamp == history[-1][0]: + return + history.clear() + history.append((stamp, value)) + if len(history) >= 3: + noise_floor = max(1e-6, float(self.config.release_min_change) * 0.025) + self.fitted[key] = _fit_curve(list(history), 1, noise_floor).predict(stamp) + else: + self.fitted[key] = value + + def step(self, threshold): + """Update after fresh samples; returns True once the baseline is armed.""" + if len(self.fitted) < len(self.KEYS): + return False + if not self.baseline: + self.baseline = dict(self.fitted) + return False + deviations = {key: self.fitted[key] - self.baseline[key] for key in self.KEYS} + key = max(deviations, key=lambda k: abs(deviations[k])) + self.deviation = deviations[key] + if abs(self.deviation) + 1e-9 >= threshold: + self.count += 1 + self.channel = key + return True + self.count = 0 + self.channel = "" + for name, deviation in deviations.items(): + if abs(deviation) < 0.5 * threshold: + self.baseline[name] += self.DRIFT_GAIN * deviation + return True class ReleasePhase(str, Enum): @@ -226,6 +314,7 @@ class ReleaseWatch: def __init__(self, config): self.config = config self._side_touch = _SideTouch(config) + self._shear_disp = _ShearDisplacement(config) self.reset() def reset(self): @@ -247,6 +336,7 @@ class ReleaseWatch: self._rates = {} self._curves = {key: ForceCurve(self.config) for key in CHANNELS} self._side_touch.reset() + self._shear_disp.reset() @property def armed(self): @@ -263,17 +353,18 @@ class ReleaseWatch: 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} + values = channel_values(self.config, frame) 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}) + values = channel_values(self.config, feedback) + self._side_touch.seed(self._pair_time(now, feedback), values) 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].observe(now if stamp is None else stamp, values[key]) self._curves[key].discard_onset() @staticmethod @@ -287,6 +378,7 @@ class ReleaseWatch: return self._protect("sample-gap", {}) self._side_touch.reset() + self._shear_disp.interrupt() self._reference.clear() self._previous.clear() self._rates.clear() @@ -421,11 +513,12 @@ class ReleaseWatch: cfg = self.config timeout = max(0.001, cfg.release_sample_timeout_seconds) - values = {key: float(getattr(feedback, key)) for key in CHANNELS} + values = channel_values(cfg, feedback) if not all(math.isfinite(value) for value in values.values()): self._previous.clear() self._rates.clear() self._side_touch.reset() + self._shear_disp.interrupt() self._curves = {key: ForceCurve(cfg) for key in CHANNELS} self._recovery_until = now + cfg.release_settle_seconds return self._protect("invalid-sample", {}) @@ -459,6 +552,7 @@ class ReleaseWatch: stale = any(now - self._last_fresh.get(k, now) > timeout for k in CHANNELS) if stale or discontinuity: self._side_touch.reset() + self._shear_disp.interrupt() self._curves = {key: ForceCurve(cfg) for key in CHANNELS} self._recovery_until = now + cfg.release_settle_seconds return self._protect("sample-gap", values, **metrics) @@ -470,27 +564,43 @@ class ReleaseWatch: "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) + displacement = float(getattr(cfg, "release_shear_displacement", 0.0) or 0.0) + if displacement > 0: + return self._displacement_step(feedback, fresh, contact_guard, displacement, metrics) + # Guarded modes: a full-force clamp shakes the shear channels, so the + # fast paths (oscillation / side touch) wait for contact hold + servo idle. + guarded = bool(getattr(cfg, "release_guard_fast_paths", False)) + fast_blocked = guarded and (motion_pending or self_tail or contact_guard) + + # Shear oscillation = intentional open/close: bypass plateau / settle wait. + if not fast_blocked: + osc_decision = self._shear_oscillation_release(now, metrics) + if osc_decision is not None: + return osc_decision + 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" + if guarded: + side_enabled = not fast_blocked and ( + self.phase == ReleasePhase.WATCHING + or self._side_touch.candidate is not None + ) + else: + side_enabled = protected or self._side_touch.candidate is not None 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, + enabled=side_enabled, + normal_masked=motion_pending or self_tail or fast_blocked, sample_time=self._pair_time(now, feedback), - onset_allowed=force_settled) + onset_allowed=force_settled and not fast_blocked) 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) @@ -660,6 +770,38 @@ class ReleaseWatch: return ReleaseDecision(self.phase, reason, allow_force_update=allow_force, **metrics) + def _displacement_step(self, feedback, fresh, contact_guard, threshold, metrics): + """Sole release path when displacement is enabled (onset paths off).""" + disp = self._shear_disp + for key in disp.KEYS: + if key in fresh: + disp.observe(key, self._previous[key][0], signed_shear_y(feedback, key)) + metrics = dict(metrics, curve_motion="shear_displacement") + if contact_guard: + disp.baseline.clear() + disp.count = 0 + self.phase = ReleasePhase.GRIP_SETTLE + return ReleaseDecision(self.phase, "grip-settle", **metrics) + if not any(key in fresh for key in disp.KEYS): + return ReleaseDecision(self.phase, "waiting-sample", allow_force_update=True, **metrics) + if not disp.step(threshold): + self._grip_settled = True + self.phase = ReleasePhase.WATCHING + return ReleaseDecision(self.phase, "ready", allow_force_update=True, **metrics) + needed = max(1, int(getattr(self.config, "release_shear_displacement_samples", 6) or 6)) + metrics["shear_change"] = abs(disp.deviation) + if disp.count >= needed: + self.phase = ReleasePhase.RELEASED + else: + self.phase = ReleasePhase.CONFIRMING if disp.count else ReleasePhase.WATCHING + return ReleaseDecision( + self.phase, "shear_displacement" if disp.count else "watch", + release=self.phase == ReleasePhase.RELEASED, + allow_force_update=not disp.count, + confirm_count=disp.count, confirm_needed=needed, channel=disp.channel, + **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. diff --git a/gripper/ui/visualizer.py b/gripper/ui/visualizer.py index e2fc89e..1faa97a 100644 --- a/gripper/ui/visualizer.py +++ b/gripper/ui/visualizer.py @@ -2483,6 +2483,7 @@ class VisualSettingsDialog(QDialog): "close_force", "grip_start_force", "hold_force", + "grip_contact_hold_position", "initial_force", "force_min", "force_max", @@ -2556,6 +2557,11 @@ class VisualSettingsDialog(QDialog): "release_shear_oscillation_min_flips", "release_shear_oscillation_amplitude", "release_shear_oscillation_confirm_samples", + "release_guard_fast_paths", + "release_shear_axes", + "release_shear_displacement", + "release_shear_displacement_samples", + "release_object_lost_open", "release_side_shear_enabled", "release_side_normal_enabled", "release_shear_onset_enabled", @@ -2603,6 +2609,7 @@ class VisualSettingsDialog(QDialog): "close_force": "闭合力", "grip_start_force": "夹住起始力", "hold_force": "保持力", + "grip_contact_hold_position": "接触后锁定当前位置（关=持续以闭合力夹紧）", "initial_force": "初始力", "force_min": "最小力", "force_max": "最大力", @@ -2662,6 +2669,11 @@ class VisualSettingsDialog(QDialog): "release_shear_oscillation_min_flips": "切向振荡最少换向次数", "release_shear_oscillation_amplitude": "切向振荡峰峰值门槛 (N)", "release_shear_oscillation_confirm_samples": "切向振荡确认帧数", + "release_guard_fast_paths": "夹紧保护期内屏蔽振荡/侧触张开", + "release_shear_axes": "张开判断切向轴 (空=跟随控制轴/y/x/xy)", + "release_shear_displacement": "Y 拟合偏移张开阈值 (N, 0=关闭)", + "release_shear_displacement_samples": "Y 拟合偏移连续确认帧数", + "release_object_lost_open": "夹爪合到底(物体脱出)自动张开", "release_side_shear_enabled": "允许切向左右差侧触张开", "release_side_normal_enabled": "允许法向左右差侧触张开", "release_shear_onset_enabled": "允许切向通道触发普通释放",