Pick and Place Tutorial

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;

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;

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;

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.
From panda_link8 to the palm of the eef the distance is 0.058, the cube starts 0.01 before 5.0 (half of the length of the cube).
Therefore, the position for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some extra padding)

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 and we are working on fixing it.
Create a vector of placings to be attempted, currently only creating 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);

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

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

Open Source Feedback

See something that needs improvement? Please open a pull request on this GitHub page