| Note: This tutorial assumes that you have completed the previous tutorials: Writing a realtime controller, Starting Gazebo, Adding Objects To The World, Teleop Arm Using move_arm. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
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.Tutorial Level: INTERMEDIATE
![/!\](/moin_static197/rostheme/img/alert.png "/!\"){kind=small}
work in progress
Contents
Get Gazebo Up and Running
Make sure you complete StartingGazebo before continuing. The AddingObjectsToTheWorld and 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:
1 <launch>
2 <!-- launch gazebo with empty world -->
3 <param name="/use_sim_time" value="true" />
4 <node name="gazebo" pkg="gazebo" type="gazebo"
5 args="$(find gazebo_worlds)/worlds/empty.world"
6 respawn="false" >
7 <env name="LD_LIBRARY_PATH"
8 value="$(find pr2_gazebo_plugins)/lib:$(find gazebo)/gazebo/lib:$(optenv LD_LIBRARY_PATH)" />
9 <env name="GAZEBO_RESOURCE_PATH"
10 value="$(find pr2_ogre):$(find ikea_ogre):$(find gazebo_worlds):$(find gazebo)/gazebo/share/gazebo" />
11 <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
12 </node>
13
14 <!-- spawn PR2 -->
15 <include file="$(find pr2_description)/robots/upload_pr2.launch" />
16 <node pkg="gazebo" type="spawn_model"
17 args="-urdf -param robot_description -model pr2"
18 respawn="false" output="screen" />
19 <node pkg="robot_state_publisher" type="state_publisher"
20 name="robot_state_publisher" output="screen">
21 <param name="publish_frequency" type="double" value="50.0" />
22 <param name="tf_prefix" type="string" value="" />
23 </node>
24 </launch>
We use it to start Gazebo via roslaunch like this:
$ roslaunch start_pr2_gazebo.launch
you should see something like this
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 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):
- 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
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
If necessary, add the direcory to your ROS_PACKAGE_PATH environment variable. E.g.
$ export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:`pwd`
Populate the direcory with files. You can use 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
- 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:
RobotState class
Robot class
Joint class
1 bool TutorialController::
2 init(pr2_mechanism::RobotState *robot, const ros::NodeHandle &n)
3 {
4 fprintf(stderr, "TutorialController::init()\n");
5
6 typedef std::vector<pr2_mechanism::Joint*> jointvec_t;
7 jointvec_t const & jvec(robot->model_->joints_);
8 for (jointvec_t::const_iterator ij(jvec.begin());
9 ij != jvec.end();
10 ++ij)
11 {
12 fprintf(stderr,
13 " n: %s\tt: %d\tmin: %g\tmax: %g\n",
14 (*ij)->name_.c_str(),
15 (*ij)->type_,
16 (*ij)->joint_limit_min_,
17 (*ij)->joint_limit_max_);
18 }
19
20 return true;
21 }
Unpack the 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
- Launch Gazebo:
$ roslaunch start_pr2_gazebo.launch
- 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.
Unpack 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
- 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.
- Build
$ make
- Launch Gazebo
$ roslaunch start_pr2_gazebo.launch
- 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).
