Implement dual-arm teleoperation with RM75 QP controller and MuJoCo backend

ik_qp/README.md

@@ -1,7 +1,8 @@
-# RM75-B 第一阶段运动学与 QP IK
+# RM75-B 运动学、QP IK 与 MuJoCo 验证
 
-本目录是一个独立的离线 Python 包，用于验证 RM75-B 的运动学与逆运动学。它不接入
-ROS 2 遥操作控制链路，也不会建立机器人连接。
+本目录是独立 Python 算法包，用于验证 RM75-B 的运动学、逆运动学和后端无关遥操作
+控制。核心包不依赖 ROS 2，也不会建立真实机器人连接；第三阶段由 `xr_rm_teleop` 提供
+薄 ROS 消息适配层并使用 MuJoCo 后端。
 
 第一阶段包含：
 
@@ -13,7 +14,23 @@ ROS 2 遥操作控制链路，也不会建立机器人连接。
 - 由两份标准单臂模型组成的双臂装配模型。
 - 可生成 JSON、CSV 和 Markdown 报告的确定性验证流程。
 
-MuJoCo、MJCF、碰撞规避和真实机器人控制明确不在本阶段范围内。
+第二阶段在完整双臂场景中使用第一阶段求解器驱动一侧机械臂，包含：
+
+- 由标准单臂 URDF 复制两条运动链生成的规范化 14 轴 MJCF。
+- 来自上传双臂模型的安装变换、平台、挂架、夹爪和 19 个 OBJ 资源。
+- 默认控制左臂、固定右臂的纯运动学 MuJoCo 播放。
+- headless EGL 自动验证和 GLFW 实时 viewer 演示。
+- MuJoCo、Pinocchio 与 RealMan Algo FK 的三方对照。
+
+第三阶段包含：
+
+- 右臂 FK、IK 和连续轨迹快速验收。
+- 按左右工具 TCP 求解的双臂仿真初始姿态。
+- 与 `XrController.msg` 字段兼容、但不导入 ROS 的 grip 相对位姿控制核心。
+- 双臂 SE(3) 目标、QP IK、关节限位、速度限制和故障联停状态机。
+- 可由未来 `RealmanRobot` 实现复用的 `RobotBackend` 协议。
+
+碰撞规避、动力学伺服和真实机器人控制仍不在当前范围内。
 
 ## 环境
 
@@ -25,8 +42,8 @@ conda env update -f environment.yml
 conda run -n qp python -m pip install -e . --no-deps
 ```
 
-RealMan API2 SDK 是外部二进制依赖，不会复制到本包中。请为验证程序指定包含
-`Robotic_Arm/` 的目录：
+环境文件会从 PyPI 安装 `Robotic_Arm 1.1.5`，通常无需额外设置。若需要强制使用
+本地 API2 SDK，可为验证程序指定包含 `Robotic_Arm/` 的目录：
 
 ```bash
 export REALMAN_SDK_ROOT=/path/to/RM_API2/Python
@@ -52,6 +69,28 @@ if result.success:
     solution_q_rad = result.q
 ```
 
+双臂 MuJoCo 场景：
+
+```python
+from rm75_ik import DualArmMuJoCo
+
+scene = DualArmMuJoCo(controlled_arm="left")
+scene.set_arm_configuration("left", solution_q_rad)
+world_flange_pose = scene.get_flange_pose("left")
+image = scene.render()
+```
+
+后端无关双臂控制：
+
+```python
+from rm75_ik import DualArmQpTeleopController, load_dual_arm_profiles
+from rm75_ik.mujoco_robot import MujocoRobot
+
+profiles = load_dual_arm_profiles(teleop_yaml, peripheral_yaml)
+robot = MujocoRobot(profiles)
+controller = DualArmQpTeleopController(robot, profiles)
+```
+
 对于任何失败状态，`IkResult.q` 均为 `None`。不得将失败或未经验证的结果发送给
 机器人。
 
@@ -80,6 +119,95 @@ REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
 验收标准和最近一次完整结果请参见
 [STAGE1_VALIDATION.md](STAGE1_VALIDATION.md)。
 
+运行第二阶段完整 headless 验证：
+
+```bash
+REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
+  conda run -n qp rm75-stage2-validate --arm left
+```
+
+启动双臂实时可视化，只驱动左臂：
+
+```bash
+conda run -n qp rm75-stage2-demo --arm left --trajectory combined
+```
+
+移动到指定目标点。目标位置位于受控机械臂基座坐标系，单位为米；未提供 RPY 时保持
+起始姿态：
+
+```bash
+conda run -n qp rm75-stage2-demo \
+  --arm left \
+  --target-position 0.45 0.0 0.3375 \
+  --duration 8 \
+  --wait-before 2 \
+  --hold-after 3
+```
+
+也可用弧度指定目标姿态：
+
+```bash
+conda run -n qp rm75-stage2-demo \
+  --arm left \
+  --target-position 0.45 0.0 0.3375 \
+  --target-rpy -3.14159 0.52360 -3.14159 \
+  --duration 8
+```
+
+手动拖动受控机械臂：
+
+```bash
+conda run -n qp rm75-stage2-demo --arm left --manual-drag
+```
+
+在 Viewer 中双击选中连杆，然后按住 `Ctrl` 并使用鼠标右键拖动。该模式使用零重力、
+有阻尼的 MuJoCo 动力学，只用于检查关节活动与模型装配，不属于 IK 验收结果。
+
+支持的演示轨迹为 `joint`、`line`、`arc`、`orientation` 和 `combined`。无桌面环境时
+可添加 `--headless --output stage2_demo.png`。阶段二报告和截图写入
+`artifacts/stage2/`，验收结果见 [STAGE2_VALIDATION.md](STAGE2_VALIDATION.md)。
+
+运行第三阶段完整 headless 验证：
+
+```bash
+conda run -n qp rm75-stage3-validate \
+  --sdk-root /path/to/RM_API2/Python \
+  --teleop-config ../xr_rm_bringup/config/dual_arm_rm75.yaml \
+  --peripheral-config ../xr_rm_bringup/config/peripherals_rm75.yaml
+```
+
+在 ROS 工作空间中启动 PICO/UDP、双臂 QP 和共享 MuJoCo viewer：
+
+```bash
+conda activate qp
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 launch xr_rm_bringup dual_arm_qp_sim.launch.py
+```
+
+该 launch 将 `robot_backend` 固定为 `mujoco`，不会连接真实机械臂。headless 运行可使用：
+
+```bash
+ros2 launch xr_rm_bringup dual_arm_qp_sim.launch.py \
+  show_viewer:=false mujoco_gl:=egl
+```
+
+常用 launch 参数：
+
+| 参数 | 默认值 | 说明 |
+|---|---:|---|
+| `udp_host` | `0.0.0.0` | XR UDP 监听地址 |
+| `udp_port` | `15000` | XR UDP 监听端口 |
+| `udp_timer_hz` | `200.0` | UDP receiver 轮询频率 |
+| `control_rate_hz` | `90.0` | 双臂 QP 控制周期频率 |
+| `show_viewer` | `true` | 是否显示共享 MuJoCo viewer |
+| `mujoco_gl` | `glfw` | MuJoCo 渲染后端；无桌面环境建议使用 `egl` |
+
+启动成功后，日志应先显示 UDP receiver 的监听地址，再显示左右臂从
+`initial_tcp_pose` 初始化的残差，最后出现 `dual-arm QP teleop ready`。第三阶段结果见
+[STAGE3_VALIDATION.md](STAGE3_VALIDATION.md)。
+
 ## 模型说明
 
 单臂 URDF 是 RM75-B 运动链几何参数的唯一来源。导入的双臂 URDF 仅用于提供左右
@@ -87,3 +215,7 @@ REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
 
 导入的双臂 URDF 中，右侧基座的视觉原点与运动学原点相差约 1 mm。第一阶段采用
 第一关节的运动学原点，并叠加文档规定的 240.5 mm 基座至第一关节偏移。
+
+第二阶段不会把原始双臂 URDF 的镜像关节角直接交给 MuJoCo。MJCF 的左右关节均采用
+标准 RealMan 弧度定义；原始双臂模型只作为安装和视觉来源。当前场景没有 actuator，
+通过设置 `qpos` 并调用 `mj_forward()` 实现确定性的运动学播放。