结果不尽人意
这次实验没有得到一个可以稳定演示的自动抓取系统。
机器狗能够建图，能够在地图上完成基础直线导航。D1 机械臂能够在 RViz 里用 MoveIt2 规划姿态，也能够通过 MQTT 桥接控制真实机械臂。D435i 能够输出深度数据，YOLO 能够识别目标，ArUco 也帮助我们完成了相机和机械臂之间的标定。
这些模块分别拿出来看，都已经跑通了一部分。它们真正连到一起以后，物体仍然没有被稳定抓起来。
这就是我想记录这次实验的原因。机器人项目最容易被忽略的部分，往往发生在模块交界处。单独测试时一切正常，接起来以后，坐标系、通信、机械结构和硬件状态会一起找上门来。
这次实验到底做了什么
我们把实验分成两条线。
第一条线是机器狗的 SLAM 和导航。Go2 使用 Livox Mid-360 激光雷达完成环境建图，地图以 PCD 点云文件保存。之后通过宇树官方的 SLAM 服务接口完成重定位和目标点导航。我们还把地图重新加载到 RViz 中，做了三维点云可视化。
第二条线是机械臂和深度视觉。D1 的模型先进入 RViz 和 MoveIt2，之后再通过 MQTT 桥接和机械臂 SDK 把规划轨迹送到真机。D435i 安装在机器狗前方，负责采集彩色图和深度图。YOLO 从彩色图里找到物体，深度图再给出目标的大致三维位置。
从纸面上看，流程很顺。
机器狗建图并接近目标
↓
D435i 采集彩色图和深度图
↓
YOLO 识别物体
↓
深度反投影得到相机坐标
↓
手眼标定转换到机械臂基座坐标
↓
MoveIt2 规划抓取姿态
↓
D1 机械臂执行抓取真正的实验过程没有这么整齐。
我们先把机器狗的地图做出来
早期的 patrol 程序里，SLAM 和 navigation 有不少问题。重定位、建图以及导航调用经常不能稳定工作，原来的 slamhandler 也留下了不少 bug。我们一度怀疑是 SLAM 服务本身没有正常运行。
后来把官方文档和 keyDemo 对照起来以后，问题被拆开了。宇树的 unitree_slam 服务端其实可以正常工作，官方例程也能够完成服务调用。真正卡住的是我们自己的客户端代码，包括参数组织、服务启动顺序和导航状态处理。
手动启动 SLAM 服务时，Mid-360 驱动和 unitree_slam 的顺序也不能随意交换。雷达没有准备好，SLAM 服务就可能在 YSN 检查阶段退出。这个问题看起来像是程序崩了，实际只是启动时机不对。
修正这些问题以后，我们成功生成了室内点云地图，并把 PCD 文件重新发布到 RViz 里显示。地图覆盖了实验区域，机器狗也跑通了基础直线导航。
不过，直线导航和真正的复杂巡航并不是一回事。官方的单目标点导航会让机器狗从当前位置走向一个目标点。手机 App 里的转弯路线依赖额外的拓扑节点和边，patrol 程序中也已经有相关代码，只是这一部分还需要继续修复和验证。
当前更稳妥的方案，是在拐弯处增加中间点，让机器狗用多段直线走出一条折线。这能满足抓取实验里“移动到目标附近”的需求，复杂自主导航留到后面处理。
机械臂先在 RViz 里学会动
D1 的机械臂模型进入 RViz 以后，可以用 MoveIt2 规划不同的末端姿态。我们检查了 URDF 中的 link、joint、collision、visual、inertial 和关节限制，也处理了夹爪双关节之间的耦合关系。
仿真里的机械臂动作比较顺利。真正接到真机以后，控制链路经历了一次调整。最开始设计的是 ros2_control 硬件接口，后来在 ARM64 真机部署时遇到问题，最终改成了 MQTT 桥接方案。
Dashboard 端由 MoveIt2 负责规划，fake controller 负责接收轨迹并按时间插值。轨迹一份发给 RViz 做状态显示，另一份通过 MQTT 发到机器狗，再由机器狗端的 D1 SDK 控制机械臂。
这个过程里也出现过状态同步问题。早期程序试图通过 fake 位置和 MQTT 真机位置之间的角度差，猜测一条轨迹是不是执行结束。机械臂停在目标姿态以后，位置差仍然可能超过阈值，程序因此一直认为轨迹没有完成。
后来我们改成显式的执行状态，让 bridge 知道轨迹什么时候开始、什么时候结束。这样 RViz 里的关节状态才能在真机执行后恢复同步。
相机看见了物体，机械臂还需要知道它在哪里
D435i 的深度链路也单独做过测试。在 0.3 m 距离处，我们用卷尺测量目标距离，再和中心区域的深度值比较，误差控制在 8 mm 以内。YOLO 和深度数据联动以后，目标检测框也能对应到相机坐标系下的三维位置。
但这还不等于机械臂已经知道物体在哪里。
相机给出的是相机坐标，MoveIt2 需要的是机械臂基座坐标。两套坐标之间需要一个固定变换，这个变换要通过手眼标定得到。我们使用 ArUco 标签采集多组机械臂姿态和相机观测数据，再求解相机到机械臂基座的刚体变换。
标定过程暴露出一个很典型的问题。程序里的关节绕轴方向和真机关节的实际旋转方向没有完全对应，部分关节的顺时针和逆时针被写反了。机械臂仍然能动，MoveIt2 也能规划，但模型计算出来的姿态和真机真实姿态逐渐偏离。
修正关节方向以后，旧的标定数据不能继续直接使用，我们需要重新采集和求解。后续验证中还发现，标定标签中心和夹爪尖端并不是同一个参考点，URDF 推算出的夹爪几何长度也和真机存在差异。
这也是为什么“标定程序运行成功”不能直接等同于“夹爪已经对准目标”。最后要看的还是夹爪尖端有没有真正落到目标位置。
最后卡住的是抓取空间
我们最终没有把视觉定位后的自动抓取稳定跑通，核心困难出在空间关系上。
D435i 固定在机器狗前方，机械臂也安装在狗的前侧。相机能看到的目标位置，未必处在 D1 适合向下抓取的区域。目标距离太近时，深度相机的测量会受影响。目标距离太远时，机械臂又可能够不到。机器狗停靠时只要有一点位置和朝向误差，末端就会继续偏移。
硬件状态也给实验增加了限制。机器狗加装机械臂以后，重心和腿部负荷发生变化。StaticWalk 步态下可以基本保持平衡，长时间运行时腿部电机仍然会发热，机器狗可能在运行十分钟左右后瘫倒。抓取还没有稳定以前，底盘本身已经需要先解决可靠性问题。
老师后来建议尝试把相机安装到机械臂上，从侧面观察和抓取目标。这个方向有可能改善当前固定相机和机械臂工作空间之间的冲突，同时也会带来新的标定、线缆和视野遮挡问题。
这次实验留下了什么
我们完成了机器狗 SLAM 建图和基础导航，完成了 D1 的 MoveIt2 规划和真机手动控制，也跑通了 YOLO、D435i 深度定位和 ArUco 标定链路。
自动抓取没有完成，原因也已经比实验开始时清楚很多。问题不只在某一段代码里，它同时涉及关节方向、坐标变换、真实机械臂几何、相机安装位置、机器狗重心和腿部电机状态。
下一轮实验最值得先做的事情，是重新设计相机和机械臂的安装关系，再继续调抓取动作。让机器狗先稳定站住，再让机械臂去抓一个处在合理工作空间里的目标，可能比继续修改某一个识别参数更有效。
这台机器狗还没有学会抓取。至少现在，我们已经知道它为什么还不会。