Chapter2_Moveit.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ROS-Industrial MicroCourse"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Chapter 2: Build a MoveIt Package"
   ]
  },
  {
   "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": [
    "## Building a MoveIt package"
   ]
  },
  {
   "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 we defined in the previous Chapter) with MoveIt. In order to generate this package, just follow all the steps described in the following exercise!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 2.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": {},
   "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": {},
   "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": {},
   "source": [
    "Now, just click the \"Browse\" button, select the xacro file you created in the Previous Chapter, and click on the \"Load Files\" button. You should now see this:<br>\n",
    "<img src=\"img/xacro_loaded.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": {},
   "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/collision_matrix.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": {},
   "source": [
    "<img src=\"img/virtual_joint.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>manipulator</b></i>, which uses the <i><b>KDLKinematicsPlugin</b></i>. Just like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/plan_group1.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, you will click on the \"Add Kin Chain\" button, and you will select the <i><b>base_link</b></i> as Base Link, and the <i><b>link_tool0</b></i> as Tip Link. Just like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/plan_group2.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": {},
   "source": [
    "<img src=\"img/plan_group3.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": [
    "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. Your robot will appear with all its joints to \"0\". So, you will name this pose as <i><b>allZeros</b></i>, and click the \"Save\" button."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/allzeros.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, repeat the operation, but this time adjusting the position of the joints so that the robot is in an specific position that we will call \"home\". You can set the joints as you want, but I recommend you to set the robot in a position that is not complicated. For instance, something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/home.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is very useful, for instance, when you know that there are some poses that your robot will have to go many times"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "g) 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": {},
   "source": [
    "<img src=\"img/conf_files1.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": {},
   "source": [
    "<img src=\"img/conf_files2.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's it! You have just created a MoveIt package for you industrial robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 2.1</p>"
   ]
  },
  {
   "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": [
    "## 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 2.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><br>\n",
    "roslaunch myrobot_moveit_config demo.launch<br>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "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": {},
   "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": "raw",
   "metadata": {},
   "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": {},
   "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": {},
   "source": [
    "<img src=\"img/moveit_rviz.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Now, move to the Planning tab. Here:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/moveit_rviz_planning.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": {},
   "source": [
    "<img src=\"img/start_position.png\" width=\"250\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) At the query section, you can choose the <i><b>random valid</b></i> option and click on the \"Update\" button. Your robot scene will be updated with the new random position that has been selected."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/scene_updated.png\" width=\"300\" />"
   ]
  },
  {
   "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 random point."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/giphy2.gif\" 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": [
    "<img src=\"img/giphy.gif\" width=\"500\" />"
   ]
  },
  {
   "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 a random position, you could set one of the predefined positions you created while building the package (home, allZeros, etc...). 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 2.2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Moving the robot in the simulation"
   ]
  },
  {
   "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 2.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": "raw",
   "metadata": {},
   "source": [
    "controller_list:\n",
    "  - name: sia10f/joint_trajectory_controller\n",
    "    action_ns: \"follow_joint_trajectory\"\n",
    "    type: FollowJointTrajectory\n",
    "    joints: [joint_s, joint_l, joint_e, joint_u, joint_r, joint_b, joint_t]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So basically, here you are defining the Action Server (and the type of message that it will use) that you will use for controlling the joints of your robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, you are setting the name of your joint trajectory controller Action Server. 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": {},
   "source": [
    "<img src=\"img/actionserver.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>/sia10f/joint_trajectory_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>sia10f_macro.xacro</b></i> file, at the <i><b>sia10f_description</b></i> package."
   ]
  },
  {
   "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": "raw",
   "metadata": {},
   "source": [
    "controller_joint_names: [joint_s, joint_l, joint_e, joint_u, joint_r, joint_b, joint_t]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Now, if you open the <i><b>myrobot_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": "raw",
   "metadata": {},
   "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_exeution.launch</b></i>."
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "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\">[/sia10f/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>/sia10f/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 file 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/move_sim.gif\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 2.3</p>"
   ]
  },
  {
   "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"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}