Refactor and enhance XR-RM teleoperation functionality

2026-05-31 20:06:07 +08:00
parent 3f48468f63
commit 948d50cab4
13 changed files with 409 additions and 354 deletions
--- a/xr_rm_bringup/config/dual_arm_rm75.yaml
+++ b/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
--- a/xr_rm_bringup/config/left_arm_rm75.yaml
+++ b/xr_rm_bringup/config/left_arm_rm75.yaml
@ -1,90 +1,52 @@
-# 左臂单独调试配置：XR 相对位移控制 RM75 TCP。
+# 左臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。
 # 末端外设由 peripherals_rm75.yaml 配置，launch 只在真机连接阶段初始化。

-# single_arm_velocity_teleop 是单臂遥操作节点名；单臂 launch 会按这个节点名读取参数。
 single_arm_velocity_teleop:
-  # ros__parameters 是 ROS2 YAML 参数入口，下面的字段会加载为节点参数。
  ros__parameters:
-    # 机械臂逻辑名称，用于日志、调试话题命名和区分左右臂。
    arm_name: left_rm75
-    # 订阅的 XR 左手柄话题，由 udp_controller_receiver 发布。
    controller_topic: /xr/left_controller
-    # 控制循环频率，单位 Hz；越高响应越细，但对通信和 CPU 要求更高。
-    control_rate_hz: 50.0
-    # 手柄消息超时时间，单位秒；超过该时间没有新数据就停止输出速度。
+    control_rate_hz: 90.0
    command_timeout_sec: 0.12

-    # XR 手柄相对位移到机器人目标位移的比例系数。
+    # 手柄相对位移 -> 目标 TCP 位姿；随后做目标低通和单帧步长限制。
    scale: 1.0
-    # 目标 TCP 位置误差转换为速度命令的线性比例增益。
-    kp_linear: 1.8
-    # 位置误差死区，单位米；小于该值时认为已到位，不再输出速度。
-    deadband_m: 0.002
-    # 速度一阶低通滤波系数，范围 0~1；越大响应越快，越小越平滑。
-    low_pass_alpha: 0.35
-    # 软件侧最大 TCP 线速度，单位 m/s；限制遥操作移动速度。
+    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.3
-    # 是否启用机器人 X/Y/Z 三个位置轴；false 的轴不会输出位移控制。
    enable_position_axes: [true, true, true]
+    current_pose_poll_hz: 10.0

-    # TCP 工作空间下限 [x, y, z]，单位米；目标点会被限制在该范围内。
    workspace_min: [-0.70, -0.60, 0.10]
-    # TCP 工作空间上限 [x, y, z]，单位米；目标点会被限制在该范围内。
    workspace_max: [0.70, 0.40, 0.70]
-    # TCP 到机器人底座轴线的圆柱半径限制 [最小半径, 最大半径]，单位米。
    cyl_radius_limit: [0.20, 0.60]
-    # 低高度判定阈值，单位米；低于该 Z 值时会启用更保守的最小半径。
    low_z_threshold: 0.20
-    # 低高度区域内的最小圆柱半径，单位米，用于避免 TCP 过靠近底座。
    low_z_min_radius: 0.21

-    # 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反，因此翻转手柄 X、Z 两列。
-    # 3x3 坐标映射矩阵，按行展开；robot_delta = matrix * xr_delta。
+    # 映射关系：机器人位移增量 = [-手柄y, 手柄z, -手柄x]。
    xr_to_robot_matrix: [0.0, -1.0, 0.0,
                         0.0, 0.0, 1.0,
                         -1.0, 0.0, 0.0]

-    # 是否使用 mock 机械臂；true 不连接真机，只在软件里积分模拟运动。
    use_mock: false
-    # mock 模式下的初始 TCP 位姿 [x, y, z, rx, ry, rz]。
    mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
-    # 左臂 RM75 控制器 IP 地址。
    robot_ip: 192.168.192.18
-    # RM75 控制器 TCP 端口。
    robot_port: 8080
-    # 厂商 SDK 速度透传避奇异参数；0 表示关闭硬件避奇异初始化。
    avoid_singularity: 0
-    # 厂商 SDK 速度透传参考坐标系类型；当前使用 1。
    frame_type: 1
-    # 速度透传 follow 标志，传给 rm_movev_canfd；false 表示不启用跟随模式。
    follow: false
-    # 是否在连接真机后尝试下发硬件安全限速、限加速度等配置。
-    configure_safety_limits: true
-    # 下发给控制器的最大直线速度，单位 m/s。
-    max_line_speed: 1.0
-    # 下发给控制器的最大角速度，单位 rad/s 或 SDK 定义单位。
-    max_angular_speed: 1.5
-    # 下发给控制器的最大直线加速度，单位 m/s^2。
-    max_line_acc: 1.0
-    # 下发给控制器的最大角加速度，单位 rad/s^2 或 SDK 定义单位。
-    max_angular_acc: 2.0
-    # 下发给控制器的单关节最大速度，单位通常为 deg/s。
-    joint_max_speed: 180.0
-    # 下发给控制器的单关节最大加速度，单位通常为 deg/s^2。
-    joint_max_acc: 180.0
-    # 连接真机后是否自动移动到 initial_joint_pose 和 initial_tcp_pose。
-    move_to_initial_pose_on_connect: false
-    # 自动初始化时先执行的关节目标位姿，单位通常为度，共 7 轴。
-    initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
-    # 自动初始化时再执行的 TCP 目标位姿 [x, y, z, rx, ry, rz]。
-    initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
-    # 初始化移动速度，传给厂商 movej/movel 接口。
-    init_move_speed: 20
-    # 真机命令模式；pose_canfd 表示通过 CANFD 位置目标方式输出控制。
-    command_mode: pose_canfd
-    # CANFD 轨迹模式参数，传给厂商 SDK，用于控制位置目标执行方式。
    canfd_trajectory_mode: 2
-    # CANFD 透传平滑/滤波参数，传给厂商 SDK。
    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
+    joint_max_speed: 180.0
+    joint_max_acc: 180.0
+    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
    debug_topic_prefix: /xr_rm
--- a/xr_rm_bringup/config/right_arm_rm75.yaml
+++ b/xr_rm_bringup/config/right_arm_rm75.yaml
@ -1,19 +1,21 @@
-# 右臂单独调试配置：XR 相对位移控制 RM75 TCP。
+# 右臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。
 # 末端外设由 peripherals_rm75.yaml 配置，launch 只在真机连接阶段初始化。

 single_arm_velocity_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: 1.0
-    kp_linear: 1.8
-    deadband_m: 0.002
-    low_pass_alpha: 0.7
-    max_linear_speed: 0.5
+    scale: 0.75
+    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.3
    enable_position_axes: [true, true, true]
+    current_pose_poll_hz: 10.0

    workspace_min: [-0.70, -0.60, 0.10]
    workspace_max: [0.70, 0.40, 0.70]
@ -21,7 +23,7 @@ single_arm_velocity_teleop:
    low_z_threshold: 0.1
    low_z_min_radius: 0.1

-    # 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反，因此翻转手柄 X、Z 两列。
+    # 映射关系：机器人位移增量 = [手柄y, 手柄z, 手柄x]。
    xr_to_robot_matrix: [0.0, 1.0, 0.0,
                         0.0, 0.0, 1.0,
                         1.0, 0.0, 0.0]
@ -33,6 +35,8 @@ single_arm_velocity_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