# 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 参考角，但不取消工作空间、速度、超时和安全停止限制。