Course_Manipulation_Unit_3.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ROS Manipulation in 5 Days"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Unit 3: Doing Motion Planning Programatically"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "",
    "image": true,
    "name": "rosject",
    "width": "5cm"
   },
   "source": [
    "<img src=\"img/rosject.png\" width=\"250\" style=\"float:left;\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* ROSject Link: http://bit.ly/2n6gh8t\n",
    "\n",
    "* Package Name: **fetch_manipulation_gazebo**\n",
    "\n",
    "* Launch File: **main.launch**"
   ]
  },
  {
   "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": [
    "In the previous chapter, you saw that you can plan and execute trajectories for your robot using the MoveIt Rviz environment. But... this is not the common case.<br><br>\n",
    "Usually, you will want to move your robot with your own scripts. And this is exactly what we are going to do in this chapter! For this course, we are going to use Python to control the robot, because it's easier and faster."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Anyways, let's go step by step. The 1st thing you will have to do is to create a MoveIt package for the Fetch robot, just as you learned to do in the Previous Chapter. So... let's go!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 3.1**</p>\n",
    "<br>\n",
    "a) Create a MoveIt package for the simulated robot as you saw in the previous chapter. You will have to add 2 Planning Groups to this package. One for the arm, and one for the end-effector.\n",
    "<br><br>\n",
    "<b>Note:</b> You will find the URDF file needed in the <b>fetch_description</b> package, and it is named <b>fetch.urdf</b>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can create as many poses as you want, but these 2 are required:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<b>Name: Start</b>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Fetch Pose 1",
    "image": true,
    "name": "fetch_start",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/fetch_start.png\" width=\"500\" style=\"float:left\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<b>Name: home</b>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Fetch Pose 2",
    "image": true,
    "name": "fetch_home",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/fetch_home.png\" width=\"500\" style=\"float:left\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Connect this MoveIt package to the simulated robot, just as you learned to do in the previous chapter."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Test that you can plan and execute trajectories using MoveIt, and that these trajectories apply to the simulated robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#3B8F10;color:white;\">**Data for Exercise 3.1**</p>\n",
    "<br>\n",
    "Check the following notes in order to complete the Exercise:\n",
    "<br><br>\n",
    "**Note 1**: It is possible that you could get confused when you try to add the Gripper to the **controllers.yaml** file. If you have it straight, go ahead and add it. If you don't, you can wait until Chapter 5 to add it, where it will be explained. It is not mandatory to add the Gripper to the controllers.yaml file now. \n",
    "<br><br>\n",
    "It is mandatory, though, to add it into the Planning Groups and the End-Effector sections when you create the Moveit package. You already know how to do that!\n",
    "<br><br>\n",
    "**Note 2**: In this case, it is not required to create a Virtual Joint, so you can leave this section empty."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#AE0202;color:white;\">**Expected Result for Exercise 3.1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Fetch in MoveIt",
    "image": true,
    "name": "fetch_moveit",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/fetch_moveit.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from \n",
    "\n",
    "HTML('<iframe width=\"560\" height=\"315\" src=\"https://www.youtube.com/embed/rfcXZcKZd8A?rel=0&amp;controls=0&amp;showinfo=0\" frameborder=\"0\" allowfullscreen></iframe>')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<iframe width=\"560\" height=\"315\" src=\"https://www.youtube.com/embed/rfcXZcKZd8A\" frameborder=\"0\" allow=\"autoplay; encrypted-media\" allowfullscreen></iframe>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 3.1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**Solution for Exercise 3.1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Please try to do it by yourself unless you get stuck or need some inspiration. You will learn much more if you fight for each exercise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/robotignite_logo_text.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you don't succeed in completing the Exercise, you can have a look at the this <a href=\"https://www.youtube.com/watch?v=rfcXZcKZd8A\" target=\"_blank\">**Video**</a>. You'll find here an step by step explanation about how to have it working."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END Solution for Exercise 3.1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! So now that you have created the MoveIt package, you're ready to begin with the main goal of this chapter! Well, almost ready... "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Before actually starting with the contents of this chapter, let's execute the following command in order to raise the Fetch robot's torso. This will make it easier in order to plan and execute trajectories with the arm of the robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<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_gazebo_demo move_torso.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see how Fetch's torso rises. Something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Fetch Raising Torso",
    "image": true,
    "name": "fetch_torso",
    "width": "6cm"
   },
   "source": [
    "<img src=\"img/fetch_torso.png\" width=\"300\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:red;color:white;\">**IMPORTANT NOTE**</p>\n",
    "<br>\n",
    "Once the robot is set up, like in the image above, you must stop the command launched by pressing <b>Ctrl+C</b> in the webshell in which it was launched."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:red;color:white;\">**IMPORTANT NOTE**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Planning a trajectory"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you've seen in the previous chapter, there is a difference between planning a trajectory, and executing it. In this first part of the chapter, we are going to see how to plan a trajectory with Python. For that, just follow the next Demo."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Demo 3.1**</p>\n",
    "<br>\n",
    "a) First of all, you'll need to launch the MoveIt RViz environment. Type the following command:\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 fetch_planning_execution.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">**NOTE 1**: Note that this command may vary depending on how you've named your MoveIt package and your launch file. In this example command, it is assuming that they're named **fetch_moveit_config** and **fetch_planning_execution.launch**, respectively.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">**NOTE 2**: If the window appears out of focus, just follow the steps described in the previous chapter.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Execute the following Python code. You can do this by clicking on the cell and pressing Ctr+Enter, or by clicking on the \"Play\" icon at the top-right corner of this notebook. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import sys\n",
    "import copy\n",
    "import rospy\n",
    "import moveit_commander\n",
    "import moveit_msgs.msg\n",
    "import geometry_msgs.msg\n",
    "\n",
    "moveit_commander.roscpp_initialize(sys.argv)\n",
    "rospy.init_node('move_group_python_interface_tutorial', anonymous=True)\n",
    "\n",
    "robot = moveit_commander.RobotCommander()\n",
    "scene = moveit_commander.PlanningSceneInterface()    \n",
    "group = moveit_commander.MoveGroupCommander(\"arm\")\n",
    "display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)\n",
    "\n",
    "pose_target = geometry_msgs.msg.Pose()\n",
    "pose_target.orientation.w = 1.0\n",
    "pose_target.position.x = 0.96\n",
    "pose_target.position.y = 0\n",
    "pose_target.position.z = 1.18\n",
    "group.set_pose_target(pose_target)\n",
    "\n",
    "plan1 = group.plan()\n",
    "\n",
    "rospy.sleep(5)\n",
    "\n",
    "moveit_commander.roscpp_shutdown()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) After a few seconds, you'll see in MoveIt RViz how the robot is planning the specified motion described in the code above."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#AE0202;color:white;\">**Expected Result for Demo 3.1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Motion Planning Fetch",
    "image": true,
    "name": "fetch_p1",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/fetch_p1.gif\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Demo 3.1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:red;color:white;\">**IMPORTANT NOTE**</p>\n",
    "<br>\n",
    "Once you have finished with the Demo, remember to RESTART the Kernel by hitting the 'RESTART KERNEL' icon at the top-right corner of the notebook."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:red;color:white;\">**IMPORTANT NOTE**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "That's great! But... how does this code work? What does each part mean? Let's break it down into smaller pieces."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import sys\n",
    "import copy\n",
    "import rospy\n",
    "import moveit_commander\n",
    "import moveit_msgs.msg\n",
    "import geometry_msgs.msg"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this section of the code, we are just importing some modules and messages that we'll need for the program. The most important one here is the moveit_commander module, which will allow us to communicate with the MoveIt RViz interface."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "moveit_commander.roscpp_initialize(sys.argv)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, we are just initializing the moveit_commander module."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rospy.init_node('move_group_python_interface_tutorial', anonymous=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, we are just initializing a ROS node."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "robot = moveit_commander.RobotCommander()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, we are creating a RobotCommander object, which is, basically, an interface to our robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "scene = moveit_commander.PlanningSceneInterface()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, we are creating a PlanningSceneInterface object, which is, basically, an interface to the world that surrounds the robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "group = moveit_commander.MoveGroupCommander(\"arm\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, we create a MoveGroupCommander object, which is an interface to the <i><b>manipulator</b></i> group of joints that we defined when we created the MoveIt package, back in the Chapter 1. This will allow us to interact with this set of joints, which, in this case, is the full arm."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, we are defining a Topic Publisher, which will publish into the <i><b>/move_group/display_planned_path<b></i> topic. By publishing into this topic, we will be able to visualize the planned motion through the MoveIt RViz interface."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "pose_target = geometry_msgs.msg.Pose()\n",
    "pose_target.orientation.w = 1.0\n",
    "pose_target.position.x = 0.7\n",
    "pose_target.position.y = -0.05\n",
    "pose_target.position.z = 1.1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, we are creating a Pose object, which is the type of message that we will send as a goal. Then, we just give values to the variables that will define the goal Pose. You can have a look at the full structure of the Pose message by issuing the following command on a WebShell: <i><b>rosmsg show geometry_msgs/Pose</b></i>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Pose Message",
    "image": true,
    "name": "pose_msg",
    "width": "7cm"
   },
   "source": [
    "<img src=\"img/pose_msg.png\" width=\"300\" />"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "plan1 = group.plan()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we are telling the \"manipulator\" group that we created previously to calculate the plan. If the plan is successfully computed, it will be displayed through MoveIt RViz."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "moveit_commander.roscpp_shutdown()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At the end, we just shut down the moveit_commander module."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 3.1**</p>\n",
    "<br>\n",
    "a) First of all, you'll need to launch the MoveIt RViz environment. Type the following command:\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 fetch_planning_execution.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">**NOTE 1**: Note that this command may vary depending on how you've named your MoveIt package and your launch file. In this example command, it is assuming that they're named **fetch_moveit_config** and **fetch_planning_execution.launch**, respectively.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">**NOTE 2**: If the window appears out of focus, just follow the steps described in the previous chapter.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Create a new package called <i><b>my_motion_scripts</b></i>. Inside this package, create a new directory called <i><b>src</b></i>, with a file named <i><b>planning_script.py</b></i>. Finally, copy the code you've just executed in the Demo above inside this file."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Inside the package, also create a launch directory that contains a launch file in order to launch the planning_script.py file."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Modify the values assigned to the <i><b>pose_target</b></i> variable. Then, launch your code and check if the new Pose was achieved successfully. Repeat the process and try it with different Poses."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#3B8F10;color:white;\">**Data for Exercise 3.1**</p>\n",
    "<br>\n",
    "Check the following notes in order to complete the exercise:\n",
    "<br><br>\n",
    "<span style=\"color:orange\">Note 1: </span>When you launch your Python script with your code, you may see the following error:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Python Error",
    "image": true,
    "name": "pyerror",
    "width": "15cm"
   },
   "source": [
    "<img src=\"img/pyerror.png\" width=\"1000\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is a known issue, and you don't need to worry about it. 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."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 3.1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Planning to a joint space goal"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Sometimes, instead of just moving the end-effector towards a goal, we can be interested in setting a goal for a specific joint. Let's see how you can do this."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Demo 3.2**</p>\n",
    "<br>\n",
    "a) First of all, you'll need to launch the MoveIt RViz environment. Type the following command:\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 fetch_planning_execution.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\"><b>NOTE 1:</b> Note that this command may vary depending on how you've named your MoveIt package and your launch file. In this example command, it is assuming that they're named <b>fetch_moveit_config</b> and <b>fetch_planning_execution.launch</b>, respectively.**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\"><b>NOTE 2:</b> If the window appears out of focus, just follow the steps described in the previous chapter.**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Execute the following Python code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import sys\n",
    "import copy\n",
    "import rospy\n",
    "import moveit_commander\n",
    "import moveit_msgs.msg\n",
    "import geometry_msgs.msg\n",
    "\n",
    "moveit_commander.roscpp_initialize(sys.argv)\n",
    "rospy.init_node('move_group_python_interface_tutorial', anonymous=True)\n",
    "\n",
    "robot = moveit_commander.RobotCommander()\n",
    "scene = moveit_commander.PlanningSceneInterface()    \n",
    "group = moveit_commander.MoveGroupCommander(\"arm\")\n",
    "display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)\n",
    "\n",
    "group_variable_values = group.get_current_joint_values()\n",
    "\n",
    "group_variable_values[5] = -1.5\n",
    "group.set_joint_value_target(group_variable_values)\n",
    "\n",
    "plan2 = group.plan()\n",
    "\n",
    "rospy.sleep(5)\n",
    "\n",
    "moveit_commander.roscpp_shutdown()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) When the code finishes executing, you'll see in MoveIt RViz how the robot is planning the specified motion described in the code above."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#AE0202;color:white;\">**Expected Result for Demo 3.2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Motion Planning Fetch 2",
    "image": true,
    "name": "fetch_p2",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/fetch_p2.gif\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Demo 3.2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:red;color:white;\">**IMPORTANT NOTE**</p>\n",
    "<br>\n",
    "Once you have finished with the Demo, remember to RESTART the Kernel by hitting the 'RESTART KERNEL' icon at the top-right corner of the notebook."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:red;color:white;\">**IMPORTANT NOTE**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "That's great, right? But as we did before, let's analyze the new code we've introduced in order to understand what's going on."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "group.clear_pose_targets()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, we are just clearing the actual values of the pose_target variable."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "group_variable_values = group.get_current_joint_values()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we are getting the current values of the joints."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "group_variable_values[3] = 1.5\n",
    "group.set_joint_value_target(group_variable_values)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, we modify the value of one of the joints, and set this new joint value as a target."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "plan2 = group.plan()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we just compute the plan for the new joint space goal."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 3.2**</p>\n",
    "<br>\n",
    "\n",
    "a) Inside the <i><b>my_motion_scripts</b></i> package, create a new file named <i><b>joint_planning.py</b></i>. Do some tests giving different values to different joints."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 3.2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Getting some useful data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Through code, you can also get some valuable data that you may require for your code. Let's see some examples."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can get the reference frame for a certain group by executing this line:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print \"Reference frame: %s\" % group.get_planning_frame()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can get the end-effector link for a certain group by executing this line:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print \"End effector: %s\" % group.get_end_effector_link()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can get a list with all of the groups of the robot, like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print \"Robot Groups:\"\n",
    "print robot.get_group_names()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can get the current values of the joints, like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print \"Current Joint Values:\"\n",
    "print group.get_current_joint_values()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can also get the current Pose of the end-effector of the robot, like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print \"Current Pose:\"\n",
    "print group.get_current_pose()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, you can check the general status of the robot, like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print \"Robot State:\"\n",
    "print robot.get_current_state()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 3.3**</p>\n",
    "<br>\n",
    "a) First of all, you'll need to launch the MoveIt RViz environment. Type the following command:\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 fetch_planning_execution.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">**NOTE 1**: Note that this command may vary depending on how you've named your MoveIt package and your launch file. In this example command, it is assuming that they're named **fetch_moveit_config** and **fetch_planning_execution.launch**, respectively.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">**NOTE 2**: If the window appears out of focus, just follow the steps described in the previous chapter.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Create a new file inside the package called <i><b>get_data.py</b></i>. Add all the new code you've learnt just above and check what results you get."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 3.3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Executing a trajectory"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, at this point, you've seen some methods that allow you to plan a trajectory with Python code. But...what about executing this trajectory with the \"real\" robot? \n",
    "<br><br>\n",
    "In fact, it's very simple. In order to execute a trajectory, you just need to call the <i><b>go()</b></i> function from the planning group. Like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "group.go(wait=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "By executing this line of code, you will be telling your robot to execute the last trajectory that has been set for the Planning Group. Let's try this in an exercise!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 3.4**</p>\n",
    "<br>\n",
    "a) First of all, you'll need to launch the MoveIt RViz environment. Type the following command:\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 fetch_planning_execution.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">**NOTE 1**: Note that this command may vary depending on how you've named your MoveIt package and your launch file. In this example command, it is assuming that they're named **fetch_moveit_config** and **fetch_planning_execution.launch**, respectively.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">**NOTE 2**: If the window appears out of focus, just follow the steps described in the previous chapter.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Create a new Python script called <i><b>execute_trajectory.py</b></i>, and copy the code shown in Demo 3.1 inside of it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Add a line into your new script in order to execute that trajectory."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) You can try this with any of the codes you have created for planning trajectories."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#AE0202;color:white;\">**Expected Result for Demo 3.2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Motion Execution Fetch",
    "image": true,
    "name": "fetch_e1",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/fetch_e1.gif\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 3.4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 3.5**</p>\n",
    "<br>\n",
    "a) First of all, you'll need to launch the MoveIt RViz environment. Type the following command:\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 fetch_planning_execution.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">**NOTE 1**: Note that this command may vary depending on how you've named your MoveIt package and your launch file. In this example command, it is assuming that they're named **fetch_moveit_config** and **fetch_planning_execution.launch**, respectively.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">**NOTE 2**: If the window appears out of focus, just follow the steps described in the previous chapter.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Modify the <i><b>execute_trajectory.py</b></i> script, so that it now concatenates at least 2 motions."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 3.5**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:red;color:white;\">**IMPORTANT NOTE**</p>\n",
    "<br>\n",
    "When you have finished this chapter, make sure to return the robot to the <b>home</b> position."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Fetch in Home Position",
    "image": true,
    "name": "home_pos",
    "width": "7cm"
   },
   "source": [
    "<img src=\"img/home_pos.png\" width=\"300\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:red;color:white;\">**IMPORTANT NOTE**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's it! You have finished this chapter! I really hope that you enjoyed it and, most of all, have learned a lot! Now, if you want to learn how you can add Perception to Motion Planning tasks, just go to the next chapter!"
   ]
  }
 ],
 "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": "3 - Doing Motion Planning programatically",
   "chapter_title": "Unit 3. Doing Motion Planning programatically",
   "course_title": "ROS MANIPULATION IN 5 DAYS"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}