想让机械臂像人一样拿起物体再放下？MoveIt 的 pick 和 place 功能就是你的答案。本文将带你一步步理解 moveit_msgs::Grasp 消息的核心字段，并手把手完成一个从添加障碍物到成功放置物体的完整示例。

一、认识 moveit_msgs::Grasp

在 MoveIt 中，一次抓取动作需要告诉机器人：用什么姿态、怎样靠近、如何闭合手指、抓完后如何撤退。这些信息都封装在 moveit_msgs::Grasp 消息中，其中最重要的字段如下：

• **pre_grasp_posture**：抓取前末端执行器的关节角度（例如：手指张开）

• **grasp_posture**：抓取时末端执行器的关节角度（例如：手指闭合）

• **grasp_pose**：末端执行器抓取物体的目标位姿

• **pre_grasp_approach**：接近物体的方向和距离

• **post_grasp_retreat**：抓取后撤离的方向和距离

• **post_place_retreat**：放置物体后撤离的方向和距离

理解这些字段后，我们就可以构造一个完整的抓取动作了。

二、搭建仿真环境：两张桌子 + 一个物体

首先，我们需要在规划场景中添加三个碰撞物体：两张桌子（table1 和 table2）以及一个待抓取的立方体（object）。

std::vector<moveit\_msgs::CollisionObject> collision\_objects; collision\_objects.resize(3); 

添加第一张桌子（放置初始物体）

collision\_objects[0].id = "table1"; collision\_objects[0].header.frame\_id = "panda\_link0"; collision\_objects[0].primitives.resize(1); collision\_objects[0].primitives[0].type = BOX; collision\_objects[0].primitives[0].dimensions = {0.2, 0.4, 0.4}; collision\_objects[0].primitive\_poses[0].position.x = 0.5; collision\_objects[0].primitive\_poses[0].position.y = 0; collision\_objects[0].primitive\_poses[0].position.z = 0.2; 

添加第二张桌子（放置目标点）

collision\_objects[1].id = "table2"; collision\_objects[1].header.frame\_id = "panda\_link0"; collision\_objects[1].primitives[0].dimensions = {0.4, 0.2, 0.4}; collision\_objects[1].primitive\_poses[0].position.x = 0; collision\_objects[1].primitive\_poses[0].position.y = 0.5; collision\_objects[1].primitive\_poses[0].position.z = 0.2; 

添加被抓物体（长条立方体）

collision\_objects[2].id = "object"; collision\_objects[2].primitives[0].dimensions = {0.02, 0.02, 0.2}; collision\_objects[2].primitive\_poses[0].position.x = 0.5; collision\_objects[2].primitive\_poses[0].position.y = 0; collision\_objects[2].primitive\_poses[0].position.z = 0.5; 

注意：物体的中心坐标是 (0.5, 0, 0.5)，正好位于 table1 的台面之上。

三、构造抓取动作（Grasp）

我们只构造一个抓取（实际应用中可以用抓取生成器生成多个候选抓取）。

设置抓取位姿

抓取时，末端执行器（panda_link8）需要对准物体中心，并旋转合适的姿态：

grasps[0].grasp\_pose.header.frame\_id = "panda\_link0"; tf2::Quaternion orientation; orientation.setRPY(-M\_PI/2, -M\_PI/4, -M\_PI/2); grasps[0].grasp\_pose.pose.orientation = tf2::toMsg(orientation); grasps[0].grasp\_pose.pose.position.x = 0.415; grasps[0].grasp\_pose.pose.position.y = 0; grasps[0].grasp\_pose.pose.position.z = 0.5; 

为什么 x 坐标是 0.415？
物体中心在 0.5，panda_link8 到手掌中心距离 0.058，再减去物体半长 0.01 和一些余量，得到 0.415。

接近路径（pre-grasp approach）

沿着 X 轴正向接近物体，最近距离 0.095m，期望距离 0.115m：

grasps[0].pre\_grasp\_approach.direction.header.frame\_id = "panda\_link0"; grasps[0].pre\_grasp\_approach.direction.vector.x = 1.0; grasps[0].pre\_grasp\_approach.min\_distance = 0.095; grasps[0].pre\_grasp\_approach.desired\_distance = 0.115; 

抓取后撤退路径（post-grasp retreat）

沿着 Z 轴正向抬起，撤离 0.25m：

grasps[0].post\_grasp\_retreat.direction.header.frame\_id = "panda\_link0"; grasps[0].post\_grasp\_retreat.direction.vector.z = 1.0; grasps[0].post\_grasp\_retreat.min\_distance = 0.1; grasps[0].post\_grasp\_retreat.desired\_distance = 0.25; 

手指动作

• 抓取前

  ：张开手指，让物体能进入掌心

 posture.joint\_names = {"panda\_finger\_joint1", "panda\_finger\_joint2"}; posture.points.resize(1); 

posture.points[0].positions = {0.04, 0.04}; posture.points[0].time\_from\_start = ros::Duration(0.5); } 

• 抓取时

  ：闭合手指，夹紧物体

 posture.joint\_names = {"panda\_finger\_joint1", "panda\_finger\_joint2"}; posture.points[0].positions = {0.00, 0.00}; 

四、执行 Pick

move\_group.setSupportSurfaceName("table1"); move\_group.pick("object", grasps); 

这里指定了支撑面为 `table1`，MoveIt 会确保机械臂在抓取过程中不会与桌子碰撞。

五、构造放置动作（Place）

放置物体需要定义 PlaceLocation，包括放置位姿、接近路径、撤离路径和手指动作。

放置位姿

将物体放到 table2 的中心上方：

place\_location[0].place\_pose.header.frame\_id = "panda\_link0"; orientation.setRPY(0, 0, M\_PI/2); place\_location[0].place\_pose.pose.orientation = tf2::toMsg(orientation); place\_location[0].place\_pose.pose.position.x = 0; place\_location[0].place\_pose.pose.position.y = 0.5; place\_location[0].place\_pose.pose.position.z = 0.5; 

放置前接近路径

沿 Z 轴负方向下降（向下接近桌面）：

place\_location[0].pre\_place\_approach.direction.vector.z = -1.0; place\_location[0].pre\_place\_approach.min\_distance = 0.095; place\_location[0].pre\_place\_approach.desired\_distance = 0.115; 

放置后撤离路径

沿 Y 轴负方向撤离：

place\_location[0].post\_place\_retreat.direction.vector.y = -1.0; place\_location[0].post\_place\_retreat.min\_distance = 0.1; place\_location[0].post\_place\_retreat.desired\_distance = 0.25; 

放置后手指张开

放置完成后，再次张开手指：

 openGripper(place\_location[0].post\_place\_posture); 

执行 Place

move\_group.setSupportSurfaceName("table2"); move\_group.place("object", place\_location); 

六、常见问题与避坑指南

1. Place 失败

   官方代码中有一个 TODO 提到：有时调用 place 会报错“所有提供的放置位置失败”。这通常是逆解求解失败或路径规划不可行导致的。可以尝试：

   • 调整 pre_place_approach 的方向和距离

   • 增加 place_location 的候选个数

   • 降低放置目标点的精度要求

2. 抓取姿态不合适

   如果抓取时末端执行器与物体或桌面碰撞，检查 grasp_pose 的坐标和姿态四元数是否正确，必要时在 RViz 中打开碰撞检测进行调试。

3. 手指关节名称错误

   Panda 机器人的手指关节名为 panda_finger_joint1 和 panda_finger_joint2，其他机器人需要相应修改。

4. 坐标系不统一

   确保所有 frame_id 都正确设置，一般设为机器人基座（如 panda_link0）。

七、总结

通过本文，你已经学会了：

• 如何使用 moveit_msgs::Grasp 定义一次完整的抓取

• 如何添加碰撞物体（桌子、物体）到规划场景

• 如何调用 pick 和 place 实现物体的搬运

MoveIt 的 pick & place 管道非常强大，你可以在此基础上扩展：使用抓取生成器自动生成多个候选抓取、添加感知模块识别物体位姿、结合 moveit_task_constructor 处理更复杂的操作序列。