## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links
## note.0=
## descriptive title for the tutorial
## title = Teleop PR2 arm in simulation
## multi-line description to be displayed in search
## description = This tutorial will give you keyboard teleop control of the PR2 arm
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for
## level=BeginnerCategory
## keywords =
####################################
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

= 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. <<BR>>
Change the controller type to: <<BR>>
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

{{{#!cplusplus
#include <termios.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

#define KEYCODE_A 0x61
#define KEYCODE_D 0x64
#define KEYCODE_S 0x73
#define KEYCODE_W 0x77
#define KEYCODE_Q 0x71
#define KEYCODE_E 0x65

class TeleopPR2Keyboard
{
  private:
  geometry_msgs::PoseStamped cmd;

  ros::NodeHandle n_;
  ros::Publisher pose_pub_;

  public:
  void init()
  {
    //header - this is impt
    cmd.header.frame_id = "/torso_lift_link";

    //Clear out our cmd - these values are roundabout initials
    cmd.pose.position.x=0.9;
    cmd.pose.position.y=0.2;
    cmd.pose.position.z=-0.3;
    cmd.pose.orientation.x=-0.00244781865415;
    cmd.pose.orientation.y=-0.548220284495;
    cmd.pose.orientation.z=0.00145617884538;
    cmd.pose.orientation.w=0.836329126239;

    pose_pub_ = n_.advertise<geometry_msgs::PoseStamped>("r_cart/command_pose", 1);

    ros::NodeHandle n_private("~");
  }

  ~TeleopPR2Keyboard()   { }
  void keyboardLoop();
};

int kfd = 0;
struct termios cooked, raw;

void quit(int sig)
{
  tcsetattr(kfd, TCSANOW, &cooked);
  exit(0);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "pr2_arms_keyboard");

  TeleopPR2Keyboard tpk;
  tpk.init();

  signal(SIGINT,quit);

  tpk.keyboardLoop();

  return(0);
}

void TeleopPR2Keyboard::keyboardLoop()
{
  char c;
  bool dirty=false;

  // get the console in raw mode
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);

  puts("Reading from keyboard");
  puts("---------------------------");
  puts("Use 'WS' to forward/back");
  puts("Use 'AD' to left/right");
  puts("Use 'QE' to up/down");

  for(;;)
  {
    // get the next event from the keyboard
    if(read(kfd, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }

    switch(c)
    {
      // Walking
    case KEYCODE_W:
      cmd.pose.position.x = cmd.pose.position.x+0.1;
      dirty = true;
      break;
    case KEYCODE_S:
      cmd.pose.position.x = cmd.pose.position.x-0.1;
      dirty = true;
      break;
    case KEYCODE_A:
      cmd.pose.position.y = cmd.pose.position.y+0.1;
      dirty = true;
      break;
    case KEYCODE_D:
      cmd.pose.position.y = cmd.pose.position.y-0.1;
      dirty = true;
      break;
    case KEYCODE_Q:
      cmd.pose.position.z = cmd.pose.position.z+0.1;
      dirty = true;
      break;
    case KEYCODE_E:
      cmd.pose.position.z = cmd.pose.position.z-0.1;
      dirty = true;
      break;

    }


    if (dirty == true)
    {
      pose_pub_.publish(cmd);
    }


  }
}
}}}
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.

## AUTOGENERATED DO NOT DELETE
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE