|
|
|
@@ -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)
|
cheng