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
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.