Course_Controllers_Unit_3.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ROS Control MicroCourse"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Chapter 3: Creating a Controller"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Estimated time to completion: <b>1 hour</b><br><br>\n",
    "This unit will explain how to create a basic controller for your robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END OF SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, up until this point, you've seen how you can connect your simulated robot to the ros_control packages to be able to control its joints. And you are using the default controllers that the ros_control packages provide, which is the most common practice. In some cases, it may be interesting to create a custom ROS controller that sends specific commands to your joints, don't you think?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's exactly what you are going to learn to do in this unit! So, in order to see how you can achieve this, follow the next exercise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 3.1</p>\n",
    "### Creating the package\n",
    "<br>\n",
    "Create a new package named my_controller with the following dependencies: <i><b>roscpp</b></i>, <i><b>pluginlib</b></i>, <i><b>controller_interface</b></i>, and <i><b>hardware_interface</b></i>. To do that, just execute the following command:\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": [
    "catkin_create_pkg my_controller roscpp pluginlib controller_interface hardware_interface"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Creating the Source code"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inside the src folder that was created in your new package, create a file named <i><b>my_controller.cpp</b></i>. Copy the following code into the file:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "#include <controller_interface/controller.h>\n",
    "#include <hardware_interface/joint_command_interface.h>\n",
    "#include <pluginlib/class_list_macros.h>\n",
    "\n",
    "namespace controller_ns{\n",
    "\n",
    "class PositionController : public controller_interface::Controller<hardware_interface::EffortJointInterface>\n",
    "{\n",
    "public:\n",
    "  bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n)\n",
    "  {\n",
    "    std::string my_joint;\n",
    "    if (!n.getParam(\"joint\", my_joint)){\n",
    "      ROS_ERROR(\"Could not find joint name\");\n",
    "      return false;\n",
    "    }\n",
    "\n",
    "    joint_ = hw->getHandle(my_joint);  // throws on failure\n",
    "    return true;\n",
    "  }\n",
    "\n",
    "  void update(const ros::Time& time, const ros::Duration& period)\n",
    "  {\n",
    "    double error = setpoint_ - joint_.getPosition();\n",
    "    joint_.setCommand(error*gain_);\n",
    "  }\n",
    "\n",
    "  void starting(const ros::Time& time) { }\n",
    "  void stopping(const ros::Time& time) { }\n",
    "\n",
    "private:\n",
    "  hardware_interface::JointHandle joint_;\n",
    "  static const double gain_ = 2.25;\n",
    "  static const double setpoint_ = 1.00;\n",
    "};\n",
    "PLUGINLIB_EXPORT_CLASS(controller_ns::PositionController, controller_interface::ControllerBase);\n",
    "}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Are you a little bit confused right now? Don't worry! Let's explain the code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#include <controller_interface/controller.h>\n",
    "#include <hardware_interface/joint_command_interface.h>\n",
    "#include <pluginlib/class_list_macros.h>\n",
    "\n",
    "namespace controller_ns{"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you are just including some files from other packages that are needed in order to create the controller, and declaring a namespace for this class (namespaces provide a method to prevent name conflicts in large projects)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class PositionController : public controller_interface::Controller<hardware_interface::EffortJointInterface>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you are just declaring the class, which will be inherited from <i><b>hardware_interface::EffortJointInterface</b></i>. This means that this controller will be able to control only joints that use an effort interface."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n)\n",
    "{\n",
    "  std::string my_joint;\n",
    "  if (!n.getParam(\"joint\", my_joint)){\n",
    "    ROS_ERROR(\"Could not find joint name\");\n",
    "    return false;\n",
    "  }\n",
    "  joint_ = hw->getHandle(my_joint);  // throws on failure\n",
    "  return true;\n",
    "}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you are creating the init() function, which will be called when your controller is loaded by the controller manager. Inside this function, you will get the name of the joint that you will control from the Parameter Server first (so from the YAML file, which you will modify later), and then you will get the joint object to use in the realtime loop."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "void update(const ros::Time& time, const ros::Duration& period)\n",
    "{\n",
    "double error = setpoint_ - joint_.getPosition();\n",
    "joint_.setCommand(error*gain_);\n",
    "}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you are defining the command that you are going to send to your joint. In this case, it's a product between an <i><b>error</b></i> variable and a <i><b>gain_</b></i> variable. The error variable is defined as the difference between the current position (<i><b>joint_.getPosition()</b></i>) of the joint and the goal position (<i><b>setpoint_</b></i>) of the joint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "void starting(const ros::Time& time) { }\n",
    "void stopping(const ros::Time& time) { }"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you are starting and stopping the controller."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "private:\n",
    "hardware_interface::JointHandle joint_;\n",
    "static const double gain_ = 2.25;\n",
    "static const double setpoint_ = 1.00;"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you are just defining a variable that you'll use through the class code."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "PLUGINLIB_EXPORT_CLASS(controller_ns::PositionController, controller_interface::ControllerBase);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you are calling the special macro plugin <i><b>PLUGINLIB_EXPORT_CLASS</b></i> in order to allow this class to be dynamically loaded."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Creating a plugin description file"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inside your package, create a new file named <i><b>controller_plugins.xml</b></i>. Copy the following code into it:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "<library path=\"lib/libmy_controller_lib\">\n",
    "  <class name=\"my_controller/PositionController\" \n",
    "         type=\"controller_ns::PositionController\"           \n",
    "         base_class_type=\"controller_interface::ControllerBase\" />\n",
    "</library>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This file is a description of the controller plugin. It indicates the path where the library will be placed when we compile the controller, as well as the name, type, and class of the controller."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Updating the package.xml file"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Go to the <i><b>package.xml</b></i> file of your package. Inside the <i><b>&lt;export&gt;</b></i> tag, place the following line:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "<controller_interface plugin=\"${prefix}/controller_plugins.xml\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This line indicates that our package will provide a plugin. This is very important because if we don't add this line, the controller manager won't be able to find our new controller and load it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Updating the CMakeLists.txt file"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Go to the <i><b>CMakeLists.txt</b></i> file of your package. Find the <i><b>add_library()</b></i> and <i><b>target_link_libraries()</b></i> functions, and replace them with these:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "add_library(my_controller_lib src/my_controller.cpp)"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "target_link_libraries(my_controller_lib ${catkin_LIBRARIES})"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Build the controller"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Go to the <i><b>catkin_ws</b></i> directory and compile your package."
   ]
  },
  {
   "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": [
    "catkin_make"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, check if your controller has been properly registered by using the following command:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "rospack plugins --attrib=plugin controller_interface"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If everything has gone well, you should visualize your plugin in the controllers list."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/my_controller.png\" width=\"1000\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Write the configuration file"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inside the package, create a new folder named <i><b>config</b></i>, with a file inside named <i><b>my_controller.yaml</b></i>, and copy the contents of the YAML file you created in the previous chapter into this file. Finally, modify the joint 1 configuration in order for it to be controlled by your new controller."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"color:red;\"><b>SOLUTION: DO NOT LOOK unless you are really stuck.</b></p>"
   ]
  },
  {
   "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: my_controller/PositionController\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": [
    "### Create a launch file"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inside the package, create a new folder named <i><b>launch</b></i>, with a launch file inside named <i><b>my_controller.launch</b></i>, that launches the control system of the robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#AE0202;color:white;\">Expected Result for Exercise 3.1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you launch your controller with the robot at its start position, you should get something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/controller_move.png\" width=\"300\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Keep in mind that depending on how the robot's joints are placed after you moved them in the previous chapter, this could change."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 3.1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's it!! You have finally completed the course. Well, almost... Now, if you want to test all that you've learned in a course project, 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": 1
}