orisys-tech/ctag2f-90d-sdk
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

较稳定/十分稳定挡改为恒力夹持：接触后不锁位置持续以闭合力顶住；张开改为 Y 切向带符号拟合偏移（保护期后记基准，连续 6 帧超阈值），夹爪合到底判定物体脱出即张开。较稳定 70%/2s/0.6N，十分稳定 100%/2.5s/0.8N。

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
cheng
2026-09-28 16:30:54 +08:00
parent 6393be9518
commit 8be8a80cff
8 changed files with 337 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/compound_eye.png
BIN
View File
Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.5 MiB

After

Width:  |  Height:  |  Size: 2.5 MiB

config/gripper.json
+9 -3
View File
@@ -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
}
docs/RELEASE_CONTROL.zh-CN.md
+9 -7
View File
@@ -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`。
gripper/config/behavior_profiles.py
+83 -51
View File
@@ -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 = {
gripper/config/settings.py
+28
View File
@@ -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")
gripper/control/controller.py
+41 -6
View File
@@ -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
gripper/control/release_watch.py
+155 -13
View File
@@ -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.
gripper/ui/visualizer.py
+12
View File
@@ -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": "允许切向通道触发普通释放",