diff --git a/PROMPT.md b/PROMPT.md index a680cad..32e246f 100644 --- a/PROMPT.md +++ b/PROMPT.md @@ -22,14 +22,14 @@ PICO 4 Ultra 双手柄位姿/按键 - PICO/OpenXR 手柄 UDP 输入解析。 - 左右手柄话题分发。 - 左右 RM75 单臂速度遥操作节点。 -- 双臂 mock、双臂真机、单臂调试 launch。 +- 通过统一的 `arm_debug.launch.py` 支持双臂 mock、双臂真机和单臂调试。 - acRealman 来源的 RM75 IP、工作空间、圆柱半径约束、初始化点位和部分安全配置。 - Tkinter 启动面板，用于打开 mock、真机、诊断和话题监控命令。 -- 可选的 trigger 到夹爪力控比例桥接节点。 当前阶段暂不实现： - XR 侧 Unity 应用。 +- 夹爪 trigger 桥接和末端执行器 launch 集成。 - 论文中的低延迟立体视频流。 - QP IK、灵巧手 retarget、全身追踪、运动捕捉 tracker。 - VLA 数据集记录和训练流程。 @@ -73,7 +73,6 @@ xr_rm_teleop/ xr_rm_teleop/__init__.py xr_rm_teleop/realman_adapter.py xr_rm_teleop/single_arm_velocity_teleop.py - xr_rm_teleop/gripper_trigger_bridge.py xr_rm_bringup/ CMakeLists.txt @@ -81,8 +80,6 @@ xr_rm_bringup/ config/dual_arm_rm75.yaml config/left_arm_rm75.yaml config/right_arm_rm75.yaml - launch/dual_arm_mock.launch.py - launch/dual_arm_realman.launch.py launch/arm_debug.launch.py tools/launcher_ui.py ``` @@ -251,15 +248,6 @@ geometry_msgs/Pose pose 注意：当前阶段只控制 TCP 平移，不控制姿态。 -### gripper_trigger_bridge.py - -实现可选节点 `GripperTriggerBridge`： - -- 订阅某个 `/xr/*_controller` -- 发布 `std_msgs/Float32` 到夹爪力控比例话题，如 `/omnipicker/right/force_ratio` -- 参数包括 `require_grip`、`trigger_deadband`、`open_command`、`min_close_command`、`max_close_command`、`low_pass_alpha` -- stale 或未按 grip 时发布开爪命令。 - ## bringup 配置需求 ### RM75 默认参数 @@ -329,19 +317,6 @@ initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910] ### launch 文件 -`dual_arm_mock.launch.py`： - -- 启动 `udp_controller_receiver` -- 启动 `left_arm_teleop` 和 `right_arm_teleop` -- 共用 `dual_arm_rm75.yaml` -- 覆盖 `use_mock=true` -- launch 参数：`udp_host`、`udp_port`、`robot_config`、`run_udp`、`run_left_arm`、`run_right_arm` - -`dual_arm_realman.launch.py`： - -- 和 mock 类似，但 `use_mock=false` -- launch 参数包含 `left_robot_ip`、`right_robot_ip`、`robot_port`、`move_to_initial_pose_on_connect` - `arm_debug.launch.py`： - 参数 `arm:=left|right|both` @@ -349,6 +324,7 @@ initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910] - 单臂时读取 `left_arm_rm75.yaml` 或 `right_arm_rm75.yaml` - 双臂时读取 `dual_arm_rm75.yaml` - 总是启动 `udp_controller_receiver` +- 这是唯一的遥操作 launch 主入口，UI 和命令行调试都优先使用它。 ### 启动面板 @@ -377,7 +353,7 @@ source install/setup.bash mock 验证： ```bash -ros2 launch xr_rm_bringup dual_arm_mock.launch.py +ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 ``` @@ -401,7 +377,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true 真机启动： ```bash -ros2 launch xr_rm_bringup dual_arm_realman.launch.py \ +ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ left_robot_ip:=192.168.192.18 \ right_robot_ip:=192.168.192.19 ``` @@ -428,7 +404,7 @@ ros2 launch xr_rm_bringup dual_arm_realman.launch.py \ 验收标准： - `colcon build --symlink-install` 能通过。 -- `ros2 launch xr_rm_bringup dual_arm_mock.launch.py` 能启动三个节点。 +- `ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true` 能启动三个节点。 - `sample_udp_sender --hand both` 后能看到左右 `/xr/*_controller` 和 `/xr_rm/*/cmd_vel`。 - 松开 grip、UDP 超时、节点退出时都会发送零速度。 - 修改 YAML 中 `xr_to_robot_matrix` 能改变对应臂方向映射。 diff --git a/README.md b/README.md index e37e63a..150fda7 100755 --- a/README.md +++ b/README.md @@ -18,7 +18,7 @@ PICO/XR 双手柄 UDP 数据 已完成： - PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。 -- 左臂、右臂、双臂的 mock 调试和真机调试 launch。 +- 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。 - RM75 真机连接适配，包含速度透传初始化、安全速度/加速度配置、可选初始化点位移动。 - Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。 @@ -40,9 +40,7 @@ src/ │ │ ├── left_arm_rm75.yaml # 左臂单独调试配置 │ │ └── right_arm_rm75.yaml # 右臂单独调试配置 │ ├── launch/ -│ │ ├── arm_debug.launch.py # 推荐主入口：arm:=left/right/both, use_mock:=true/false -│ │ ├── dual_arm_mock.launch.py # 双臂 mock 快速入口 -│ │ └── dual_arm_realman.launch.py +│ │ └── arm_debug.launch.py # 统一主入口：arm:=left/right/both, use_mock:=true/false │ └── tools/ │ └── launcher_ui.py # 图形化调试启动面板 ├── xr_rm_input/ @@ -57,8 +55,7 @@ src/ └── xr_rm_teleop/ └── xr_rm_teleop/ ├── single_arm_velocity_teleop.py - ├── realman_adapter.py - └── gripper_trigger_bridge.py + └── realman_adapter.py ``` `single_arm_velocity_teleop` 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 `left_arm_teleop` 和 `right_arm_teleop`。 @@ -177,7 +174,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ ## Launch 入口说明 -`arm_debug.launch.py` 是当前推荐主入口。 +`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 mock、单臂真机和双臂真机按钮都调用它。 常用参数： @@ -190,8 +187,6 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ - `robot_port`：RM75 TCP 端口，默认 `8080`。 - `move_to_initial_pose_on_connect`：连接后是否执行 `movej`/`movel` 初始化，默认 `false`。 -`dual_arm_mock.launch.py` 和 `dual_arm_realman.launch.py` 是双臂快速入口，适合脚本化启动。它们也支持 `run_udp`、`run_left_arm`、`run_right_arm` 等开关，用于只启动部分节点。 - ## 配置文件说明 `xr_rm_bringup/config/dual_arm_rm75.yaml` 是双臂配置主文件，包含两个 ROS 节点命名空间： @@ -260,7 +255,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ - `hand`：`left` 或 `right`。 - `grip`：运动使能。`true` 时进入相对位移控制，`false` 时停止。 -- `trigger`：扳机值，范围 `0.0-1.0`。当前控制主链路不使用它，`gripper_trigger_bridge.py` 可将其转换为夹爪力控比例。 +- `trigger`：扳机值，范围 `0.0-1.0`。当前控制主链路不使用它，预留给后续夹爪集成。 - `pos`：手柄位置，长度 3。 - `quat`：手柄姿态四元数，默认按 `xyzw` 解析。 @@ -298,7 +293,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ ### 3. 集成末端执行器 -- 将 `gripper_trigger_bridge.py` 纳入 bringup launch。 +- 新增夹爪控制节点或桥接节点，并纳入 `arm_debug.launch.py` 与 `launcher_ui.py`。 - 明确夹爪 topic、力控比例、开合方向和安全上限。 - 在 `launcher_ui.py` 中加入夹爪测试按钮和夹爪状态监控。 - 将 `trigger` 从“预留字段”变成稳定的夹爪控制输入。 diff --git a/xr_rm_bringup/launch/dual_arm_mock.launch.py b/xr_rm_bringup/launch/dual_arm_mock.launch.py deleted file mode 100755 index 07cf13d..0000000 --- a/xr_rm_bringup/launch/dual_arm_mock.launch.py +++ /dev/null @@ -1,65 +0,0 @@ -"""双臂 mock 遥操作启动入口。 - -该 launch 文件用于不连接真实 RM75 的离线闭环验证：启动 UDP 手柄接收节点， -并分别启动左、右两个 `single_arm_velocity_teleop` 节点。两个遥操作节点使用 -同一份双臂配置文件，但通过节点名读取各自的参数命名空间。 -""" - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - # 通过 robot_config 参数选择 xr_rm_bringup/config 下的双臂配置文件。 - config_file = PathJoinSubstitution([ - FindPackageShare("xr_rm_bringup"), - "config", - LaunchConfiguration("robot_config"), - ]) - - return LaunchDescription([ - # UDP 监听参数，需要和 PICO 端或 sample_udp_sender 的目标地址保持一致。 - DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), - DeclareLaunchArgument("udp_port", default_value="15000"), - DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"), - # 这些开关便于单独验证 UDP、左臂或右臂链路。 - DeclareLaunchArgument("run_udp", default_value="true"), - DeclareLaunchArgument("run_left_arm", default_value="true"), - DeclareLaunchArgument("run_right_arm", default_value="true"), - # 接收 PICO/XR 侧 UDP 数据，并按左右手字段分发到对应手柄话题。 - Node( - package="xr_rm_input", - executable="udp_controller_receiver", - name="udp_controller_receiver", - output="screen", - condition=IfCondition(LaunchConfiguration("run_udp")), - parameters=[{ - "udp_host": LaunchConfiguration("udp_host"), - "udp_port": LaunchConfiguration("udp_port"), - "left_topic": "/xr/left_controller", - "right_topic": "/xr/right_controller", - }], - ), - # 左臂模拟节点：不连接真实 RM75，只积分速度命令用于验证坐标方向。 - Node( - package="xr_rm_teleop", - executable="single_arm_velocity_teleop", - name="left_arm_teleop", - output="screen", - condition=IfCondition(LaunchConfiguration("run_left_arm")), - parameters=[config_file, {"use_mock": True}], - ), - # 右臂模拟节点：与左臂共用同一配置文件，但读取右手柄话题。 - Node( - package="xr_rm_teleop", - executable="single_arm_velocity_teleop", - name="right_arm_teleop", - output="screen", - condition=IfCondition(LaunchConfiguration("run_right_arm")), - parameters=[config_file, {"use_mock": True}], - ), - ]) diff --git a/xr_rm_bringup/launch/dual_arm_realman.launch.py b/xr_rm_bringup/launch/dual_arm_realman.launch.py deleted file mode 100755 index 3b0de23..0000000 --- a/xr_rm_bringup/launch/dual_arm_realman.launch.py +++ /dev/null @@ -1,94 +0,0 @@ -"""双 RM75 真机遥操作启动入口。 - -该 launch 文件用于连接左右两台真实 RM75：启动 UDP 手柄接收节点，并分别启动 -左、右两个真机遥操作节点。默认不会自动移动到初始点，只有显式设置 -`move_to_initial_pose_on_connect:=true` 后才会执行初始化点位移动。 -""" - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterValue -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - # 双臂真机默认读取 xr_rm_bringup/config/dual_arm_rm75.yaml。 - config_file = PathJoinSubstitution([ - FindPackageShare("xr_rm_bringup"), - "config", - LaunchConfiguration("robot_config"), - ]) - - return LaunchDescription([ - # UDP 监听参数，需要与 PICO 端发送地址和端口匹配。 - DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), - DeclareLaunchArgument("udp_port", default_value="15000"), - # 左右 RM75 的默认现场 IP，可在 launch 命令中按实际网络覆盖。 - DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"), - DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"), - DeclareLaunchArgument("robot_port", default_value="8080"), - DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"), - # 调试时可关闭部分节点，只验证 UDP 或单侧机械臂。 - DeclareLaunchArgument("run_udp", default_value="true"), - DeclareLaunchArgument("run_left_arm", default_value="true"), - DeclareLaunchArgument("run_right_arm", default_value="true"), - # 真机默认不自动动臂，确认安全区清空后再手动打开。 - DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"), - # 接收 PICO/XR 侧 UDP 数据，并按左右手字段分发到对应手柄话题。 - Node( - package="xr_rm_input", - executable="udp_controller_receiver", - name="udp_controller_receiver", - output="screen", - condition=IfCondition(LaunchConfiguration("run_udp")), - parameters=[{ - "udp_host": LaunchConfiguration("udp_host"), - "udp_port": LaunchConfiguration("udp_port"), - "left_topic": "/xr/left_controller", - "right_topic": "/xr/right_controller", - }], - ), - # 左臂真机节点：连接 left_robot_ip，并读取 left_arm_teleop 参数命名空间。 - Node( - package="xr_rm_teleop", - executable="single_arm_velocity_teleop", - name="left_arm_teleop", - output="screen", - condition=IfCondition(LaunchConfiguration("run_left_arm")), - parameters=[ - config_file, - { - "use_mock": False, - "robot_ip": LaunchConfiguration("left_robot_ip"), - "robot_port": LaunchConfiguration("robot_port"), - "move_to_initial_pose_on_connect": ParameterValue( - LaunchConfiguration("move_to_initial_pose_on_connect"), - value_type=bool, - ), - }, - ], - ), - # 右臂真机节点：连接 right_robot_ip，并读取 right_arm_teleop 参数命名空间。 - Node( - package="xr_rm_teleop", - executable="single_arm_velocity_teleop", - name="right_arm_teleop", - output="screen", - condition=IfCondition(LaunchConfiguration("run_right_arm")), - parameters=[ - config_file, - { - "use_mock": False, - "robot_ip": LaunchConfiguration("right_robot_ip"), - "robot_port": LaunchConfiguration("robot_port"), - "move_to_initial_pose_on_connect": ParameterValue( - LaunchConfiguration("move_to_initial_pose_on_connect"), - value_type=bool, - ), - }, - ], - ), - ]) diff --git a/xr_rm_teleop/setup.py b/xr_rm_teleop/setup.py index 3ebf1c3..7cc31b1 100755 --- a/xr_rm_teleop/setup.py +++ b/xr_rm_teleop/setup.py @@ -1,7 +1,6 @@ """xr_rm_teleop 包安装配置。 -该包提供基于 XR 相对位移的 RM75 笛卡尔速度遥操作节点，以及把 -手柄 trigger 映射到夹爪力控命令的桥接节点。 +该包提供基于 XR 相对位移的 RM75 笛卡尔速度遥操作节点。 """ from setuptools import setup @@ -25,7 +24,6 @@ setup( tests_require=["pytest"], entry_points={ "console_scripts": [ - "gripper_trigger_bridge = xr_rm_teleop.gripper_trigger_bridge:main", "single_arm_velocity_teleop = xr_rm_teleop.single_arm_velocity_teleop:main", ], }, diff --git a/xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py b/xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py deleted file mode 100755 index 414a1a6..0000000 --- a/xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py +++ /dev/null @@ -1,119 +0,0 @@ -"""手柄 trigger 到夹爪力控指令的桥接节点。 - -订阅 XR 手柄消息，将 trigger 压力映射为 OmniPicker 可使用的归一化 -闭合力比例，并在消息超时或未握持时自动回到开爪指令。 -""" - -from __future__ import annotations - -import rclpy -from rclpy.node import Node -from std_msgs.msg import Float32 - -from xr_rm_interfaces.msg import XrController - - -def _clamp(value: float, low: float, high: float) -> float: - return min(max(value, low), high) - - -class GripperTriggerBridge(Node): - """将 PICO 扳机键映射为 OmniPicker 可订阅的归一化力控指令。""" - - def __init__(self) -> None: - super().__init__("gripper_trigger_bridge") - - self.declare_parameter("controller_topic", "/xr/right_controller") - self.declare_parameter("command_topic", "/omnipicker/right/force_ratio") - self.declare_parameter("publish_rate_hz", 30.0) - self.declare_parameter("stale_timeout_sec", 0.25) - self.declare_parameter("require_grip", True) - # 扳机键输入小于死区时保持开爪，避免手柄轻微误触导致夹爪闭合。 - self.declare_parameter("trigger_deadband", 0.05) - self.declare_parameter("open_command", 0.0) - # 阶段一以保护番茄为先，闭合力控上限应从低值开始实测。 - self.declare_parameter("min_close_command", 0.15) - self.declare_parameter("max_close_command", 0.45) - self.declare_parameter("low_pass_alpha", 0.5) - - controller_topic = str(self.get_parameter("controller_topic").value) - command_topic = str(self.get_parameter("command_topic").value) - publish_rate_hz = float(self.get_parameter("publish_rate_hz").value) - - self._stale_timeout_sec = float(self.get_parameter("stale_timeout_sec").value) - self._require_grip = bool(self.get_parameter("require_grip").value) - self._trigger_deadband = float(self.get_parameter("trigger_deadband").value) - self._open_command = float(self.get_parameter("open_command").value) - self._min_close_command = float(self.get_parameter("min_close_command").value) - self._max_close_command = float(self.get_parameter("max_close_command").value) - self._low_pass_alpha = float(self.get_parameter("low_pass_alpha").value) - self._validate_parameters() - - self._last_msg: XrController | None = None - self._last_msg_time = None - self._filtered_command = self._open_command - - self._publisher = self.create_publisher(Float32, command_topic, 10) - self.create_subscription(XrController, controller_topic, self._on_controller, 10) - self.create_timer(1.0 / publish_rate_hz, self._publish_command) - - self.get_logger().info( - f"夹爪 trigger 桥接已启动：{controller_topic} -> {command_topic}" - ) - - def _on_controller(self, msg: XrController) -> None: - self._last_msg = msg - self._last_msg_time = self.get_clock().now() - - def _publish_command(self) -> None: - command = self._open_command - - # 手柄消息超时后立即退回开爪，避免夹爪在通信中断时保持闭合力。 - if self._last_msg is not None and self._last_msg_time is not None: - age = (self.get_clock().now() - self._last_msg_time).nanoseconds * 1e-9 - if age <= self._stale_timeout_sec: - command = self._command_from_trigger(self._last_msg) - - # 对夹爪指令做一阶平滑，避免扳机键抖动直接传到力控命令。 - alpha = self._low_pass_alpha - self._filtered_command = alpha * command + (1.0 - alpha) * self._filtered_command - msg = Float32() - msg.data = float(self._filtered_command) - self._publisher.publish(msg) - - def _command_from_trigger(self, msg: XrController) -> float: - if self._require_grip and not msg.grip: - return self._open_command - - trigger = _clamp(float(msg.trigger), 0.0, 1.0) - if trigger <= self._trigger_deadband: - return self._open_command - - ratio = (trigger - self._trigger_deadband) / (1.0 - self._trigger_deadband) - return self._min_close_command + ratio * (self._max_close_command - self._min_close_command) - - def _validate_parameters(self) -> None: - if not 0.0 <= self._trigger_deadband < 1.0: - raise ValueError("trigger_deadband must be in [0, 1)") - if self._min_close_command < self._open_command: - raise ValueError("min_close_command must be >= open_command") - if self._max_close_command < self._min_close_command: - raise ValueError("max_close_command must be >= min_close_command") - if not 0.0 <= self._low_pass_alpha <= 1.0: - raise ValueError("low_pass_alpha must be in [0, 1]") - - -def main(args: list[str] | None = None) -> None: - rclpy.init(args=args) - node = GripperTriggerBridge() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main()