meiqi/TG3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: update hand gestures and direct locomotion

Browse Source
This commit is contained in:
meiqi
2026-08-11 14:35:51 +08:00
parent 89a8c3418f
commit 4cceebfa5f
11 changed files with 469 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

153
tg3_local_teleop/tg3_local_teleop.py
View File

@@ -34,10 +34,12 @@ from std_srvs.srv import Trigger
from data_collection import RecordingToggleGate, left_joystick_pressed
from gesture_toggle import (
GuardedMomentaryGesture,
GestureToggle,
normalized_pose_to_positions,
right_a_pressed,
right_b_pressed,
select_right_hand_target,
select_hand_target,
)
@@ -425,6 +427,28 @@ class LocalTeleopBridge(Node):
int(self.hands_cfg.get("position_min", 1)),
int(self.hands_cfg.get("position_max", 1000)),
)
self.right_a_pose_enabled = self.hands_enabled and bool(
self.hands_cfg.get("right_a_pose_enabled", False)
)
self.right_a_pose = GuardedMomentaryGesture(
release_seconds=float(
self.hands_cfg.get("right_a_pose_release_seconds", 0.5)
)
)
right_a_target = self.hands_cfg.get(
"right_a_pose_positions", [428, 735, 500, 77, 77, 72]
)
self.right_a_pose_target = [int(value) for value in right_a_target]
hand_position_min = int(self.hands_cfg.get("position_min", 1))
hand_position_max = int(self.hands_cfg.get("position_max", 1000))
if len(self.right_a_pose_target) != 6 or not all(
hand_position_min <= value <= hand_position_max
for value in self.right_a_pose_target
):
raise ValueError(
"hands.right_a_pose_positions must contain 6 values within "
"the configured BrainCo position range"
)
self.locomotion_cfg = config.get("locomotion", {})
self.locomotion_enabled = bool(
self.locomotion_cfg.get("enabled", False)
@@ -474,11 +498,11 @@ class LocalTeleopBridge(Node):
self.hand_output_ready = False
self.runtime_hand_output_reasons: list[str] = []
self.foreign_hand_source_seen = False
self.walk_combo_started_at: float | None = None
self.walk_active = False
self.walk_command = [0.0, 0.0]
self.walk_publish_count = 0
self.walk_zero_frames_remaining = 0
self.walk_require_neutral = True
self.last_walk_publish_at = 0.0
self.publisher = self.create_publisher(JointState, ros_cfg["command_topic"], 10)
@@ -994,9 +1018,27 @@ class LocalTeleopBridge(Node):
if gesture_toggled:
state = "ACTIVE" if self.right_point_gesture.active else "INACTIVE"
self.get_logger().warning(
f"RIGHT-HAND POINT GESTURE {state}: right B held for "
f"BILATERAL POINT GESTURE {state}: right B held for "
f"{self.right_point_gesture.hold_seconds:.1f}s"
)
if self.right_a_pose_enabled:
pose_changed = self.right_a_pose.update(
now,
armed=self.armed,
input_healthy=(
self.armed
and sample is not None
and not runtime_hand_reasons
),
pressed=(
None if sample is None else right_a_pressed(sample.data)
),
)
if pose_changed:
state = "ACTIVE" if self.right_a_pose.active else "INACTIVE"
self.get_logger().warning(
f"RIGHT-HAND A POSE {state}: momentary right A"
)
if self.returning_home:
self._tick_home(now)
@@ -1028,15 +1070,34 @@ class LocalTeleopBridge(Node):
self.hand_output_ready = True
desired_hands = self._hand_targets(sample.data)
if self.right_point_gesture_enabled:
for side in HAND_SIDES:
freeze_reference = self.last_hand_commands[side]
if (
freeze_reference is None
or len(freeze_reference) != 6
):
freeze_reference = self.robot_hand_positions[side]
desired_hands[side] = select_hand_target(
desired_hands[side],
freeze_reference,
self.right_point_gesture_target,
active=self.right_point_gesture.active,
freeze=self.right_point_gesture.freeze_right_hand,
)
if self.right_a_pose_enabled:
freeze_reference = self.last_hand_commands["right"]
if freeze_reference is None or len(freeze_reference) != 6:
freeze_reference = self.robot_hand_positions["right"]
desired_hands["right"] = select_right_hand_target(
# Right A has momentary priority over the persistent B
# bilateral gesture on the right hand; releasing A
# returns through the same slew limiter to B or live
# xTELE input. The left B target remains unchanged.
desired_hands["right"] = select_hand_target(
desired_hands["right"],
freeze_reference,
self.right_point_gesture_target,
active=self.right_point_gesture.active,
freeze=self.right_point_gesture.freeze_right_hand,
self.right_a_pose_target,
active=self.right_a_pose.active,
freeze=self.right_a_pose.freeze_right_hand,
)
commands = {
side: self._slew_hand(side, desired_hands[side], now)
@@ -1225,11 +1286,14 @@ class LocalTeleopBridge(Node):
self.active_session_id = session_id
if self.right_point_gesture_enabled:
self.right_point_gesture.new_session()
if self.right_a_pose_enabled:
self.right_a_pose.new_session()
if getattr(self, "data_collection_enabled", False):
self.data_capture_session_id = session_id
self.data_collection_gate.new_session()
self.data_last_transition = "new teleoperation session"
self.armed = True
self.walk_require_neutral = True
# Start both slew limiters at measured robot feedback, never at a
# potentially distant first network target.
self.last_command = list(self.robot_arm_positions or [])
@@ -1294,11 +1358,14 @@ class LocalTeleopBridge(Node):
return
if self.right_point_gesture_enabled:
self.right_point_gesture.new_session()
if self.right_a_pose_enabled:
self.right_a_pose.new_session()
if getattr(self, "data_collection_enabled", False):
self.data_capture_session_id = "direct_" + uuid.uuid4().hex
self.data_collection_gate.new_session()
self.data_last_transition = "new direct-LAN teleoperation session"
self.armed = True
self.walk_require_neutral = True
# Start the slew limiter at measured robot feedback. Using None here
# would make the first armed frame jump directly to the TS1P target.
self.last_command = list(self.robot_arm_positions or [])
@@ -1329,6 +1396,8 @@ class LocalTeleopBridge(Node):
# Existing STOP behavior leaves the physical hand at its last
# limited command until a later, newly armed session.
self.right_point_gesture.disarm(reason)
if self.right_a_pose_enabled:
self.right_a_pose.disarm(reason)
self.armed = False
self.hand_output_ready = False
self.runtime_hand_output_reasons = []
@@ -1359,12 +1428,8 @@ class LocalTeleopBridge(Node):
def _tick_locomotion(self, now: float, sample: ArmSnapshot) -> None:
cfg = self.locomotion_cfg
try:
left_buttons = sample.data["button"]["left"]
right_buttons = sample.data["button"]["right"]
left_joystick = sample.data["joystick"]["left"]
right_joystick = sample.data["joystick"]["right"]
left_z = len(left_buttons) >= 3 and bool(left_buttons[2])
right_c = len(right_buttons) >= 3 and bool(right_buttons[2])
if len(left_joystick) != 2 or len(right_joystick) != 2:
raise ValueError("left and right joysticks must each contain two axes")
# xTELE 0.1.2 stores each TS1P stick as [vertical, horizontal].
@@ -1397,19 +1462,19 @@ class LocalTeleopBridge(Node):
self._stop_locomotion(f"invalid locomotion input: {exc}")
return
forward_active = right_c and shaped_forward != 0.0
yaw_active = left_z and shaped_yaw != 0.0
if not forward_active and not yaw_active:
self._stop_locomotion(
"right C + left forward and left Z + right yaw are both inactive"
)
self._tick_walk_zero_burst(now)
return
forward_active = shaped_forward != 0.0
yaw_active = shaped_yaw != 0.0
if self.walk_require_neutral:
if forward_active or yaw_active:
self._stop_locomotion(
"waiting for both locomotion sticks to return to neutral"
)
self._tick_walk_zero_burst(now)
return
self.walk_require_neutral = False
if self.walk_combo_started_at is None:
self.walk_combo_started_at = now
hold_seconds = float(cfg["hold_seconds"])
if now - self.walk_combo_started_at < hold_seconds:
if not forward_active and not yaw_active:
self._stop_locomotion("both locomotion stick axes are neutral")
self._tick_walk_zero_burst(now)
return
@@ -1417,8 +1482,8 @@ class LocalTeleopBridge(Node):
self.walk_active = True
self.walk_zero_frames_remaining = 0
self.get_logger().warning(
"LOCAL HBWALK VELOCITY STARTED: right C + left-stick forward "
"or left Z + right-stick yaw"
"LOCAL HBWALK VELOCITY STARTED: direct left-stick forward "
"or right-stick yaw"
)
linear_limit = max_forward if signed_forward >= 0.0 else max_reverse
@@ -1436,7 +1501,6 @@ class LocalTeleopBridge(Node):
was_active = self.walk_active
had_nonzero_command = any(abs(value) > 1e-9 for value in self.walk_command)
self.walk_active = False
self.walk_combo_started_at = None
if was_active or had_nonzero_command:
now = time.monotonic()
self._publish_walk(0.0, 0.0, now)
@@ -1942,7 +2006,7 @@ class LocalTeleopBridge(Node):
"right_point_gesture_enabled": self.right_point_gesture_enabled,
"right_point_gesture_binding": (
"right_B hold "
f"{self.right_point_gesture.hold_seconds:.1f}s toggle"
f"{self.right_point_gesture.hold_seconds:.1f}s bilateral toggle"
),
"right_point_gesture_active": (
self.right_point_gesture.active
@@ -1990,12 +2054,43 @@ class LocalTeleopBridge(Node):
if self.right_point_gesture_enabled
else None
),
"right_a_pose_enabled": self.right_a_pose_enabled,
"right_a_pose_binding": "right_A hold; release restores live input",
"right_a_pose_active": (
self.right_a_pose.active if self.right_a_pose_enabled else False
),
"right_a_pose_state": (
self.right_a_pose.state if self.right_a_pose_enabled else "disabled"
),
"right_a_pose_requires_release": (
self.right_a_pose.require_release
if self.right_a_pose_enabled
else False
),
"right_a_pose_release_s": (
round(self.right_a_pose.release_elapsed(now), 2)
if self.right_a_pose_enabled
else 0.0
),
"right_a_pose_activation_count": (
self.right_a_pose.activation_count
if self.right_a_pose_enabled
else 0
),
"right_a_pose_last_transition": (
self.right_a_pose.last_transition
if self.right_a_pose_enabled
else "disabled"
),
"right_a_pose_target_positions": (
self.right_a_pose_target if self.right_a_pose_enabled else None
),
"locomotion_enabled": self.locomotion_enabled,
"locomotion_binding": (
"right_C + left_stick_vertical; "
"left_Z + right_stick_horizontal, immediate"
"left_stick_vertical; right_stick_horizontal, direct"
),
"locomotion_active": self.walk_active,
"locomotion_requires_neutral": self.walk_require_neutral,
"locomotion_hold_s": 0.0,
"locomotion_command": {
"linear_x_m_s": self.walk_command[0],