「这是我参与2022首次更文挑战的第1天，活动详情查看：2022首次更文挑战」。
本文章写的是如何使用C++编程，进行物体的抓取和放置。在MoveIt 的编程，最常用到的接口类是MoveGroupInterface。MoveGroupInterface类提供了机器人的常用基本操作：设置关节角度，机器人姿态目标，路径规划，控制机器人运动
MoveGroupInterface通过ROS的话题topics、服务services和动作actions和MoveGroup节点通讯。
moveit_msgs::Grasp 消息的相关字段是： - trajectory_msgs/JointTrajectory pre_grasp_posture - 在我们进行掌握之前，这定义了末端执行器组中关节的轨迹位置。
trajectory_msgs/JointTrajectory grasp_posture - 这定义了末端执行器组中关节的轨迹位置，用于抓取物体。
geometry_msgs/PoseStamped grasp_pose - 应该抓住的末端执行器的姿势。
moveit_msgs/GripperTranslation pre_grasp_approach - 用于定义接近物体的方向和行进距离。
moveit_msgs/GripperTranslation post_grasp_retreat - 用于定义抓取物体后移动的方向和行进距离。
moveit_msgs/GripperTranslation post_place_retreat - 用于定义物体放置在某个位置时的移动方向和行进距离。
创建环境
创建矢量以容纳3个碰撞对象。
std::vector<moveit_msgs::CollisionObject> collision_objects;collision_objects.resize(3);
添加最初将保留多维数据集的第一个表。
collision_objects[0].id = "table1";
collision_objects[0].header.frame_id = "panda_link0";
/* Define the primitive and its dimensions. */
collision_objects[0].primitives.resize(1);
collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
collision_objects[0].primitives[0].dimensions.resize(3);
collision_objects[0].primitives[0].dimensions[0] = 0.2;
collision_objects[0].primitives[0].dimensions[1] = 0.4;
collision_objects[0].primitives[0].dimensions[2] = 0.4;
/* Define the pose of the table. */
collision_objects[0].primitive_poses.resize(1);
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";
/* Define the primitive and its dimensions. */
collision_objects[1].primitives.resize(1);
collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[1].primitives[0].dimensions.resize(3);
collision_objects[1].primitives[0].dimensions[0] = 0.4;
collision_objects[1].primitives[0].dimensions[1] = 0.2;
collision_objects[1].primitives[0].dimensions[2] = 0.4;
/* Define the pose of the table. */
collision_objects[1].primitive_poses.resize(1);
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].header.frame_id = "panda_link0";
collision_objects[2].id = "object";
/* Define the primitive and its dimensions. */
collision_objects[2].primitives.resize(1);
collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[2].primitives[0].dimensions.resize(3);
collision_objects[2].primitives[0].dimensions[0] = 0.02;
collision_objects[2].primitives[0].dimensions[1] = 0.02;
collision_objects[2].primitives[0].dimensions[2] = 0.2;
/* Define the pose of the object. */
collision_objects[2].primitive_poses.resize(1);
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;
抓取管道 创建一个要尝试的抓取向量，目前只创建单个抓取。当使用抓取生成器生成和测试多个抓取时，这基本上是有用的。
std::vector<moveit_msgs::Grasp> grasps;grasps.resize(1);
这是panda_link8的姿势。
从panda_link8到eef的掌心，距离是0.058，立方体在5.0之前开始0.01（立方体长度的一半）。
因此，panda_link8的位置= 5 - （立方体的长度/ 2 - 距离b / w 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;
设置抓取前的路径
/* Defined with respect to frame_id */
grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
/* Direction is set as positive x axis */
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;
设置返回的路线
/* Defined with respect to frame_id */
grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
/* Direction is set as positive z axis */
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;
Setting posture of eef before grasp
openGripper(grasps[0].pre_grasp_posture);
openGripper function
/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";
/* Set them as open, wide enough for the object to fit. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.04;
posture.points[0].positions[1] = 0.04;
posture.points[0].time_from_start = ros::Duration(0.5);
Setting posture of eef during grasp
closedGripper(grasps[0].grasp_posture);
closedGripper function
/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";
/* Set them as closed. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.00;
posture.points[0].positions[1] = 0.00;
posture.points[0].time_from_start = ros::Duration(0.5);
将支撑面设置为table1。
move_group.setSupportSurfaceName("table1");
使用给定的抓取来调用拾取器来拾取对象
move_group.pick("object", grasps);
TODO（@ridhwanluthra） - 调用场所功能可能导致“所有提供的场所位置失败。以详细模式重试最后一个位置。“这是一个已知问题，我们正在努力修复它。
创建要尝试的位置向量，目前仅创建单个位置。
std::vector<moveit_msgs::PlaceLocation> place_location;
place_location.resize(1);
设置地点位置姿势
place_location[0].place_pose.header.frame_id = "panda_link0";
tf2::Quaternion orientation;
orientation.setRPY(0, 0, M_PI / 2);
place_location[0].place_pose.pose.orientation = tf2::toMsg(orientation);
/* While placing it is the exact location of the center of the object. */
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;
Setting place location pose
place_location[0].place_pose.header.frame_id = "panda_link0";tf2::Quaternion orientation;orientation.setRPY(0, 0, M_PI / 2);place_location[0].place_pose.pose.orientation = tf2::toMsg(orientation);
/* While placing it is the exact location of the center of the object. */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;
前进路线
/* Defined with respect to frame_id /place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";/ Direction is set as negative z axis */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;
抓取后撤退路线
/* Defined with respect to frame_id /place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";/ Direction is set as negative y axis */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;
Setting posture of eef after placing object
/* Similar to the pick case */openGripper(place_location[0].post_place_posture);
Set support surface as table2.
group.setSupportSurfaceName("table2");
Call place to place the object using the place locations given.
group.place("object", place_location);
