Pick and Place¶

In MoveIt, grasping is done using the MoveGroup interface. In order to grasp an object we need to create moveit_msgs::Grasp msg which will allow defining the various poses and postures involved in a grasping operation. Watch this video to see the output of this tutorial:

Getting Started¶

If you haven’t already done so, make sure you’ve completed the steps in Getting Started.

Running The Demo¶

Open two terminals. In the first terminal start RViz and wait for everything to finish loading:

roslaunch panda_moveit_config demo.launch

In the second terminal run the pick and place tutorial:

rosrun moveit_tutorials pick_place_tutorial

You should see something similar to the video at the beginning of this tutorial.

Understanding `moveit_msgs::Grasp`¶

For complete documentation refer to moveit_msgs/Grasp.msg.

The relevant fields of the message are:-

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.

The Entire Code¶

The entire code can be seen here in the moveit_tutorials GitHub project.

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

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

开源反馈

有地方需要改进？请在 GitHub page 新开一个 pull request。

Pick and Place¶

Getting Started¶

Running The Demo¶

Understanding moveit_msgs::Grasp¶

The Entire Code¶

Creating Environment¶

Pick Pipeline¶

Setting grasp pose¶

Setting pre-grasp approach¶

Setting post-grasp retreat¶

Setting posture of eef before grasp¶

openGripper function¶

Setting posture of eef during grasp¶

closedGripper function¶

Place Pipeline¶

Setting place location pose¶

Setting pre-place approach¶

Setting post-grasp retreat¶

Setting posture of eef after placing object¶

Understanding `moveit_msgs::Grasp`¶