Documentation Version
You're reading the documentation for a stable version of MoveIt that is not being developed further. For information on the recommended version, please have a look at Main.
Pick and Place
NOTE: The functionality used in this tutorial is deprecated. To perform a pick and place operation, MoveIt Task Constructor (MTC) should be used (Pick and Place with MoveIt Task Constructor).
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-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);