在MoveIt中，抓取是使用MoveGroup接口完成的。为了抓住一个物体，需要创建moveit_msg::graspmsg，它允许定义抓取操作中涉及的各种poses和postures。
一.Running The Demo
打开两个终端。在第一个终端启动RViz，等待一切加载完毕：
roslaunch panda_moveit_config demo.launch
在第二个终端运行pick和place教程：
rosrun moveit_tutorials pick_place_tutorial
应该看到与本教程开头的视频类似的内容。
二.Understanding moveit_msgs::Grasp
完整的文档请参考moveit_msgs/Grasp.msg。
消息的相关字段是：
- 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：这是用来定义移动的方向，一旦物体被放置在某个位置和要移动的距离。
三.The Entire Code
1.Creating Environment
创建向量来容纳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;
2.Pick Pipeline
创建要尝试的抓握向量，目前只创建单个抓握。这在使用抓握生成器生成和测试多个抓握时非常有用。
std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(1);
[1]Setting grasp pose
这是panda_link8的姿势。从panda_link8到eef掌心的距离是0.058，立方体在5.0之前开始0.01[立方体长度的一半]。因此，panda_link8的位置 = 5 - [立方体的长度/2 - 距离b/w panda_link8和eef的手掌-一些额外的填充]。
[2]Setting pre-grasp approach
/* 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;
[3]Setting post-grasp retreat
/* 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;
[4]Setting posture of eef before grasp
openGripper(grasps[0].pre_grasp_posture);
[5]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);
[6]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);
设置支撑面为表1。
move_group.setSupportSurfaceName("table1");
调用pick来使用给定的grasps拾取对象。
move_group.pick("object", grasps);
3.Place Pipeline
TODO(@ridhwanluthra) - 调用place函数可能导致“所有提供的位置位置失败。在详细模式下重试最后一个位置”。这是一个众所周知的问题，我们正在努力解决它。创建要尝试的位置向量，目前仅创建单个放置位置。
std::vector<moveit_msgs::PlaceLocation> place_location;
place_location.resize(1);
[1]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;
[2]Setting pre-place approach
/* 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;
[3]Setting post-grasp retreat
/* 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;
[4]Setting posture of eef after placing object
/* Similar to the pick case */
openGripper(place_location[0].post_place_posture);
将支撑面设置为表2。
group.setSupportSurfaceName("table2");
调用place来使用给定的放置位置对象。
group.place("object", place_location);
参考文献：
[1]Pick and Place Tutorial：http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pick_place/pick_place_tutorial.html
[2]pick_place：https://github.com/ros-planning/moveit_tutorials/tree/kinetic-devel/doc/pick_place
