Ros2 - Moveit2 - Pick And Place(拾取和放置)

拾取和放置

注意：本教程中使用的功能已弃用。要执行拾取和放置操作，应使用 MoveIt 任务构造器 (MTC)（使用 MoveIt 任务构造器拾取和放置）。

 

在 MoveIt 中，抓取是使用 MoveGroup 接口完成的。为了抓取一个物体，我们需要创建moveit_msgs::Graspmsg，以便定义抓取操作中涉及的各种姿势和姿态。观看此视频以查看本教程的输出：

https://youtu.be/QBJPxx_63Bs

入门

如果您还没有这样做，请确保您已经完成入门指南中的步骤。

运行演示

打开两个终端。在第一个终端中启动 RViz 并等待所有内容完成加载：

roslaunch panda_moveit_config demo.launch

在第二个终端中运行拾取和放置教程：

rosrun moveit_tutorials pick_place_tutorial

您应该会看到与本教程开头的视频类似的内容。

理解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- 这用于定义物体放置在某个位置后移动的方向和行进的距离。

 

完整代码

整个代码可以在moveit_tutorials GitHub 项目中看到。

创建环境

创建向量来容纳 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[0].primitive_poses[0].orientation.w = 1.0;

添加第二张表，我们将在其中放置立方体。

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[1].primitive_poses[0].orientation.w = 1.0;

定义我们将要操作的对象

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;
collision_objects[2].primitive_poses[0].orientation.w = 1.0;

选择管道

创建要尝试的抓握向量，目前仅创建单个抓握。这在使用抓握生成器生成和测试多个抓握时非常有用。

std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(1);

设置抓握姿势

这是 panda_link8 的姿势。
确保在设置 grasp_pose 时，将其设置为操纵器中最后一个链接的姿势，在本例中为panda_link8。您必须补偿从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;

抓握前设置 eef 姿势

openGripper(grasps[0].pre_grasp_posture);

openGripper 函数

/* 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);

抓握时 eef 的姿势设定

closedGripper(grasps[0].grasp_posture);

closedGripper 功能

/* 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 来使用给定的抓握方式拾取物体

move_group.pick("object", grasps);

放置管道

TODO(@ridhwanluthra) - 调用 place 函数可能会导致“所有提供的 place 位置都失败。正在以详细模式重试最后一个位置。”这是一个已知问题。

理想情况下，您会创建一个要尝试的 place 位置向量，尽管在此示例中，我们仅创建了一个 place 位置。

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);

/* For place location, we set the value to 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;

放置物体后设置eef的姿势

/* Similar to the pick case */
openGripper(place_location[0].post_place_posture);

设置支撑面如Table2。

group.setSupportSurfaceName("table2");

调用 place 来使用给定的位置放置物体。

group.place("object", place_location);