灵敏挡与稳定挡代码分离：灵敏挡使用 6393be9 原版 release_watch（release_watch_sensitive.py），较稳定/十分稳定使用 release_watch.py，按挡位分发；速度按挡位设置（灵敏 55%，较稳定/十分稳定 30%）；切换挡位后界面速度/最大力滑块同步。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
cheng
@@ -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"
|
||||
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user