## page was renamed from controllers/Tutorials/Running a controller in simulation
## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## note.0=[[controllers/Tutorials/Writing a realtime controller|Writing a realtime controller]]
## note.1=[[simulator_gazebo/Tutorials/StartingGazebo|Starting Gazebo]]
## note.2=[[simulator_gazebo/Tutorials/AddingObjectsToTheWorld|Adding Objects To The World]]
## note.3=[[move_arm/Tutorials/MoveArmJointGoal|Teleop Arm Using move_arm]]
## descriptive title for the tutorial
## title = Incrementally develop and test a controller in simulation.
## description = This tutorial walks you through the process of creating s simple custom controller, incrementally adding functionality to it and testing each step in simulation.
## level= IntermediateCategory
## keywords =
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

/!\ '''work in progress'''

<<TableOfContents(4)>>


= Get Gazebo Up and Running =

Make sure you complete [[simulator_gazebo/Tutorials/StartingGazebo|StartingGazebo]] before continuing. The [[simulator_gazebo/Tutorials/AddingObjectsToTheWorld|AddingObjectsToTheWorld]] and [[move_arm/Tutorials/MoveArmJointGoal|Teleop Arm Using move_arm]] tutorials will provide you with further insights into what is going on.


= A Word about Launch and Parameter Files =

In order to make the cycle of development and testing easier, we create (and update) two launch files, one for starting up Gazebo with a robot in it, and one for spawning the controller we are testing. There also is one parameter file that tells [[pr2_controller_manager]] about the controller plugins to launch, and later a configuration file for the tutorial controller.


== Launch file for Gazebo ==

This is essentially copy-pasted from two launch files referenced in the Gazebo tutorials, and it stays constant throughout the incremental development steps further down.
{{{
 $ cat `rospack find gazebo`/launch/empty_world.launch
 $ cat `rospack find pr2_gazebo`/pr2.launch
}}}
If you have trouble with this launch file, you should have a look at [[gazebo]] and [[pr2_gazebo]] to see if anything has changed there.

 * The {{{start_pr2_gazebo.launch}}} we'll be using looks like this:
{{{#!xml
<launch>
  <!-- launch gazebo with empty world -->
  <param name="/use_sim_time" value="true" />
  <node name="gazebo" pkg="gazebo" type="gazebo"
        args="$(find gazebo_worlds)/worlds/empty.world"
        respawn="false" >
    <env name="LD_LIBRARY_PATH"
	 value="$(find pr2_gazebo_plugins)/lib:$(find gazebo)/gazebo/lib:$(optenv LD_LIBRARY_PATH)" />
    <env name="GAZEBO_RESOURCE_PATH"
	 value="$(find pr2_ogre):$(find ikea_ogre):$(find gazebo_worlds):$(find gazebo)/gazebo/share/gazebo" />
    <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
  </node>
  
  <!-- spawn PR2 -->
  <include file="$(find pr2_description)/robots/upload_pr2.launch" />
  <node pkg="gazebo" type="spawn_model"
        args="-urdf -param robot_description -model pr2"
        respawn="false" output="screen" />
  <node pkg="robot_state_publisher" type="state_publisher"
        name="robot_state_publisher" output="screen">
    <param name="publish_frequency" type="double" value="50.0" />
    <param name="tf_prefix" type="string" value="" />
  </node>
</launch>
}}}

 * We use it to start Gazebo via [[roslaunch]] like this:
  {{{
   $ roslaunch start_pr2_gazebo.launch
  }}}
  you should see something like this<<BR>>
  {{attachment:gazebo-pr2.png}}


== Launch files for testing the controller ==

Looking at an example for inspiration:
{{{
 $ cat `rospack find pr2_gazebo`/controllers/l_arm_position_controller.launch
}}}
we see that
 * we define which controller to run on which joint using a [[http://en.wikipedia.org/wiki/Yaml|YAML]] file that gets loaded onto the ROS parameter server
 * we use the {{{spawner}}} script from [[pr2_controller_manager]] and give it a list of controllers to launch

 * We will start out with a simple {{{test.yaml}}} file that specifies a single controller of type {{{TutorialController}}}. This may evolve as we add more features to our controller, or as we combine it with other controllers.
  {{{
controller_tutorial:
  type: TutorialController
  }}}

 * We use a simple {{{test.launch}}} file that loads the {{{test.yaml}}} file and spawns a {{{controller_tutorial}}} (later steps will add to this file):
  {{{#!xml
<launch>
  <rosparam file="$(find controller_tutorial)/test.yaml"
            command="load" />
  <node pkg="pr2_controller_manager"
        type="spawner"
        name="controller_tutorial"
        args="controller_tutorial"
        output="screen" />
</launch>
  }}}

 * With these two test files, it is straightforward to start the controller we are developing:
  {{{
   $ roslaunch test.launch
  }}}
  Note that any console output from your controller will appear in the terminal where you started Gazebo, not the one where you just launched your controller.


= Incremental Development =


== Step zero: create package directory and files ==

 1. Create a directory to contain the controller code. In the following, we will call it {{{controller_tutorial}}}. ''Change the names in the {{{manifest.xml}}} and other files accordingly if you chose another name.''
  {{{
   $ mkdir controller_tutorial
   $ cd controller_tutorial
  }}}

 2. If necessary, add the direcory to your {{{ROS_PACKAGE_PATH}}} environment variable. E.g.
  {{{
   $ export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:`pwd`
  }}}

 3. Populate the direcory with files. You can use [[attachment:step0.tar.gz]] to do so, which contains just an empty shell that compiles, links, and registers itself as a plugin:
  {{{
   $ tar xfvz step0.tar.gz
   CMakeLists.txt
   Makefile
   controller_plugins.xml
   controller_tutorial.cpp
   manifest.xml
  }}}

 4. Build and check if the plugin has been correctly registered:
  {{{
   $ make
   $ rospack plugins --attrib=plugin pr2_controller_interface
  }}}
  The output should contain a line with {{{controller_tutorial}}} (probably the last line).


== Step one: listing robot links ==

The {{{controller_tutorial.cpp}}} from step zero does not actually do anything. So let's start by just having it list the available robot links. For this, we add code to the {{{TutorialController::init()}}} which actually uses the {{{pr2_mechanism::RobotState}}} passed as parameter. The relevant snippet uses documentation from the [[pr2_mechanism_model]] package:
 * [[http://www.ros.org/doc/api/pr2_mechanism_model/html/classpr2__mechanism_1_1RobotState.html|RobotState]] class
 * [[http://www.ros.org/doc/api/pr2_mechanism_model/html/classpr2__mechanism_1_1Robot.html|Robot]] class
 * [[http://www.ros.org/doc/api/pr2_mechanism_model/html/classpr2__mechanism_1_1Joint.html|Joint]] class

{{{#!cplusplus
bool TutorialController::
init(pr2_mechanism::RobotState *robot, const ros::NodeHandle &n)
{
  fprintf(stderr, "TutorialController::init()\n");
  
  typedef std::vector<pr2_mechanism::Joint*> jointvec_t;
  jointvec_t const & jvec(robot->model_->joints_);
  for (jointvec_t::const_iterator ij(jvec.begin());
       ij != jvec.end();
       ++ij)
    {
      fprintf(stderr,
	      "  n: %s\tt: %d\tmin: %g\tmax: %g\n",
	      (*ij)->name_.c_str(),
	      (*ij)->type_,
	      (*ij)->joint_limit_min_,
	      (*ij)->joint_limit_max_);
    }
  
  return true;
}
}}}

 1. Unpack the [[attachment:step1.tar.gz]] tarball to get the code into the {{{controller_tutorial}}} directory and build it:
  {{{
   $ tar xvfz step1.tar.gz
   CMakeLists.txt
   Makefile
   controller_plugins.xml
   controller_tutorial.cpp
   manifest.xml
   start_pr2_gazebo.launch
   test.launch
   test.yaml
   $ make
  }}}

 2. Launch Gazebo:
  {{{
   $ roslaunch start_pr2_gazebo.launch
  }}}

 3. In another terminal, launch the controller:
  {{{
   $ roslaunch `rospack find controller_tutorial`/test.launch
  }}}
  You should see some output like the following in the terminal where you started Gazebo:
  {{{
TutorialController::init()
  n: base_laser_joint	t: 4	min: 0	max: 0
  n: bl_caster_l_wheel_joint	t: 2	min: 0	max: 0
  n: bl_caster_r_wheel_joint	t: 2	min: 0	max: 0
  n: bl_caster_rotation_joint	t: 2	min: 0	max: 0
  n: br_caster_l_wheel_joint	t: 2	min: 0	max: 0
  n: br_caster_r_wheel_joint	t: 2	min: 0	max: 0
  n: br_caster_rotation_joint	t: 2	min: 0	max: 0
  n: double_stereo_frame_joint	t: 4	min: 0	max: 0
  n: fl_caster_l_wheel_joint	t: 2	min: 0	max: 0
  n: fl_caster_r_wheel_joint	t: 2	min: 0	max: 0
  n: fl_caster_rotation_joint	t: 2	min: 0	max: 0
  n: fr_caster_l_wheel_joint	t: 2	min: 0	max: 0
  n: fr_caster_r_wheel_joint	t: 2	min: 0	max: 0
  n: fr_caster_rotation_joint	t: 2	min: 0	max: 0
  n: head_pan_joint	t: 1	min: -2.92	max: 2.92
  n: head_plate_frame_joint	t: 4	min: 0	max: 0
  n: head_tilt_joint	t: 1	min: -0.55	max: 1.047
  n: high_def_frame_joint	t: 4	min: 0	max: 0
  n: high_def_optical_frame_joint	t: 4	min: 0	max: 0
  n: l_elbow_flex_joint	t: 1	min: -2.3	max: 0.1
  n: l_forearm_cam_frame_joint	t: 4	min: 0	max: 0
  n: l_forearm_cam_optical_frame_joint	t: 4	min: 0	max: 0
  n: l_forearm_joint	t: 4	min: 0	max: 0
  n: l_forearm_roll_joint	t: 2	min: 0	max: 0
  n: l_gripper_float_joint	t: 3	min: -0.05	max: 0.001
  n: l_gripper_joint	t: 3	min: 0	max: 0.09
  n: l_gripper_l_finger_joint	t: 1	min: 0	max: 0.548
  n: l_gripper_l_finger_tip_joint	t: 1	min: 0	max: 0.548
  n: l_gripper_motor_accelerometer_joint	t: 4	min: 0	max: 0
  n: l_gripper_palm_joint	t: 4	min: 0	max: 0
  n: l_gripper_r_finger_joint	t: 1	min: 0	max: 0.548
  n: l_gripper_r_finger_tip_joint	t: 1	min: 0	max: 0.548
  n: l_gripper_tool_joint	t: 4	min: 0	max: 0
  n: l_shoulder_lift_joint	t: 1	min: -0.5236	max: 1.3963
  n: l_shoulder_pan_joint	t: 1	min: -0.714602	max: 2.2854
  n: l_upper_arm_joint	t: 4	min: 0	max: 0
  n: l_upper_arm_roll_joint	t: 1	min: -0.8	max: 3.9
  n: l_wrist_flex_joint	t: 1	min: -0.1	max: 2.2
  n: l_wrist_roll_joint	t: 2	min: 0	max: 0
  n: laser_tilt_joint	t: 4	min: 0	max: 0
  n: laser_tilt_mount_joint	t: 1	min: -0.785	max: 1.57
  n: narrow_stereo_frame_joint	t: 4	min: 0	max: 0
  n: narrow_stereo_l_stereo_camera_frame_joint	t: 4	min: 0	max: 0
  n: narrow_stereo_optical_frame_joint	t: 4	min: 0	max: 0
  n: narrow_stereo_r_stereo_camera_frame_joint	t: 4	min: 0	max: 0
  n: plug_holder_joint	t: 4	min: 0	max: 0
  n: r_elbow_flex_joint	t: 1	min: -2.3	max: 0.1
  n: r_forearm_cam_frame_joint	t: 4	min: 0	max: 0
  n: r_forearm_cam_optical_frame_joint	t: 4	min: 0	max: 0
  n: r_forearm_joint	t: 4	min: 0	max: 0
  n: r_forearm_roll_joint	t: 2	min: 0	max: 0
  n: r_gripper_float_joint	t: 3	min: -0.05	max: 0.001
  n: r_gripper_joint	t: 3	min: 0	max: 0.09
  n: r_gripper_l_finger_joint	t: 1	min: 0	max: 0.548
  n: r_gripper_l_finger_tip_joint	t: 1	min: 0	max: 0.548
  n: r_gripper_motor_accelerometer_joint	t: 4	min: 0	max: 0
  n: r_gripper_palm_joint	t: 4	min: 0	max: 0
  n: r_gripper_r_finger_joint	t: 1	min: 0	max: 0.548
  n: r_gripper_r_finger_tip_joint	t: 1	min: 0	max: 0.548
  n: r_gripper_tool_joint	t: 4	min: 0	max: 0
  n: r_shoulder_lift_joint	t: 1	min: -0.5236	max: 1.3963
  n: r_shoulder_pan_joint	t: 1	min: -2.2854	max: 0.714602
  n: r_upper_arm_joint	t: 4	min: 0	max: 0
  n: r_upper_arm_roll_joint	t: 1	min: -3.9	max: 0.8
  n: r_wrist_flex_joint	t: 1	min: -0.1	max: 2.2
  n: r_wrist_roll_joint	t: 2	min: 0	max: 0
  n: sensor_mount_frame_joint	t: 4	min: 0	max: 0
  n: torso_lift_joint	t: 3	min: 0	max: 0.2
  n: wide_stereo_frame_joint	t: 4	min: 0	max: 0
  n: wide_stereo_l_stereo_camera_frame_joint	t: 4	min: 0	max: 0
  n: wide_stereo_optical_frame_joint	t: 4	min: 0	max: 0
  n: wide_stereo_r_stereo_camera_frame_joint	t: 4	min: 0	max: 0
  }}}
  As you can see, there are quite a few joints on PR2...


== Step two: commanding sinusoidal torques ==

Now it is time to actually command some torques. To achieve this, we will simply send some sinusoidal torques to some of the joints. The names of the joints to command and parameters of the sine waves will be read from a YAML file. The name of that file will be read from the parameter server when the controller starts up.

 1. Unpack [[attachment:step2.tar.gz]] into the {{{controller_tutorial}}} directory:
  {{{
   $ tar xfvz step2.tar.gz
   CMakeLists.txt
   Makefile
   controller_plugins.xml
   controller_tutorial.cpp
   manifest.xml
   start_pr2_gazebo.launch
   test.launch
   test.yaml
   wave.yaml
  }}}

 2. Look at the relevant changes:
  * Our dependence on the YAML parser has been added to {{{manifest.xml}}}
  * {{{controller_tutorial.cpp}}} now uses a simple data structure for storing the waveform parameters for each of the joints that is being commanded by the controller. The mapping from joints to waveforms happens in {{{TutorialController::init()}}}, and the update of the command torques in {{{TutorialController::update()}}}. Also, {{{TutorialController::starting()}}} is used to record the start time of the controller, such that at each tick we can determine the number of seconds (wallclock) since we started.
  * A private parameter has been added to the {{{test.launch}}} file, such that the {{{TutorialController::init()}}} can get the name of the waveform file name from the parameter server.
  * The new {{{wave.yaml}}} file maps joint names to the parameters of the sinusoidal torque that we are going to command them with.

 3. Build
  {{{
   $ make
  }}}

 4. Launch Gazebo
  {{{
   $ roslaunch start_pr2_gazebo.launch
  }}}

 5. Launch test
  {{{
   $ roslaunch test.launch
  }}}
  The terminal where you started Gazebo should now spew out 8 columns of changing numbers, and the simulated robot should be moving its arms (in a not so useful manner).



## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## WhereToPutCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE