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.

Moving the torso

Overview and prerequisites

This tutorial shows how to move the torso of the PR-2 robot up and down, i.e., make the robot taller or shorter.

Commanding the torso to move up and down requires three components:

• a trajectory controller that directly sends commands to the joint motors;
• an action node that accepts as a command a desired position for the torso, and converts that to low-level commands for the controller;
• the higher level program that sends desired positions to the action node.

The first two of these components are available in ROS. In this tutorial, we will show you how to use them by writing the third component, the higher level program.

Before you begin, bring up a robot, either on the hardware or in gazebo.

Package setup

In order to create a ROS node that sends goals to the torso action, the first thing we'll need to do is create a package. To do this we'll use the handy roscreate-pkg command where we want to create the package directory:

roscreate-pkg simple_torso roscpp actionlib pr2_controllers_msgs

After this is done we'll need to roscd to the package we created, since we'll be using it as our workspace.

roscd simple_torso

Running a robot

Bring up a robot, either on the hardware or in gazebo.

The torso action and trajectory controller

The torso trajectory controller is brought up on robot start-up. In general, a controller can be in one of three states:

• not loaded
• loaded, but not running (stopped)
• running

In general, you can use the pr2_controller_manager to check which controllers are available. After you bring up the robot, use the following command:

rosrun pr2_controller_manager pr2_controller_manager list

In the list that is printed, look for a line starting with torso_controller . If you find:

• torso_controller (running) : the controller is running; you can skip to the next section of this tutorial

• torso_controller (stopped) : the controller is loaded, but not running. To start it, use again the pr2_controller_manager :

  rosrun pr2_controller_manager pr2_controller_manager start torso_controller

• no mention of torso_controller : the controller has not been loaded; this probably means that something went wrong during robot start-up. Abort this tutorial and investigate the cause for that.

Note that this usage of the pr2_controller_manager also applies to other controllers, such as the arm trajectory controller or the gripper controller.

The action node

The action node is also brought up automatically on robot start-up. To check for it, you can look at the list of active nodes in the torso_controller namespace:

rosnode list torso_controller

Look for the line /torso_controller/position_joint_action_node . If you don't find it, the action node was not brought up at robot start-up. Abort this tutorial and investigate the cause for that.

The torso action goal

The torso action takes in messages of type pr2_controllers_msgs/SingleJointPositionGoal, which has two float64 fields, 'position' and 'max_velocity', and one ROS duration field, 'min_duration.' The 'position' field specifies the desired torso position in meters (all the way down is 0.0, and all the way up is approximately 0.6). The 'max_velocity' field and the 'min_duration' field limit how fast the torso will get to its position goal, by specifying either a limit on the velocity to use while moving to that position (m/s), or a minimum amount of time (seconds) to use to get there. Either 'max_velocity' or 'min_duration' can be left unspecified, in which case they will be ignored. If both are left unspecified, the torso will move to the desired location as fast as possible.

Creating the action client

Now we will make an action client node that lifts and lowers the torso while limiting the speed.

Put the following into src/simple_torso.cpp:

   1 #include <ros/ros.h>
   2 #include <pr2_controllers_msgs/SingleJointPositionAction.h>
   3 #include <actionlib/client/simple_action_client.h>
   4 
   5 // Our Action interface type, provided as a typedef for convenience
   6 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::SingleJointPositionAction> TorsoClient;
   7 
   8 class Torso{
   9 private:
  10   TorsoClient *torso_client_;
  11 
  12 public:
  13   //Action client initialization
  14   Torso(){
  15     
  16     torso_client_ = new TorsoClient("torso_controller/position_joint_action", true);
  17 
  18     //wait for the action server to come up
  19     while(!torso_client_->waitForServer(ros::Duration(5.0))){
  20       ROS_INFO("Waiting for the torso action server to come up");
  21     }
  22   }
  23 
  24   ~Torso(){
  25     delete torso_client_;
  26   }
  27 
  28   //tell the torso to go up
  29   void up(){
  30 
  31     pr2_controllers_msgs::SingleJointPositionGoal up;
  32     up.position = 0.195;  //all the way up is 0.2
  33     up.min_duration = ros::Duration(2.0);
  34     up.max_velocity = 1.0;
  35     
  36     ROS_INFO("Sending up goal");
  37     torso_client_->sendGoal(up);
  38     torso_client_->waitForResult();
  39   }
  40 
  41   //tell the torso to go down
  42   void down(){
  43 
  44     pr2_controllers_msgs::SingleJointPositionGoal down;
  45     down.position = 0.0;
  46     down.min_duration = ros::Duration(2.0);
  47     down.max_velocity = 1.0;
  48 
  49     ROS_INFO("Sending down goal");
  50     torso_client_->sendGoal(down);
  51     torso_client_->waitForResult();
  52   }    
  53 };
  54 
  55 int main(int argc, char** argv){
  56   ros::init(argc, argv, "simple_trajectory");
  57 
  58   Torso torso;
  59   
  60   torso.up();
  61   torso.down();
  62 
  63   return 0;
  64 }

Building

Add the following line to the CMakeLists.txt:

rosbuild_add_executable(simple_torso src/simple_torso.cpp)

and make the binary by typing 'make' in the simple_torso directory.

Running

• Bring up gazebo or a robot.

• Start the ROS console (rxconsole) to view any errors that occur.

• Make sure the torso controller and torso action are running.
• Run the simple_torso program:

bin/simple_torso

You should see the robot move up and down.