使用MoveIt进行抓取与放置--Pick and Place流程

想让机械臂像人一样拿起物体再放下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::vectormoveit\_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.5panda_link8 到手掌中心距离 0.058再减去物体半长 0.01 和一些余量得到 0.415。接近路径pre-grasp approach沿着 X 轴正向接近物体最近距离 0.095m期望距离 0.115mgrasps[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.25mgrasps[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};四、执行 Pickmove\_group.setSupportSurfaceName(table1); move\_group.pick(object, grasps);这里指定了支撑面为 table1MoveIt 会确保机械臂在抓取过程中不会与桌子碰撞。五、构造放置动作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);执行 Placemove\_group.setSupportSurfaceName(table2); move\_group.place(object, place\_location);六、常见问题与避坑指南Place 失败官方代码中有一个 TODO 提到有时调用place会报错“所有提供的放置位置失败”。这通常是逆解求解失败或路径规划不可行导致的。可以尝试调整pre_place_approach的方向和距离增加place_location的候选个数降低放置目标点的精度要求抓取姿态不合适如果抓取时末端执行器与物体或桌面碰撞检查grasp_pose的坐标和姿态四元数是否正确必要时在 RViz 中打开碰撞检测进行调试。手指关节名称错误Panda 机器人的手指关节名为panda_finger_joint1和panda_finger_joint2其他机器人需要相应修改。坐标系不统一确保所有frame_id都正确设置一般设为机器人基座如panda_link0。七、总结通过本文你已经学会了如何使用moveit_msgs::Grasp定义一次完整的抓取如何添加碰撞物体桌子、物体到规划场景如何调用pick和place实现物体的搬运MoveIt 的 pick place 管道非常强大你可以在此基础上扩展使用抓取生成器自动生成多个候选抓取、添加感知模块识别物体位姿、结合moveit_task_constructor处理更复杂的操作序列。