Course_ShadowHandInstructionManual_Unit4.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Mastering with ROS: Shadow Hand"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Unit 4: Smart Grasping System"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Estimated time of completion: <b>1h</b><br><br>\n",
    "This Unit will show you how to interact and what is the purpose of the different functions integrated in the Smart Grasping System."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END OF SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The Smart Grasping System, developed by the Shadow Robot Company, is basically a set of functions and tools which main purpose is to ease the accomplishment of grasping tasks. This functions are written in Python, and they can be found in the **smart_grasper.py** file, within the **smart_grasping_sandbox** package. During this Unit, we are going to analyze and test the functionaility of some of the main functions that this Smart Grasping System provides."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Code explanation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Before starting to explain the different functions, let's have a quick look at the SmartGrasper class definition and initialization. Here you have the code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class SmartGrasper(object):\n",
    "\n",
    "    __last_joint_state = None\n",
    "    __current_model_name = \"cricket_ball\"\n",
    "    __path_to_models = \"/root/.gazebo/models/\"\n",
    "    \n",
    "    def __init__(self):\n",
    "        \"\"\"\n",
    "        This constructor initialises the different necessary connections to the topics and services\n",
    "        and resets the world to start in a good position.\n",
    "        \"\"\"\n",
    "        rospy.init_node(\"smart_grasper\")\n",
    "        \n",
    "        self.__joint_state_sub = rospy.Subscriber(\"/joint_states\", JointState, \n",
    "                                                  self.__joint_state_cb, queue_size=1)\n",
    "\n",
    "        rospy.wait_for_service(\"/gazebo/get_model_state\", 10.0)\n",
    "        rospy.wait_for_service(\"/gazebo/reset_world\", 10.0)\n",
    "        self.__reset_world = rospy.ServiceProxy(\"/gazebo/reset_world\", Empty)\n",
    "        self.__get_pose_srv = rospy.ServiceProxy(\"/gazebo/get_model_state\", GetModelState)\n",
    "\n",
    "        rospy.wait_for_service(\"/gazebo/pause_physics\")\n",
    "        self.__pause_physics = rospy.ServiceProxy(\"/gazebo/pause_physics\", Empty)\n",
    "        rospy.wait_for_service(\"/gazebo/unpause_physics\")\n",
    "        self.__unpause_physics = rospy.ServiceProxy(\"/gazebo/unpause_physics\", Empty)\n",
    "        rospy.wait_for_service(\"/controller_manager/switch_controller\")\n",
    "        self.__switch_ctrl = rospy.ServiceProxy(\"/controller_manager/switch_controller\", SwitchController)\n",
    "        rospy.wait_for_service(\"/gazebo/set_model_configuration\")\n",
    "        self.__set_model = rospy.ServiceProxy(\"/gazebo/set_model_configuration\", SetModelConfiguration)\n",
    "        \n",
    "        rospy.wait_for_service(\"/gazebo/delete_model\")\n",
    "        self.__delete_model = rospy.ServiceProxy(\"/gazebo/delete_model\", DeleteModel)\n",
    "        rospy.wait_for_service(\"/gazebo/spawn_sdf_model\")\n",
    "        self.__spawn_model = rospy.ServiceProxy(\"/gazebo/spawn_sdf_model\", SpawnModel)\n",
    "        \n",
    "        rospy.wait_for_service('/get_planning_scene', 10.0)\n",
    "        self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)\n",
    "        self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True)\n",
    "\n",
    "        self.arm_commander = MoveGroupCommander(\"arm\")\n",
    "        self.hand_commander = MoveGroupCommander(\"hand\")\n",
    "        \n",
    "        self.__hand_traj_client = SimpleActionClient(\"/hand_controller/follow_joint_trajectory\", \n",
    "                                                     FollowJointTrajectoryAction)\n",
    "        self.__arm_traj_client = SimpleActionClient(\"/arm_controller/follow_joint_trajectory\", \n",
    "                                                    FollowJointTrajectoryAction)\n",
    "                                              \n",
    "        if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:\n",
    "            rospy.logfatal(\"Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.\")\n",
    "            raise Exception(\"Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.\")\n",
    "                                              \n",
    "        if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:\n",
    "            rospy.logfatal(\"Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.\")\n",
    "            raise Exception(\"Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.\")\n",
    "            \n",
    "        self.reset_world()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Within this code, several things are being done:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1. First of all, we are initializing some variables, which will be used by the different functions. This variable are **__last_joint_state**, **__current_model_name** and **__path_to_models**.\n",
    "<br><br>\n",
    "2. Second, inside the constructor of the class, we are creating all the necessary connections to the different topics and services of the simulation.\n",
    "<br><br>\n",
    "3. Finally, at the end of the constructor, we call the **reset_world()** function, which will reset the world in order to start in the proper position."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! So with the proper introductions made, let's start to check the main functions!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### reset_world"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first function we find in the code, is the **reset_world()** function. It is used to reset the object poses in the world and the robot joint angles. So, to set the world to an initial state. Here's the code."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def reset_world(self):\n",
    "\n",
    "    self.__switch_ctrl.call(start_controllers=[], \n",
    "                            stop_controllers=[\"hand_controller\", \"arm_controller\", \"joint_state_controller\"], \n",
    "                            strictness=SwitchControllerRequest.BEST_EFFORT)\n",
    "    self.__pause_physics.call()\n",
    "\n",
    "    joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', \n",
    "                   'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']\n",
    "    joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0]\n",
    "\n",
    "    self.__set_model.call(model_name=\"smart_grasping_sandbox\", \n",
    "                          urdf_param_name=\"robot_description\",\n",
    "                          joint_names=joint_names, \n",
    "                          joint_positions=joint_positions)\n",
    "\n",
    "    timer = Timer(0.0, self.__start_ctrl)\n",
    "    timer.start()\n",
    "\n",
    "    time.sleep(0.1)\n",
    "    self.__unpause_physics.call()\n",
    "\n",
    "    self.__reset_world.call()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to use it, you will have to use the following structure:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from smart_grasping_sandbox.smart_grasper import SmartGrasper\n",
    "import time\n",
    "\n",
    "sgs = SmartGrasper()\n",
    "sgs.reset_world()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So... what are you doing here? Very easy! \n",
    "<br><br>\n",
    "1. First of all, you are importing the **SmartGrasper** class from the **smart_grasper.py** file. \n",
    "<br>\n",
    "2. Then, you import the **time** module, because it will be used inside the **reset_world()** function. \n",
    "<br>\n",
    "3. Next, you create an object of the SmartGrasper class.\n",
    "<br>\n",
    "4. And finally, you call the reset_world() function."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is the structure you will have to use to call any of the functions of the SmartGrasper class."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### get_object_pose"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next function is the **get_object_pose()**. The purpose of this function is very clear. Return the pose of the object to grasp."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_object_pose(self):\n",
    "        \n",
    "    return self.__get_pose_srv.call(self.__current_model_name, \"world\").pose"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see in the code, it is getting the pose of the object stated in the **__current_model_name** variable, which as you can see in the initialization of the class, it is set to **cricket_ball**, which is the name in the simulation of the red ball."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, in order to use this function, you will have to use:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from smart_grasping_sandbox.smart_grasper import SmartGrasper\n",
    "\n",
    "sgs = SmartGrasper()\n",
    "sgs.get_object_pose()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### get_tip_pose"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **get_tip_pose()** function is also very simple and clear. It will return the pose of the end-effector, which in this case is the Shadow Hand itself."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_tip_pose(self):\n",
    "\n",
    "    return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 4.1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) Inside your workspace, create a new folder (**NOT A ROS PACKAGE**) called **my_grasping_scripts**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Inside this folder, write a simple Python script that will call the **get_object_pose** and **get_tip_pose** functions, and print the result in the screen."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) If, for instance, you name your file **exercise1.py**, in order to execute it, you can type the following command:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "python my_script.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should get something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/ex_4_1.png\" width=\"800\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">**NOTE**: Don't worry about the error that appears at the end of the output. This is a known issue. It is caused by certain launched processes that were not closed correctly, but it doesn't has any effect on the execution of the code itself. This error should be fixed for the newest ROS distributions, like Kinetic.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 4.1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### move_tip_absolute"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next function we find is the **move_tip_absolute()**. Here is the code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def move_tip_absolute(self, target):\n",
    "\n",
    "    self.arm_commander.set_start_state_to_current_state()\n",
    "    self.arm_commander.set_pose_targets([target])\n",
    "    plan = self.arm_commander.plan()\n",
    "    if not self.arm_commander.execute(plan):\n",
    "        return False\n",
    "    return True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, this function gets as input a **target** variable. This variable is a **geometry_msgs/Pose** message. The function, then, moves the Shadow Hand to that poisition in the world frame."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### move_tip"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We also have the **move_tip()** function. The difference between this function and the previous one, is that in this one, you pass directly to the function the **(x, y, z, yaw, pitch, roll)** values that you want to move your Shadow Hand. Here you have the code."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):\n",
    "\n",
    "    transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),\n",
    "                            PyKDL.Vector(-x, -y, -z))\n",
    "\n",
    "    tip_pose = self.get_tip_pose()\n",
    "    tip_pose_kdl = posemath.fromMsg(tip_pose)\n",
    "    final_pose = toMsg(tip_pose_kdl * transform)\n",
    "\n",
    "    self.arm_commander.set_start_state_to_current_state()\n",
    "    self.arm_commander.set_pose_targets([final_pose])\n",
    "    plan = self.arm_commander.plan()\n",
    "    if not  self.arm_commander.execute(plan):\n",
    "        return False\n",
    "    return True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 4.2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) Using the **move_tip** function, create a Python script that moves the Shadow Hand 0.15 meters down in the y axis."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should get something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/move_tip.gif\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 4.2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### send_command"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next function we are going to check is the **send_command()** function. Here is the code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def send_command(self, command, duration=0.2):\n",
    "\n",
    "    hand_goal = None\n",
    "    arm_goal = None\n",
    "\n",
    "    for joint, target in command.items():\n",
    "        if \"H1\" in joint:\n",
    "            if not hand_goal:\n",
    "                hand_goal = FollowJointTrajectoryGoal()\n",
    "\n",
    "                point = JointTrajectoryPoint()\n",
    "                point.time_from_start = rospy.Duration.from_sec(duration)\n",
    "\n",
    "                hand_goal.trajectory.points.append(point)\n",
    "\n",
    "            hand_goal.trajectory.joint_names.append(joint)\n",
    "            hand_goal.trajectory.points[0].positions.append(target)\n",
    "        else:\n",
    "            if not arm_goal:\n",
    "                arm_goal = FollowJointTrajectoryGoal()\n",
    "\n",
    "                point = JointTrajectoryPoint()\n",
    "                point.time_from_start = rospy.Duration.from_sec(duration)\n",
    "\n",
    "                arm_goal.trajectory.points.append(point)\n",
    "\n",
    "            arm_goal.trajectory.joint_names.append(joint)\n",
    "            arm_goal.trajectory.points[0].positions.append(target)\n",
    "\n",
    "    if arm_goal:\n",
    "        self.__arm_traj_client.send_goal_and_wait(arm_goal)\n",
    "    if hand_goal:\n",
    "        self.__hand_traj_client.send_goal_and_wait(hand_goal)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, this function gets as input a **command** variable. This variable is a dictionary that contains **joint_names** associated with a **target** value. Something like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "{\"H1_F1J1\": -1.0, \"shoulder_pan_joint\": 1.0}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The values for each joint_name will define a trajectory, which will then be executed, if correct, by the robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 4.3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) Using the **send_command** function, create a Python script that moves the robot to a position like to this one:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/cmd_position.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The values of the joints in this position are the following:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/cmd_joint_values.png\" width=\"800\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 4.3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### get_current_joint_state"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This function is also very easy and clear. It simply returns the current values of the joints of the robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_current_joint_state(self):\n",
    "\n",
    "    joints_position = {n: p for n, p in\n",
    "                       zip(self.__last_joint_state.name,\n",
    "                           self.__last_joint_state.position)}\n",
    "    joints_velocity = {n: v for n, v in\n",
    "                       zip(self.__last_joint_state.name,\n",
    "                       self.__last_joint_state.velocity)}\n",
    "    joints_effort = {n: v for n, v in\n",
    "                     zip(self.__last_joint_state.name, \n",
    "                     self.__last_joint_state.effort)}\n",
    "    return joints_position, joints_velocity, joints_effort"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### open_hand"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **open_hand()**, as the names itself states, it just opens the Shadow Hand, in order to prepare it for a Grasping action. Here you can check the code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def open_hand(self):\n",
    "\n",
    "        self.hand_commander.set_named_target(\"open\")\n",
    "        plan = self.hand_commander.plan()\n",
    "        if not self.hand_commander.execute(plan, wait=True):\n",
    "            return False\n",
    "\n",
    "        return True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### close_hand"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Just as the previous function, the **close_hand()** function just closes the Shadow Hand."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def close_hand(self):\n",
    "\n",
    "    self.hand_commander.set_named_target(\"close\")\n",
    "    plan = self.hand_commander.plan()\n",
    "    if not self.hand_commander.execute(plan, wait=True):\n",
    "        return False\n",
    "\n",
    "    return True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 4.4</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) Using the **open_hand()** and **close_hand()** functions, create a simple Python script that opens and closes the Shadow Hand two times each."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should get something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/open_close_hand.gif\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 4.4</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### check_fingers_collisions"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next function is the **check_fingers_collision()**. Here you can see the code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def check_fingers_collisions(self, enable=True):\n",
    "\n",
    "    objects = [\"cricket_ball__link\", \"drill__link\", \"cafe_table__link\"]\n",
    "\n",
    "    while self.__pub_planning_scene.get_num_connections() < 1:\n",
    "        rospy.loginfo(\"waiting for someone to subscribe to the /planning_scene\")\n",
    "        rospy.sleep(0.1)\n",
    "\n",
    "    request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)\n",
    "    response = self.__get_planning_scene(request)\n",
    "    acm = response.scene.allowed_collision_matrix\n",
    "\n",
    "    for object_name in objects:\n",
    "        if object_name not in acm.entry_names:\n",
    "            # add object to allowed collision matrix\n",
    "            acm.entry_names += [object_name]\n",
    "            for row in range(len(acm.entry_values)):\n",
    "                acm.entry_values[row].enabled += [False]\n",
    "\n",
    "            new_row = deepcopy(acm.entry_values[0])\n",
    "            acm.entry_values += {new_row}\n",
    "\n",
    "    for index_entry_values, entry_values in enumerate(acm.entry_values):\n",
    "        if \"H1_F\" in acm.entry_names[index_entry_values]:\n",
    "            for index_value, _ in enumerate(entry_values.enabled):\n",
    "                if acm.entry_names[index_value] in objects:\n",
    "                    if enable:\n",
    "                        acm.entry_values[index_entry_values].enabled[index_value] = False\n",
    "                    else:\n",
    "                        acm.entry_values[index_entry_values].enabled[index_value] = True\n",
    "        elif acm.entry_names[index_entry_values] in objects:\n",
    "            for index_value, _ in enumerate(entry_values.enabled):\n",
    "                if \"H1_F\" in acm.entry_names[index_value]:\n",
    "                    if enable:\n",
    "                        acm.entry_values[index_entry_values].enabled[index_value] = False\n",
    "                    else:\n",
    "                        acm.entry_values[index_entry_values].enabled[index_value] = True\n",
    "\n",
    "    planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm)\n",
    "    self.__pub_planning_scene.publish(planning_scene_diff)\n",
    "    rospy.sleep(1.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you call this function with the enable parameter to True, it will check for possible collisions between the fingers of the SHadow Hand and the table or the objects."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "sgs.check_fingers_collisions(True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you call this function with the enable argument to False, it will disable this collision checking."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "sgs.check_fingers_collisions(False)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### pick"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **pick()** function is one of the most important ones, because it contains many of the functions we've seen inside it. By calling to this function, you will start a complete pick action. This is the function you called in the first Demo you did in the Introdution the Course! Here you have the code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def pick(self):\n",
    "\n",
    "    rospy.loginfo(\"Moving to Pregrasp\")\n",
    "    self.open_hand()\n",
    "    time.sleep(0.1)\n",
    "\n",
    "    ball_pose = self.get_object_pose()\n",
    "    ball_pose.position.z += 0.5\n",
    "\n",
    "    #setting an absolute orientation (from the top)\n",
    "    quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)\n",
    "    ball_pose.orientation.x = quaternion[0]\n",
    "    ball_pose.orientation.y = quaternion[1]\n",
    "    ball_pose.orientation.z = quaternion[2]\n",
    "    ball_pose.orientation.w = quaternion[3]\n",
    "\n",
    "    self.move_tip_absolute(ball_pose)\n",
    "    time.sleep(0.1)\n",
    "\n",
    "    rospy.loginfo(\"Grasping\")\n",
    "    self.move_tip(y=-0.164)\n",
    "    time.sleep(0.1)\n",
    "    self.check_fingers_collisions(False)\n",
    "    time.sleep(0.1)\n",
    "    self.close_hand()\n",
    "    time.sleep(0.1)\n",
    "\n",
    "    rospy.loginfo(\"Lifting\")\n",
    "    for _ in range(5):\n",
    "        self.move_tip(y=0.01)\n",
    "        time.sleep(0.1)\n",
    "\n",
    "    self.check_fingers_collisions(True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's analyze part by part what this functions does:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1- First of all, it opens the Shadow Hand."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rospy.loginfo(\"Moving to Pregrasp\")\n",
    "self.open_hand()\n",
    "time.sleep(0.1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "2- Next, it gets the position of the object to grasp, and adds to the z axis 0.5 meters, so that the Shadow Hand will go right above the object."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "ball_pose = self.get_object_pose()\n",
    "ball_pose.position.z += 0.5"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "3- Next, it makes sure that the Shadow Hand will be orientated facing the table. It does this by setting the corresponding orientation value, and the calling the **move_tip_abosulte()** function to apply this orientation to the Shadow Hand."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)\n",
    "ball_pose.orientation.x = quaternion[0]\n",
    "ball_pose.orientation.y = quaternion[1]\n",
    "ball_pose.orientation.z = quaternion[2]\n",
    "ball_pose.orientation.w = quaternion[3]\n",
    "\n",
    "self.move_tip_absolute(ball_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "4- Next, it moves the Shadow Hand a little bit down using the **move_tip** function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self.move_tip(y=-0.164)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "4- It disable the collision checking, because the fingers are going to be in collision with the object to grasp when it closes the hand."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self.check_fingers_collisions(False)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "5- And it closes the hand in order to catch the ball."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self.close_hand()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "6- Finally, it moves the Shadow Hand up again, in order to lift a little bit the ball."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "for _ in range(5):\n",
    "    self.move_tip(y=0.01)\n",
    "    time.sleep(0.1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So... what do you say? Now that you know how this function works, and what are the purposes of each functions, it doesn't look that complicated, right?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's it! In the Next Unit, you will learn how to use Object Recognition in order to get the position of the object you want to grasp, without getting from the simulation data (like it is being donde now)."
   ]
  }
 ],
 "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"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}