# 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. 数据源与质量基线 实验固定使用： ```text /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 帧触发过目标限幅，轨迹本身包含足够的约束压力。 只使用满足以下条件的最长连续区间： ```text teleop_active && action_valid && command_sent ``` 共同末端目标取 `debug/tcp/final_target_pose`。该字段已通过原系统的工作空间限制、目标 平滑和单帧笛卡尔步长限制，适合作为三种逆运动学方法的共同安全输入。共同初始关节角 取有效区间第一帧的 `observations/qpos[:7]`。 episode 中后续 `observations/qpos`、`debug/qp/raw_target`、QP 成功标志和耗时只用于 数据质量核对，不替代任何方法在本实验中的离线计算结果。 ## 5. 统一复放架构 数据流为： ```text 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)`。位置误差和姿态误差分别为： ```text e_p = p_d - p e_R = Log(R^T R_d) ``` 求解时的角速度误差表达必须与所用 `local_world_aligned` Jacobian 的坐标表达一致； 姿态误差大小统一使用目标与实际旋转矩阵之间的最短夹角评价。伪逆和 DLS 使用相同的 位置、姿态反馈增益、相同 Jacobian、相同 90 Hz 步长和相同数值迭代框架。 三种方法对单个目标最多执行 30 次数值迭代。满足以下两个条件时记为收敛： ```text 位置误差 <= 0.002 m 姿态误差 <= 0.005 rad ``` ### 7.2 Jacobian 伪逆法 伪逆法按报告公式计算： ```text q_dot = pinv(J) * v_d ``` 其中 `v_d` 由共同的六维位姿反馈误差生成。实现直接使用 NumPy 的 Moore-Penrose 伪逆，不增加零空间任务、阻尼或自适应奇异值阈值，以保持基线定义清楚。 ### 7.3 DLS 方法 DLS 按报告公式计算： ```text q_dot = J^T * inv(J * J^T + mu^2 * I) * v_d ``` 公式保持与报告一致；数值实现使用线性方程求解，不显式计算矩阵逆。 只扫描固定阻尼系数，不实现自适应 DLS。候选值使用对数尺度的小集合： ```text 0.001, 0.003, 0.01, 0.03, 0.1, 0.3 ``` 每个候选均完整复放 episode，先按求解成功率从高到低选择，再在成功率相同的候选中 最小化： ```text 位置 RMSE / 0.002 + 姿态 RMSE / 0.005 ``` 若仍并列，选择最大关节速度更小的候选。阻尼扫描使用同一条评价轨迹，因此最终文字 必须说明该 DLS 是“在当前轨迹上选优的固定阻尼基线”；这一口径对 DLS 较有利，不能 将其解释为跨轨迹最优参数。 阻尼扫描耗时不计入三种方法的在线求解时间对比。 ### 7.4 当前优化 QP QP 直接实例化现有 `xr_rm_teleop.placo_ik_solver.PlacoIkSolver`，使用 90 Hz 步长、 当前双臂 URDF 和右臂配置中的参数： ```text 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. 文件与产物 实验实现优先保持最小范围： ```text xr_rm_teleop/test/ik_method_comparison.py xr_rm_teleop/test/test_ik_method_comparison.py ``` 前者包含命令行入口、HDF5 读取、重采样、三种方法复放、指标计算和绘图；后者只覆盖 无法由现有测试保护的新非平凡逻辑，不新增测试框架或通用评测抽象。 默认输出目录为： ```text output/ik_comparison/episode_0/ ``` 产物包括： ```text 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： ```bash 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 实现。