Add Placo IK solver and associated tests.

README.md

@@ -7,11 +7,12 @@ PICO/XR 双手柄 UDP JSON
   -> xr_rm_input/udp_controller_receiver
   -> /xr/left_controller 与 /xr/right_controller
   -> xr_rm_teleop/single_arm_velocity_teleop
-  -> 左右 RM75 笛卡尔相对位姿透传控制
+  -> Placo QP 单步逆解
+  -> 左右 RM75 七关节角透传控制
   -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
 ```
 
-当前控制方式是“手柄相对位姿透传”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP，经过工作空间限幅、目标低通、姿态低通和单帧步长限制后，通过 `rm_movep_canfd` 下发目标位姿。松开 `grip`、UDP 超时或节点退出时会请求机械臂慢停。
+当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP，经过工作空间限幅、目标低通、姿态低通和单帧步长限制后，每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip`、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。
 
 ## 当前范围
 
@@ -19,7 +20,8 @@ PICO/XR 双手柄 UDP JSON
 
 - PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
 - 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
-- RM75 真机连接适配，包含 `rm_movep_canfd` 位姿透传、安全速度/加速度配置、可选初始化点位移动。
+- RM75 真机连接适配，包含关节反馈缓存、`rm_movej_canfd` 关节透传、安全速度/加速度配置、可选初始化点位移动。
+- Placo 0.9.4 RM75 QP 逆解；收到首帧有效关节反馈后才启用，求解失败时保留上一组有效关节目标。
 - 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
 - Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
 - 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip`、`trigger` 和 pose 诊断字段。
@@ -67,7 +69,9 @@ src/
 │   └── msg/
 │       └── XrController.msg        # hand/grip/trigger/pose
 └── xr_rm_teleop/
+    ├── models/rm75/                # RM75 URDF 与网格
     └── xr_rm_teleop/
+        ├── placo_ik_solver.py      # Placo 0.9.4 单步 QP 逆解
         ├── single_arm_velocity_teleop.py
         ├── realman_adapter.py
         └── fun_peripheral.py
@@ -90,6 +94,17 @@ source install/setup.bash
 
 真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。
 
+遥操作节点固定由 `/home/robot/miniconda3/envs/xr/bin/python` 启动，并复用其中的 Python 3.10、Placo 0.9.4、Pinocchio 3.7.0 和 NumPy 2.2.6。`ros2`、`colcon` 和 `udp_controller_receiver` 仍使用系统 Python。禁止通过 `pip --user`、`sudo pip` 或系统安装升级 Placo、Pinocchio、EigenPy 和 NumPy。
+
+只读检查 Placo 版本：
+
+```bash
+/home/robot/miniconda3/envs/xr/bin/python -c \
+  "import importlib.metadata; print(importlib.metadata.version('placo'))"
+```
+
+输出必须为 `0.9.4`。
+
 如果希望 `launcher_ui.py` 从任意目录找到工作空间，可以设置：
 
 ```bash
@@ -150,17 +165,25 @@ sudo update-alternatives --config x-terminal-emulator
 
 打开 `launcher_ui.py`，点击 `Check Env`。如果 `install/setup.bash` 缺失，先回工作空间根目录重新执行 `colcon build --symlink-install`。
 
-第二步：跑 mock 闭环。
+第二步：分别跑左、右臂 mock 闭环。
 
-在 `Simulation` 模式运行 `One-Click Dual Mock Demo`，或分开运行：
+分两个终端依次验证左臂：
 
 ```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
-ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 \
+ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port 15000 \
-  --pattern axis_sweep --seconds 60 --both-mode staggered
+  --pattern axis_sweep --seconds 30
 ```
 
-`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`。
+停止左臂进程后，再分别验证右臂：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
+ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port 15000 \
+  --pattern axis_sweep --seconds 30
+```
+
+`sample_udp_sender` 默认使用 `axis_sweep` 扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。
 
 观察：
 
@@ -182,8 +205,8 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
 ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
 ```
 
-单臂真机默认执行配置文件中的 `movej(initial_joint_pose)`；现场需要跳过时，显式传入
+所有配置默认都不会执行 `movej(initial_joint_pose)`；只有确认安全区清空后，才可显式传入
-`move_to_initial_pose_on_connect:=false`。
+`move_to_initial_pose_on_connect:=true`。
 
 第四步：双臂真机。
 
@@ -216,15 +239,14 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `robot_port`：RM75 TCP 端口，默认 `8080`。
 - `left_avoid_singularity` / `right_avoid_singularity`：左右臂避奇异参数，默认左 `0`、右 `1`。
 - `avoid_singularity`：非空时覆盖左右臂避奇异参数。
-- `frame_type`：`rm_movep_canfd` 坐标系类型，默认 `1`。
+- `control_rate_hz`：同步关节反馈、执行一次 QP 并发送一次关节目标的频率，默认 `90.0`。
-- `control_rate_hz`：`rm_movep_canfd` 目标位姿发送频率，默认 `90.0`。
+- `follow`：传给 `rm_movej_canfd` 的跟随标志，默认 `false`。
-- `follow`：传给 `rm_movep_canfd` 的跟随标志，默认 `false`。
 - `configure_safety_limits`：连接真机后是否配置速度/加速度安全参数，默认 `true`。
 - `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。
 - `enable_trigger_gripper_control`：是否允许用 `trigger` 点击切换对应夹爪状态，默认 `true`。
 - `trigger_close_threshold`：trigger 点击判定阈值，默认 `0.95`。
 - `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
-- `move_to_initial_pose_on_connect`：连接后是否执行 `movej(initial_joint_pose)`；默认 `auto`，单臂配置启用、双臂配置禁用，也可显式传 `true`/`false` 覆盖。
+- `move_to_initial_pose_on_connect`：连接后是否执行 `movej(initial_joint_pose)`；默认 `auto`，沿用 YAML 中的 `false`，也可显式传 `true`/`false` 覆盖。
 
 ## 配置文件说明
 
@@ -235,7 +257,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 
 `left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。
 
-`xr_rm_bringup/config/peripherals_rm75.yaml` 保存末端工具坐标、负载和左右臂外设选择。当前配置为左臂 `scissorgripper=2`、右臂 `scissorgripper=1`，真机连接阶段会初始化外设，后续开合命令复用同一个 RealMan 连接。
+`xr_rm_bringup/config/peripherals_rm75.yaml` 保存末端工具坐标、负载和左右臂外设选择。当前左臂使用 `minisci`（沿工具局部 Z 偏移 `0.19 m`），右臂使用 `omnipic`（沿工具局部 Z 偏移 `0.16 m`）。URDF 仍只建模到法兰 `link_7`；QP 在 TCP 目标与法兰目标之间应用完整工具刚体变换。真机连接阶段会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 RealMan 连接。
 
 重点控制参数：
 
@@ -250,9 +272,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
 - `cyl_radius_limit`：基座圆柱半径限制。
 - `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
-- `current_pose_poll_hz`：低频读取真机当前 TCP 的频率；控制中不再每帧阻塞读取状态。
+- `initial_joint_pose`：mock 的初始关节反馈，以及显式开启初始化移动时的真机初始关节角。
-- `mock_initial_pose`：mock 模式初始 TCP 位姿。
-- `initial_joint_pose`：可选真机初始关节角。
 
 当前 `/xr/*_controller` 的 Project 坐标约定：
 

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -1,10 +1,10 @@
 # 阶段一：PICO 遥操作双 RM75 平台配置。
 #
-# 当前控制方式是“相对位姿透传”：
+# 当前控制方式是“相对 TCP + 单步 QP”：
 # 按下 grip 时锁定当前手柄位姿和 TCP 位姿，之后将手柄相对位移和相对旋转
 # 映射为目标 TCP 位姿，经过工作空间限幅、目标低通、姿态低通和单帧步长
-# 限制后，通过 rm_movep_canfd 下发。cmd_vel 仅作为目标位姿变化率调试话题，
+# 限制后，通过 Placo 单步 QP 和 rm_movej_canfd 下发 7 个关节目标。
-# 不是机械臂执行命令。
+# cmd_vel 仅作为目标位姿变化率调试话题，不是机械臂执行命令。
 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。
 
 left_arm_teleop:
@@ -27,8 +27,6 @@ left_arm_teleop:
     orientation_deadband_rad: 0.005
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.6
-    current_pose_poll_hz: 10.0
-
     workspace_min: [-0.70, -0.60, 0.10]
     workspace_max: [0.70, 0.40, 0.70]
     cyl_radius_limit: [0.20, 0.60]
@@ -42,11 +40,9 @@ left_arm_teleop:
                          -1.0, 0.0, 0.0]
 
     use_mock: false
-    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: 0
-    frame_type: 1
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0
@@ -81,8 +77,6 @@ right_arm_teleop:
     orientation_deadband_rad: 0.005
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.6
-    current_pose_poll_hz: 10.0
-
     workspace_min: [-0.70, -0.60, 0.10]
     workspace_max: [0.70, 0.40, 0.70]
     cyl_radius_limit: [0.20, 0.60]
@@ -96,11 +90,9 @@ right_arm_teleop:
                          1.0, 0.0, 0.0]
 
     use_mock: false
-    mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
     robot_ip: 192.168.192.19
     robot_port: 8080
     avoid_singularity: 1
-    frame_type: 1
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -1,4 +1,4 @@
-# 左臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。
+# 左臂单独调试配置：XR TCP 目标经 Placo QP 转换为 RM75 关节目标。
 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。
 
 single_arm_velocity_teleop:
@@ -21,8 +21,6 @@ single_arm_velocity_teleop:
     orientation_deadband_rad: 0.005
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.6
-    current_pose_poll_hz: 10.0
-
     workspace_min: [-0.70, -0.60, 0.10]
     workspace_max: [0.70, 0.40, 0.70]
     cyl_radius_limit: [0.20, 0.60]
@@ -35,11 +33,9 @@ single_arm_velocity_teleop:
                          -1.0, 0.0, 0.0]
 
     use_mock: false
-    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: 0
-    frame_type: 1
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0
@@ -50,7 +46,7 @@ single_arm_velocity_teleop:
     max_angular_acc: 2.0
     joint_max_speed: 180.0
     joint_max_acc: 180.0
-    move_to_initial_pose_on_connect: true
+    move_to_initial_pose_on_connect: false
     initial_joint_pose: [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52]
     init_move_speed: 20
     debug_topic_prefix: /xr_rm

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -1,4 +1,4 @@
-# 右臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。
+# 右臂单独调试配置：XR TCP 目标经 Placo QP 转换为 RM75 关节目标。
 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。
 
 single_arm_velocity_teleop:
@@ -20,8 +20,6 @@ single_arm_velocity_teleop:
     orientation_deadband_rad: 0.005
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.5
-    current_pose_poll_hz: 10.0
-
     workspace_min: [-0.60, -0.60, 0.10]
     workspace_max: [0.60, 0.70, 0.55]
     cyl_radius_limit: [0.10, 0.70]
@@ -34,22 +32,20 @@ single_arm_velocity_teleop:
                          1.0, 0.0, 0.0]
 
     use_mock: false
-    mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
     robot_ip: 192.168.192.19
     robot_port: 8080
     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_line_speed: 0.25
-    max_angular_speed: 1.5
+    max_angular_speed: 0.6
-    max_line_acc: 1.0
+    max_line_acc: 1.3
-    max_angular_acc: 2.0
+    max_angular_acc: 3.0
     joint_max_speed: 180.0
-    joint_max_acc: 180.0
+    joint_max_acc: 300.0
-    move_to_initial_pose_on_connect: true
+    move_to_initial_pose_on_connect: True
     initial_joint_pose: [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04]
     init_move_speed: 20
     debug_topic_prefix: /xr_rm

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"),
+        # 每周期同步一次实际关节反馈、执行一次 Placo QP，再发送 rm_movej_canfd。
-        # 现场调参入口：默认按 PICO 90Hz 输入节奏发送 rm_movep_canfd。
         DeclareLaunchArgument("control_rate_hz", default_value="90.0"),
         # 默认低跟随；高跟随请确认控制器和网络能稳定满足厂商周期要求后再打开。
         DeclareLaunchArgument("follow", default_value="false"),

xr_rm_teleop/models/rm75/RM75-B.urdf

@@ -0,0 +1,453 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-1-g15f4949  Build Version: 1.6.7594.29634
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="RM75-B">
+  <link
+    name="base_link">
+    <inertial>
+      <origin
+        xyz="0.00049987 5.2709E-05 0.060019"
+        rpy="0 0 0" />
+      <mass
+        value="1.862" />
+      <inertia
+        ixx="0.0017232"
+        ixy="-3.1058E-06"
+        ixz="-3.7924E-05"
+        iyy="0.0017051"
+        iyz="1.3691E-06"
+        izz="0.00090158" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="link_1">
+    <inertial>
+      <origin
+        xyz="0.000241 -0.013273 -0.00995"
+        rpy="0 0 0" />
+      <mass
+        value="1.574" />
+      <inertia
+        ixx="0.002487573"
+        ixy="0.000009663"
+        ixz="-0.000007909"
+        iyy="0.002321038"
+        iyz="0.000179393"
+        izz="0.001450554" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_1"
+    type="revolute">
+    <origin
+      xyz="0 0 0.2405"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="60"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_2">
+    <inertial>
+      <origin
+        xyz="-0.000357 -0.106789 0.005329"
+        rpy="0 0 0" />
+      <mass
+        value="1.217" />
+      <inertia
+        ixx="0.003494121"
+        ixy="0.000002921"
+        ixz="-0.000005613"
+        iyy="0.000892721"
+        iyz="-0.000583884"
+        izz="0.003444080" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_2"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_1" />
+    <child
+      link="link_2" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.2689"
+      upper="2.2689"
+      effort="60"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_3">
+    <inertial>
+      <origin
+        xyz="0.000003 -0.01398 -0.011324"
+        rpy="0 0 0" />
+      <mass
+        value="1.11" />
+      <inertia
+        ixx="0.001836663"
+        ixy="0.000002259"
+        ixz="-0.000004216"
+        iyy="0.001498875"
+        iyz="0.000037167"
+        izz="0.001062545" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_3"
+    type="revolute">
+    <origin
+      xyz="0 -0.256 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_2" />
+    <child
+      link="link_3" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="30"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_4">
+    <inertial>
+      <origin
+        xyz="-0.000005 -0.084658 0.004747"
+        rpy="0 0 0" />
+      <mass
+        value="0.685" />
+      <inertia
+        ixx="0.001282444"
+        ixy="-0.000000551"
+        ixz="-0.000000630"
+        iyy="0.000373013"
+        iyz="-0.000232084"
+        izz="0.001256177" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_4"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_3" />
+    <child
+      link="link_4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.356"
+      upper="2.356"
+      effort="30"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_5">
+    <inertial>
+      <origin
+        xyz="0.000078 -0.012937 -0.008781"
+        rpy="0 0 0" />
+      <mass
+        value="0.619" />
+      <inertia
+        ixx="0.000627336"
+        ixy="0.000001636"
+        ixz="-0.000001345"
+        iyy="0.000542455"
+        iyz="0.000034970"
+        izz="0.000370291" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_5"
+    type="revolute">
+    <origin
+      xyz="0 -0.21 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_4" />
+    <child
+      link="link_5" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_6">
+    <inertial>
+      <origin
+        xyz="-0.000014 -0.078524 0.002819"
+        rpy="0 0 0" />
+      <mass
+        value="0.602" />
+      <inertia
+        ixx="0.000780774"
+        ixy="-0.000000121"
+        ixz="-0.000000469"
+        iyy="0.000289973"
+        iyz="-0.000120513"
+        izz="0.000763955" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_6"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_5" />
+    <child
+      link="link_6" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.234"
+      upper="2.234"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_7">
+    <inertial>
+      <origin
+        xyz="0.001094 -0.000077 -0.010119"
+        rpy="0 0 0" />
+      <mass
+        value="0.107" />
+      <inertia
+        ixx="0.000044123"
+        ixy="-0.000000064"
+        ixz="0.0000003"
+        iyy="0.000035078"
+        iyz="-0.000000029"
+        izz="0.000065445" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_7.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_7"
+    type="revolute">
+    <origin
+      xyz="0 -0.144 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_6" />
+    <child
+      link="link_7" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-6.28"
+      upper="6.28"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+</robot>

xr_rm_teleop/setup.py

@@ -1,8 +1,10 @@
 """xr_rm_teleop 包安装配置。
 
-该包提供基于 XR 相对位姿的 RM75 笛卡尔位姿透传遥操作节点。
+该包提供基于 XR 相对位姿和 Placo QP 的 RM75 遥操作节点。
 """
 
+from glob import glob
+
 from setuptools import setup
 
 package_name = "xr_rm_teleop"
@@ -14,6 +16,14 @@ setup(
     data_files=[
         ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
         (f"share/{package_name}", ["package.xml"]),
+        (
+            f"share/{package_name}/models/rm75",
+            ["models/rm75/RM75-B.urdf"],
+        ),
+        (
+            f"share/{package_name}/models/rm75/meshes",
+            glob("models/rm75/meshes/*.STL"),
+        ),
     ],
     install_requires=["setuptools"],
     zip_safe=True,

xr_rm_teleop/test/placo_ik_smoke.py

@@ -0,0 +1,79 @@
+from __future__ import annotations
+
+import math
+import sys
+import time
+from pathlib import Path
+
+import numpy as np
+
+from xr_rm_teleop.placo_ik_solver import PlacoIkSolver
+from xr_rm_teleop.realman_adapter import ArmPose
+
+
+CASES = {
+    "left": (
+        [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
+        [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],
+    ),
+    "right": (
+        [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
+        [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],
+    ),
+}
+
+
+def angle_error(actual: list[float], target: list[float]) -> float:
+    deltas = [
+        math.atan2(math.sin(a - b), math.cos(a - b))
+        for a, b in zip(actual, target)
+    ]
+    return math.sqrt(sum(value * value for value in deltas))
+
+
+def main() -> None:
+    urdf_path = Path(sys.argv[1]).resolve()
+    for arm, (joint_degrees, tool_pose) in CASES.items():
+        solver = PlacoIkSolver(str(urdf_path), tool_pose, 1.0 / 90.0)
+        joints = np.deg2rad(joint_degrees).tolist()
+        current = solver.update_joint_state(joints)
+        target = ArmPose(
+            current.x + 0.01,
+            current.y,
+            current.z,
+            current.rx,
+            current.ry,
+            current.rz + 0.05,
+        )
+
+        solve_durations = []
+        for _ in range(45):
+            solver.update_joint_state(joints)
+            started_at = time.perf_counter()
+            joints = solver.solve(target)
+            solve_durations.append(time.perf_counter() - started_at)
+
+        actual = solver.update_joint_state(joints)
+        position_error = np.linalg.norm(
+            np.asarray(actual.xyz()) - np.asarray(target.xyz())
+        )
+        orientation_error = angle_error(actual.rpy(), target.rpy())
+        assert len(joints) == 7
+        assert np.isfinite(joints).all()
+        assert np.allclose(
+            solver.base_configuration,
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+        )
+        assert position_error <= 0.005
+        assert orientation_error <= math.radians(2.0)
+        print(
+            f"{arm}: position_error={position_error:.6f}m, "
+            f"orientation_error={math.degrees(orientation_error):.3f}deg, "
+            f"solve_avg={1000.0 * np.mean(solve_durations):.3f}ms, "
+            f"solve_max={1000.0 * max(solve_durations):.3f}ms, "
+            f"solve_overruns={sum(value > 1.0 / 90.0 for value in solve_durations)}"
+        )
+
+
+if __name__ == "__main__":
+    main()

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -1,4 +1,10 @@
+import math
+
+import pytest
+
 from xr_rm_teleop.realman_adapter import RealManAdapter
+from xr_rm_teleop.realman_adapter import MockRealManAdapter
+from xr_rm_teleop.fun_peripheral import PeripheralConfig
 
 
 def test_initial_pose_uses_joint_move_only() -> None:
@@ -17,3 +23,66 @@ def test_initial_pose_uses_joint_move_only() -> None:
     adapter._move_to_initial_pose()
 
     assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]
+
+
+def test_peripheral_config_exposes_selected_tool() -> None:
+    config = PeripheralConfig(
+        scissorgripper=1,
+        tools_in_ee={
+            "first": [[0.0] * 7, [0.0] * 7],
+            "second": [[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0], [0.0] * 7],
+        },
+    )
+
+    assert config.tool_name == "second"
+    assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
+
+
+def test_joint_feedback_is_cached_in_radians() -> None:
+    class FakeArm:
+        def rm_get_joint_degree(self):
+            return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
+
+    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
+    adapter._arm = FakeArm()
+
+    adapter._read_joint_state_once()
+    snapshot = adapter.get_latest_joint_state()
+
+    assert snapshot is not None
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
+    )
+
+
+def test_joint_target_uses_movej_canfd_in_degrees() -> None:
+    class FakeArm:
+        def __init__(self) -> None:
+            self.calls = []
+
+        def rm_movej_canfd(self, *args):
+            self.calls.append(args)
+            return 0
+
+    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
+    adapter._arm = FakeArm()
+    target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
+
+    adapter.send_joint_target(target, follow=False)
+
+    assert len(adapter._arm.calls) == 1
+    degrees, follow, expand, trajectory_mode, radio = adapter._arm.calls[0]
+    assert degrees == pytest.approx([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
+    assert (follow, expand, trajectory_mode, radio) == (False, 0, 2, 0)
+
+
+def test_mock_joint_feedback_is_available_without_vendor_sdk() -> None:
+    adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
+
+    adapter.connect()
+    snapshot = adapter.get_latest_joint_state()
+
+    assert snapshot is not None
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
+    )

xr_rm_teleop/test/test_joint_control.py

@@ -0,0 +1,164 @@
+import time
+from types import SimpleNamespace
+
+import pytest
+
+from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
+from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop
+
+
+class FakeLogger:
+    def warn(self, *args, **kwargs):
+        del args, kwargs
+
+    def error(self, *args, **kwargs):
+        del args, kwargs
+
+
+class FakeTime:
+    def __sub__(self, other):
+        del other
+        return SimpleNamespace(nanoseconds=0)
+
+
+def test_missing_or_stale_feedback_does_not_enable_qp() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._command_timeout_sec = 0.12
+    teleop._adapter = SimpleNamespace(get_latest_joint_state=lambda: None)
+
+    assert teleop._fresh_joint_state() is None
+
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.0] * 7,
+            time.monotonic() - 1.0,
+        )
+    )
+    assert teleop._fresh_joint_state() is None
+
+
+def test_stale_feedback_stops_before_active_control() -> None:
+    stopped = []
+    entered = []
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.0] * 7,
+            time.monotonic() - 1.0,
+        )
+    )
+    teleop._command_timeout_sec = 0.12
+    teleop._joint_feedback_ready = True
+    teleop._arm_name = "right_rm75"
+    teleop._last_msg = SimpleNamespace(
+        grip=True,
+        pose=SimpleNamespace(
+            position=SimpleNamespace(x=0.0, y=0.0, z=0.0),
+            orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0),
+        ),
+    )
+    teleop._last_msg_time = FakeTime()
+    teleop._active = False
+    teleop._enable_orientation_control = False
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
+    teleop._enter_active_control = lambda *args: entered.append(args)
+
+    teleop._control_tick()
+
+    assert stopped == [True]
+    assert entered == []
+
+
+def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
+    class FakeSolver:
+        def __init__(self) -> None:
+            self.solve_calls = 0
+
+        def update_joint_state(self, joints):
+            assert joints == [0.1] * 7
+            return ArmPose(0.3, 0.0, 0.2)
+
+        def solve(self, target):
+            del target
+            self.solve_calls += 1
+            return [0.2] * 7
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._ik_solver = FakeSolver()
+    teleop._active = False
+    teleop._last_valid_joint_target = None
+    teleop._last_current_pose = None
+
+    pose = teleop._sync_joint_feedback(
+        JointStateSnapshot([0.1] * 7, time.monotonic())
+    )
+
+    assert pose == ArmPose(0.3, 0.0, 0.2)
+    assert teleop._last_valid_joint_target == [0.1] * 7
+    assert teleop._ik_solver.solve_calls == 0
+
+
+def test_qp_failure_returns_last_known_good_target() -> None:
+    class FailingSolver:
+        def solve(self, target):
+            del target
+            raise RuntimeError("NaN in QP solution")
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._ik_solver = FailingSolver()
+    teleop._last_valid_joint_target = [0.1] * 7
+    teleop._arm_name = "right_rm75"
+    teleop.get_logger = lambda: FakeLogger()
+
+    target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
+
+    assert target == pytest.approx([0.1] * 7)
+    assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
+
+
+def test_qp_success_updates_last_known_good_target() -> None:
+    class SuccessfulSolver:
+        def solve(self, target):
+            del target
+            return [0.2] * 7
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._ik_solver = SuccessfulSolver()
+    teleop._last_valid_joint_target = [0.1] * 7
+    teleop._arm_name = "left_rm75"
+    teleop.get_logger = lambda: FakeLogger()
+
+    target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
+
+    assert target == pytest.approx([0.2] * 7)
+    assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
+
+
+def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
+    class FailingAdapter:
+        def __init__(self) -> None:
+            self.stop_calls = 0
+
+        def send_joint_target(self, joints, follow):
+            del joints, follow
+            raise RuntimeError("send failed")
+
+        def stop(self):
+            self.stop_calls += 1
+
+    reset_calls = []
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._adapter = FailingAdapter()
+    teleop._follow = False
+    teleop._arm_name = "left_rm75"
+    teleop._stop_sent = False
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._safe_stop = lambda reset_active: reset_calls.append(reset_active)
+
+    sent = teleop._send_joint_target([0.1] * 7)
+
+    assert not sent
+    assert teleop._adapter.stop_calls == 1
+    assert reset_calls == [True]

xr_rm_teleop/test/test_orientation_control.py

@@ -1,9 +1,10 @@
 import math
+import time
 from types import SimpleNamespace
 
 import pytest
 
-from xr_rm_teleop.realman_adapter import ArmPose, MockRealManAdapter
+from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
 from xr_rm_teleop.single_arm_velocity_teleop import (
     SingleArmVelocityTeleop,
     _euler_to_quaternion,
@@ -95,6 +96,19 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
     teleop._arm_name = "test_rm75"
     teleop._command_timeout_sec = 0.12
     teleop._enable_orientation_control = True
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.1] * 7,
+            time.monotonic(),
+        )
+    )
+    teleop._ik_solver = SimpleNamespace(
+        update_joint_state=lambda joints: ArmPose(0.3, 0.0, 0.2)
+    )
+    teleop._active = False
+    teleop._last_valid_joint_target = None
+    teleop._last_current_pose = None
+    teleop._joint_feedback_ready = True
     stopped = []
     teleop.get_clock = lambda: FakeClock()
     teleop.get_logger = lambda: FakeLogger()
@@ -113,12 +127,3 @@ def test_quaternion_roundtrip_for_small_rpy() -> None:
 def test_zero_quaternion_is_invalid() -> None:
     with pytest.raises(ValueError):
         _normalize_quaternion([0.0, 0.0, 0.0, 0.0])
-
-
-def test_mock_adapter_uses_shortest_angular_velocity() -> None:
-    adapter = MockRealManAdapter([0.0, 0.0, 0.0, 3.13, 0.0, -3.13], 0.1)
-
-    adapter.send_cartesian_target(ArmPose(0.0, 0.0, 0.0, -3.13, 0.0, 3.13), False)
-
-    assert abs(adapter.last_velocity[3]) < 1.0
-    assert abs(adapter.last_velocity[5]) < 1.0

xr_rm_teleop/test/test_placo_transforms.py

@@ -0,0 +1,49 @@
+import math
+
+import numpy as np
+import pytest
+
+from xr_rm_teleop.placo_ik_solver import (
+    PlacoIkSolver,
+    _arm_pose_to_transform,
+    _tool_pose_to_transform,
+    _transform_to_arm_pose,
+)
+from xr_rm_teleop.realman_adapter import ArmPose
+
+
+def test_tool_offset_rotates_with_flange_and_roundtrips() -> None:
+    flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0)
+    tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
+
+    base_to_flange = _arm_pose_to_transform(flange_pose)
+    flange_to_tool = _tool_pose_to_transform(tool_pose)
+    base_to_tool = base_to_flange @ flange_to_tool
+    recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool)
+
+    assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20])
+    assert recovered_flange == pytest.approx(base_to_flange)
+
+
+def test_transform_to_arm_pose_roundtrip() -> None:
+    expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40)
+
+    actual = _transform_to_arm_pose(_arm_pose_to_transform(expected))
+
+    assert actual.xyz() == pytest.approx(expected.xyz())
+    assert actual.rpy() == pytest.approx(expected.rpy())
+
+
+def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
+    solver._velocity_limits = np.ones(7)
+    solver._dt = 0.1
+    solver._actual_joints = np.zeros(7)
+
+    with pytest.raises(ValueError, match="finite"):
+        solver._validate_result(np.full(7, np.nan))
+    with pytest.raises(ValueError, match="position"):
+        solver._validate_result(np.full(7, 2.0))
+    with pytest.raises(ValueError, match="velocity"):
+        solver._validate_result(np.full(7, 0.2))

xr_rm_teleop/xr_rm_teleop/fun_peripheral.py

@@ -25,6 +25,14 @@ class PeripheralConfig:
     tools_in_ee: dict[str, list[list[float]]]
     set_initial_tool_state: bool = False
 
+    @property
+    def tool_name(self) -> str:
+        return list(self.tools_in_ee)[self.scissorgripper]
+
+    @property
+    def tool_pose(self) -> list[float]:
+        return list(self.tools_in_ee[self.tool_name][0])
+
 
 def load_peripheral_config(config_file: str, arm: str) -> PeripheralConfig:
     """从 bringup YAML 读取指定左右臂的外设配置。"""

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -0,0 +1,209 @@
+"""RM75 的 Placo 0.9.4 单步 QP 逆解。"""
+
+from __future__ import annotations
+
+import math
+from importlib.metadata import PackageNotFoundError, version
+from pathlib import Path
+
+import numpy as np
+
+from .realman_adapter import ArmPose
+
+
+EXPECTED_PLACO_VERSION = "0.9.4"
+RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
+RM75_Q_SLICE = slice(7, 14)
+
+
+def _rpy_to_rotation(roll: float, pitch: float, yaw: float) -> np.ndarray:
+    cr, sr = math.cos(roll), math.sin(roll)
+    cp, sp = math.cos(pitch), math.sin(pitch)
+    cy, sy = math.cos(yaw), math.sin(yaw)
+    return np.array(
+        [
+            [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
+            [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
+            [-sp, cp * sr, cp * cr],
+        ],
+        dtype=float,
+    )
+
+
+def _rotation_to_rpy(rotation: np.ndarray) -> tuple[float, float, float]:
+    pitch = math.asin(-float(np.clip(rotation[2, 0], -1.0, 1.0)))
+    if abs(math.cos(pitch)) > 1e-9:
+        roll = math.atan2(float(rotation[2, 1]), float(rotation[2, 2]))
+        yaw = math.atan2(float(rotation[1, 0]), float(rotation[0, 0]))
+    else:
+        roll = math.atan2(-float(rotation[1, 2]), float(rotation[1, 1]))
+        yaw = 0.0
+    return roll, pitch, yaw
+
+
+def _arm_pose_to_transform(pose: ArmPose) -> np.ndarray:
+    transform = np.eye(4)
+    transform[:3, :3] = _rpy_to_rotation(pose.rx, pose.ry, pose.rz)
+    transform[:3, 3] = pose.xyz()
+    return transform
+
+
+def _tool_pose_to_transform(tool_pose: list[float]) -> np.ndarray:
+    values = np.asarray(tool_pose, dtype=float)
+    if values.shape != (7,) or not np.isfinite(values).all():
+        raise ValueError("tool pose must contain 7 finite values")
+    x, y, z, qx, qy, qz, qw = values
+    norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
+    if norm <= 1e-9:
+        raise ValueError("tool quaternion norm must be positive")
+    qx, qy, qz, qw = qx / norm, qy / norm, qz / norm, qw / norm
+    transform = np.eye(4)
+    transform[:3, :3] = np.array(
+        [
+            [
+                1 - 2 * (qy * qy + qz * qz),
+                2 * (qx * qy - qz * qw),
+                2 * (qx * qz + qy * qw),
+            ],
+            [
+                2 * (qx * qy + qz * qw),
+                1 - 2 * (qx * qx + qz * qz),
+                2 * (qy * qz - qx * qw),
+            ],
+            [
+                2 * (qx * qz - qy * qw),
+                2 * (qy * qz + qx * qw),
+                1 - 2 * (qx * qx + qy * qy),
+            ],
+        ]
+    )
+    transform[:3, 3] = [x, y, z]
+    return transform
+
+
+def _transform_to_arm_pose(transform: np.ndarray) -> ArmPose:
+    roll, pitch, yaw = _rotation_to_rpy(transform[:3, :3])
+    return ArmPose(
+        float(transform[0, 3]),
+        float(transform[1, 3]),
+        float(transform[2, 3]),
+        roll,
+        pitch,
+        yaw,
+    )
+
+
+class PlacoIkSolver:
+    def __init__(
+        self,
+        urdf_path: str,
+        tool_pose: list[float],
+        dt: float,
+    ) -> None:
+        if dt <= 0.0:
+            raise ValueError("dt must be positive")
+        try:
+            installed_version = version("placo")
+            import placo
+        except (ImportError, PackageNotFoundError) as exc:
+            raise RuntimeError(
+                "Placo 0.9.4 must come from "
+                "/home/robot/miniconda3/envs/xr"
+            ) from exc
+        if installed_version != EXPECTED_PLACO_VERSION:
+            raise RuntimeError(
+                f"Placo {EXPECTED_PLACO_VERSION} is required, got {installed_version}"
+            )
+
+        model_path = Path(urdf_path).expanduser().resolve()
+        if not model_path.is_file():
+            raise FileNotFoundError(f"RM75 URDF not found: {model_path}")
+
+        self._dt = dt
+        self._robot = placo.RobotWrapper(str(model_path))
+        if self._robot.state.q.shape != (14,):
+            raise RuntimeError(
+                f"expected Placo q shape (14,), got {self._robot.state.q.shape}"
+            )
+        if list(self._robot.joint_names()) != RM75_JOINT_NAMES:
+            raise RuntimeError(
+                f"unexpected RM75 joint order: {list(self._robot.joint_names())}"
+            )
+        offsets = [
+            self._robot.get_joint_offset(name) for name in RM75_JOINT_NAMES
+        ]
+        if offsets != list(range(7, 14)):
+            raise RuntimeError(f"unexpected RM75 q offsets: {offsets}")
+
+        self._joint_limits = np.asarray(
+            [self._robot.get_joint_limits(name) for name in RM75_JOINT_NAMES]
+        )
+        velocity_offsets = [
+            self._robot.get_joint_v_offset(name) for name in RM75_JOINT_NAMES
+        ]
+        self._velocity_limits = np.asarray(
+            [
+                self._robot.model.velocityLimit[index]
+                for index in velocity_offsets
+            ]
+        )
+        self._tool_transform = _tool_pose_to_transform(tool_pose)
+        self._tool_inverse = np.linalg.inv(self._tool_transform)
+        self._actual_joints: np.ndarray | None = None
+
+        self._solver = placo.KinematicsSolver(self._robot)
+        self._solver.dt = dt
+        self._solver.mask_fbase(True)
+        self._solver.enable_velocity_limits(True)
+        self._frame_task = self._solver.add_frame_task("link_7", np.eye(4))
+        self._frame_task.configure("rm75_frame", "soft", 1.0)
+        manipulability = self._solver.add_manipulability_task(
+            "link_7",
+            "both",
+            1.0,
+        )
+        manipulability.configure("rm75_manipulability", "soft", 5e-2)
+        self._solver.add_kinetic_energy_regularization_task(1e-6)
+
+    @property
+    def base_configuration(self) -> list[float]:
+        return self._robot.state.q[:7].tolist()
+
+    def update_joint_state(self, joints: list[float]) -> ArmPose:
+        values = np.asarray(joints, dtype=float)
+        if values.shape != (7,) or not np.isfinite(values).all():
+            raise ValueError("joint state must contain 7 finite values")
+        is_first_feedback = self._actual_joints is None
+        self._actual_joints = values.copy()
+        self._robot.state.q[RM75_Q_SLICE] = values
+        self._robot.update_kinematics()
+        base_to_flange = self._robot.get_T_world_frame("link_7")
+        if is_first_feedback:
+            self._frame_task.T_world_frame = base_to_flange.copy()
+        base_to_tool = base_to_flange @ self._tool_transform
+        return _transform_to_arm_pose(base_to_tool)
+
+    def solve(self, target_tool_pose: ArmPose) -> list[float]:
+        if self._actual_joints is None:
+            raise RuntimeError("joint state must be initialized before QP solve")
+        self._frame_task.T_world_frame = (
+            _arm_pose_to_transform(target_tool_pose) @ self._tool_inverse
+        )
+        self._solver.solve(True)
+        result = np.asarray(
+            self._robot.state.q[RM75_Q_SLICE],
+            dtype=float,
+        ).copy()
+        self._validate_result(result)
+        return result.tolist()
+
+    def _validate_result(self, result: np.ndarray) -> None:
+        if result.shape != (7,) or not np.isfinite(result).all():
+            raise ValueError("QP result must contain 7 finite values")
+        lower = self._joint_limits[:, 0]
+        upper = self._joint_limits[:, 1]
+        if np.any(result < lower - 1e-9) or np.any(result > upper + 1e-9):
+            raise ValueError("QP result violates RM75 joint position limits")
+        max_step = self._velocity_limits * self._dt + 1e-9
+        if np.any(np.abs(result - self._actual_joints) > max_step):
+            raise ValueError("QP result violates RM75 one-cycle velocity limits")

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -1,21 +1,15 @@
-"""RM75 机械臂适配层。
+"""RM75 机械臂关节反馈、关节透传和停止适配层。"""
-
-对上提供统一的当前位姿读取、笛卡尔位姿目标发送和停止接口；对下根据配置
-选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
-"""
 
 from __future__ import annotations
 
 import math
+import threading
+import time
 from dataclasses import dataclass
 from numbers import Number
 from typing import Any
 
 
-def _angle_delta(target: float, current: float) -> float:
-    return math.atan2(math.sin(target - current), math.cos(target - current))
-
-
 @dataclass
 class ArmPose:
     x: float
@@ -32,55 +26,64 @@ class ArmPose:
         return [self.rx, self.ry, self.rz]
 
 
-class MockRealManAdapter:
+@dataclass(frozen=True)
-    """无机械臂时使用的运动学模拟器，用于验证 ROS2 遥操链路。"""
+class JointStateSnapshot:
+    positions: list[float]
+    received_at: float
 
-    def __init__(self, initial_pose: list[float], dt: float) -> None:
+
-        self._pose = ArmPose(*initial_pose[:6])
+class MockRealManAdapter:
-        self._dt = dt
+    """不导入厂商 SDK 的关节状态 mock。"""
-        self.last_velocity = [0.0] * 6
+
+    def __init__(self, initial_joint_degrees: list[float]) -> None:
+        if len(initial_joint_degrees) != 7 or not all(
+            math.isfinite(value) for value in initial_joint_degrees
+        ):
+            raise ValueError("initial joint pose must contain 7 finite values")
+        self._joint_positions = [
+            math.radians(value) for value in initial_joint_degrees
+        ]
+        self.last_joint_target: list[float] | None = None
         self.last_tool_open: bool | None = None
 
     def connect(self) -> None:
         return
 
-    def get_current_pose(self) -> ArmPose:
+    def get_latest_joint_state(self) -> JointStateSnapshot:
-        return self._pose
+        return JointStateSnapshot(
+            list(self._joint_positions),
+            time.monotonic(),
+        )
 
-    def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
+    def send_joint_target(self, joints: list[float], follow: bool) -> None:
         del follow
-        self.last_velocity = [
+        if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
-            (pose.x - self._pose.x) / self._dt,
+            raise ValueError("joint target must contain 7 finite values")
-            (pose.y - self._pose.y) / self._dt,
+        self._joint_positions = list(joints)
-            (pose.z - self._pose.z) / self._dt,
+        self.last_joint_target = list(joints)
-            _angle_delta(pose.rx, self._pose.rx) / self._dt,
-            _angle_delta(pose.ry, self._pose.ry) / self._dt,
-            _angle_delta(pose.rz, self._pose.rz) / self._dt,
-        ]
-        self._pose = pose
 
     def stop(self) -> None:
-        self.last_velocity = [0.0] * 6
+        return
 
     def close(self) -> None:
         self.stop()
 
-    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
+    def configure_peripheral(self, config: Any, peripheral_arm: str) -> None:
-        del config_file, peripheral_arm
+        del config, peripheral_arm
 
     def set_tool_enabled(self, open_tool: bool) -> None:
         self.last_tool_open = open_tool
 
 
 class RealManAdapter:
-    """睿尔曼 Python API2 的笛卡尔位姿透传适配层。"""
+    """复用一个睿尔曼 Python API2 连接的关节适配层。"""
 
     def __init__(
         self,
         robot_ip: str,
         robot_port: int,
         avoid_singularity: int,
-        frame_type: int,
+        feedback_period: float,
         logger: Any | None = None,
         configure_safety_limits: bool = True,
         max_line_speed: float = 1.0,
@@ -98,7 +101,9 @@ class RealManAdapter:
         self._robot_ip = robot_ip
         self._robot_port = robot_port
         self._avoid_singularity = avoid_singularity
-        self._frame_type = frame_type
+        if feedback_period <= 0.0:
+            raise ValueError("feedback_period must be positive")
+        self._feedback_period = feedback_period
         self._logger = logger
         self._configure_safety_limits = configure_safety_limits
         self._max_line_speed = max_line_speed
@@ -114,6 +119,11 @@ class RealManAdapter:
         self._canfd_radio = canfd_radio
         self._scissorgripper: int | None = None
         self._arm: Any | None = None
+        self._joint_state_lock = threading.Lock()
+        self._latest_joint_state: JointStateSnapshot | None = None
+        self._feedback_stop = threading.Event()
+        self._feedback_thread: threading.Thread | None = None
+        self._feedback_fault_logged = False
 
     def connect(self) -> None:
         try:
@@ -130,38 +140,48 @@ class RealManAdapter:
             "RealMan connected: "
             f"ip={self._robot_ip}, port={self._robot_port}, "
             f"avoid_singularity={self._avoid_singularity}, "
-            f"frame_type={self._frame_type}, command=rm_movep_canfd"
+            "command=rm_movej_canfd"
         )
         if self._configure_safety_limits:
             self._apply_safety_limits()
         if self._move_to_initial_pose_on_connect:
             self._move_to_initial_pose()
+        self._feedback_stop.clear()
+        self._feedback_thread = threading.Thread(
+            target=self._feedback_loop,
+            name=f"rm75_feedback_{self._robot_ip}",
+            daemon=True,
+        )
+        self._feedback_thread.start()
 
-    def get_current_pose(self) -> ArmPose:
+    def get_latest_joint_state(self) -> JointStateSnapshot | None:
-        self._require_arm()
+        with self._joint_state_lock:
-        state = self._arm.rm_get_current_arm_state()
+            if self._latest_joint_state is None:
-        pose = self._find_pose(state)
+                return None
-        if pose is None:
+            return JointStateSnapshot(
-            raise RuntimeError(f"无法从睿尔曼状态中解析当前 TCP 位姿：{state!r}")
+                list(self._latest_joint_state.positions),
-        return ArmPose(*pose[:6])
+                self._latest_joint_state.received_at,
+            )
 
-    def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
+    def send_joint_target(self, joints: list[float], follow: bool) -> None:
         self._require_arm()
-        ret = self._arm.rm_movep_canfd(
+        if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
-            [pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz],
+            raise ValueError("joint target must contain 7 finite values")
+        ret = self._arm.rm_movej_canfd(
+            [math.degrees(value) for value in joints],
             follow,
+            0,
             self._canfd_trajectory_mode,
             self._canfd_radio,
         )
-        self._check_return(ret, "rm_movep_canfd")
+        self._check_return(ret, "rm_movej_canfd")
 
-    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
+    def configure_peripheral(self, config: Any, peripheral_arm: str) -> None:
         self._require_arm()
-        from .fun_peripheral import load_peripheral_config, peripheral_cfg
+        from .fun_peripheral import peripheral_cfg
 
-        config = load_peripheral_config(config_file, peripheral_arm)
         self._scissorgripper = config.scissorgripper
-        tool_name = list(config.tools_in_ee.keys())[config.scissorgripper]
+        tool_name = config.tool_name
         self._log_info(
             "开始配置 RealMan 末端外设："
             f"arm={peripheral_arm}, scissorgripper={config.scissorgripper}, "
@@ -196,6 +216,12 @@ class RealManAdapter:
         if self._arm is None:
             return
         self.stop()
+        self._feedback_stop.set()
+        if self._feedback_thread is not None:
+            self._feedback_thread.join(timeout=3.0)
+            if self._feedback_thread.is_alive():
+                self._log_warn("RealMan 关节反馈线程未在 3 秒内退出。")
+            self._feedback_thread = None
         try:
             self._arm.rm_delete_robot_arm()
         finally:
@@ -205,6 +231,37 @@ class RealManAdapter:
         if self._arm is None:
             raise RuntimeError("睿尔曼机械臂尚未连接")
 
+    def _feedback_loop(self) -> None:
+        while not self._feedback_stop.is_set():
+            try:
+                self._read_joint_state_once()
+                self._feedback_fault_logged = False
+            except Exception as exc:
+                if not self._feedback_fault_logged:
+                    self._log_warn(f"RealMan 关节反馈读取失败：{exc}")
+                    self._feedback_fault_logged = True
+            self._feedback_stop.wait(self._feedback_period)
+
+    def _read_joint_state_once(self) -> None:
+        self._require_arm()
+        result = self._arm.rm_get_joint_degree()
+        self._check_return(result, "rm_get_joint_degree")
+        if not isinstance(result, tuple) or len(result) < 2:
+            raise RuntimeError(f"rm_get_joint_degree 返回格式错误：{result!r}")
+        degrees = result[1]
+        if (
+            not isinstance(degrees, (list, tuple))
+            or len(degrees) != 7
+            or not all(isinstance(value, Number) for value in degrees)
+        ):
+            raise RuntimeError(f"RM75 关节反馈必须包含 7 个数值：{degrees!r}")
+        positions = [math.radians(float(value)) for value in degrees]
+        if not all(math.isfinite(value) for value in positions):
+            raise RuntimeError("RM75 关节反馈包含 NaN/Inf")
+        snapshot = JointStateSnapshot(positions, time.monotonic())
+        with self._joint_state_lock:
+            self._latest_joint_state = snapshot
+
     def _apply_safety_limits(self) -> None:
         # 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。
         self._try_call("rm_set_avoid_singularity_mode", int(self._avoid_singularity))
@@ -260,74 +317,3 @@ class RealManAdapter:
     @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 位姿字段。
-        if isinstance(obj, dict):
-            for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
-                pose = cls._as_pose(obj.get(key))
-                if pose is not None:
-                    return pose
-            for value in obj.values():
-                pose = cls._find_pose(value)
-                if pose is not None:
-                    return pose
-        elif isinstance(obj, (list, tuple)):
-            pose = cls._as_pose(obj)
-            if pose is not None:
-                return pose
-            for value in obj:
-                pose = cls._find_pose(value)
-                if pose is not None:
-                    return pose
-        elif hasattr(obj, "to_dictionary"):
-            try:
-                return cls._find_pose(obj.to_dictionary(7))
-            except TypeError:
-                return cls._find_pose(obj.to_dictionary())
-        elif hasattr(obj, "to_dict"):
-            return cls._find_pose(obj.to_dict())
-        else:
-            for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
-                if hasattr(obj, key):
-                    pose = cls._as_pose(getattr(obj, key))
-                    if pose is not None:
-                        return pose
-        return None
-
-    @staticmethod
-    def _as_pose(value: Any) -> list[float] | None:
-        if isinstance(value, (list, tuple)) and len(value) >= 6:
-            if all(isinstance(item, Number) for item in value[:6]):
-                return [float(item) for item in value[:6]]
-        if isinstance(value, dict):
-            position = value.get("position")
-            euler = value.get("euler")
-            if isinstance(position, dict) and isinstance(euler, dict):
-                keys = ("x", "y", "z")
-                rpy_keys = ("rx", "ry", "rz")
-                if all(key in position for key in keys) and all(key in euler for key in rpy_keys):
-                    return [
-                        float(position["x"]),
-                        float(position["y"]),
-                        float(position["z"]),
-                        float(euler["rx"]),
-                        float(euler["ry"]),
-                        float(euler["rz"]),
-                    ]
-        if all(hasattr(value, attr) for attr in ("position", "euler")):
-            position = getattr(value, "position")
-            euler = getattr(value, "euler")
-            if all(hasattr(position, key) for key in ("x", "y", "z")) and all(
-                hasattr(euler, key) for key in ("rx", "ry", "rz")
-            ):
-                return [
-                    float(position.x),
-                    float(position.y),
-                    float(position.z),
-                    float(euler.rx),
-                    float(euler.ry),
-                    float(euler.rz),
-                ]
-        return None

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -1,7 +1,7 @@
-"""RM75 单臂 XR 相对位姿透传遥操作节点。
+"""RM75 单臂 XR 相对位姿 QP 遥操作节点。
 
 节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位姿
-映射成机器人坐标系中的目标 TCP，并通过 rm_movep_canfd 持续下发目标位姿。
+映射成机器人坐标系中的目标 TCP，再通过 Placo 和 rm_movej_canfd 下发关节目标。
 """
 
 from __future__ import annotations
@@ -9,6 +9,7 @@ from __future__ import annotations
 import math
 import queue
 import threading
+import time
 from typing import Iterable
 
 import rclpy
@@ -19,7 +20,14 @@ from std_msgs.msg import Bool
 
 from xr_rm_interfaces.msg import XrController
 
-from .realman_adapter import ArmPose, MockRealManAdapter, RealManAdapter
+from .fun_peripheral import load_peripheral_config
+from .placo_ik_solver import PlacoIkSolver
+from .realman_adapter import (
+    ArmPose,
+    JointStateSnapshot,
+    MockRealManAdapter,
+    RealManAdapter,
+)
 
 
 def _norm(values: Iterable[float]) -> float:
@@ -176,13 +184,11 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("low_z_threshold", 0.20)
         self.declare_parameter("low_z_min_radius", 0.21)
         self.declare_parameter("xr_to_robot_matrix", [0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0])
-        self.declare_parameter("current_pose_poll_hz", 10.0)
         self.declare_parameter("use_mock", True)
-        self.declare_parameter("mock_initial_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
+        self.declare_parameter("robot_urdf_path", "")
         self.declare_parameter("robot_ip", "192.168.1.18")
         self.declare_parameter("robot_port", 8080)
         self.declare_parameter("avoid_singularity", 1)
-        self.declare_parameter("frame_type", 1)
         self.declare_parameter("follow", False)
         self.declare_parameter("configure_safety_limits", True)
         self.declare_parameter("max_line_speed", 1.0)
@@ -232,7 +238,6 @@ class SingleArmVelocityTeleop(Node):
         self._low_z_threshold = float(self.get_parameter("low_z_threshold").value)
         self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value)
         self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
-        self._current_pose_poll_hz = float(self.get_parameter("current_pose_poll_hz").value)
         self._follow = self._bool_parameter("follow")
         self._enable_tool_control = self._bool_parameter("enable_tool_control")
         self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
@@ -254,7 +259,8 @@ class SingleArmVelocityTeleop(Node):
         self._last_sent_orientation: list[float] | None = None
         self._last_command_time: Time | None = None
         self._last_current_pose: ArmPose | None = None
-        self._last_current_pose_time: Time | None = None
+        self._last_valid_joint_target: list[float] | None = None
+        self._joint_feedback_ready = False
         self._stop_sent = True
         self._trigger_tool_open = True
         self._last_trigger_pressed: bool | None = None
@@ -262,6 +268,17 @@ class SingleArmVelocityTeleop(Node):
         self._tool_worker_stop = threading.Event()
         self._tool_worker_thread: threading.Thread | None = None
 
+        peripheral_arm = self._peripheral_arm_name()
+        config_file = str(self.get_parameter("peripheral_config_file").value)
+        self._peripheral_config = load_peripheral_config(
+            config_file,
+            peripheral_arm,
+        )
+        self._ik_solver = PlacoIkSolver(
+            str(self.get_parameter("robot_urdf_path").value),
+            self._peripheral_config.tool_pose,
+            self._dt,
+        )
         self._adapter = self._make_adapter()
         self._adapter.connect()
         self._setup_tool_control()
@@ -276,24 +293,24 @@ class SingleArmVelocityTeleop(Node):
         self.create_subscription(XrController, topic, self._on_controller, 10)
         self.create_timer(self._dt, self._control_tick)
         self.get_logger().info(
-            f"{self._arm_name} 位姿透传遥操节点已启动，监听话题：{topic}, "
+            f"{self._arm_name} Placo QP 遥操节点已启动，监听话题：{topic}, "
             f"dt={self._dt:.4f}s, follow={self._follow}, "
             f"orientation_control={self._enable_orientation_control}"
         )
 
     def _make_adapter(self):
-        # mock 和真机共享同一位姿目标链路，只在适配层切换执行方式。
+        initial_joint_pose = self._float_list_parameter(
+            "initial_joint_pose",
+            7,
+        )
         if self._bool_parameter("use_mock"):
-            return MockRealManAdapter(
+            return MockRealManAdapter(initial_joint_pose)
-                [float(v) for v in self.get_parameter("mock_initial_pose").value],
-                self._dt,
-            )
 
         return RealManAdapter(
             robot_ip=self.get_parameter("robot_ip").value,
             robot_port=int(self.get_parameter("robot_port").value),
             avoid_singularity=int(self.get_parameter("avoid_singularity").value),
-            frame_type=int(self.get_parameter("frame_type").value),
+            feedback_period=self._dt,
             logger=self.get_logger(),
             configure_safety_limits=self._bool_parameter("configure_safety_limits"),
             max_line_speed=float(self.get_parameter("max_line_speed").value),
@@ -303,13 +320,20 @@ class SingleArmVelocityTeleop(Node):
             joint_max_speed=float(self.get_parameter("joint_max_speed").value),
             joint_max_acc=float(self.get_parameter("joint_max_acc").value),
             move_to_initial_pose_on_connect=self._bool_parameter("move_to_initial_pose_on_connect"),
-            initial_joint_pose=self._float_list_parameter("initial_joint_pose", 7),
+            initial_joint_pose=initial_joint_pose,
             init_move_speed=int(self.get_parameter("init_move_speed").value),
             canfd_trajectory_mode=int(self.get_parameter("canfd_trajectory_mode").value),
             canfd_radio=int(self.get_parameter("canfd_radio").value),
         )
 
     def _setup_tool_control(self) -> None:
+        peripheral_arm = self._peripheral_arm_name()
+        if self._bool_parameter("configure_peripheral_on_connect"):
+            self._adapter.configure_peripheral(
+                self._peripheral_config,
+                peripheral_arm,
+            )
+
         if not self._enable_tool_control:
             if self._enable_trigger_gripper_control:
                 self.get_logger().warn(
@@ -317,11 +341,6 @@ class SingleArmVelocityTeleop(Node):
                 )
             return
 
-        peripheral_arm = self._peripheral_arm_name()
-        config_file = str(self.get_parameter("peripheral_config_file").value)
-        if self._bool_parameter("configure_peripheral_on_connect"):
-            self._adapter.configure_peripheral(config_file, peripheral_arm)
-
         self._start_tool_worker()
 
         topic = str(self.get_parameter("tool_command_topic").value).strip()
@@ -436,6 +455,32 @@ class SingleArmVelocityTeleop(Node):
 
     def _control_tick(self) -> None:
         now = self.get_clock().now()
+        snapshot = self._fresh_joint_state()
+        if snapshot is None:
+            if self._joint_feedback_ready:
+                self.get_logger().warn(
+                    f"{self._arm_name} 关节反馈缺失或过期，机械臂停止。",
+                    throttle_duration_sec=1.0,
+                )
+            self._joint_feedback_ready = False
+            self._safe_stop(reset_active=True)
+            return
+        try:
+            current_pose = self._sync_joint_feedback(snapshot)
+        except Exception as exc:
+            self.get_logger().error(
+                f"{self._arm_name} 关节反馈同步到 Placo 失败：{exc}",
+                throttle_duration_sec=1.0,
+            )
+            self._joint_feedback_ready = False
+            self._safe_stop(reset_active=True)
+            return
+        if not self._joint_feedback_ready:
+            self.get_logger().info(
+                f"{self._arm_name} 已收到首帧有效关节反馈，QP 可以启用。"
+            )
+            self._joint_feedback_ready = True
+
         if self._last_msg is None or self._last_msg_time is None:
             self._safe_stop(reset_active=True)
             return
@@ -469,13 +514,17 @@ class SingleArmVelocityTeleop(Node):
             self._safe_stop(reset_active=True)
             return
         if not self._active:
-            self._enter_active_control(controller_now, controller_quat, now)
+            self._enter_active_control(
+                controller_now,
+                controller_quat,
+                current_pose,
+                now,
+            )
             return
 
         assert self._controller_start is not None
         assert self._robot_start_pose is not None
 
-        self._maybe_refresh_current_pose(now)
         raw_target_xyz = self._raw_target_from_controller(controller_now)
         raw_target_rpy = self._raw_orientation_from_controller(controller_quat)
         workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target_xyz)
@@ -507,7 +556,8 @@ class SingleArmVelocityTeleop(Node):
         )
 
         self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped)
-        if self._send_cartesian_target(target_pose):
+        joint_target = self._solve_joint_target(target_pose)
+        if self._send_joint_target(joint_target):
             self._last_sent_target = sent_target
             self._last_sent_orientation = sent_orientation
             self._last_command_time = now
@@ -517,18 +567,9 @@ class SingleArmVelocityTeleop(Node):
         self,
         controller_now: list[float],
         controller_quat: tuple[float, float, float, float],
+        robot_pose: ArmPose,
         now: Time,
     ) -> None:
-        try:
-            robot_pose = self._read_current_pose_for_control(now)
-        except Exception as exc:
-            self.get_logger().error(
-                f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}",
-                throttle_duration_sec=1.0,
-            )
-            self._safe_stop(reset_active=True)
-            return
-
         robot_xyz = robot_pose.xyz()
         self._active = True
         self._controller_start = controller_now
@@ -755,27 +796,45 @@ class SingleArmVelocityTeleop(Node):
             for i in range(3)
         ]
 
-    def _read_current_pose_for_control(self, now: Time) -> ArmPose:
+    def _fresh_joint_state(self) -> JointStateSnapshot | None:
-        pose = self._adapter.get_current_pose()
+        snapshot = self._adapter.get_latest_joint_state()
-        self._last_current_pose = pose
+        if snapshot is None:
-        self._last_current_pose_time = now
+            return None
-        return pose
+        age = time.monotonic() - snapshot.received_at
+        if age < 0.0 or age > self._command_timeout_sec:
+            return None
+        if (
+            len(snapshot.positions) != 7
+            or not all(math.isfinite(value) for value in snapshot.positions)
+        ):
+            return None
+        return snapshot
 
-    def _maybe_refresh_current_pose(self, now: Time) -> None:
+    def _sync_joint_feedback(
-        if self._current_pose_poll_hz <= 0.0:
+        self,
-            return
+        snapshot: JointStateSnapshot,
-        if self._last_current_pose_time is not None:
+    ) -> ArmPose:
-            age = (now - self._last_current_pose_time).nanoseconds * 1e-9
+        current_pose = self._ik_solver.update_joint_state(
-            if age < 1.0 / self._current_pose_poll_hz:
+            snapshot.positions
-                return
+        )
+        self._last_current_pose = current_pose
+        if not self._active or self._last_valid_joint_target is None:
+            self._last_valid_joint_target = list(snapshot.positions)
+        return current_pose
 
+    def _solve_joint_target(self, target_pose: ArmPose) -> list[float]:
+        if self._last_valid_joint_target is None:
+            raise RuntimeError("valid joint feedback has not been initialized")
         try:
-            self._read_current_pose_for_control(now)
+            result = self._ik_solver.solve(target_pose)
         except Exception as exc:
             self.get_logger().warn(
-                f"{self._arm_name} 低频读取 TCP 位姿失败，继续透传目标：{exc}",
+                f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}",
                 throttle_duration_sec=1.0,
             )
+            return list(self._last_valid_joint_target)
+        self._last_valid_joint_target = list(result)
+        return list(result)
 
     def _safe_stop(self, reset_active: bool) -> None:
         if not self._stop_sent:
@@ -818,16 +877,16 @@ class SingleArmVelocityTeleop(Node):
             return ArmPose(*self._last_sent_target, *rpy)
         return None
 
-    def _send_cartesian_target(self, pose: ArmPose) -> bool:
+    def _send_joint_target(self, joints: list[float]) -> bool:
         try:
-            self._adapter.send_cartesian_target(pose, self._follow)
+            self._adapter.send_joint_target(joints, self._follow)
         except Exception as exc:
             self.get_logger().error(
-                f"{self._arm_name} 发送位姿透传命令失败：{exc}",
+                f"{self._arm_name} 发送关节透传命令失败：{exc}",
                 throttle_duration_sec=1.0,
             )
-            self._active = False
             self._send_stop_once()
+            self._safe_stop(reset_active=True)
             return False
         return True
 
@@ -917,8 +976,6 @@ class SingleArmVelocityTeleop(Node):
             raise ValueError("target_filter_fast_threshold_m must be >= 0")
         if self._max_linear_speed <= 0.0:
             raise ValueError("max_linear_speed must be > 0")
-        if self._current_pose_poll_hz < 0.0:
-            raise ValueError("current_pose_poll_hz must be >= 0")
         if self._orientation_deadband_rad < 0.0:
             raise ValueError("orientation_deadband_rad must be >= 0")
         if not 0.0 <= self._orientation_filter_alpha <= 1.0: