# RM75-B 第一阶段运动学与 QP IK

本目录是一个独立的离线 Python 包，用于验证 RM75-B 的运动学与逆运动学。它不接入
ROS 2 遥操作控制链路，也不会建立机器人连接。

第一阶段包含：

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

MuJoCo、MJCF、碰撞规避和真实机器人控制明确不在本阶段范围内。

## 环境

经过验证的环境定义在 `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
```

RealMan 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
```

对于任何失败状态，`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)。

## 模型说明

单臂 URDF 是 RM75-B 运动链几何参数的唯一来源。导入的双臂 URDF 仅用于提供左右
安装变换；求解器不使用其中的镜像关节限位和固化的关节零位偏移。

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