From 7f8ebefadcef2b707392d5c1e125a653218ab6f6 Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Mon, 8 Jun 2026 15:48:51 +0800 Subject: [PATCH] update trigger control logic --- README.md | 16 +++++++------- docs/pico_udp_sender_ubuntu22_setup.md | 14 ++++++------ xr_rm_bringup/launch/arm_debug.launch.py | 2 +- xr_rm_teleop/xr_rm_teleop/fun_peripheral.py | 2 +- .../single_arm_velocity_teleop.py | 22 +++++++++++-------- 5 files changed, 30 insertions(+), 26 deletions(-) diff --git a/README.md b/README.md index 4ed320a..80d03cd 100755 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ PICO/XR 双手柄 UDP JSON - PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。 - 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。 - RM75 真机连接适配，包含 `rm_movep_canfd` 位姿透传、安全速度/加速度配置、可选初始化点位移动。 -- 真机模式下，按住对应手柄 `grip` 时可用 `trigger` 控制对应夹爪：`trigger >= 0.95` 闭合，否则打开。 +- 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。 - Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。 - 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip`、`trigger` 和 pose 诊断字段。 @@ -218,8 +218,8 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ - `follow`：传给 `rm_movep_canfd` 的跟随标志，默认 `false`。 - `configure_safety_limits`：连接真机后是否配置速度/加速度安全参数，默认 `true`。 - `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。 -- `enable_trigger_gripper_control`：是否允许按住 `grip` 时用 `trigger` 控制对应夹爪，默认 `true`。 -- `trigger_close_threshold`：夹爪闭合阈值，默认 `0.95`；低于该值且 `grip=true` 时打开。 +- `enable_trigger_gripper_control`：是否允许用 `trigger` 点击切换对应夹爪状态，默认 `true`。 +- `trigger_close_threshold`：trigger 点击判定阈值，默认 `0.95`。 - `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。 - `move_to_initial_pose_on_connect`：连接后是否执行 `movej`/`movel` 初始化，默认 `false`。 @@ -260,7 +260,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ ## 末端工具开合 -真机 launch 默认会在遥操作节点内启用工具控制。按住左/右手柄 `grip` 时，对应手柄 `trigger >= 0.95` 会闭合对应夹爪，`trigger < 0.95` 会打开对应夹爪；`grip=false` 时 trigger 不会改变夹爪状态。 +真机 launch 默认会在遥操作节点内启用工具控制。左/右手柄 `trigger` 从低于阈值按到 `>= 0.95` 时，会切换一次对应夹爪开/关状态，并保持到下一次点击。`grip` 仍只控制机械臂运动，不影响夹爪 trigger 切换。 也可以用 Bool 话题手动控制开合，`true` 表示打开，`false` 表示闭合： @@ -272,7 +272,7 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: tr ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: false}" ``` -桌面 UI 的 `Left Arm` 和 `Right Arm` 模式里也有对应的 Tool Open/Close 命令项；`Dual Arm` 真机模式下可直接通过左右手柄 `grip+trigger` 分别控制夹爪。 +桌面 UI 的 `Left Arm` 和 `Right Arm` 模式里也有对应的 Tool Open/Close 命令项；`Dual Arm` 真机模式下可直接通过左右手柄 `trigger` 分别切换夹爪。 ## UDP 数据格式 @@ -333,7 +333,7 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa - `seq`：Unity 端递增包序号，用于后续丢包分析。 - `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。 - `grip`：运动使能。`true` 时进入相对位移控制，`false` 时停止。 -- `trigger`：扳机值，范围 `0.0-1.0`。真机模式下需要同时按住 `grip` 才会驱动对应夹爪，`>=0.95` 闭合，否则打开。 +- `trigger`：扳机值，范围 `0.0-1.0`。真机模式下跨过 `0.95` 的上升沿会切换对应夹爪开/关状态。 - `pos`：手柄位置，长度 3。 - `quat`：手柄姿态四元数，默认按 `xyzw` 解析。 - `pose_valid`：姿态是否可信。ROS 接收端看到 `false` 会强制 `grip=false`。 @@ -354,8 +354,8 @@ PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并 3. 单臂启动，`move_to_initial_pose_on_connect:=false`。 4. 手握急停，按住 `grip` 后只做小幅单轴移动。 5. 逐个确认上/下、前/后、左/右方向。 -6. 保持 `grip=true`，轻按/松开对应 `trigger`，确认对应夹爪闭合/打开且左右不串臂。 -7. 确认松开 `grip` 后机械臂慢停，`/xr_rm//cmd_vel` 回到零；此时 trigger 不应再改变夹爪。 +6. 点击对应 `trigger`，确认每次点击都会切换对应夹爪状态，松开 trigger 后状态保持且左右不串臂。 +7. 确认松开 `grip` 后机械臂慢停，`/xr_rm//cmd_vel` 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。 8. 左右臂都确认后，再进入双臂模式。 当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。 diff --git a/docs/pico_udp_sender_ubuntu22_setup.md b/docs/pico_udp_sender_ubuntu22_setup.md index c3ef7cc..95950fe 100644 --- a/docs/pico_udp_sender_ubuntu22_setup.md +++ b/docs/pico_udp_sender_ubuntu22_setup.md @@ -262,7 +262,7 @@ sudo ufw status - Trigger / A / X：编辑当前行或触发当前行 - B / Y：保存并应用 - Menu：在配置面板和运行 HUD 之间切换 -- Grip：ROS 侧遥操作使能；未按下时机械臂停止，且 trigger 不驱动夹爪 +- Grip：ROS 侧遥操作使能；未按下时机械臂停止 验证现象： @@ -271,7 +271,7 @@ sudo ufw status - HUD 显示包计数、发送端点、左右姿态状态和 KeepAwake 状态。 - `pose_valid=false` 时，ROS 接收端会强制该手柄 `grip=false`，即使 PICO 端按下了 Grip。 - 按住 `grip` 并移动手柄时，mock 模式下 `/xr_rm/*/target_pose` 应连续变化，`/xr_rm/*/cmd_vel` 会显示目标位姿变化率。 -- 真机模式下，必须按住对应手柄 `grip` 才能用 `trigger` 控制对应夹爪；`trigger >= 0.95` 闭合，否则打开。 +- 真机模式下，点击对应手柄 `trigger` 会切换并保持对应夹爪开/关状态。 - 松开 `grip` 后，机械臂慢停，`cmd_vel` 应回到零。 ## 9. ROS2 端验证 @@ -310,7 +310,7 @@ ros2 topic hz /xr/right_controller - 按左手 `grip` 时，`/xr/left_controller` 的 `grip` 变成 `true`。 - 按右手 `grip` 时，`/xr/right_controller` 的 `grip` 变成 `true`。 - 扳机从松开到按下时，`trigger` 从 `0.0` 附近变到 `1.0` 附近。 -- 单臂或双臂真机 launch 已启动时，只有 `grip=true` 的那只手柄会让对应夹爪响应 trigger。 +- 单臂或双臂真机 launch 已启动时，对应手柄每次点击 `trigger` 都会切换并保持对应夹爪状态。 - 平移手柄时，`pose.position` 连续变化。 如果收不到包，先抓 UDP： @@ -428,8 +428,8 @@ Unity APK 每个周期发送一个双手柄 JSON 包： | `frame_id` | string | 默认 `xr_world` | | `controllers.left` | object | 左手柄 | | `controllers.right` | object | 右手柄 | -| `grip` | bool | ROS 遥操作启停；也作为 trigger 夹爪控制使能 | -| `trigger` | float | `0.0-1.0`；真机模式下 `grip=true` 且 `trigger >= 0.95` 闭合夹爪，否则打开 | +| `grip` | bool | ROS 遥操作启停 | +| `trigger` | float | `0.0-1.0`；真机模式下跨过 `0.95` 的上升沿切换夹爪开/关状态 | | `pos` | float[3] | `[x, y, z]` | | `quat` | float[4] | `[qx, qy, qz, qw]` | | `pose_valid` | bool | 姿态有效性；`false` 时 ROS receiver 强制 `grip=false` | @@ -494,7 +494,7 @@ PXR `pxr_predict` 原始坐标按现场实测通常表现为右移 `native.z+` - 机械臂工作区清空。 - PICO topic 在 mock 下已经稳定。 - `grip=false` 时机械臂慢停，`/xr_rm//cmd_vel` 为 0。 -- 夹爪验证时必须保持对应手柄 `grip=true`，再用 `trigger` 从松开到按下确认打开/闭合。 +- 夹爪验证时点击对应手柄 `trigger`，确认每次点击都会切换并保持开/关状态。 - `move_to_initial_pose_on_connect` 保持 `false`。 单臂真机： @@ -523,7 +523,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ | ROS2 收不到 UDP | 检查同一 Wi-Fi、Target IP、Target Port、UFW、`UDP Sending ON`、左右手柄是否 `valid pxr/unity` | | tcpdump 有包但 topic 没数据 | JSON 字段不对，确认包含 `controllers.left/right`，且 `pos` 长度 3、`quat` 长度 4 | | 有 topic 但机械臂不动 | 检查 `grip`、`pose_valid` 日志、teleop 节点订阅话题、工作空间限幅、UDP 超时 | -| trigger 不控制夹爪 | 确认是真机 launch、`enable_tool_control=true`、已按住对应 `grip`，且 `trigger` 能到 0.95 以上 | +| trigger 不控制夹爪 | 确认是真机 launch、`enable_tool_control=true`，且 `trigger` 能从低值跨到 0.95 以上 | | 按左手右臂动 | PICO 端 left/right 获取或填充反了 | | 松开 grip 后仍有速度 | 确认 PICO 持续发送 `grip=false`，并检查 teleop 超时停止 | | 经常超时 | 发送频率太低、网络丢包、PICO 应用后台暂停；提高 `sendHz` 并保持应用前台运行 | diff --git a/xr_rm_bringup/launch/arm_debug.launch.py b/xr_rm_bringup/launch/arm_debug.launch.py index a99b3ce..fa3eae9 100644 --- a/xr_rm_bringup/launch/arm_debug.launch.py +++ b/xr_rm_bringup/launch/arm_debug.launch.py @@ -272,7 +272,7 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument("configure_safety_limits", default_value="true"), # 工具控制通过遥操作节点复用同一个 RealMan 连接，避免两个进程抢同一机械臂连接。 DeclareLaunchArgument("enable_tool_control", default_value="true"), - # 按住 grip 时，trigger >= 阈值闭合夹爪，否则打开；grip=false 时不改变夹爪。 + # trigger 上升沿切换夹爪开/关；grip 仍只控制机械臂运动。 DeclareLaunchArgument("enable_trigger_gripper_control", default_value="true"), DeclareLaunchArgument("trigger_close_threshold", default_value="0.95"), # 连接成功后是否配置外设；关闭后仅订阅开合话题，但开合前需要另行完成外设配置。 diff --git a/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py b/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py index 6ce1d1e..e752ff1 100644 --- a/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py +++ b/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py @@ -6,7 +6,7 @@ scissorgripper 参数约定： 2：控制器 DO3/DO4 控制的小型剪刀夹爪 当前 XR 工程在真机连接阶段初始化外设，遥操作节点随后复用同一个 -RealMan 连接执行手动 topic 或 grip-gated trigger 夹爪开合命令。 +RealMan 连接执行手动 topic 或 trigger toggle 夹爪开合命令。 """ from __future__ import annotations diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py index 7863a97..3bb5d46 100755 --- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py +++ b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py @@ -129,7 +129,8 @@ class SingleArmVelocityTeleop(Node): self._last_current_pose: ArmPose | None = None self._last_current_pose_time: Time | None = None self._stop_sent = True - self._last_trigger_tool_open: bool | None = None + self._trigger_tool_open = True + self._last_trigger_pressed: bool | None = None self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None self._tool_worker_stop = threading.Event() self._tool_worker_thread: threading.Thread | None = None @@ -201,7 +202,7 @@ class SingleArmVelocityTeleop(Node): topic = f"{self._debug_topic_prefix}/{self._arm_name}/tool_enable" self.create_subscription(Bool, topic, self._on_tool_command, 10) trigger_text = ( - f", trigger 需 grip=true 且 >= {self._trigger_close_threshold:.2f} 时闭合" + f", trigger >= {self._trigger_close_threshold:.2f} 上升沿切换开合" if self._enable_trigger_gripper_control else "" ) @@ -212,7 +213,7 @@ class SingleArmVelocityTeleop(Node): def _on_tool_command(self, msg: Bool) -> None: open_tool = bool(msg.data) - self._last_trigger_tool_open = None + self._trigger_tool_open = open_tool self._enqueue_tool_command(open_tool, "topic") def _start_tool_worker(self) -> None: @@ -292,16 +293,19 @@ class SingleArmVelocityTeleop(Node): def _handle_trigger_gripper(self, msg: XrController) -> None: if not self._enable_tool_control or not self._enable_trigger_gripper_control: return - if not msg.grip: + + trigger_pressed = msg.trigger >= self._trigger_close_threshold + if self._last_trigger_pressed is None: + self._last_trigger_pressed = trigger_pressed return - should_close = msg.trigger >= self._trigger_close_threshold - open_tool = not should_close - if self._last_trigger_tool_open == open_tool: + rising_edge = trigger_pressed and not self._last_trigger_pressed + self._last_trigger_pressed = trigger_pressed + if not rising_edge: return - self._last_trigger_tool_open = open_tool - self._enqueue_tool_command(open_tool, "trigger") + self._trigger_tool_open = not self._trigger_tool_open + self._enqueue_tool_command(self._trigger_tool_open, "trigger") def _control_tick(self) -> None: now = self.get_clock().now()