acRealman_xr/docs/superpowers/specs/2026-08-24-rm75-ik-method-comparison-design.md

RM75 三种逆运动学方法离线对比实验设计

1. 目标

基于现有番茄采摘 episode 的右臂目标位姿轨迹，在完全一致的机械臂模型、初始关节 状态、时间轴、收敛判据和输出安全限制下，对比以下三种七自由度逆运动学方法：

1. Jacobian Moore-Penrose 伪逆法；
2. 阻尼最小二乘法（Damped Least Squares，DLS）；
3. 当前项目中的优化 Placo QP 方法。

实验输出用于补充中期报告 2.3.4 节预留的三张图，并同时生成逐采样数据、汇总指标和 可直接粘贴到报告中的中文结果分析。实验必须由真实计算结果驱动，不预设或硬编码 “QP 更优”的结论。

2. 现有上下文

2.1 报告要求

中期报告 2.3.4 节已经确定：

• 三种方法使用同一机械臂模型、初始关节状态和末端目标轨迹；
• 统计位置 RMSE、姿态 RMSE、归一化关节安全裕度、最大关节速度、求解时间和 求解成功率；
• 章节末尾预留三张对比图。

现有图号从图 2-10 跳到图 2-14，因此本实验生成图 2-11、图 2-12 和图 2-13。

2.2 当前 QP 与报告文字的差异

报告 2.3.2 节主要描述六维末端软任务、动能正则化、关节位置和速度限制。当前分支的 PlacoIkSolver 还包含：

• J3 初始构型软引导；
• J4 硬下限和预警区软缓冲；
• 接近奇异区时动态启用的六维可操作度任务；
• QP 失败时恢复实际关节状态并保持上一安全输出。

本实验使用当前优化 QP，而不是关闭上述辅助任务的基础 QP。最终分析文件需要提供一段 方法补充文字，避免报告方法描述与对比对象不一致。

3. 范围与安全边界

3.1 本次包含

• 只读加载一个现有右臂 episode；
• 离线重采样目标位姿；
• 在同一 URDF 上运行三种逆运动学方法；
• 复用当前 QP 代码和右臂 YAML 参数；
• 对三种方法使用相同的输出端安全处理；
• 生成 SVG、300 dpi PNG、CSV、JSON 和中文 Markdown 分析。

3.2 本次不包含

• 不连接真机，不移动机械臂，不操作夹爪；
• 不启动新的 PICO 录制；
• 不修改生产遥操作节点、launch、YAML 默认值或公开 API；
• 不使用 episode 中已经记录的 QP 关节结果充当本次 QP 结果；
• 不模拟电机、通信和接触动力学；
• 不直接编辑用户提供的 PDF。

实验只使用当前 Conda 环境已经安装的 NumPy、h5py、Matplotlib 和 Placo，不新增项目 依赖。

因此，结果应表述为“基于真实遥操作目标轨迹的离线运动学对比”，不得表述为新的真机 在线控制对比。

4. 数据源与质量基线

实验固定使用：

/home/robot/ACT_Data/tomato_pick/episode_0.hdf5

该文件的已核对属性如下：

• 机械臂：right_rm75；
• 样本数：484；
• 有效时长：约 16.1 s；
• 保存采样率：约 30 Hz；
• 位姿顺序：x,y,z,qx,qy,qz,qw；
• 所有目标位姿、当前位姿和关节状态均为有限值；
• 目标和当前四元数范数接近 1；
• 483 帧为遥操作激活且已发送命令；
• 记录时 QP 尝试 483 次并成功 483 次；
• 132 帧触发过目标限幅，轨迹本身包含足够的约束压力。

只使用满足以下条件的最长连续区间：

teleop_active && action_valid && command_sent

共同末端目标取 debug/tcp/final_target_pose。该字段已通过原系统的工作空间限制、目标 平滑和单帧笛卡尔步长限制，适合作为三种逆运动学方法的共同安全输入。共同初始关节角 取有效区间第一帧的 observations/qpos[:7]。

episode 中后续 observations/qpos、debug/qp/raw_target、QP 成功标志和耗时只用于 数据质量核对，不替代任何方法在本实验中的离线计算结果。

5. 统一复放架构

数据流为：

episode_0.hdf5
  -> 有效区间与共同初始状态
  -> 30 Hz 目标位姿重采样到 90 Hz
  -> 伪逆 / DLS / 当前优化 QP 三路独立复放
  -> 共同输出安全层
  -> 正向运动学和逐采样指标
  -> CSV / JSON / 三张图 / 中文分析

三种方法各自维护独立的关节状态和上一周期关节速度。每个方法的下一状态只能由该方法 本周期的安全输出推进，三路之间不共享可变状态。

离线状态推进采用理想位置跟随，即共同输出限速器给出的关节目标直接作为下一 90 Hz 周期的关节状态。这一简化隔离了逆运动学方法本身，不引入未建模的电机和网络差异。

6. 目标轨迹重采样

原 episode 按约 30 Hz 保存，而当前遥操作控制器使用 90 Hz。重采样使用 episode 的 debug/timestamps/control_monotonic_ns，目标时间轴保持原始起止时刻并以 1/90 s 采样：

• 位置使用分段线性插值；
• 姿态使用归一化四元数的最短弧 SLERP；
• 相邻四元数点积为负时先翻转后一四元数，避免绕长弧插值；
• 第一个和最后一个重采样位姿必须与原始有效区间端点一致；
• 不对目标轨迹额外放大、延长或人工加入困难片段。

7. 三种逆运动学方法

7.1 共同任务定义

当前关节状态为 q，正向运动学得到当前 TCP 位姿 (p, R)，目标位姿为 (p_d, R_d)。位置误差和姿态误差分别为：

e_p = p_d - p
e_R = Log(R^T R_d)

求解时的角速度误差表达必须与所用 local_world_aligned Jacobian 的坐标表达一致； 姿态误差大小统一使用目标与实际旋转矩阵之间的最短夹角评价。伪逆和 DLS 使用相同的 位置、姿态反馈增益、相同 Jacobian、相同 90 Hz 步长和相同数值迭代框架。

三种方法对单个目标最多执行 30 次数值迭代。满足以下两个条件时记为收敛：

位置误差 <= 0.002 m
姿态误差 <= 0.005 rad

7.2 Jacobian 伪逆法

伪逆法按报告公式计算：

q_dot = pinv(J) * v_d

其中 v_d 由共同的六维位姿反馈误差生成。实现直接使用 NumPy 的 Moore-Penrose 伪逆，不增加零空间任务、阻尼或自适应奇异值阈值，以保持基线定义清楚。

7.3 DLS 方法

DLS 按报告公式计算：

q_dot = J^T * inv(J * J^T + mu^2 * I) * v_d

公式保持与报告一致；数值实现使用线性方程求解，不显式计算矩阵逆。

只扫描固定阻尼系数，不实现自适应 DLS。候选值使用对数尺度的小集合：

0.001, 0.003, 0.01, 0.03, 0.1, 0.3

每个候选均完整复放 episode，先按求解成功率从高到低选择，再在成功率相同的候选中 最小化：

位置 RMSE / 0.002 + 姿态 RMSE / 0.005

若仍并列，选择最大关节速度更小的候选。阻尼扫描使用同一条评价轨迹，因此最终文字 必须说明该 DLS 是“在当前轨迹上选优的固定阻尼基线”；这一口径对 DLS 较有利，不能 将其解释为跨轨迹最优参数。

阻尼扫描耗时不计入三种方法的在线求解时间对比。

7.4 当前优化 QP

QP 直接实例化现有 xr_rm_teleop.placo_ik_solver.PlacoIkSolver，使用 90 Hz 步长、 当前双臂 URDF 和右臂配置中的参数：

qp_j3_reference_deg: -89.57
qp_j3_weight: 0.0001
qp_j4_min_deg: 10.0
qp_j4_warn_deg: 25.0
qp_j4_weight: 0.0001
qp_manipulability_sigma_stop: 0.01
qp_manipulability_sigma_warn: 0.04
qp_manipulability_weight: 0.0001

保留现有六维末端软任务、1e-6 动能正则化、URDF 关节位置和速度限制、30 次迭代、 收敛阈值、输入变换校验、失败恢复和结果有效性检查。实验脚本不复制或重写 QP。

8. 共同输出安全层与失败处理

三种方法使用同一安全口径：

1. 每次数值迭代结果必须为 7 个有限关节值；
2. 数值迭代的关节状态不得超出 URDF 位置范围；
3. 相邻数值迭代的关节变化不得超过 URDF 速度上限乘以 1/90 s；
4. 有效求解结果继续经过生产控制器现有的关节速度/加速度限制逻辑；
5. 输出端最大关节速度为 180 deg/s，最大关节加速度为 300 deg/s^2；
6. 未在 30 次内收敛、出现非有限值或违反硬边界时，本周期记为失败并保持上一安全 关节状态；
7. 失败不会停止离线复放，时间轴继续推进，并记录失败次数和最长连续失败长度。

QP 在优化内部主动处理关节边界；伪逆和 DLS 在每次候选步之后接受同样的硬检查。 共同输出层不会消除算法差异：基线仍可能因候选步无效而失败或保持，QP 则可能在优化 过程中找到满足约束的解。

9. 评价指标

9.1 末端跟踪

• 逐采样位置误差 ||p_d - p||，单位为 mm；
• 逐采样姿态夹角误差，单位为 degree；
• 全轨迹位置 RMSE，报告中仍以 m 给出，图中用 mm；
• 全轨迹姿态 RMSE，报告公式使用 rad，图中用 degree。

9.2 关节运动

• 每个采样时刻七关节绝对速度的最大值，单位为 deg/s；
• 全轨迹最大关节速度；
• 超过或触发共同速度/加速度限制器的周期数；
• 按报告式 (2-28) 计算的逐采样最小归一化关节安全裕度；
• 全轨迹最小归一化关节安全裕度。

速度图不再使用归一化速度。当前 URDF 中右臂七个关节的速度上限均为 3.14 rad/s （约 180 deg/s），直接展示实际速度更直观且与报告文字一致。

9.3 求解性能

• 收敛成功周期数和成功率；
• 失败周期数和最长连续失败长度；
• 单周期 IK 求解平均时间和最大时间，单位为 ms。

耗时只覆盖单次 IK 求解，不包含 HDF5 读取、重采样、指标汇总和绘图。先执行一次完整 预热复放，再对选定参数的三种方法各重复 10 次。轨迹和非耗时指标必须在重复复放间 保持确定；平均和最大耗时从 10 次计时复放汇总。

10. 三张图设计

10.1 图 2-11 三种逆运动学方法末端位姿跟踪误差对比

使用上下两个共享时间轴的子图：

• (a) 位置误差时序，单位 mm；
• (b) 姿态误差时序，单位 degree。

三种方法使用固定颜色、不同线型，并在失败保持区间添加不遮挡曲线的标记。图中不绘制 episode 原始 QP 误差曲线。

10.2 图 2-12 三种逆运动学方法关节运动约束对比

使用两个共享时间轴的子图：

• (a) 每个时刻的最大关节速度，单位 deg/s，并绘制 180 deg/s 虚线；
• (b) 每个时刻的最小归一化关节安全裕度，数值越大表示离关节上下限越远。

10.3 图 2-13 三种逆运动学方法综合性能指标对比

使用 2 x 3 六个小型分组柱状图，避免不同量纲共用坐标轴：

1. 位置 RMSE；
2. 姿态 RMSE；
3. 最大关节速度；
4. 最小归一化关节安全裕度；
5. 平均和最大求解时间；
6. 求解成功率。

柱顶标注精确数值。最终配色需兼顾色盲识别和灰度打印，除颜色外再使用线型、标记和 图例区分方法。

11. 文件与产物

实验实现优先保持最小范围：

xr_rm_teleop/test/ik_method_comparison.py
xr_rm_teleop/test/test_ik_method_comparison.py

前者包含命令行入口、HDF5 读取、重采样、三种方法复放、指标计算和绘图；后者只覆盖 无法由现有测试保护的新非平凡逻辑，不新增测试框架或通用评测抽象。

默认输出目录为：

output/ik_comparison/episode_0/

产物包括：

samples.csv
summary.json
figure_2_11_tracking_error.svg
figure_2_11_tracking_error.png
figure_2_12_joint_constraints.svg
figure_2_12_joint_constraints.png
figure_2_13_summary.svg
figure_2_13_summary.png
analysis_2.3.4.md

samples.csv 使用长表结构，每行对应“方法 + 时间点”，至少包含目标位姿、实际位姿、 位置误差、姿态误差、七关节角、七关节速度、最小安全裕度、求解耗时、成功标志和限制 触发标志。summary.json 保存输入路径、Git 提交、参数、选定 DLS 阻尼、指标和产物路径， 保证结果可追溯。

analysis_2.3.4.md 使用中文撰写，包含：

• 数据来源和离线实验口径；
• DLS 最终阻尼和选择规则；
• 三张图的建议图题与图注；
• 与式 (2-25) 至式 (2-28) 对应的数值结果；
• 对优势、代价和异常结果的客观分析；
• 当前优化 QP 相对报告 2.3.2 节的补充方法说明。

12. 错误处理

以下情况在生成任何正式图前立即报错：

• episode 路径不存在或不是 HDF5；
• 必需字段或属性缺失；
• 数组长度不一致；
• 找不到至少包含两个样本的连续有效遥操作区间；
• 时间戳非严格递增；
• 位姿、关节角或四元数含 NaN/Inf；
• 四元数无法正规化；
• episode 机械臂不是 right_rm75；
• URDF 或当前右臂配置不存在；
• Placo 版本不是项目固定的 0.9.4；
• 任一方法没有生成与统一时间轴等长的结果；
• CSV、JSON 和绘图使用的汇总数值不一致。

单个目标的逆运动学失败属于实验结果，按上一安全状态保持，不中止整条轨迹。输入数据 结构错误、模型错误和结果长度错误属于实验无效，必须中止并说明原因。

13. 测试与验证

13.1 聚焦测试

最小测试至少覆盖：

• 30 Hz 到 90 Hz 重采样保持首尾位置和姿态；
• SLERP 选择最短弧并输出单位四元数；
• 姿态夹角误差在单位旋转和已知小角度下正确；
• 关节安全裕度与式 (2-28) 一致；
• 无效候选触发失败保持而不是推进状态；
• DLS 选择规则按成功率、归一化误差和最大速度依次决策；
• 汇总指标与逐采样数据一致。

13.2 真实模型冒烟验证

使用当前双臂 URDF 和右臂初始关节角，对三种方法各运行一小段真实目标位姿序列，确认：

• 输出始终为有限 7 维关节值；
• 没有输出越过 URDF 关节位置边界；
• 失败时保持上一安全状态；
• 当前 QP 直接走现有 PlacoIkSolver，没有本地复制实现。

13.3 项目级验证

从工作空间根目录 /home/robot/WS_xr 执行，并先加载 ROS2 Humble：

source /opt/ros/humble/setup.bash
colcon build --symlink-install
pytest src/xr_rm_teleop/test/test_orientation_control.py

随后使用项目固定的 Conda Python 运行聚焦测试和完整离线实验。验证完成后还需检查：

• 三张 PNG 无裁切、重叠、乱码或不可辨识曲线；
• SVG 可编辑且文字完整；
• PNG 为 300 dpi；
• 图题、坐标轴、单位和图例为中文论文风格；
• summary.json 与图中柱顶数值一致；
• 同一输入重复运行时，除耗时外的结果一致。

14. 验收标准

满足以下条件才视为完成：

1. 三种方法从完全相同的 episode 目标轨迹和初始关节状态开始；
2. 当前优化 QP 复用现有实现和右臂参数；
3. 三种方法使用同一输出安全口径，任何失败均安全保持；
4. DLS 固定阻尼选择过程和最终值可追溯；
5. 生成三张与报告公式和图号一致的正式对比图；
6. 生成完整 CSV、JSON 和中文 2.3.4 分析文字；
7. 所有实际执行的测试和构建结果如实记录；
8. 不连接真机、不修改生产控制默认值、不新增依赖和重复 QP 实现。