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.

Teleop PR2 arm in simulation

Start Simulator

The first step is to start the simulator. We'll use the standard pr2_gazebo simulator, so launch:

$ roscore
$ roslaunch pr2_gazebo pr2_empty_world.launch

and the simulator with a pr2 in it should appear

Cartesian Controller

We'll use the JTTeleop controller, available in the pr2_cockpit stack. This controller uses multiple parameters, which we can set in a configuration file and then load with rosparam. However, we'll also need to stop the currently running trajectory controllers, so make a launch file jtteleop.launch:

<launch>
 <node name="stop_r_arm"
    pkg="pr2_controller_manager" type="pr2_controller_manager"
    args="stop r_arm_controller" />

 <rosparam ns="r_cart">
   type: JTTeleopController
   root_name: torso_lift_link
   tip_name: r_gripper_tool_frame
   k_posture: 25.0
   jacobian_inverse_damping: 0.01
   pose_command_filter: 0.01
   cart_gains:
     trans:
       p: 800.0
       d: 15.0
     rot:
       p: 80.0
       d: 1.2
   joint_feedforward:
     r_shoulder_pan_joint: 3.33
     r_shoulder_lift_joint: 1.16
     r_upper_arm_roll_joint: 0.1
     r_elbow_flex_joint: 0.25
     r_forearm_roll_joint: 0.133
     r_wrist_flex_joint: 0.0727
     r_wrist_roll_joint: 0.0727
   joint_max_effort:
     r_shoulder_pan_joint: 11.88
     r_shoulder_lift_joint: 11.64
     r_upper_arm_roll_joint: 6.143
     r_elbow_flex_joint: 6.804
     r_forearm_roll_joint: 8.376
     r_wrist_flex_joint: 5.568
     r_wrist_roll_joint: 5.568

   vel_saturation_trans: 2.0
   vel_saturation_rot: 4.0
 </rosparam>
<node name="spawn_cart"
       pkg="pr2_controller_manager" type="spawner"
       args="r_cart" />
</launch>

Note: For Electric onwards, the controller has moved to a different package.
Change the controller type to:
type: robot_mechanism_controllers/JTCartesianController

This file stops the current right-arm controller, loads in the parameters for our cartesian controller, and then spawns it.

Save this file and launch it:

$ roslaunch jtteleop.launch

Control

The JTTeleop controller uses geometry_msgs/PoseStamped messages to control the end effector of the right arm. We'll whip up a small keyboard teleop client to send this out, a modified version of the standard base teleop client.

First, make a package with the necessary dependencies

$ roscreate-pkg teleop_arms geometry_msgs roscpp

Next, edit the CMakeLists.txt to make the executable we'll be writing:

cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rosbuild_init()
rosbuild_add_executable(teleop_pr2_arms_keyboard src/teleop_pr2_arms_keyboard.cpp)

Now, open up src/teleop_pr2_arms_keyboard.cpp in your favorite editor, and paste

   1 #include <termios.h>
   2 #include <signal.h>
   3 #include <stdio.h>
   4 #include <stdlib.h>
   5 
   6 #include <ros/ros.h>
   7 #include <geometry_msgs/PoseStamped.h>
   8 
   9 #define KEYCODE_A 0x61
  10 #define KEYCODE_D 0x64
  11 #define KEYCODE_S 0x73
  12 #define KEYCODE_W 0x77
  13 #define KEYCODE_Q 0x71
  14 #define KEYCODE_E 0x65
  15 
  16 class TeleopPR2Keyboard
  17 {
  18   private:
  19   geometry_msgs::PoseStamped cmd;
  20 
  21   ros::NodeHandle n_;
  22   ros::Publisher pose_pub_;
  23 
  24   public:
  25   void init()
  26   {
  27     //header - this is impt
  28     cmd.header.frame_id = "/torso_lift_link";
  29 
  30     //Clear out our cmd - these values are roundabout initials
  31     cmd.pose.position.x=0.9;
  32     cmd.pose.position.y=0.2;
  33     cmd.pose.position.z=-0.3;
  34     cmd.pose.orientation.x=-0.00244781865415;
  35     cmd.pose.orientation.y=-0.548220284495;
  36     cmd.pose.orientation.z=0.00145617884538;
  37     cmd.pose.orientation.w=0.836329126239;
  38 
  39     pose_pub_ = n_.advertise<geometry_msgs::PoseStamped>("r_cart/command_pose", 1);
  40 
  41     ros::NodeHandle n_private("~");
  42   }
  43 
  44   ~TeleopPR2Keyboard()   { }
  45   void keyboardLoop();
  46 };
  47 
  48 int kfd = 0;
  49 struct termios cooked, raw;
  50 
  51 void quit(int sig)
  52 {
  53   tcsetattr(kfd, TCSANOW, &cooked);
  54   exit(0);
  55 }
  56 
  57 int main(int argc, char** argv)
  58 {
  59   ros::init(argc, argv, "pr2_arms_keyboard");
  60 
  61   TeleopPR2Keyboard tpk;
  62   tpk.init();
  63 
  64   signal(SIGINT,quit);
  65 
  66   tpk.keyboardLoop();
  67 
  68   return(0);
  69 }
  70 
  71 void TeleopPR2Keyboard::keyboardLoop()
  72 {
  73   char c;
  74   bool dirty=false;
  75 
  76   // get the console in raw mode
  77   tcgetattr(kfd, &cooked);
  78   memcpy(&raw, &cooked, sizeof(struct termios));
  79   raw.c_lflag &=~ (ICANON | ECHO);
  80   // Setting a new line, then end of file
  81   raw.c_cc[VEOL] = 1;
  82   raw.c_cc[VEOF] = 2;
  83   tcsetattr(kfd, TCSANOW, &raw);
  84 
  85   puts("Reading from keyboard");
  86   puts("---------------------------");
  87   puts("Use 'WS' to forward/back");
  88   puts("Use 'AD' to left/right");
  89   puts("Use 'QE' to up/down");
  90 
  91   for(;;)
  92   {
  93     // get the next event from the keyboard
  94     if(read(kfd, &c, 1) < 0)
  95     {
  96       perror("read():");
  97       exit(-1);
  98     }
  99 
 100     switch(c)
 101     {
 102       // Walking
 103     case KEYCODE_W:
 104       cmd.pose.position.x = cmd.pose.position.x+0.1;
 105       dirty = true;
 106       break;
 107     case KEYCODE_S:
 108       cmd.pose.position.x = cmd.pose.position.x-0.1;
 109       dirty = true;
 110       break;
 111     case KEYCODE_A:
 112       cmd.pose.position.y = cmd.pose.position.y+0.1;
 113       dirty = true;
 114       break;
 115     case KEYCODE_D:
 116       cmd.pose.position.y = cmd.pose.position.y-0.1;
 117       dirty = true;
 118       break;
 119     case KEYCODE_Q:
 120       cmd.pose.position.z = cmd.pose.position.z+0.1;
 121       dirty = true;
 122       break;
 123     case KEYCODE_E:
 124       cmd.pose.position.z = cmd.pose.position.z-0.1;
 125       dirty = true;
 126       break;
 127 
 128     }
 129 
 130 
 131     if (dirty == true)
 132     {
 133       pose_pub_.publish(cmd);
 134     }
 135 
 136 
 137   }
 138 }

Make the program with

$ rosmake teleop_arms

and then run

$ rosrun teleop_arms teleop_pr2_arms_keyboard

for cool keyboard control of the endpoint of the robot's right arm.

Explained

This code builds a PoseStamped message for sending to the JTTeleop controller on /r_cart/command_pose. For each keypress, the commanded x,y,or z positions of the robot's gripper (tip_name in the parameters) is updated. It is vitally necessary to set the frame_id in the header of the message to match the root_name of the JTTeleop parameters.