We have to create a table and a grasp object similar to the robot environment. Here we are creating a table almost the same as in the gazebo simulation. If the table size and the pose of gazebo and MoveIt! are same, we can set the position of the grasping object:

    def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()


        #Table position
        p.pose.position.x = 0.45
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22


        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)


        # Table size from ~/.gazebo/models/grasp_table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))


        return p.pose

Following is the creation of the grasp object. We are creating a random grasp object here. You can change the pose and size of it according to your environment:

    def _add_grasp_block_(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
       p.header.stamp = rospy.Time.now()


        p.pose.position.x = 0.25  
        p.pose.position.y = 0.05
        p.pose.position.z = 0.32


        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)


        # Grasp Object can size from ~/.gazebo/models/grasp_object/model.sdf,
        self._scene.add_box(name, p, (0.03, 0.03, 0.09))


        return p.pose

After creating the grasp object and the grasp table, we will see how to set the pick position and the place position from the following code snippet. Here the pose of the grasp object created in the planning scene is retrieved and fed into the place pose in which the Y axis of the place pose is subtracted by 0.06. So when the pick and place happens, the object will place into 0.06 away from the object in the Y direction.

        # Add table and grap object to the planning scene:


        self._pose_table    = self._add_table(self._table_object_name)
        self._pose_grasp_obj = self._add_grasp_block_(self._grasp_object_name)


        rospy.sleep(1.0)


        # Define target place pose:
        self._pose_place = Pose()


        self._pose_place.position.x = self._pose_grasp_obj.position.x
        self._pose_place.position.y = self._pose_grasp_obj.position.y - 0.06
        self._pose_place.position.z = self._pose_grasp_obj.position.z


        self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))

The next step is to generate the grasp Pose Array data for visualization and then send the grasp goal to the grasp server. If there is a grasp sequence, it will be published, else it will show as an error.

def _generate_grasps(self, pose, width):


        # Create goal:
        goal = GenerateGraspsGoal()
        goal.pose  = pose
        goal.width = width


        ......................
        .......................


        state = self._grasps_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
            return None


        grasps = self._grasps_ac.get_result().grasps


        # Publish grasps (for debugging/visualization purposes):
        self._publish_grasps(grasps)


        return grasps

This function will create a Pose Array data for the pose of the place.

   def _generate_places(self, target):


        # Generate places:
        places = []
        now = rospy.Time.now()
        for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)):
            # Create place location:
            place = PlaceLocation()


            ............................................
.......................................
   # Add place:
            places.append(place)


        # Publish places (for debugging/visualization purposes):
        self._publish_places(places)

The following function will create a goal object for picking the object, which has to send into MoveIt!.

def _create_pickup_goal(self, group, target, grasps):
        """
        Create a MoveIt!! PickupGoal
        """


        # Create goal:
        goal = PickupGoal()


        goal.group_name  = group
        goal.target_name = target


                        .............
        return goal

Also, there is the def _create_place_goal(self, group, target, places) function to create place goal for MoveIt!.

The important functions which are performing picking and placing are given below.

These functions will generate a pick and place sequence, which will be sent to MoveIt! and print the result of the motion planning, whether it is succeeded or not:

def _pickup(self, group, target, width)
def _place(self, group, target, place)