dual arm control from sample_udp_sender.py

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -23,13 +23,13 @@ left_arm_teleop:
     kp_linear: 1.8
     deadband_m: 0.002
     low_pass_alpha: 0.35
-    max_linear_speed: 0.04
+    max_linear_speed: 0.12
     enable_position_axes: [true, true, true]
 
     # 来自 acDual-arm 的 bounds_p[left]：
     # x[-0.50, 0.50]，y[-0.60, -0.20]，z[0.10, 0.50]。
-    workspace_min: [-0.50, -0.60, 0.10]
+    workspace_min: [-0.70, -0.60, 0.10]
-    workspace_max: [0.50, -0.20, 0.50]
+    workspace_max: [0.70, 0.40, 0.70]
     cyl_radius_limit: [0.20, 0.60]
     low_z_threshold: 0.20
     low_z_min_radius: 0.21
@@ -45,7 +45,7 @@ left_arm_teleop:
     mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
     robot_ip: 192.168.192.18
     robot_port: 8080
-    avoid_singularity: 1
+    avoid_singularity: 0
     frame_type: 1
     follow: false
     configure_safety_limits: true
@@ -60,6 +60,9 @@ left_arm_teleop:
     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:
@@ -73,7 +76,7 @@ right_arm_teleop:
     kp_linear: 1.8
     deadband_m: 0.002
     low_pass_alpha: 0.35
-    max_linear_speed: 0.04
+    max_linear_speed: 0.12
     enable_position_axes: [true, true, true]
 
     # 来自 acDual-arm 的 bounds_p[right]：

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -11,11 +11,11 @@ single_arm_velocity_teleop:
     kp_linear: 1.8
     deadband_m: 0.002
     low_pass_alpha: 0.35
-    max_linear_speed: 0.04
+    max_linear_speed: 0.2
     enable_position_axes: [true, true, true]
 
-    workspace_min: [-0.50, -0.60, 0.10]
+    workspace_min: [-0.70, -0.60, 0.10]
-    workspace_max: [0.50, -0.20, 0.50]
+    workspace_max: [0.70, 0.40, 0.70]
     cyl_radius_limit: [0.20, 0.60]
     low_z_threshold: 0.20
     low_z_min_radius: 0.21
@@ -28,7 +28,7 @@ single_arm_velocity_teleop:
     mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
     robot_ip: 192.168.192.18
     robot_port: 8080
-    avoid_singularity: 1
+    avoid_singularity: 0
     frame_type: 1
     follow: false
     configure_safety_limits: true
@@ -42,4 +42,7 @@ single_arm_velocity_teleop:
     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

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -11,11 +11,11 @@ single_arm_velocity_teleop:
     kp_linear: 1.8
     deadband_m: 0.002
     low_pass_alpha: 0.35
-    max_linear_speed: 0.04
+    max_linear_speed: 0.2
     enable_position_axes: [true, true, true]
 
     workspace_min: [-0.70, -0.60, 0.10]
-    workspace_max: [0.50, 0.40, 0.70]
+    workspace_max: [0.70, 0.40, 0.70]
     cyl_radius_limit: [0.20, 0.60]
     low_z_threshold: 0.20
     low_z_min_radius: 0.21

xr_rm_bringup/launch/arm_debug.launch.py

@@ -1,3 +1,10 @@
+"""单臂/双臂通用调试入口。
+
+该 launch 文件用于现场调试阶段按需启动左臂、右臂或双臂，并可通过
+`use_mock` 在 mock 模式和 RM75 真机模式之间切换。它会固定启动 UDP
+手柄接收节点，再根据 `arm:=left|right|both` 选择对应的遥操作节点。
+"""
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
@@ -6,10 +13,12 @@ from launch_ros.substitutions import FindPackageShare
 
 
 def _as_bool(value: str) -> bool:
+    """把 launch 字符串参数转换成 Python bool，便于在 OpaqueFunction 中分支。"""
     return value.strip().lower() in ("1", "true", "yes", "on")
 
 
 def _config_file(name: str) -> PathJoinSubstitution:
+    """生成 xr_rm_bringup/config 下配置文件的可安装路径。"""
     return PathJoinSubstitution([
         FindPackageShare("xr_rm_bringup"),
         "config",
@@ -18,6 +27,7 @@ def _config_file(name: str) -> PathJoinSubstitution:
 
 
 def _udp_receiver_node() -> Node:
+    """接收 PICO/XR UDP 数据，并发布左右手柄 ROS2 话题。"""
     return Node(
         package="xr_rm_input",
         executable="udp_controller_receiver",
@@ -32,7 +42,15 @@ def _udp_receiver_node() -> Node:
     )
 
 
-def _single_arm_node(arm: str, use_mock: bool, move_to_initial_pose: bool) -> Node:
+def _single_arm_node(
+    arm: str,
+    use_mock: bool,
+    move_to_initial_pose: bool,
+    avoid_singularity: int,
+    frame_type: int,
+    configure_safety_limits: 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")
     return Node(
@@ -46,13 +64,24 @@ def _single_arm_node(arm: str, use_mock: bool, move_to_initial_pose: bool) -> No
                 "use_mock": use_mock,
                 "robot_ip": robot_ip,
                 "robot_port": LaunchConfiguration("robot_port"),
+                "avoid_singularity": avoid_singularity,
+                "frame_type": frame_type,
+                "configure_safety_limits": configure_safety_limits,
                 "move_to_initial_pose_on_connect": move_to_initial_pose,
             },
         ],
     )
 
 
-def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]:
+def _dual_arm_nodes(
+    use_mock: bool,
+    move_to_initial_pose: bool,
+    left_avoid_singularity: int,
+    right_avoid_singularity: int,
+    frame_type: int,
+    configure_safety_limits: bool,
+) -> list[Node]:
+    """创建双臂节点；两个节点共用双臂 YAML，但节点名区分左右臂参数命名空间。"""
     config_file = _config_file("dual_arm_rm75.yaml")
     return [
         Node(
@@ -66,6 +95,9 @@ def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]:
                     "use_mock": use_mock,
                     "robot_ip": LaunchConfiguration("left_robot_ip"),
                     "robot_port": LaunchConfiguration("robot_port"),
+                    "avoid_singularity": left_avoid_singularity,
+                    "frame_type": frame_type,
+                    "configure_safety_limits": configure_safety_limits,
                     "move_to_initial_pose_on_connect": move_to_initial_pose,
                 },
             ],
@@ -81,6 +113,9 @@ def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]:
                     "use_mock": use_mock,
                     "robot_ip": LaunchConfiguration("right_robot_ip"),
                     "robot_port": LaunchConfiguration("robot_port"),
+                    "avoid_singularity": right_avoid_singularity,
+                    "frame_type": frame_type,
+                    "configure_safety_limits": configure_safety_limits,
                     "move_to_initial_pose_on_connect": move_to_initial_pose,
                 },
             ],
@@ -89,33 +124,78 @@ def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]:
 
 
 def _launch_setup(context, *args, **kwargs):
+    """运行时读取 launch 参数，决定启动单臂还是双臂。"""
     del args, kwargs
     arm = LaunchConfiguration("arm").perform(context).strip().lower()
     use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
     move_to_initial_pose = _as_bool(
         LaunchConfiguration("move_to_initial_pose_on_connect").perform(context)
     )
+    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))
+    configure_safety_limits = _as_bool(
+        LaunchConfiguration("configure_safety_limits").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))
+        nodes.extend(
+            _dual_arm_nodes(
+                use_mock,
+                move_to_initial_pose,
+                left_avoid_singularity,
+                right_avoid_singularity,
+                frame_type,
+                configure_safety_limits,
+            )
+        )
     else:
-        nodes.append(_single_arm_node(arm, use_mock, move_to_initial_pose))
+        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,
+                configure_safety_limits,
+            )
+        )
     return nodes
 
 
 def generate_launch_description() -> LaunchDescription:
     return LaunchDescription([
+        # 调试目标：left/right/both；默认右臂方便单臂逐步上真机。
         DeclareLaunchArgument("arm", default_value="right"),
+        # true 时只跑 mock，不连接 RM75；false 时通过 RealMan SDK 连接真机。
         DeclareLaunchArgument("use_mock", default_value="true"),
+        # UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。
         DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
         DeclareLaunchArgument("udp_port", default_value="15000"),
+        # 左右 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"),
+        DeclareLaunchArgument("configure_safety_limits", default_value="true"),
+        # 默认不自动移动到初始点，确认安全区后再显式打开。
         DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
+        # OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。
         OpaqueFunction(function=_launch_setup),
     ])

xr_rm_bringup/launch/dual_arm_mock.launch.py

@@ -1,3 +1,10 @@
+"""双臂 mock 遥操作启动入口。
+
+该 launch 文件用于不连接真实 RM75 的离线闭环验证：启动 UDP 手柄接收节点，
+并分别启动左、右两个 `single_arm_velocity_teleop` 节点。两个遥操作节点使用
+同一份双臂配置文件，但通过节点名读取各自的参数命名空间。
+"""
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.conditions import IfCondition
@@ -7,6 +14,7 @@ from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description() -> LaunchDescription:
+    # 通过 robot_config 参数选择 xr_rm_bringup/config 下的双臂配置文件。
     config_file = PathJoinSubstitution([
         FindPackageShare("xr_rm_bringup"),
         "config",
@@ -14,9 +22,11 @@ def generate_launch_description() -> LaunchDescription:
     ])
 
     return LaunchDescription([
+        # UDP 监听参数，需要和 PICO 端或 sample_udp_sender 的目标地址保持一致。
         DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
         DeclareLaunchArgument("udp_port", default_value="15000"),
         DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"),
+        # 这些开关便于单独验证 UDP、左臂或右臂链路。
         DeclareLaunchArgument("run_udp", default_value="true"),
         DeclareLaunchArgument("run_left_arm", default_value="true"),
         DeclareLaunchArgument("run_right_arm", default_value="true"),

xr_rm_bringup/launch/dual_arm_realman.launch.py

@@ -1,3 +1,10 @@
+"""双 RM75 真机遥操作启动入口。
+
+该 launch 文件用于连接左右两台真实 RM75：启动 UDP 手柄接收节点，并分别启动
+左、右两个真机遥操作节点。默认不会自动移动到初始点，只有显式设置
+`move_to_initial_pose_on_connect:=true` 后才会执行初始化点位移动。
+"""
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.conditions import IfCondition
@@ -8,6 +15,7 @@ from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description() -> LaunchDescription:
+    # 双臂真机默认读取 xr_rm_bringup/config/dual_arm_rm75.yaml。
     config_file = PathJoinSubstitution([
         FindPackageShare("xr_rm_bringup"),
         "config",
@@ -15,16 +23,21 @@ def generate_launch_description() -> LaunchDescription:
     ])
 
     return LaunchDescription([
+        # UDP 监听参数，需要与 PICO 端发送地址和端口匹配。
         DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
         DeclareLaunchArgument("udp_port", default_value="15000"),
+        # 左右 RM75 的默认现场 IP，可在 launch 命令中按实际网络覆盖。
         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("robot_config", default_value="dual_arm_rm75.yaml"),
+        # 调试时可关闭部分节点，只验证 UDP 或单侧机械臂。
         DeclareLaunchArgument("run_udp", default_value="true"),
         DeclareLaunchArgument("run_left_arm", default_value="true"),
         DeclareLaunchArgument("run_right_arm", default_value="true"),
+        # 真机默认不自动动臂，确认安全区清空后再手动打开。
         DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
+        # 接收 PICO/XR 侧 UDP 数据，并按左右手字段分发到对应手柄话题。
         Node(
             package="xr_rm_input",
             executable="udp_controller_receiver",
@@ -38,6 +51,7 @@ def generate_launch_description() -> LaunchDescription:
                 "right_topic": "/xr/right_controller",
             }],
         ),
+        # 左臂真机节点：连接 left_robot_ip，并读取 left_arm_teleop 参数命名空间。
         Node(
             package="xr_rm_teleop",
             executable="single_arm_velocity_teleop",
@@ -57,6 +71,7 @@ def generate_launch_description() -> LaunchDescription:
                 },
             ],
         ),
+        # 右臂真机节点：连接 right_robot_ip，并读取 right_arm_teleop 参数命名空间。
         Node(
             package="xr_rm_teleop",
             executable="single_arm_velocity_teleop",

xr_rm_bringup/tools/launcher_ui.py

@@ -1,4 +1,10 @@
 #!/usr/bin/env python3
+"""XR-RM 桌面调试启动器。
+
+提供 Tkinter 图形界面，按“仿真/左臂/右臂/双臂/诊断”组织常用 ROS2
+launch、sample_udp_sender、topic 监控和环境检查命令，降低现场调试时的命令输入成本。
+"""
+
 from __future__ import annotations
 
 import ctypes
@@ -25,6 +31,13 @@ except ImportError as exc:
 DEFAULT_LEFT_IP = "192.168.192.18"
 DEFAULT_RIGHT_IP = "192.168.192.19"
 DEFAULT_PORT = "15000"
+SAMPLE_SENDER_SECONDS = "30"
+SAMPLE_SENDER_STAGGERED_SECONDS = "60"
+SAMPLE_SENDER_ARGS = (
+    f"--host 127.0.0.1 --port {DEFAULT_PORT} "
+    "--pattern axis_sweep --amplitude 0.5 "
+    "--hold-seconds 4.0 --center-seconds 1.2 --initial-center-seconds 1.0"
+)
 TERMINAL_TITLE_PREFIX = "XR-RM Terminal - "
 TOPIC_MONITOR_TITLE = "XR-RM Topic Monitor"
 TOPIC_MONITOR_ACTION = "__xr_rm_topic_monitor__"
@@ -62,7 +75,22 @@ def _with_index(items: list[tuple[str, str]]) -> list[tuple[str, str]]:
     return [(f"{idx}. {title}", cmd) for idx, (title, cmd) in enumerate(items, start=1)]
 
 
+def _sample_udp_sender_command(
+    hand: str,
+    seconds: str = SAMPLE_SENDER_SECONDS,
+    both_mode: str = "synchronized",
+) -> str:
+    command = (
+        f"ros2 run xr_rm_input sample_udp_sender --hand {hand} "
+        f"{SAMPLE_SENDER_ARGS} --seconds {seconds}"
+    )
+    if hand == "both":
+        command += f" --both-mode {both_mode}"
+    return command
+
+
 def _find_workspace_root() -> Path:
+    # 优先使用显式环境变量，便于从安装目录、源码目录或桌面快捷方式启动。
     env_workspace = os.environ.get("XR_RM_WS")
     if env_workspace:
         return Path(env_workspace).expanduser().resolve()
@@ -77,6 +105,7 @@ def _find_workspace_root() -> Path:
 
 
 def _source_lines(workspace_root: Path) -> list[str]:
+    # 每个新终端都重新 source ROS 和工作空间，避免依赖启动器自身的 shell 环境。
     ros_setup = Path("/opt/ros/humble/setup.bash")
     install_setup = workspace_root / "install" / "setup.bash"
     lines = [
@@ -100,11 +129,13 @@ def _source_lines(workspace_root: Path) -> list[str]:
 
 
 def _one_click_mock(arm: str, hand: str) -> str:
+    sender_seconds = SAMPLE_SENDER_STAGGERED_SECONDS if hand == "both" else SAMPLE_SENDER_SECONDS
+    both_mode = "staggered" if hand == "both" else "synchronized"
     return "\n".join([
         f"ros2 launch xr_rm_bringup arm_debug.launch.py arm:={arm} use_mock:=true &",
         "launch_pid=$!",
         "sleep 2",
-        f"ros2 run xr_rm_input sample_udp_sender --hand {hand} --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 20",
+        _sample_udp_sender_command(hand, sender_seconds, both_mode),
         "echo",
         "echo 'Sample sender finished. The launch process is still running in this terminal.'",
         "echo 'Press Ctrl-C here, or use the cleanup button in the launcher, to stop it.'",
@@ -133,22 +164,23 @@ def _finalize_items(
 
 
 def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
+    # UI 列表只维护命令模板；真正执行时统一套上工作空间 source 和终端包装。
     if mode == "Simulation":
         items = [
             ("Left Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true"),
             ("Right Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true"),
             ("Dual Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true"),
             (
-                "Sample UDP Sender (Left, 10s)",
+                "Sample UDP Sender (Left, 30s)",
-                f"ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
+                _sample_udp_sender_command("left"),
             ),
             (
-                "Sample UDP Sender (Right, 10s)",
+                "Sample UDP Sender (Right, 30s)",
-                f"ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
+                _sample_udp_sender_command("right"),
             ),
             (
-                "Sample UDP Sender (Both, 10s)",
+                "Sample UDP Sender (Both Staggered, 60s)",
-                f"ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
+                _sample_udp_sender_command("both", SAMPLE_SENDER_STAGGERED_SECONDS, "staggered"),
             ),
             ("One-Click Left Mock Demo", _one_click_mock("left", "left")),
             ("One-Click Right Mock Demo", _one_click_mock("right", "right")),
@@ -165,8 +197,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
                 "move_to_initial_pose_on_connect:=false",
             ),
             (
-                "Sample UDP Sender (Left, 10s)",
+                "Sample UDP Sender (Left, 30s)",
-                f"ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
+                _sample_udp_sender_command("left"),
             ),
         ]
         one_click = None
@@ -180,8 +212,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
                 "move_to_initial_pose_on_connect:=false",
             ),
             (
-                "Sample UDP Sender (Right, 10s)",
+                "Sample UDP Sender (Right, 30s)",
-                f"ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
+                _sample_udp_sender_command("right"),
             ),
         ]
         one_click = None
@@ -196,8 +228,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
                 "move_to_initial_pose_on_connect:=false",
             ),
             (
-                "Sample UDP Sender (Both, 10s)",
+                "Sample UDP Sender (Both Staggered, 60s)",
-                f"ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
+                _sample_udp_sender_command("both", SAMPLE_SENDER_STAGGERED_SECONDS, "staggered"),
             ),
         ]
         one_click = None
@@ -213,6 +245,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
 
 
 class LauncherApp:
+    """XR-RM 调试启动器主窗口，负责命令列表、终端启动和监控窗口管理。"""
+
     def __init__(self, root: tk.Tk) -> None:
         self.root = root
         self.workspace_root = _find_workspace_root()
@@ -421,6 +455,7 @@ class LauncherApp:
         return result.returncode == 0
 
     def check_prerequisites(self) -> None:
+        # 环境检查只做轻量探测，避免在 UI 线程里执行耗时的网络或硬件连接动作。
         ok: list[str] = []
         warnings: list[str] = []
         errors: list[str] = []
@@ -604,16 +639,72 @@ class LauncherApp:
         return Path(script_file.name)
 
     def topic_monitor_rect(self) -> tuple[int, int, int, int]:
-        screen_width = max(self.root.winfo_screenwidth(), 1)
+        # 监控窗口尽量放在启动器所在显示器，双屏现场调试时更容易找回窗口。
-        screen_height = max(self.root.winfo_screenheight(), 1)
+        monitor_left, monitor_top, monitor_width, monitor_height = self.active_monitor_rect()
-        width = min(screen_width - 160, max(960, int(screen_width * 0.75)))
+        width = min(monitor_width - 120, max(960, int(monitor_width * 0.75)))
-        height = min(screen_height - 160, max(560, int(screen_height * 0.66)))
+        height = min(monitor_height - 120, max(560, int(monitor_height * 0.66)))
-        width = max(width, min(screen_width, 900))
+        width = max(width, min(monitor_width, 900))
-        height = max(height, min(screen_height, 520))
+        height = max(height, min(monitor_height, 520))
-        left = max((screen_width - width) // 2, 0)
+        left = monitor_left + max((monitor_width - width) // 2, 0)
-        top = max((screen_height - height) // 2, 0)
+        top = monitor_top + max((monitor_height - height) // 2, 0)
         return width, height, left, top
 
+    def active_monitor_rect(self) -> tuple[int, int, int, int]:
+        monitors = self.xrandr_monitor_rects()
+        if not monitors:
+            return (0, 0, max(self.root.winfo_screenwidth(), 1), max(self.root.winfo_screenheight(), 1))
+
+        self.root.update_idletasks()
+        root_center = (
+            self.root.winfo_rootx() + max(self.root.winfo_width(), 1) // 2,
+            self.root.winfo_rooty() + max(self.root.winfo_height(), 1) // 2,
+        )
+        pointer = (self.root.winfo_pointerx(), self.root.winfo_pointery())
+
+        for point_x, point_y in (root_center, pointer):
+            for left, top, width, height, _primary in monitors:
+                if left <= point_x < left + width and top <= point_y < top + height:
+                    return (left, top, width, height)
+
+        for left, top, width, height, primary in monitors:
+            if primary:
+                return (left, top, width, height)
+
+        left, top, width, height, _primary = monitors[0]
+        return (left, top, width, height)
+
+    @staticmethod
+    def xrandr_monitor_rects() -> list[tuple[int, int, int, int, bool]]:
+        if not shutil.which("xrandr"):
+            return []
+
+        try:
+            output = subprocess.check_output(
+                ["xrandr", "--query"],
+                text=True,
+                stderr=subprocess.DEVNULL,
+            )
+        except Exception:
+            return []
+
+        monitors: list[tuple[int, int, int, int, bool]] = []
+        pattern = re.compile(
+            r"^\S+ connected(?P<primary> primary)? "
+            r"(?P<w>\d+)x(?P<h>\d+)\+(?P<x>-?\d+)\+(?P<y>-?\d+)"
+        )
+        for line in output.splitlines():
+            match = pattern.match(line)
+            if not match:
+                continue
+            monitors.append((
+                int(match.group("x")),
+                int(match.group("y")),
+                int(match.group("w")),
+                int(match.group("h")),
+                bool(match.group("primary")),
+            ))
+        return monitors
+
     def window_ids_matching_title(self, title: str) -> list[int]:
         if not shutil.which("xprop"):
             return []
@@ -641,16 +732,87 @@ class LauncherApp:
                 window_ids.append(int(xid_text, 16))
         return window_ids
 
-    def force_monitor_window_geometry(self, title: str, attempts_left: int = 40) -> None:
+    def force_monitor_window_geometry(
+        self,
+        title: str,
+        target_rect: tuple[int, int, int, int],
+        attempts_left: int = 30,
+    ) -> None:
         if attempts_left <= 0 or not self.root.winfo_exists():
             return
 
         window_ids = self.window_ids_matching_title(title)
-        if window_ids:
+        if not window_ids:
-            width, height, left, top = self.topic_monitor_rect()
+            self.root.after(
+                80,
+                lambda: self.force_monitor_window_geometry(title, target_rect, attempts_left - 1),
+            )
+            return
+
+        if any(self.monitor_window_needs_geometry(window_id, target_rect) for window_id in window_ids):
+            width, height, left, top = target_rect
             self.apply_x11_window_geometry(window_ids, left, top, width, height)
 
-        self.root.after(250, lambda: self.force_monitor_window_geometry(title, attempts_left - 1))
+    def monitor_window_needs_geometry(self, window_id: int, target_rect: tuple[int, int, int, int]) -> bool:
+        props = self.x11_window_props(window_id)
+        if "_NET_WM_STATE_FULLSCREEN" in props or "_NET_WM_STATE_MAXIMIZED_HORZ" in props:
+            return True
+        if "_NET_WM_STATE_MAXIMIZED_VERT" in props:
+            return True
+
+        geometry = self.x11_window_geometry(window_id)
+        if geometry is None:
+            return True
+
+        actual_left, actual_top, actual_width, actual_height = geometry
+        target_width, target_height, target_left, target_top = target_rect
+        if abs(actual_width - target_width) > 180 or abs(actual_height - target_height) > 180:
+            return True
+        if abs(actual_left - target_left) > 180 or abs(actual_top - target_top) > 180:
+            return True
+        return False
+
+    @staticmethod
+    def x11_window_props(window_id: int) -> str:
+        if not shutil.which("xprop"):
+            return ""
+        try:
+            return subprocess.check_output(
+                ["xprop", "-id", hex(window_id), "_NET_WM_STATE"],
+                text=True,
+                stderr=subprocess.DEVNULL,
+            )
+        except Exception:
+            return ""
+
+    @staticmethod
+    def x11_window_geometry(window_id: int) -> tuple[int, int, int, int] | None:
+        if not shutil.which("xwininfo"):
+            return None
+        try:
+            output = subprocess.check_output(
+                ["xwininfo", "-id", hex(window_id)],
+                text=True,
+                stderr=subprocess.DEVNULL,
+            )
+        except Exception:
+            return None
+
+        values: dict[str, int] = {}
+        patterns = {
+            "left": r"Absolute upper-left X:\s+(-?\d+)",
+            "top": r"Absolute upper-left Y:\s+(-?\d+)",
+            "width": r"Width:\s+(\d+)",
+            "height": r"Height:\s+(\d+)",
+        }
+        for key, pattern in patterns.items():
+            match = re.search(pattern, output)
+            if match:
+                values[key] = int(match.group(1))
+
+        if len(values) != 4:
+            return None
+        return values["left"], values["top"], values["width"], values["height"]
 
     def apply_x11_window_geometry(self, window_ids: list[int], left: int, top: int, width: int, height: int) -> None:
         lib_path = ctypes.util.find_library("X11")
@@ -769,8 +931,8 @@ class LauncherApp:
         event_mask = (1 << 19) | (1 << 20)  # SubstructureNotifyMask | SubstructureRedirectMask
         xlib.XSendEvent(display, root, False, event_mask, ctypes.byref(event))
 
-    def write_topic_monitor_config(self) -> Path:
+    def write_topic_monitor_config(self, target_rect: tuple[int, int, int, int] | None = None) -> Path:
-        width, height, left, top = self.topic_monitor_rect()
+        width, height, left, top = target_rect or self.topic_monitor_rect()
         left_script, right_script = [
             self.write_topic_monitor_script(title, topic)
             for title, topic in TOPIC_MONITORS
@@ -778,6 +940,8 @@ class LauncherApp:
         layout_name = "xr_rm_topic_monitor"
         config_text = "\n".join([
             "[global_config]",
+            "  geometry_hinting = False",
+            "  window_state = normal",
             "  suppress_multiple_term_dialog = True",
             "[keybindings]",
             "[profiles]",
@@ -822,8 +986,8 @@ class LauncherApp:
             config_file.write(config_text)
         return Path(config_file.name)
 
-    def write_ros_graph_monitor_config(self) -> Path:
+    def write_ros_graph_monitor_config(self, target_rect: tuple[int, int, int, int] | None = None) -> Path:
-        width, height, left, top = self.topic_monitor_rect()
+        width, height, left, top = target_rect or self.topic_monitor_rect()
         topic_script, node_script = [
             self.write_ros_graph_monitor_script(title, command)
             for title, command in ROS_GRAPH_MONITORS
@@ -831,6 +995,8 @@ class LauncherApp:
         layout_name = "xr_rm_ros_graph_monitor"
         config_text = "\n".join([
             "[global_config]",
+            "  geometry_hinting = False",
+            "  window_state = normal",
             "  suppress_multiple_term_dialog = True",
             "[keybindings]",
             "[profiles]",
@@ -892,7 +1058,8 @@ class LauncherApp:
             )
             return
 
-        config_path = self.write_topic_monitor_config()
+        target_rect = self.topic_monitor_rect()
+        config_path = self.write_topic_monitor_config(target_rect)
         command = [
             terminal,
             "--no-dbus",
@@ -910,7 +1077,7 @@ class LauncherApp:
                 stdout=subprocess.DEVNULL,
                 stderr=subprocess.DEVNULL,
             )
-            self.root.after(150, lambda: self.force_monitor_window_geometry(TOPIC_MONITOR_TITLE))
+            self.root.after(60, lambda: self.force_monitor_window_geometry(TOPIC_MONITOR_TITLE, target_rect))
             self.status.config(text="Opened Terminator controller topic monitor.")
         except Exception as exc:
             messagebox.showerror("Topic Monitor Failed", f"Failed to launch topic monitor:\n{exc}")
@@ -933,7 +1100,8 @@ class LauncherApp:
             )
             return
 
-        config_path = self.write_ros_graph_monitor_config()
+        target_rect = self.topic_monitor_rect()
+        config_path = self.write_ros_graph_monitor_config(target_rect)
         command = [
             terminal,
             "--no-dbus",
@@ -951,7 +1119,7 @@ class LauncherApp:
                 stdout=subprocess.DEVNULL,
                 stderr=subprocess.DEVNULL,
             )
-            self.root.after(150, lambda: self.force_monitor_window_geometry(ROS_GRAPH_MONITOR_TITLE))
+            self.root.after(60, lambda: self.force_monitor_window_geometry(ROS_GRAPH_MONITOR_TITLE, target_rect))
             self.status.config(text="Opened Terminator ROS graph monitor.")
         except Exception as exc:
             messagebox.showerror("ROS Graph Monitor Failed", f"Failed to launch ROS graph monitor:\n{exc}")
@@ -997,6 +1165,7 @@ class LauncherApp:
         if not confirm:
             return
 
+        # 只清理由启动器常用命令创建的 ROS/监控进程，并保护当前 UI 进程。
         patterns = [
             "ros2 launch xr_rm_bringup",
             "ros2 run xr_rm_input",

xr_rm_bringup/tools/realman_dual_arm_state_monitor.py

@@ -0,0 +1,215 @@
+#!/usr/bin/env python3
+"""独立双臂状态监控脚本（轮询版）。
+
+优化点：
+- 不再依赖 UDP 回调；
+- 直接通过 rm_get_current_arm_state() 按句柄轮询左右臂状态；
+- 避免同进程双回调潜在冲突导致的左臂显示异常。
+"""
+
+from __future__ import annotations
+
+import argparse
+import time
+from dataclasses import dataclass
+from typing import Optional
+
+import numpy as np
+
+from realman_pkg.Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e
+from realman_pkg.core.arg_cfg import host_ip, ip_l, ip_r, port_l, port_r
+
+
+@dataclass
+class ArmSnapshot:
+    """单臂状态快照。"""
+
+    connected: bool
+    ret_code: int
+    last_update: float
+    joint_deg: list[float]
+    pose_euler: np.ndarray
+    err_text: str
+
+
+class ArmStatePoller:
+    """单臂状态轮询器。"""
+
+    def __init__(
+        self,
+        name: str,
+        robot_ip: str,
+        robot_port: int,
+        level: int = 3,
+        mode: int = 2,
+    ):
+        self.name = name
+        self.robot_ip = robot_ip
+        self.robot_port = int(robot_port)
+        self.level = int(level)
+        self.mode = int(mode)
+
+        self.robot: Optional[RoboticArm] = None
+        self.robot_handle = None
+        self.connected = False
+
+        self.last_snapshot = ArmSnapshot(
+            connected=False,
+            ret_code=-999,
+            last_update=0.0,
+            joint_deg=[0.0] * 7,
+            pose_euler=np.zeros(6, dtype=np.float64),
+            err_text="not connected",
+        )
+
+        self._connect()
+
+    def _connect(self):
+        """连接机械臂。"""
+        try:
+            self.robot = RoboticArm(rm_thread_mode_e(self.mode))
+            self.robot_handle = self.robot.rm_create_robot_arm(self.robot_ip, self.robot_port, self.level)
+            if self.robot_handle.id == -1:
+                self.connected = False
+                self.last_snapshot.connected = False
+                self.last_snapshot.err_text = f"connect failed ({self.robot_ip}:{self.robot_port})"
+                print(f"[{self.name}] 连接失败: {self.last_snapshot.err_text}")
+                return
+
+            self.connected = True
+            self.last_snapshot.connected = True
+            self.last_snapshot.err_text = "ok"
+            print(f"[{self.name}] 已连接, handle_id={self.robot_handle.id}, ip={self.robot_ip}, port={self.robot_port}")
+        except Exception as exc:
+            self.connected = False
+            self.last_snapshot.connected = False
+            self.last_snapshot.err_text = f"connect exception: {exc}"
+            print(f"[{self.name}] 连接异常: {exc}")
+
+    def poll(self) -> ArmSnapshot:
+        """轮询一次当前状态。"""
+        if (not self.connected) or (self.robot is None):
+            return self.last_snapshot
+
+        try:
+            ret, state = self.robot.rm_get_current_arm_state()
+            now_t = time.time()
+
+            if ret != 0:
+                self.last_snapshot.ret_code = ret
+                self.last_snapshot.err_text = f"rm_get_current_arm_state ret={ret}"
+                return self.last_snapshot
+
+            joint = list(state.get("joint", [0.0] * 7))
+            pose = list(state.get("pose", [0.0] * 6))
+            err = state.get("err", {})
+
+            if len(joint) < 7:
+                joint.extend([0.0] * (7 - len(joint)))
+            elif len(joint) > 7:
+                joint = joint[:7]
+
+            if len(pose) < 6:
+                pose.extend([0.0] * (6 - len(pose)))
+            elif len(pose) > 6:
+                pose = pose[:6]
+
+            self.last_snapshot = ArmSnapshot(
+                connected=True,
+                ret_code=ret,
+                last_update=now_t,
+                joint_deg=joint,
+                pose_euler=np.array(pose, dtype=np.float64),
+                err_text=str(err),
+            )
+            return self.last_snapshot
+        except Exception as exc:
+            self.last_snapshot.err_text = f"poll exception: {exc}"
+            return self.last_snapshot
+
+    def close(self):
+        """断开连接。"""
+        if self.robot is None:
+            return
+        try:
+            self.robot.rm_delete_robot_arm()
+        except Exception:
+            pass
+
+
+def format_arm_block(name: str, snap: ArmSnapshot, now_t: float) -> str:
+    """格式化单臂输出。"""
+    if not snap.connected:
+        return f"[{name}] 未连接: {snap.err_text}"
+
+    age = now_t - snap.last_update if snap.last_update > 0 else float("inf")
+    stale_flag = "(stale)" if age > 2.0 else ""
+
+    j = ", ".join(f"{v:7.2f}" for v in snap.joint_deg)
+    p = snap.pose_euler
+    return (
+        f"[{name}] update_age={age:5.2f}s {stale_flag}, ret={snap.ret_code}\n"
+        f"  Joint(deg): [{j}]\n"
+        f"  Pose[x y z rx ry rz]: [{p[0]: .4f}, {p[1]: .4f}, {p[2]: .4f}, {p[3]: .4f}, {p[4]: .4f}, {p[5]: .4f}]\n"
+        f"  Err: {snap.err_text}"
+    )
+
+
+def build_arg_parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(description="Realman 双臂状态监控脚本（轮询版）")
+    parser.add_argument("--left-ip", default=ip_l, help="左臂 IP")
+    parser.add_argument("--right-ip", default=ip_r, help="右臂 IP")
+    parser.add_argument("--left-port", type=int, default=port_l, help="左臂 TCP 端口")
+    parser.add_argument("--right-port", type=int, default=port_r, help="右臂 TCP 端口")
+    parser.add_argument("--refresh", type=float, default=0.5, help="刷新周期（秒）")
+    parser.add_argument("--level", type=int, default=3, help="连接级别")
+    parser.add_argument("--mode", type=int, default=2, help="线程模式 0/1/2")
+    parser.add_argument("--host-ip", default=host_ip, help="仅展示当前配置用，轮询版不使用")
+    return parser
+
+
+def main():
+    args = build_arg_parser().parse_args()
+
+    print(f"配置: left={args.left_ip}:{args.left_port}, right={args.right_ip}:{args.right_port}, host_ip={args.host_ip}")
+
+    left = ArmStatePoller(
+        name="LEFT",
+        robot_ip=args.left_ip,
+        robot_port=args.left_port,
+        level=args.level,
+        mode=args.mode,
+    )
+    right = ArmStatePoller(
+        name="RIGHT",
+        robot_ip=args.right_ip,
+        robot_port=args.right_port,
+        level=args.level,
+        mode=args.mode,
+    )
+
+    print("\n开始监控双臂状态（轮询），按 Ctrl+C 退出。\n")
+
+    try:
+        while True:
+            now_t = time.time()
+            l_snap = left.poll()
+            r_snap = right.poll()
+
+            print("\033[2J\033[H", end="")
+            print(time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(now_t)))
+            print("=" * 88)
+            print(format_arm_block("LEFT", l_snap, now_t))
+            print("-" * 88)
+            print(format_arm_block("RIGHT", r_snap, now_t))
+            print("=" * 88)
+            time.sleep(max(args.refresh, 0.05))
+    except KeyboardInterrupt:
+        print("\n退出监控...")
+    finally:
+        left.close()
+        right.close()
+
+
+if __name__ == "__main__":
+    main()

xr_rm_input/launch/udp_receiver.launch.py

@@ -1,3 +1,10 @@
+"""XR 手柄 UDP 接收节点的独立启动入口。
+
+该 launch 文件只启动 `udp_controller_receiver`，用于低层通信调试：确认 PICO
+或 sample_udp_sender 发出的 UDP JSON 能被 ROS2 接收，并发布到指定手柄话题。
+完整单臂/双臂遥操作请优先使用 xr_rm_bringup 中的 launch 文件。
+"""
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
@@ -6,9 +13,12 @@ from launch_ros.actions import Node
 
 def generate_launch_description() -> LaunchDescription:
     return LaunchDescription([
+        # UDP 监听地址和端口，需要与发送端配置一致。
         DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
         DeclareLaunchArgument("udp_port", default_value="15000"),
+        # 兼容旧版单手柄调试入口；双臂调试会使用 left_topic/right_topic。
         DeclareLaunchArgument("topic", default_value="/xr/right_controller"),
+        # 仅启动 UDP 接收节点，不启动任何机械臂控制节点。
         Node(
             package="xr_rm_input",
             executable="udp_controller_receiver",

xr_rm_input/setup.py

@@ -1,3 +1,9 @@
+"""xr_rm_input 包安装配置。
+
+该包负责把 XR/PICO 端发来的 UDP 手柄数据接入 ROS2，并提供一个
+sample_udp_sender 方便在没有真实手柄时做链路调试。
+"""
+
 from glob import glob
 from setuptools import setup
 

xr_rm_input/xr_rm_input/__init__.py

@@ -1 +1,4 @@
+"""XR 输入包。
 
+包含 UDP 手柄数据接收节点和用于现场调试的模拟 UDP 发送脚本。
+"""

xr_rm_input/xr_rm_input/sample_udp_sender.py

@@ -1,3 +1,9 @@
+"""XR 手柄 UDP 数据模拟发送器。
+
+用于在没有真实 PICO/XR 手柄时向 udp_controller_receiver 发送测试数据。
+默认轨迹按 +X/-X、+Y/-Y、+Z/-Z 成对扫轴，便于用肉眼检查机械臂方向映射。
+"""
+
 import argparse
 import json
 import math
@@ -5,14 +11,188 @@ import socket
 import time
 
 
+BASE_POS = [0.0, 1.0, 0.0]
+# 方向名称会打印到终端，方便现场观察机械臂运动时对照坐标轴。
+AXIS_DIRECTIONS = (
+    ("XR +X", (1.0, 0.0, 0.0)),
+    ("XR -X", (-1.0, 0.0, 0.0)),
+    ("XR +Y", (0.0, 1.0, 0.0)),
+    ("XR -Y", (0.0, -1.0, 0.0)),
+    ("XR +Z", (0.0, 0.0, 1.0)),
+    ("XR -Z", (0.0, 0.0, -1.0)),
+)
+AXIS_SWEEP_SEGMENTS = (
+    ("XR +X  right", (1.0, 0.0, 0.0), "hold"),
+    ("XR -X  left", (-1.0, 0.0, 0.0), "hold"),
+    ("CENTER / X done", (0.0, 0.0, 0.0), "center"),
+    ("XR +Y  up", (0.0, 1.0, 0.0), "hold"),
+    ("XR -Y  down", (0.0, -1.0, 0.0), "hold"),
+    ("CENTER / Y done", (0.0, 0.0, 0.0), "center"),
+    ("XR +Z  back", (0.0, 0.0, 1.0), "hold"),
+    ("XR -Z  front", (0.0, 0.0, -1.0), "hold"),
+    ("CENTER / Z done", (0.0, 0.0, 0.0), "center"),
+)
+
+
+def _validate_positive(name: str, value: float) -> float:
+    if value <= 0.0:
+        raise argparse.ArgumentTypeError(f"{name} must be > 0")
+    return value
+
+
+def _positive_float(name: str):
+    def parser(value: str) -> float:
+        return _validate_positive(name, float(value))
+
+    return parser
+
+
+def _position_from_direction(direction: tuple[float, float, float], amplitude: float) -> list[float]:
+    return [
+        BASE_POS[0] + amplitude * direction[0],
+        BASE_POS[1] + amplitude * direction[1],
+        BASE_POS[2] + amplitude * direction[2],
+    ]
+
+
+def _pattern_period(pattern: str, hold_seconds: float, center_seconds: float) -> float:
+    if pattern == "axis_sweep":
+        return sum(
+            hold_seconds if segment_type == "hold" else center_seconds
+            for _, _, segment_type in AXIS_SWEEP_SEGMENTS
+        )
+    if pattern == "axis_steps":
+        return (hold_seconds + center_seconds) * len(AXIS_DIRECTIONS)
+    return 5.0
+
+
+def _axis_sweep_position(
+    t: float,
+    amplitude: float,
+    hold_seconds: float,
+    center_seconds: float,
+    initial_center_seconds: float,
+) -> tuple[str, list[float]]:
+    if t < initial_center_seconds:
+        return "CENTER / lock origin", BASE_POS.copy()
+
+    # +方向和-方向连续切换，比每次都很快回中心更容易看清实际运动方向。
+    cycle_t = (t - initial_center_seconds) % _pattern_period(
+        "axis_sweep",
+        hold_seconds,
+        center_seconds,
+    )
+    for label, direction, segment_type in AXIS_SWEEP_SEGMENTS:
+        duration = hold_seconds if segment_type == "hold" else center_seconds
+        if cycle_t < duration:
+            return label, _position_from_direction(direction, amplitude)
+        cycle_t -= duration
+    return "CENTER", BASE_POS.copy()
+
+
+def _axis_step_position(
+    t: float,
+    amplitude: float,
+    hold_seconds: float,
+    center_seconds: float,
+    initial_center_seconds: float,
+) -> tuple[str, list[float]]:
+    if t < initial_center_seconds:
+        return "CENTER / lock origin", BASE_POS.copy()
+
+    # 每个方向由“保持目标点”和“回中心”两段组成，循环遍历六个方向。
+    segment_seconds = hold_seconds + center_seconds
+    cycle_t = (t - initial_center_seconds) % (segment_seconds * len(AXIS_DIRECTIONS))
+    direction_index = int(cycle_t // segment_seconds)
+    segment_t = cycle_t - direction_index * segment_seconds
+    label, direction = AXIS_DIRECTIONS[direction_index]
+
+    if segment_t >= hold_seconds:
+        return f"CENTER after {label}", BASE_POS.copy()
+
+    return label, _position_from_direction(direction, amplitude)
+
+
+def _sine_position(t: float, amplitude: float, phase: float) -> tuple[str, list[float]]:
+    return "SINE X", [
+        amplitude * math.sin(2.0 * math.pi * 0.2 * t + phase),
+        BASE_POS[1],
+        BASE_POS[2],
+    ]
+
+
+def _pattern_position(
+    pattern: str,
+    t: float,
+    amplitude: float,
+    hold_seconds: float,
+    center_seconds: float,
+    initial_center_seconds: float,
+    phase: float,
+) -> tuple[str, list[float]]:
+    if pattern == "sine":
+        return _sine_position(t, amplitude, phase)
+    if pattern == "axis_steps":
+        return _axis_step_position(
+            t,
+            amplitude,
+            hold_seconds,
+            center_seconds,
+            initial_center_seconds,
+        )
+    return _axis_sweep_position(
+        t,
+        amplitude,
+        hold_seconds,
+        center_seconds,
+        initial_center_seconds,
+    )
+
+
 def main() -> None:
     parser = argparse.ArgumentParser(description="通过 UDP 发送一段模拟 XR 左/右手柄数据。")
     parser.add_argument("--host", default="127.0.0.1")
     parser.add_argument("--port", type=int, default=15000)
-    parser.add_argument("--seconds", type=float, default=5.0)
+    parser.add_argument("--seconds", type=_positive_float("seconds"), default=30.0)
-    parser.add_argument("--hz", type=float, default=60.0)
+    parser.add_argument("--hz", type=_positive_float("hz"), default=60.0)
-    parser.add_argument("--amplitude", type=float, default=0.04)
+    parser.add_argument("--amplitude", type=_positive_float("amplitude"), default=0.50)
+    parser.add_argument(
+        "--pattern",
+        choices=("axis_sweep", "axis_steps", "sine"),
+        default="axis_sweep",
+        help="axis_sweep 成对扫轴最适合肉眼看方向；axis_steps 每个方向后回中心；sine 保留旧轨迹。",
+    )
+    parser.add_argument(
+        "--hold-seconds",
+        type=_positive_float("hold-seconds"),
+        default=4.0,
+        help="axis_sweep/axis_steps 模式下，每个方向保持多久。增大可让机械臂走得更明显。",
+    )
+    parser.add_argument(
+        "--center-seconds",
+        type=_positive_float("center-seconds"),
+        default=1.2,
+        help="axis_sweep/axis_steps 模式下，回中心段保持多久。",
+    )
+    parser.add_argument(
+        "--initial-center-seconds",
+        type=_positive_float("initial-center-seconds"),
+        default=1.0,
+        help="axis_sweep/axis_steps 模式下，开头保持中心点多久，用于让遥操节点锁定原点。",
+    )
+    parser.add_argument(
+        "--trigger",
+        type=float,
+        default=0.0,
+        help="发送的 trigger 值。方向调试默认不驱动夹爪。",
+    )
     parser.add_argument("--hand", choices=("left", "right", "both"), default="right")
+    parser.add_argument(
+        "--both-mode",
+        choices=("synchronized", "staggered"),
+        default="synchronized",
+        help="hand=both 时 synchronized 左右臂同时动；staggered 先左后右，便于逐只观察。",
+    )
     args = parser.parse_args()
 
     sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
@@ -20,37 +200,86 @@ def main() -> None:
     t0 = time.monotonic()
 
     hands = ("left", "right") if args.hand == "both" else (args.hand,)
+    # 每只手柄独立发送同格式数据包，接收端根据 hand 字段分发左右话题。
     packets = {
         hand: {
             "t": 0.0,
             "hand": hand,
             "grip": False,
             "trigger": 0.0,
-            "pos": [0.0, 1.0, 0.0],
+            "pos": BASE_POS.copy(),
             "quat": [0.0, 0.0, 0.0, 1.0],
         }
         for hand in hands
     }
+    pattern_period = _pattern_period(args.pattern, args.hold_seconds, args.center_seconds)
+    one_hand_seconds = args.initial_center_seconds + pattern_period
+    full_sweep_seconds = one_hand_seconds * len(hands) if args.both_mode == "staggered" else one_hand_seconds
+    print(
+        "Sample UDP sender: "
+        f"hand={args.hand}, both_mode={args.both_mode}, pattern={args.pattern}, "
+        f"amplitude={args.amplitude:.3f}m, hold={args.hold_seconds:.2f}s, "
+        f"center={args.center_seconds:.2f}s, seconds={args.seconds:.1f}s",
+        flush=True,
+    )
+    if args.pattern != "sine" and args.seconds < full_sweep_seconds:
+        print(
+            "提示：当前 seconds 不足以跑完一轮完整方向序列；"
+            f"建议至少 {full_sweep_seconds:.1f}s。",
+            flush=True,
+        )
+    last_labels = {hand: None for hand in hands}
 
     while time.monotonic() - t0 < args.seconds:
         t = time.monotonic() - t0
-        # 模拟按住握持键后缓慢移动手柄，用来测试相对位移控制链路。
+        # 默认用清晰的成对扫轴测试相对位移链路，便于肉眼确认坐标映射。
         for index, packet in enumerate(packets.values()):
-            phase = index * math.pi
+            hand = packet["hand"]
+            pattern_t = t
+            if args.hand == "both" and args.both_mode == "staggered":
+                hand_start_t = index * one_hand_seconds
+                hand_end_t = hand_start_t + one_hand_seconds
+                if t < hand_start_t:
+                    label, pos = "WAIT / center", BASE_POS.copy()
+                elif t >= hand_end_t:
+                    label, pos = "DONE / center", BASE_POS.copy()
+                else:
+                    pattern_t = t - hand_start_t
+                    label, pos = _pattern_position(
+                        args.pattern,
+                        pattern_t,
+                        args.amplitude,
+                        args.hold_seconds,
+                        args.center_seconds,
+                        args.initial_center_seconds,
+                        index * math.pi,
+                    )
+            else:
+                label, pos = _pattern_position(
+                    args.pattern,
+                    pattern_t,
+                    args.amplitude,
+                    args.hold_seconds,
+                    args.center_seconds,
+                    args.initial_center_seconds,
+                    index * math.pi,
+                )
+
+            if label != last_labels[hand]:
+                print(f"[{t:5.2f}s] {hand:<5} {label:<22} pos={pos}", flush=True)
+                last_labels[hand] = label
+
             packet["t"] = t
             packet["grip"] = True
-            packet["trigger"] = max(0.0, math.sin(2.0 * math.pi * 0.1 * t))
+            packet["trigger"] = min(max(args.trigger, 0.0), 1.0)
-            packet["pos"] = [
+            packet["pos"] = pos
-                args.amplitude * math.sin(2.0 * math.pi * 0.2 * t + phase),
-                1.0,
-                0.0,
-            ]
             sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port))
         time.sleep(dt)
 
     for packet in packets.values():
         packet["grip"] = False
         packet["trigger"] = 0.0
+        packet["pos"] = BASE_POS.copy()
         sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port))
 
 

xr_rm_input/xr_rm_input/udp_controller_receiver.py

@@ -1,3 +1,9 @@
+"""XR 手柄 UDP 接收节点。
+
+从 UDP JSON 数据包中解析左右手柄位姿、握持键和扳机值，并发布为
+`xr_rm_interfaces/XrController` 消息，供遥操作和夹爪节点订阅。
+"""
+
 import json
 import socket
 from collections.abc import Iterable, Mapping
@@ -90,6 +96,7 @@ class UdpControllerReceiver(Node):
 
         controllers = payload.get("controllers", payload.get("controller"))
         if isinstance(controllers, Mapping):
+            # 兼容两类格式：controllers 本身就是单个手柄，或按 left/right 分组。
             if self._contains_pose(controllers):
                 hand = self._normalize_hand(controllers.get("hand", self._default_hand))
                 yield hand, controllers
@@ -143,6 +150,7 @@ class UdpControllerReceiver(Node):
 
     def _extract_pose(self, payload: Mapping[str, Any]) -> tuple[list[float], list[float]]:
         pose = payload.get("pose")
+        # 支持多种常见字段命名，方便 Unity/OpenXR/Python 调试脚本接入同一节点。
         if isinstance(pose, Mapping):
             pos = pose.get("position", pose.get("pos", pose.get("p")))
             quat = pose.get("orientation", pose.get("quat", pose.get("q", [0.0, 0.0, 0.0, 1.0])))

xr_rm_teleop/setup.py

@@ -1,3 +1,9 @@
+"""xr_rm_teleop 包安装配置。
+
+该包提供基于 XR 相对位移的 RM75 笛卡尔速度遥操作节点，以及把
+手柄 trigger 映射到夹爪力控命令的桥接节点。
+"""
+
 from setuptools import setup
 
 package_name = "xr_rm_teleop"

xr_rm_teleop/xr_rm_teleop/__init__.py

@@ -1 +1,4 @@
+"""XR 到 RM75 遥操作节点包。
 
+核心模块包括单臂速度遥操作、RealMan SDK 适配层和夹爪 trigger 桥接。
+"""

xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py

@@ -1,3 +1,9 @@
+"""手柄 trigger 到夹爪力控指令的桥接节点。
+
+订阅 XR 手柄消息，将 trigger 压力映射为 OmniPicker 可使用的归一化
+闭合力比例，并在消息超时或未握持时自动回到开爪指令。
+"""
+
 from __future__ import annotations
 
 import rclpy
@@ -62,6 +68,7 @@ class GripperTriggerBridge(Node):
     def _publish_command(self) -> None:
         command = self._open_command
 
+        # 手柄消息超时后立即退回开爪，避免夹爪在通信中断时保持闭合力。
         if self._last_msg is not None and self._last_msg_time is not None:
             age = (self.get_clock().now() - self._last_msg_time).nanoseconds * 1e-9
             if age <= self._stale_timeout_sec:

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -1,5 +1,12 @@
+"""RM75 机械臂适配层。
+
+对上提供统一的当前位姿读取、笛卡尔速度发送和停止接口；对下根据配置
+选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
+"""
+
 from __future__ import annotations
 
+import time
 from dataclasses import dataclass
 from numbers import Number
 from typing import Any
@@ -72,6 +79,9 @@ class RealManAdapter:
         initial_joint_pose: list[float] | None = None,
         initial_tcp_pose: list[float] | None = None,
         init_move_speed: int = 20,
+        command_mode: str = "velocity",
+        canfd_trajectory_mode: int = 2,
+        canfd_radio: int = 0,
     ) -> None:
         self._robot_ip = robot_ip
         self._robot_port = robot_port
@@ -90,7 +100,12 @@ class RealManAdapter:
         self._initial_joint_pose = initial_joint_pose
         self._initial_tcp_pose = initial_tcp_pose
         self._init_move_speed = init_move_speed
+        self._command_mode = command_mode
+        self._canfd_trajectory_mode = canfd_trajectory_mode
+        self._canfd_radio = canfd_radio
         self._arm: Any | None = None
+        if self._command_mode not in ("velocity", "pose_canfd"):
+            raise ValueError("command_mode must be one of: velocity, pose_canfd")
 
     def connect(self) -> None:
         try:
@@ -103,17 +118,21 @@ class RealManAdapter:
         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}, dt_ms={self._dt_ms}, "
+            f"command_mode={self._command_mode}"
+        )
         if self._configure_safety_limits:
             self._apply_safety_limits()
         if self._move_to_initial_pose_on_connect:
             self._move_to_initial_pose()
-        # 速度透传初始化必须和控制循环周期一致，避免真实机械臂出现周期不稳定。
+        if self._command_mode == "velocity":
-        ret = self._arm.rm_set_movev_canfd_init(
+            self._init_movev_canfd()
-            self._avoid_singularity,
+        else:
-            self._frame_type,
+            self._log_info("command_mode=pose_canfd，跳过 rm_set_movev_canfd_init。")
-            self._dt_ms,
-        )
-        self._check_return(ret, "rm_set_movev_canfd_init")
 
     def get_current_pose(self) -> ArmPose:
         self._require_arm()
@@ -125,13 +144,31 @@ class RealManAdapter:
 
     def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None:
         self._require_arm()
+        if self._command_mode == "pose_canfd":
+            return
         ret = self._arm.rm_movev_canfd(velocity, follow, 0, 0)
         self._check_return(ret, "rm_movev_canfd")
 
+    def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
+        self._require_arm()
+        if self._command_mode != "pose_canfd":
+            raise RuntimeError("send_cartesian_target requires command_mode=pose_canfd")
+        ret = self._arm.rm_movep_canfd(
+            [pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz],
+            follow,
+            self._canfd_trajectory_mode,
+            self._canfd_radio,
+        )
+        self._check_return(ret, "rm_movep_canfd")
+
+    def uses_pose_targets(self) -> bool:
+        return self._command_mode == "pose_canfd"
+
     def stop(self) -> None:
         if self._arm is None:
             return
         try:
+            if self._command_mode == "velocity":
                 self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0)
             self._arm.rm_set_arm_slow_stop()
         except Exception:
@@ -151,7 +188,8 @@ class RealManAdapter:
             raise RuntimeError("睿尔曼机械臂尚未连接")
 
     def _apply_safety_limits(self) -> None:
-        self._try_call("rm_set_avoid_singularity_mode", True)
+        # 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。
+        self._try_call("rm_set_avoid_singularity_mode", int(self._avoid_singularity))
         self._try_call("rm_set_arm_max_line_speed", self._max_line_speed)
         self._try_call("rm_set_arm_max_angular_speed", self._max_angular_speed)
         self._try_call("rm_set_arm_max_line_acc", self._max_line_acc)
@@ -169,6 +207,46 @@ class RealManAdapter:
         ret = self._arm.rm_movel(self._initial_tcp_pose, self._init_move_speed, 0, 0, 1)
         self._check_return(ret, "rm_movel(initial_tcp_pose)")
 
+    def _init_movev_canfd(self) -> None:
+        attempts = 3
+        candidates = [self._avoid_singularity]
+        if self._avoid_singularity != 0:
+            candidates.append(0)
+
+        # 某些现场控制器初始化避奇异模式会超时，失败后自动降级到 0 再重试。
+        last_ret: Any = None
+        for avoid_singularity in candidates:
+            if avoid_singularity != self._avoid_singularity:
+                self._log_warn("rm_set_movev_canfd_init 降级为 avoid_singularity=0 后重试。")
+
+            for attempt in range(1, attempts + 1):
+                ret = self._arm.rm_set_movev_canfd_init(
+                    avoid_singularity,
+                    self._frame_type,
+                    self._dt_ms,
+                )
+                last_ret = ret
+                code = self._return_code(ret)
+                if code == 0:
+                    self._avoid_singularity = avoid_singularity
+                    self._log_info(
+                        "rm_set_movev_canfd_init 成功："
+                        f"avoid_singularity={avoid_singularity}, "
+                        f"frame_type={self._frame_type}, dt_ms={self._dt_ms}"
+                    )
+                    return
+
+                self._log_warn(
+                    "rm_set_movev_canfd_init 失败："
+                    f"avoid_singularity={avoid_singularity}, "
+                    f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, "
+                    f"attempt={attempt}/{attempts}, ret={ret!r}"
+                )
+                if attempt < attempts:
+                    time.sleep(0.2)
+
+        self._check_return(last_ret, "rm_set_movev_canfd_init")
+
     def _try_call(self, name: str, *args: Any) -> None:
         func = getattr(self._arm, name, None)
         if func is None:
@@ -185,18 +263,28 @@ class RealManAdapter:
         if self._logger is not None:
             self._logger.warn(message)
 
-    @staticmethod
+    def _log_info(self, message: str) -> None:
-    def _check_robot_handle(handle: Any) -> None:
+        if self._logger is not None:
+            self._logger.info(message)
+
+    def _check_robot_handle(self, handle: Any) -> None:
         handle_id = getattr(handle, "id", None)
         if handle_id == -1:
-            raise RuntimeError("rm_create_robot_arm failed: socket error or robot unreachable")
+            raise RuntimeError(
+                "rm_create_robot_arm failed: TCP may be reachable, but the RealMan "
+                f"controller did not return robot info from {self._robot_ip}:{self._robot_port}"
+            )
 
     @staticmethod
     def _check_return(ret: Any, name: str) -> None:
-        code = ret[0] if isinstance(ret, tuple) and ret else ret
+        code = RealManAdapter._return_code(ret)
         if isinstance(code, int) and code != 0:
             raise RuntimeError(f"{name} failed with code {code}: {ret!r}")
 
+    @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 位姿字段。

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -1,3 +1,9 @@
+"""RM75 单臂 XR 相对位移遥操作节点。
+
+节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位移
+映射到机器人坐标系中的目标 TCP，并通过笛卡尔速度命令跟随该目标。
+"""
+
 from __future__ import annotations
 
 import math
@@ -60,6 +66,9 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("initial_joint_pose", [0.0] * 7)
         self.declare_parameter("initial_tcp_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
         self.declare_parameter("init_move_speed", 20)
+        self.declare_parameter("command_mode", "velocity")
+        self.declare_parameter("canfd_trajectory_mode", 2)
+        self.declare_parameter("canfd_radio", 0)
         self.declare_parameter("debug_topic_prefix", "/xr_rm")
 
         self._arm_name = str(self.get_parameter("arm_name").value)
@@ -107,6 +116,7 @@ class SingleArmVelocityTeleop(Node):
         self.get_logger().info(f"{self._arm_name} 速度遥操节点已启动，监听话题：{topic}")
 
     def _make_adapter(self):
+        # mock 和真机共享同一控制逻辑，只在适配层切换运动命令的执行方式。
         if self._bool_parameter("use_mock"):
             return MockRealManAdapter(
                 [float(v) for v in self.get_parameter("mock_initial_pose").value],
@@ -131,6 +141,9 @@ class SingleArmVelocityTeleop(Node):
             initial_joint_pose=self._float_list_parameter("initial_joint_pose", 7),
             initial_tcp_pose=self._float_list_parameter("initial_tcp_pose", 6),
             init_move_speed=int(self.get_parameter("init_move_speed").value),
+            command_mode=str(self.get_parameter("command_mode").value),
+            canfd_trajectory_mode=int(self.get_parameter("canfd_trajectory_mode").value),
+            canfd_radio=int(self.get_parameter("canfd_radio").value),
         )
 
     def _on_controller(self, msg: XrController) -> None:
@@ -206,6 +219,23 @@ class SingleArmVelocityTeleop(Node):
         ]
         cartesian_velocity = self._filtered_velocity + [0.0, 0.0, 0.0]
         self._publish_debug(robot_pose, target, cartesian_velocity)
+        if self._adapter_uses_pose_targets():
+            step_target = [
+                robot_now[i] + cartesian_velocity[i] * self._dt
+                for i in range(3)
+            ]
+            step_target = self._clamp_workspace(step_target)
+            self._send_cartesian_target(
+                ArmPose(
+                    x=step_target[0],
+                    y=step_target[1],
+                    z=step_target[2],
+                    rx=robot_pose.rx,
+                    ry=robot_pose.ry,
+                    rz=robot_pose.rz,
+                )
+            )
+        else:
             self._send_cartesian_velocity(cartesian_velocity)
 
     @staticmethod
@@ -221,6 +251,7 @@ class SingleArmVelocityTeleop(Node):
         ]
 
     def _clamp_workspace(self, target: list[float]) -> list[float]:
+        # 先做盒状工作空间限幅，再叠加底座圆柱半径限制。
         clamped = [
             _clamp(target[i], self._workspace_min[i], self._workspace_max[i])
             for i in range(3)
@@ -259,6 +290,8 @@ class SingleArmVelocityTeleop(Node):
     def _safe_stop(self) -> None:
         # 所有断连、松手、退出路径都走这里，确保发送零速度。
         self._filtered_velocity = [0.0, 0.0, 0.0]
+        if self._adapter_uses_pose_targets():
+            return
         self._send_cartesian_velocity([0.0] * 6)
 
     def _send_cartesian_velocity(self, velocity: list[float]) -> None:
@@ -271,6 +304,20 @@ class SingleArmVelocityTeleop(Node):
             )
             self._active = False
 
+    def _send_cartesian_target(self, pose: ArmPose) -> None:
+        try:
+            self._adapter.send_cartesian_target(pose, self._follow)
+        except Exception as exc:
+            self.get_logger().error(
+                f"{self._arm_name} 发送位姿透传命令失败：{exc}",
+                throttle_duration_sec=1.0,
+            )
+            self._active = False
+
+    def _adapter_uses_pose_targets(self) -> bool:
+        uses_pose_targets = getattr(self._adapter, "uses_pose_targets", None)
+        return bool(uses_pose_targets is not None and uses_pose_targets())
+
     def _publish_debug(self, current_pose: ArmPose, target_xyz: list[float], velocity: list[float]) -> None:
         stamp = self.get_clock().now().to_msg()
         current_msg = self._pose_msg(stamp, current_pose)