Add Placo IK solver and associated tests.

xr_rm_bringup/launch/arm_debug.launch.py

@@ -5,6 +5,8 @@
 手柄接收节点，再根据 `arm:=left|right|both` 选择对应的遥操作节点。
 """
 
+from pathlib import Path
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
@@ -12,6 +14,9 @@ from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
 
+XR_PYTHON = "/home/robot/miniconda3/envs/xr/bin/python"
+
+
 def _as_bool(value: str) -> bool:
     """把 launch 字符串参数转换成 Python bool，便于在 OpaqueFunction 中分支。"""
     return value.strip().lower() in ("1", "true", "yes", "on")
@@ -26,6 +31,15 @@ def _config_file(name: str) -> PathJoinSubstitution:
     ])
 
 
+def _rm75_urdf() -> PathJoinSubstitution:
+    return PathJoinSubstitution([
+        FindPackageShare("xr_rm_teleop"),
+        "models",
+        "rm75",
+        "RM75-B.urdf",
+    ])
+
+
 def _initial_pose_override(value: str) -> dict[str, bool]:
     return {} if value == "auto" else {"move_to_initial_pose_on_connect": _as_bool(value)}
 
@@ -52,7 +66,6 @@ def _single_arm_node(
     use_mock: bool,
     move_to_initial_pose: str,
     avoid_singularity: int,
-    frame_type: int,
     control_rate_hz: float,
     follow: bool,
     configure_safety_limits: bool,
@@ -70,6 +83,7 @@ def _single_arm_node(
         executable="single_arm_velocity_teleop",
         name="single_arm_velocity_teleop",
         output="screen",
+        prefix=[XR_PYTHON],
         parameters=[
             _config_file(config_name),
             {
@@ -77,7 +91,7 @@ def _single_arm_node(
                 "robot_ip": robot_ip,
                 "robot_port": LaunchConfiguration("robot_port"),
                 "avoid_singularity": avoid_singularity,
-                "frame_type": frame_type,
+                "robot_urdf_path": _rm75_urdf(),
                 "control_rate_hz": control_rate_hz,
                 "follow": follow,
                 "configure_safety_limits": configure_safety_limits,
@@ -103,7 +117,6 @@ def _dual_arm_nodes(
     move_to_initial_pose: str,
     left_avoid_singularity: int,
     right_avoid_singularity: int,
-    frame_type: int,
     control_rate_hz: float,
     follow: bool,
     configure_safety_limits: bool,
@@ -120,6 +133,7 @@ def _dual_arm_nodes(
             executable="single_arm_velocity_teleop",
             name="left_arm_teleop",
             output="screen",
+            prefix=[XR_PYTHON],
             parameters=[
                 config_file,
                 {
@@ -127,7 +141,7 @@ def _dual_arm_nodes(
                     "robot_ip": LaunchConfiguration("left_robot_ip"),
                     "robot_port": LaunchConfiguration("robot_port"),
                     "avoid_singularity": left_avoid_singularity,
-                    "frame_type": frame_type,
+                    "robot_urdf_path": _rm75_urdf(),
                     "control_rate_hz": control_rate_hz,
                     "follow": follow,
                     "configure_safety_limits": configure_safety_limits,
@@ -147,6 +161,7 @@ def _dual_arm_nodes(
             executable="single_arm_velocity_teleop",
             name="right_arm_teleop",
             output="screen",
+            prefix=[XR_PYTHON],
             parameters=[
                 config_file,
                 {
@@ -154,7 +169,7 @@ def _dual_arm_nodes(
                     "robot_ip": LaunchConfiguration("right_robot_ip"),
                     "robot_port": LaunchConfiguration("robot_port"),
                     "avoid_singularity": right_avoid_singularity,
-                    "frame_type": frame_type,
+                    "robot_urdf_path": _rm75_urdf(),
                     "control_rate_hz": control_rate_hz,
                     "follow": follow,
                     "configure_safety_limits": configure_safety_limits,
@@ -175,6 +190,11 @@ def _dual_arm_nodes(
 def _launch_setup(context, *args, **kwargs):
     """运行时读取 launch 参数，决定启动单臂还是双臂。"""
     del args, kwargs
+    if not Path(XR_PYTHON).is_file():
+        raise RuntimeError(
+            f"XR Python not found: {XR_PYTHON}; "
+            "Placo 0.9.4 must not be installed globally"
+        )
     arm = LaunchConfiguration("arm").perform(context).strip().lower()
     use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
     move_to_initial_pose = LaunchConfiguration(
@@ -187,7 +207,6 @@ def _launch_setup(context, *args, **kwargs):
     right_avoid_singularity = int(
         avoid_override or LaunchConfiguration("right_avoid_singularity").perform(context)
     )
-    frame_type = int(LaunchConfiguration("frame_type").perform(context))
     control_rate_hz = float(LaunchConfiguration("control_rate_hz").perform(context))
     follow = _as_bool(LaunchConfiguration("follow").perform(context))
     configure_safety_limits = _as_bool(
@@ -217,7 +236,6 @@ def _launch_setup(context, *args, **kwargs):
                 move_to_initial_pose,
                 left_avoid_singularity,
                 right_avoid_singularity,
-                frame_type,
                 control_rate_hz,
                 follow,
                 configure_safety_limits,
@@ -235,7 +253,6 @@ def _launch_setup(context, *args, **kwargs):
                 use_mock,
                 move_to_initial_pose,
                 avoid_singularity,
-                frame_type,
                 control_rate_hz,
                 follow,
                 configure_safety_limits,
@@ -268,8 +285,7 @@ def generate_launch_description() -> LaunchDescription:
         DeclareLaunchArgument("right_avoid_singularity", default_value="1"),
         # 非空时作为左右臂全局覆盖，例如 avoid_singularity:=0。
         DeclareLaunchArgument("avoid_singularity", default_value=""),
-        DeclareLaunchArgument("frame_type", default_value="1"),
-        # 现场调参入口：默认按 PICO 90Hz 输入节奏发送 rm_movep_canfd。
+        # 每周期同步一次实际关节反馈、执行一次 Placo QP，再发送 rm_movej_canfd。
         DeclareLaunchArgument("control_rate_hz", default_value="90.0"),
         # 默认低跟随；高跟随请确认控制器和网络能稳定满足厂商周期要求后再打开。
         DeclareLaunchArgument("follow", default_value="false"),