===== Implementing a Joint Space Controller =====

In this exercise you will use standard ROS libraries and interfaces to write a position-resolved joint-space controller for the PR2. This will be genuine low-level controller running in hard realtime on the robot.

For the next couple of tasks, make sure to have understood [[http://ros.org/wiki/pr2_mechanism/Tutorials/Writing%20a%20realtime%20joint%20controller|this tutorial]].

==== Creating the ROS Package ====

  * Create a new package for our controllers called iai_seminar_manipulation_controllers. Make sure it depends on the necessary pr2-interface packages.
  * Copy IAISeminarMultiJointPositionController.h for the controller in a suitable include-sub-directory.
  * Create the corresponding cpp-file in src, and add empty implementations for the functions declared in the h-file.
  * Add our still empty controller as a __regular__ library (NOT a controller plugin library) to the CMakeLists, and try compiling.

==== Exporting the Controller as a Controller Plugin ====

In order to tell the pr2 controller manager that our controller can be loaded and run as a realtime controller we need to register it as a controller plugin. Adapt the example from [[http://ros.org/wiki/pr2_mechanism/Tutorials/Writing%20a%20realtime%20joint%20controller|here]] to do so, and test if the plugin is visible with

<code>
rospack plugins --attrib=plugin pr2_controller_interface
</code>
iai_seminar_manipulation_controllers should now be in the list of controllers. Tipp: In case you wonder what the real name of your library is: Look for it after in your /lib dirrectory...

==== Loading, starting and stopping our Controller Plugin ====

Just to make sure we have set up everything correctly, let's use the pr2_controller_manager to load, start and stop our (still empty) controller.

  * Start our simulation environment with the PR2 from earlier.
  * Check if the controller manager knows about our controller plugin:

<code>
$ rosrun pr2_controller_manager pr2_controller_manager list-types
  ...some other controllers...
  iai_seminar_manipulation_controllers/<YourControllerClassName>
  ...some other controllers...
</code>

  * In order to actual load an instance of our controller class we need to choose a name for it, and let the pr2_controller_manager know that the controller so-named is associated with this type of controller. We do this by setting the parameter 'type' within the namespace of our controller-instance to the type of our controller. This might sound a bit complicated bit, but it isn't. Let's assume we wanna call our controller iai_joint_position_controller. Then we set the parameter like this:

<code>
$ rosparam set rosparam set iai_joint_position_controller/type iai_seminar_manipulation_controllers/<YourControllerClassName>
</code>

  * Loading it into the controller manager is then done like this:
<code>
$ rosrun pr2_controller_manager pr2_controller_manager load iai_joint_position_controller
</code>

  * The controller manager should now print our controller a stopped:
<code>
$ rosrun pr2_controller_manager pr2_controller_manager list
</code>

  * After starting it:
<code>
$ rosrun pr2_controller_manager pr2_controller_manager start iai_joint_position_controller
</code>

  * It should show up as started:
<code>
$ rosrun pr2_controller_manager pr2_controller_manager list
</code>

NOTE: Our controller code is now running on the simulated PR2. Right now, it does not do anything because you haven't filed any code in init(), start(), update(), stop(), yet.

  * Stopping the controller from the console is done like this:
<code>
$ rosrun pr2_controller_manager pr2_controller_manager stop iai_joint_position_controller
</code>

More examples of how to use the pr2_controller_manager can be found [[http://ros.org/wiki/pr2_mechanism/Tutorials/Running%20a%20realtime%20joint%20controller|here]].

==== Adding a launch-file ====
We will now add a launch-file to start our controllers automatically:
  * From the materials-directory, copy the file pr2_multi_joint_position_controllers.yaml into the config directory of our controllers package.
  * Create a new launch-file with a name sounding like it starts the joint position controllers on the pr2.
  * In this launch-file, Load the parameters given in the yaml-file.
  * Also in the launch-file, use the spawner of the pr2_controller_manager to load both controllers in a __stopped__ state.

For hints, look again [[http://www.ros.org/wiki/pr2_mechanism/Tutorials/Running%20a%20realtime%20joint%20controller|here]] and [[http://www.ros.org/wiki/pr2_controller_manager|here]].

==== Implementing the basic init-function ====
Now, we start implementing the actual controller functionalities. By setting up our controller in the init-phase:

  * Use our parameter server utilities to load the names of the joints from the parameter server. Note, look into the yaml-file of our controller to look up the name of the parameter holding the joint names.
  * Get a pointer to each of these joints and save all of them in a vector.
  * Check if all joints have been calibrated.
  * If anything goes wrong during init, throw a meaningful error-message and abort init by returning false.

==== Reporting information from the controller ====

We want our controller to publish its current state information during every 10th update-call. For that we will use a realtime-publisher from the realtime_tools package because the regular publisher is not thread-safe.
  * At the end of the init-function, init the realtime_publisher_ to publish on topic state in the local namespace. Also resize the message in the publisher by resizing all its internal points to the number of joints we want to control. It also makes sense to copy the name of the joints we want to control into the message. NOTE: Only in the init() function of a controller should you call init()- or resize- operations on any of the objects in the controller. Such operations usually result in memory allocation of a size unknown at compile time. Such allocations typically breake realtime-safety.

Hint: The documentation of the realtime publisher is [[http://www.ros.org/wiki/realtime_tools|here]], and we want to use [[http://ros.org/doc/api/pr2_controllers_msgs/html/msg/JointTrajectoryControllerState.html|this message type]]. Also have a look at [[http://ros.org/doc/groovy/api/realtime_tools/html/classrealtime__tools_1_1RealtimePublisher.html|API of the realtime publisher]].

  * In the update-call, copy the current position and velocities values from each joint into the corresponding slot of the message, and publish it.

For testing, launch our controller launch-file and start the controllers via the console. Afterwards you can listen to the messages on the topic from the console:
<code>
$ rostopic echo r_arm_iai_joint_position_controller/state
</code>

==== Getting a command inside ====

Our controller should be able to react to external commands. Therefore it shall listen to a topic 'command' in its own namespace. Since we are implementing a realtime process, we need to protect our internal command data structure 'position_command_' from read-write conflicts. A good way to do this is to use a double buffer, guarded by a boost::mutex. A __very__ thorough introduction on double buffers can be found [[http://gameprogrammingpatterns.com/double-buffer.html|here]]. Perform your own websearch to find out how to use boost::scoped_locks with a boost::mutex.

  * implement the command_callback to copy the new command from the msg inside our controller (using a double buffer and the mutex defined in the h-file)
  * in the update, always read the current command from the double buffer (also guarded by the mutex), and copy the buffered command into the internal position_command_. Hint: Implement and use the method copy_from_command_buffer for this. It is supposed to be atomic...
  * You can test your code by again echoing the state-topic of the controller and sending a new command from the console:

<code>
$ rostopic pub /l_arm_iai_joint_position_controller/command std_msgs/Float64MultiArray '{data: [0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6]}'
</code>
==== Using PIDs ====

For doing the actual control we will use the PID-controller implementation provided by Willow Garage. The h-file already declares a vector of such controllers. 

  * In the init function: Create and push a new PID for every joint to this internal vector. 
  * Also in the init function: read the PID gains from the namespace of the controller. The yaml-file loaded in our launch-file already loads the gains onto the parameter server. Hint: The [[http://ros.org/doc/groovy/api/control_toolbox/html/classcontrol__toolbox_1_1Pid.html|PID class]] has a very useful init-function that reads the gain parameters from a given namespace. Try using it.
  * Throw a ROS-error if loading of the gains failed. This will warn you of initialization errors.
==== Filling in start ====

Now, we will fill the start-hook:
  * Reset our double buffer for the command to something useful.
  * Reset the PID (not the gains, though).
  * Remember the starting time. Hint: You can get it from the [[http://www.ros.org/doc/api/pr2_mechanism_model/html/classpr2__mechanism__model_1_1RobotState.html|robot interface]].



==== Closing the loop ====

Finally, we will calculate the actual feedback control:
  * get the current time from the robot, calculate some delta-time. and remember the current time for the next control cycle
  * still: copy in the command from the double buffer
  * for every joint: calculate the error between actual and commanded position
  * for every joint: feed the error to the pid-controllers, and command their efforts to the joints of the robot
  * You can peek again [[http://ros.org/wiki/pr2_mechanism/Tutorials/Writing%20a%20realtime%20joint%20controller|here]].
  * As a last step: keep on publishing the current state of the controller.
==== Using our controller from ====

TBD.