244 lines
8.7 KiB
Python
244 lines
8.7 KiB
Python
#!/usr/bin/env python3
|
|
"""Pure state machine for the right-B pointing-hand gesture.
|
|
|
|
This module deliberately has no ROS dependencies so the hold/release safety
|
|
rules can be exercised offline before the bridge is deployed on a robot.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
from typing import Any, Mapping, Sequence
|
|
|
|
|
|
class GestureToggle:
|
|
"""Toggle one persistent gesture with a guarded long press.
|
|
|
|
A new session always starts release-locked. A continuous release clears
|
|
that lock, then a continuous press toggles the gesture once. The same
|
|
physical press cannot toggle twice; another stable release is required.
|
|
"""
|
|
|
|
def __init__(self, hold_seconds: float, release_seconds: float) -> None:
|
|
if not math.isfinite(hold_seconds) or hold_seconds <= 0.0:
|
|
raise ValueError("gesture hold time must be positive and finite")
|
|
if not math.isfinite(release_seconds) or release_seconds <= 0.0:
|
|
raise ValueError("gesture release time must be positive and finite")
|
|
self.hold_seconds = float(hold_seconds)
|
|
self.release_seconds = float(release_seconds)
|
|
self.active = False
|
|
self.hold_started_at: float | None = None
|
|
self.release_started_at: float | None = None
|
|
self.require_release = True
|
|
self.armed = False
|
|
self.input_healthy = False
|
|
self.button_pressed: bool | None = None
|
|
self._freeze_right_hand = False
|
|
self.toggle_count = 0
|
|
self.last_transition = "initialized"
|
|
|
|
def new_session(self) -> None:
|
|
"""Start release-locked and never carry a gesture across sessions."""
|
|
|
|
self.active = False
|
|
self.hold_started_at = None
|
|
self.release_started_at = None
|
|
self.require_release = True
|
|
self.armed = True
|
|
self.input_healthy = False
|
|
self.button_pressed = None
|
|
self._freeze_right_hand = True
|
|
self.last_transition = "new_session"
|
|
|
|
def disarm(self, reason: str = "disarmed") -> None:
|
|
"""Clear the logical gesture without commanding any hand movement."""
|
|
|
|
self.active = False
|
|
self.hold_started_at = None
|
|
self.release_started_at = None
|
|
self.require_release = True
|
|
self.armed = False
|
|
self.input_healthy = False
|
|
self.button_pressed = None
|
|
self._freeze_right_hand = False
|
|
self.last_transition = reason
|
|
|
|
def update(
|
|
self,
|
|
now: float,
|
|
*,
|
|
armed: bool,
|
|
input_healthy: bool,
|
|
pressed: bool | None,
|
|
) -> bool:
|
|
"""Advance the state and return ``True`` only when a toggle occurs.
|
|
|
|
``pressed=None`` represents a missing or malformed B-button sample. It
|
|
cancels a pending hold and requires a fresh stable release. A hand
|
|
feedback gap behaves the same way, but an already active gesture is
|
|
retained so it can resume through the bridge's measured-feedback slew.
|
|
"""
|
|
|
|
if not math.isfinite(now):
|
|
raise ValueError("gesture clock must be finite")
|
|
if not armed:
|
|
if self.armed or self.active:
|
|
self.disarm()
|
|
return False
|
|
if not self.armed:
|
|
self.new_session()
|
|
|
|
self.input_healthy = bool(input_healthy)
|
|
self.button_pressed = pressed
|
|
if not input_healthy or pressed is None:
|
|
self.hold_started_at = None
|
|
self.release_started_at = None
|
|
self.require_release = True
|
|
if not self.active:
|
|
self._freeze_right_hand = True
|
|
return False
|
|
|
|
if self.require_release:
|
|
self.hold_started_at = None
|
|
if pressed:
|
|
self.release_started_at = None
|
|
return False
|
|
if self.release_started_at is None:
|
|
self.release_started_at = now
|
|
return False
|
|
if now - self.release_started_at >= self.release_seconds:
|
|
self.require_release = False
|
|
self.release_started_at = None
|
|
if not self.active:
|
|
self._freeze_right_hand = False
|
|
return False
|
|
|
|
self.release_started_at = None
|
|
if not pressed:
|
|
if self.hold_started_at is not None:
|
|
# A short/interrupted press is not a toggle. Debounce the
|
|
# release before another hold can start.
|
|
self.hold_started_at = None
|
|
self.release_started_at = now
|
|
self.require_release = True
|
|
if not self.active:
|
|
self._freeze_right_hand = True
|
|
return False
|
|
|
|
if self.hold_started_at is None:
|
|
self.hold_started_at = now
|
|
if not self.active:
|
|
self._freeze_right_hand = True
|
|
return False
|
|
if now - self.hold_started_at < self.hold_seconds:
|
|
return False
|
|
|
|
self.active = not self.active
|
|
self.toggle_count += 1
|
|
self.last_transition = "activated" if self.active else "deactivated"
|
|
# Activation uses the point target. Deactivation starts slewing back
|
|
# to live hand input immediately; the release lock only prevents a
|
|
# second toggle from the same physical press.
|
|
self._freeze_right_hand = False
|
|
self.hold_started_at = None
|
|
self.release_started_at = None
|
|
self.require_release = True
|
|
return True
|
|
|
|
@property
|
|
def freeze_right_hand(self) -> bool:
|
|
"""Whether the bridge must hold its last right-hand command.
|
|
|
|
While inactive, freeze through an incomplete B hold and its required
|
|
release. A completed deactivation starts returning to live input at
|
|
once; its release lock prevents only another toggle. This keeps a
|
|
short B press from leaking through xTELE's processed command stream.
|
|
"""
|
|
|
|
return self.armed and not self.active and self._freeze_right_hand
|
|
|
|
def hold_elapsed(self, now: float) -> float:
|
|
if self.hold_started_at is None:
|
|
return 0.0
|
|
return max(0.0, now - self.hold_started_at)
|
|
|
|
def release_elapsed(self, now: float) -> float:
|
|
if self.release_started_at is None:
|
|
return 0.0
|
|
return max(0.0, now - self.release_started_at)
|
|
|
|
@property
|
|
def state(self) -> str:
|
|
if not self.armed:
|
|
return "disarmed"
|
|
if not self.input_healthy:
|
|
return "input_unhealthy"
|
|
if self.button_pressed is None:
|
|
return "invalid_button"
|
|
if self.require_release:
|
|
return "awaiting_release"
|
|
if self.hold_started_at is not None:
|
|
return "holding_stop" if self.active else "holding_start"
|
|
return "active" if self.active else "idle"
|
|
|
|
|
|
def right_b_pressed(data: Mapping[str, Any]) -> bool | None:
|
|
"""Return TS1P right-B state, or ``None`` for a malformed sample."""
|
|
|
|
try:
|
|
buttons = data["button"]
|
|
if not isinstance(buttons, Mapping):
|
|
return None
|
|
right = buttons["right"]
|
|
if not isinstance(right, (list, tuple)) or len(right) < 2:
|
|
return None
|
|
value = right[1]
|
|
if isinstance(value, bool):
|
|
return value
|
|
if isinstance(value, int) and value in (0, 1):
|
|
return bool(value)
|
|
return None
|
|
except (KeyError, TypeError):
|
|
return None
|
|
|
|
|
|
def normalized_pose_to_positions(
|
|
pose: Sequence[float], minimum: int, maximum: int
|
|
) -> list[int]:
|
|
"""Convert a six-motor normalized BrainCo pose to driver positions."""
|
|
|
|
if len(pose) != 6:
|
|
raise ValueError("gesture pose must contain 6 normalized values")
|
|
values = [float(value) for value in pose]
|
|
if not all(math.isfinite(value) and 0.0 <= value <= 1.0 for value in values):
|
|
raise ValueError("gesture pose must contain finite values in [0, 1]")
|
|
if minimum < 0 or maximum <= minimum:
|
|
raise ValueError("invalid BrainCo position range")
|
|
return [
|
|
int(round(minimum + value * (maximum - minimum))) for value in values
|
|
]
|
|
|
|
|
|
def select_right_hand_target(
|
|
source_target: Sequence[int],
|
|
previous_command: Sequence[int] | None,
|
|
gesture_target: Sequence[int],
|
|
*,
|
|
active: bool,
|
|
freeze: bool,
|
|
) -> list[int]:
|
|
"""Apply the right-only gesture override or fail-safe input freeze."""
|
|
|
|
source = list(source_target)
|
|
gesture = list(gesture_target)
|
|
if len(source) != 6 or len(gesture) != 6:
|
|
raise ValueError("right-hand targets must each contain 6 positions")
|
|
if active:
|
|
return gesture
|
|
if freeze:
|
|
previous = [] if previous_command is None else list(previous_command)
|
|
if len(previous) != 6:
|
|
raise ValueError("cannot freeze without a complete previous command")
|
|
return previous
|
|
return source
|