Artificial Intelligence Wiki

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 this tutorial.

• Create a new package for our controllers called iai_seminar_manipulation_controllers. Make sure it depends on the necessary pr2-interface packages.
• Create a new h-file for the controller in a suitable include-sub-directory, and add the minimally necessary function headers to comply with the controller interface. Choose a fitting name for the controller. ;)
• 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.

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 here to do so, and test if the plugin is visible with

rospack plugins --attrib=plugin pr2_controller_interface

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…

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:

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

• 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:

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

• Loading it into the controller manager is then done like this:

$ rosrun pr2_controller_manager pr2_controller_manager load iai_joint_position_controller

• The controller manager should now print our controller a stopped:

$ rosrun pr2_controller_manager pr2_controller_manager list

• After starting it:

$ rosrun pr2_controller_manager pr2_controller_manager start iai_joint_position_controller

• It should show up as started:

$ rosrun pr2_controller_manager pr2_controller_manager list

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:

$ rosrun pr2_controller_manager pr2_controller_manager stop iai_joint_position_controller

More examples of how to use the pr2_controller_manager can be found here.

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 your launch directory.
• 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 here and here.

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.

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 here, and we want to use this message type. Also have a look at 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:

$ rostopic echo r_arm_iai_joint_position_controller/state

$ 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]}'