# 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 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.

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

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

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

#### 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
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;
##### Setting pre-grasp approach
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;
##### Setting post-grasp retreat
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;
##### Setting posture of eef before grasp
openGripper(grasps[0].pre_grasp_posture);
##### openGripper function
/* 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);
##### Setting posture of eef during grasp
closedGripper(grasps[0].grasp_posture);
##### closedGripper function
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);

#### Place Pipeline

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);
##### Setting place location pose
  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;
##### Setting pre-place approach
/* Defined with respect to frame_id */
/* 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
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;
##### 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);