Course_Manipulation_Unit_2.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ROS Manipulation in 5 Days"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Unit 2: Motion Planning using Graphical Interfaces"
   ]
  },
  {
   "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 create a Moveit Package for your industrial robot. By completing this unit, you will be able to create a package that allows your robot to perform motion planning."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END OF SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## What is MoveIt?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "MoveIt is a ROS framework that allows you to perform motion planning with an specific robot. And... what does this mean? Well, it bascially means that it allows you to plan a movement(motion) from a point A to a point B, without colliding with anything."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "MoveIt is a very complex and useful tool. So, within this MicroCourse, we are not going to dive into the details of how MoveIt works, or all the features it provides. If you are intereseted in learning more about MoveIt, you can have a look ath the official website here: http://moveit.ros.org/"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Fortunately, MoveIt provides a very nice and easy-to-use GUI, which will help us to be able to interact with the robot in order to perform motion planning. However, before being able to actually use MoveIt, we need to build a package. This package will generate all the configuration and launch files required for using our defined robot (the one that is defined in the URDF file) with MoveIt. \n",
    "<br><br>\n",
    "In order to generate this package, just follow all the steps described in the following exercise!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Generating MoveIt! configuration package using Setup Assistant tool"
   ]
  },
  {
   "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 Setup Assistant. You can do that by typing 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><br>\n",
    "roslaunch moveit_setup_assistant setup_assistant.launch<br>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "MoveIt Setup Assistant",
    "image": true,
    "name": "setup_assistant",
    "width": "9cm"
   },
   "source": [
    "Now, if you open the Graphic Tools by hitting this icon \n",
    "<img src=\"img/font-awesome_desktop.png\"width=\"25\"/>\n",
    "you will see something like this:\n",
    "<img src=\"img/setup_assistant.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! You now are at the MoveIt Setup Assistant. The next thing you'll need to is to load your robot file. So let's continue!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Create New Package",
    "image": true,
    "name": "load_xacro",
    "width": "9cm"
   },
   "source": [
    "b) Click on the \"Create New MoveIt Configuration Package\" button. A new section like this will appear:<br>\n",
    "<img src=\"img/load_xacro.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Load URDF",
    "image": true,
    "name": "shadow_rviz1",
    "width": "9cm"
   },
   "source": [
    "Now, just click the \"Browse\" button, select the URDF file named **model.urdf** located in the **shadow_tc** package, and click on the \"Load Files\" button. \n",
    "You will probably need to copy this file into your workspace.\n",
    "You should now see something like this:<br>\n",
    "<img src=\"img/shadow_moveit1.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! So now, you've loaded the xacro file of your robot to the MoveIt Setup Assistant. Now, let's start configuring some things."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Self-Collisions",
    "image": true,
    "name": "shadow_rviz2",
    "width": "9cm"
   },
   "source": [
    "c) Go to the \"Self-Collisions\" tab, and click on the \"Regenerate Default Collision Matrix\" button. You will end with something like this:<br>\n",
    "<img src=\"img/shadow_moveit2.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you are just defining some pairs of links that don't need to be considered when performing collision checking. For instance, because they are adjacent links, so they will always be in collision."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Next, move to the \"Virtual Joints\" tab. Here, you will define a virtual joint for the base of the robot. Click the \"Add Virtual Joint\" button, and set the name of this joint to <i><b>FixedBase</b></i>, and the parent to <i><b>world</b></i>. Just like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Virtual Joints",
    "image": true,
    "name": "shadow_rviz3",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/shadow_moveit3.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, click the \"Save\" button. Basically, what you are doing here is to create an \"imaginary\" joint that will connect the base of your robot with the simulated world."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "e) Now, open the \"Planning Groups\" tab and click the \"Add Group\" button. Now, you will create a new group called <i><b>arm</b></i>, which will use the <i><b>KDLKinematicsPlugin</b></i>. Just like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Planning Group",
    "image": true,
    "name": "arm0",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/arm0.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, you will click on the \"Add Joints\" button, and you will select all the <i><b>joints</b></i> that form the arm of the robot, excluding the gripper. Just like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Add Joints",
    "image": true,
    "name": "add_joints",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/add_joints.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, click the \"Save\" button and you will end up with something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Planning Group 2",
    "image": true,
    "name": "arm",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/arm.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So now, you've defined a group of links for performing Motion Planning with, and you've defined the plugin you want to use to calculate those Plans."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, repeat the same process, but this time for the gripper. In this case, you <b>DO NOT have to define any Kinematics Solver</b>. If you are not sure of what joints to add to the hand, you can have a look at the below image."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Gripper Joints",
    "image": true,
    "name": "gripper_joints",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/shadow_moveit4.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At the end, you should end up with something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Planning Groups 3",
    "image": true,
    "name": "shadow_rviz5",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/shadow_moveit5.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "f) Now, you are going to create a couple of predefined poses for your robot. Go to the \"Robot Poses\" tab and click on the \"Add Pose\" button. In the left side of the screen, you will be able to define the name of the pose and the planning group it's refered to. In this case, we will name the 1st Pose <i><b>open</b></i>, and it will be related, obviously, to the <i><b>hand</b></i> Group."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Hand Pose",
    "image": true,
    "name": "pose_hand",
    "width": "7cm"
   },
   "source": [
    "<img src=\"img/pose_hand.png\" width=\"300\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, you will have to define the positions of the joints that will be related to this Pose. For this case, you can set them as in the image below:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Hand Pose 1",
    "image": true,
    "name": "poses1",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/shadow_moveit6.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, repeat the operation, but this time we will define the <i><b>close</b></i> Pose. For instance, it could be something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Hand Pose 2",
    "image": true,
    "name": "poses2",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/shadow_moveit7.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, let's create an <i><b>start</b></i> Pose for the <i><b>arm</b></i> Group. It could be something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Arm Pose",
    "image": true,
    "name": "poses3",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/shadow_moveit8.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At the end, you should have something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Final Robot Poses",
    "image": true,
    "name": "poses4",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/shadow_moveit9.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "g) The next step will be to set up the End-Effector of the robot. For that, just go the <b>End Effectors</b> tab, and click on the \"Add End Effector\" button. We will name our End Effector <i><b>hand</b></i>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "End-Effectors",
    "image": true,
    "name": "end_eff",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/end_eff.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "h) Now, just enter your name and e-mail at the \"Author Information\" tab."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "i) Finally, go to the \"Configuration Files\" tab and click the \"Browse\" button. Navigate to the <i><b>catkin_ws/src</b></i> directory, create a new directory, and name it <i><b>myrobot_moveit_config</b></i>. \"Choose\" the directory you've just created."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Generate Package",
    "image": true,
    "name": "conf_pkg",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/conf_pkg.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    " Now, click the \"Generate Package\" button. If everyting goes well, you should see something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Package Generated",
    "image": true,
    "name": "conf_pkg2",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/shadow_moveit10.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's it! You have just created a MoveIt package for your articulated robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 3.1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;\">\n",
    "<tr>\n",
    "<th>\n",
    "<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>If, for any reason, you need to <b>edit</b> your MoveIt package (for instances, in futures exercises you detect that you did an error), you can do that by selecting the <b>Edit Existing Moveit Configuration Package</b> option in the Setup Assistant, and then selecting your package. \n",
    "<br>\n",
    "<img src=\"img/setup_assistant.png\" width=\"500\" />\n",
    "<br>\n",
    "<span style=\"color:orange\">Note 2: </span>If you modify your MoveIt package, you will need to restart the simulation in order to make this changes to have effect. You can do that by moving to the Next Chapter (Chapter 3), and then going back to this one (Chapter 2).\n",
    "<br>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's it! You've created your MoveIt package for your robot. But... now what?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now that you've already created a MoveIt package, and you've worked a little bit with it, let's take a deeper look at some key aspects of Moveit."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's start with a quick look at MoveIt architecture. Understanding the architecture of MoveIt! helps to program and interface the robot to MoveIt. Here, you can have a look at a diagram showing MoveIt architecture."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/moveit_architecture.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### The move_group node"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can say that **move group** is the heart of MoveIt, as this node acts as an integrator of the various components of the robot and delivers actions/services according to the user's needs."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **move group** node collects robot information, such as the PointCloud, the joint state of the robot, and the transforms (TFs) of the robot in the form of topics and services."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "From the parameter server, it collects the robot kinematics data, such as robot description (URDF), SRDF (Semantic Robot Description Format), and the configuration files. The SRDF file and the configuration files are generated while\n",
    "we generate a MoveIt! package for our robot. The configuration files contain the parameter file for setting joint limits, perception, kinematics, end effector, and so on. These are the files that have been created in the **config** folder of your package."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When MoveIt! gets all of this information about the robot and its configuration, we can say it is properly configured and we can start commanding the robot from the user interfaces. We can either use C++ or Python MoveIt! APIs to command the move group node to perform actions, such as pick/place, IK, or FK, among others. Using the RViz motion planning plugin, we can command the robot from the RViz GUI itself. And this is what you are going to do in the next section!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Basic Motion Planning"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Well... to start, you can just launch the MoveIt Rviz environment and begin to do some tests about Motion Planning. So, follow the next exercise in order to do so!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 3.2</p>\n",
    "<br>\n",
    "a) Execute the following command in order to start the MoveIt RViz demo environment.\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_moveit_config demo.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\"><b>NOTE:</b> It may happen that the Moveit Rviz window appears out of focus. Like this:</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Bad Focus",
    "image": true,
    "name": "focus_bad",
    "width": "7cm"
   },
   "source": [
    "<img src=\"img/focus_bad.png\" width=\"300\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">If this is your case, just type the following command into the Web Shell:</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "wmctrl -r :ACTIVE: -e 0,65,24,1500,550"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">Now, your MoveIt Rviz window should appear like this:</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Good Focus",
    "image": true,
    "name": "focus_good",
    "width": "7cm"
   },
   "source": [
    "<img src=\"img/focus_good.png\" width=\"300\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\">Now, you can just double-click on the top coloured part of the window in order to maximize.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If everything goes OK, you will see something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "MoveIt RViz with UR5",
    "image": true,
    "name": "shadow_rviz0",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/shadow_moveit11.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Now, move to the Planning tab. Here:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Planning Tab",
    "image": true,
    "name": "shadow_rviz9",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/shadow_moveit12.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Before start Planning anything, it is always a good practice to update the current Start State."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Query",
    "image": true,
    "name": "start_position",
    "width": "6cm"
   },
   "source": [
    "<img src=\"img/start_position.png\" width=\"250\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) At the query section, in the Goal State, you can choose the <i><b>start</b></i> option (which one of the Poses you defined in the Previous Exercise) and click on the \"Update\" button. Your robot scene will be updated with the new position that has been selected."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Updated Scene",
    "image": true,
    "name": "update_scene",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/shadow_moveit13.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "e) Now, you can click on the \"Plan\" button at the \"Commands\" section. The robot will begin to plan a trajectory to reach that point."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Robot Planning",
    "image": true,
    "name": "plan_traj",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/shadow_moveit14.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "f) Finally, if you click on the \"Execute\" button, the robot will execute that trajectory."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "g) Now just play with the new tool! You can repeat this same process some more times. For instance, instead of moving the robot to the start position, you could set a random valid position as goal. You can also try to check and uncheck the different visualization options that appear in the upper \"Displays\" section."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 3.2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You've now seen how to perform some basic Motion Planning through the MoveIt RViz GUI, and you're a little more familiar with MoveIt. So... let's discuss some interesting points!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### MoveIt! planning scene"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The term \"planning scene\" is used to represent the world around the robot and also store the state of the robot itself. The planning scene monitor inside of move_group maintains the planning scene representation. The move_group node consists\n",
    "of another section called the world geometry monitor, which builds the world geometry from the sensors of the robot and from the user input."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The planning scene monitor reads the joint_states topic from the robot, and the sensor information and world geometry from the world geometry monitor. The world scene monitor reads from the occupancy map monitor, which uses 3D perception to build a 3D representation of the environment, called Octomap. The Octomap can be generated from PointClouds, which are handled by a PointCloud occupancy map update plugin and depth images handled by a depth image occupancy map updater. You will see this part in the next chapter, when we introduce Perception."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### MoveIt! kinematics handling"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "MoveIt! provides a great flexibility for switching the inverse kinematics algorithms using the robot plugins. Users can write their own IK solver as a MoveIt! plugin and switch from the default solver plugin whenever required. The default IK solver in MoveIt! is a numerical jacobian-based solver."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Compared to the analytic solvers, the numerical solver can take time to solve IK. The package called IKFast can be used to generate a C++ code for solving IK using analytical methods, which can be used for different kinds of robot manipulators and perform better if the DOF is less than 6. This C++ code can also be converted into the MoveIt! plugin by using some ROS tools."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Forward kinematics and finding jacobians are already integrated into the MoveIt! RobotState class, so we don't need to use plugins for solving FK."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### MoveIt! collision checking"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The CollisionWorld object inside MoveIt! is used to find collisions inside a planning scene, which are using the FCL (Flexible Collision Library) package as a backend. MoveIt! supports collision checking for different types of objects, such as meshes; primitive shapes, such as boxes, cylinders, cones, spheres, and such; and Octomap."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The collision checking is one of the computationally expensive tasks during motion planning. To reduce this computation, MoveIt! provides a matrix called ACM (Allowed Collision Matrix). It contains a binary value corresponding to the need to check for collision between two pairs of bodies. If the value of the matrix is 1, it means that the collision of the corresponding pair is not needed. We can set the value as 1 where the bodies are always so far that they would never collide with each other. Optimizing ACM can reduce the total computation needed for collision avoidance. This was done when you were creating the package, if you remember!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Moving the real robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Until now, though, you've only moved the robot in the Moveit application. This is very useful because you can do many tests without worrying about any damage. Anyways, the final goal will be always to move the real robot, right?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The MoveIt package you've created is able to provide the necessary ROS services and actions in order to plan and execute trajectories, but it isn't able to pass this trajectories to the real robot. All the Kinematics you've been performing were executed in an internal simulator that MoveIt provides. In order to communicate with the real robot, it will be necessary to do a couple of modifications to the MoveIt package you've created at the beginning of the Chapter. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Obviously, in this Course you don't have a real robot to do this, so you will apply the same but for moving the simulated robot. In order to see what you need to change in your MoveIt package, just follow the next Exercise."
   ]
  },
  {
   "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 create a file to define how you will control the joints of your \"real\" robot. Inside the <i><b>config</b></i> folder of your moveit package, create a new file named <i><b>controllers.yaml</b></i>. Copy the following content inside it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "controller_list:\n",
    "  - name: arm_controller\n",
    "    action_ns: follow_joint_trajectory\n",
    "    type: FollowJointTrajectory\n",
    "    joints:\n",
    "      - shoulder_pan_joint\n",
    "      - shoulder_lift_joint\n",
    "      - elbow_joint\n",
    "      - wrist_1_joint\n",
    "      - wrist_2_joint\n",
    "      - wrist_3_joint\n",
    "  - name: hand_controller\n",
    "    action_ns: follow_joint_trajectory\n",
    "    type: FollowJointTrajectory\n",
    "    joints:\n",
    "      - H1_F1J1\n",
    "      - H1_F1J2\n",
    "      - H1_F1J3\n",
    "      - H1_F2J1\n",
    "      - H1_F2J2\n",
    "      - H1_F2J3\n",
    "      - H1_F3J1\n",
    "      - H1_F3J2\n",
    "      - H1_F3J3"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So basically, here you are defining the Action Servers that you will use for controlling the joints of your robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, you are setting the name of the joint trajectory controller Action Server for controlling the arm of the robot. And how do you know that? Well, if you do a <i>rostopic list</i> in any Web Shell, you'll find between your topics the following structure:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "arm controller Action Server",
    "image": true,
    "name": "arm_as",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/arm_as.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So this way, you can know that your robot has a joint trajectory controller Action Server that is called <i><b>/arm_controller/follow_joint_trajectory/</b></i>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Also, you can find out by checking the message that this Action uses, that it is of the type <i><b>FollowJointTrajectory</b></i>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, you already know the names of the joints that your robot uses. You saw them while you were creating the MoveIt package, and you can also find them in the <i><b>model.urdf</b></i> file, at the <i><b>fh_desc</b></i> package."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, it simply repeats the process described just now, but for the <i><b>/hand_controller/follow_joint/trajectory</b></i> action server."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "hand controller Action Server",
    "image": true,
    "name": "hand_as",
    "width": "9cm"
   },
   "source": [
    "<img src=\"img/hand_as.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Next, you'll have to create a file to define the names of the joints of your robot. Again inside the <i><b>config</b></i> directory, create a new file called <i><b>joint_names.yaml</b></i>, and copy the following content in it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "controller_joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint, H1_F1J1, H1_F1J2, H1_F1J3, H1_F2J1, H1_F2J2, H1_F2J3, H1_F3J1, H1_F3J2, H1_F3J3]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Now, if you open the <i><b>smart_grasping_sandbox_moveit_controller_manager.launch.xml</b></i>, which is inside the <i><b>launch</b></i> directory, you'll see that it's empty. Put the next content inside it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "  <rosparam file=\"$(find myrobot_moveit_config)/config/controllers.yaml\"/>\n",
    "  <param name=\"use_controller_manager\" value=\"false\"/>\n",
    "  <param name=\"trajectory_execution/execution_duration_monitoring\" value=\"false\"/>\n",
    "  <param name=\"moveit_controller_manager\" value=\"moveit_simple_controller_manager/MoveItSimpleControllerManager\"/>\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What you are doing here is basically load the <i>controllers.yaml</i> file you just created, and the <i>MoveItSimpleControllerManager</i> plugin, which will allow you to send the plans calculated in MoveIt to your \"real\" robot, in this case, the simulated robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Finally, you will have to create a new launch file that sets up all the system to control your robot. So, inside the <i><b>launch</b></i> directory, create a new launch file called <i><b>myrobot_planning_execution.launch</b></i>."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "\n",
    "  <rosparam command=\"load\" file=\"$(find myrobot_moveit_config)/config/joint_names.yaml\"/>\n",
    "\n",
    "  <include file=\"$(find myrobot_moveit_config)/launch/planning_context.launch\" >\n",
    "    <arg name=\"load_robot_description\" value=\"true\" />\n",
    "  </include>\n",
    "\n",
    "  <node name=\"joint_state_publisher\" pkg=\"joint_state_publisher\" type=\"joint_state_publisher\">\n",
    "    <param name=\"/use_gui\" value=\"false\"/>\n",
    "    <rosparam param=\"/source_list\">[/joint_states]</rosparam>\n",
    "  </node>\n",
    "\n",
    "  <include file=\"$(find myrobot_moveit_config)/launch/move_group.launch\">\n",
    "    <arg name=\"publish_monitored_planning_scene\" value=\"true\" />\n",
    "  </include>\n",
    "\n",
    "  <include file=\"$(find myrobot_moveit_config)/launch/moveit_rviz.launch\">\n",
    "    <arg name=\"config\" value=\"true\"/>\n",
    "  </include>\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So finally here, we are loading the <i>joint_names.yaml</i> file, and launching some launch files we need in order to set up the MoveIt environment. You can check what those launch file do, if you want. But let's focus a moment on the <i>joint_state_publisher</i> node that is being launched."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you do again a <i>rostopic list</i>, you will see that there is a topic called <i><b>/joint_states</b></i>. Into this topic is where the state of the joints of the simulated robot are published. So, we need to put this topic into the <i><b>/source_list</b></i> parameter, so MoveIt can know where the robot is at each moment."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "f) Finally, you just have to launch this launch file you just created (<b>myrobot_planning_execution.launch</b>) and Plan a trajectory, just as you learnt to do in the previous exercise. Once the trajectory is planned, you can press the \"Execute\" button in order to execute the trajectory in the simulated robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/shadow_execute_gif.gif\" width=\"700\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 3.3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Summarizing"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Assume that we know the starting pose of the robot, a desired goal pose of the robot, the geometrical description of the robot, and geometrical description of the world. Then, motion planning is the technique to find an optimum path that moves the robot gradually from the start pose to the goal pose, while never touching any obstacles in the world, and without colliding with the robot links.\n",
    "\n",
    "Here the geometrical description of the robot is our URDF file and the geometrical description of the world can also be included in URDF, and using the laser scanner/3D vision sensor, we can generate the world in 3D, which can help to avoid dynamic obstacles, rather than static objects defined using URDF.\n",
    "\n",
    "In the case of the robotic arm, the motion planner should find a trajectory (consisting of joint spaces of each joint) in which the links of the robot should never collide with the environment, avoid self-collision (collision between two robot links), and also not violate the joint limits.\n",
    "\n",
    "MoveIt! can talk to the motion planners through the plugin interface. We can use any motion planner by simply changing the plugin. This method is highly extensible, so we can try our own custom motion planners using this interface. The move_group node talks to the motion planner plugin via the ROS action/services. The default planner for the move_group node is OMPL (http://ompl.kavrakilab.org/).\n",
    "\n",
    "To start motion planning, we should send a motion planning request to the motion planner, which specifies our planning requirements. The planning requirement may be setting a new goal pose of the end effector; for example, for a pick and place operation.\n",
    "\n",
    "We can set additional kinematic constraints for the motion planners. Given next are some inbuilt constraints in MoveIt!:\n",
    "\n",
    "* Position constraints: These restrict the position of a link\n",
    "* Orientation constraints: These restrict the orientation of a link\n",
    "* Visibility constraints: These restrict a point on the link to be visible in a particular area (view of a sensor)\n",
    "* Joint constraints: These restrict a joint within its joint limits\n",
    "* User-specified constraints: With these constraints, the user can define his own constraints using the callback functions\n",
    "\n",
    "Using these constraints, we can send a motion planning request and the planner will generate a suitable trajectory, according to the request. The move_group node will generate the suitable trajectory from the motion planner, which obeys all of the constraints. This can be sent to the robot joint trajectory controllers."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! So now you already know how to interact with your robot using the MoveIt RViz application, which is a very interesting and useful tool. But, this is not the only way you can communicate with your robot!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the next unit, you will see how you can perform Motion Planning with code. So... let's go!"
   ]
  }
 ],
 "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": "2 - Motion Planning using Graphical Interfaces",
   "chapter_title": "Chapter 2. Motion Planning using Graphical Interfaces",
   "course_title": "ROS MANIPULATION IN 5 DAYS"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}