第五章：抓取放置功能实现

  第四章已经解决了机器人如何在场景中移动的问题。本章继续解决另一个核心任务：Bobac 到达桌子附近之后，如何识别桌面目标，并完成抓取和放置。

  抓取放置不是一个单独动作，而是一条连续流程。机器人需要先看见目标，再判断目标在哪里、怎么摆放，然后规划机械臂动作，最后配合夹爪完成夹取和松开。

观察桌面
→ 识别目标
→ 计算三维位置
→ 转换到机械臂坐标系
→ 生成抓取动作
→ 控制夹爪
→ 放置并撤离

  本章按这条流程展开。每一节先讲清楚这个功能解决什么问题，再给出验证方法。示例程序只用于帮助大家检查各环节是否打通，不作为完整比赛答案。

提示: 以下方案仅供参考，参赛选手可自行选择适合自己的方案进行开发

5.1 抓取放置任务拆解

5.1.1 机器人抓笔需要哪些判断

  人抓一支笔时，会自然完成下面这些判断：

桌面上有没有笔
哪一个物体是笔
笔离手有多远
笔是横着、竖着还是斜着
手应该从哪个方向靠近
什么时候张开手指
什么时候夹住
夹住后放到哪里

  机器人也要完成同样的判断，只是每一步都必须变成明确的数据输入和输出。

阶段	要解决的问题	输出结果
观察	相机是否看到桌面目标	RGB 图像、深度图
识别	图像中哪一块是目标物体	类别、检测框、mask
定位	目标相对相机在哪里	相机坐标系下的三维点
方向估计	目标怎么摆放	目标长轴方向
坐标转换	目标在机械臂坐标系下在哪里	base_link_arm 下的目标点和方向
运动规划	机械臂怎样靠近目标	预抓取点、抓取点、抬起点
夹爪控制	什么时候打开和闭合	夹爪关节命令
流程编排	各动作按什么顺序执行	完整抓取放置状态流程

  后面的内容就围绕这张表展开。学习时不要先盯着某个程序文件，而是先看清楚每一步在整条抓取链路中的位置。

5.1.2 本章运行环境

  本章实验需要两个终端环境配合使用：

环境	作用
Isaac Sim 容器/终端	启动仿真场景，加载 Bobac、桌面、相机、深度图、TF 和关节控制接口
ROS 容器/终端	运行 ROS2 节点，处理图像、深度、TF、规划和夹爪控制

  抓取相关实验默认使用 Isaac Sim 中的抓取观察场景：

    cd /root/robotac_isaac_assets
    ./integrated_runtime/run_demo_scene.sh --world bobac_observation

  这个场景用于单独验证抓取链路。如果要做完整任务联调，可以使用：

    cd /root/robotac_isaac_assets
    ./integrated_runtime/run_demo_scene.sh --world Bobac

5.1.3 编译示例程序

  在 ROS 容器中编译本章示例：

    cd /workspace/demo_ws
    source /opt/ros/humble/setup.bash
    colcon build --symlink-install --packages-select grasp_demo_interfaces grasp_demo_pkg
    source install/setup.bash

  后续每个 ROS 终端都默认先执行：

    cd /workspace/demo_ws
    source /opt/ros/humble/setup.bash
    source install/setup.bash

5.2 阶段一：让相机看到目标

5.2.1 为什么先检查相机

  识别、深度定位和抓取规划都依赖相机。如果相机没有图像，或者目标没有进入画面，后面的模型和规划都没有意义。

实操：

0. 在 isaacsim 容器 并点击 开始仿真

    cd ~/robotac_isaac_assets
    ./integrated_runtime/run_demo_scene.sh --world bobac_observation

1. 移动到观察位姿

  先在 ROS 容器中确认相机相关话题存在：

    ros2 topic list | grep arm_camera

  正常情况下应能看到：

/arm_camera/rgb
/arm_camera/depth
/arm_camera/camera_info

  再在 ROS 容器中确认 RGB 图像有数据：

    ros2 topic echo /arm_camera/rgb --once

  机械臂相机安装在手臂上。手臂姿态不同，相机看到的画面也不同。识别前先让机械臂到一个固定观察位姿，可以让桌面目标稳定进入视野。

  在 ROS 容器中运行：

# 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch grasp_demo_pkg observation_pose_demo.launch.py

5.2.3 查看相机画面

  如果在 ROS 容器中可以打开 RViz，可以添加 /arm_camera/rgb 图像显示：

→

  正常情况下可以看到桌面和目标物体：

  如果当前环境无法打开图形界面，可以继续使用后面的 YOLOE 调试图来确认目标是否进入画面。

5.3 阶段二：识别图像中的目标

5.3.0 什么是视觉识别模型？

  相机能够采集 RGB 图像，但图像本身只是由像素组成的矩阵。对机器人来说，只有图像还不够，它还需要知道图像中哪些区域对应目标物体，目标大概在哪里，边界轮廓是什么。

  视觉识别模型的作用，就是把原始图像转换成机器人后续程序可以使用的结构化结果。例如：

图像内容	识别结果	后续用途
桌面上的目标物体	目标类别	判断是否找到了需要抓取的物体
目标在图像中的位置	检测框	确定目标大致中心区域
目标的轮廓区域	mask	辅助计算目标方向和深度位置

  因此，在抓取任务中，视觉识别不是直接控制机械臂，而是先回答一个更基础的问题：目标在图像的哪一块区域里。只有先找到这块区域，后面才能继续结合深度图计算三维位置。

5.3.1 为什么使用 YOLOE

  本实验选择 YOLOE 作为目标识别模型，主要是因为它能够在一张 RGB 图像中同时给出目标类别、检测框和分割 mask。对于抓取任务来说，这三类信息都很有用：类别用于确认目标，检测框用于确定目标的大致位置，mask 可以进一步描述目标轮廓。

  这里需要注意，YOLOE 的输出仍然是图像层面的结果。它不会直接给出机械臂应该移动到哪里，也不会直接完成抓取规划。它只负责把相机图像中的目标区域找出来，为后面的深度定位和坐标转换提供输入。

输入	输出
/arm_camera/rgb	目标类别、置信度、检测框、mask、调试图

  所以从这一节开始，流程就从“相机能看到目标”进入到“程序能识别目标”。完成这一步后，后面的章节会继续把图像中的目标位置转换为空间中的三维位置。

注意: 这里我们仅仅是说明会是使用 YoloE 作为示范，若识别效果不好，请自行训练视觉模型

5.3.2 启动识别

0. 在 isaacsim 容器 并点击 开始仿真

    cd ~/robotac_isaac_assets
    ./integrated_runtime/run_demo_scene.sh --world bobac_observation

1. 移动到观察位姿

  机械臂相机安装在手臂上。手臂姿态不同，相机看到的画面也不同。识别前先让机械臂到一个固定观察位姿，可以让桌面目标稳定进入视野。

  在 ROS 容器中运行：

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch grasp_demo_pkg observation_pose_demo.launch.py

2. 启动识别程序

  确认观察位姿已经执行后，在 ROS 容器 中启动识别：

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch grasp_demo_pkg perception_demo.launch.py

3. 查看识别结果   在 ROS 容器 中查看识别结果：

bashros2
    ros2 topic echo /demo_grasp/confidence --once
    ros2 topic echo /demo_grasp/bbox --once

  如果能看到 pen 或 pencil，说明目标已经被识别到。

或者我们可以直接在 /workspace/demo_ws/grasp_demo_debug/last_detection_annotated.png 中直接看到识别到的结果

整体操作演示如下：

5.3.3 识别结果怎么看

  识别成功后会生成调试文件：

/workspace/demo_ws/grasp_demo_debug/last_detection.txt
/workspace/demo_ws/grasp_demo_debug/last_detection_annotated.png
/workspace/demo_ws/grasp_demo_debug/last_detection_mask.png

  查看文本结果：

    cat /workspace/demo_ws/grasp_demo_debug/last_detection.txt

  其中 uv 是目标中心像素，bbox 是检测框，mask_area 是分割区域面积。下面是一次成功识别的参考图：

5.3.4 如果没有识别结果

  按下面顺序检查：

    ros2 topic echo /arm_camera/rgb --once
    ros2 topic echo /demo_grasp/label --once
    ls /workspace/demo_ws/grasp_demo_debug

  常见原因包括：相机没有图像、观察位姿不对、目标没有入镜、类别名不匹配、置信度阈值不合适。

5.4 阶段三：从图像区域估计三维位置

5.4.1 识别结果为什么不能直接用于抓取

  上一节中，YOLOE 已经能够从 RGB 图像中找到目标，并输出检测框和 mask。但这些结果仍然属于图像坐标。例如目标中心可能位于图像中的某个像素 (u, v)，这个信息只能说明目标出现在画面的哪个位置。

  机械臂不能直接移动到“图像第几行第几列”。它需要知道目标在空间中的位置，也就是目标相对于相机大约在前方多远、左右偏多少、上下偏多少。因此，这一节要解决的问题是：把图像中的目标区域转换成相机坐标系下的三维点。

YOLOE 检测框 / mask
+ 深度图
+ 相机内参
→ 相机坐标系下的目标点

5.4.2 深度图提供什么信息

  RGB 图像告诉我们“目标在画面哪里”，深度图告诉我们“画面中某个位置距离相机多远”。在理想情况下，只要知道目标中心像素 (u, v)，再从深度图中读取该像素对应的深度值，就可以得到目标到相机的距离。

  但实际深度图可能存在噪声、空洞或边缘误差，所以通常不会只取一个像素点。更稳妥的做法是在目标中心附近，或者在 mask 覆盖的区域内选取一小块有效深度，再用中位数等方式得到一个更稳定的深度值。

检测框 / mask
→ 选择目标区域内的像素
→ 读取有效深度
→ 得到目标距离 z

5.4.3 从像素反投影到相机坐标系

  只知道深度 z 还不够，还需要结合相机内参，才能把图像坐标转换成相机坐标系下的三维坐标。相机内参中最常用的四个量是：

fx, fy, cx, cy

  其中 (cx, cy) 可以理解为图像中心点附近的位置，fx、fy 描述相机成像时像素坐标和空间角度之间的关系。假设目标中心像素是 (u, v)，该位置的深度是 z，则可以用下面的方式得到相机坐标系下的点：

x = (u - cx) * z / fx
y = (v - cy) * z / fy
z = depth[v, u]

  这样得到的 (x, y, z) 不是机械臂基座坐标系下的位置，而是相机坐标系下的位置。也就是说，它描述的是目标相对于相机的位置关系。后面还需要通过 TF，把这个点继续转换到机械臂基座坐标系下。

5.4.4 为什么还要估计目标方向

  对于笔这类细长物体，只知道中心点并不够。夹爪接近目标时，还需要知道目标大致朝哪个方向摆放。否则机械臂即使移动到了目标附近，也可能因为夹爪方向不合适而无法稳定夹取。

  这里可以利用 YOLOE 输出的 mask 来估计目标的长轴方向。简单来说，就是从 mask 中找到目标覆盖的像素区域，再估计这片区域的主要延伸方向。

目标 mask
→ 提取目标区域像素
→ 估计主要延伸方向
→ 得到目标长轴方向

  本节得到的目标点和长轴方向，只是抓取规划的输入信息。它们还不是最终的机械臂轨迹，也不是完整的抓取策略。后续还需要完成坐标转换、接近姿态设计和运动规划。

5.4.5 启动深度定位

0. 在 isaacsim 容器 并点击 开始仿真

    cd ~/robotac_isaac_assets
    ./integrated_runtime/run_demo_scene.sh --world bobac_observation

1. 移动到观察位姿

  机械臂相机安装在手臂上。手臂姿态不同，相机看到的画面也不同。识别前先让机械臂到一个固定观察位姿，可以让桌面目标稳定进入视野。

  在 ROS 容器中运行：

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch grasp_demo_pkg observation_pose_demo.launch.py

2. 启动深度定位程序

  深度定位需要同时使用 RGB 识别结果、深度图和相机内参。在 ROS 容器 中启动：

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch grasp_demo_pkg localization_demo.launch.py

3. 查看定位结果

  查看相机坐标系下的目标点：

    ros2 topic echo /demo_grasp/object_point_camera --once

  查看相机坐标系下的目标长轴方向：

    ros2 topic echo /demo_grasp/long_axis_camera --once

  如果能够看到目标点和长轴方向，说明程序已经完成了从图像区域到相机坐标系三维信息的转换。下一节会继续把这些结果从相机坐标系转换到机械臂基座坐标系。

5.5 阶段四：转换到机械臂坐标系

5.5.1 为什么需要坐标转换

  深度定位得到的是相机坐标系下的目标点。但机械臂规划需要知道目标在机械臂基坐标系下的位置。

如同下图一样：

   由于 Bobac 的深度相机安装在机械臂末端，视觉检测和深度定位得到的目标位置默认都位于相机坐标系下。机械臂规划时通常需要以机械臂基座 base_link_arm 作为参考坐标系，因此需要先通过 TF 将目标点从相机坐标系转换到 base_link_arm 坐标系下。完成坐标转换后，后续才能根据目标在机械臂基座坐标系中的位置，进一步进行逆运动学求解或运动规划。

相机坐标系：目标相对相机在哪里 机械臂坐标系：目标相对机械臂底座在哪里

  因此必须通过 TF 把结果从相机坐标系转换到 base_link_arm。

object_point_camera
→ TF: arm_Camera 到 base_link_arm
→ object_point_base

  注意：Bobac 机械臂相机坐标系是 arm_Camera，中间的 C 是大写；相机话题仍然是 /arm_camera/...。

5.5.2 启动坐标转换

0. 在 isaacsim 容器 并点击 开始仿真

    cd ~/robotac_isaac_assets
    ./integrated_runtime/run_demo_scene.sh --world bobac_observation

1. 移动到观察位姿

  机械臂相机安装在手臂上。手臂姿态不同，相机看到的画面也不同。识别前先让机械臂到一个固定观察位姿，可以让桌面目标稳定进入视野。

  在 ROS 容器中运行：

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch grasp_demo_pkg observation_pose_demo.launch.py

2. 启动 TF 转换程序

  在 ROS 容器中启动 TF 转换验证：

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch grasp_demo_pkg pose_tf_demo.launch.py

  在 ROS 容器中查看转换后的相对于机械臂底座的坐标系 base_link_arm 的目标点：

    ros2 topic echo /demo_grasp/object_point_base --once

  在 ROS 容器中查看转换后的接近方向和长轴方向：

    ros2 topic echo /demo_grasp/normal_base --once
    ros2 topic echo /demo_grasp/long_axis_base --once

验证这个点是否正确？

我们可以直接利用 isaacsim 的资产结构的坐标系系统即可做一次验证：

我们只需要暂停仿真，然后在机器人的机械臂基座 base_link_arm 中创建一个 cube 块，然后将这个 cube 的位置移动到相应的位置即可验证出这个检测并转换后的点是否在我们要抓取的目标物品上，具体方式可看下面的演示：

5.6 阶段五：从目标位置到机械臂动作

5.6.1 运动规划要解决什么

  现在已经知道目标在 base_link_arm 下的位置和方向。下一步不是简单地把机械臂“挪过去”，而是要规划一个可执行的靠近动作。

  完整规划通常包括：

目标点
→ 预抓取点
→ 抓取点
→ 抬起点
→ 放置点
→ 关节轨迹

  其中最关键的是：末端姿态是否正确、关节角是否可解、路径是否安全、动作是否可重复执行。

5.6.2 基础动作验证

  本章提供一个基础动作验证，用来确认目标点可以进入机械臂控制链路。它只让机械臂大致朝目标区域运动，不做真实 IK、MoveIt 规划和碰撞检测。

提示: 该部分仅讲解运动规划的输入输出内容，示例程序没有使用任何运动规划，需要参赛选手自行实现，可使用任何方式（Moveit/Curobo）

实验步骤：

0. 在 isaacsim 容器 并点击 开始仿真

    cd ~/robotac_isaac_assets
    ./integrated_runtime/run_demo_scene.sh --world bobac_observation

1. 移动到观察位姿

  机械臂相机安装在手臂上。手臂姿态不同，相机看到的画面也不同。识别前先让机械臂到一个固定观察位姿，可以让桌面目标稳定进入视野。

  在 ROS 容器中运行：

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch grasp_demo_pkg observation_pose_demo.launch.py

2. 启动基础规划测试程序

  在 ROS 容器中启动基础规划测试：

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch grasp_demo_pkg planning_demo.launch.py

3. 发送要到达的目标点

  在 ROS 容器中发送一个测试目标点：

    ros_env ros2 action send_goal /demo_plan_to_pose grasp_demo_interfaces/action/PlanToPose \
    "{target: {header: {frame_id: 'base_link_arm'}, point: {x: -0.45, y: 0.5, z: 0.25}}, \
    normal: {header: {frame_id: 'base_link_arm'}, vector: {x: 0.0, y: 0.0, z: -1.0}}, \
    long_axis: {header: {frame_id: 'base_link_arm'}, vector: {x: 1.0, y: 0.0, z: 0.0}}, \
    execute: true, label: 'approx_plan_demo'}"

4. 观察结果

  在 ROS 容器中观察结果：

    ros2 topic echo /demo_grasp/commanded_pose --once
    ros2 topic echo /hand_command

观察项	正常现象
/demo_grasp/commanded_pose	输出候选末端位姿
/hand_command	输出机械臂关节命令
Isaac Sim 画面	机械臂产生可见动作

完整实操演示如下：

5.6.3 参赛者需要实现什么

  基础验证只能说明控制链路通了，不能代替真正的抓取规划。完整任务中需要自己实现：

能力	说明
预抓取点生成	先到目标上方或侧前方安全位置，避免直接撞向桌面
抓取姿态生成	根据 normal、long_axis 和夹爪结构确定末端姿态
逆运动学	求出能到达目标姿态的关节角
轨迹规划	生成平滑、满足关节限制的运动路径
碰撞检查	避免碰到桌面、目标物和机器人自身
执行判断	规划失败或执行失败时停止、重试或换策略

  可以使用 MoveIt，也可以自己实现逆运动学。关键是让机械臂稳定、可重复地到达抓取姿态，而不是只让它动一下。

5.7 阶段六：控制夹爪

5.7.1 夹爪在流程中的作用

  夹爪动作必须和机械臂运动配合。典型顺序是：

到达预抓取点
→ 打开夹爪
→ 接近抓取点
→ 闭合夹爪
→ 抬起目标
→ 到达放置点
→ 打开夹爪

  所以夹爪需要单独验证，确保打开、闭合和指定开合位置都能正常执行。

5.7.2 启动夹爪测试

实验步骤：

0. 在 isaacsim 容器 并点击 开始仿真

    cd ~/robotac_isaac_assets
    ./integrated_runtime/run_demo_scene.sh --world bobac_observation

1. 移动到观察位姿

  机械臂相机安装在手臂上。手臂姿态不同，相机看到的画面也不同。识别前先让机械臂到一个固定观察位姿，可以让桌面目标稳定进入视野。

  在 ROS 容器中运行：

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch grasp_demo_pkg observation_pose_demo.launch.py

2. 启动控制夹爪程序

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch grasp_demo_pkg gripper_demo.launch.py

  打开夹爪：

    ros2 action send_goal /demo_gripper_command grasp_demo_interfaces/action/GripperCommand "{command: 'open', position: 100.0, speed: 50.0, wait_for_completion: true}"

  关闭夹爪：

    ros2 action send_goal /demo_gripper_command grasp_demo_interfaces/action/GripperCommand "{command: 'close', position: 0.0, speed: 50.0, wait_for_completion: true}"

  移动到一半开合位置：

    ros2 action send_goal /demo_gripper_command grasp_demo_interfaces/action/GripperCommand "{command: 'position', position: 50.0, speed: 50.0, wait_for_completion: true}"

  同时查看关节命令：

    ros2 topic echo /hand_command

具体操作演示如下：

5.8 阶段七：完整流程设计

5.8.1 完整抓取放置应该怎样组织

  完整流程不是把前面的命令简单排成一串，而是一个带判断的状态流程。

1. 到观察位姿
2. 等待图像稳定
3. 识别目标
4. 计算目标三维位置
5. 转换到 base_link_arm
6. 生成预抓取点和抓取点
7. 规划到预抓取点
8. 打开夹爪
9. 规划到抓取点
10. 闭合夹爪
11. 抬起目标
12. 移动到放置点
13. 打开夹爪
14. 退回安全位置

  每一步都应该有成功条件和失败处理。

阶段	成功条件	失败处理
识别	检测到目标且置信度足够	重新观察或调整阈值
深度定位	得到有效三维点	重新取深度或重新识别
TF 转换	得到 base_link_arm 下目标点	检查 TF 树和时间戳
规划	得到可执行轨迹	调整预抓取点或重新规划
夹取	夹爪闭合后目标未掉落	调整抓取点和夹爪闭合位置
放置	目标落到指定区域	调整放置高度和撤离方向

  它不是完整抓取程序，不会代替参赛者完成可靠 IK、轨迹规划、碰撞检测和失败恢复。

5.8.2 实现路线建议

方案	适合情况	注意事项
MoveIt 规划	希望使用成熟 IK、轨迹规划和碰撞检测	需要配置好模型、规划组和控制器
自行实现 IK	机械臂结构清楚、任务区域固定	要处理关节限制和奇异位形
示教点加插值	快速验证固定区域动作	泛化能力弱，目标变化大时容易失败

  建议按最小闭环逐步推进：

固定点抓取成功
→ 感知点抓取成功
→ 加入放置动作
→ 加入失败重试
→ 接入完整任务流程

5.9 本章小结

  本章围绕抓取放置任务，依次完成了观察、识别、深度定位、坐标转换、��础动作验证和夹爪控制。

相机看见目标
→ YOLOE 找到目标
→ 深度图算出三维点
→ TF 转到机械臂坐标系
→ 规划机械臂靠近方式
→ 夹爪完成抓取和释放

  示例程序的作用是帮助大家验证各个环节是否可用。真正的完整任务，需要参赛者自己完成可靠的抓取规划、轨迹执行、夹爪时序和失败恢复。

  实现时始终记住三件事：

这一阶段需要什么输入
这一阶段应该输出什么结果
失败后应该回到哪一步重新处理

  只要这条逻辑清楚，就可以逐步把感知、规划、夹爪和状态机组合成稳定的完整抓取放置系统。