Add IK types, validation, and tests for RM75 kinematics

ik_qp/README.md

@@ -1,36 +1,89 @@
-### This repo is for inverse kinematics and verification
+# RM75-B 第一阶段运动学与 QP IK
 
-In this branch, the qp-based inverse kinematics method is modified as a python class. The user can call it as in `main.py`
+本目录是一个独立的离线 Python 包，用于验证 RM75-B 的运动学与逆运动学。它不接入
+ROS 2 遥操作控制链路，也不会建立机器人连接。
 
-Inverse Kinematics (IK) is numerically obtained through quadratic programming (QP).
+第一阶段包含：
 
-Verification is done with Mujoco simulation.
+- 由 Pinocchio 加载的标准单臂 RM75-B URDF。
+- 基于 SE(3) 的正运动学、局部坐标雅可比矩阵和位姿残差。
+- 支持热启动的 OSQP 微分逆运动学。
+- 作为独立参考的 RealMan API2 Algo FK。
+- 物理关节限位配置和项目专用的遥操作关节限位配置。
+- 由两份标准单臂模型组成的双臂装配模型。
+- 可生成 JSON、CSV 和 Markdown 报告的确定性验证流程。
 
-Key specifications:
-1. Time consumption.
-2. Success rate
-3. Minial joint variation.
+MuJoCo、MJCF、碰撞规避和真实机器人控制明确不在本阶段范围内。
 
-Next:\
-Comparison with Realman official IK method.
-Embedded with current demo.
+## 环境
 
+经过验证的环境定义在 `environment.yml` 中：
 
-### Comparison (05June2026):
-
-- With current dual arm joint limit, 
+```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
 ```
-ub = np.array([150.0, 110.0, 170.0,  130, 175.0, 125.0,  179.0])
-lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0,  -179.0])
-```
-the success rates for **qp-based ik** and **realman Algo ik** are **63%** and **46%**.\
-At least one solver works out the ik, rate = **74%**.
 
-- With realman-75 physical joint limit,
-```
-ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])
-lb = -ub
-```
-the success rates for **qp-based ik** and **realman Algo ik** are **76%** and **51%**.\
-At least one solver works out the ik, rate = **84%**.
+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 基座至第一关节偏移。