Tutorials/drcsim/changing controller configuration


 * 1) DRC Tutorial: Changing controllers

You should first go through the whole-body trajectory control tutorial.

This tutorial will demonstrate how to change the controller configuration that's running on the DRC robot. In the course of this tutorial, we'll write a new roslaunch file to bring up the simulation with a specific controller configuration.

Controllers are loaded by the pr2_controller_manager, which is in turn loaded as part of a Gazebo plugin. This arrangement allows the controller to run in-line with the simulation, approximating the on-robot situation in which the controller runs in a real-time environment. We can use most of the ROS robot mechanism controllers, which have been used to control a variety of robots, in both hardware and simulation.

Important note: The approach to robot control described here is not the best or only way to control the DRC robot. It is provided for demonstration purposes. DRC participants should be aware that we expect the control system in simulation to change substantially as more sophisticated controller become available.


 * 1) Install DRC Simulator ##

Click to see the instructions for installing the DRC simulator and associated utilities.


 * 1) Create a new ROS package ##

If you haven't already, create a ros directory in your home directory and add it to your `$ROS_PACKAGE_PATH`. From the command line

mkdir ~/ros echo "export ROS_PACKAGE_PATH=~/ros:\$ROS_PACKAGE_PATH" >> ~/.bashrc source ~/.bashrc

If you haven't already, install `rviz` (this is a temporary requirement and will go away):

sudo apt-get install ros-fuerte-visualization

Use roscreate-pkg to create a ROS package for this tutorial, depending on a ROS package called `drc_robot_utils`:

cd ~/ros roscreate-pkg change_controller_tutorial drc_robot_utils

Create a `launch` directory in this package:

roscd change_controller_tutorial mkdir launch


 * 1) The Code ##

Move to the directory trajectory_control_tutorial/src roscd change_controller_tutorial/launch

Add a file called mysim.launch:

gedit mysim.launch

Copy the following code into it:







 



<node name="drc_robot_leg_torso_position_controllers_spawner" pkg="pr2_controller_manager" type="spawner" output="screen" args="--wait-for=/calibrated l_leg_kny_position_controller r_leg_kny_position_controller l_leg_lax_position_controller r_leg_lax_position_controller l_leg_lhy_position_controller r_leg_lhy_position_controller l_leg_mhx_position_controller r_leg_mhx_position_controller l_leg_uay_position_controller r_leg_uay_position_controller l_leg_uhz_position_controller r_leg_uhz_position_controller neck_ay_position_controller back_lbz_position_controller back_mby_position_controller back_ubx_position_controller" />


 * 1) Trying it out ##

If you haven't brought down your previous instance of the DRC simulator, kill the process by pressing Control + C in its terminal. Now launch the file you just wrote:

roslaunch change_controller_tutorial mysim.launch

You should see the DRC robot standing in an empty world. It will likely sway back and forth; that's an artifact of the controllers holding position on the joints.


 * 1) Restarting ###

If the robot falls over, go to the Gazebo "Edit" menu and click on "Reset Model Poses". That will teleport the robot back to its initial pose.


 * 1) The Code Explained ##



Include an underlying launch file that will run Gazebo. This file will bring up the usual DRC robot model in an empty world, without any controllers running.



Include an underlying launch file that will run the pr2_controller_manager, which will in turn load, start, and stop controllers on demand.



Publish fake calibration flags, which are required to satisfy controllers that are written to wait until they've heard from the joints that they're calibrated (common for joints without absolute encoders).



Load a controller configuration file onto the ROS parameter server. This configuration will be retrieved by the controllers themselves later when we spawn them. In this case, we're loading configuration for a ROS Joint Trajectory Action Controller to run on each of the robot's arms. To see the details of the configuration:

roscd drc_robot gedit ros/arm_controllers.yaml

Consult the documentation for that controller for the meanings of the parameters. The names of the controllers in that file are important; they'll be passed as arguments to the controller later in this file. You can create your own controller configuration file in your own package; if you do that, just adjust the `file` attribute of the `rosparam` tag.



Load a controller configuration file as before, but this one defines a ROS Joint Position Controller. To see the details of the configuration:

roscd drc_robot gedit ros/leg_torso_position_controllers.yaml

Consult the documentation to edit.



Spawn the left and right arm controllers. These controllers will fetch their configuration from the parameter server.

<node name="drc_robot_leg_torso_position_controllers_spawner" pkg="pr2_controller_manager" type="spawner" output="screen" args="--wait-for=/calibrated l_leg_kny_position_controller r_leg_kny_position_controller l_leg_lax_position_controller r_leg_lax_position_controller l_leg_lhy_position_controller r_leg_lhy_position_controller l_leg_mhx_position_controller r_leg_mhx_position_controller l_leg_uay_position_controller r_leg_uay_position_controller l_leg_uhz_position_controller r_leg_uhz_position_controller neck_ay_position_controller back_lbz_position_controller back_mby_position_controller back_ubx_position_controller" />

<node name="drc_robot_leg_torso_position_controllers_spawner" pkg="pr2_controller_manager" type="spawner" output="screen" args="--wait-for=/calibrated l_leg_kny_position_controller r_leg_kny_position_controller l_leg_lax_position_controller r_leg_lax_position_controller l_leg_lhy_position_controller r_leg_lhy_position_controller l_leg_mhx_position_controller r_leg_mhx_position_controller l_leg_uay_position_controller r_leg_uay_position_controller l_leg_uhz_position_controller r_leg_uhz_position_controller neck_ay_position_controller back_lbz_position_controller back_mby_position_controller back_ubx_position_controller" />

Spawn the joint position controllers for the rest of the body. These controllers will fetch their configuration from the parameter server.


 * 1) Changing Controllers in Command Line ##

Alternatively, with an instance of DRC robot running in gazebo, one can swap out the controller while simulation is running. To do this, we leverage controller manipulation scripts offered in [pr2_controller_manager](http://ros.org/wiki/pr2_controller_manager) package.

If you haven't done so already, check to see that you have a single instance of DRC robot simulation running (via `roslaunch drc_robot_utils drc_robot.launch`).

To list the currently loaded controllers and their perspective states, invoke the `pr2_controller_manager` script with the `list` command:

$ rosrun pr2_controller_manager pr2_controller_manager list drc_controller ( running )

To stop the controller, do

$ rosrun pr2_controller_manager pr2_controller_manager stop drc_controller Stopped ['drc_controller'] successfully

The robot should become uncontrolled and collapse to the ground. To add new controllers, first add the controller configuration yaml file to the ROS parameter server:

rosparam load `rospack find drc_robot`/ros/leg_torso_position_controllers.yaml rosparam load `rospack find drc_robot`/ros/arm_controllers.yaml

Then load the controllers as defined in the configuration files into the pr2_controller_manager by running the `pr2_controller_manager` script with `load` command:

$ rosrun pr2_controller_manager pr2_controller_manager load l_leg_kny_position_controller r_leg_kny_position_controller l_leg_lax_position_controller r_leg_lax_position_controller l_leg_lhy_position_controller r_leg_lhy_position_controller l_leg_mhx_position_controller r_leg_mhx_position_controller l_leg_uay_position_controller r_leg_uay_position_controller l_leg_uhz_position_controller r_leg_uhz_position_controller neck_ay_position_controller back_lbz_position_controller back_mby_position_controller back_ubx_position_controller Loaded l_leg_kny_position_controller Loaded r_leg_kny_position_controller Loaded l_leg_lax_position_controller Loaded r_leg_lax_position_controller Loaded l_leg_lhy_position_controller Loaded r_leg_lhy_position_controller Loaded l_leg_mhx_position_controller Loaded r_leg_mhx_position_controller Loaded l_leg_uay_position_controller Loaded r_leg_uay_position_controller Loaded l_leg_uhz_position_controller Loaded r_leg_uhz_position_controller Loaded neck_ay_position_controller Loaded back_lbz_position_controller Loaded back_mby_position_controller Loaded back_ubx_position_controller

$ rosrun pr2_controller_manager pr2_controller_manager load l_arm_controller r_arm_controller Loaded l_arm_controller Loaded r_arm_controller

Similarly, to start the controllers, you can use the `start` option:

$ rosrun pr2_controller_manager pr2_controller_manager start l_leg_kny_position_controller r_leg_kny_position_controller l_leg_lax_position_controller r_leg_lax_position_controller l_leg_lhy_position_controller r_leg_lhy_position_controller l_leg_mhx_position_controller r_leg_mhx_position_controller l_leg_uay_position_controller r_leg_uay_position_controller l_leg_uhz_position_controller r_leg_uhz_position_controller neck_ay_position_controller back_lbz_position_controller back_mby_position_controller back_ubx_position_controller Started ['l_leg_kny_position_controller'] successfully Started ['r_leg_kny_position_controller'] successfully Started ['l_leg_lax_position_controller'] successfully Started ['r_leg_lax_position_controller'] successfully Started ['l_leg_lhy_position_controller'] successfully Started ['r_leg_lhy_position_controller'] successfully Started ['l_leg_mhx_position_controller'] successfully Started ['r_leg_mhx_position_controller'] successfully Started ['l_leg_uay_position_controller'] successfully Started ['r_leg_uay_position_controller'] successfully Started ['l_leg_uhz_position_controller'] successfully Started ['r_leg_uhz_position_controller'] successfully Started ['neck_ay_position_controller'] successfully Started ['back_lbz_position_controller'] successfully Started ['back_mby_position_controller'] successfully Started ['back_ubx_position_controller'] successfully

and

$ rosrun pr2_controller_manager pr2_controller_manager start l_arm_controller r_arm_controller

Started ['l_arm_controller'] successfully Started ['r_arm_controller'] successfully