feat: add head control and joint data decoder
This commit is contained in:
226
tg3_local_teleop/head_control.py
Normal file
226
tg3_local_teleop/head_control.py
Normal file
@@ -0,0 +1,226 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Pure safety state for direct TS1P right-stick head-pitch control."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Any, Mapping
|
||||
|
||||
|
||||
def right_stick_vertical(data: Mapping[str, Any]) -> float | None:
|
||||
"""Return the TS1P right-stick vertical axis or fail closed."""
|
||||
|
||||
try:
|
||||
joysticks = data["joystick"]
|
||||
if not isinstance(joysticks, Mapping):
|
||||
return None
|
||||
right = joysticks["right"]
|
||||
if not isinstance(right, (list, tuple)) or len(right) != 2:
|
||||
return None
|
||||
value = float(right[0])
|
||||
if not math.isfinite(value) or abs(value) > 1.2:
|
||||
return None
|
||||
return max(-1.0, min(1.0, value))
|
||||
except (KeyError, TypeError, ValueError):
|
||||
return None
|
||||
|
||||
|
||||
def left_z_pressed(data: Mapping[str, Any]) -> bool | None:
|
||||
"""Return the physical left Z state, accepting only explicit binary data."""
|
||||
|
||||
try:
|
||||
buttons = data["button"]
|
||||
if not isinstance(buttons, Mapping):
|
||||
return None
|
||||
left = buttons["left"]
|
||||
if not isinstance(left, (list, tuple)) or len(left) < 3:
|
||||
return None
|
||||
raw = left[2]
|
||||
if isinstance(raw, bool):
|
||||
return raw
|
||||
if isinstance(raw, int) and not isinstance(raw, bool) and raw in (0, 1):
|
||||
return bool(raw)
|
||||
return None
|
||||
except (KeyError, TypeError):
|
||||
return None
|
||||
|
||||
|
||||
def head_pitch_command_axis(data: Mapping[str, Any]) -> float | None:
|
||||
"""Gate right-stick pitch behind left Z; malformed input fails closed."""
|
||||
|
||||
pressed = left_z_pressed(data)
|
||||
axis = right_stick_vertical(data)
|
||||
if pressed is None or axis is None:
|
||||
return None
|
||||
return axis if pressed else 0.0
|
||||
|
||||
|
||||
class HeadPitchController:
|
||||
"""Integrate a joystick velocity request into a bounded pitch target."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
deadzone: float,
|
||||
max_speed_rad_s: float,
|
||||
max_accel_rad_s2: float,
|
||||
min_pitch_rad: float,
|
||||
max_pitch_rad: float,
|
||||
axis_sign: float,
|
||||
max_dt_s: float = 0.1,
|
||||
) -> None:
|
||||
values = (
|
||||
deadzone,
|
||||
max_speed_rad_s,
|
||||
max_accel_rad_s2,
|
||||
min_pitch_rad,
|
||||
max_pitch_rad,
|
||||
axis_sign,
|
||||
max_dt_s,
|
||||
)
|
||||
if not all(math.isfinite(value) for value in values):
|
||||
raise ValueError("head-pitch settings must be finite")
|
||||
if not 0.0 <= deadzone < 1.0:
|
||||
raise ValueError("head-pitch deadzone must be in [0, 1)")
|
||||
if max_speed_rad_s <= 0.0 or max_accel_rad_s2 <= 0.0:
|
||||
raise ValueError("head-pitch speed and acceleration must be positive")
|
||||
if min_pitch_rad >= max_pitch_rad:
|
||||
raise ValueError("head-pitch range is invalid")
|
||||
if axis_sign == 0.0 or max_dt_s <= 0.0:
|
||||
raise ValueError("head-pitch sign and maximum dt must be non-zero")
|
||||
self.deadzone = float(deadzone)
|
||||
self.max_speed_rad_s = float(max_speed_rad_s)
|
||||
self.max_accel_rad_s2 = float(max_accel_rad_s2)
|
||||
self.min_pitch_rad = float(min_pitch_rad)
|
||||
self.max_pitch_rad = float(max_pitch_rad)
|
||||
self.axis_sign = math.copysign(1.0, axis_sign)
|
||||
self.max_dt_s = float(max_dt_s)
|
||||
self.armed = False
|
||||
self.require_neutral = True
|
||||
self.active = False
|
||||
self.target_rad: float | None = None
|
||||
self.velocity_rad_s = 0.0
|
||||
self.last_update_at: float | None = None
|
||||
self.last_transition = "initialized"
|
||||
|
||||
def new_session(self) -> None:
|
||||
self.armed = True
|
||||
self.require_neutral = True
|
||||
self.active = False
|
||||
self.target_rad = None
|
||||
self.velocity_rad_s = 0.0
|
||||
self.last_update_at = None
|
||||
self.last_transition = "new_session"
|
||||
|
||||
def disarm(self, reason: str = "disarmed") -> None:
|
||||
self.armed = False
|
||||
self.require_neutral = True
|
||||
self.active = False
|
||||
self.target_rad = None
|
||||
self.velocity_rad_s = 0.0
|
||||
self.last_update_at = None
|
||||
self.last_transition = reason
|
||||
|
||||
def _shaped_axis(self, axis: float) -> float:
|
||||
magnitude = abs(axis)
|
||||
if magnitude <= self.deadzone:
|
||||
return 0.0
|
||||
normalized = (magnitude - self.deadzone) / (1.0 - self.deadzone)
|
||||
return math.copysign(normalized, axis)
|
||||
|
||||
def update(
|
||||
self,
|
||||
now: float,
|
||||
*,
|
||||
armed: bool,
|
||||
input_healthy: bool,
|
||||
feedback_healthy: bool,
|
||||
external_busy: bool,
|
||||
axis: float | None,
|
||||
feedback_position_rad: float | None,
|
||||
) -> float | None:
|
||||
"""Return one absolute pitch target, or ``None`` when output is gated."""
|
||||
|
||||
if not math.isfinite(now):
|
||||
raise ValueError("head-pitch clock must be finite")
|
||||
if not armed:
|
||||
if self.armed:
|
||||
self.disarm()
|
||||
return None
|
||||
if not self.armed:
|
||||
self.new_session()
|
||||
|
||||
feedback_valid = (
|
||||
feedback_position_rad is not None
|
||||
and math.isfinite(float(feedback_position_rad))
|
||||
)
|
||||
if (
|
||||
not input_healthy
|
||||
or not feedback_healthy
|
||||
or external_busy
|
||||
or axis is None
|
||||
or not feedback_valid
|
||||
):
|
||||
self.require_neutral = True
|
||||
self.active = False
|
||||
self.target_rad = None
|
||||
self.velocity_rad_s = 0.0
|
||||
self.last_update_at = now
|
||||
if external_busy:
|
||||
self.last_transition = "external_command_busy"
|
||||
elif not feedback_healthy or not feedback_valid:
|
||||
self.last_transition = "feedback_unhealthy"
|
||||
else:
|
||||
self.last_transition = "input_unhealthy"
|
||||
return None
|
||||
|
||||
feedback = float(feedback_position_rad)
|
||||
shaped = self._shaped_axis(float(axis))
|
||||
if self.require_neutral:
|
||||
self.active = False
|
||||
self.target_rad = feedback
|
||||
self.velocity_rad_s = 0.0
|
||||
self.last_update_at = now
|
||||
if shaped == 0.0:
|
||||
self.require_neutral = False
|
||||
self.last_transition = "neutral_ready"
|
||||
return None
|
||||
|
||||
if shaped == 0.0:
|
||||
self.active = False
|
||||
self.target_rad = feedback
|
||||
self.velocity_rad_s = 0.0
|
||||
self.last_update_at = now
|
||||
self.last_transition = "neutral"
|
||||
return None
|
||||
|
||||
if self.target_rad is None:
|
||||
self.target_rad = feedback
|
||||
dt = 0.0
|
||||
if self.last_update_at is not None:
|
||||
dt = min(self.max_dt_s, max(0.0, now - self.last_update_at))
|
||||
self.last_update_at = now
|
||||
desired_velocity = shaped * self.axis_sign * self.max_speed_rad_s
|
||||
max_velocity_step = self.max_accel_rad_s2 * dt
|
||||
self.velocity_rad_s += max(
|
||||
-max_velocity_step,
|
||||
min(max_velocity_step, desired_velocity - self.velocity_rad_s),
|
||||
)
|
||||
self.target_rad = max(
|
||||
self.min_pitch_rad,
|
||||
min(
|
||||
self.max_pitch_rad,
|
||||
self.target_rad + self.velocity_rad_s * dt,
|
||||
),
|
||||
)
|
||||
self.active = True
|
||||
self.last_transition = "active"
|
||||
return self.target_rad
|
||||
|
||||
|
||||
__all__ = [
|
||||
"HeadPitchController",
|
||||
"head_pitch_command_axis",
|
||||
"left_z_pressed",
|
||||
"right_stick_vertical",
|
||||
]
|
||||
Reference in New Issue
Block a user