Course_Manipulation_Unit_5.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ROS Manipulation in 5 Days"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Unit 5: Grasping"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**SUMMARY**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Estimated time of completion: <b>2h</b><br><br>\n",
    "This unit will show you how to perform motion planning with Python. By completing this unit, you will be able to create a Python program that performs motion planning on your robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END OF SUMMARY**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## What is Grasping?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When working with manipulator robots, one of the main goals to reach is picking an object up from one position, and placing it in another one, which is commonly known as pick & place. We call <b>Grasping</b> the process of picking an object up by the robot end-effector (which can be a hand, a gripper, etc.). Although this may sound like a very simple task, it is not. In fact, it's a very complex process, because lots of variables need to be taken into account when picking the object.\n",
    "<br><br>\n",
    "We humans handle our grasping using our intelligence, but in a robot, we have to create rules for it. One of the constraints in grasping is force; the gripper/end-effector should adjust the grasping force for picking up the object, but not make any deformation on the object while grasping it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Grasping using MoveIt!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First of all, we are going to see how to perform Grasping in MoveIt. This means, without controlling the real robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And for that, we are going to be using the <i><b>moveit_simple_grasps</b></i> package. This package is very useful in order to generate grasp poses for simple objects, such as blocks or cylinders. And it's quite simple, which helps in the process of learning. This package takes the position of the object to be grasped as input, and generates the necessary grasping sequences in order to pick the object up. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This package already supports robots such as Baxter, REEM, and Clam arm, among others. But it is quite easy to interface any other custom manipulator robot to the package, without having to modify too many of the codes. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 6.1**</p>\n",
    "<br>\n",
    "a) First of all, you will create a new package called <i><b>myrobot_grasping</b></i>. Inside this package, create a new folder named <i><b>launch</b></i>, which will contain a launch file called <i><b>grasp_generator_server.launch</b></i>. Copy the following content inside this file."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "  <arg name=\"robot\" default=\"fetch\"/>\n",
    "  <arg name=\"group\"        default=\"arm\"/>\n",
    "  <arg name=\"end_effector\" default=\"gripper\"/>\n",
    "\n",
    "  <node pkg=\"moveit_simple_grasps\" type=\"moveit_simple_grasps_server\" name=\"moveit_simple_grasps_server\">\n",
    "    <param name=\"group\"        value=\"$(arg group)\"/> \n",
    "    <param name=\"end_effector\" value=\"$(arg end_effector)\"/>\n",
    "    <rosparam command=\"load\" file=\"$(find myrobot_grasping)/config/$(arg robot)_grasp_data.yaml\"/> \n",
    "  </node>\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Basically, what we are doing in this launch file is starting a grasp server that will provide grasp sequences to a grasp client node. We need to specify the following into the node:\n",
    "\n",
    "* <b>Planning Group of the arm</b>\n",
    "* <b>Planning Group of end-effector</b>\n",
    "\n",
    "Also, we are loading the <i><b>fetch_grasp_data.yaml</b></i> file, which will contain detailed information about the gripper. We are going to create this file right now."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Inside this package, create a new folder named <i><b>config</b></i>, and create a new file named <i><b>fetch_grasp_data.yaml</b></i> inside this folder. Copy the following contents into this file."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "base_link: 'base_link'\n",
    "gripper:\n",
    "  #The end effector name for grasping\n",
    "  end_effector_name: 'gripper'\n",
    "  \n",
    "  # Gripper joints\n",
    "  joints: ['l_gripper_finger_joint', 'r_gripper_finger_joint']\n",
    "  \n",
    "  #Posture of grippers before grasping\n",
    "  pregrasp_posture: [0.048, 0.048]\n",
    "  pregrasp_time_from_start: 4.0\n",
    "  grasp_posture: [0.016, 0.016]\n",
    "  grasp_time_from_start: 4.0\n",
    "  postplace_time_from_start: 4.0\n",
    "  \n",
    "  # Desired pose from end effector to grasp [x, y, z] + [R, P, Y]\n",
    "  grasp_pose_to_eef: [-0.12, 0.0, 0.0]\n",
    "  grasp_pose_to_eef_rotation: [0.0, 0.0, 0.0]\n",
    "  end_effector_parent_link: 'wrist_roll_link'"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, in this file, we are basically providing detailed information about the gripper. These parameters can be tuned in order to improve the grasping process. The most important parameters that we are defining here are the following:\n",
    "\n",
    "* <b>end_effector_name</b>: The name of the end-effector group, as stated in the MoveIt package.\n",
    "* <b>joints</b>: The joints that form the gripper.\n",
    "* <b>pregrasp_posture</b>: The position of the gripper before grasping (OPEN position).\n",
    "* <b>pregrasp_time_from_start</b>: The time to wait before grasping.\n",
    "* <b>grasp_posture</b>: The position of the gripper when grasping (CLOSED position).\n",
    "* <b>grasp_time_from_start</b>: The time to wait after grasping.\n",
    "* <b>postplace_time_from_start</b>: The name of the end-effector.\n",
    "* <b>grasp_pose_to_eef</b>: The desired pose from end-effector to grasp - [x,y,z].\n",
    "* <b>grasp_pose_to_eef</b>: The desired pose from end-effector to grasp - [roll, pitch, yaw].\n",
    "* <b>end_effector_parent_link</b>: The name of the link that connects the gripper to the robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 6.1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, you have now created the basic structure to create a Grasping Server, which will allow you to send object positions to this server, and it will provide you with a sequence in order to grasp the specified object.\n",
    "<br><br>\n",
    "But, for that, we need to communicate with that server, right? And that's exactly what you are going to do in the next section!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Creating a pick and place task"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "There are various ways in which we can perform pick and place tasks. For instance, we could send the robot a predefined sequence of joint values directly, which will cause the robot to always perform the same predefined motion. So, for this case, we must always place the object in the same position. This method is called <b>forward kinematics</b> because you need to previously know the sequence of joint values that are required in order to perform a certain trajectory.\n",
    "<br><br>\n",
    "Another method would be by using <b>inverse kinematics</b> (IK), without any vision feedback. In this case, we provide the robot with the Pose (x, y, and z) where the object to picked is, and by doing some IK calculations, the robot would know which motions it needs to perform in order to reach the object's Pose.\n",
    "<br><br>\n",
    "Finally, another method would be to use <b>inverse kinematics</b>, but with vision support or feedback. In this case, we would use a node that would identify the Pose of the object by reading the data of the sensors of the robot; for instance, a Kinect camera. Then, this vision node would provide the Pose of the object, and again, by using IK calculations, the robot would know the required motions to perform in order to reach the object.\n",
    "<br><br>\n",
    "You are probably thinking that the most efficient and stylish method is the third one, and you're right. But, this method would require you to have some Object Recognition knowledge, which is not the subject of this course. So, for this course, we are going to use the second method, which in the end, is the same as the third one, without the object recognition part. If you already have some Perception knowledge, you can freely add an Object Recognition node that sends the Pose of the object, without having to set this Pose in the code itself."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, to summarize, what you are going to do in this section of the course is create a node that will send an object Pose to the Grasping Server, in order to receive the appropiate joint values to reach that object. Well then, enough talking, let's get to work!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 6.2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) Inside the package that you created in the previous exercise, create a new folder named <i><b>scripts</b></i>. Inside this folder, create a new file named <i><b>pick_and_place.py</b></i>. Copy the following code inside it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "\n",
    "from moveit_commander import RobotCommander, PlanningSceneInterface\n",
    "from moveit_commander import roscpp_initialize, roscpp_shutdown\n",
    "\n",
    "from actionlib import SimpleActionClient, GoalStatus\n",
    "\n",
    "from geometry_msgs.msg import Pose, PoseStamped, PoseArray, Quaternion\n",
    "from moveit_msgs.msg import PickupAction, PickupGoal\n",
    "from moveit_msgs.msg import PlaceAction, PlaceGoal\n",
    "from moveit_msgs.msg import PlaceLocation\n",
    "from moveit_msgs.msg import MoveItErrorCodes\n",
    "from moveit_simple_grasps.msg import GenerateGraspsAction, GenerateGraspsGoal, GraspGeneratorOptions\n",
    "\n",
    "from tf.transformations import quaternion_from_euler\n",
    "\n",
    "import sys\n",
    "import copy\n",
    "import numpy\n",
    "\n",
    "\n",
    "# Create dict with human readable MoveIt! error codes:\n",
    "moveit_error_dict = {}\n",
    "for name in MoveItErrorCodes.__dict__.keys():\n",
    "    if not name[:1] == '_':\n",
    "        code = MoveItErrorCodes.__dict__[name]\n",
    "        moveit_error_dict[code] = name\n",
    "\n",
    "\n",
    "class Pick_Place:\n",
    "    def __init__(self):\n",
    "        # Retrieve params:\n",
    "        self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')\n",
    "        self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')\n",
    "\n",
    "        self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)\n",
    "\n",
    "        self._arm_group     = rospy.get_param('~arm', 'arm')\n",
    "        self._gripper_group = rospy.get_param('~gripper', 'gripper')\n",
    "\n",
    "        self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2)\n",
    "        self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1)\n",
    "\n",
    "        # Create (debugging) publishers:\n",
    "        self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)\n",
    "        self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)\n",
    "\n",
    "        # Create planning scene and robot commander:\n",
    "        self._scene = PlanningSceneInterface()\n",
    "        self._robot = RobotCommander()\n",
    "\n",
    "        rospy.sleep(1.0)\n",
    "\n",
    "        # Clean the scene:\n",
    "        self._scene.remove_world_object(self._table_object_name)\n",
    "        self._scene.remove_world_object(self._grasp_object_name)\n",
    "\n",
    "        # Add table and Coke can objects to the planning scene:\n",
    "        self._pose_table    = self._add_table(self._table_object_name)\n",
    "        self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)\n",
    "\n",
    "        rospy.sleep(1.0)\n",
    "\n",
    "        # Define target place pose:\n",
    "        self._pose_place = Pose()\n",
    "\n",
    "        self._pose_place.position.x = self._pose_coke_can.position.x \n",
    "        self._pose_place.position.y = self._pose_coke_can.position.y - 0.10\n",
    "        self._pose_place.position.z = self._pose_coke_can.position.z + 0.08\n",
    "\n",
    "        self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))\n",
    "\n",
    "        # Retrieve groups (arm and gripper):\n",
    "        self._arm     = self._robot.get_group(self._arm_group)\n",
    "        self._gripper = self._robot.get_group(self._gripper_group)\n",
    "\n",
    "        # Create grasp generator 'generate' action client:\n",
    "        self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)\n",
    "        if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):\n",
    "            rospy.logerr('Grasp generator action client not available!')\n",
    "            rospy.signal_shutdown('Grasp generator action client not available!')\n",
    "            return\n",
    "\n",
    "        # Create move group 'pickup' action client:\n",
    "        self._pickup_ac = SimpleActionClient('/pickup', PickupAction)\n",
    "        if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):\n",
    "            rospy.logerr('Pick up action client not available!')\n",
    "            rospy.signal_shutdown('Pick up action client not available!')\n",
    "            return\n",
    "\n",
    "        # Create move group 'place' action client:\n",
    "        self._place_ac = SimpleActionClient('/place', PlaceAction)\n",
    "        if not self._place_ac.wait_for_server(rospy.Duration(5.0)):\n",
    "            rospy.logerr('Place action client not available!')\n",
    "            rospy.signal_shutdown('Place action client not available!')\n",
    "            return\n",
    "\n",
    "        # Pick Coke can object:\n",
    "        while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):\n",
    "            rospy.logwarn('Pick up failed! Retrying ...')\n",
    "            rospy.sleep(1.0)\n",
    "\n",
    "        rospy.loginfo('Pick up successfully')\n",
    "\n",
    "        # Place Coke can object on another place on the support surface (table):\n",
    "        while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):\n",
    "            rospy.logwarn('Place failed! Retrying ...')\n",
    "            rospy.sleep(1.0)\n",
    "\n",
    "        rospy.loginfo('Place successfully')\n",
    "\n",
    "    def __del__(self):\n",
    "        # Clean the scene:\n",
    "        self._scene.remove_world_object(self._grasp_object_name)\n",
    "        self._scene.remove_world_object(self._table_object_name)\n",
    "\n",
    "    def _add_table(self, name):\n",
    "        p = PoseStamped()\n",
    "        p.header.frame_id = self._robot.get_planning_frame()\n",
    "        p.header.stamp = rospy.Time.now()\n",
    "\n",
    "        p.pose.position.x = 1.0\n",
    "        p.pose.position.y = 0.08\n",
    "        p.pose.position.z = 1.02\n",
    "\n",
    "        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))\n",
    "        p.pose.orientation = Quaternion(*q)\n",
    "\n",
    "        # Table size from ~/.gazebo/models/table/model.sdf, using the values\n",
    "        # for the surface link.\n",
    "        self._scene.add_box(name, p, (0.86, 0.86, 0.02))\n",
    "\n",
    "        return p.pose\n",
    "\n",
    "    def _add_grasp_block_(self, name):\n",
    "        p = PoseStamped()\n",
    "        p.header.frame_id = self._robot.get_planning_frame()\n",
    "        p.header.stamp = rospy.Time.now()\n",
    "\n",
    "        p.pose.position.x = 0.62   \n",
    "        p.pose.position.y = 0.21\n",
    "        p.pose.position.z = 1.07\n",
    "\n",
    "        q = quaternion_from_euler(0.0, 0.0, 0.0)\n",
    "        p.pose.orientation = Quaternion(*q)\n",
    "\n",
    "        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,\n",
    "        # using the measure tape tool from meshlab.\n",
    "        # The box is the bounding box of the coke cylinder.\n",
    "        # The values are taken from the cylinder base diameter and height.\n",
    "        self._scene.add_box(name, p, (0.077, 0.077, 0.070))\n",
    "\n",
    "        return p.pose\n",
    "\n",
    "    def _generate_grasps(self, pose, width):\n",
    "        \"\"\"\n",
    "        Generate grasps by using the grasp generator generate action; based on\n",
    "        server_test.py example on moveit_simple_grasps pkg.\n",
    "        \"\"\"\n",
    "\n",
    "        # Create goal:\n",
    "        goal = GenerateGraspsGoal()\n",
    "\n",
    "        goal.pose  = pose\n",
    "        goal.width = width\n",
    "\n",
    "        options = GraspGeneratorOptions()\n",
    "        # simple_graps.cpp doesn't implement GRASP_AXIS_Z!\n",
    "        #options.grasp_axis      = GraspGeneratorOptions.GRASP_AXIS_Z\n",
    "        options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP\n",
    "        options.grasp_rotation  = GraspGeneratorOptions.GRASP_ROTATION_FULL\n",
    "\n",
    "        # @todo disabled because it works better with the default options\n",
    "        #goal.options.append(options)\n",
    "\n",
    "        # Send goal and wait for result:\n",
    "        state = self._grasps_ac.send_goal_and_wait(goal)\n",
    "        if state != GoalStatus.SUCCEEDED:\n",
    "            rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())\n",
    "            return None\n",
    "\n",
    "        grasps = self._grasps_ac.get_result().grasps\n",
    "\n",
    "        # Publish grasps (for debugging/visualization purposes):\n",
    "        self._publish_grasps(grasps)\n",
    "\n",
    "        return grasps\n",
    "\n",
    "    def _generate_places(self, target):\n",
    "        \"\"\"\n",
    "        Generate places (place locations), based on\n",
    "        https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/\n",
    "        baxter_pick_place/src/block_pick_place.cpp\n",
    "        \"\"\"\n",
    "\n",
    "        # Generate places:\n",
    "        places = []\n",
    "        now = rospy.Time.now()\n",
    "        for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)):\n",
    "            # Create place location:\n",
    "            place = PlaceLocation()\n",
    "\n",
    "            place.place_pose.header.stamp = now\n",
    "            place.place_pose.header.frame_id = self._robot.get_planning_frame()\n",
    "\n",
    "            # Set target position:\n",
    "            place.place_pose.pose = copy.deepcopy(target)\n",
    "\n",
    "            # Generate orientation (wrt Z axis):\n",
    "            q = quaternion_from_euler(0.0, 0.0, angle )\n",
    "            place.place_pose.pose.orientation = Quaternion(*q)\n",
    "\n",
    "            # Generate pre place approach:\n",
    "            place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist\n",
    "            place.pre_place_approach.min_distance = self._approach_retreat_min_dist\n",
    "\n",
    "            place.pre_place_approach.direction.header.stamp = now\n",
    "            place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame()\n",
    "\n",
    "            place.pre_place_approach.direction.vector.x =  0\n",
    "            place.pre_place_approach.direction.vector.y =  0\n",
    "            place.pre_place_approach.direction.vector.z = 0.2\n",
    "\n",
    "            # Generate post place approach:\n",
    "            place.post_place_retreat.direction.header.stamp = now\n",
    "            place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame()\n",
    "\n",
    "            place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist\n",
    "            place.post_place_retreat.min_distance = self._approach_retreat_min_dist\n",
    "\n",
    "            place.post_place_retreat.direction.vector.x = 0\n",
    "            place.post_place_retreat.direction.vector.y = 0\n",
    "            place.post_place_retreat.direction.vector.z = 0.2\n",
    "\n",
    "            # Add place:\n",
    "            places.append(place)\n",
    "\n",
    "        # Publish places (for debugging/visualization purposes):\n",
    "        self._publish_places(places)\n",
    "\n",
    "        return places\n",
    "\n",
    "    def _create_pickup_goal(self, group, target, grasps):\n",
    "        \"\"\"\n",
    "        Create a MoveIt! PickupGoal\n",
    "        \"\"\"\n",
    "\n",
    "        # Create goal:\n",
    "        goal = PickupGoal()\n",
    "\n",
    "        goal.group_name  = group\n",
    "        goal.target_name = target\n",
    "\n",
    "        goal.possible_grasps.extend(grasps)\n",
    "\n",
    "        goal.allowed_touch_objects.append(target)\n",
    "\n",
    "        goal.support_surface_name = self._table_object_name\n",
    "\n",
    "        # Configure goal planning options:\n",
    "        goal.allowed_planning_time = 7.0\n",
    "\n",
    "        goal.planning_options.planning_scene_diff.is_diff = True\n",
    "        goal.planning_options.planning_scene_diff.robot_state.is_diff = True\n",
    "        goal.planning_options.plan_only = False\n",
    "        goal.planning_options.replan = True\n",
    "        goal.planning_options.replan_attempts = 20\n",
    "\n",
    "        return goal\n",
    "\n",
    "    def _create_place_goal(self, group, target, places):\n",
    "        \"\"\"\n",
    "        Create a MoveIt! PlaceGoal\n",
    "        \"\"\"\n",
    "\n",
    "        # Create goal:\n",
    "        goal = PlaceGoal()\n",
    "\n",
    "        goal.group_name           = group\n",
    "        goal.attached_object_name = target\n",
    "\n",
    "        goal.place_locations.extend(places)\n",
    "\n",
    "        # Configure goal planning options:\n",
    "        goal.allowed_planning_time = 7.0\n",
    "\n",
    "        goal.planning_options.planning_scene_diff.is_diff = True\n",
    "        goal.planning_options.planning_scene_diff.robot_state.is_diff = True\n",
    "        goal.planning_options.plan_only = False\n",
    "        goal.planning_options.replan = True\n",
    "        goal.planning_options.replan_attempts = 20\n",
    "\n",
    "        return goal\n",
    "\n",
    "    def _pickup(self, group, target, width):\n",
    "        \"\"\"\n",
    "        Pick up a target using the planning group\n",
    "        \"\"\"\n",
    "\n",
    "        # Obtain possible grasps from the grasp generator server:\n",
    "        grasps = self._generate_grasps(self._pose_coke_can, width)\n",
    "\n",
    "        # Create and send Pickup goal:\n",
    "        goal = self._create_pickup_goal(group, target, grasps)\n",
    "\n",
    "        state = self._pickup_ac.send_goal_and_wait(goal)\n",
    "        if state != GoalStatus.SUCCEEDED:\n",
    "            rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())\n",
    "            return None\n",
    "\n",
    "        result = self._pickup_ac.get_result()\n",
    "\n",
    "        # Check for error:\n",
    "        err = result.error_code.val\n",
    "        if err != MoveItErrorCodes.SUCCESS:\n",
    "            rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))\n",
    "\n",
    "            return False\n",
    "\n",
    "        return True\n",
    "\n",
    "    def _place(self, group, target, place):\n",
    "        \"\"\"\n",
    "        Place a target using the planning group\n",
    "        \"\"\"\n",
    "\n",
    "        # Obtain possible places:\n",
    "        places = self._generate_places(place)\n",
    "\n",
    "        # Create and send Place goal:\n",
    "        goal = self._create_place_goal(group, target, places)\n",
    "\n",
    "        state = self._place_ac.send_goal_and_wait(goal)\n",
    "        if state != GoalStatus.SUCCEEDED:\n",
    "            rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())\n",
    "            return None\n",
    "\n",
    "        result = self._place_ac.get_result()\n",
    "\n",
    "        # Check for error:\n",
    "        err = result.error_code.val\n",
    "        if err != MoveItErrorCodes.SUCCESS:\n",
    "            rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))\n",
    "\n",
    "            return False\n",
    "\n",
    "        return True\n",
    "\n",
    "    def _publish_grasps(self, grasps):\n",
    "        \"\"\"\n",
    "        Publish grasps as poses, using a PoseArray message\n",
    "        \"\"\"\n",
    "\n",
    "        if self._grasps_pub.get_num_connections() > 0:\n",
    "            msg = PoseArray()\n",
    "            msg.header.frame_id = self._robot.get_planning_frame()\n",
    "            msg.header.stamp = rospy.Time.now()\n",
    "\n",
    "            for grasp in grasps:\n",
    "                p = grasp.grasp_pose.pose\n",
    "\n",
    "                msg.poses.append(Pose(p.position, p.orientation))\n",
    "\n",
    "            self._grasps_pub.publish(msg)\n",
    "\n",
    "    def _publish_places(self, places):\n",
    "        \"\"\"\n",
    "        Publish places as poses, using a PoseArray message\n",
    "        \"\"\"\n",
    "\n",
    "        if self._places_pub.get_num_connections() > 0:\n",
    "            msg = PoseArray()\n",
    "            msg.header.frame_id = self._robot.get_planning_frame()\n",
    "            msg.header.stamp = rospy.Time.now()\n",
    "\n",
    "            for place in places:\n",
    "                msg.poses.append(place.place_pose.pose)\n",
    "\n",
    "            self._places_pub.publish(msg)\n",
    "\n",
    "\n",
    "def main():\n",
    "    p = Pick_Place()\n",
    "\n",
    "    rospy.spin()\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    roscpp_initialize(sys.argv)\n",
    "    rospy.init_node('pick_and_place')\n",
    "\n",
    "    main()\n",
    "\n",
    "    roscpp_shutdown()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Yes, I know... this is a lot of code! And there's no explanation at all! But, don't worry if you don't understand much of the code at this point. After completing this exercise, we are going to go into detail about the main parts of the code. Trust me!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) First of all, launch the demo.launch file generated in the MoveIt package in Chapter 3.\n",
    "<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch fetch_moveit_config demo.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Set the \"Planning Attempts\" option to something like 6."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Planning Attempts",
    "image": true,
    "name": "planning_att",
    "width": "6cm"
   },
   "source": [
    "<img src=\"img/planning_att.png\" width=\"300\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Next, launch the grasping server you created in the previous exercise.\n",
    "<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch myrobot_grasping grasp_generator_server.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "e) Finally, launch this Python script that you've just created, and move to the MoveIt RViz screen in order to visualize what's going on.\n",
    "<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosrun myrobot_grasping pick_and_place.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\"><b>IMPORTANT READ:</b> Since the update of the system to ROS Kinetic, the simulation of the pick and place action in MoveIt may fail. If you try to run it and it fails, just move one and continue following the Notebook. You will do this pick and place action in the next exercise, which is exactly the same as this one but executing it in the Gazebo simulated robot. That exercise should be working fine. Sorry for the inconveniences.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#AE0202;color:white;\">**Expected Result for Exercise 6.2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Initial status:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Grasp Object and Table in RViz",
    "image": true,
    "name": "grasp_demo1",
    "width": "12cm"
   },
   "source": [
    "<img src=\"img/grasp_demo1.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Going to grasp pose:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Planning Trajectory",
    "image": true,
    "name": "grasp_demo2",
    "width": "12cm"
   },
   "source": [
    "<img src=\"img/grasp_demo2.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Grasping object:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Grasping Object",
    "image": true,
    "name": "grasp_demo3",
    "width": "12cm"
   },
   "source": [
    "<img src=\"img/grasp_demo3.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 6.2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Several things happened in the previous exercise, so let's go by parts!\n",
    "\n",
    "* First of all, you saw a couple of green blocks appearing in the RViz screen, right? Can you guess what they are?\n",
    "<br>\n",
    "Well, that were the table and the object to be grasped, of course! Inside the pick_and_place.py script, we have created these 2 objects and added them into the MoveIt Planning Scene. Whenever you add an object to the Planning Scene, it will appear in this way.\n",
    "* Second, the <b>Pick</b> Action starts. After getting the grasp object position, our node sends this position to the grasp server, which will generate IK and check if there's any valid IK in order to pick the object up. If it finds any feasible IK solution, the arm will begin to execute the specified motions in order to pick the object up.</li>\n",
    "* After the object is picked by the gripper, the <b>Place</b> Action starts. Again, our node will send a Pose to the grasp server where the arm should place the object. Then, the server will check for a valid IK solution for that specified Pose. If it finds any valid solution, the gripper will move to that position and release the object.</li>\n",
    "\n",
    "You can have a look at the topics <b>/grasp</b> and <b>/place</b> in order to better see what's going on.\n",
    "<br><br>\n",
    "Ok, so now you have a better idea of what happened in the previous exercise. Let's take a deeper look into the code in order to see how all of these work."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Explaining the code"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First of all, let's check how we created and added both the table and the grasping object."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _add_table(self, name):\n",
    "    p = PoseStamped()\n",
    "    p.header.frame_id = self._robot.get_planning_frame()\n",
    "    p.header.stamp = rospy.Time.now()\n",
    "    #Table position\n",
    "    p.pose.position.x = 0.45\n",
    "    p.pose.position.y = 0.0\n",
    "    p.pose.position.z = 0.22\n",
    "    q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))\n",
    "    p.pose.orientation = Quaternion(*q)\n",
    "    # Table size \n",
    "    self._scene.add_box(name, p, (0.5, 0.4, 0.02))\n",
    "    return p.pose"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this section of the code, what we are doing is creating and adding a table to the Planning Scene. Quite self explanatory, right?\n",
    "<br><br>\n",
    "We first create a PoseStamped message, and we fill it with the necessary information: the frame_id, the time stamp, and the position and orientation where the table will be placed. Then, we add the table to the scene using the add_box function."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following is the creation of the grasp object."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _add_grasp_block_(self, name):\n",
    "    p = PoseStamped()\n",
    "    p.header.frame_id = self._robot.get_planning_frame()\n",
    "    p.header.stamp = rospy.Time.now()\n",
    "    p.pose.position.x = 0.62   \n",
    "    p.pose.position.y = 0.21\n",
    "    p.pose.position.z = 1.07\n",
    "    q = quaternion_from_euler(0.0, 0.0, 0.0)\n",
    "    p.pose.orientation = Quaternion(*q)\n",
    "    # Grasp Object can size \n",
    "    self._scene.add_box(name, p, (0.03, 0.03, 0.09))\n",
    "    return p.pose"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the above section of the code, what we are doing is the exact same thing as what we did with the table, but for the grasp object. Nothing new here!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "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 grasp object will be placed 0.06 meters (6cm) away from the initial pose of the object in the direction of Y."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Add table and grap object to the planning scene:\n",
    "self._pose_table    = self._add_table(self._table_object_name)\n",
    "self._pose_grasp_obj = self._add_grasp_block_(self._grasp_object_name)\n",
    "rospy.sleep(1.0)\n",
    "# Define target place pose:\n",
    "self._pose_place = Pose()\n",
    "self._pose_place.position.x = self._pose_grasp_obj.position.x\n",
    "self._pose_place.position.y = self._pose_grasp_obj.position.y - 0.06\n",
    "self._pose_place.position.z = self._pose_grasp_obj.position.z\n",
    "self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "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; otherwise, it will show as an error."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _generate_grasps(self, pose, width):\n",
    "    # Create goal:\n",
    "    goal = GenerateGraspsGoal()\n",
    "    goal.pose  = pose\n",
    "    goal.width = width\n",
    "    ......................\n",
    "    .......................\n",
    "    state = self._grasps_ac.send_goal_and_wait(goal)\n",
    "    if state != GoalStatus.SUCCEEDED:\n",
    "        rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())\n",
    "        return None\n",
    "    grasps = self._grasps_ac.get_result().grasps\n",
    "    # Publish grasps (for debugging/visualization purposes):\n",
    "    self._publish_grasps(grasps)\n",
    "    return grasps"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This function will create a Pose Array data for the pose of the place."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _generate_places(self, target):\n",
    "    # Generate places:\n",
    "    places = []\n",
    "    now = rospy.Time.now()\n",
    "    for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)):\n",
    "        # Create place location:\n",
    "        place = PlaceLocation()\n",
    "        ............................................\n",
    "        .......................................\n",
    "        # Add place:\n",
    "        places.append(place)\n",
    "    # Publish places (for debugging/visualization purposes):\n",
    "    self._publish_places(places)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next function is **_create_pickup_goal()**, which will create a goal for picking up the grasping object. This goal has to be sent into MoveIt!."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _create_pickup_goal(self, group, target, grasps):\n",
    "    \"\"\"\n",
    "    Create a MoveIt! PickupGoal\n",
    "    \"\"\"\n",
    "\n",
    "    # Create goal:\n",
    "    goal = PickupGoal()\n",
    "\n",
    "    goal.group_name  = group\n",
    "    goal.target_name = target\n",
    "\n",
    "    goal.possible_grasps.extend(grasps)\n",
    "\n",
    "    goal.allowed_touch_objects.append(target)\n",
    "\n",
    "    goal.support_surface_name = self._table_object_name\n",
    "\n",
    "    # Configure goal planning options:\n",
    "    goal.allowed_planning_time = 7.0\n",
    "\n",
    "    goal.planning_options.planning_scene_diff.is_diff = True\n",
    "    goal.planning_options.planning_scene_diff.robot_state.is_diff = True\n",
    "    goal.planning_options.plan_only = False\n",
    "    goal.planning_options.replan = True\n",
    "    goal.planning_options.replan_attempts = 20\n",
    "\n",
    "    return goal"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Also, there is the **_create_place_goal()** function, which creates a place goal for MoveIt."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _create_place_goal(self, group, target, places):\n",
    "    \"\"\"\n",
    "    Create a MoveIt! PlaceGoal\n",
    "    \"\"\"\n",
    "\n",
    "    # Create goal:\n",
    "    goal = PlaceGoal()\n",
    "\n",
    "    goal.group_name           = group\n",
    "    goal.attached_object_name = target\n",
    "\n",
    "    goal.place_locations.extend(places)\n",
    "\n",
    "    # Configure goal planning options:\n",
    "    goal.allowed_planning_time = 7.0\n",
    "\n",
    "    goal.planning_options.planning_scene_diff.is_diff = True\n",
    "    goal.planning_options.planning_scene_diff.robot_state.is_diff = True\n",
    "    goal.planning_options.plan_only = False\n",
    "    goal.planning_options.replan = True\n",
    "    goal.planning_options.replan_attempts = 20\n",
    "\n",
    "    return goal"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The important functions that are performing picking and placing are given below.\n",
    "These functions will generate a pick and place sequence, which will be sent to MoveIt and print the results of the motion planning, whether it is successful or not:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _pickup(self, group, target, width):\n",
    "    \"\"\"\n",
    "    Pick up a target using the planning group\n",
    "    \"\"\"\n",
    "\n",
    "    # Obtain possible grasps from the grasp generator server:\n",
    "    grasps = self._generate_grasps(self._pose_coke_can, width)\n",
    "\n",
    "    # Create and send Pickup goal:\n",
    "    goal = self._create_pickup_goal(group, target, grasps)\n",
    "\n",
    "    state = self._pickup_ac.send_goal_and_wait(goal)\n",
    "    if state != GoalStatus.SUCCEEDED:\n",
    "        rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())\n",
    "        return None\n",
    "\n",
    "    result = self._pickup_ac.get_result()\n",
    "\n",
    "    # Check for error:\n",
    "    err = result.error_code.val\n",
    "    if err != MoveItErrorCodes.SUCCESS:\n",
    "        rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))\n",
    "\n",
    "        return False\n",
    "\n",
    "    return True"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _place(self, group, target, place):\n",
    "    \"\"\"\n",
    "    Place a target using the planning group\n",
    "    \"\"\"\n",
    "\n",
    "    # Obtain possible places:\n",
    "    places = self._generate_places(place)\n",
    "\n",
    "    # Create and send Place goal:\n",
    "    goal = self._create_place_goal(group, target, places)\n",
    "\n",
    "    state = self._place_ac.send_goal_and_wait(goal)\n",
    "    if state != GoalStatus.SUCCEEDED:\n",
    "        rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())\n",
    "        return None\n",
    "\n",
    "    result = self._place_ac.get_result()\n",
    "\n",
    "    # Check for error:\n",
    "    err = result.error_code.val\n",
    "    if err != MoveItErrorCodes.SUCCESS:\n",
    "        rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))\n",
    "\n",
    "        return False\n",
    "\n",
    "    return True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Grasping in the Real Robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, now you've seen how to perform Grasping in MoveIt. But... what about applying this to the real robot? Well, actually, it's quite easy at this point."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 6.3**</p>\n",
    "<br>\n",
    "a) First of all, execute the following commands in order to spawn a table and a Grasping object into the simulation.\n",
    "<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosrun gazebo_ros spawn_model -database table -gazebo -model table -x 1.30 -y 0 -z 0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosrun gazebo_ros spawn_model -database demo_cube -gazebo -model grasp_cube -x 0.65 -y 0.15 -z 1.07"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should end up with something like this."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Grasping Object and Table in simulation",
    "image": true,
    "name": "add_scene",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/add_scene.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Launch MoveIt with Perception.\n",
    "<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch fetch_moveit_config myrobot_planning_execution.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Perception in RViz",
    "image": true,
    "name": "grasp_perception",
    "width": "12cm"
   },
   "source": [
    "<img src=\"img/grasp_perception.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Set the \"Planning Attempts\" option to something like 6."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Planning Attempts",
    "image": true,
    "name": "planning_att",
    "width": "6cm"
   },
   "source": [
    "<img src=\"img/planning_att.png\" width=\"300\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Launch the grasping server you created in the previous exercise.\n",
    "<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #2</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch myrobot_grasping grasp_generator_server.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "e) Launch this Python script.\n",
    "<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #3</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosrun myrobot_grasping pick_and_place.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#AE0202;color:white;\">**Expected Result for Exercise 6.3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "RViz view:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Scene in Rviz",
    "image": true,
    "name": "grasp_perception2",
    "width": "12cm"
   },
   "source": [
    "<img src=\"img/grasp_perception2.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Simulation view:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Grasping in the simulation",
    "image": true,
    "name": "pick_place",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/pick_place.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 6.3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's all! You've finished the course! Well... almost finished. If you go to the next unit, you'll be able to do a project where you will test everything you've learned during the course."
   ]
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python [Root]",
   "language": "python",
   "name": "Python [Root]"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  },
  "latex_metadata": {
   "chapter": "5 - Grasping",
   "chapter_title": "Unit 5. Grasping",
   "course_title": "ROS MANIPULATION IN 5 DAYS"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}