## page was renamed from pr2/Tutorials/Moving the arm through a Cartesian pose trajectory
## page was renamed from pr2/Tutorials/Moving the arm through a Cartesian pose trajectory using inverse kinematics and the joint trajectory action
## page was renamed from pr2/Tutorials/Commanding a Cartesian pose trajectory using inverse kinematics and the joint trajectory action
## 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= [[pr2_kinematics/Tutorials/Tutorial 5|Collision free inverse kinematics for the PR2 arms]] and [[pr2_controllers/Tutorials/Moving the arm using the Joint Trajectory Action|Moving the arm using the Joint Trajectory Action]]
## descriptive title for the tutorial
## title = Moving the arm through a Cartesian pose trajectory using inverse kinematics and the joint trajectory action
## multi-line description to be displayed in search 
## description = This tutorial teaches you how to move the arm through Cartesian pose trajectories using inverse kinematics and low-level joint controllers (the [[joint_trajectory_action]]).
## 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= IntermediateCategory
## keywords = moving move arm ik inverse kinematics cartesian pose gripper low-level
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

If your goal is to move the arm along a '''pre-specified joint trajectory''', without needing inverse kinematics, please see [[pr2_controllers/Tutorials/Moving the arm using the Joint Trajectory Action|Moving the arm using the Joint Trajectory Action]].

=== Overview ===

In this tutorial we will focus on moving the arm through sequences of Cartesian poses using inverse kinematics and an '''action interface''' to the low-level '''Joint Trajectory Controller'''. Note that, at this level, we are not concerned with many aspects of arm movement, such as:
 * generation of trajectories with smooth velocity and acceleration profiles
 * obstacle avoidance or motion planning
Those components can be found in a higher level package called [[move_arm]]. However, if [[move_arm]] is not available, or you want to be using a lower-level interface, this tutorial shows the way. 

If you just want to move the arm through a joint angle sequence (rather than a Cartesian pose sequence) using a lower-level interface than [[move_arm]], see [[pr2_controllers/Tutorials/Moving the arm using the Joint Trajectory Action|Moving the arm using the Joint Trajectory Action]]

Before you begin, bring up a robot, either [[WgWiki:/PR2/StartRobot|on the hardware]] or [[simulator_gazebo/Tutorials/StartingGazebo|in gazebo]].

=== The Joint Trajectory Controller ===

The arm 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 `r_arm_controller` . If you find:
 * `r_arm_controller (running)` : the controller is running; you can skip to the next section of this tutorial
 * `r_arm_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 r_arm_controller
}}}

 * no mention of `r_arm_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 head trajectory controller or the gripper controller.


==== Package setup ====

We will now create a service that takes in sequences of desired Cartesian poses for the PR2 gripper, uses inverse kinematics to convert those Cartesian poses to arm joint angles, and then asks the [[joint_trajectory_action]] to execute the resulting sequence of joint angles. 

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 ik_trajectory_tutorial actionlib roscpp joint_trajectory_action geometry_msgs kinematics_msgs pr2_controllers_msgs tf
}}}

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

{{{
roscd ik_trajectory_tutorial
}}}

==== Creating the service message ====

Now we need to define a service message that we can use to input sequences of Cartesian poses, and get back some feedback as to whether the trajectory succeeded.  
We will use a single header to tell us what frame our poses will be in, and an array of [[CodeAPI:geometry_msgs/html/msg/Pose.html|Pose]] messages to specify the position and orientation of each waypoint.

Put the following into '''srv/ExecuteCartesianIKTrajectory.srv''':
{{{
Header header
geometry_msgs/Pose[] poses
---
uint32 success
}}}

Now uncomment the line 
{{{
rosbuild_gensrv()
}}}
in your CMakeLists.txt and type 'make' in the ik_trajectory_tutorial directory to generate the service message files.

==== Creating the service node ====

Put the following into '''src/ik_trajectory_tutorial.cpp''':

{{{
#!cplusplus block=ik_trajectory_tutorial
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <ros/ros.h>
#include <pr2_controllers_msgs/JointTrajectoryAction.h>
#include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
#include <actionlib/client/simple_action_client.h>
#include <kinematics_msgs/GetPositionIK.h>
#include <kinematics_msgs/GetPositionFK.h>
#include <ik_trajectory_tutorial/ExecuteCartesianIKTrajectory.h>
#include <vector>

#define MAX_JOINT_VEL 0.5  //in radians/sec

static const std::string ARM_IK_NAME = "/pr2_right_arm_kinematics/get_ik";
typedef actionlib::SimpleActionClient<pr2_controllers_msgs::
                                      JointTrajectoryAction> TrajClient;

class IKTrajectoryExecutor{

private:
  ros::NodeHandle node;
  ros::ServiceClient ik_client;
  ros::ServiceServer service;
  pr2_controllers_msgs::JointTrajectoryGoal goal;
  kinematics_msgs::GetPositionIK::Request  ik_request;
  kinematics_msgs::GetPositionIK::Response ik_response;  
  TrajClient *action_client;

public:
  IKTrajectoryExecutor(){

    //create a client function for the IK service
    ik_client = node.serviceClient<kinematics_msgs::GetPositionIK>
      (ARM_IK_NAME, true);    

    //wait for the various services to be ready
    ROS_INFO("Waiting for services to be ready");
    ros::service::waitForService(ARM_IK_NAME);
    ROS_INFO("Services ready");

    //tell the joint trajectory action client that we want 
    //to spin a thread by default
    action_client = new TrajClient("r_arm_controller/joint_trajectory_action", true);

    //wait for the action server to come up
    while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
    }

    //register a service to input desired Cartesian trajectories
    service = node.advertiseService("execute_cartesian_ik_trajectory", 
        &IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);

    //have to specify the order of the joints we're sending in our 
    //joint trajectory goal, even if they're already on the param server
    goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
    goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
    goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
    goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
    goal.trajectory.joint_names.push_back("r_wrist_roll_joint");

  }

  ~IKTrajectoryExecutor(){
    delete action_client;
  }


  //run inverse kinematics on a PoseStamped (7-dof pose 
  //(position + quaternion orientation) + header specifying the 
  //frame of the pose)
  //tries to stay close to double start_angles[7]
  //returns the solution angles in double solution[7] 
  bool run_ik(geometry_msgs::PoseStamped pose, double start_angles[7], 
              double solution[7], std::string link_name){

    kinematics_msgs::GetPositionIK::Request  ik_request;
    kinematics_msgs::GetPositionIK::Response ik_response;  
 
    ik_request.timeout = ros::Duration(5.0);
ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_shoulder_pan_joint");
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_shoulder_lift_joint");
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_upper_arm_roll_joint");
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_elbow_flex_joint");
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_forearm_roll_joint");
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_wrist_flex_joint");
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_wrist_roll_joint");

    ik_request.ik_request.ik_link_name = link_name; 

    ik_request.ik_request.pose_stamped = pose;
    ik_request.ik_request.ik_seed_state.joint_state.position.resize(7);

    for(int i=0; i<7; i++) ik_request.ik_request.ik_seed_state.joint_state.position[i] = start_angles[i];

    ROS_INFO("request pose: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);

    bool ik_service_call = ik_client.call(ik_request,ik_response);
    if(!ik_service_call){
      ROS_ERROR("IK service call failed!");  
      return 0;
    }
    if(ik_response.error_code.val == ik_response.error_code.SUCCESS){
      for(int i=0; i<7; i++){
        solution[i] = ik_response.solution.joint_state.position[i];
      }
      ROS_INFO("solution angles: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", 
             solution[0],solution[1], solution[2],solution[3],
             solution[4],solution[5],solution[6]);
      ROS_INFO("IK service call succeeded");
      return 1;
    }
    ROS_INFO("IK service call error code: %d", ik_response.error_code.val);
    return 0;
  }


  //figure out where the arm is now  
  void get_current_joint_angles(double current_angles[7]){
    int i;

    //get a single message from the topic 'r_arm_controller/state'
    pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr state_msg = 
      ros::topic::waitForMessage<pr2_controllers_msgs::JointTrajectoryControllerState>
      ("r_arm_controller/state");
    
    //extract the joint angles from it
    for(i=0; i<7; i++){
      current_angles[i] = state_msg->actual.positions[i];
    }
  }


  //send a desired joint trajectory to the joint trajectory action
  //and wait for it to finish
  bool execute_joint_trajectory(std::vector<double *> joint_trajectory){
    int i, j; 
    int trajectorylength = joint_trajectory.size();

    //get the current joint angles
    double current_angles[7];    
    get_current_joint_angles(current_angles);

    //fill the goal message with the desired joint trajectory
    goal.trajectory.points.resize(trajectorylength+1);

    //set the first trajectory point to the current position
    goal.trajectory.points[0].positions.resize(7);
    goal.trajectory.points[0].velocities.resize(7);
    for(j=0; j<7; j++){
      goal.trajectory.points[0].positions[j] = current_angles[j];
      goal.trajectory.points[0].velocities[j] = 0.0; 
    }

    //make the first trajectory point start 0.25 seconds from when we run
    goal.trajectory.points[0].time_from_start = ros::Duration(0.25);     

    //fill in the rest of the trajectory
    double time_from_start = 0.25;
    for(i=0; i<trajectorylength; i++){
      goal.trajectory.points[i+1].positions.resize(7);
      goal.trajectory.points[i+1].velocities.resize(7);

      //fill in the joint positions (velocities of 0 mean that the arm
      //will try to stop briefly at each waypoint)
      for(j=0; j<7; j++){
        goal.trajectory.points[i+1].positions[j] = joint_trajectory[i][j];
        goal.trajectory.points[i+1].velocities[j] = 0.0;
      }

      //compute a desired time for this trajectory point using a max 
      //joint velocity
      double max_joint_move = 0;
      for(j=0; j<7; j++){
        double joint_move = fabs(goal.trajectory.points[i+1].positions[j] 
                                 - goal.trajectory.points[i].positions[j]);
        if(joint_move > max_joint_move) max_joint_move = joint_move;
      }
      double seconds = max_joint_move/MAX_JOINT_VEL;
      ROS_INFO("max_joint_move: %0.3f, seconds: %0.3f", max_joint_move, seconds);
      time_from_start += seconds;
      goal.trajectory.points[i+1].time_from_start = 
        ros::Duration(time_from_start);
    }

    //when to start the trajectory
    goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.25);

    ROS_INFO("Sending goal to joint_trajectory_action");
    action_client->sendGoal(goal);

    action_client->waitForResult();

    //get the current joint angles for debugging
    get_current_joint_angles(current_angles);
    ROS_INFO("joint angles after trajectory: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f\n",current_angles[0],current_angles[1],current_angles[2],current_angles[3],current_angles[4],current_angles[5],current_angles[6]);

    if(action_client->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
      ROS_INFO("Hooray, the arm finished the trajectory!");
      return 1;
    }
    ROS_INFO("The arm failed to execute the trajectory.");
    return 0;
  }


  //service function for execute_cartesian_ik_trajectory
  bool execute_cartesian_ik_trajectory(
         ik_trajectory_tutorial::ExecuteCartesianIKTrajectory::Request &req,
         ik_trajectory_tutorial::ExecuteCartesianIKTrajectory::Response &res){

    int trajectory_length = req.poses.size();
    int i, j;
    
    //IK takes in Cartesian poses stamped with the frame they belong to
    geometry_msgs::PoseStamped stamped_pose;
    stamped_pose.header = req.header;
    stamped_pose.header.stamp = ros::Time::now();
    bool success;
    std::vector<double *> joint_trajectory;

    //get the current joint angles (to find ik solutions close to)
    double last_angles[7];    
    get_current_joint_angles(last_angles);

    //find IK solutions for each point along the trajectory 
    //and stick them into joint_trajectory
    for(i=0; i<trajectory_length; i++){
      
      stamped_pose.pose = req.poses[i];
      double *trajectory_point = new double[7];
      success = run_ik(stamped_pose, last_angles, trajectory_point, "r_wrist_roll_link");
      joint_trajectory.push_back(trajectory_point);

      if(!success){
        ROS_ERROR("IK solution not found for trajectory point number %d!\n", i);
        return 0;
      }
      for(j=0; j<7; j++) last_angles[j] = trajectory_point[j];
    }        

    //run the resulting joint trajectory
    ROS_INFO("executing joint trajectory");
    success = execute_joint_trajectory(joint_trajectory);
    res.success = success;
    
    return success;
  }

};


int main(int argc, char** argv){

  //init ROS node
  ros::init(argc, argv, "cartesian_ik_trajectory_executor");

  IKTrajectoryExecutor ik_traj_exec = IKTrajectoryExecutor();

  ROS_INFO("Waiting for cartesian trajectories to execute");
  ros::spin();
  
  return 0;
}
}}}


Now we'll go through subsets of the code more carefully.

<<CodeRef(ik_trajectory_tutorial,23,54)>>
Here we are creating clients for the services and actions we'll be using: pr2_arm_navigation_kinematics's /pr2_right_arm_kinematics/get_ik for inverse kinematics, and the joint_trajectory_action's JointTrajectoryAction to execute joint trajectories.  We also advertise our own service to take in desired Cartesian pose sequences, execute_cartesian_ik_trajectory.

<<CodeRef(ik_trajectory_tutorial,72,118)>>
A function to perform inverse kinematics, much as used/described in the [[pr2_kinematics/Tutorials/Tutorial 5|Collision free inverse kinematics for the PR2 arms]]tutorial.

<<CodeRef(ik_trajectory_tutorial,121,134)>>
This function gets the current right arm angles from the state being broadcast by the r_arm_controller, by listening for a single message.

<<CodeRef(ik_trajectory_tutorial,137,207)>>
Because the [[joint_trajectory_action]] assumes that we will input not only our desired Cartesian poses but also the time at which we would like the arm to be at each waypoint, we need to compute reasonable times for each waypoint.  In this code we do so by enforcing a maximum joint speed.  For waypoints after the first, we can compute the maximum change in joint angles (after IK is run) on the two segment endpoints.  However, because the first waypoint we ask for could be arbitrarily far away from our current arm position, we need to check where the arm is now to decide how long is a reasonable time to use to get there.  

<<CodeRef(ik_trajectory_tutorial,210,251)>>
Our service function.  Takes in a sequence of desired Cartesian poses in the reference frame of the header's frame_id, finds joint angles that bring the arm there using IK, and then feeds the resulting joint angle sequence to the joint_trajectory_action.

==== Building ====

Add the following line to the CMakeLists.txt:

{{{
rosbuild_add_executable(ik_trajectory_tutorial src/ik_trajectory_tutorial.cpp)
}}}

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

==== Test program ====

Because our ik_trajectory_tutorial program runs a service that waits for service calls to come in, we need to write a client program to send it Cartesian trajectories.  

Put the following in scripts/test_ik_trajectory_tutorial.py:
{{{
#!python
#test client for ik_trajectory_tutorial

import roslib
roslib.load_manifest('ik_trajectory_tutorial')
import rospy
from ik_trajectory_tutorial.srv import ExecuteCartesianIKTrajectory
from geometry_msgs.msg import Pose
import time
import sys
import pdb
import tf

#execute a Cartesian trajectory defined by a root frame, a list of 
#positions (x,y,z), and a list of orientations (quaternions: x,y,z,w)
def call_execute_cartesian_ik_trajectory(frame, positions, orientations):
    rospy.wait_for_service("execute_cartesian_ik_trajectory")

    #fill in the header (don't need seq)
    header = rospy.Header()
    header.frame_id = frame
    header.stamp = rospy.get_rostime()

    #fill in the poses
    poses = []
    for (position, orientation) in zip(positions, orientations):
        pose = Pose()
        pose.position.x = position[0]
        pose.position.y = position[1]
        pose.position.z = position[2]
        pose.orientation.x = orientation[0]
        pose.orientation.y = orientation[1]
        pose.orientation.z = orientation[2]
        pose.orientation.w = orientation[3]
        poses.append(pose)

    #call the service to execute the trajectory
    print "calling execute_cartesian_ik_trajectory"
    try:
        s = rospy.ServiceProxy("execute_cartesian_ik_trajectory", \
                                   ExecuteCartesianIKTrajectory)
        resp = s(header, poses)
    except rospy.ServiceException, e:
        print "error when calling execute_cartesian_ik_trajectory: %s"%e
        return 0
    return resp.success

#pretty-print list to string
def pplist(list):
    return ' '.join(['%2.3f'%x for x in list])

#print out the positions, velocities, and efforts of the right arm joints
if __name__ == "__main__":
    rospy.init_node("test_cartesian_ik_trajectory_executer")
    tf_listener = tf.TransformListener()
    time.sleep(.5) #give the transform listener time to get some frames

    # not needed, fix tutorial
    joint_names = ["r_shoulder_pan_joint",
                   "r_shoulder_lift_joint",
                   "r_upper_arm_roll_joint",
                   "r_elbow_flex_joint",
                   "r_forearm_roll_joint",
                   "r_wrist_flex_joint",
                   "r_wrist_roll_joint"]

    positions = [[.76, -.19, .83], [0.59, -0.36, 0.93]]
    orientations = [[.02, -.09, 0.0, 1.0], [0.65, -0.21, .38, .62]]

    success = call_execute_cartesian_ik_trajectory("/base_link", \
            positions, orientations)

    #check the final pose
    (trans, rot) = tf_listener.lookupTransform('base_link', 'r_wrist_roll_link', rospy.Time(0))
    print "end Cartesian pose: trans", pplist(trans), "rot", pplist(rot)

    if success:
        print "trajectory succeeded!"
    else:
        print "trajectory failed."
}}}

==== Launching ====

Before we can run, we need to launch a couple things: the IK services and the joint_states_listener.

Put the following into launch/ik_trajectory_tutorial.launch:
{{{
<launch>
  <!-- load ik -->
  <include file="$(find pr2_arm_navigation_kinematics)/launch/pr2_ik_rarm_node.launch"/>
</launch>
}}}

==== Running ====

 * Bring up a robot, either [[pr2_robot/Tutorials/Starting up the PR2|on the hardware]] or [[pr2_simulator/Tutorials/StartingPR2Simulation|in gazebo]]. 
 * Run [[rxconsole]], to see any errors that occur.
 * Run the launch file:
{{{
roslaunch launch/ik_trajectory_tutorial.launch
}}}
 * Run the ik_trajectory_tutorial service:
{{{
bin/ik_trajectory_tutorial
}}}
 * Run the test program:
{{{
python scripts/test_ik_trajectory_tutorial.py
}}}

If the test succeeds, you should see "Hooray, the arm finished the trajectory!" and the end pose should be close to the desired Cartesian position for our second waypoint.

Instead of writing the Python test client test_ik_trajectory_tutorial.py, you may as well use the command line for the same effect:
{{{
rosservice call /execute_cartesian_ik_trajectory -- "{header: { frame_id: /base_link}, poses: [{position: [0.76, -0.19, 0.83], orientation:[0.02, -0.09, 0.0, 1.0]}, {position: [0.59, -0.36, 0.93], orientation: [0.65, -0.21, .38, .62]}]}"
}}}

## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## JointTrajectoryActionTutorial