# RM75-B 运动学、QP IK 与 MuJoCo 验证

本目录是独立 Python 算法包，用于验证 RM75-B 的运动学、逆运动学和后端无关遥操作
控制。核心包不依赖 ROS 2，也不会建立真实机器人连接；第三阶段由 `xr_rm_teleop` 提供
薄 ROS 消息适配层并使用 MuJoCo 后端。

第一阶段包含：

- 由 Pinocchio 加载的标准单臂 RM75-B URDF。
- 基于 SE(3) 的正运动学、局部坐标雅可比矩阵和位姿残差。
- 支持热启动的 OSQP 微分逆运动学。
- 作为独立参考的 RealMan API2 Algo FK。
- 物理关节限位配置和项目专用的遥操作关节限位配置。
- 由两份标准单臂模型组成的双臂装配模型。
- 可生成 JSON、CSV 和 Markdown 报告的确定性验证流程。

第二阶段在完整双臂场景中使用第一阶段求解器驱动一侧机械臂，包含：

- 由标准单臂 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` 协议。

碰撞规避、动力学伺服和真实机器人控制仍不在当前范围内。

## 环境

经过验证的环境定义在 `environment.yml` 中：

```bash
cd /home/robot/WS_xr/src/ik_qp
conda env update -f environment.yml
conda run -n qp python -m pip install -e . --no-deps
```

环境文件会从 PyPI 安装 `Robotic_Arm 1.1.5`，通常无需额外设置。若需要强制使用
本地 API2 SDK，可为验证程序指定包含 `Robotic_Arm/` 的目录：

```bash
export REALMAN_SDK_ROOT=/path/to/RM_API2/Python
```

## 公共 API

```python
from rm75_ik import (
    DualArmAssembly,
    RM75IkSolver,
    RM75Kinematics,
    RealManFkReference,
    teleop_joint_limits,
)

kinematics = RM75Kinematics(limits=teleop_joint_limits())
solver = RM75IkSolver(kinematics)
target = kinematics.forward(target_q_rad)
result = solver.solve(target, current_q_rad)

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`。不得将失败或未经验证的结果发送给
机器人。

每一对 `RM75Kinematics`/`RM75IkSolver` 都持有可变的 Pinocchio 和 OSQP 状态，因此
只能由一个控制线程使用。未来的双臂控制器应为每条机械臂分别持有一对实例。

## 验证

运行快速单元测试：

```bash
REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
  conda run -n qp python -m pytest -q
```

运行完整、严格的第一阶段基准测试：

```bash
REALMAN_SDK_ROOT=/path/to/RM_API2/Python \
  conda run -n qp rm75-stage1-validate
```

如需进行小规模冒烟测试，请添加 `--quick`。报告将写入 `artifacts/stage1/`，并且
该目录有意设置为由 Git 忽略。

验收标准和最近一次完整结果请参见
[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 仅用于提供左右
安装变换；求解器不使用其中的镜像关节限位和固化的关节零位偏移。

导入的双臂 URDF 中，右侧基座的视觉原点与运动学原点相差约 1 mm。第一阶段采用
第一关节的运动学原点，并叠加文档规定的 240.5 mm 基座至第一关节偏移。

第二阶段不会把原始双臂 URDF 的镜像关节角直接交给 MuJoCo。MJCF 的左右关节均采用
标准 RealMan 弧度定义；原始双臂模型只作为安装和视觉来源。当前场景没有 actuator，
通过设置 `qpos` 并调用 `mj_forward()` 实现确定性的运动学播放。