MoveIt教程[14]：Pick and Place Tutorial

教程[14]：Pick and Place Tutorial"/>

MoveIt教程[14]：Pick and Place Tutorial

在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：.html
[2]pick_place：