feat: harden TG3 teleop and add right-B point gesture
This commit is contained in:
LengedZhao
243
tg3_local_teleop/gesture_toggle.py
Normal file
243
tg3_local_teleop/gesture_toggle.py
Normal file
@@ -0,0 +1,243 @@
|
||||
#!/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
|
||||
Reference in New Issue
Block a user