Pick and Place
NOTE: 本教程中使用的功能已弃用。要执行拾取和放置操作，应使用 MoveIt 任务构造器 (MTC) (Pick and Place with MoveIt Task Constructor).
在 MoveIt 中，抓取是使用 MoveGroup 接口完成的。为了抓取物体，我们需要创建“moveit_msgs::Grasp”消息，这将允许定义抓取操作中涉及的各种姿势和姿态。 观看此视频以查看本教程的输出：
入门
如果您尚未完成，请确保您已完成 Getting Started.
运行演示
打开两个终端。在第一个终端中启动 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- This defines the trajectory position of the joints in the end effector group before we go in for the grasp.trajectory_msgs/JointTrajectory grasp_posture- This defines the trajectory position of the joints in the end effector group for grasping the object.geometry_msgs/PoseStamped grasp_pose- Pose of the end effector in which it should attempt grasping.moveit_msgs/GripperTranslation pre_grasp_approach- This is used to define the direction from which to approach the object and the distance to travel.moveit_msgs/GripperTranslation post_grasp_retreat- This is used to define the direction in which to move once the object is grasped and the distance to travel.moveit_msgs/GripperTranslation post_place_retreat- This is used to define the direction in which to move once the object is placed at some location and the distance to travel.
完整代码
完整代码可见 here 在 moveit_tutorials GitHub 项目中。
Creating Environment
Create vector to hold 3 collision objects.
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.resize(3);
Add the first table where the cube will originally be kept.
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;
Add the second table where we will be placing the cube.
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;
Define the object that we will be manipulating
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;
Pick Pipeline
Create a vector of grasps to be attempted, currently only creating single grasp. This is essentially useful when using a grasp generator to generate and test multiple grasps.
std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(1);
Setting grasp pose
This is the pose of panda_link8.
Make sure that when you set the grasp_pose, you are setting it to be the pose of the last link in
your manipulator which in this case would be panda_link8 You will have to compensate for the
transform from panda_link8 to the palm of the end effector.
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;
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;
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;
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);
Set support surface as table1.
move_group.setSupportSurfaceName("table1");
Call pick to pick up the object using the grasps given
move_group.pick("object", grasps);
Place Pipeline
TODO(@ridhwanluthra) - Calling place function may lead to “All supplied place locations failed. Retrying last
location in verbose mode.” This is a known issue.
Ideally, you would create a vector of place locations to be attempted although in this example, we only create
a single place location.
std::vector<moveit_msgs::PlaceLocation> place_location;
place_location.resize(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);
/* 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;
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;
Setting post-place 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;
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);