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:

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

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

roslaunch mara_moveit_config demo.launch

In the second terminal run the pick and place tutorial:

rosrun mara_moveit_tutorials pick_place_tutorial

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

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 can be seen here in the moveit_tutorials GitHub project.

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 = "world";

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

Add the second table where we will be placing the cube.

collision_objects[1].id = "table2";
collision_objects[1].header.frame_id = "world";

/* 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.4;
collision_objects[1].primitive_poses[0].position.z = 0.847;

Define the object that we will be manipulating

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

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);
grasps[0].grasp_pose.header.frame_id = "world";
tf2::Quaternion orientation;
orientation.setRPY(-1.5701278, 0, 0);
grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);

grasps[0].grasp_pose.pose.position.x = 0.542542881168;
grasps[0].grasp_pose.pose.position.y = -0.000345518257861;
grasps[0].grasp_pose.pose.position.z = 1.20300168666;
grasps[0].pre_grasp_approach.direction.header.frame_id = "world";
/* 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;
grasps[0].post_grasp_retreat.direction.header.frame_id = "world";
/* 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;
openGripper(grasps[0].pre_grasp_posture);
/* Add both finger joints of MARA robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "right_inner_knuckle_joint";
posture.joint_names[1] = "left_inner_knuckle_joint";

/* 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);
closedGripper(grasps[0].grasp_posture);
posture.joint_names.resize(2);
posture.joint_names[0] = "right_inner_knuckle_joint";
posture.joint_names[1] = "left_inner_knuckle_joint";

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

There is a known error where it could call “All supplied place locations failed. Retrying last location in verbose mode.” They 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);
  place_location[0].place_pose.header.frame_id = "world";

  place_location[0].place_pose.pose.orientation.x = -0.494481092405;
  place_location[0].place_pose.pose.orientation.y = -0.512224064265;
  place_location[0].place_pose.pose.orientation.z = 0.496681875859;
  place_location[0].place_pose.pose.orientation.w = 0.496409177427;

  /* While placing it is the exact location of the center of the object. */
  place_location[0].place_pose.pose.position.x = 0.0048252247931;
  place_location[0].place_pose.pose.position.y = 0.397275288898;
  place_location[0].place_pose.pose.position.z = 1.18918246187;
/* Defined with respect to frame_id */
place_location[0].pre_place_approach.direction.header.frame_id = "world";
/* 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;
place_location[0].post_place_retreat.direction.header.frame_id = "world";
/* 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;
/* Similar to the pick case */
openGripper(place_location[0].post_place_posture);
group.setSupportSurfaceName("table2");
group.place("object", place_location);