1. G2 轨迹桥收尾(上午)

- 修复 Humble rclpy 的 action 结果码问题(execute 回调重构)
- 真机验证:小行程轨迹、取消保持、起点拒绝,全部通过

2. G3:MoveIt 从零打通(耗时最长)

- 安装 MoveIt + 建 moveit_config 包
- 参数结构调试(卡最久):对照官方 panda 配置,确认 Humble 的参数名——kinematics
在 robot_description_kinematics、管线在 planning_pipelines+顶层 ompl
子树、参数必须走 launch 内联 dict
- 碰撞模型:vendor STL 数据污染(±0.5/±1/±99 多层垃圾顶点)+ 网格巨大(单文件 39
万三角形)→ 改用人工原语盒体,启动 4 分钟 → 2 秒
- 桌面参数:猜测 1.0m 导致肩膀"插进"桌子,你实测 73.6cm/89cm 后一切正常
- 验证通过:RRT-Connect 规划 + 笛卡尔 fraction=1.000

3. 模型按实测修正(根因排查)

- 臂长:你量肩→指尖 85cm,URDF 缩放 1.043(0.677→0.70m reach)
- 躯干:肩高 1.17m → 髋关节 z 修正 0.37→0.44
- 相机高度 145cm 验证一致 ✓
- 感知映射:深度读数偏大 ~11%,body 系 x 偏大 ~17cm、z 偏大 ~8cm → 加
PERCEPTION_OFFSET 校正

4. 感知鲁棒性

- 轴线估计改为多采样+中值过滤(修掉下 1/4 点落到桌面把轴线拉歪)
- 修掉图像 y 轴方向搞反的 bug(顶底互换)
- 识别出检测在多个目标间跳动的干扰

5. G5:抓取集成(下午)

- pick_plan.py:检测 → 候选 → RRT → 直线接近,全链路打通
- 你要侧抓 → 加侧抓候选(直立瓶补水平接近)+ --side 模式
- 修掉四元数旋转 bug、MoveIt 笛卡尔 IK 分支卡死(换本地链式 IK)
- 按你的三条意见重构:
- 拼接连续:倒推链(从抓取构型反向走直线)→ RRT 关节空间目标(容差 0.002 rad)→
拼接点最大差 0.11°
- 时间戳完整:段2 每步 0.25s,桥按时间插值
- 安全:瓶子加进场景、逐路点碰撞检查、无碰撞通过

最终状态

- 侧抓完整轨迹已生成:RRT 41 点(3.97s)+ 直线 21 点(5.0s),总
8.97s,拼接连续、无碰撞、带时间戳,存于 /tmp/grasp_traj.json
- 待执行:轨迹经 x86 桥下发,机械臂会动(等你确认)

今天暴露的系统性问题(后续要处理)

1. URDF 与实物多处尺寸偏差(腿/臂/感知映射)——建议做一次完整的几何标定
2. 感知深度 ~11% 系统偏差——需要多距离标定
3. vendor STL 数据污染——碰撞模型需长期方案(或用 vendor 修好的 mesh)