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 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 \
-  --pattern axis_sweep --seconds 60 --both-mode staggered
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
+ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port 15000 \
+  --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)`；现场需要跳过时，显式传入
-`move_to_initial_pose_on_connect:=false`。
+所有配置默认都不会执行 `movej(initial_joint_pose)`；只有确认安全区清空后，才可显式传入
+`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`：`rm_movep_canfd` 目标位姿发送频率，默认 `90.0`。
-- `follow`：传给 `rm_movep_canfd` 的跟随标志，默认 `false`。
+- `control_rate_hz`：同步关节反馈、执行一次 QP 并发送一次关节目标的频率，默认 `90.0`。
+- `follow`：传给 `rm_movej_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 的频率；控制中不再每帧阻塞读取状态。
-- `mock_initial_pose`：mock 模式初始 TCP 位姿。
-- `initial_joint_pose`：可选真机初始关节角。
+- `initial_joint_pose`：mock 的初始关节反馈，以及显式开启初始化移动时的真机初始关节角。
 
 当前 `/xr/*_controller` 的 Project 坐标约定：