# XR-RM75 双臂遥操作

基于 **Ubuntu 22.04、ROS2 Humble、PICO 4 Ultra 和睿尔曼 RM75** 的双臂 XR 遥操作
工作空间，支持单臂/双臂 Mock 与真机控制，以及 MuJoCo 运动学显示。

> [!WARNING]
> 真机命令会连接并控制机械臂。首次运行必须从 Mock 和单臂低速验证开始，确保急停
> 可用且工作区无人。当前项目没有双臂碰撞检测或避障。

## 当前能力

- PICO/XR 双手柄 UDP 输入，相对位姿目标与 Placo QP 七关节控制。
- 单臂/双臂 Mock 与真机、手柄/话题夹爪控制，以及只读 MuJoCo 双臂显示。
- 工作空间/圆柱限位、速度限制、指令超时和安全慢停。
- 统一 launch、Tkinter 启动面板、调试话题和 Mock 输入工具。

尚未完成：D405/D435 视频流、数据记录、相机标定、目标检测、双臂碰撞避障、任务级状态机，以及 PICO 与 ROS 的完整时间同步和状态回传。

## 系统架构

```text
PICO / XRoboToolkit
  -> UDP JSON
  -> xr_rm_input/udp_controller_receiver
  -> /xr/left_controller、/xr/right_controller
  -> xr_rm_teleop/single_arm_velocity_teleop
  -> 相对 TCP 目标 + Placo QP
  -> Mock 或 RM75 rm_movej_canfd
  -> joint_states / 调试话题
       -> 可选 xr_rm_mujoco/dual_arm_simulator
```

工作空间包含五个 ROS2 包：`xr_rm_input` 负责手柄输入，`xr_rm_interfaces` 定义消息，
`xr_rm_teleop` 实现遥操作与真机适配，`xr_rm_bringup` 提供启动和配置，
`xr_rm_mujoco` 负责只读运动学显示。

`single_arm_velocity_teleop` 每个实例只控制一台机械臂；双臂模式分别启动 `left_arm_teleop` 和 `right_arm_teleop`。

## 环境与构建

在工作空间根目录执行：

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
rosdep update
rosdep install --from-paths src -y --ignore-src
colcon build --symlink-install
source install/setup.bash
```

遥操作和 MuJoCo 节点固定使用 `/home/robot/miniconda3/envs/xr/bin/python`，
其中固定 Placo 0.9.4、Pinocchio 3.7.0、NumPy 2.2.6 和 MuJoCo 3.10.0；不要从
用户或系统 Python 覆盖这些版本。

真机模式另需睿尔曼 Python API2；Mock 模式不依赖厂商 SDK。

## 快速开始

以下命令均在 `/home/robot/WS_xr` 执行，并先 source ROS2 与 `install/setup.bash`。

### Mock

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
```

另开终端发送模拟手柄数据：

```bash
ros2 run xr_rm_input sample_udp_sender \
  --hand both --host 127.0.0.1 --port 15000 \
  --pattern axis_sweep --seconds 30
```

单臂调试时将 `arm` 改为 `left` 或 `right`。推荐先分别完成左右单臂
Mock，再进入双臂或真机验证。

### MuJoCo

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py \
  arm:=both use_mock:=true use_mujoco:=true
```

MuJoCo 只订阅关节状态，不参与控制。真机显示可将 `use_mock` 改为 `false`，
但该命令会同时连接两台 RM75。

### PICO 输入

只保留一个 UDP 输入源，然后启动 XRoboToolkit bridge：

```bash
ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
  --host 127.0.0.1 --port 15000 --hz 90
```

确认左右 topic 持续接收数据：

```bash
ros2 topic hz /xr/left_controller
ros2 topic hz /xr/right_controller
```

图形启动面板可运行 `python3 src/xr_rm_bringup/tools/launcher_ui.py`，提供
Simulation、MuJoCo、Real Hardware 和 Diagnostics 模式。

### 真机

确认对应 YAML 中 `move_to_initial_pose_on_connect: false`，再从单臂开始：

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
```

单臂方向、限位、急停、超时停止和夹爪均验证后，才能启动双臂：

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
```

按住 `grip` 控制对应机械臂；松开后停止。点击 `trigger` 切换对应夹爪开/关。
左手 X、右手 A 会请求对应机械臂回到配置的初始位姿；真机使用前必须清空安全区。

## Launch 参数

统一入口为 `xr_rm_bringup/launch/arm_debug.launch.py`：

| 参数 | 默认值 | 说明 |
| --- | --- | --- |
| `arm` | `right` | `left`、`right` 或 `both` |
| `use_mock` | `true` | `false` 会连接真机 |
| `use_mujoco` | `false` | 仅支持 `arm:=both` |
| `udp_host` | `0.0.0.0` | UDP 监听地址 |
| `udp_port` | `15000` | UDP 监听端口 |
| `udp_timer_hz` | `200.0` | UDP receiver 轮询频率 |

## 配置

| 文件 | 用途 |
| --- | --- |
| `dual_arm_rm75.yaml` | 双臂节点、网络、控制与安全参数 |
| `left_arm_rm75.yaml` | 左臂单独调试 |
| `right_arm_rm75.yaml` | 右臂单独调试 |
| `peripherals_rm75.yaml` | 工具坐标、负载和末端执行器 |
| `dual_arm_mujoco.yaml` | MuJoCo 刷新频率 |

修改某一侧控制参数时，同时检查单臂和双臂 YAML 是否需要同步。必须保留工作空间/
圆柱限位、线速度与角速度限制、关节速度/加速度限制、指令超时和安全停止逻辑。

`configure_safety_limits` 不得默认关闭；
`move_to_initial_pose_on_connect` 必须保持默认 `false`。

## 测试

在工作空间根目录执行：

```bash
source /opt/ros/humble/setup.bash
colcon build --symlink-install
colcon test --event-handlers console_direct+
colcon test-result --verbose
```

涉及遥操作姿态控制时，额外运行：

```bash
pytest src/xr_rm_teleop/test/test_orientation_control.py
```

真机验证不属于自动测试。默认使用 `use_mock:=true`，未经现场安全确认不要连接或
移动机械臂。