Course_Controllers_Unit_2-Copy1.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ROS Controllers MicroCourse"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Chapter 2: Configuring the controllers"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Estimated time of completion: <b>1'5h</b><br><br>\n",
    "This Unit will explain what configurations you need to do in order to be able to use the ros_control package with your robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END OF SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Configuring the URDF (Transmissions)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to be able to use the ros_control package in your simulation, you first need to add some things to the URDF file that defines your robot. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first element you will need to define are the transmissions. The transmission elements are used to describe the relationship between a joint and an actuator. A transmission transforms efforts/flow variables such that their product - power - remains constant."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Anyways, in order to better understand how this elements work, have a look at the next Exercise and its following explanation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 2.1</p>\n",
    "<br>\n",
    "a) Go to the rrbot_description package, and copy the xacro file that defines your the simulated robot into your workspace. For doing that, just execute the following commands:\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "Execute in WebShell #1\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "roscd rrbot_description/urdf/"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "cp rrbot.xacro /home/user/catkin_ws/src/"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) You will now be able to visualize the files through the IDE, which is much more pleasant."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div style=\"text-align: left\">\n",
    "<img src=\"img/add_rviz.png\" width=\"250\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "<img src=\"img/robotmodel.png\" width=\"300\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "<img src=\"img/base_link.png\" width=\"250\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Have a look at the transmission tags that are defined in this file. You can find any word in the IDE by activating the \"Find\" tool, hitting Ctrl+F."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 2.1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Explaining the file"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you have already seen in the file, the transmission tags have 3 main elements that you need to define:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ul>\n",
    "<li><b>&lt;type&gt;</b>: It defines the type of transmission that will be used. Currently, the only available option is <i><b>transmission_interface/SimpleTransmission</b></i>.</li> \n",
    "<br>\n",
    "<img src=\"img/type_tag.png\" width=\"600\" />\n",
    "<br>\n",
    "<li><b>&lt;joint&gt;</b>: It defines the joint connected to this transmission. The name of the joint must match a join that already is defined for the robot (in its URDF files). You also need to define the hardware interface it will use, in this case the <i><b>EffortJointInterface</b></i>.</li>\n",
    "<br>\n",
    "<img src=\"img/joint_tag2.png\" width=\"600\" />\n",
    "<br>\n",
    "<li><b>&lt;actuator&gt;</b>: It defines the actuator connected to this transmission. You will define here the hardware interface (as you did for the joint) and the mechanical reduction (this one is optional, but it is usually defined and set to 1).</li>\n",
    "<br>\n",
    "<img src=\"img/actuator_tag2.png\" width=\"600\" />\n",
    "<br>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Configuring the URDF (Plugin)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Besides defining the transmission tags, you also need to add the gazebo ros control plugin to your URDF files. As we did before, follow the next exercise and its following explanation in order to better understand how this works."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 2.2</p>\n",
    "<br>\n",
    "a) Go to the rrbot_description package, and copy the file that contains the gazebo tags into your workspace. For doing that, just execute the following commands:\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "Execute in WebShell #1\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "roscd rrbot_description/urdf/"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "cp rrbot.gazebo /home/user/catkin_ws/src/"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Have a look at the top gazebo tag of this file and try to guess what it does."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 2.2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Explaining the file"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you have seen, it looks quite simple. But... what does this exactly do? Let's see!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Basically, what this plugin does is to read the information contained in the transmission tags you saw before, and load it properly so that Gazebo can understand it. When loading this plugin, you will always have to add a plugin tag indicating the <i>name</i> of the plugin and the <i>filename</i> that contains it."
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "<plugin name=\"gazebo_ros_control\" filename=\"libgazebo_ros_control.so\">\n",
    "</plugin>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inside this <i>&lt;plugin&gt;</i> tag, there are some tags that you can define:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ul>\n",
    "<li><b>&lt;robotNamespace&gt;</b>: The namespace to be used by the instance of this plugin. By default, it will put the name of the robot that's in the URDF file.</li>\n",
    "<li><b>&lt;controlPeriod&gt;</b>: The period of the controller update (in seconds). By default, it uses the Gazebo period. This tag is usually left to it's default value, so it's not defined.</li>\n",
    "<li><b>&lt;robotParam&gt;</b>: The location of the robot_description parameter on the parameter server. By default, it uses '/robot_description'. This tag is usually left to it's default value, so it's not defined.</li>\n",
    "<li><b>&lt;robotSimType&gt;</b>: The pluginlib name of a custom robot hardware interface to be used. By default, it uses 'DefaultRobotHWSim'.</li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, in most cases, you would just define the <i><b>&lt;robotNamespace&gt;</b></i> tag. But what about the <i><b>&lt;robotSimType&gt;</b></i> tag?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This tag will be used just in the case that you want to define a custom hardware interface, so for more advanced users. For what concerns this Course, we are not going to see how to do this, so dont worry too much."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the default behavior, which is either not setting this tag or setting it to 'DefaultRobotHWSim', you will be able to use the following interfaces, which are the ones you saw in the previous Chapter:\n",
    "<ul>\n",
    "<li><b>JointStateInterface</b></li>\n",
    "<li><b>EffortJointInterface</b></li>\n",
    "<li><b>PositionJointInterface</b></li>\n",
    "<li><b>VelocityJointInterface</b></li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Configuring and launching the controllers"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you have defined the transmissions in your URDF file, and loaded the gazebo ros control plugin, it's time to actually begin configuring the controllers."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to better understand how you would do this, just follow the next Exercise and its explanations!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 2.3</p>\n",
    "<br>\n",
    "a) Create a new package named <b><i>my_robot_control</i></b>. Inide this package, create 2 new folders, one named <b><i>config</i></b> and the other one named <b><i>launch</i></b>.\n",
    "<br><br>\n",
    "b) Inside the config folder, create a file named <b><i>my_robot_control.yaml</i></b>. Write into this file the following structure:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "rrbot:\n",
    "  joint_state_controller:\n",
    "    type: joint_state_controller/JointStateController\n",
    "    publish_rate: 50  \n",
    "\n",
    "  joint1_position_controller:\n",
    "    type: effort_controllers/JointPositionController\n",
    "    joint: joint1\n",
    "    pid: {p: 100.0, i: 0.01, d: 10.0}\n",
    "  joint2_position_controller:\n",
    "    type: effort_controllers/JointPositionController\n",
    "    joint: joint2\n",
    "    pid: {p: 100.0, i: 0.01, d: 10.0}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So here, you are basically defining 2 types of controllers: the <b>state controllers</b> (which you saw in the Previous Chapter that ALWAYS need to be defined) and the <b>effort controllers</b>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For the position controllers, you need to define the type of plugin that will use (in this case it uses the effort_controllers plugin that accepts Position commands as input), the name of the joint that it will control, and the pid gains. You'll see later a little bit more about this pid gains. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you already saw in the previous Chapter, the joint_state_controller will allow you to know the state of the joints of your robot at any moment. It will publish this data into the <b>/joint_states</b> topic. For this one, you just need to define the type of plugin that it will use, and the publish rate."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Inside the launch folder, create a new file named <i><b>my_robot_control.launch</b></i>. Copy the following contents into that file:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "<launch>\n",
    "\n",
    "  <rosparam file=\"$(find my_robot_control)/config/my_robot_control.yaml\" command=\"load\"/>\n",
    "\n",
    "  <node name=\"controller_spawner\" pkg=\"controller_manager\" type=\"spawner\" respawn=\"false\"\n",
    "    output=\"screen\" ns=\"/rrbot\" args=\"joint1_position_controller joint2_position_controller joint_state_controller\"/>\n",
    "\n",
    "  <node name=\"robot_state_publisher\" pkg=\"robot_state_publisher\" type=\"robot_state_publisher\"\n",
    "    respawn=\"false\" output=\"screen\">\n",
    "    <remap from=\"/joint_states\" to=\"/rrbot/joint_states\" />\n",
    "  </node>\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's break down this file and explain it more in detail:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<rosparam file=\"$(find my_robot_control)/config/my_robot_control.yaml\" command=\"load\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What you are doing here is to load the control file you created previously into the ROS Parameter Server."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<node name=\"controller_spawner\" pkg=\"controller_manager\" type=\"spawner\" respawn=\"false\"\n",
    "    output=\"screen\" ns=\"/rrbot\" args=\"joint1_position_controller joint2_position_controller joint_state_controller\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What you are doing here is to  start the two joint position controllers by running a python script that makes a service call to the ros_control controller manager. The service calls tell the controller manager which controllers you want to load. It also loads a third controller that publishes the joint states of all the joints with hardware_interfaces and advertises the topic on /joint_states."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<node name=\"robot_state_publisher\" pkg=\"robot_state_publisher\" type=\"robot_state_publisher\"\n",
    "    respawn=\"false\" output=\"screen\">\n",
    "    <remap from=\"/joint_states\" to=\"/rrbot/joint_states\" />\n",
    "  </node>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, here you are starting a robot_state_publisher node that simply listens to /joint_states messages from the joint_state_controller, and then publishes the transforms to /tf. This allows you to see your simulated robot in Rviz as well as do other tasks."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Launch the file you've just created by executing the following command."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "Execute in WebShell #1\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "roslaunch my_robot_control my_robot_control.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you execute a <i>rostopic list</i> in any shell, you will see that the following topics appear."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/controller_topics.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These are the topics that have been created by the launch file you've executed, more specifically by the controller_spawner node. The <b>command</b> topics will allow you to send goals to your joints, while the <b>joint_states</b> topic will provide you information about the current state of the joints."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "e) Now, you can test your controllers by publishing into their command topics. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "Execute in WebShell #2\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "rostopic pub -1 /rrbot/joint1_position_controller/command std_msgs/Float64 \"data: 1.5\""
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "rostopic pub -1 /rrbot/joint2_position_controller/command std_msgs/Float64 \"data: 1.0\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#AE0202;color:white;\">Expected Result for Exercise 2.3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div style=\"text-align: middle\">\n",
    "<img src=\"img/pos1.png\" width=\"300\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "<img src=\"img/pos2.png\" width=\"225\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 2.3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Quite easy, right? Probably now some things you didn't understand from the Previous Chapter, start to make more sense."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Well, with the previous Exercise you've seen how to create a control package in order to be able to communicate with your joints, and you've launched you controllers within a launch file, which is the most common and recommended way to do this. However, this is not the only way you can load your controllers. Let's see an alternative way."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Launching your controllers through service calls"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Besides using the launch file you saw before, you could also load the controllers by using service calls to the controller manager. We are going to explain this way, because for some special cases, it might be useful. But as I previously said, the common and recommended way to load the controllers is using a launch file."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, in order to see how this could be done, just follow the next Exercise!\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 2.4</p>\n",
    "<br>\n",
    "a) Inside the launch folder, create another launch file called <i><b>my_robot_conotrol2.launch</b></i>. Inside this new file, copy the contents from the previous launch file, but remove the controller_spawner node that loads the controllers. <br><br>\n",
    "b) Launch this new launch file."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Now, execute the following commands in order to load de controllers."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "Execute in WebShell #1\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "rosservice call /rrbot/controller_manager/load_controller \"name: 'joint1_position_controller'\""
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "rosservice call /rrbot/controller_manager/load_controller \"name: 'joint2_position_controller'\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, here you are performing a service call to the controller manager, providing the names of the controllers (that are defined in the YAML file) that you want to load."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Now, execute the following command in order to start the controllers."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "Execute in WebShell #2\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "rosservice call /rrbot/controller_manager/switch_controller \"{start_controllers: ['joint1_position_controller','joint2_position_controller'], stop_controllers: [], strictness: 2}\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When you load the controllers, they are loaded in a stopped state, so you need to start them."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "e) Now, you can interact with you controllers as you did in the previous Exercise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "f) To stop the controllers, you can execute the following command."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "Execute in WebShell #2\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "rosservice call /rrbot/controller_manager/switch_controller \"{start_controllers: [], stop_controllers: ['joint1_position_controller','joint2_position_controller'], strictness: 2}\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#AE0202;color:white;\">Expected Result for Exercise 2.4</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Same results as for the Previous Exercise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 2.4</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Interacting with the joints in a graphical way"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Until now, you've interacted with the joints of your robot by sending commands to the appropriate topic. But you can also do this by using a very interesting tool that ROS provdies: <b>rqt_gui</b>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to see how you can do this, follow the Next Exercise!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 2.5</p>\n",
    "<br>\n",
    "a) Open the rqt_gui tool by executing the following command."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "Execute in WebShell #1\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "rosrun rqt_gui rqt_gui"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Hit the icon with a screen in the top-right corner of the IDE window \n",
    "<img src=\"img/font-awesome_desktop.png\"/> \n",
    "in order to open the Graphic Interface."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the new window that will open, you will see something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "<img src=\"img/rqt_gui.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) On the 'Plugins' menu, select 'Topic' and 'Message Publisher'."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/publisher.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) In the new window that will apear, choose the topic <i><b>/rrbot/joint1_position_controller/command</b></i>, which is the topic where we can send commands to the joint 1, and click the '+' button."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/add.png\" width=\"800\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "e) Now, enable the publisher in the below section by clicking on the check button that appears at the left of the topic name, and set the publish rate to 100.00."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/freq_enable.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "f) Next, expand the publisher by clicking the '+' icon at the left of the row. In the 'expression' column, try different radian values and check how the joint moves in the simulation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "g) Now, try to put in the 'expression' column the next expression: <i><b>sin(i/100)</b></i>. Check what happens in the simulation. You should see the robot moving in the next way:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/sin_moves.gif\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, you are making the joint1 of the robot switch its values by using a sine wave. The letter <i><b>i</b></i> is the variable that rqt uses for time."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "h) Next, add a plot display by going to the 'Plugins' menu, 'Visualization', and 'Plot'. Add the topic <i><b>/rrbot/joint1_position_controller/command</b></i> and click the '+' button."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div style=\"text-align: middle\">\n",
    "<img src=\"img/plot.png\" width=\"300\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "<img src=\"img/add_plot.png\" width=\"425\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/sin_plot.gif\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So here, you are visualizing the commands you are sending to the joint 1."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "i) Plot also the topic <i><b>/rrbot/joint1_position_controller/state/process_value</b></i>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/process_value.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This topic shows the actual real position of the joint, so you want this 2 lines to be as equal as possible."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/sin_plot2.gif\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "j) Finally, you are going to open the Dynamic Reconfigure tool, by going to 'Plugins', 'Configuration' and 'Dynamic Reconfigure'. This tool allows you to dynamically modify some configuration values of your robot. In the left panel, click on the 'Expand All' button, and click on the pid option of the joint 1. Tune the pid values in order to make the 2 lines as close as possible. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div style=\"text-align: middle\">\n",
    "<img src=\"img/dynamic_reconfigure.png\" width=\"400\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "<img src=\"img/dynamic_reconfigure2.png\" width=\"425\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 2.5</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's all for this Unit! Quite interesting, right? Now, if you want to learn how to create your own controller, go to the Next Unit!"
   ]
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python [Root]",
   "language": "python",
   "name": "Python [Root]"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}