From a196cec75018a99fd426b479e260558a5468e48f Mon Sep 17 00:00:00 2001
From: YikaiFu-cart <yikai.fu@nicecart.ai>
Date: Wed, 12 Aug 2026 22:56:48 +0800
Subject: [PATCH] =?UTF-8?q?docs:=20=E6=B7=BB=E5=8A=A0J3=E4=BB=BF=E7=9C=9F?=
 =?UTF-8?q?=E6=A0=87=E5=AE=9A=E6=96=B9=E6=A1=88?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 ...26-08-12-rm75-j3-simulation-calibration.md | 406 ++++++++++++++++++
 ...2-rm75-j3-simulation-calibration-design.md | 245 +++++++++++
 2 files changed, 651 insertions(+)
 create mode 100644 docs/superpowers/plans/2026-08-12-rm75-j3-simulation-calibration.md
 create mode 100644 docs/superpowers/specs/2026-08-12-rm75-j3-simulation-calibration-design.md

diff --git a/docs/superpowers/plans/2026-08-12-rm75-j3-simulation-calibration.md b/docs/superpowers/plans/2026-08-12-rm75-j3-simulation-calibration.md
new file mode 100644
index 0000000..1ecb919
--- /dev/null
+++ b/docs/superpowers/plans/2026-08-12-rm75-j3-simulation-calibration.md
@@ -0,0 +1,406 @@
+# RM75 双臂 J3 参考角仿真标定实施计划
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** 使用当前双臂 URDF、Placo QP 和 MuJoCo 运动学模型运行可复现的左右臂 J3 参考角粗扫与细扫，并输出评分、稳定区间和推荐角度。
+
+**Architecture:** 新增一个仅供离线实验使用的脚本，负责生成 18 条严格六维 TCP 轨迹、建立三类 QP 试验配置、运行候选角度扫描、计算硬门槛与并列评分，并生成 CSV/JSON/Markdown 结果。生产控制器、QP 求解器和 YAML 均不修改；测试只覆盖轨迹、评分和一个真实 Placo/MuJoCo 冒烟评估。
+
+**Tech Stack:** Python 3.10、NumPy、Placo 0.9.4、MuJoCo 3.10、pytest、ROS2 Humble 工作空间。
+
+---
+
+## 文件结构
+
+- 新增 `xr_rm_teleop/test/j3_reference_calibration.py`：离线轨迹生成、QP/MuJoCo 评估、评分、结果输出和命令行入口。
+- 新增 `xr_rm_teleop/test/test_j3_reference_calibration.py`：轨迹端点、严格姿态插值、百分位评分、平台选择和真实模型冒烟测试。
+- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/summary.csv`：候选角度汇总。
+- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/trajectories.csv`：逐轨迹指标。
+- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/result.json`：机器可读结果。
+- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/report.md`：左右臂推荐角度、平台区间、基线对比和最差轨迹。
+
+### Task 1：用失败测试固定轨迹与评分行为
+
+**Files:**
+- Create: `xr_rm_teleop/test/test_j3_reference_calibration.py`
+- Create: `xr_rm_teleop/test/j3_reference_calibration.py`
+
+- [ ] **Step 1：写轨迹生成失败测试**
+
+测试使用以下公开接口：
+
+```python
+def build_task_trajectories(
+    initial_world_pose: np.ndarray,
+    control_rate_hz: float = 90.0,
+    max_linear_speed: float = 0.15,
+    max_angular_speed: float = 0.5,
+) -> list[Trajectory]:
+    ...
+```
+
+断言：
+
+```python
+def test_build_task_trajectories_creates_nine_strict_6d_routes() -> None:
+    initial = np.eye(4)
+    initial[:3, 3] = [0.35, 0.20, 0.10]
+
+    trajectories = build_task_trajectories(initial)
+
+    assert len(trajectories) == 9
+    assert {(route.harvest_y, route.harvest_z) for route in trajectories} == {
+        (y, z)
+        for y in (0.30, 0.40, 0.50)
+        for z in (-0.30, -0.20, -0.10)
+    }
+    for route in trajectories:
+        assert np.allclose(route.poses[0], initial)
+        assert np.allclose(route.poses[-1], initial)
+        basket = route.waypoints[5]
+        assert basket[0, 3] == pytest.approx(initial[0, 3])
+        assert basket[1, 3] == pytest.approx(initial[1, 3])
+        assert basket[2, 3] == pytest.approx(initial[2, 3] - 0.40)
+        assert basket[:3, 2] == pytest.approx([0.0, 0.0, -1.0], abs=1e-6)
+```
+
+- [ ] **Step 2：写评分和平台选择失败测试**
+
+公开接口：
+
+```python
+def rank_candidates(rows: list[CandidateMetrics]) -> list[CandidateMetrics]:
+    ...
+
+def choose_stable_platform(
+    ranked: list[CandidateMetrics],
+    scan_step_deg: float,
+) -> tuple[float, tuple[float, float]]:
+    ...
+```
+
+测试构造三个硬门槛相同的候选，断言评分严格等于：
+
+```python
+score = (
+    0.45 * r_sigma
+    + 0.20 * r_q4
+    + 0.20 * r_elbow
+    + 0.10 * r_smooth
+    + 0.05 * r_track
+)
+```
+
+并断言连续候选均达到最高分的 98% 时返回平台中点，而不是孤立端点。
+
+- [ ] **Step 3：运行测试并确认按预期失败**
+
+Run:
+
+```bash
+source /opt/ros/humble/setup.bash
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
+```
+
+Expected: FAIL，原因是 `j3_reference_calibration` 或公开函数尚不存在。
+
+### Task 2：实现最小轨迹与评分模块
+
+**Files:**
+- Create: `xr_rm_teleop/test/j3_reference_calibration.py`
+- Test: `xr_rm_teleop/test/test_j3_reference_calibration.py`
+
+- [ ] **Step 1：实现旋转和 SE(3) 插值**
+
+只使用 NumPy 和标准库：
+
+```python
+def rotation_angle(rotation: np.ndarray) -> float:
+    cosine = np.clip((np.trace(rotation) - 1.0) * 0.5, -1.0, 1.0)
+    return float(math.acos(cosine))
+
+
+def rotation_vector(rotation: np.ndarray) -> np.ndarray:
+    angle = rotation_angle(rotation)
+    if angle <= 1e-12:
+        return np.zeros(3)
+    axis = np.array([
+        rotation[2, 1] - rotation[1, 2],
+        rotation[0, 2] - rotation[2, 0],
+        rotation[1, 0] - rotation[0, 1],
+    ]) / (2.0 * math.sin(angle))
+    return axis * angle
+
+
+def interpolate_pose(start: np.ndarray, end: np.ndarray, count: int) -> list[np.ndarray]:
+    relative = end[:3, :3] @ start[:3, :3].T
+    vector = rotation_vector(relative)
+    return [
+        make_pose(
+            start[:3, 3] + alpha * (end[:3, 3] - start[:3, 3]),
+            so3_exp(alpha * vector) @ start[:3, :3],
+        )
+        for alpha in np.linspace(0.0, 1.0, count + 1)[1:]
+    ]
+```
+
+`rotation_vector` 对接近 180° 的情况使用特征向量兜底，避免工具朝下转换产生除零。
+
+- [ ] **Step 2：实现九条本侧完整轨迹**
+
+定义不可变数据类：
+
+```python
+@dataclass(frozen=True)
+class Trajectory:
+    name: str
+    harvest_y: float
+    harvest_z: float
+    waypoints: tuple[np.ndarray, ...]
+    poses: tuple[np.ndarray, ...]
+```
+
+航点固定为：初始、预接近、采摘、预接近、筐上方、筐内、筐上方、初始。每段点数为：
+
+```python
+duration = max(
+    translation_distance / max_linear_speed,
+    rotation_distance / max_angular_speed,
+)
+steps = max(1, math.ceil(duration * control_rate_hz))
+```
+
+- [ ] **Step 3：实现百分位排名和并列评分**
+
+同值获得同一百分位，单一取值获得 1.0。平滑性和跟踪排名分别定义为：
+
+```python
+r_smooth = 0.5 * rank_low(motion_cost) + 0.5 * rank_low(max_joint_speed)
+r_track = 0.5 * rank_low(max_position_error) + 0.5 * rank_low(max_orientation_error)
+```
+
+硬门槛按 `(N_fail, -N_complete)` 字典序先筛选；只有满足位置误差、姿态误差、J4、
+关节限位、速度和跳变条件的候选进入综合评分。
+
+- [ ] **Step 4：运行测试确认通过**
+
+Run:
+
+```bash
+source /opt/ros/humble/setup.bash
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
+```
+
+Expected: 轨迹与评分测试 PASS。
+
+### Task 3：用失败测试固定真实 Placo/MuJoCo 单轨迹评估
+
+**Files:**
+- Modify: `xr_rm_teleop/test/test_j3_reference_calibration.py`
+- Modify: `xr_rm_teleop/test/j3_reference_calibration.py`
+
+- [ ] **Step 1：写真实模型冒烟失败测试**
+
+接口：
+
+```python
+def evaluate_trajectory(
+    arm: str,
+    trajectory: Trajectory,
+    variant: Variant,
+    urdf_path: Path,
+    initial_joint_degrees: tuple[float, ...],
+) -> TrajectoryMetrics:
+    ...
+```
+
+使用左臂从初始 TCP 沿公共 `+Y` 移动 1 mm 的两点轨迹，断言：
+
+```python
+assert metrics.cycles == 2
+assert metrics.failures == 0
+assert math.isfinite(metrics.min_sigma)
+assert metrics.min_q4_margin_deg > 0.0
+assert metrics.max_position_error_m <= 2e-3
+assert metrics.max_orientation_error_rad <= 5e-3
+```
+
+测试还将求得的七关节状态写入 `DualArmKinematicModel` 并断言按名称读回一致。
+
+- [ ] **Step 2：运行冒烟测试并确认按预期失败**
+
+Run:
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py::test_evaluate_trajectory_uses_real_placo_and_mujoco -q
+```
+
+Expected: FAIL，原因是评估器尚未实现。
+
+- [ ] **Step 3：实现三类 QP 变体**
+
+```python
+@dataclass(frozen=True)
+class Variant:
+    name: str
+    q3_reference_deg: float | None
+    enable_manipulability: bool
+    q4_min_deg: float | None
+```
+
+- `original`：三个可选项均关闭；
+- `manip_j4`：位置可操作度权重 `1e-4`，J4 硬下限 10°；
+- `q3_<angle>`：在 `manip_j4` 基础上加入 J3 软任务，权重 `1e-5`。
+
+J4 约束使用 Placo 0.9.4 的 `add_joint_space_half_spaces_constraint(A, b)`，构造
+`-q4 <= -q4_min`。J3 使用 `add_joints_task()`；位置可操作度使用
+`add_manipulability_task(tcp_frame, "position", 1.0)`。
+
+- [ ] **Step 4：实现逐周期评估与失败保持**
+
+每个目标点前将上一有效关节状态同步给 Placo。求解失败时：
+
+```python
+failures += 1
+solver.update_joint_state(last_valid_joints)
+current_joints = last_valid_joints.copy()
+```
+
+不把失败后的 Placo 内部迭代状态带到下一周期。成功状态写入 MuJoCo，并记录六维
+雅可比最小奇异值、J4 余量、肘部外展量、TCP 误差、关节速度和运动代价。
+
+- [ ] **Step 5：运行全部标定脚本测试**
+
+Run:
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
+```
+
+Expected: 全部 PASS。
+
+### Task 4：运行粗扫、细扫并生成结果
+
+**Files:**
+- Modify: `xr_rm_teleop/test/j3_reference_calibration.py`
+- Generate: `docs/superpowers/results/2026-08-12-rm75-j3-calibration/*`
+
+- [ ] **Step 1：实现命令行和结果输出**
+
+命令行：
+
+```bash
+python j3_reference_calibration.py \
+  --urdf <path> \
+  --config <dual_arm_rm75.yaml> \
+  --output-dir <directory> \
+  --phase coarse|fine|all
+```
+
+粗扫结束后对每侧选择最高分候选，在其 ±10°、原扫描边界内以 2° 细扫。CSV 使用
+`csv.DictWriter`，JSON 使用 `json.dump`，Markdown 报告由同一汇总对象生成，不新增依赖。
+
+- [ ] **Step 2：运行完整仿真标定**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
+/home/robot/miniconda3/envs/xr/bin/python \
+  src/xr_rm_teleop/test/j3_reference_calibration.py \
+  --urdf src/xr_rm_teleop/models/dual_rm75/Dual_arm.urdf \
+  --config src/xr_rm_bringup/config/dual_arm_rm75.yaml \
+  --output-dir src/docs/superpowers/results/2026-08-12-rm75-j3-calibration \
+  --phase all
+```
+
+Expected: 左右臂粗扫和细扫完成；输出两个基线、全部候选、推荐角度和平台区间。
+
+- [ ] **Step 3：检查结果完整性**
+
+Run:
+
+```bash
+/home/robot/miniconda3/envs/xr/bin/python - <<'PY'
+import json
+from pathlib import Path
+
+path = Path('src/docs/superpowers/results/2026-08-12-rm75-j3-calibration/result.json')
+data = json.loads(path.read_text(encoding='utf-8'))
+assert set(data['arms']) == {'left', 'right'}
+for arm in data['arms'].values():
+    assert arm['coarse_candidates']
+    assert arm['fine_candidates']
+    assert arm['recommended_reference_deg'] is not None
+    assert len(arm['stable_interval_deg']) == 2
+print('result integrity: OK')
+PY
+```
+
+Expected: `result integrity: OK`。
+
+### Task 5：工作空间验证与结果复核
+
+**Files:**
+- Verify only.
+
+- [ ] **Step 1：运行新增测试和相关现有测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py \
+  src/xr_rm_teleop/test/test_placo_transforms.py \
+  src/xr_rm_mujoco/test/test_dual_arm_simulator.py -q
+```
+
+Expected: 全部 PASS。
+
+- [ ] **Step 2：按项目规则构建工作空间**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+Expected: 相关 ROS2 包构建成功。
+
+- [ ] **Step 3：运行姿态控制回归测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_orientation_control.py -q
+```
+
+Expected: 全部 PASS。
+
+- [ ] **Step 4：人工复核结果报告**
+
+确认：
+
+- 每侧确有 9 条完整轨迹；
+- `original`、`manip_j4` 和 J3 候选均存在；
+- 推荐角来自硬门槛通过集合；
+- 平台选择符合 98% 规则；
+- 报告明确列出失败轨迹，且没有把失败更多的候选排到前面；
+- 没有修改生产控制器和 YAML。
diff --git a/docs/superpowers/specs/2026-08-12-rm75-j3-simulation-calibration-design.md b/docs/superpowers/specs/2026-08-12-rm75-j3-simulation-calibration-design.md
new file mode 100644
index 0000000..3b64ea2
--- /dev/null
+++ b/docs/superpowers/specs/2026-08-12-rm75-j3-simulation-calibration-design.md
@@ -0,0 +1,245 @@
+# RM75 双臂 J3 参考角仿真标定设计
+
+## 1. 目标
+
+在不连接真机、不修改现有生产控制参数的前提下，基于当前双臂 URDF、Placo QP
+求解器和 MuJoCo 运动学模型，分别标定左臂与右臂的第三关节软引导参考角：
+
+\[
+q_{3,\mathrm{ref}}^{L,*},\qquad q_{3,\mathrm{ref}}^{R,*}。
+\]
+
+标定结果只作为当前机器人初始姿态、采摘区域、工具安装和本侧收集筐布局下的
+仿真初值。后续必须通过 mock 完整控制链路和真机低速试验复验，允许根据实测结果
+更新参数。
+
+## 2. 范围
+
+本轮只做离线参数标定：
+
+- 左右臂分别从当前 YAML 初始关节姿态出发；
+- 左臂放入左臂初始 TCP 下方约 40 cm 的本侧收集筐；
+- 右臂放入右臂初始 TCP 下方约 40 cm 的本侧收集筐；
+- 每个轨迹点同时指定 TCP 位置与姿态，保持严格六维跟踪；
+- 第四关节下限暂定左右臂均为 10°；
+- 使用现有 Placo 任务接口临时加入 J3 软任务和位置可操作度任务；
+- 将每个有效关节结果同步写入现有 MuJoCo 双臂模型，检查关节映射和状态有效性；
+- 输出候选角度的逐轨迹指标、汇总排名和推荐平台区间。
+
+本轮不修改 `placo_ik_solver.py`、遥操作节点或 YAML，不测试真机，不加入任务阶段
+状态机、自动姿态放松、碰撞规划或新依赖。
+
+## 3. 坐标与姿态约定
+
+- 双臂机器人公共坐标系 `+Y` 为正前方；
+- 公共坐标系 `+Z` 为机器人垂直向上；
+- 左右方向使用公共坐标系 `X`；
+- 采摘点保持对应机械臂初始 TCP 的横向 `X` 位置；
+- 采摘与退出阶段保持初始 TCP 姿态；
+- 从退出点移动到收集筐上方时，TCP 姿态采用四元数球面插值，平滑旋转为工具工作
+  轴沿公共坐标系 `-Z`；
+- 收集筐上方至筐内的垂直下降段保持工具朝下姿态；
+- 返回初始位姿时平滑恢复初始 TCP 姿态。
+
+“严格六维”表示每个时刻的位置和姿态目标均参与同一 QP，仿真不会因接近奇异点
+而自动降低姿态权重。
+
+## 4. 轨迹族
+
+### 4.1 采摘点
+
+每条机械臂使用 3 个前向距离和 3 个高度：
+
+\[
+y_h\in\{0.30,0.40,0.50\}\ \mathrm{m},
+\]
+
+\[
+z_h\in\{-0.30,-0.20,-0.10\}\ \mathrm{m}。
+\]
+
+这些点位于用户给定的前方 30～50 cm、相对机械臂基座高度 ±50 cm 范围内，并且
+是当前固定初始工具姿态下离线预扫描得到的主要可解高度区间。左右臂各 9 条轨迹，
+共 18 条完整轨迹。
+
+### 4.2 单条完整轨迹
+
+每条轨迹由以下连续段组成：
+
+1. 初始 TCP 位姿；
+2. 采摘点前方 5 cm 的预接近点；
+3. 沿公共 `+Y` 直线进入采摘点；
+4. 沿原路径退回预接近点；
+5. 移动到本侧收集筐上方 10 cm，同时平滑旋转到工具朝下；
+6. 垂直下降 10 cm，到达初始 TCP 下方约 40 cm 的收集筐目标；
+7. 垂直抬升 10 cm；
+8. 返回初始 TCP 位姿。
+
+轨迹按当前控制频率 90 Hz 离散，平移速度不超过 0.15 m/s，角速度不超过
+0.5 rad/s。每条轨迹均从相同初始关节状态重新开始，避免上一候选角或上一轨迹的
+状态污染下一次评估。
+
+### 4.3 边界轨迹
+
+工作区边界、不可达目标和 QP 失败恢复轨迹不参与第一轮 J3 参数排名。J3 参数确定后，
+再使用这些轨迹验证失败保持和恢复逻辑，避免不可达点数量掩盖参考角本身的差异。
+
+## 5. QP 试验配置
+
+主任务保持现有严格六维相对位姿任务，内部等价于：
+
+\[
+\left\|J_p\Delta q-e_p\right\|^2
++\left\|J_R\Delta q-e_R\right\|^2，
+\]
+
+其中误差定义为目标减当前。仿真脚本不改变 Placo 的误差符号。
+
+在主任务之外临时加入：
+
+\[
+w_e\left(q_3+\Delta q_3-q_{3,\mathrm{ref}}\right)^2,
+\qquad w_e=10^{-5}，
+\]
+
+以及 TCP 位置可操作度任务：
+
+\[
+-w_m\nabla m_p(q)^T\Delta q,
+\qquad w_m=10^{-4}。
+\]
+
+保留当前动能正则、URDF 关节位置限制和关节速度限制。第四关节临时增加：
+
+\[
+q_4\ge10^\circ。
+\]
+
+## 6. 参数扫描
+
+### 6.1 粗扫
+
+左臂：
+
+\[
+q_{3,\mathrm{ref}}^L\in\{0^\circ,10^\circ,\ldots,100^\circ\}。
+\]
+
+右臂：
+
+\[
+q_{3,\mathrm{ref}}^R\in\{0^\circ,-10^\circ,\ldots,-120^\circ\}。
+\]
+
+### 6.2 细扫
+
+在粗扫最优候选附近 ±10° 内以 2° 为步长再次扫描。若多个相邻候选没有明显差异，
+选择稳定平台区的中心，而不是选择孤立的单点峰值。
+
+### 6.3 基线
+
+同时运行两组基线：
+
+- `original`：当前原始 QP，不含 J3 软任务、位置可操作度任务和 J4 额外下限；
+- `manip_j4`：不含 J3 软任务，但加入位置可操作度任务和 J4 额外下限。
+
+所有 J3 候选均在 `manip_j4` 基础上只改变 J3 参考角。最终结果必须同时报告：
+
+- 相对当前原始 QP 的改善；
+- 相对“只加位置可操作度”的改善；
+- J3 软任务是否降低六维跟踪成功率。
+
+## 7. 记录指标
+
+对每个候选角度、每条轨迹记录：
+
+- QP 求解失败周期数 `N_fail`；
+- 完成全部轨迹的数量 `N_complete`；
+- 整条轨迹六维雅可比的最小奇异值 `sigma_min`；
+- 第四关节最小安全余量 `m_q4 = min(q4 - 10°)`；
+- 肘部最小外展量 `d_elbow`；
+- 最大 TCP 位置误差和姿态误差；
+- 最大关节速度；
+- 累计关节运动代价 `E_q`；
+- 是否出现超过阈值的单周期关节构型跳变。
+
+肘部外展量使用公共坐标系中第四连杆位置计算：
+
+\[
+d_{\mathrm{elbow}}^L=-x_{\mathrm{elbow}}^L,
+\qquad
+d_{\mathrm{elbow}}^R=x_{\mathrm{elbow}}^R。
+\]
+
+当前 URDF 碰撞网格对相邻连杆存在已知自碰撞警告，因此本轮不把 MuJoCo/Placo
+碰撞距离加入评分，避免错误碰撞几何影响 J3 选择。
+
+## 8. 选择规则与评分函数
+
+### 8.1 硬门槛
+
+候选角度首先按以下顺序筛选：
+
+1. `N_fail` 最少；
+2. `N_complete` 最多；
+3. 最大位置误差不超过 2 mm；
+4. 最大姿态误差不超过 0.005 rad；
+5. 不违反第四关节、URDF 关节位置和速度限制；
+6. 不出现超过配置阈值的单周期关节跳变。
+
+只在通过同一组硬门槛的候选之间使用评分函数。这样不能用较高可操作度抵消更多的
+QP 失败或更差的 TCP 跟踪。
+
+### 8.2 并列候选评分函数
+
+对通过硬门槛的候选，将各项指标在同一机械臂的候选集合内转换为 `[0,1]` 的百分位
+排名。数值越大越好的指标直接排名，数值越小越好的指标反向排名：
+
+- `r_sigma`：全轨迹最小奇异值排名；
+- `r_q4`：第四关节最小安全余量排名；
+- `r_elbow`：肘部最小外展量排名；
+- `r_smooth`：累计关节运动代价与最大关节速度的联合反向排名；
+- `r_track`：最大六维 TCP 跟踪误差的反向排名。
+
+并列候选的综合评分为：
+
+\[
+S(q_{3,\mathrm{ref}})
+=0.45r_{\sigma}
++0.20r_{q4}
++0.20r_{\mathrm{elbow}}
++0.10r_{\mathrm{smooth}}
++0.05r_{\mathrm{track}}。
+\]
+
+选择：
+
+\[
+q_{3,\mathrm{ref}}^*=\arg\max S(q_{3,\mathrm{ref}})。
+\]
+
+最小奇异值权重最高，因为本轮首要目标是降低奇异点和 QP 失败风险；第四关节余量
+和肘部外展各占 0.20；平滑性和跟踪误差用于区分性能接近的候选。若评分最高点与
+相邻角度差异小于 2%，取相邻稳定平台的中心角度。
+
+### 8.3 结果报告
+
+左右臂分别输出：
+
+- 推荐参考角；
+- 推荐稳定区间；
+- 粗扫与细扫排名表；
+- 与两组基线的指标对比；
+- 最差轨迹及其失败位置；
+- 是否建议保留左右臂统一的第四关节 10° 下限。
+
+## 9. 实施边界与后续流程
+
+标定完成后的顺序为：
+
+1. 根据仿真结果形成正式 QP 修改规格；
+2. 将左右臂 J3 参考角作为独立可调参数写入对应 YAML；
+3. 在 `use_mock:=true` 下运行完整遥操作控制链路；
+4. 加入 QP 失败时不提交笛卡尔目标历史的修复并验证恢复；
+5. 经过安全评审后，在真机上以低速、小范围方式复验；
+6. 根据真机日志更新 J3 参考角，但不取消工作空间、速度、超时和安全停止限制。