Add URDF model for RM75-B OmniPicker with detailed link and joint specifications

README.md

@@ -12,7 +12,7 @@ PICO/XR 双手柄 UDP JSON
   -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
 ```
 
-当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP，经过工作空间限幅、目标低通、姿态低通和单帧步长限制后，每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip`、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。
+当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP。姿态目标使用旋转矩阵和 SO(3) 最短路径完成死区、滤波与限速，不经过 RPY。每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip`、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。
 
 ## 当前范围
 
@@ -69,7 +69,9 @@ src/
 │   └── msg/
 │       └── XrController.msg        # hand/grip/trigger/pose
 └── xr_rm_teleop/
-    ├── models/rm75/                # RM75 URDF 与网格
+    ├── 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
@@ -239,7 +241,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `robot_port`：RM75 TCP 端口，默认 `8080`。
 - `left_avoid_singularity` / `right_avoid_singularity`：左右臂避奇异参数，默认左 `0`、右 `1`。
 - `avoid_singularity`：非空时覆盖左右臂避奇异参数。
-- `control_rate_hz`：同步关节反馈、执行一次 QP 并发送一次关节目标的频率，默认 `90.0`。
+- `control_rate_hz`：同步关节反馈、执行一次 QP 并发送一次关节目标的频率，默认 `125.0`。
 - `follow`：传给 `rm_movej_canfd` 的跟随标志，默认 `false`。
 - `configure_safety_limits`：连接真机后是否配置速度/加速度安全参数，默认 `true`。
 - `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。
@@ -257,7 +259,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` 保存末端工具坐标、负载和左右臂外设选择。当前左臂使用 `minisci`（沿工具局部 Z 偏移 `0.19 m`），右臂使用 `omnipic`（沿工具局部 Z 偏移 `0.16 m`）。URDF 仍只建模到法兰 `link_7`；QP 在 TCP 目标与法兰目标之间应用完整工具刚体变换。真机连接阶段会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 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 连接。
 
 重点控制参数：
 
@@ -267,8 +269,8 @@ 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 坐标的映射矩阵。