From 4cceebfa5fba1f38be725204488c9f40fa170773 Mon Sep 17 00:00:00 2001 From: meiqi <2510105031@mails.szu.edu.cn> Date: Tue, 11 Aug 2026 14:35:51 +0800 Subject: [PATCH] feat: update hand gestures and direct locomotion --- .gitignore | 6 + README.md | 8 +- docs/天工3.0同构臂遥操与数采跨设备迁移手册.md | 7 +- docs/天工3.0本地同构臂遥操迁移部署指南.md | 16 +- tg3_local_teleop/README.md | 24 +-- tg3_local_teleop/config.toml | 20 ++- tg3_local_teleop/gesture_toggle.py | 164 +++++++++++++++++- tg3_local_teleop/test_gesture_toggle.py | 85 +++++++++ tg3_local_teleop/test_session_gate.py | 55 +++++- tg3_local_teleop/tg3_local_teleop.py | 153 ++++++++++++---- tg3_omnisocket_transport/README.md | 2 +- 11 files changed, 469 insertions(+), 71 deletions(-) diff --git a/.gitignore b/.gitignore index a2d5cae..1f9a1b8 100644 --- a/.gitignore +++ b/.gitignore @@ -14,6 +14,12 @@ python/build/ *.egg-info/ *.so +# Final datasets stay local and must never be committed. Keep only the two +# tracked instructions/ignore files inside the destination directory. +Data_Get/* +!Data_Get/.gitignore +!Data_Get/README.md + # Local deployment and editor leftovers. .deploy-*/ .DS_Store diff --git a/README.md b/README.md index d691d9c..64ed837 100644 --- a/README.md +++ b/README.md @@ -45,10 +45,12 @@ TOML 解析检查。若还要核对外置 OmniSocketGo 版本，可设置任务 - 左 Z + 右 C 连续 3 秒：开始遥操；再次连续 3 秒：结束并限速回 Home。 若 Hub 尚未运行，本次长按只检测一次并立即结束，不发送控制数据；服务器启动后须先 稳定松开，再重新长按 3 秒。pending 和活动会话都不会自动重试或自动恢复。 -- 右 C + 左摇杆上下：HBWALK 前进/后退。 -- 左 Z + 右摇杆左右：HBWALK 原地转向。 -- 右 B 连续 1 秒：右手进入厂商“单食指”姿态；松开至少 0.5 秒后再次连续 +- 遥操开启且双摇杆先回中过一次后，左摇杆上下直接控制 HBWALK 前进/后退。 +- 遥操开启且双摇杆先回中过一次后，右摇杆左右直接控制 HBWALK 原地转向。 +- 右 B 连续 1 秒：左右手同时进入厂商“单食指”姿态；松开至少 0.5 秒后再次连续 1 秒退出。手指动作继续受 `400 units/s` 限速。 +- 按住右 A：右手限速摆到目标位置 `[428,735,500,77,77,72]`；松开 A 后恢复 + 原有右手输入。A 在每个新遥操会话中必须先稳定松开 0.5 秒，防止跨会话误触。 - 左摇杆按下（L3）连续 1 秒：开始数采；松开至少 0.5 秒后再次连续 1 秒： 结束并保存。只有遥操已开启时才接受；Z+C 结束遥操或安全解除会自动结束数采。 diff --git a/docs/天工3.0同构臂遥操与数采跨设备迁移手册.md b/docs/天工3.0同构臂遥操与数采跨设备迁移手册.md index 4f14bec..420209d 100644 --- a/docs/天工3.0同构臂遥操与数采跨设备迁移手册.md +++ b/docs/天工3.0同构臂遥操与数采跨设备迁移手册.md @@ -628,9 +628,9 @@ python3 -m json.tool Data_Get/sync_status.json 2. 长按左 Z + 右 C 3 秒启动； 3. 先验证双臂小幅单关节跟随； 4. 小幅验证左右扳机对应的双手； -5. 长按右 B 1 秒验证右手单食指，再松开 0.5 秒、长按 1 秒退出； -6. 右 C + 左摇杆小幅验证前后，松开任一输入应立即零速； -7. 左 Z + 右摇杆小幅验证原地转向； +5. 长按右 B 1 秒验证左右手同时单食指，再松开 0.5 秒、长按 1 秒退出； +6. 确认双摇杆回中后，直接小幅推动左摇杆验证前后，回中应立即零速； +7. 直接小幅横推右摇杆验证原地转向，回中应立即零速； 8. L3 长按 1 秒开始短数采，再次长按 1 秒结束； 9. 长按 Z+C 3 秒结束遥操，确认限速回新机器人的 Home。 @@ -741,4 +741,3 @@ EAI： 数采/VERIFIED： 机器人临时副本回收： ``` - diff --git a/docs/天工3.0本地同构臂遥操迁移部署指南.md b/docs/天工3.0本地同构臂遥操迁移部署指南.md index 916019c..f51fe70 100644 --- a/docs/天工3.0本地同构臂遥操迁移部署指南.md +++ b/docs/天工3.0本地同构臂遥操迁移部署指南.md @@ -839,7 +839,7 @@ mode = monitor-only | 左右扳机 BrainCo 手控 | 5003 原始 `hand.position` 存在；当前禁止武装 | | 右 B 长按 1 秒单食指 | 5003 原始 B 键存在；当前禁止武装 | | Z+C 长按 3 秒启动/停止、STOP 后限速 Home | 改由机器人本地计时；断流可锁存按键，当前禁止武装 | -| C + 左摇杆行走、Z + 右摇杆转向 | 代码路径存在；当前必须 `enabled=false` | +| 左摇杆直接行走、右摇杆直接转向 | 代码路径存在；当前必须 `enabled=false` | | xTELE 5001 处理后的其他六维组合手势 | 不保留；机器人只订阅 5003 | | OmniSocket session ID、Peer 校验、300 ms 晚包过滤 | 不使用 | | 公网 Hub、EAI OmniSocket sender | 不需要 | @@ -855,7 +855,7 @@ Hub 模式，不是本节的“完全无 Hub”直连。 因此纯 ZMQ 模式断网后： - 最后一帧双臂和手目标仍会以桥的周期重复发布； -- 如果行走仍启用且最后一帧是非零速度组合，非零 `/hric/robot/cmd_vel` 也会持续发布； +- 如果行走仍启用且最后一帧摇杆速度非零，非零 `/hric/robot/cmd_vel` 也会持续发布； - 如果最后一帧仍按住 Z+C，本地 3 秒计时会在没有新数据时继续，可能在断网后武装或停止； - 网络恢复后会直接继续当前 armed 会话，不要求重新 Z+C； - `connected=true` 不能证明数据仍然新鲜。 @@ -914,8 +914,8 @@ systemctl --user enable --now tg3-omnisocket-sender.service 5. 松开两个扳机，长按左 Z + 右 C 3 秒启动； 6. 先做小幅单关节跟随，再逐渐扩大动作； 7. 分别小幅扣左右扳机，验证双手方向、范围和限速； -8. 保持右摇杆回中，连续长按右 B 1 秒，确认右手以 `400 units/s` 限速进入厂商 - “单食指”姿态；松开至少 0.5 秒后再次长按 1 秒，确认退出并恢复正常右手跟随； +8. 保持右摇杆回中，连续长按右 B 1 秒，确认左右手都以 `400 units/s` 限速进入厂家 + “单食指”姿态；松开至少 0.5 秒后再次长按 1 秒，确认退出并恢复双手正常跟随； 9. 仅在 OmniSocket/5001 合并模式下：先停止并确认 `armed=false`，按飞书指南逐个选择 其他手势组合键；检查 EAI `status.json` 的 `command_frames_accepted` 增长，并在 EAI 本机 5001 抓帧确认处理后的 @@ -923,11 +923,9 @@ systemctl --user enable --now tg3-omnisocket-sender.service 10. 仅在 OmniSocket/5001 合并模式下：再次武装后检查 `command_hand_merges` 增长和机器人 `iarm_hand_position` 为六维， 再只做小幅动作，逐个验证所需的其他组合手势方向和限速； -11. 仅在 `[locomotion].enabled=true` 且相应链路断流保护已经验证后：保持右 C，把左摇杆 - 小幅向前推出死区，确认下一个 50 Hz 周期立即响应；松 C，确认 - 立即零速停止。再次按 C/推杆也应立即响应，不再等待 3 秒； -12. 在同一行走前提下，保持左 Z，把右摇杆小幅横推，确认机器人原地转向；松开任一输入 - 应立即清零角速度； +11. 仅在 `[locomotion].enabled=true` 且相应链路断流保护已经验证后：先确认双摇杆回中， + 再把左摇杆小幅向前推出死区，确认下一个 50 Hz 周期立即响应；回中后确认立即零速停止； +12. 在同一行走前提下，直接把右摇杆小幅横推，确认机器人原地转向；回中应立即清零角速度； 13. 再次长按 Z+C 停止，观察双臂限速回到新机器人保存的 Home； 14. 仅在 OmniSocket 模式、机器人已静止且急停可用时测试 Hub 断线：EAI sender 的 KCP 反馈超时约为 500 ms，触发后会作废 session；机器人若持续约 2 秒收不到活动帧，会另行 diff --git a/tg3_local_teleop/README.md b/tg3_local_teleop/README.md index 0522efe..37b4e88 100644 --- a/tg3_local_teleop/README.md +++ b/tg3_local_teleop/README.md @@ -13,7 +13,7 @@ TS1P 同构臂 -> 天工 3.0 双臂 -> /left_hand/set_motor_multi + /right_hand/set_motor_multi -> 天工 3.0 BrainCo Revo2 双灵巧手 - -> /hric/robot/cmd_vel（右 C + 左摇杆前后 / 左 Z + 右摇杆转向；50 Hz TwistStamped） + -> /hric/robot/cmd_vel（左摇杆前后 / 右摇杆转向；50 Hz TwistStamped） -> 天工 3.0 HBWALK 行走 ``` @@ -29,9 +29,9 @@ TS1P 同构臂 - 回 Home 过程中用新的 Z+C 3 秒会话可取消回位并重新启动遥操。 - 本地桥控制左右各 7 个手臂关节、BrainCo Revo2 双灵巧手和 HBWALK 前后/转向；不控制腰和头。 -- 行走：遥操已启动后，右 `C` + 左摇杆上下控制前后，左 `Z` + 右摇杆左右控制原地 - 转向；两个组合都在下一个 `50 Hz` 周期立即响应，不再等待 3 秒。松开对应按键或 - 摇杆回中立即把该轴清零，再按也立即恢复。 +- 行走：遥操已启动且双摇杆先回中过一次后，左摇杆上下直接控制前后，右摇杆左右 + 直接控制原地转向；无需组合键，在下一个 `50 Hz` 周期立即响应。摇杆回中立即把 + 对应轴清零，再推动也立即恢复。 前后仍使用二次细控曲线；转向在死区后使用线性曲线，使右摇杆中段有足够角速度， 但最大值仍受官方 `0.8 rad/s` 上限约束。 按官方半身行走 Topic 范围限幅：前进 `1.0 m/s`、后退 `0.8 m/s`、转向 @@ -43,15 +43,19 @@ TS1P 同构臂 - 同构臂当前的一维手部开合量以机器人实测默认打开姿态为 0 端点，并按厂家 xTELE 的 BrainCoRevo2 保守“常规”抓握姿态作为 1 端点映射到六电机； 若 xTELE 后续直接输出六维归一化位置，会自动使用六维位置。 -- 右手指向手势：遥操已启动且灵巧手反馈健康时，连续长按右 `B` 1 秒开启；松开 +- 双手指向手势：遥操已启动且灵巧手反馈健康时，连续长按右 `B` 1 秒开启；松开 至少 `0.5 s` 后，再连续长按右 `B` 1 秒关闭。首次启动和每个新遥操会话都必须先 稳定松开 B，保持同一次按压不会反复切换。计时未满、中途松开、输入畸形或反馈 - 中断均不触发；计时期间冻结右手最后一条命令，左手和双臂仍照常跟随。 -- 指向只覆盖右手。目标采用工控机 xTELE `GestureController` 的 BrainCoRevo2 第 2 号 + 中断均不触发；计时期间冻结双手各自最后一条命令，双臂仍照常跟随。 +- 指向同时覆盖左右手。目标采用工控机 xTELE `GestureController` 的 BrainCoRevo2 第 2 号 “单食指”手势 `state 0`：归一化目标 `[0.2, 0.688, 0.0, 0.98, 0.98, 0.98]`，对应当前 `1~1000` 位置范围约为 `[201, 688, 1, 980, 980, 980]`；顺序为大拇指弯曲、大拇指旋转、食指、中指、 无名指、小拇指。进入和退出手势都继续使用每秒最多 400 个位置单位的现有限速。 +- 右 `A` 是右手瞬时姿态键：按住时采用六电机目标位置 + `[428, 735, 500, 77, 77, 72]`，松开后恢复已有右手输入；若右 B 双手指向手势已开启， + A 按住期间只在右手优先，左手维持单食指；松开后右手恢复单食指。A 也必须在新会话中先稳定松开 `0.5 s`， + 全程继续使用同一个 `400 units/s` 限速器。 ## 安全门控 @@ -65,8 +69,8 @@ HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node` 实际发出的速度由本桥按官方 Topic 范围限制为 `linear.x=[-0.8, 1.0] m/s`、 `angular.z=[-0.8, 0.8] rad/s`；本桥不发布侧移速度。 -行走速度只在同一次遥操武装、`HBWALK/HBWALK/running`、网络数据新鲜且右 C + 左摇杆 -前后或左 Z + 右摇杆横向即时成立时发布。解除组合后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、 +行走速度只在同一次遥操武装、`HBWALK/HBWALK/running`、网络数据新鲜且左摇杆前后 +或右摇杆横向越过死区时发布。摇杆回中后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、 回 Home 或机器人退出 HBWALK 也走同一停止逻辑。 灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个 @@ -74,7 +78,7 @@ HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node` 和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态 或检测到另一命令源时，双臂和双手会一起解除武装并停止发布。停止遥操后灵巧手保持 最后目标，双臂按原逻辑限速回 Home。BrainCo 驱动原有的电流、堵转和碰撞保护不作修改。 -匹配的 STOP、安全解除武装或服务退出都会清除右 B 指向手势的逻辑状态，但不会在 +匹配的 STOP、安全解除武装或服务退出都会清除右 B 与右 A 手势的逻辑状态，但不会在 STOP 后额外发送张手/恢复命令；下一次会话从机器人实测手指位置重新限速跟随。 ## 限速双臂回 Home diff --git a/tg3_local_teleop/config.toml b/tg3_local_teleop/config.toml index 3b1131a..617232c 100644 --- a/tg3_local_teleop/config.toml +++ b/tg3_local_teleop/config.toml @@ -172,16 +172,15 @@ topics = [ "/ob_camera_waist/depth/image_raw/compressedDepth" = 20.0 [locomotion] -# The two original immediate bindings are independent: right C + left-stick -# vertical controls translation; left Z + right-stick horizontal controls -# in-place yaw. Both act on the next 50 Hz tick, with no second hold. +# Once Z+C has armed teleoperation and both sticks have been observed neutral, +# left-stick vertical directly controls translation and right-stick horizontal +# directly controls in-place yaw. No locomotion button or second hold is used. enabled = true command_topic = "/hric/robot/cmd_vel" # This bridge never publishes FSM commands. The robot must already report # HBWALK/running through the existing safety gate before velocity is allowed. # Match the TG3 secondary-development TwistStamped example. frame_id = "pelvis" -hold_seconds = 0.0 joystick_deadzone = 0.2 joystick_expo = 2.0 # Turning is linear after the deadzone so medium right-stick travel is not @@ -224,8 +223,8 @@ invert_scalar = false open_normalized = [0.4, 0.4, 0.05, 0.05, 0.05, 0.05] closed_normalized = [0.9, 0.5, 0.45, 0.52, 0.52, 0.45] -# Right B owns a robot-side, release-guarded pointing gesture while a teleop -# session is armed. Both activation and deactivation require one continuous +# Right B owns a robot-side, release-guarded bilateral pointing gesture while +# a teleop session is armed. Both activation and deactivation require one continuous # 1-second hold, separated by at least 0.5 seconds of stable release. A new # session is always release-locked. The pose is xTELE GestureController's # BrainCoRevo2 gesture 2 ("single index finger"), state 0, in motor order: @@ -235,6 +234,15 @@ right_b_point_gesture_hold_seconds = 1.0 right_b_point_gesture_release_seconds = 0.5 right_b_point_pose_normalized = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98] +# Right A is a momentary, robot-side pose: hold A to move the right hand to +# this measured BrainCo MotorStatus position and release A to resume the +# existing right-hand input (or an active right-B gesture). A new teleop +# session must first observe A released for 0.5 seconds. The normal hand slew +# limit remains in force; these are raw driver positions, not normalized data. +right_a_pose_enabled = true +right_a_pose_release_seconds = 0.5 +right_a_pose_positions = [428, 735, 500, 77, 77, 72] + [home] # Deliberately slower than manual teleoperation. This Home pose was captured # from the robot's real arm feedback on 2026-08-06. Joint order is 11..17, diff --git a/tg3_local_teleop/gesture_toggle.py b/tg3_local_teleop/gesture_toggle.py index bc7f94f..b99b60e 100644 --- a/tg3_local_teleop/gesture_toggle.py +++ b/tg3_local_teleop/gesture_toggle.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Pure state machine for the right-B pointing-hand gesture. +"""Pure state machines for robot-side right-hand button gestures. 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. @@ -182,6 +182,145 @@ class GestureToggle: return "active" if self.active else "idle" +class GuardedMomentaryGesture: + """Hold one gesture only while its button is pressed. + + A newly armed teleoperation session must first observe a stable release, + preventing a button held before START from moving the hand. Missing or + malformed input fails closed and requires another stable release. + """ + + def __init__(self, release_seconds: float) -> None: + if not math.isfinite(release_seconds) or release_seconds <= 0.0: + raise ValueError("gesture release time must be positive and finite") + self.release_seconds = float(release_seconds) + self.active = False + 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.activation_count = 0 + self.last_transition = "initialized" + + def new_session(self) -> None: + self.active = False + 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: + self.active = False + 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 gate and report only active/inactive edge changes.""" + + if not math.isfinite(now): + raise ValueError("gesture clock must be finite") + was_active = self.active + if not armed: + if self.armed or self.active: + self.disarm() + return was_active != self.active + 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.active = False + self.release_started_at = None + self.require_release = True + self._freeze_right_hand = True + self.last_transition = "input_unhealthy" + return was_active != self.active + + if self.require_release: + self.active = False + self._freeze_right_hand = True + if pressed: + self.release_started_at = None + elif self.release_started_at is None: + self.release_started_at = now + elif now - self.release_started_at >= self.release_seconds: + self.require_release = False + self.release_started_at = None + self._freeze_right_hand = False + self.last_transition = "release_ready" + return was_active != self.active + + self.release_started_at = None + self._freeze_right_hand = False + self.active = bool(pressed) + if self.active != was_active: + if self.active: + self.activation_count += 1 + self.last_transition = "activated" + else: + self.last_transition = "deactivated" + return was_active != self.active + + @property + def freeze_right_hand(self) -> bool: + return self.armed and not self.active and self._freeze_right_hand + + 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" + return "active" if self.active else "idle" + + +def right_a_pressed(data: Mapping[str, Any]) -> bool | None: + """Return TS1P right-A 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) < 1: + return None + value = right[0] + 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 right_b_pressed(data: Mapping[str, Any]) -> bool | None: """Return TS1P right-B state, or ``None`` for a malformed sample.""" @@ -219,7 +358,7 @@ def normalized_pose_to_positions( ] -def select_right_hand_target( +def select_hand_target( source_target: Sequence[int], previous_command: Sequence[int] | None, gesture_target: Sequence[int], @@ -227,7 +366,7 @@ def select_right_hand_target( active: bool, freeze: bool, ) -> list[int]: - """Apply the right-only gesture override or fail-safe input freeze.""" + """Apply one hand's gesture override or fail-safe input freeze.""" source = list(source_target) gesture = list(gesture_target) @@ -241,3 +380,22 @@ def select_right_hand_target( raise ValueError("cannot freeze without a complete previous command") return previous return source + + +def select_right_hand_target( + source_target: Sequence[int], + previous_command: Sequence[int] | None, + gesture_target: Sequence[int], + *, + active: bool, + freeze: bool, +) -> list[int]: + """Backward-compatible name for the original right-only implementation.""" + + return select_hand_target( + source_target, + previous_command, + gesture_target, + active=active, + freeze=freeze, + ) diff --git a/tg3_local_teleop/test_gesture_toggle.py b/tg3_local_teleop/test_gesture_toggle.py index 871e9ee..9304c10 100644 --- a/tg3_local_teleop/test_gesture_toggle.py +++ b/tg3_local_teleop/test_gesture_toggle.py @@ -4,9 +4,12 @@ from __future__ import annotations import unittest from gesture_toggle import ( + GuardedMomentaryGesture, GestureToggle, normalized_pose_to_positions, + right_a_pressed, right_b_pressed, + select_hand_target, select_right_hand_target, ) @@ -227,6 +230,13 @@ class GestureToggleTest(unittest.TestCase): class GestureHelpersTest(unittest.TestCase): + def test_right_a_is_index_zero_and_malformed_values_fail_closed(self) -> None: + self.assertTrue(right_a_pressed({"button": {"right": [1, 0, 0]}})) + self.assertFalse(right_a_pressed({"button": {"right": [0, 1, 1]}})) + self.assertIsNone(right_a_pressed({"button": {"right": []}})) + self.assertIsNone(right_a_pressed({"button": {"right": [-1]}})) + self.assertIsNone(right_a_pressed({"button": {"right": [0.5]}})) + def test_right_b_is_index_one_and_malformed_values_fail_closed(self) -> None: self.assertTrue(right_b_pressed({"button": {"right": [0, 1, 0]}})) self.assertFalse(right_b_pressed({"button": {"right": [1, 0, 1]}})) @@ -263,6 +273,81 @@ class GestureHelpersTest(unittest.TestCase): source, ) + def test_same_point_target_can_be_applied_to_both_hands(self) -> None: + point = [201, 688, 1, 980, 980, 980] + sources = { + "left": [401, 401, 51, 51, 51, 51], + "right": [402, 402, 52, 52, 52, 52], + } + previous = { + "left": [410, 420, 60, 70, 80, 90], + "right": [430, 440, 100, 110, 120, 130], + } + selected = { + side: select_hand_target( + sources[side], + previous[side], + point, + active=True, + freeze=False, + ) + for side in ("left", "right") + } + self.assertEqual(selected, {"left": point, "right": point}) + + +class GuardedMomentaryGestureTest(unittest.TestCase): + def setUp(self) -> None: + self.gesture = GuardedMomentaryGesture(release_seconds=0.5) + self.gesture.new_session() + + def test_held_at_session_start_cannot_activate(self) -> None: + self.assertFalse( + self.gesture.update( + 0.0, armed=True, input_healthy=True, pressed=True + ) + ) + self.assertFalse( + self.gesture.update( + 10.0, armed=True, input_healthy=True, pressed=True + ) + ) + self.assertFalse(self.gesture.active) + self.assertTrue(self.gesture.freeze_right_hand) + + def test_press_is_immediate_after_stable_release_and_release_restores(self) -> None: + self.gesture.update(0.0, armed=True, input_healthy=True, pressed=False) + self.gesture.update(0.5, armed=True, input_healthy=True, pressed=False) + self.assertFalse(self.gesture.require_release) + self.assertTrue( + self.gesture.update( + 0.51, armed=True, input_healthy=True, pressed=True + ) + ) + self.assertTrue(self.gesture.active) + self.assertEqual(self.gesture.activation_count, 1) + self.assertTrue( + self.gesture.update( + 0.52, armed=True, input_healthy=True, pressed=False + ) + ) + self.assertFalse(self.gesture.active) + self.assertFalse(self.gesture.freeze_right_hand) + + def test_malformed_input_deactivates_and_requires_release(self) -> None: + self.gesture.update(0.0, armed=True, input_healthy=True, pressed=False) + self.gesture.update(0.5, armed=True, input_healthy=True, pressed=False) + self.gesture.update(0.6, armed=True, input_healthy=True, pressed=True) + self.assertTrue(self.gesture.active) + self.assertTrue( + self.gesture.update( + 0.7, armed=True, input_healthy=True, pressed=None + ) + ) + self.assertFalse(self.gesture.active) + self.assertTrue(self.gesture.require_release) + self.assertTrue(self.gesture.freeze_right_hand) + if __name__ == "__main__": unittest.main() diff --git a/tg3_local_teleop/test_session_gate.py b/tg3_local_teleop/test_session_gate.py index de068d6..698fbc2 100644 --- a/tg3_local_teleop/test_session_gate.py +++ b/tg3_local_teleop/test_session_gate.py @@ -82,6 +82,7 @@ class RobotSessionGateTest(unittest.TestCase): bridge.cfg = {"control": {"auto_home_on_stop": True}} bridge.hands_enabled = False bridge.right_point_gesture_enabled = False + bridge.right_a_pose_enabled = False bridge.robot_arm_positions = [0.0] * 14 bridge.last_command = None bridge.last_publish_at = 0.0 @@ -229,10 +230,9 @@ class RobotSessionGateTest(unittest.TestCase): bridge._source_restart_reason(12.0, active), ) - def test_locomotion_reacts_immediately_after_repress(self) -> None: + def test_locomotion_reacts_directly_to_sticks_without_buttons(self) -> None: bridge = LocalTeleopBridge.__new__(LocalTeleopBridge) bridge.locomotion_cfg = { - "hold_seconds": 0.0, "joystick_deadzone": 0.2, "joystick_expo": 2.0, "yaw_joystick_expo": 1.0, @@ -243,10 +243,10 @@ class RobotSessionGateTest(unittest.TestCase): "yaw_axis_sign": -1.0, "zero_burst_frames": 10, } - bridge.walk_combo_started_at = None bridge.walk_active = False bridge.walk_command = [0.0, 0.0] bridge.walk_zero_frames_remaining = 0 + bridge.walk_require_neutral = False bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge) published: list[tuple[float, float]] = [] @@ -262,7 +262,7 @@ class RobotSessionGateTest(unittest.TestCase): { "button": { "left": [False, False, False], - "right": [False, False, True], + "right": [False, False, False], }, "joystick": { "left": [1.0, 0.0], @@ -278,7 +278,7 @@ class RobotSessionGateTest(unittest.TestCase): "right": [False, False, False], }, "joystick": { - "left": [1.0, 0.0], + "left": [0.0, 0.0], "right": [0.0, 0.0], }, }, @@ -294,7 +294,7 @@ class RobotSessionGateTest(unittest.TestCase): turning = ArmSnapshot( { "button": { - "left": [False, False, True], + "left": [False, False, False], "right": [False, False, False], }, "joystick": { @@ -323,6 +323,49 @@ class RobotSessionGateTest(unittest.TestCase): bridge._tick_locomotion(1.4, old_wrong_binding) self.assertEqual(published[-1], (0.0, 0.0)) + def test_locomotion_requires_one_neutral_sample_after_arming(self) -> None: + bridge = LocalTeleopBridge.__new__(LocalTeleopBridge) + bridge.locomotion_cfg = { + "joystick_deadzone": 0.2, + "joystick_expo": 2.0, + "yaw_joystick_expo": 1.0, + "max_forward_m_s": 1.0, + "max_reverse_m_s": 0.8, + "max_angular_rad_s": 0.8, + "forward_axis_sign": 1.0, + "yaw_axis_sign": -1.0, + "zero_burst_frames": 10, + } + bridge.walk_active = False + bridge.walk_command = [0.0, 0.0] + bridge.walk_zero_frames_remaining = 0 + bridge.walk_require_neutral = True + bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge) + published: list[tuple[float, float]] = [] + + def publish( + self: object, linear_x: float, angular_z: float, _now: float + ) -> None: + published.append((linear_x, angular_z)) + self.walk_command = [linear_x, angular_z] + + bridge._publish_walk = MethodType(publish, bridge) + deflected = ArmSnapshot( + {"joystick": {"left": [1.0, 0.0], "right": [0.0, 0.0]}}, + received_at=2.0, + ) + neutral = ArmSnapshot( + {"joystick": {"left": [0.0, 0.0], "right": [0.0, 0.0]}}, + received_at=2.1, + ) + bridge._tick_locomotion(2.0, deflected) + self.assertTrue(bridge.walk_require_neutral) + self.assertEqual(bridge.walk_command, [0.0, 0.0]) + bridge._tick_locomotion(2.1, neutral) + self.assertFalse(bridge.walk_require_neutral) + bridge._tick_locomotion(2.2, deflected) + self.assertEqual(published[-1], (1.0, -0.0)) + def test_l3_hold_reaches_nonblocking_capture_request(self) -> None: bridge = LocalTeleopBridge.__new__(LocalTeleopBridge) bridge.data_collection_enabled = True diff --git a/tg3_local_teleop/tg3_local_teleop.py b/tg3_local_teleop/tg3_local_teleop.py index 35c1f00..d139677 100755 --- a/tg3_local_teleop/tg3_local_teleop.py +++ b/tg3_local_teleop/tg3_local_teleop.py @@ -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], diff --git a/tg3_omnisocket_transport/README.md b/tg3_omnisocket_transport/README.md index 33e1dca..c23667e 100644 --- a/tg3_omnisocket_transport/README.md +++ b/tg3_omnisocket_transport/README.md @@ -29,7 +29,7 @@ xTELE 已处理的标量或六维 BrainCoRevo2 目标。5002 的界面事件和 右侧原始按键顺序是 `A/B/C`。按住右 `B` 时，sender 仍合并 5001 的左手 目标，但保留 5003 的原始右手开合量，不用 xTELE 处理后的右手目标覆盖。 -这样右 B 的自定义长按手势还未满 1 秒时，原厂 B 处理不会让右手提前动作。 +这样右 B 的自定义双手长按手势还未满 1 秒时，原厂 B 处理不会让右手提前动作。 松开 B 并连续稳定释放 `0.5 s` 后，还要等新鲜 5001 右手目标连续至少 `0.25 s` 恢复到按键前基线或当前 5003 原始开合量，才恢复双侧合并；无法确认时持续使用原始 右手值，防止释放沿后的锁存/延迟目标漏入。