Course_ShadowHandInstructionManual_Unit6.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Mastering with ROS: Shadow Hand"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Unit 6: Project"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/closer_camera.png\" width=\"800\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This last unit is meant to practice some of the knowledge that you've gained during this course. You will do this by doing a project, which will be based on the same simulation you used for the previous Chapter. Using this simulation, you will have to use the knowledge you've gained during the Course in order to make the robot Grasp the ball using Object Recognition. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You may also require some extra knowledge that is not given in this Course. For instance, how to create a ROS Service or a Publisher. If you feel so, you can get this knowledge in the **ROS in 5 Days Course**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The Project will be divided in 5 parts, which are described below:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. Read the position of the object."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First of all, you will need to create a Tf listener, which will read the Tf value between the world frame and the frame of the object to grasp. This is exactly what you did at the end of the previous Unit, but now you will have to do it with code."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The basic structure to create a tf_listener is as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "caption": "Pose 1",
    "collapsed": true,
    "image": true,
    "name": "rb1_pose1",
    "width": "9cm"
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "import tf\n",
    "\n",
    "if __name__ == '__main__':\n",
    "   rospy.init_node('tf_listener')\n",
    "\n",
    "   listener = tf.TransformListener()\n",
    "  \n",
    "   rate = rospy.Rate(10.0)\n",
    "\n",
    "   while not rospy.is_shutdown():\n",
    "       try:\n",
    "           (trans,rot) = listener.lookupTransform('/frame1', '/frame2', rospy.Time(0))\n",
    "       except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):\n",
    "           continue\n",
    "            \n",
    "        print trans\n",
    "        print rot\n",
    "\n",
    "       rate.sleep()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Pose 2",
    "image": true,
    "name": "rb1_pose2",
    "width": "8cm"
   },
   "source": [
    "Where **/frame1** is the name of the world frame and **/frame2** is the name of the object to grasp."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. Publish this position into a topic"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you can successfully read the transform between the world frame and your object, you will need to publish this data into a topic. So, you will have to add a Publisher to the tf_listener code, that gets the data from the tf and publishes it into a topic."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The message that publishes this Publisher, should be a **geometry_msgs/Pose**, since this is the type of message that the Smart Grasping System uses in order store the position of the object to grasp. The structure of this message is the following:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "rqt GUI",
    "image": true,
    "name": "rqt",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/PoseMsg.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3. Create a service for reading this data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a service that, when called, it returns the position of the object to grasp in a **geometry_msgs/Pose** message. Once your service is working correctly, create a simple ROS program that calls this service. This is exactly the same that the **SmartGrasper** class is doing in order to get the position of the object from the simulation data."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, it definees the service:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rospy.wait_for_service(\"/gazebo/get_model_state\", 10.0)\n",
    "self.__get_pose_srv = rospy.ServiceProxy(\"/gazebo/get_model_state\", GetModelState)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And here, it executes a call to the service:"
   ]
  },
  {
   "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": [
    "### 4. Integrate your code with the Smart Grasping System"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a copy of the **SmartGrasper** class into your workspace. You can find the code in the **smart_grasping_sandbox** package. Integrate your service definition and function to call this service in the code of the SMartGrasper class."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, in the **pick()** function, substitute the lines where the **get_object_pose()** function is being called for a call to the function you have added."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 5. Test and Correct"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It's very likely that the position you get from the Object Recognition node is not extremely accurate, so you will probably have to adjust some of the values in the Grasping process. For that, I suggest that you use the **move_tip()** function, which will allow you to correct errors in the position of the Shadow Hand."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 6. Congratulations! You completed the Course!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, that's it! You have completed 100% of the course! But... what could you do now? If you want to know what you can do after finishing this course, have a look at the next unit!"
   ]
  }
 ],
 "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": "6 - Course Project",
   "chapter_title": "Unit 6. Course Project",
   "course_title": "ROS MANIPULATION IN 5 DAYS"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}