Refactor and enhance XR-RM teleoperation functionality

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -1,34 +1,28 @@
 # 阶段一：PICO 遥操作双 RM75 平台配置。
 #
-# 参数来源：acDual-arm-YikaiFuTest/realman_pkg/core/arg_cfg.py
-# - 左臂 IP：192.168.192.18
-# - 右臂 IP：192.168.192.19
-# - 左右臂工作空间、圆柱半径约束、初始化点位和 IP 沿用最新 acRealman 配置。
-# - PICO 手柄位移映射根据论文 OpenXR 坐标和现场双臂 base 坐标图单独设置。
-#
-# 注意：当前项目仍然采用“手柄相对位移”控制方式。
-# 按下握持键时锁定当前手柄位姿和 TCP 位姿，之后只跟随相对位移。
-# acDual-arm 项目使用的是戴盟绝对 PoseStamped 重映射，因此这里只迁移
-# 坐标标定和安全参数，不迁移其绝对位姿控制链路。
+# 当前控制方式是“相对位姿透传”：
+# 按下 grip 时锁定当前手柄位姿和 TCP 位姿，之后将手柄相对位移映射为目标
+# TCP 位姿，经过工作空间限幅、目标低通和单帧步长限制后，通过 rm_movep_canfd
+# 下发。cmd_vel 仅作为目标位姿变化率调试话题，不是机械臂执行命令。
 # 末端外设由 peripherals_rm75.yaml 配置，launch 只在真机连接阶段初始化。
 
 left_arm_teleop:
   ros__parameters:
     arm_name: left_rm75
     controller_topic: /xr/left_controller
-    control_rate_hz: 50.0
+    control_rate_hz: 90.0
     command_timeout_sec: 0.12
 
-    # 实验台番茄测试先采用保守参数；完成三轴方向检查和实体急停测试后再提高速度。
+    # 位姿目标生成与平滑参数。
     scale: 0.75
-    kp_linear: 1.8
-    deadband_m: 0.002
-    low_pass_alpha: 0.35
+    deadband_m: 0.001
+    target_filter_alpha: 0.65
+    target_filter_alpha_fast: 0.9
+    target_filter_fast_threshold_m: 0.03
     max_linear_speed: 0.2
     enable_position_axes: [true, true, true]
+    current_pose_poll_hz: 10.0
 
-    # 来自 acDual-arm 的 bounds_p[left]：
-    # x[-0.50, 0.50]，y[-0.60, -0.20]，z[0.10, 0.50]。
     workspace_min: [-0.70, -0.60, 0.10]
     workspace_max: [0.70, 0.40, 0.70]
     cyl_radius_limit: [0.20, 0.60]
@@ -36,8 +30,6 @@ left_arm_teleop:
     low_z_min_radius: 0.21
 
     # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
-    # 左臂 base 坐标：+X 向下，+Z 向右，前方工作区对应 -Y。
-    # 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反。
     # 映射关系：机器人位移增量 = [-手柄y, 手柄z, -手柄x]。
     xr_to_robot_matrix: [0.0, -1.0, 0.0,
                          0.0, 0.0, 1.0,
@@ -50,6 +42,8 @@ left_arm_teleop:
     avoid_singularity: 0
     frame_type: 1
     follow: false
+    canfd_trajectory_mode: 2
+    canfd_radio: 0
     configure_safety_limits: true
     max_line_speed: 1.0
     max_angular_speed: 1.5
@@ -57,32 +51,28 @@ left_arm_teleop:
     max_angular_acc: 2.0
     joint_max_speed: 180.0
     joint_max_acc: 180.0
-    # 真机 launch 默认不自动移动；需要沿用 acRealman 上电初始化时再显式打开。
     move_to_initial_pose_on_connect: false
     initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
     initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
     init_move_speed: 20
-    command_mode: pose_canfd
-    canfd_trajectory_mode: 2
-    canfd_radio: 0
     debug_topic_prefix: /xr_rm
 
 right_arm_teleop:
   ros__parameters:
     arm_name: right_rm75
     controller_topic: /xr/right_controller
-    control_rate_hz: 50.0
+    control_rate_hz: 90.0
     command_timeout_sec: 0.12
 
     scale: 0.75
-    kp_linear: 1.8
-    deadband_m: 0.002
-    low_pass_alpha: 0.35
+    deadband_m: 0.001
+    target_filter_alpha: 0.65
+    target_filter_alpha_fast: 0.9
+    target_filter_fast_threshold_m: 0.03
     max_linear_speed: 0.2
     enable_position_axes: [true, true, true]
+    current_pose_poll_hz: 10.0
 
-    # 来自 acDual-arm 的 bounds_p[right]：
-    # x[-0.70, 0.50]，y[-0.60, 0.40]，z[0.10, 0.70]。
     workspace_min: [-0.70, -0.60, 0.10]
     workspace_max: [0.70, 0.40, 0.70]
     cyl_radius_limit: [0.20, 0.60]
@@ -90,8 +80,6 @@ right_arm_teleop:
     low_z_min_radius: 0.21
 
     # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
-    # 右臂 base 坐标：+X 向上，+Z 向左，前方工作区对应 -Y。
-    # 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反。
     # 映射关系：机器人位移增量 = [手柄y, 手柄z, 手柄x]。
     xr_to_robot_matrix: [0.0, 1.0, 0.0,
                          0.0, 0.0, 1.0,
@@ -104,6 +92,8 @@ right_arm_teleop:
     avoid_singularity: 1
     frame_type: 1
     follow: false
+    canfd_trajectory_mode: 2
+    canfd_radio: 0
     configure_safety_limits: true
     max_line_speed: 1.0
     max_angular_speed: 1.5