Remove outdated design documents for RM75 control and feedback systems

feat: Implement UDP feedback for RM75 robot arms
feat: Implement absolute scheduling for feedback loop and enhance tool frame configuration
2026-07-29 16:24:07 +08:00 · 2026-07-29 15:26:59 +08:00 · 2026-07-29 09:44:51 +08:00 · 2026-07-28 17:06:06 +08:00 · 2026-07-28 10:47:49 +08:00 · 2026-07-27 19:30:30 +08:00
46 changed files with 3316 additions and 687 deletions
@@ -227,7 +227,9 @@

 ## Git 与提交

-除非用户明确要求，否则不要自动提交、推送、创建分支或修改远程仓库。
+除非用户明确要求，否则不要自动提交、推送或修改远程仓库。
+
+使用 Superpowers 执行计划时，允许 subagent 按相关 skill 创建和使用独立 worktree 及其配套本地分支；其他情况下，除非用户明确要求，不要自动创建分支。

 如果用户要求生成提交信息，提交信息应：

@@ -7,11 +7,12 @@ PICO/XR 双手柄 UDP JSON
  -> xr_rm_input/udp_controller_receiver
  -> /xr/left_controller 与 /xr/right_controller
  -> xr_rm_teleop/single_arm_velocity_teleop
-  -> 左右 RM75 笛卡尔相对位姿透传控制
+  -> Placo QP 单步逆解
+  -> 左右 RM75 七关节角透传控制
  -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
 ```

-当前控制方式是“手柄相对位姿透传”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP，经过工作空间限幅、目标低通、姿态低通和单帧步长限制后，通过 `rm_movep_canfd` 下发目标位姿。松开 `grip`、UDP 超时或节点退出时会请求机械臂慢停。
+当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP。姿态目标使用旋转矩阵和 SO(3) 最短路径完成死区、滤波与限速，不经过 RPY。每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip`、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。

 ## 当前范围

@@ -19,7 +20,8 @@ PICO/XR 双手柄 UDP JSON

 - PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
 - 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
- RM75 真机连接适配，包含 `rm_movep_canfd` 位姿透传、安全速度/加速度配置、可选初始化点位移动。
+- RM75 真机连接适配，包含关节反馈缓存、`rm_movej_canfd` 关节透传、安全速度/加速度配置、可选初始化点位移动。
+- Placo 0.9.4 RM75 QP 逆解；收到首帧有效关节反馈后才启用，求解失败时保留上一组有效关节目标。
 - 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
 - Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
 - 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip`、`trigger` 和 pose 诊断字段。
@@ -67,7 +69,11 @@ src/
 │   └── msg/
 │       └── XrController.msg        # hand/grip/trigger/pose
 └── xr_rm_teleop/
+    ├── models/
+    │   ├── rm75/                   # 旧 RM75 模型资源（launch 不再选用）
+    │   └── rm75_omnipicker/        # RM75 + OmniPicker fixed URDF 与网格
    └── xr_rm_teleop/
+        ├── placo_ik_solver.py      # Placo 0.9.4 单步 QP 逆解
        ├── single_arm_velocity_teleop.py
        ├── realman_adapter.py
        └── fun_peripheral.py
@@ -90,6 +96,17 @@ source install/setup.bash

 真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。

+遥操作节点固定由 `/home/robot/miniconda3/envs/xr/bin/python` 启动，并复用其中的 Python 3.10、Placo 0.9.4、Pinocchio 3.7.0 和 NumPy 2.2.6。`ros2`、`colcon` 和 `udp_controller_receiver` 仍使用系统 Python。禁止通过 `pip --user`、`sudo pip` 或系统安装升级 Placo、Pinocchio、EigenPy 和 NumPy。
+
+只读检查 Placo 版本：
+
+```bash
+/home/robot/miniconda3/envs/xr/bin/python -c \
+  "import importlib.metadata; print(importlib.metadata.version('placo'))"
+```
+
+输出必须为 `0.9.4`。
+
 如果希望 `launcher_ui.py` 从任意目录找到工作空间，可以设置：

 ```bash
@@ -150,17 +167,25 @@ sudo update-alternatives --config x-terminal-emulator

 打开 `launcher_ui.py`，点击 `Check Env`。如果 `install/setup.bash` 缺失，先回工作空间根目录重新执行 `colcon build --symlink-install`。

-第二步：跑 mock 闭环。
+第二步：分别跑左、右臂 mock 闭环。

-在 `Simulation` 模式运行 `One-Click Dual Mock Demo`，或分开运行：
+分两个终端依次验证左臂：

 ```bash
-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 \
-  --pattern axis_sweep --seconds 60 --both-mode staggered
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
+ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port 15000 \
+  --pattern axis_sweep --seconds 30
 ```

-`sample_udp_sender` 默认使用 `axis_sweep` 成对扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。`--hand both --both-mode staggered --seconds 60` 会先左后右，适合肉眼确认左右臂方向；如果只想左右同时动，可用 `--both-mode synchronized`。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。
+停止左臂进程后，再分别验证右臂：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
+ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port 15000 \
+  --pattern axis_sweep --seconds 30
+```
+
+`sample_udp_sender` 默认使用 `axis_sweep` 扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。

 观察：

@@ -182,23 +207,18 @@ 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
 ```

-单臂真机默认执行配置文件中的 `movej(initial_joint_pose)`；现场需要跳过时，显式传入
-`move_to_initial_pose_on_connect:=false`。
+所有 YAML 默认都不会执行 `movej(initial_joint_pose)`。只有确认安全区清空后，才可在当前使用的
+`left_arm_rm75.yaml`、`right_arm_rm75.yaml` 或 `dual_arm_rm75.yaml` 中将
+`move_to_initial_pose_on_connect` 改为 `true`。

 第四步：双臂真机。

 ```bash
-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
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
 ```

-双臂默认不会自动移动到初始化点。以后需要启用时，在确认安全区清空后显式打开：
-
-```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
-  move_to_initial_pose_on_connect:=true
-```
+双臂默认不会自动移动到初始化点；机器人地址、控制参数和初始化移动开关均从
+`dual_arm_rm75.yaml` 读取。

 ## Launch 入口说明

@@ -211,20 +231,9 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `udp_host`：UDP 监听地址，默认 `0.0.0.0`。
 - `udp_port`：UDP 监听端口，默认 `15000`。
 - `udp_timer_hz`：UDP receiver 轮询频率，默认 `200.0`。
- `left_robot_ip`：左臂 IP，默认 `192.168.192.18`。
- `right_robot_ip`：右臂 IP，默认 `192.168.192.19`。
- `robot_port`：RM75 TCP 端口，默认 `8080`。
- `left_avoid_singularity` / `right_avoid_singularity`：左右臂避奇异参数，默认左 `0`、右 `1`。
- `avoid_singularity`：非空时覆盖左右臂避奇异参数。
- `frame_type`：`rm_movep_canfd` 坐标系类型，默认 `1`。
- `control_rate_hz`：`rm_movep_canfd` 目标位姿发送频率，默认 `90.0`。
- `follow`：传给 `rm_movep_canfd` 的跟随标志，默认 `false`。
- `configure_safety_limits`：连接真机后是否配置速度/加速度安全参数，默认 `true`。
- `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `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(initial_joint_pose)`；默认 `auto`，单臂配置启用、双臂配置禁用，也可显式传 `true`/`false` 覆盖。
+
+机器人 IP/端口、控制频率、CANFD、限速、工具和初始化位姿等行为参数只由对应 YAML
+配置，launch 不再提供同名覆盖项。

 ## 配置文件说明

@@ -235,7 +244,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \

 `left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。

-`xr_rm_bringup/config/peripherals_rm75.yaml` 保存末端工具坐标、负载和左右臂外设选择。当前配置为左臂 `scissorgripper=2`、右臂 `scissorgripper=1`，真机连接阶段会初始化外设，后续开合命令复用同一个 RealMan 连接。
+`xr_rm_bringup/config/peripherals_rm75.yaml` 保存真实控制器使用的末端工具坐标、负载和左右臂外设选择，文件内容保持原状。Placo 使用 `xr_rm_teleop/models/rm75_omnipicker` 中的一体化 fixed URDF，直接控制相对 `omnipicker_base_link` 沿 `+Z` 偏移 `0.16 m` 的 `omnipicker_tcp`，不再把外设 YAML 的工具位姿重复转换到 QP。真机连接阶段仍会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 RealMan 连接。

 重点控制参数：

@@ -245,14 +254,19 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `target_filter_fast_threshold_m`：进入快速滤波区间的目标变化阈值。
 - `max_linear_speed`：目标位姿单帧步长限制对应的最大线速度。
 - `enable_orientation_control`：是否把手柄相对旋转映射到 TCP 姿态。
- `orientation_filter_alpha` / `orientation_deadband_rad`：目标 TCP 姿态低通和死区。
- `max_orientation_speed`：目标姿态单帧步长限制对应的最大角速度。
+- `orientation_filter_alpha` / `orientation_deadband_rad`：按 SO(3) 最短旋转角处理的目标 TCP 姿态滤波和死区。
+- `max_orientation_speed`：目标 TCP 姿态沿 SO(3) 最短路径的最大角速度，当前为 `0.5 rad/s`。
 - `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
 - `cyl_radius_limit`：基座圆柱半径限制。
 - `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
- `current_pose_poll_hz`：低频读取真机当前 TCP 的频率；控制中不再每帧阻塞读取状态。
- `mock_initial_pose`：mock 模式初始 TCP 位姿。
- `initial_joint_pose`：可选真机初始关节角。
+- `robot_ip` / `robot_port`：RM75 TCP 控制连接地址。
+- `realtime_push_host_ip`：连接机械臂 Wi-Fi 后本机实际 IPv4；可用
+  `ip -4 route get 192.168.192.19` 查看输出中的 `src`，当前为 `192.168.192.148`。
+- `realtime_push_port`：UDP 主动反馈端口；左臂 `8089`、右臂 `8090`，同机双臂不能重复。
+- `realtime_push_cycle_ms`：UDP 主动反馈周期，当前为厂商支持的 `5 ms`。
+- `follow` / `canfd_trajectory_mode`：`rm_movej_canfd` 的高跟随和轨迹模式参数。
+- 当前三份 YAML 默认均使用 `follow: false` 完成安全基线验证；确认关节加速度与反馈稳定后，再单独测试高跟随。
+- `initial_joint_pose`：mock 的初始关节反馈，以及显式开启初始化移动时的真机初始关节角。

 当前 `/xr/*_controller` 的 Project 坐标约定：

@@ -410,7 +424,7 @@ ros2 topic echo /xr/right_controller --field trigger

 1. 确认急停、网络、机械臂工作区和人员位置。
 2. `launcher_ui.py` 中先 `Ping Left RM75` 或 `Ping Right RM75`。
-3. 单臂启动，`move_to_initial_pose_on_connect:=false`。
+3. 确认对应 YAML 中 `move_to_initial_pose_on_connect: false` 后单臂启动。
 4. 手握急停，按住 `grip` 后只做小幅单轴移动。
 5. 逐个确认上/下、前/后、左/右方向。
 6. 小角度转动手柄，确认 `/xr_rm/<arm>/target_pose` 姿态和 `/xr_rm/<arm>/cmd_vel.twist.angular` 变化符合预期。
@@ -1,20 +0,0 @@
-schema: spec-driven
-
-# Project context (optional)
-# This is shown to AI when creating artifacts.
-# Add your tech stack, conventions, style guides, domain knowledge, etc.
-# Example:
-#   context: |
-#     Tech stack: TypeScript, React, Node.js
-#     We use conventional commits
-#     Domain: e-commerce platform
-
-# Per-artifact rules (optional)
-# Add custom rules for specific artifacts.
-# Example:
-#   rules:
-#     proposal:
-#       - Keep proposals under 500 words
-#       - Always include a "Non-goals" section
-#     tasks:
-#       - Break tasks into chunks of max 2 hours
@@ -1,17 +1,17 @@
 # 阶段一：PICO 遥操作双 RM75 平台配置。
 #
-# 当前控制方式是“相对位姿透传”：
+# 当前控制方式是“相对 TCP + 单步 QP”：
 # 按下 grip 时锁定当前手柄位姿和 TCP 位姿，之后将手柄相对位移和相对旋转
 # 映射为目标 TCP 位姿，经过工作空间限幅、目标低通、姿态低通和单帧步长
-# 限制后，通过 rm_movep_canfd 下发。cmd_vel 仅作为目标位姿变化率调试话题，
-# 不是机械臂执行命令。
+# 限制后，通过 Placo 单步 QP 和 rm_movej_canfd 下发 7 个关节目标。
+# cmd_vel 仅作为目标位姿变化率调试话题，不是机械臂执行命令。
 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。

 left_arm_teleop:
  ros__parameters:
    arm_name: left_rm75
    controller_topic: /xr/left_controller
-    control_rate_hz: 90.0
+    control_rate_hz: 125.0
    command_timeout_sec: 0.12

    # 位姿目标生成与平滑参数。
@@ -26,9 +26,7 @@ left_arm_teleop:
    enable_orientation_axes: [true, true, true]
    orientation_deadband_rad: 0.005
    orientation_filter_alpha: 0.65
-    max_orientation_speed: 0.6
-    current_pose_poll_hz: 10.0
-
+    max_orientation_speed: 0.5
    workspace_min: [-0.70, -0.60, 0.10]
    workspace_max: [0.70, 0.40, 0.70]
    cyl_radius_limit: [0.20, 0.60]
@@ -41,16 +39,20 @@ left_arm_teleop:
                         0.0, 0.0, 1.0,
                         -1.0, 0.0, 0.0]

-    use_mock: false
-    mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
    robot_ip: 192.168.192.18
    robot_port: 8080
+    realtime_push_host_ip: 192.168.192.148
+    realtime_push_port: 8089
+    realtime_push_cycle_ms: 5
    avoid_singularity: 0
-    frame_type: 1
    follow: false
    canfd_trajectory_mode: 2
    canfd_radio: 0
    configure_safety_limits: true
+    enable_tool_control: true
+    enable_trigger_gripper_control: true
+    trigger_close_threshold: 0.95
+    configure_peripheral_on_connect: true
    max_line_speed: 1.0
    max_angular_speed: 1.5
    max_line_acc: 1.0
@@ -66,7 +68,7 @@ right_arm_teleop:
  ros__parameters:
    arm_name: right_rm75
    controller_topic: /xr/right_controller
-    control_rate_hz: 90.0
+    control_rate_hz: 125.0
    command_timeout_sec: 0.12

    scale: 0.75
@@ -80,9 +82,7 @@ right_arm_teleop:
    enable_orientation_axes: [true, true, true]
    orientation_deadband_rad: 0.005
    orientation_filter_alpha: 0.65
-    max_orientation_speed: 0.6
-    current_pose_poll_hz: 10.0
-
+    max_orientation_speed: 0.5
    workspace_min: [-0.70, -0.60, 0.10]
    workspace_max: [0.70, 0.40, 0.70]
    cyl_radius_limit: [0.20, 0.60]
@@ -95,16 +95,20 @@ right_arm_teleop:
                         0.0, 0.0, 1.0,
                         1.0, 0.0, 0.0]

-    use_mock: false
-    mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
    robot_ip: 192.168.192.19
    robot_port: 8080
+    realtime_push_host_ip: 192.168.192.148
+    realtime_push_port: 8090
+    realtime_push_cycle_ms: 5
    avoid_singularity: 1
-    frame_type: 1
    follow: false
    canfd_trajectory_mode: 2
    canfd_radio: 0
    configure_safety_limits: true
+    enable_tool_control: true
+    enable_trigger_gripper_control: true
+    trigger_close_threshold: 0.95
+    configure_peripheral_on_connect: true
    max_line_speed: 1.0
    max_angular_speed: 1.5
    max_line_acc: 1.0
@@ -1,11 +1,11 @@
-# 左臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。
+# 左臂单独调试配置：XR TCP 目标经 Placo QP 转换为 RM75 关节目标。
 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。

 single_arm_velocity_teleop:
  ros__parameters:
    arm_name: left_rm75
    controller_topic: /xr/left_controller
-    control_rate_hz: 90.0
+    control_rate_hz: 125.0
    command_timeout_sec: 0.12

    # 手柄相对位姿 -> 目标 TCP 位姿；随后做目标低通、姿态低通和单帧步长限制。
@@ -20,9 +20,7 @@ single_arm_velocity_teleop:
    enable_orientation_axes: [true, true, true]
    orientation_deadband_rad: 0.005
    orientation_filter_alpha: 0.65
-    max_orientation_speed: 0.6
-    current_pose_poll_hz: 10.0
-
+    max_orientation_speed: 0.5
    workspace_min: [-0.70, -0.60, 0.10]
    workspace_max: [0.70, 0.40, 0.70]
    cyl_radius_limit: [0.20, 0.60]
@@ -34,23 +32,27 @@ single_arm_velocity_teleop:
                         0.0, 0.0, 1.0,
                         -1.0, 0.0, 0.0]

-    use_mock: false
-    mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
    robot_ip: 192.168.192.18
    robot_port: 8080
+    realtime_push_host_ip: 192.168.192.148
+    realtime_push_port: 8089
+    realtime_push_cycle_ms: 5
    avoid_singularity: 0
-    frame_type: 1
    follow: false
    canfd_trajectory_mode: 2
    canfd_radio: 0
    configure_safety_limits: true
+    enable_tool_control: true
+    enable_trigger_gripper_control: true
+    trigger_close_threshold: 0.95
+    configure_peripheral_on_connect: true
    max_line_speed: 1.0
    max_angular_speed: 1.5
    max_line_acc: 1.0
    max_angular_acc: 2.0
    joint_max_speed: 180.0
    joint_max_acc: 180.0
-    move_to_initial_pose_on_connect: true
+    move_to_initial_pose_on_connect: false
    initial_joint_pose: [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52]
    init_move_speed: 20
    debug_topic_prefix: /xr_rm
@@ -1,11 +1,11 @@
-# 右臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。
+# 右臂单独调试配置：XR TCP 目标经 Placo QP 转换为 RM75 关节目标。
 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。

 single_arm_velocity_teleop:
  ros__parameters:
    arm_name: right_rm75
    controller_topic: /xr/right_controller
-    control_rate_hz: 90.0
+    control_rate_hz: 125.0
    command_timeout_sec: 0.12

    scale: 0.7
@@ -20,11 +20,9 @@ single_arm_velocity_teleop:
    orientation_deadband_rad: 0.005
    orientation_filter_alpha: 0.65
    max_orientation_speed: 0.5
-    current_pose_poll_hz: 10.0
-
-    workspace_min: [-0.60, -0.60, 0.10]
-    workspace_max: [0.60, 0.70, 0.55]
-    cyl_radius_limit: [0.10, 0.70]
+    workspace_min: [-0.70, -0.70, 0.10]
+    workspace_max: [0.70, 0.70, 0.75]
+    cyl_radius_limit: [0.10, 0.80]
    low_z_threshold: 0.1
    low_z_min_radius: 0.1

@@ -33,23 +31,28 @@ single_arm_velocity_teleop:
                         0.0, 0.0, 1.0,
                         1.0, 0.0, 0.0]

-    use_mock: false
-    mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
    robot_ip: 192.168.192.19
    robot_port: 8080
+    realtime_push_host_ip: 192.168.192.148
+    realtime_push_port: 8090
+    realtime_push_cycle_ms: 5
    avoid_singularity: 1
-    frame_type: 1
+    # 厂商 MovejCANFD 示例默认低跟随；高跟随仅在验证规划轨迹后单独开启。
    follow: false
    canfd_trajectory_mode: 2
    canfd_radio: 0
    configure_safety_limits: true
-    max_line_speed: 1.0
-    max_angular_speed: 1.5
-    max_line_acc: 1.0
-    max_angular_acc: 2.0
+    enable_tool_control: true
+    enable_trigger_gripper_control: true
+    trigger_close_threshold: 0.95
+    configure_peripheral_on_connect: true
+    max_line_speed: 0.25
+    max_angular_speed: 0.6
+    max_line_acc: 1.3
+    max_angular_acc: 3.0
    joint_max_speed: 180.0
-    joint_max_acc: 180.0
-    move_to_initial_pose_on_connect: true
+    joint_max_acc: 300.0
+    move_to_initial_pose_on_connect: false
    initial_joint_pose: [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04]
    init_move_speed: 20
    debug_topic_prefix: /xr_rm
@@ -5,6 +5,8 @@
 手柄接收节点，再根据 `arm:=left|right|both` 选择对应的遥操作节点。
 """

+from pathlib import Path
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
@@ -12,6 +14,9 @@ from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare


+XR_PYTHON = "/home/robot/miniconda3/envs/xr/bin/python"
+
+
 def _as_bool(value: str) -> bool:
    """把 launch 字符串参数转换成 Python bool，便于在 OpaqueFunction 中分支。"""
    return value.strip().lower() in ("1", "true", "yes", "on")
@@ -26,8 +31,14 @@ def _config_file(name: str) -> PathJoinSubstitution:
    ])


-def _initial_pose_override(value: str) -> dict[str, bool]:
-    return {} if value == "auto" else {"move_to_initial_pose_on_connect": _as_bool(value)}
+def _rm75_urdf() -> PathJoinSubstitution:
+    return PathJoinSubstitution([
+        FindPackageShare("xr_rm_teleop"),
+        "models",
+        "rm75_omnipicker",
+        "urdf",
+        "RM75-B_OmniPicker_fixed.urdf",
+    ])


 def _udp_receiver_node() -> Node:
@@ -50,42 +61,21 @@ def _udp_receiver_node() -> Node:
 def _single_arm_node(
    arm: str,
    use_mock: bool,
-    move_to_initial_pose: str,
-    avoid_singularity: int,
-    frame_type: int,
-    control_rate_hz: float,
-    follow: bool,
-    configure_safety_limits: bool,
-    enable_tool_control: bool,
-    enable_trigger_gripper_control: bool,
-    trigger_close_threshold: float,
-    configure_peripheral_on_connect: bool,
 ) -> Node:
    """创建单臂调试节点；左/右臂分别使用独立 YAML，节点名保持单臂默认名。"""
    config_name = "left_arm_rm75.yaml" if arm == "left" else "right_arm_rm75.yaml"
-    robot_ip = LaunchConfiguration("left_robot_ip" if arm == "left" else "right_robot_ip")
    arm_name = _arm_name(arm)
    return Node(
        package="xr_rm_teleop",
        executable="single_arm_velocity_teleop",
        name="single_arm_velocity_teleop",
        output="screen",
+        prefix=[XR_PYTHON],
        parameters=[
            _config_file(config_name),
            {
                "use_mock": use_mock,
-                "robot_ip": robot_ip,
-                "robot_port": LaunchConfiguration("robot_port"),
-                "avoid_singularity": avoid_singularity,
-                "frame_type": frame_type,
-                "control_rate_hz": control_rate_hz,
-                "follow": follow,
-                "configure_safety_limits": configure_safety_limits,
-                **_initial_pose_override(move_to_initial_pose),
-                "enable_tool_control": enable_tool_control,
-                "enable_trigger_gripper_control": enable_trigger_gripper_control,
-                "trigger_close_threshold": trigger_close_threshold,
-                "configure_peripheral_on_connect": configure_peripheral_on_connect,
+                "robot_urdf_path": _rm75_urdf(),
                "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                "peripheral_arm": arm,
                "tool_command_topic": f"/xr_rm/{arm_name}/tool_enable",
@@ -98,20 +88,7 @@ def _arm_name(arm: str) -> str:
    return "left_rm75" if arm == "left" else "right_rm75"


-def _dual_arm_nodes(
-    use_mock: bool,
-    move_to_initial_pose: str,
-    left_avoid_singularity: int,
-    right_avoid_singularity: int,
-    frame_type: int,
-    control_rate_hz: float,
-    follow: bool,
-    configure_safety_limits: bool,
-    enable_tool_control: bool,
-    enable_trigger_gripper_control: bool,
-    trigger_close_threshold: float,
-    configure_peripheral_on_connect: bool,
-) -> list[Node]:
+def _dual_arm_nodes(use_mock: bool) -> list[Node]:
    """创建双臂节点；两个节点共用双臂 YAML，但节点名区分左右臂参数命名空间。"""
    config_file = _config_file("dual_arm_rm75.yaml")
    return [
@@ -120,22 +97,12 @@ def _dual_arm_nodes(
            executable="single_arm_velocity_teleop",
            name="left_arm_teleop",
            output="screen",
+            prefix=[XR_PYTHON],
            parameters=[
                config_file,
                {
                    "use_mock": use_mock,
-                    "robot_ip": LaunchConfiguration("left_robot_ip"),
-                    "robot_port": LaunchConfiguration("robot_port"),
-                    "avoid_singularity": left_avoid_singularity,
-                    "frame_type": frame_type,
-                    "control_rate_hz": control_rate_hz,
-                    "follow": follow,
-                    "configure_safety_limits": configure_safety_limits,
-                    **_initial_pose_override(move_to_initial_pose),
-                    "enable_tool_control": enable_tool_control,
-                    "enable_trigger_gripper_control": enable_trigger_gripper_control,
-                    "trigger_close_threshold": trigger_close_threshold,
-                    "configure_peripheral_on_connect": configure_peripheral_on_connect,
+                    "robot_urdf_path": _rm75_urdf(),
                    "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                    "peripheral_arm": "left",
                    "tool_command_topic": "/xr_rm/left_rm75/tool_enable",
@@ -147,22 +114,12 @@ def _dual_arm_nodes(
            executable="single_arm_velocity_teleop",
            name="right_arm_teleop",
            output="screen",
+            prefix=[XR_PYTHON],
            parameters=[
                config_file,
                {
                    "use_mock": use_mock,
-                    "robot_ip": LaunchConfiguration("right_robot_ip"),
-                    "robot_port": LaunchConfiguration("robot_port"),
-                    "avoid_singularity": right_avoid_singularity,
-                    "frame_type": frame_type,
-                    "control_rate_hz": control_rate_hz,
-                    "follow": follow,
-                    "configure_safety_limits": configure_safety_limits,
-                    **_initial_pose_override(move_to_initial_pose),
-                    "enable_tool_control": enable_tool_control,
-                    "enable_trigger_gripper_control": enable_trigger_gripper_control,
-                    "trigger_close_threshold": trigger_close_threshold,
-                    "configure_peripheral_on_connect": configure_peripheral_on_connect,
+                    "robot_urdf_path": _rm75_urdf(),
                    "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                    "peripheral_arm": "right",
                    "tool_command_topic": "/xr_rm/right_rm75/tool_enable",
@@ -175,76 +132,22 @@ def _dual_arm_nodes(
 def _launch_setup(context, *args, **kwargs):
    """运行时读取 launch 参数，决定启动单臂还是双臂。"""
    del args, kwargs
+    if not Path(XR_PYTHON).is_file():
+        raise RuntimeError(
+            f"XR Python not found: {XR_PYTHON}; "
+            "Placo 0.9.4 must not be installed globally"
+        )
    arm = LaunchConfiguration("arm").perform(context).strip().lower()
    use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
-    move_to_initial_pose = LaunchConfiguration(
-        "move_to_initial_pose_on_connect"
-    ).perform(context).strip().lower()
-    avoid_override = LaunchConfiguration("avoid_singularity").perform(context).strip()
-    left_avoid_singularity = int(
-        avoid_override or LaunchConfiguration("left_avoid_singularity").perform(context)
-    )
-    right_avoid_singularity = int(
-        avoid_override or LaunchConfiguration("right_avoid_singularity").perform(context)
-    )
-    frame_type = int(LaunchConfiguration("frame_type").perform(context))
-    control_rate_hz = float(LaunchConfiguration("control_rate_hz").perform(context))
-    follow = _as_bool(LaunchConfiguration("follow").perform(context))
-    configure_safety_limits = _as_bool(
-        LaunchConfiguration("configure_safety_limits").perform(context)
-    )
-    configure_peripheral_on_connect = _as_bool(
-        LaunchConfiguration("configure_peripheral_on_connect").perform(context)
-    )
-    enable_tool_control = _as_bool(
-        LaunchConfiguration("enable_tool_control").perform(context)
-    )
-    enable_trigger_gripper_control = _as_bool(
-        LaunchConfiguration("enable_trigger_gripper_control").perform(context)
-    )
-    trigger_close_threshold = float(
-        LaunchConfiguration("trigger_close_threshold").perform(context)
-    )

    if arm not in ("left", "right", "both"):
        raise ValueError("arm must be one of: left, right, both")

    nodes = [_udp_receiver_node()]
    if arm == "both":
-        nodes.extend(
-            _dual_arm_nodes(
-                use_mock,
-                move_to_initial_pose,
-                left_avoid_singularity,
-                right_avoid_singularity,
-                frame_type,
-                control_rate_hz,
-                follow,
-                configure_safety_limits,
-                enable_tool_control,
-                enable_trigger_gripper_control,
-                trigger_close_threshold,
-                configure_peripheral_on_connect,
-            )
-        )
+        nodes.extend(_dual_arm_nodes(use_mock))
    else:
-        avoid_singularity = left_avoid_singularity if arm == "left" else right_avoid_singularity
-        nodes.append(
-            _single_arm_node(
-                arm,
-                use_mock,
-                move_to_initial_pose,
-                avoid_singularity,
-                frame_type,
-                control_rate_hz,
-                follow,
-                configure_safety_limits,
-                enable_tool_control,
-                enable_trigger_gripper_control,
-                trigger_close_threshold,
-                configure_peripheral_on_connect,
-            )
-        )
+        nodes.append(_single_arm_node(arm, use_mock))
    return nodes


@@ -259,30 +162,6 @@ def generate_launch_description() -> LaunchDescription:
        DeclareLaunchArgument("udp_port", default_value="15000"),
        # UDP receiver 轮询频率高于 PICO 发送频率，减少 socket 中等待时间。
        DeclareLaunchArgument("udp_timer_hz", default_value="200.0"),
-        # 左右 RM75 默认 IP，可在命令行中按现场网络覆盖。
-        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("left_avoid_singularity", default_value="0"),
-        DeclareLaunchArgument("right_avoid_singularity", default_value="1"),
-        # 非空时作为左右臂全局覆盖，例如 avoid_singularity:=0。
-        DeclareLaunchArgument("avoid_singularity", default_value=""),
-        DeclareLaunchArgument("frame_type", default_value="1"),
-        # 现场调参入口：默认按 PICO 90Hz 输入节奏发送 rm_movep_canfd。
-        DeclareLaunchArgument("control_rate_hz", default_value="90.0"),
-        # 默认低跟随；高跟随请确认控制器和网络能稳定满足厂商周期要求后再打开。
-        DeclareLaunchArgument("follow", default_value="false"),
-        DeclareLaunchArgument("configure_safety_limits", default_value="true"),
-        # 工具控制通过遥操作节点复用同一个 RealMan 连接，避免两个进程抢同一机械臂连接。
-        DeclareLaunchArgument("enable_tool_control", default_value="true"),
-        # trigger 上升沿切换夹爪开/关；grip 仍只控制机械臂运动。
-        DeclareLaunchArgument("enable_trigger_gripper_control", default_value="true"),
-        DeclareLaunchArgument("trigger_close_threshold", default_value="0.95"),
-        # 连接成功后是否配置外设；关闭后仅订阅开合话题，但开合前需要另行完成外设配置。
-        DeclareLaunchArgument("configure_peripheral_on_connect", default_value="true"),
-        # auto 时由单/双臂 YAML 决定；也可显式传 true/false 覆盖。
-        DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="auto"),
        # OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。
        OpaqueFunction(function=_launch_setup),
    ])
@@ -0,0 +1,94 @@
+import importlib.util
+import signal
+import subprocess
+import unittest
+from pathlib import Path
+from unittest import mock
+
+
+MODULE_PATH = Path(__file__).parents[1] / "tools" / "launcher_ui.py"
+SPEC = importlib.util.spec_from_file_location("launcher_ui", MODULE_PATH)
+launcher_ui = importlib.util.module_from_spec(SPEC)
+assert SPEC.loader is not None
+SPEC.loader.exec_module(launcher_ui)
+
+
+class LauncherCleanupTest(unittest.TestCase):
+    def test_xrobotoolkit_cleanup_policy_only_stops_service_on_window_close(self) -> None:
+        stop_all_patterns = set(
+            launcher_ui._xrobotoolkit_cleanup_patterns(stop_pc_service=False)
+        )
+        window_close_patterns = set(
+            launcher_ui._xrobotoolkit_cleanup_patterns(stop_pc_service=True)
+        )
+
+        self.assertLessEqual(
+            {"RobotLinuxDemo.x86_64", "PXREAClientUnity"},
+            stop_all_patterns,
+        )
+        self.assertNotIn("RoboticsServiceProcess", stop_all_patterns)
+        self.assertIn("RoboticsServiceProcess", window_close_patterns)
+
+    def test_close_and_stop_all_select_different_pc_service_policies(self) -> None:
+        app = object.__new__(launcher_ui.LauncherApp)
+        calls = []
+
+        class Root:
+            destroyed = False
+
+            def destroy(self) -> None:
+                self.destroyed = True
+
+        app.root = Root()
+        app.stop_launched_processes = lambda **kwargs: calls.append(kwargs) or True
+
+        app.kill_launched_processes()
+        app.on_close_requested()
+
+        self.assertEqual(
+            calls,
+            [
+                {"confirm": True, "notify": True, "stop_pc_service": False},
+                {"confirm": True, "notify": False, "stop_pc_service": True},
+            ],
+        )
+        self.assertTrue(app.root.destroyed)
+
+    def test_stop_all_keeps_oldest_pc_service_and_stops_duplicates(self) -> None:
+        app = object.__new__(launcher_ui.LauncherApp)
+        app.status = mock.Mock()
+        app.close_related_terminal_windows = lambda: 0
+
+        def fake_check_output(command, **_kwargs):
+            if command == ["pgrep", "-o", "-f", "RoboticsServiceProcess"]:
+                return "101\n"
+            if command == ["pgrep", "-f", "RoboticsServiceProcess"]:
+                return "101\n202\n303\n"
+            raise subprocess.CalledProcessError(1, command)
+
+        with (
+            mock.patch.object(
+                launcher_ui.subprocess,
+                "check_output",
+                side_effect=fake_check_output,
+            ),
+            mock.patch.object(launcher_ui.os, "kill") as kill,
+            mock.patch.object(launcher_ui.time, "sleep"),
+        ):
+            app.stop_launched_processes(
+                confirm=False,
+                notify=False,
+                stop_pc_service=False,
+            )
+
+        self.assertEqual(
+            kill.call_args_list,
+            [
+                mock.call(202, signal.SIGTERM),
+                mock.call(303, signal.SIGTERM),
+            ],
+        )
+
+
+if __name__ == "__main__":
+    unittest.main()
@@ -121,6 +121,17 @@ def _xrobotoolkit_bridge_command() -> str:
    )


+def _xrobotoolkit_cleanup_patterns(*, stop_pc_service: bool) -> tuple[str, ...]:
+    patterns = (
+        XROBOTOOLKIT_BRIDGE_EXECUTABLE,
+        "RobotLinuxDemo.x86_64",
+        "PXREAClientUnity",
+    )
+    if stop_pc_service:
+        return (*patterns, "RoboticsServiceProcess")
+    return patterns
+
+
 def _tool_command(arm: str, open_tool: bool) -> str:
    arm_name = "left_rm75" if arm == "left" else "right_rm75"
    value = "true" if open_tool else "false"
@@ -289,8 +300,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
            ("Ping Left RM75", f"ping -c 4 {DEFAULT_LEFT_IP}"),
            (
                "Left Arm RealMan Launch",
-                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false "
-                f"left_robot_ip:={DEFAULT_LEFT_IP}",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false",
            ),
            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
            ("Left Tool Open", _tool_command("left", True)),
@@ -306,8 +316,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
            ("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
            (
                "Right Arm RealMan Launch",
-                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false "
-                f"right_robot_ip:={DEFAULT_RIGHT_IP}",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false",
            ),
            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
            ("Right Tool Open", _tool_command("right", True)),
@@ -324,9 +333,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
            ("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
            (
                "Dual Arm RealMan Launch",
-                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false "
-                f"left_robot_ip:={DEFAULT_LEFT_IP} right_robot_ip:={DEFAULT_RIGHT_IP} "
-                "move_to_initial_pose_on_connect:=false",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false",
            ),
            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
            (
@@ -1283,13 +1290,27 @@ class LauncherApp:
        return closed

    def on_close_requested(self) -> None:
-        if self.stop_launched_processes(confirm=True, notify=False):
+        if self.stop_launched_processes(
+            confirm=True,
+            notify=False,
+            stop_pc_service=True,
+        ):
            self.root.destroy()

    def kill_launched_processes(self) -> None:
-        self.stop_launched_processes(confirm=True, notify=True)
+        self.stop_launched_processes(
+            confirm=True,
+            notify=True,
+            stop_pc_service=False,
+        )

-    def stop_launched_processes(self, *, confirm: bool, notify: bool) -> bool:
+    def stop_launched_processes(
+        self,
+        *,
+        confirm: bool,
+        notify: bool,
+        stop_pc_service: bool,
+    ) -> bool:
        if confirm and not messagebox.askyesno(
            "Confirm Stop",
            "Stop XR-RM launcher terminals, topic monitors, ROS nodes, and bridge processes started from this workspace?",
@@ -1302,7 +1323,7 @@ class LauncherApp:
            "ros2 run xr_rm_input",
            "ros2 run xr_rm_teleop",
            "XR_RM_LAUNCHER_SESSION=1",
-            XROBOTOOLKIT_BRIDGE_EXECUTABLE,
+            *_xrobotoolkit_cleanup_patterns(stop_pc_service=stop_pc_service),
            TERMINAL_TITLE_PREFIX,
            TOPIC_MONITOR_TITLE,
            CMD_VEL_MONITOR_TITLE,
@@ -1326,6 +1347,20 @@ class LauncherApp:
            "ros2 topic list",
            "ros2 node list",
        ]
+        pc_service_keep_pid: int | None = None
+        if not stop_pc_service:
+            try:
+                output = subprocess.check_output(
+                    ["pgrep", "-o", "-f", "RoboticsServiceProcess"],
+                    text=True,
+                )
+                pc_service_keep_pid = int(output.strip())
+                patterns.append("RoboticsServiceProcess")
+            except (subprocess.CalledProcessError, ValueError):
+                pass
+            except Exception as exc:
+                print(f"Failed to find the oldest RoboticsServiceProcess: {exc}")
+
        protected = {os.getpid(), os.getppid()}
        killed: set[int] = set()

@@ -1345,6 +1380,8 @@ class LauncherApp:
                    continue
                if pid in protected or pid in killed:
                    continue
+                if pattern == "RoboticsServiceProcess" and pid == pc_service_keep_pid:
+                    continue
                try:
                    os.kill(pid, signal.SIGTERM)
                    killed.add(pid)
@@ -0,0 +1,453 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-1-g15f4949  Build Version: 1.6.7594.29634
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="RM75-B">
+  <link
+    name="base_link">
+    <inertial>
+      <origin
+        xyz="0.00049987 5.2709E-05 0.060019"
+        rpy="0 0 0" />
+      <mass
+        value="1.862" />
+      <inertia
+        ixx="0.0017232"
+        ixy="-3.1058E-06"
+        ixz="-3.7924E-05"
+        iyy="0.0017051"
+        iyz="1.3691E-06"
+        izz="0.00090158" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="link_1">
+    <inertial>
+      <origin
+        xyz="0.000241 -0.013273 -0.00995"
+        rpy="0 0 0" />
+      <mass
+        value="1.574" />
+      <inertia
+        ixx="0.002487573"
+        ixy="0.000009663"
+        ixz="-0.000007909"
+        iyy="0.002321038"
+        iyz="0.000179393"
+        izz="0.001450554" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_1"
+    type="revolute">
+    <origin
+      xyz="0 0 0.2405"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="60"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_2">
+    <inertial>
+      <origin
+        xyz="-0.000357 -0.106789 0.005329"
+        rpy="0 0 0" />
+      <mass
+        value="1.217" />
+      <inertia
+        ixx="0.003494121"
+        ixy="0.000002921"
+        ixz="-0.000005613"
+        iyy="0.000892721"
+        iyz="-0.000583884"
+        izz="0.003444080" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_2"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_1" />
+    <child
+      link="link_2" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.2689"
+      upper="2.2689"
+      effort="60"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_3">
+    <inertial>
+      <origin
+        xyz="0.000003 -0.01398 -0.011324"
+        rpy="0 0 0" />
+      <mass
+        value="1.11" />
+      <inertia
+        ixx="0.001836663"
+        ixy="0.000002259"
+        ixz="-0.000004216"
+        iyy="0.001498875"
+        iyz="0.000037167"
+        izz="0.001062545" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_3"
+    type="revolute">
+    <origin
+      xyz="0 -0.256 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_2" />
+    <child
+      link="link_3" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="30"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_4">
+    <inertial>
+      <origin
+        xyz="-0.000005 -0.084658 0.004747"
+        rpy="0 0 0" />
+      <mass
+        value="0.685" />
+      <inertia
+        ixx="0.001282444"
+        ixy="-0.000000551"
+        ixz="-0.000000630"
+        iyy="0.000373013"
+        iyz="-0.000232084"
+        izz="0.001256177" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_4"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_3" />
+    <child
+      link="link_4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.356"
+      upper="2.356"
+      effort="30"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_5">
+    <inertial>
+      <origin
+        xyz="0.000078 -0.012937 -0.008781"
+        rpy="0 0 0" />
+      <mass
+        value="0.619" />
+      <inertia
+        ixx="0.000627336"
+        ixy="0.000001636"
+        ixz="-0.000001345"
+        iyy="0.000542455"
+        iyz="0.000034970"
+        izz="0.000370291" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_5"
+    type="revolute">
+    <origin
+      xyz="0 -0.21 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_4" />
+    <child
+      link="link_5" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_6">
+    <inertial>
+      <origin
+        xyz="-0.000014 -0.078524 0.002819"
+        rpy="0 0 0" />
+      <mass
+        value="0.602" />
+      <inertia
+        ixx="0.000780774"
+        ixy="-0.000000121"
+        ixz="-0.000000469"
+        iyy="0.000289973"
+        iyz="-0.000120513"
+        izz="0.000763955" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_6"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_5" />
+    <child
+      link="link_6" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.234"
+      upper="2.234"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_7">
+    <inertial>
+      <origin
+        xyz="0.001094 -0.000077 -0.010119"
+        rpy="0 0 0" />
+      <mass
+        value="0.107" />
+      <inertia
+        ixx="0.000044123"
+        ixy="-0.000000064"
+        ixz="0.0000003"
+        iyy="0.000035078"
+        iyz="-0.000000029"
+        izz="0.000065445" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_7.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_7"
+    type="revolute">
+    <origin
+      xyz="0 -0.144 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_6" />
+    <child
+      link="link_7" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-6.28"
+      upper="6.28"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+</robot>
@@ -0,0 +1,503 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<robot name="RM75_B_OmniPicker_fixed">
+  <link name="base_link">
+    <inertial>
+      <origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0"/>
+      <mass value="1.862"/>
+      <inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/base_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/base_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_1">
+    <inertial>
+      <origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0"/>
+      <mass value="1.574"/>
+      <inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_1.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_1.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_1" type="revolute">
+    <origin xyz="0 0 0.2405" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.106" upper="3.106" effort="60" velocity="3.14"/>
+  </joint>
+  <link name="link_2">
+    <inertial>
+      <origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0"/>
+      <mass value="1.217"/>
+      <inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_2.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_2.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_2" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0"/>
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14"/>
+  </joint>
+  <link name="link_3">
+    <inertial>
+      <origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0"/>
+      <mass value="1.11"/>
+      <inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_3.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_3.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_3" type="revolute">
+    <origin xyz="0 -0.256 0" rpy="1.5708 0 0"/>
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.106" upper="3.106" effort="30" velocity="3.14"/>
+  </joint>
+  <link name="link_4">
+    <inertial>
+      <origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0"/>
+      <mass value="0.685"/>
+      <inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_4.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_4.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_4" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0"/>
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.356" upper="2.356" effort="30" velocity="3.14"/>
+  </joint>
+  <link name="link_5">
+    <inertial>
+      <origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0"/>
+      <mass value="0.619"/>
+      <inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_5.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_5.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_5" type="revolute">
+    <origin xyz="0 -0.21 0" rpy="1.5708 0 0"/>
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.106" upper="3.106" effort="10" velocity="3.14"/>
+  </joint>
+  <link name="link_6">
+    <inertial>
+      <origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0"/>
+      <mass value="0.602"/>
+      <inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_6.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_6.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_6" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0"/>
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.234" upper="2.234" effort="10" velocity="3.14"/>
+  </joint>
+  <link name="link_7">
+    <inertial>
+      <origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0"/>
+      <mass value="0.107"/>
+      <inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_7.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_7.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_7" type="revolute">
+    <origin xyz="0 -0.144 0" rpy="1.5708 0 0"/>
+    <parent link="link_6"/>
+    <child link="link_7"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-6.28" upper="6.28" effort="10" velocity="3.14"/>
+  </joint>
+  <!-- RM75 end-flange alias. link_7 is treated as the tool mounting frame. -->
+  <link name="rm75_flange"/>
+  <joint name="rm75_link7_to_flange" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="link_7"/>
+    <child link="rm75_flange"/>
+  </joint>
+  <!-- OmniPicker mounting transform. Adjust xyz/rpy here if an adapter plate or different clocking is used. -->
+  <joint name="rm75_flange_to_omnipicker" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="rm75_flange"/>
+    <child link="omnipicker_base_link"/>
+  </joint>
+  <link name="omnipicker_base_link">
+    <inertial>
+      <origin xyz="-0.00005520 1.2341E-05 0.03296193" rpy="0 0 0"/>
+      <mass value="0.25641368"/>
+      <inertia ixx="4.6351E-04" ixy="-1.0E-08" ixz="-1.04E-06" iyy="4.4525E-04" iyz="-5.00E-08" izz="1.0438E-04"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/base_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.89804 0.91765 0.92941 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="omnipicker_hand_narrow1_Link">
+    <inertial>
+      <origin xyz="0.0094685 0.0068806 6.5437E-05" rpy="0 0 0"/>
+      <mass value="0.025428"/>
+      <inertia ixx="1.9574E-06" ixy="-4.2911E-07" ixz="-3.7111E-11" iyy="2.3919E-06" iyz="-3.008E-10" izz="1.5501E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow1_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow1_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_narrow1_joint" type="fixed">
+    <origin xyz="0 -0.0195 0.0565" rpy="-2.9951 -1.5708 -0.15964"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_hand_narrow1_Link"/>
+  </joint>
+  <link name="omnipicker_hand_narrow2_Link">
+    <inertial>
+      <origin xyz="0.0088027 -0.007035 1.6424E-05" rpy="0 0 0"/>
+      <mass value="0.0040132"/>
+      <inertia ixx="1.0307E-07" ixy="5.7851E-08" ixz="9.5801E-11" iyy="1.1385E-07" iyz="-2.81E-11" izz="1.7009E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow2_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow2_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_narrow2_joint" type="fixed">
+    <origin xyz="0.030852 0.018551 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_narrow1_Link"/>
+    <child link="omnipicker_hand_narrow2_Link"/>
+  </joint>
+  <link name="omnipicker_hand_narrow3_Link">
+    <inertial>
+      <origin xyz="0.012508 -0.0079729 9.4339E-05" rpy="0 0 0"/>
+      <mass value="0.018029"/>
+      <inertia ixx="1.1403E-06" ixy="5.5159E-07" ixz="-4.0096E-13" iyy="2.5704E-06" iyz="1.0951E-12" izz="2.3252E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow3_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow3_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_narrow3_joint" type="fixed">
+    <origin xyz="0.018118 -0.01574 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_narrow2_Link"/>
+    <child link="omnipicker_hand_narrow3_Link"/>
+  </joint>
+  <link name="omnipicker_hand_narrow4_Link">
+  </link>
+  <joint name="omnipicker_hand_narrow4_joint" type="fixed">
+    <origin xyz="0 -0.0104 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_narrow3_Link"/>
+    <child link="omnipicker_hand_narrow4_Link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.14" upper="3.14" effort="0" velocity="0"/>
+  </joint>
+  <link name="omnipicker_hand_narrow_loop_Link">
+    <inertial>
+      <origin xyz="0.014869 -0.0036066 0.00029307" rpy="0 0 0"/>
+      <mass value="0.022591"/>
+      <inertia ixx="4.3916E-06" ixy="1.114E-07" ixz="-4.9655E-12" iyy="4.737E-06" iyz="1.7121E-11" izz="6.0445E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow_loop_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow_loop_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_narrow_loop_joint" type="fixed">
+    <origin xyz="0 -0.021633 0.07387" rpy="-2.9951 -1.5708 -0.15964"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_hand_narrow_loop_Link"/>
+  </joint>
+  <link name="omnipicker_hand_wide1_Link">
+    <inertial>
+      <origin xyz="0.0095051 -0.0068479 6.8268E-05" rpy="0 0 0"/>
+      <mass value="0.025428"/>
+      <inertia ixx="1.9565E-06" ixy="4.2798E-07" ixz="2.3844E-10" iyy="2.3928E-06" iyz="-1.4454E-10" izz="1.5501E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide1_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide1_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_wide1_joint" type="fixed">
+    <origin xyz="0 0.0195 0.0565" rpy="-2.9951 -1.5708 -0.15964"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_hand_wide1_Link"/>
+  </joint>
+  <link name="omnipicker_hand_wide2_Link">
+    <inertial>
+      <origin xyz="0.0088027 0.007035 -1.6424E-05" rpy="0 0 0"/>
+      <mass value="0.0040132"/>
+      <inertia ixx="1.0307E-07" ixy="-5.7851E-08" ixz="-9.58E-11" iyy="1.1385E-07" iyz="-2.81E-11" izz="1.7009E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide2_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide2_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_wide2_joint" type="fixed">
+    <origin xyz="0.030852 -0.018551 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_wide1_Link"/>
+    <child link="omnipicker_hand_wide2_Link"/>
+  </joint>
+  <link name="omnipicker_hand_wide3_Link">
+    <inertial>
+      <origin xyz="0.016206 0.0094593 4.7668E-05" rpy="0 0 0"/>
+      <mass value="0.035835"/>
+      <inertia ixx="8.5056E-06" ixy="-1.1363E-06" ixz="-4.2908E-11" iyy="1.1235E-05" iyz="-2.9251E-11" izz="4.6309E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide3_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide3_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_wide3_joint" type="fixed">
+    <origin xyz="0.018118 0.01574 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_wide2_Link"/>
+    <child link="omnipicker_hand_wide3_Link"/>
+  </joint>
+  <link name="omnipicker_hand_wide4_Link">
+  </link>
+  <joint name="omnipicker_hand_wide4_joint" type="fixed">
+    <origin xyz="0 0.0104 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_wide3_Link"/>
+    <child link="omnipicker_hand_wide4_Link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.14" upper="3.14" effort="0" velocity="0"/>
+  </joint>
+  <link name="omnipicker_hand_wide_loop_Link">
+    <inertial>
+      <origin xyz="0.016268 0.0040555 0.00030323" rpy="0 0 0"/>
+      <mass value="0.025142"/>
+      <inertia ixx="5.887E-06" ixy="-1.1234E-07" ixz="2.1954E-11" iyy="6.236E-06" iyz="-9.473E-12" izz="6.2389E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide_loop_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide_loop_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_wide_loop_joint" type="fixed">
+    <origin xyz="0 0.021633 0.07387" rpy="-2.9951 -1.5708 -0.15964"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_hand_wide_loop_Link"/>
+  </joint>
+  <link name="omnipicker_mount_frame"/>
+  <joint name="omnipicker_base_to_mount_frame" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_mount_frame"/>
+  </joint>
+  <link name="omnipicker_tcp"/>
+  <joint name="omnipicker_tcp_joint" type="fixed">
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_tcp"/>
+    <origin xyz="0 0 0.16" rpy="0 0 0"/>
+  </joint>
+</robot>
@@ -1,8 +1,10 @@
 """xr_rm_teleop 包安装配置。

-该包提供基于 XR 相对位姿的 RM75 笛卡尔位姿透传遥操作节点。
+该包提供基于 XR 相对位姿和 Placo QP 的 RM75 遥操作节点。
 """

+from glob import glob
+
 from setuptools import setup

 package_name = "xr_rm_teleop"
@@ -14,6 +16,26 @@ setup(
    data_files=[
        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
        (f"share/{package_name}", ["package.xml"]),
+        (
+            f"share/{package_name}/models/rm75",
+            ["models/rm75/RM75-B.urdf"],
+        ),
+        (
+            f"share/{package_name}/models/rm75/meshes",
+            glob("models/rm75/meshes/*.STL"),
+        ),
+        (
+            f"share/{package_name}/models/rm75_omnipicker/urdf",
+            glob("models/rm75_omnipicker/urdf/*.urdf"),
+        ),
+        (
+            f"share/{package_name}/models/rm75_omnipicker/meshes/rm75",
+            glob("models/rm75_omnipicker/meshes/rm75/*.STL"),
+        ),
+        (
+            f"share/{package_name}/models/rm75_omnipicker/meshes/omnipicker",
+            glob("models/rm75_omnipicker/meshes/omnipicker/*.STL"),
+        ),
    ],
    install_requires=["setuptools"],
    zip_safe=True,
@@ -0,0 +1,94 @@
+from __future__ import annotations
+
+import math
+import sys
+import time
+from pathlib import Path
+
+import numpy as np
+
+from xr_rm_teleop.placo_ik_solver import PlacoIkSolver
+
+
+CASES = {
+    "left": [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
+    "right": [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
+}
+
+
+def rotation_z(angle: float) -> np.ndarray:
+    cosine = math.cos(angle)
+    sine = math.sin(angle)
+    return np.asarray(
+        [
+            [cosine, -sine, 0.0],
+            [sine, cosine, 0.0],
+            [0.0, 0.0, 1.0],
+        ]
+    )
+
+
+def angle_error(actual: np.ndarray, target: np.ndarray) -> float:
+    cosine = np.clip((np.trace(target @ actual.T) - 1.0) * 0.5, -1.0, 1.0)
+    return float(math.acos(cosine))
+
+
+def main() -> None:
+    urdf_path = Path(sys.argv[1]).resolve()
+    for arm, joint_degrees in CASES.items():
+        initial_joints = np.deg2rad(joint_degrees)
+        drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
+        joints = initial_joints.tolist()
+        stationary_target = drift_solver.update_joint_state(joints)
+        flange = drift_solver._robot.get_T_world_frame("link_7")
+        flange_to_tcp = np.linalg.inv(flange) @ stationary_target
+        assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, 0.16])
+        assert np.allclose(flange_to_tcp[:3, :3], np.eye(3))
+        for _ in range(250):
+            drift_solver.update_joint_state(joints)
+            joints = drift_solver.solve(stationary_target)
+        drift_degrees = float(
+            np.max(np.abs(np.rad2deg(np.asarray(joints) - initial_joints)))
+        )
+        assert drift_degrees <= 0.05, (
+            f"{arm} stationary target drifted {drift_degrees:.3f}deg"
+        )
+
+        solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
+        joints = initial_joints.tolist()
+        current = solver.update_joint_state(joints)
+        assert current.shape == (4, 4)
+        target = current.copy()
+        target[0, 3] += 0.01
+        target[:3, :3] = rotation_z(0.05) @ target[:3, :3]
+
+        solve_durations = []
+        for _ in range(250):
+            solver.update_joint_state(joints)
+            started_at = time.perf_counter()
+            joints = solver.solve(target)
+            solve_durations.append(time.perf_counter() - started_at)
+
+        actual = solver.update_joint_state(joints)
+        position_error = np.linalg.norm(actual[:3, 3] - target[:3, 3])
+        orientation_error = angle_error(actual[:3, :3], target[:3, :3])
+        assert len(joints) == 7
+        assert np.isfinite(joints).all()
+        assert np.allclose(
+            solver.base_configuration,
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+        )
+        assert position_error <= 0.005
+        assert orientation_error <= math.radians(2.0)
+        print(
+            f"{arm}: position_error={position_error:.6f}m, "
+            f"orientation_error={math.degrees(orientation_error):.3f}deg, "
+            f"stationary_drift={drift_degrees:.3f}deg, "
+            f"solve_avg={1000.0 * np.mean(solve_durations):.3f}ms, "
+            f"solve_max={1000.0 * max(solve_durations):.3f}ms, "
+            f"solve_overruns={sum(value > 1.0 / 125.0 for value in solve_durations)}"
+        )
+
+
+if __name__ == "__main__":
+    main()
@@ -1,4 +1,13 @@
+import math
+import sys
+from types import ModuleType, SimpleNamespace
+
+import pytest
+
+from xr_rm_teleop import realman_adapter
 from xr_rm_teleop.realman_adapter import RealManAdapter
+from xr_rm_teleop.realman_adapter import MockRealManAdapter
+from xr_rm_teleop.fun_peripheral import PeripheralConfig, _configure_tool_frame


 def test_initial_pose_uses_joint_move_only() -> None:
@@ -11,9 +20,435 @@ def test_initial_pose_uses_joint_move_only() -> None:
            return 0

    joints = [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
-    adapter = RealManAdapter("127.0.0.1", 8080, 0, 1, initial_joint_pose=joints)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+        initial_joint_pose=joints,
+    )
    adapter._arm = FakeArm()

    adapter._move_to_initial_pose()

    assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]
+
+
+def test_peripheral_config_exposes_selected_tool() -> None:
+    config = PeripheralConfig(
+        scissorgripper=1,
+        tools_in_ee={
+            "first": [[0.0] * 7, [0.0] * 7],
+            "second": [[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0], [0.0] * 7],
+        },
+    )
+
+    assert config.tool_name == "second"
+    assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
+
+
+@pytest.mark.parametrize(
+    ("existing", "expected_operation"),
+    [(False, "create"), (True, "update")],
+)
+def test_tool_frame_is_created_or_updated(existing, expected_operation) -> None:
+    class FakeArm:
+        def __init__(self) -> None:
+            self.calls = []
+
+        def rm_get_total_tool_frame(self):
+            self.calls.append(("get",))
+            names = ["omnipic"] if existing else []
+            return {"return_code": 0, "tool_names": names}
+
+        def rm_set_manual_tool_frame(self, *, frame):
+            self.calls.append(("create", frame))
+            return 0
+
+        def rm_update_tool_frame(self, *, frame):
+            self.calls.append(("update", frame))
+            return 0
+
+        def rm_change_tool_frame(self, tool_name):
+            self.calls.append(("change", tool_name))
+            return 0
+
+    arm = FakeArm()
+    frame = object()
+
+    _configure_tool_frame(arm, frame, "omnipic")
+
+    assert arm.calls == [
+        ("get",),
+        (expected_operation, frame),
+        ("change", "omnipic"),
+    ]
+
+
+@pytest.mark.parametrize(
+    ("existing", "failure", "operation"),
+    [
+        (False, "query", "rm_get_total_tool_frame"),
+        (False, "create", "rm_set_manual_tool_frame"),
+        (True, "update", "rm_update_tool_frame"),
+        (False, "change", "rm_change_tool_frame"),
+    ],
+)
+def test_tool_frame_sdk_failures_are_reported(existing, failure, operation) -> None:
+    class FakeArm:
+        def rm_get_total_tool_frame(self):
+            names = ["omnipic"] if existing else []
+            return {
+                "return_code": 1 if failure == "query" else 0,
+                "tool_names": names,
+            }
+
+        def rm_set_manual_tool_frame(self, *, frame):
+            del frame
+            return 1 if failure == "create" else 0
+
+        def rm_update_tool_frame(self, *, frame):
+            del frame
+            return 1 if failure == "update" else 0
+
+        def rm_change_tool_frame(self, tool_name):
+            del tool_name
+            return 1 if failure == "change" else 0
+
+    with pytest.raises(RuntimeError, match=operation):
+        _configure_tool_frame(FakeArm(), object(), "omnipic")
+
+
+def _udp_state(
+    *,
+    robot_ip: str = "127.0.0.1",
+    joints=None,
+    error_code: int = 0,
+    joint_enabled=None,
+    joint_error_codes=None,
+    arm_error_codes=None,
+    arm_current_status: int = 0,
+):
+    if joints is None:
+        joints = [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
+    if joint_enabled is None:
+        joint_enabled = [True] * 7
+    if joint_error_codes is None:
+        joint_error_codes = [0] * 7
+    if arm_error_codes is None:
+        arm_error_codes = []
+    return SimpleNamespace(
+        errCode=error_code,
+        arm_ip=robot_ip.encode(),
+        joint_status=SimpleNamespace(
+            joint_position=joints,
+            joint_en_flag=joint_enabled,
+            joint_err_code=joint_error_codes,
+        ),
+        err=SimpleNamespace(
+            err_len=len(arm_error_codes),
+            err=list(arm_error_codes),
+        ),
+        arm_current_status=arm_current_status,
+    )
+
+
+def _install_fake_sdk(monkeypatch, *, push_return=0, send_feedback=True):
+    class FakeThreadMode:
+        RM_TRIPLE_MODE_E = 2
+
+    class FakePushConfig:
+        def __init__(self, *args):
+            self.args = args
+
+    class FakeArm:
+        instance = None
+
+        def __init__(self, mode):
+            self.mode = mode
+            self.callback = None
+            self.config = None
+            self.delete_calls = 0
+            FakeArm.instance = self
+
+        def rm_create_robot_arm(self, robot_ip, robot_port):
+            self.robot_ip = robot_ip
+            self.robot_port = robot_port
+            return SimpleNamespace(id=1)
+
+        def rm_realtime_arm_state_call_back(self, callback):
+            self.callback = callback
+
+        def rm_set_realtime_push(self, config):
+            self.config = config
+            if push_return == 0 and send_feedback:
+                self.callback(_udp_state())
+            return push_return
+
+        def rm_delete_robot_arm(self):
+            self.delete_calls += 1
+            return 0
+
+    sdk = ModuleType("Robotic_Arm.rm_robot_interface")
+    sdk.RoboticArm = FakeArm
+    sdk.rm_thread_mode_e = FakeThreadMode
+    sdk.rm_realtime_push_config_t = FakePushConfig
+    sdk.rm_realtime_arm_state_callback_ptr = lambda callback: callback
+    package = ModuleType("Robotic_Arm")
+    package.rm_robot_interface = sdk
+    monkeypatch.setitem(sys.modules, "Robotic_Arm", package)
+    monkeypatch.setitem(sys.modules, "Robotic_Arm.rm_robot_interface", sdk)
+    return SimpleNamespace(RoboticArm=FakeArm)
+
+
+def test_udp_feedback_is_cached_in_radians(monkeypatch) -> None:
+    monotonic = iter([10.0, 10.005])
+    monkeypatch.setattr(realman_adapter.time, "monotonic", lambda: next(monotonic))
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._accept_realtime_feedback = True
+
+    adapter._on_realtime_arm_state(_udp_state())
+    first = adapter.get_latest_joint_state()
+    adapter._on_realtime_arm_state(_udp_state())
+    second = adapter.get_latest_joint_state()
+
+    assert first is not None
+    assert first.positions == pytest.approx(
+        [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
+    )
+    assert first.read_duration_ms is None
+    assert first.update_interval_ms is None
+    assert second is not None
+    assert second.read_duration_ms is None
+    assert second.update_interval_ms == pytest.approx(5.0)
+
+
+@pytest.mark.parametrize(
+    "state",
+    [
+        _udp_state(error_code=-3),
+        _udp_state(robot_ip="192.168.192.18"),
+        _udp_state(joints=[0.0] * 6),
+        _udp_state(joints=[0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]),
+    ],
+)
+def test_invalid_udp_feedback_does_not_replace_snapshot(state) -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._accept_realtime_feedback = True
+    adapter._on_realtime_arm_state(_udp_state(joints=[1.0] * 7))
+    before = adapter.get_latest_joint_state()
+
+    adapter._on_realtime_arm_state(state)
+
+    assert adapter.get_latest_joint_state() == before
+
+
+def test_udp_joint_fault_marks_snapshot_unready() -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._accept_realtime_feedback = True
+
+    adapter._on_realtime_arm_state(
+        _udp_state(
+            joint_enabled=[True, True, False, True, True, True, True],
+            joint_error_codes=[0, 0, 17, 0, 0, 0, 0],
+            arm_error_codes=[42],
+            arm_current_status=9,
+        )
+    )
+
+    snapshot = adapter.get_latest_joint_state()
+    assert snapshot is not None
+    assert snapshot.motion_ready is False
+
+
+def test_udp_stop_status_marks_snapshot_unready_without_joint_error() -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._accept_realtime_feedback = True
+
+    adapter._on_realtime_arm_state(_udp_state(arm_current_status=9))
+
+    snapshot = adapter.get_latest_joint_state()
+    assert snapshot is not None
+    assert snapshot.motion_ready is False
+
+
+def test_udp_zero_arm_error_code_is_motion_ready() -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._accept_realtime_feedback = True
+
+    adapter._on_realtime_arm_state(_udp_state(arm_error_codes=[0]))
+
+    snapshot = adapter.get_latest_joint_state()
+    assert snapshot is not None
+    assert snapshot.motion_ready is True
+
+
+def test_udp_fault_and_recovery_are_logged_once_per_transition() -> None:
+    class FakeLogger:
+        def __init__(self) -> None:
+            self.infos = []
+            self.warnings = []
+
+        def info(self, message):
+            self.infos.append(message)
+
+        def warn(self, message):
+            self.warnings.append(message)
+
+    logger = FakeLogger()
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+        logger=logger,
+    )
+    adapter._accept_realtime_feedback = True
+    adapter._on_realtime_arm_state(_udp_state())
+    fault = _udp_state(
+        joint_enabled=[False] * 7,
+        joint_error_codes=[17, 0, 0, 0, 0, 0, 0],
+        arm_error_codes=[42],
+        arm_current_status=9,
+    )
+
+    adapter._on_realtime_arm_state(fault)
+    adapter._on_realtime_arm_state(fault)
+    adapter._on_realtime_arm_state(_udp_state())
+
+    assert len(logger.warnings) == 1
+    assert "joint_errors=[17, 0, 0, 0, 0, 0, 0]" in logger.warnings[0]
+    assert len([message for message in logger.infos if "恢复正常" in message]) == 1
+
+
+def test_connect_configures_udp_feedback_and_waits_for_first_frame(monkeypatch) -> None:
+    fake_sdk = _install_fake_sdk(monkeypatch)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "192.168.192.148",
+        8090,
+        configure_safety_limits=False,
+    )
+
+    adapter.connect()
+
+    arm = fake_sdk.RoboticArm.instance
+    assert arm is not None
+    assert arm.config.args == (5, True, 8090, 0, "192.168.192.148")
+    assert arm.callback is adapter._realtime_callback
+    assert adapter.get_latest_joint_state() is not None
+    assert not hasattr(adapter, "_feedback_thread")
+
+
+def test_udp_configuration_failure_cleans_up_robot_handle(monkeypatch) -> None:
+    fake_sdk = _install_fake_sdk(monkeypatch, push_return=1)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "192.168.192.148",
+        8090,
+        configure_safety_limits=False,
+    )
+
+    with pytest.raises(RuntimeError, match="rm_set_realtime_push"):
+        adapter.connect()
+
+    assert fake_sdk.RoboticArm.instance.delete_calls == 1
+    assert adapter._arm is None
+
+
+def test_udp_first_frame_timeout_cleans_up_robot_handle(monkeypatch) -> None:
+    fake_sdk = _install_fake_sdk(monkeypatch, send_feedback=False)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "192.168.192.148",
+        8090,
+        configure_safety_limits=False,
+    )
+    adapter._feedback_ready = SimpleNamespace(
+        clear=lambda: None,
+        set=lambda: None,
+        wait=lambda timeout: False,
+    )
+
+    with pytest.raises(RuntimeError, match="within 2 seconds"):
+        adapter.connect()
+
+    assert fake_sdk.RoboticArm.instance.delete_calls == 1
+    assert adapter._arm is None
+
+
+def test_joint_target_uses_movej_canfd_in_degrees() -> None:
+    class FakeArm:
+        def __init__(self) -> None:
+            self.calls = []
+
+        def rm_movej_canfd(self, *args):
+            self.calls.append(args)
+            return 0
+
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._arm = FakeArm()
+    target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
+
+    adapter.send_joint_target(target, follow=False)
+
+    assert len(adapter._arm.calls) == 1
+    degrees, follow, expand, trajectory_mode, radio = adapter._arm.calls[0]
+    assert degrees == pytest.approx([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
+    assert (follow, expand, trajectory_mode, radio) == (False, 0, 2, 0)
+
+
+def test_mock_joint_feedback_is_available_without_vendor_sdk() -> None:
+    adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
+
+    adapter.connect()
+    snapshot = adapter.get_latest_joint_state()
+
+    assert snapshot is not None
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
+    )
@@ -0,0 +1,351 @@
+import math
+import time
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from xr_rm_teleop.realman_adapter import JointStateSnapshot
+from xr_rm_teleop.single_arm_velocity_teleop import (
+    SingleArmVelocityTeleop,
+    _make_transform,
+    _so3_exp,
+)
+
+
+class FakeLogger:
+    def info(self, *args, **kwargs):
+        del args, kwargs
+
+    def warn(self, *args, **kwargs):
+        del args, kwargs
+
+    def error(self, *args, **kwargs):
+        del args, kwargs
+
+
+class FakeTime:
+    def __sub__(self, other):
+        del other
+        return SimpleNamespace(nanoseconds=0)
+
+
+def test_missing_or_stale_feedback_does_not_enable_qp() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._command_timeout_sec = 0.12
+    teleop._adapter = SimpleNamespace(get_latest_joint_state=lambda: None)
+
+    assert teleop._fresh_joint_state() is None
+
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.0] * 7,
+            time.monotonic() - 1.0,
+        )
+    )
+    assert teleop._fresh_joint_state() is None
+
+
+def test_disabled_joint_feedback_does_not_enable_qp() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._command_timeout_sec = 0.12
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.0] * 7,
+            time.monotonic(),
+            motion_ready=False,
+        )
+    )
+
+    assert teleop._fresh_joint_state() is None
+
+
+def test_joint_command_step_limits_acceleration_from_rest() -> None:
+    dt = 1.0 / 125.0
+    target, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
+        target=[0.2] * 7,
+        previous_target=[0.0] * 7,
+        previous_velocity=[0.0] * 7,
+        max_speed=math.radians(180.0),
+        max_acceleration=math.radians(300.0),
+        dt=dt,
+    )
+
+    assert velocity == pytest.approx([math.radians(2.4)] * 7)
+    assert target == pytest.approx([math.radians(0.0192)] * 7)
+
+
+def test_feedback_fault_blocks_grip_until_release() -> None:
+    class FakeClock:
+        def now(self):
+            return FakeTime()
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.1] * 7,
+            time.monotonic(),
+        )
+    )
+    teleop._command_timeout_sec = 0.12
+    teleop._joint_feedback_ready = True
+    teleop._arm_name = "right_rm75"
+    teleop._last_msg = SimpleNamespace(
+        grip=True,
+        pose=SimpleNamespace(
+            position=SimpleNamespace(x=0.0, y=0.0, z=0.0),
+            orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0),
+        ),
+    )
+    teleop._last_msg_time = FakeTime()
+    teleop._active = False
+    teleop._enable_orientation_control = False
+    teleop._last_valid_joint_target = None
+    teleop._last_current_pose = None
+    teleop._ik_solver = SimpleNamespace(
+        update_joint_state=lambda joints: np.eye(4)
+    )
+    teleop._grip_rearm_required = True
+    teleop.get_clock = lambda: FakeClock()
+    teleop.get_logger = lambda: FakeLogger()
+    stopped = []
+    entered = []
+    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
+    teleop._enter_active_control = lambda *args: entered.append(args)
+
+    teleop._control_tick()
+    assert entered == []
+
+    teleop._last_msg.grip = False
+    teleop._control_tick()
+    assert teleop._grip_rearm_required is False
+
+    teleop._last_msg.grip = True
+    teleop._control_tick()
+    assert len(entered) == 1
+
+
+def test_stale_feedback_stops_before_active_control() -> None:
+    stopped = []
+    entered = []
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.0] * 7,
+            time.monotonic() - 1.0,
+        )
+    )
+    teleop._command_timeout_sec = 0.12
+    teleop._joint_feedback_ready = True
+    teleop._arm_name = "right_rm75"
+    teleop._last_msg = SimpleNamespace(
+        grip=True,
+        pose=SimpleNamespace(
+            position=SimpleNamespace(x=0.0, y=0.0, z=0.0),
+            orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0),
+        ),
+    )
+    teleop._last_msg_time = FakeTime()
+    teleop._active = False
+    teleop._enable_orientation_control = False
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
+    teleop._enter_active_control = lambda *args: entered.append(args)
+
+    teleop._control_tick()
+
+    assert stopped == [True]
+    assert entered == []
+
+
+def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
+    class FakeSolver:
+        def __init__(self) -> None:
+            self.solve_calls = 0
+
+        def update_joint_state(self, joints):
+            assert joints == [0.1] * 7
+            transform = np.eye(4)
+            transform[:3, 3] = [0.3, 0.0, 0.2]
+            return transform
+
+        def solve(self, target):
+            del target
+            self.solve_calls += 1
+            return [0.2] * 7
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._ik_solver = FakeSolver()
+    teleop._active = False
+    teleop._last_valid_joint_target = None
+    teleop._last_current_pose = None
+
+    pose = teleop._sync_joint_feedback(
+        JointStateSnapshot([0.1] * 7, time.monotonic())
+    )
+
+    assert pose == pytest.approx(
+        _make_transform([0.3, 0.0, 0.2], np.eye(3))
+    )
+    assert teleop._last_valid_joint_target == [0.1] * 7
+    assert teleop._ik_solver.solve_calls == 0
+
+
+def test_qp_failure_returns_last_known_good_target() -> None:
+    class FailingSolver:
+        def solve(self, target):
+            del target
+            raise RuntimeError("NaN in QP solution")
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._ik_solver = FailingSolver()
+    teleop._last_valid_joint_target = [0.1] * 7
+    teleop._arm_name = "right_rm75"
+    teleop.get_logger = lambda: FakeLogger()
+
+    target = teleop._solve_joint_target(np.eye(4))
+
+    assert target == pytest.approx([0.1] * 7)
+    assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
+
+
+def test_qp_success_updates_last_known_good_target() -> None:
+    class SuccessfulSolver:
+        def solve(self, target):
+            del target
+            return [0.2] * 7
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._ik_solver = SuccessfulSolver()
+    teleop._last_valid_joint_target = [0.1] * 7
+    teleop._arm_name = "left_rm75"
+    teleop.get_logger = lambda: FakeLogger()
+
+    target = teleop._solve_joint_target(np.eye(4))
+
+    assert target == pytest.approx([0.2] * 7)
+    assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
+
+
+def test_enter_active_control_initializes_se3_orientation_state() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    transform = _make_transform(
+        [0.3, -0.1, 0.2],
+        _so3_exp(np.asarray([0.1, -0.2, 0.3])),
+    )
+    published = []
+    teleop._arm_name = "right_rm75"
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._publish_debug = lambda *args: published.append(args)
+
+    teleop._enter_active_control(
+        [0.0, 0.0, 0.0],
+        (0.0, 0.0, 0.0, 1.0),
+        transform,
+        FakeTime(),
+    )
+
+    assert teleop._robot_start_transform == pytest.approx(transform)
+    assert teleop._filtered_target == pytest.approx(transform[:3, 3])
+    assert teleop._filtered_orientation_target == pytest.approx(transform[:3, :3])
+    assert teleop._last_sent_orientation == pytest.approx(transform[:3, :3])
+    assert len(published) == 1
+
+
+def test_command_angular_velocity_uses_so3_rotation_vector() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._dt = 0.1
+    teleop._last_sent_target = [0.0, 0.0, 0.0]
+    teleop._last_sent_orientation = np.eye(3)
+    teleop._last_command_time = None
+
+    velocity = teleop._estimate_command_velocity(
+        [0.0, 0.0, 0.0],
+        _so3_exp(np.asarray([0.0, 0.0, 0.1])),
+        FakeTime(),
+    )
+
+    assert velocity == pytest.approx([0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
+
+
+def test_timing_stats_logs_summary_and_clears_window() -> None:
+    messages = []
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "right_rm75"
+    teleop._dt = 0.008
+    teleop._timing_stats_window = 3
+    teleop._timing_samples = {
+        name: []
+        for name in (
+            "period",
+            "total",
+            "qp",
+            "send",
+            "feedback_age",
+            "feedback_read",
+            "feedback_interval",
+        )
+    }
+    teleop._last_timing_feedback_received_at = None
+    teleop.get_logger = lambda: SimpleNamespace(
+        info=lambda message: messages.append(message)
+    )
+    first_feedback = JointStateSnapshot([0.0] * 7, 10.0, 2.0, None)
+    second_feedback = JointStateSnapshot([0.0] * 7, 10.011, 3.0, 11.0)
+
+    teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0, first_feedback)
+    teleop._record_timing_sample(8.0, 8.0, 1.5, 0.6, 3.5, first_feedback)
+    assert messages == []
+
+    teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0, second_feedback)
+
+    assert len(messages) == 1
+    assert "right_rm75 timing n=3 deadline=8.000 ms" in messages[0]
+    assert (
+        "period[n=3 mean=8.000 p95=8.900 p99=8.980 "
+        "max=9.000 ms overruns=1]"
+    ) in messages[0]
+    assert (
+        "total[n=3 mean=8.000 p95=9.800 p99=9.960 "
+        "max=10.000 ms overruns=1]"
+    ) in messages[0]
+    assert "qp[n=3" in messages[0]
+    assert "send[n=3" in messages[0]
+    assert "feedback_age[n=3" in messages[0]
+    assert "feedback_read[n=2" in messages[0]
+    assert "feedback_interval[n=1" in messages[0]
+    assert all(not samples for samples in teleop._timing_samples.values())
+
+
+def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
+    class FailingAdapter:
+        def __init__(self) -> None:
+            self.stop_calls = 0
+
+        def send_joint_target(self, joints, follow):
+            del joints, follow
+            raise RuntimeError("send failed")
+
+        def stop(self):
+            self.stop_calls += 1
+
+    reset_calls = []
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._adapter = FailingAdapter()
+    teleop._follow = False
+    teleop._arm_name = "left_rm75"
+    teleop._stop_sent = False
+    teleop._last_joint_command_target = [0.0] * 7
+    teleop._last_joint_command_velocity = [0.0] * 7
+    teleop._joint_command_max_speed = math.radians(180.0)
+    teleop._joint_command_max_acceleration = math.radians(300.0)
+    teleop._dt = 1.0 / 125.0
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._safe_stop = lambda reset_active: reset_calls.append(reset_active)
+
+    sent = teleop._send_joint_target([0.1] * 7)
+
+    assert not sent
+    assert teleop._adapter.stop_calls == 1
+    assert reset_calls == [True]
@@ -1,14 +1,20 @@
 import math
+import time
 from types import SimpleNamespace

+import numpy as np
 import pytest

-from xr_rm_teleop.realman_adapter import ArmPose, MockRealManAdapter
+from xr_rm_teleop.realman_adapter import JointStateSnapshot
 from xr_rm_teleop.single_arm_velocity_teleop import (
    SingleArmVelocityTeleop,
-    _euler_to_quaternion,
+    _make_transform,
+    _matrix_to_quaternion,
    _normalize_quaternion,
-    _quaternion_to_euler,
+    _project_rotation,
+    _quaternion_to_matrix,
+    _so3_exp,
+    _so3_log,
 )


@@ -17,7 +23,10 @@ def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
    teleop._enable_orientation_control = True
    teleop._enable_orientation_axes = [True, True, True]
    teleop._controller_orientation_start = (0.0, 0.0, 0.0, 1.0)
-    teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.1, -0.2, 0.3)
+    teleop._robot_start_transform = _make_transform(
+        [0.3, 0.0, 0.2],
+        _so3_exp(np.asarray([0.1, -0.2, 0.3])),
+    )
    teleop._xr_to_robot_matrix = [
        0.0, 1.0, 0.0,
        0.0, 0.0, 1.0,
@@ -26,47 +35,111 @@ def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
    return teleop


-def assert_angles_close(actual: list[float] | tuple[float, ...], expected: list[float]) -> None:
-    assert len(actual) == len(expected)
-    for actual_value, expected_value in zip(actual, expected):
-        assert math.atan2(math.sin(actual_value - expected_value), math.cos(actual_value - expected_value)) == pytest.approx(0.0)
-
-
 def test_identity_controller_orientation_keeps_tcp_orientation() -> None:
    teleop = _make_teleop_for_orientation()

    target = teleop._raw_orientation_from_controller((0.0, 0.0, 0.0, 1.0))

-    assert_angles_close(target, teleop._robot_start_pose.rpy())
+    assert target == pytest.approx(teleop._robot_start_transform[:3, :3])


 def test_xr_relative_rotation_maps_through_xr_to_robot_matrix() -> None:
    teleop = _make_teleop_for_orientation()
-    teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.0, 0.0, 0.0)
-    xr_roll = _euler_to_quaternion(0.2, 0.0, 0.0)
+    teleop._robot_start_transform = np.eye(4)
+    xr_roll = _matrix_to_quaternion(_so3_exp(np.asarray([0.2, 0.0, 0.0])))

    target = teleop._raw_orientation_from_controller(xr_roll)

-    assert_angles_close(target, [0.0, 0.0, 0.2])
+    assert _so3_log(target) == pytest.approx([0.0, 0.0, 0.2])


-def test_orientation_deadband_filter_and_speed_limit() -> None:
+def test_quaternion_sign_does_not_change_rotation() -> None:
+    quaternion = _normalize_quaternion((0.2, -0.3, 0.1, 0.9))
+
+    assert _quaternion_to_matrix(quaternion) == pytest.approx(
+        _quaternion_to_matrix(tuple(-value for value in quaternion))
+    )
+
+
+@pytest.mark.parametrize("pitch", [math.pi / 2.0 - 1e-5, -math.pi / 2.0 + 1e-5])
+def test_small_rotation_near_gimbal_lock_stays_small(pitch: float) -> None:
+    teleop = _make_teleop_for_orientation()
+    start_rotation = _so3_exp(np.asarray([0.0, pitch, 0.0]))
+    teleop._robot_start_transform = _make_transform([0.3, 0.0, 0.2], start_rotation)
+    teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
+    controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.01, 0.0, 0.0])))
+
+    target = teleop._raw_orientation_from_controller(controller)
+
+    error = _so3_log(target @ start_rotation.T)
+    assert np.linalg.norm(error) == pytest.approx(0.01)
+
+
+def test_crossing_old_rpy_branch_uses_shortest_rotation() -> None:
+    teleop = _make_teleop_for_orientation()
+    start_rotation = _so3_exp(np.asarray([0.0, math.pi / 2.0 - 0.001, 0.0]))
+    teleop._robot_start_transform = _make_transform([0.3, 0.0, 0.2], start_rotation)
+    teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
+    controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.0, 0.002, 0.0])))
+
+    target = teleop._raw_orientation_from_controller(controller)
+
+    assert _so3_log(target @ start_rotation.T) == pytest.approx(
+        [0.0, 0.002, 0.0],
+        abs=1e-9,
+    )
+
+
+def test_disabled_orientation_axis_zeros_robot_rotation_vector_component() -> None:
+    teleop = _make_teleop_for_orientation()
+    teleop._robot_start_transform = np.eye(4)
+    teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
+    teleop._enable_orientation_axes = [True, False, True]
+    controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.1, 0.2, 0.3])))
+
+    target = teleop._raw_orientation_from_controller(controller)
+
+    assert _so3_log(target) == pytest.approx([0.1, 0.0, 0.3])
+
+
+def test_orientation_deadband_filter_and_speed_limit_use_so3_angle() -> None:
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._orientation_deadband_rad = 0.01
    teleop._orientation_filter_alpha = 0.5
    teleop._max_orientation_speed = 0.5
-    teleop._dt = 0.1
-    teleop._last_sent_orientation = [0.0, 0.0, 0.0]
-    teleop._filtered_orientation_target = [0.0, 0.0, 0.0]
+    teleop._dt = 1.0 / 125.0
+    teleop._last_sent_orientation = np.eye(3)
+    teleop._filtered_orientation_target = np.eye(3)

-    assert teleop._apply_orientation_deadband([0.001, 0.0, 0.0]) == [0.0, 0.0, 0.0]
+    inside_deadband = _so3_exp(np.asarray([0.006, 0.006, 0.0]))
+    assert teleop._apply_orientation_deadband(inside_deadband) == pytest.approx(np.eye(3))

-    filtered = teleop._filter_orientation_target([0.2, 0.0, 0.0])
-    assert_angles_close(filtered, [0.1, 0.0, 0.0])
+    target = _so3_exp(np.asarray([0.2, 0.0, 0.0]))
+    filtered = teleop._filter_orientation_target(target)
+    assert _so3_log(filtered) == pytest.approx([0.1, 0.0, 0.0])

-    limited, was_limited = teleop._limit_orientation_step([0.2, 0.0, 0.0])
+    limited, was_limited = teleop._limit_orientation_step(target)
    assert was_limited
-    assert_angles_close(limited, [0.05, 0.0, 0.0])
+    assert np.linalg.norm(_so3_log(limited)) == pytest.approx(0.5 / 125.0)
+
+
+def test_rotation_matrix_to_debug_quaternion_is_normalized() -> None:
+    quaternion = _matrix_to_quaternion(_so3_exp(np.asarray([0.2, -0.1, 0.3])))
+
+    assert np.isfinite(quaternion).all()
+    assert np.linalg.norm(quaternion) == pytest.approx(1.0)
+
+
+def test_rotation_projection_accepts_small_error_and_rejects_invalid_matrix() -> None:
+    near_rotation = np.eye(3)
+    near_rotation[0, 1] = 1e-5
+
+    projected = _project_rotation(near_rotation)
+
+    assert projected.T @ projected == pytest.approx(np.eye(3))
+    assert np.linalg.det(projected) == pytest.approx(1.0)
+    with pytest.raises(ValueError):
+        _project_rotation(np.diag([2.0, 1.0, 1.0]))


 def test_invalid_controller_quaternion_stops_current_tick() -> None:
@@ -95,6 +168,17 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
    teleop._arm_name = "test_rm75"
    teleop._command_timeout_sec = 0.12
    teleop._enable_orientation_control = True
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.1] * 7,
+            time.monotonic(),
+        )
+    )
+    teleop._ik_solver = SimpleNamespace(update_joint_state=lambda joints: np.eye(4))
+    teleop._active = False
+    teleop._last_valid_joint_target = None
+    teleop._last_current_pose = None
+    teleop._joint_feedback_ready = True
    stopped = []
    teleop.get_clock = lambda: FakeClock()
    teleop.get_logger = lambda: FakeLogger()
@@ -105,20 +189,6 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
    assert stopped == [True]


-def test_quaternion_roundtrip_for_small_rpy() -> None:
-    quat = _normalize_quaternion(_euler_to_quaternion(0.2, -0.1, 0.3))
-    assert_angles_close(_quaternion_to_euler(quat), [0.2, -0.1, 0.3])
-
-
 def test_zero_quaternion_is_invalid() -> None:
    with pytest.raises(ValueError):
        _normalize_quaternion([0.0, 0.0, 0.0, 0.0])
-
-
-def test_mock_adapter_uses_shortest_angular_velocity() -> None:
-    adapter = MockRealManAdapter([0.0, 0.0, 0.0, 3.13, 0.0, -3.13], 0.1)
-
-    adapter.send_cartesian_target(ArmPose(0.0, 0.0, 0.0, -3.13, 0.0, 3.13), False)
-
-    assert abs(adapter.last_velocity[3]) < 1.0
-    assert abs(adapter.last_velocity[5]) < 1.0
@@ -0,0 +1,84 @@
+from pathlib import Path
+from xml.etree import ElementTree
+
+import numpy as np
+import pytest
+
+from xr_rm_teleop.placo_ik_solver import (
+    PlacoIkSolver,
+    _validated_transform,
+)
+
+
+def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
+    urdf_path = (
+        Path(__file__).resolve().parents[1]
+        / "models"
+        / "rm75_omnipicker"
+        / "urdf"
+        / "RM75-B_OmniPicker_fixed.urdf"
+    )
+    root = ElementTree.parse(urdf_path).getroot()
+    moving_joint_names = [
+        joint.attrib["name"]
+        for joint in root.findall("joint")
+        if joint.attrib["type"] != "fixed"
+    ]
+    tcp_joint = root.find("joint[@name='omnipicker_tcp_joint']")
+    mesh_filenames = [
+        mesh.attrib["filename"]
+        for mesh in root.findall(".//mesh")
+    ]
+
+    assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
+    assert all(
+        filename.startswith(
+            "package://xr_rm_teleop/models/rm75_omnipicker/meshes/"
+        )
+        for filename in mesh_filenames
+    )
+    assert tcp_joint is not None
+    assert tcp_joint.attrib["type"] == "fixed"
+    assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
+    assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
+    assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
+    assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
+
+
+def test_validated_transform_accepts_finite_se3_and_returns_a_copy() -> None:
+    transform = np.eye(4)
+    transform[:3, 3] = [0.3, -0.1, 0.2]
+
+    actual = _validated_transform(transform)
+
+    assert actual == pytest.approx(transform)
+    assert actual is not transform
+
+
+@pytest.mark.parametrize(
+    "transform",
+    [
+        np.eye(3),
+        np.full((4, 4), np.nan),
+        np.vstack([np.eye(3, 4), [0.0, 0.0, 0.0, 2.0]]),
+        np.diag([2.0, 1.0, 1.0, 1.0]),
+    ],
+)
+def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None:
+    with pytest.raises(ValueError):
+        _validated_transform(transform)
+
+
+def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
+    solver._velocity_limits = np.ones(7)
+    solver._dt = 0.1
+    solver._actual_joints = np.zeros(7)
+
+    with pytest.raises(ValueError, match="finite"):
+        solver._validate_result(np.full(7, np.nan))
+    with pytest.raises(ValueError, match="position"):
+        solver._validate_result(np.full(7, 2.0))
+    with pytest.raises(ValueError, match="velocity"):
+        solver._validate_result(np.full(7, 0.2))
@@ -25,6 +25,14 @@ class PeripheralConfig:
    tools_in_ee: dict[str, list[list[float]]]
    set_initial_tool_state: bool = False

+    @property
+    def tool_name(self) -> str:
+        return list(self.tools_in_ee)[self.scissorgripper]
+
+    @property
+    def tool_pose(self) -> list[float]:
+        return list(self.tools_in_ee[self.tool_name][0])
+

 def load_peripheral_config(config_file: str, arm: str) -> PeripheralConfig:
    """从 bringup YAML 读取指定左右臂的外设配置。"""
@@ -94,6 +102,40 @@ def _tool_name_for_index(tools_in_ee: dict[str, list[list[float]]], scissorgripp
    return list(tools_in_ee.keys())[scissorgripper]


+def _check_sdk_return(result: Any, operation: str) -> None:
+    if result != 0:
+        raise RuntimeError(f"{operation} failed with code {result}: {result!r}")
+
+
+def _configure_tool_frame(robot, tool_frame, tool_name: str) -> None:
+    frames = robot.rm_get_total_tool_frame()
+    if not isinstance(frames, dict):
+        raise RuntimeError(
+            f"rm_get_total_tool_frame returned invalid data: {frames!r}"
+        )
+    _check_sdk_return(
+        frames.get("return_code"),
+        "rm_get_total_tool_frame",
+    )
+    tool_names = frames.get("tool_names")
+    if not isinstance(tool_names, (list, tuple)):
+        raise RuntimeError(
+            f"rm_get_total_tool_frame returned invalid tool_names: {tool_names!r}"
+        )
+
+    if tool_name in tool_names:
+        operation = "rm_update_tool_frame"
+        result = robot.rm_update_tool_frame(frame=tool_frame)
+    else:
+        operation = "rm_set_manual_tool_frame"
+        result = robot.rm_set_manual_tool_frame(frame=tool_frame)
+    _check_sdk_return(result, operation)
+    _check_sdk_return(
+        robot.rm_change_tool_frame(tool_name),
+        "rm_change_tool_frame",
+    )
+
+
 def cal_tool_frame(handle, scissorgripper, tools_in_ee):
    """根据末端工具配置生成 RealMan 工具坐标系。"""
    from Robotic_Arm.rm_robot_interface import rm_frame_t
@@ -152,8 +194,7 @@ def peripheral_cfg(

    time.sleep(0.2)
    tool_frame, tool_name = cal_tool_frame(robot, scissorgripper, tools_in_ee)
-    robot.rm_set_manual_tool_frame(frame=tool_frame)
-    robot.rm_change_tool_frame(tool_name)
+    _configure_tool_frame(robot, tool_frame, tool_name)

    if scissorgripper == 0:
        # 剪刀夹爪通过工具板数字输出控制。
@@ -0,0 +1,142 @@
+"""RM75 的 Placo 0.9.4 单步 QP 逆解。"""
+
+from __future__ import annotations
+
+from importlib.metadata import PackageNotFoundError, version
+from pathlib import Path
+
+import numpy as np
+
+EXPECTED_PLACO_VERSION = "0.9.4"
+RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
+RM75_Q_SLICE = slice(7, 14)
+
+
+def _validated_transform(transform: np.ndarray) -> np.ndarray:
+    values = np.asarray(transform, dtype=float)
+    if values.shape != (4, 4) or not np.isfinite(values).all():
+        raise ValueError("target transform must be a finite 4x4 matrix")
+    if not np.allclose(values[3], [0.0, 0.0, 0.0, 1.0], atol=1e-9):
+        raise ValueError("target transform must have a valid homogeneous row")
+
+    rotation = values[:3, :3]
+    if (
+        np.linalg.norm(rotation.T @ rotation - np.eye(3)) > 1e-3
+        or np.linalg.det(rotation) <= 0.0
+    ):
+        raise ValueError("target transform must contain a valid rotation")
+    u, _, vt = np.linalg.svd(rotation)
+    projected = u @ vt
+    if np.linalg.det(projected) <= 0.0:
+        raise ValueError("target transform must contain a proper rotation")
+
+    result = values.copy()
+    result[:3, :3] = projected
+    return result
+
+
+class PlacoIkSolver:
+    def __init__(
+        self,
+        urdf_path: str,
+        dt: float,
+    ) -> None:
+        if dt <= 0.0:
+            raise ValueError("dt must be positive")
+        try:
+            installed_version = version("placo")
+            import placo
+        except (ImportError, PackageNotFoundError) as exc:
+            raise RuntimeError(
+                "Placo 0.9.4 must come from "
+                "/home/robot/miniconda3/envs/xr"
+            ) from exc
+        if installed_version != EXPECTED_PLACO_VERSION:
+            raise RuntimeError(
+                f"Placo {EXPECTED_PLACO_VERSION} is required, got {installed_version}"
+            )
+
+        model_path = Path(urdf_path).expanduser().resolve()
+        if not model_path.is_file():
+            raise FileNotFoundError(f"RM75 URDF not found: {model_path}")
+
+        self._dt = dt
+        self._robot = placo.RobotWrapper(str(model_path))
+        if self._robot.state.q.shape != (14,):
+            raise RuntimeError(
+                f"expected Placo q shape (14,), got {self._robot.state.q.shape}"
+            )
+        if list(self._robot.joint_names()) != RM75_JOINT_NAMES:
+            raise RuntimeError(
+                f"unexpected RM75 joint order: {list(self._robot.joint_names())}"
+            )
+        offsets = [
+            self._robot.get_joint_offset(name) for name in RM75_JOINT_NAMES
+        ]
+        if offsets != list(range(7, 14)):
+            raise RuntimeError(f"unexpected RM75 q offsets: {offsets}")
+
+        self._joint_limits = np.asarray(
+            [self._robot.get_joint_limits(name) for name in RM75_JOINT_NAMES]
+        )
+        velocity_offsets = [
+            self._robot.get_joint_v_offset(name) for name in RM75_JOINT_NAMES
+        ]
+        self._velocity_limits = np.asarray(
+            [
+                self._robot.model.velocityLimit[index]
+                for index in velocity_offsets
+            ]
+        )
+        self._actual_joints: np.ndarray | None = None
+
+        self._solver = placo.KinematicsSolver(self._robot)
+        self._solver.dt = dt
+        self._solver.mask_fbase(True)
+        self._solver.enable_velocity_limits(True)
+        self._frame_task = self._solver.add_frame_task(
+            "omnipicker_tcp",
+            np.eye(4),
+        )
+        self._frame_task.configure("rm75_frame", "soft", 1.0)
+        self._solver.add_kinetic_energy_regularization_task(1e-6)
+
+    @property
+    def base_configuration(self) -> list[float]:
+        return self._robot.state.q[:7].tolist()
+
+    def update_joint_state(self, joints: list[float]) -> np.ndarray:
+        values = np.asarray(joints, dtype=float)
+        if values.shape != (7,) or not np.isfinite(values).all():
+            raise ValueError("joint state must contain 7 finite values")
+        is_first_feedback = self._actual_joints is None
+        self._actual_joints = values.copy()
+        self._robot.state.q[RM75_Q_SLICE] = values
+        self._robot.update_kinematics()
+        base_to_tool = self._robot.get_T_world_frame("omnipicker_tcp")
+        if is_first_feedback:
+            self._frame_task.T_world_frame = base_to_tool.copy()
+        return base_to_tool.copy()
+
+    def solve(self, target_tool_pose: np.ndarray) -> list[float]:
+        if self._actual_joints is None:
+            raise RuntimeError("joint state must be initialized before QP solve")
+        self._frame_task.T_world_frame = _validated_transform(target_tool_pose)
+        self._solver.solve(True)
+        result = np.asarray(
+            self._robot.state.q[RM75_Q_SLICE],
+            dtype=float,
+        ).copy()
+        self._validate_result(result)
+        return result.tolist()
+
+    def _validate_result(self, result: np.ndarray) -> None:
+        if result.shape != (7,) or not np.isfinite(result).all():
+            raise ValueError("QP result must contain 7 finite values")
+        lower = self._joint_limits[:, 0]
+        upper = self._joint_limits[:, 1]
+        if np.any(result < lower - 1e-9) or np.any(result > upper + 1e-9):
+            raise ValueError("QP result violates RM75 joint position limits")
+        max_step = self._velocity_limits * self._dt + 1e-9
+        if np.any(np.abs(result - self._actual_joints) > max_step):
+            raise ValueError("QP result violates RM75 one-cycle velocity limits")
@@ -1,21 +1,16 @@
-"""RM75 机械臂适配层。
-
-对上提供统一的当前位姿读取、笛卡尔位姿目标发送和停止接口；对下根据配置
-选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
-"""
+"""RM75 机械臂关节反馈、关节透传和停止适配层。"""

 from __future__ import annotations

+import ipaddress
 import math
+import threading
+import time
 from dataclasses import dataclass
 from numbers import Number
 from typing import Any


-def _angle_delta(target: float, current: float) -> float:
-    return math.atan2(math.sin(target - current), math.cos(target - current))
-
-
@dataclass
 class ArmPose:
    x: float
@@ -32,55 +27,69 @@ class ArmPose:
        return [self.rx, self.ry, self.rz]


-class MockRealManAdapter:
-    """无机械臂时使用的运动学模拟器，用于验证 ROS2 遥操链路。"""
+@dataclass(frozen=True)
+class JointStateSnapshot:
+    positions: list[float]
+    received_at: float
+    read_duration_ms: float | None = None
+    update_interval_ms: float | None = None
+    motion_ready: bool = True

-    def __init__(self, initial_pose: list[float], dt: float) -> None:
-        self._pose = ArmPose(*initial_pose[:6])
-        self._dt = dt
-        self.last_velocity = [0.0] * 6
+
+class MockRealManAdapter:
+    """不导入厂商 SDK 的关节状态 mock。"""
+
+    def __init__(self, initial_joint_degrees: list[float]) -> None:
+        if len(initial_joint_degrees) != 7 or not all(
+            math.isfinite(value) for value in initial_joint_degrees
+        ):
+            raise ValueError("initial joint pose must contain 7 finite values")
+        self._joint_positions = [
+            math.radians(value) for value in initial_joint_degrees
+        ]
+        self.last_joint_target: list[float] | None = None
        self.last_tool_open: bool | None = None

    def connect(self) -> None:
        return

-    def get_current_pose(self) -> ArmPose:
-        return self._pose
+    def get_latest_joint_state(self) -> JointStateSnapshot:
+        return JointStateSnapshot(
+            list(self._joint_positions),
+            time.monotonic(),
+        )

-    def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
+    def send_joint_target(self, joints: list[float], follow: bool) -> None:
        del follow
-        self.last_velocity = [
-            (pose.x - self._pose.x) / self._dt,
-            (pose.y - self._pose.y) / self._dt,
-            (pose.z - self._pose.z) / self._dt,
-            _angle_delta(pose.rx, self._pose.rx) / self._dt,
-            _angle_delta(pose.ry, self._pose.ry) / self._dt,
-            _angle_delta(pose.rz, self._pose.rz) / self._dt,
-        ]
-        self._pose = pose
+        if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
+            raise ValueError("joint target must contain 7 finite values")
+        self._joint_positions = list(joints)
+        self.last_joint_target = list(joints)

    def stop(self) -> None:
-        self.last_velocity = [0.0] * 6
+        return

    def close(self) -> None:
        self.stop()

-    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
-        del config_file, peripheral_arm
+    def configure_peripheral(self, config: Any, peripheral_arm: str) -> None:
+        del config, peripheral_arm

    def set_tool_enabled(self, open_tool: bool) -> None:
        self.last_tool_open = open_tool


 class RealManAdapter:
-    """睿尔曼 Python API2 的笛卡尔位姿透传适配层。"""
+    """复用一个睿尔曼 Python API2 连接的关节适配层。"""

    def __init__(
        self,
        robot_ip: str,
        robot_port: int,
        avoid_singularity: int,
-        frame_type: int,
+        realtime_push_host_ip: str,
+        realtime_push_port: int,
+        realtime_push_cycle_ms: int = 5,
        logger: Any | None = None,
        configure_safety_limits: bool = True,
        max_line_speed: float = 1.0,
@@ -98,7 +107,22 @@ class RealManAdapter:
        self._robot_ip = robot_ip
        self._robot_port = robot_port
        self._avoid_singularity = avoid_singularity
-        self._frame_type = frame_type
+        try:
+            self._realtime_push_host_ip = str(
+                ipaddress.IPv4Address(realtime_push_host_ip)
+            )
+        except ipaddress.AddressValueError as exc:
+            raise ValueError(
+                "realtime_push_host_ip must be a valid IPv4 address"
+            ) from exc
+        if not 1 <= realtime_push_port <= 65535:
+            raise ValueError("realtime_push_port must be between 1 and 65535")
+        if realtime_push_cycle_ms <= 0 or realtime_push_cycle_ms % 5 != 0:
+            raise ValueError(
+                "realtime_push_cycle_ms must be a positive multiple of 5"
+            )
+        self._realtime_push_port = realtime_push_port
+        self._realtime_push_cycle_ms = realtime_push_cycle_ms
        self._logger = logger
        self._configure_safety_limits = configure_safety_limits
        self._max_line_speed = max_line_speed
@@ -114,54 +138,115 @@ class RealManAdapter:
        self._canfd_radio = canfd_radio
        self._scissorgripper: int | None = None
        self._arm: Any | None = None
+        self._joint_state_lock = threading.Lock()
+        self._latest_joint_state: JointStateSnapshot | None = None
+        self._feedback_ready = threading.Event()
+        self._realtime_callback: Any | None = None
+        self._accept_realtime_feedback = False
+        self._feedback_fault_logged = False
+        self._last_motion_status: tuple[Any, ...] | None = None

    def connect(self) -> None:
        try:
-            from Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e
+            from Robotic_Arm.rm_robot_interface import (
+                RoboticArm,
+                rm_realtime_arm_state_callback_ptr,
+                rm_realtime_push_config_t,
+                rm_thread_mode_e,
+            )
        except ImportError as exc:
            raise RuntimeError(
                "未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。"
            ) from exc

        self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
-        handle = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
-        self._check_robot_handle(handle)
-        self._log_info(
-            "RealMan connected: "
-            f"ip={self._robot_ip}, port={self._robot_port}, "
-            f"avoid_singularity={self._avoid_singularity}, "
-            f"frame_type={self._frame_type}, command=rm_movep_canfd"
-        )
-        if self._configure_safety_limits:
-            self._apply_safety_limits()
-        if self._move_to_initial_pose_on_connect:
-            self._move_to_initial_pose()
+        try:
+            handle = self._arm.rm_create_robot_arm(
+                self._robot_ip,
+                self._robot_port,
+            )
+            self._check_robot_handle(handle)
+            self._log_info(
+                "RealMan connected: "
+                f"ip={self._robot_ip}, port={self._robot_port}, "
+                f"avoid_singularity={self._avoid_singularity}, "
+                "command=rm_movej_canfd"
+            )
+            if self._configure_safety_limits:
+                self._apply_safety_limits()
+            if self._move_to_initial_pose_on_connect:
+                self._move_to_initial_pose()
+            self._feedback_ready.clear()
+            self._accept_realtime_feedback = True
+            self._realtime_callback = rm_realtime_arm_state_callback_ptr(
+                self._on_realtime_arm_state
+            )
+            self._arm.rm_realtime_arm_state_call_back(
+                self._realtime_callback
+            )
+            config = rm_realtime_push_config_t(
+                self._realtime_push_cycle_ms,
+                True,
+                self._realtime_push_port,
+                0,
+                self._realtime_push_host_ip,
+            )
+            self._check_return(
+                self._arm.rm_set_realtime_push(config),
+                "rm_set_realtime_push",
+            )
+            if not self._feedback_ready.wait(timeout=2.0):
+                raise RuntimeError(
+                    "RealMan UDP realtime feedback did not receive a valid "
+                    "frame within 2 seconds"
+                )
+            self._log_info(
+                "RealMan UDP realtime feedback ready: "
+                f"host={self._realtime_push_host_ip}:"
+                f"{self._realtime_push_port}, "
+                f"cycle={self._realtime_push_cycle_ms} ms"
+            )
+        except Exception:
+            self._accept_realtime_feedback = False
+            try:
+                self._arm.rm_delete_robot_arm()
+            except Exception:
+                pass
+            self._arm = None
+            self._realtime_callback = None
+            raise

-    def get_current_pose(self) -> ArmPose:
-        self._require_arm()
-        state = self._arm.rm_get_current_arm_state()
-        pose = self._find_pose(state)
-        if pose is None:
-            raise RuntimeError(f"无法从睿尔曼状态中解析当前 TCP 位姿：{state!r}")
-        return ArmPose(*pose[:6])
+    def get_latest_joint_state(self) -> JointStateSnapshot | None:
+        with self._joint_state_lock:
+            if self._latest_joint_state is None:
+                return None
+            return JointStateSnapshot(
+                list(self._latest_joint_state.positions),
+                self._latest_joint_state.received_at,
+                self._latest_joint_state.read_duration_ms,
+                self._latest_joint_state.update_interval_ms,
+                self._latest_joint_state.motion_ready,
+            )

-    def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
+    def send_joint_target(self, joints: list[float], follow: bool) -> None:
        self._require_arm()
-        ret = self._arm.rm_movep_canfd(
-            [pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz],
+        if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
+            raise ValueError("joint target must contain 7 finite values")
+        ret = self._arm.rm_movej_canfd(
+            [math.degrees(value) for value in joints],
            follow,
+            0,
            self._canfd_trajectory_mode,
            self._canfd_radio,
        )
-        self._check_return(ret, "rm_movep_canfd")
+        self._check_return(ret, "rm_movej_canfd")

-    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
+    def configure_peripheral(self, config: Any, peripheral_arm: str) -> None:
        self._require_arm()
-        from .fun_peripheral import load_peripheral_config, peripheral_cfg
+        from .fun_peripheral import peripheral_cfg

-        config = load_peripheral_config(config_file, peripheral_arm)
        self._scissorgripper = config.scissorgripper
-        tool_name = list(config.tools_in_ee.keys())[config.scissorgripper]
+        tool_name = config.tool_name
        self._log_info(
            "开始配置 RealMan 末端外设："
            f"arm={peripheral_arm}, scissorgripper={config.scissorgripper}, "
@@ -195,16 +280,119 @@ class RealManAdapter:
    def close(self) -> None:
        if self._arm is None:
            return
+        self._accept_realtime_feedback = False
        self.stop()
        try:
            self._arm.rm_delete_robot_arm()
        finally:
            self._arm = None
+            self._realtime_callback = None

    def _require_arm(self) -> None:
        if self._arm is None:
            raise RuntimeError("睿尔曼机械臂尚未连接")

+    def _on_realtime_arm_state(self, data: Any) -> None:
+        if not self._accept_realtime_feedback:
+            return
+        try:
+            if data is None or int(data.errCode) != 0:
+                raise ValueError("invalid realtime feedback error code")
+            arm_ip = data.arm_ip
+            if isinstance(arm_ip, bytes):
+                arm_ip = arm_ip.decode("utf-8").split("\x00", 1)[0]
+            if str(arm_ip) != self._robot_ip:
+                raise ValueError(
+                    f"unexpected realtime feedback source: {arm_ip}"
+                )
+            degrees = list(data.joint_status.joint_position)
+            if (
+                len(degrees) != 7
+                or not all(isinstance(value, Number) for value in degrees)
+            ):
+                raise ValueError(
+                    "RM75 UDP feedback must contain 7 numeric joints"
+                )
+            positions = [math.radians(float(value)) for value in degrees]
+            if not all(math.isfinite(value) for value in positions):
+                raise ValueError("RM75 UDP feedback contains NaN/Inf")
+            joint_enabled = [
+                bool(value) for value in data.joint_status.joint_en_flag
+            ]
+            joint_errors = [
+                int(value) for value in data.joint_status.joint_err_code
+            ]
+            if len(joint_enabled) != 7 or len(joint_errors) != 7:
+                raise ValueError(
+                    "RM75 UDP feedback must contain 7 joint states"
+                )
+            arm_error_count = int(data.err.err_len)
+            arm_errors = [
+                int(value) for value in list(data.err.err)[:arm_error_count]
+            ]
+            arm_errors = [code for code in arm_errors if code != 0]
+            arm_current_status = int(data.arm_current_status)
+            motion_ready = (
+                0 <= arm_current_status <= 8
+                and all(joint_enabled)
+                and not any(joint_errors)
+                and not arm_errors
+            )
+            motion_status = (
+                arm_current_status,
+                tuple(joint_enabled),
+                tuple(joint_errors),
+                tuple(arm_errors),
+                motion_ready,
+            )
+            received_at = time.monotonic()
+            with self._joint_state_lock:
+                update_interval_ms = (
+                    None
+                    if self._latest_joint_state is None
+                    else (
+                        received_at
+                        - self._latest_joint_state.received_at
+                    )
+                    * 1000.0
+                )
+                self._latest_joint_state = JointStateSnapshot(
+                    positions,
+                    received_at,
+                    None,
+                    update_interval_ms,
+                    motion_ready,
+                )
+            self._log_motion_status_transition(motion_status)
+            self._feedback_fault_logged = False
+            self._feedback_ready.set()
+        except Exception as exc:
+            if not self._feedback_fault_logged:
+                self._log_warn(
+                    f"RealMan UDP realtime feedback invalid: {exc}"
+                )
+                self._feedback_fault_logged = True
+
+    def _log_motion_status_transition(
+        self,
+        status: tuple[Any, ...],
+    ) -> None:
+        previous = self._last_motion_status
+        if status == previous:
+            return
+        self._last_motion_status = status
+        arm_status, joint_enabled, joint_errors, arm_errors, ready = status
+        details = (
+            f"arm_status={arm_status}, "
+            f"joint_enabled={list(joint_enabled)}, "
+            f"joint_errors={list(joint_errors)}, "
+            f"arm_errors={list(arm_errors)}"
+        )
+        if not ready:
+            self._log_warn(f"RealMan UDP 报警或掉使能：{details}")
+        elif previous is not None and not previous[-1]:
+            self._log_info(f"RealMan UDP 运动状态恢复正常：{details}")
+
    def _apply_safety_limits(self) -> None:
        # 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。
        self._try_call("rm_set_avoid_singularity_mode", int(self._avoid_singularity))
@@ -260,74 +448,3 @@ class RealManAdapter:
    @staticmethod
    def _return_code(ret: Any) -> Any:
        return ret[0] if isinstance(ret, tuple) and ret else ret
-
-    @classmethod
-    def _find_pose(cls, obj: Any) -> list[float] | None:
-        # 不同 SDK 版本返回字段可能略有差异，因此递归查找常见 TCP 位姿字段。
-        if isinstance(obj, dict):
-            for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
-                pose = cls._as_pose(obj.get(key))
-                if pose is not None:
-                    return pose
-            for value in obj.values():
-                pose = cls._find_pose(value)
-                if pose is not None:
-                    return pose
-        elif isinstance(obj, (list, tuple)):
-            pose = cls._as_pose(obj)
-            if pose is not None:
-                return pose
-            for value in obj:
-                pose = cls._find_pose(value)
-                if pose is not None:
-                    return pose
-        elif hasattr(obj, "to_dictionary"):
-            try:
-                return cls._find_pose(obj.to_dictionary(7))
-            except TypeError:
-                return cls._find_pose(obj.to_dictionary())
-        elif hasattr(obj, "to_dict"):
-            return cls._find_pose(obj.to_dict())
-        else:
-            for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
-                if hasattr(obj, key):
-                    pose = cls._as_pose(getattr(obj, key))
-                    if pose is not None:
-                        return pose
-        return None
-
-    @staticmethod
-    def _as_pose(value: Any) -> list[float] | None:
-        if isinstance(value, (list, tuple)) and len(value) >= 6:
-            if all(isinstance(item, Number) for item in value[:6]):
-                return [float(item) for item in value[:6]]
-        if isinstance(value, dict):
-            position = value.get("position")
-            euler = value.get("euler")
-            if isinstance(position, dict) and isinstance(euler, dict):
-                keys = ("x", "y", "z")
-                rpy_keys = ("rx", "ry", "rz")
-                if all(key in position for key in keys) and all(key in euler for key in rpy_keys):
-                    return [
-                        float(position["x"]),
-                        float(position["y"]),
-                        float(position["z"]),
-                        float(euler["rx"]),
-                        float(euler["ry"]),
-                        float(euler["rz"]),
-                    ]
-        if all(hasattr(value, attr) for attr in ("position", "euler")):
-            position = getattr(value, "position")
-            euler = getattr(value, "euler")
-            if all(hasattr(position, key) for key in ("x", "y", "z")) and all(
-                hasattr(euler, key) for key in ("rx", "ry", "rz")
-            ):
-                return [
-                    float(position.x),
-                    float(position.y),
-                    float(position.z),
-                    float(euler.rx),
-                    float(euler.ry),
-                    float(euler.rz),
-                ]
-        return None
Author	SHA1	Message	Date
YikaiFu-cart	f795c06d44	Remove outdated design documents for RM75 control and feedback systems	2026-07-29 16:24:07 +08:00
YikaiFu-cart	08996434e5	feat: Implement UDP feedback for RM75 robot arms	2026-07-29 15:26:59 +08:00
YikaiFu-cart	687a0b401a	feat: Implement absolute scheduling for feedback loop and enhance tool frame configuration	2026-07-29 09:44:51 +08:00
YikaiFu-cart	2a12eea4d5	Add URDF model for RM75-B OmniPicker with detailed link and joint specifications	2026-07-28 17:06:06 +08:00
YikaiFu-cart	fae5a560fb	Add Placo IK solver and associated tests.	2026-07-28 10:47:49 +08:00
YikaiFu-cart	bfd50e1035	feat: add RM75 Placo single-step QP inverse kinematics design document	2026-07-27 19:30:30 +08:00