Enhance orientation control for RM75 arms in XR teleoperation

README.md

@@ -11,7 +11,7 @@ PICO/XR 双手柄 UDP JSON
   -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
 ```
 
-当前控制方式是“手柄相对位姿透传”遥操作：按住 `grip` 时锁定当前手柄位置和机械臂 TCP 位置，之后根据手柄相对位移生成目标 TCP，经过工作空间限幅、目标低通和单帧步长限制后，通过 `rm_movep_canfd` 下发目标位姿。松开 `grip`、UDP 超时或节点退出时会请求机械臂慢停。
+当前控制方式是“手柄相对位姿透传”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP，经过工作空间限幅、目标低通、姿态低通和单帧步长限制后，通过 `rm_movep_canfd` 下发目标位姿。松开 `grip`、UDP 超时或节点退出时会请求机械臂慢停。
 
 ## 当前范围
 
@@ -160,7 +160,7 @@ 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
 ```
 
-`sample_udp_sender` 默认使用 `axis_sweep` 成对扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。`--hand both --both-mode staggered --seconds 60` 会先左后右，适合肉眼确认左右臂方向；如果只想左右同时动，可用 `--both-mode synchronized`。
+`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`。
 
 观察：
 
@@ -241,6 +241,9 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `target_filter_alpha` / `target_filter_alpha_fast`：目标 TCP 低通滤波系数，快速移动时自动使用更大的系数。
 - `target_filter_fast_threshold_m`：进入快速滤波区间的目标变化阈值。
 - `max_linear_speed`：目标位姿单帧步长限制对应的最大线速度。
+- `enable_orientation_control`：是否把手柄相对旋转映射到 TCP 姿态。
+- `orientation_filter_alpha` / `orientation_deadband_rad`：目标 TCP 姿态低通和死区。
+- `max_orientation_speed`：目标姿态单帧步长限制对应的最大角速度。
 - `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
 - `cyl_radius_limit`：基座圆柱半径限制。
 - `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
@@ -332,10 +335,10 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
 - `t` / `source_time`：Unity 端 `Time.realtimeSinceStartupAsDouble`，用于后续延迟分析。
 - `seq`：Unity 端递增包序号，用于后续丢包分析。
 - `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。
-- `grip`：运动使能。`true` 时进入相对位移控制，`false` 时停止。
+- `grip`：运动使能。`true` 时进入相对位姿控制，`false` 时停止。
 - `trigger`：扳机值，范围 `0.0-1.0`。真机模式下跨过 `0.95` 的上升沿会切换对应夹爪开/关状态。
 - `pos`：手柄位置，长度 3。
-- `quat`：手柄姿态四元数，默认按 `xyzw` 解析。
+- `quat`：手柄姿态四元数，默认按 `xyzw` 解析；遥操作节点会用 grip 锁定后的相对旋转控制 TCP 姿态。
 - `pose_valid`：姿态是否可信。ROS 接收端看到 `false` 会强制 `grip=false`。
 - `pose_source`：`pxr_predict`、`unity_xr` 或 `none`，用于判断姿态来自 PICO 预测接口还是 Unity XR fallback。
 - `tracking_state` / `controller_status`：Unity/PICO 侧追踪诊断值，只用于日志和排查。
@@ -354,9 +357,10 @@ PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并
 3. 单臂启动，`move_to_initial_pose_on_connect:=false`。
 4. 手握急停，按住 `grip` 后只做小幅单轴移动。
 5. 逐个确认上/下、前/后、左/右方向。
-6. 点击对应 `trigger`，确认每次点击都会切换对应夹爪状态，松开 trigger 后状态保持且左右不串臂。
-7. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。
-8. 左右臂都确认后，再进入双臂模式。
+6. 小角度转动手柄，确认 `/xr_rm/<arm>/target_pose` 姿态和 `/xr_rm/<arm>/cmd_vel.twist.angular` 变化符合预期。
+7. 点击对应 `trigger`，确认每次点击都会切换对应夹爪状态，松开 trigger 后状态保持且左右不串臂。
+8. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。
+9. 左右臂都确认后，再进入双臂模式。
 
 当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。