## page was renamed from kinematics_pr2/Tutorials/Tutorial 4
## page was renamed from kinematics_pr2/Tutorials/PR2PositionInverseKinematics
## 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 3| Forward kinematics for the PR2 arms]]
## descriptive title for the tutorial
## title = Inverse kinematics for the PR2 arms
## multi-line description to be displayed in search 
## description = This tutorial will show you how to use a kinematics node to solve the inverse kinematics and get the joint positions for a desired cartesian position of the PR2 arms.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=[[pr2_kinematics/Tutorials/Tutorial 5|Collision free inverse kinematics for the PR2 arms]]
## next.1.link=
## what level user is this tutorial for 
## level= IntermediateCategory
## keywords =
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>
In this tutorial, you will learn how to use the PR2 kinematics node to compute inverse kinematics for the tip link of the PR2 arm.

== ROS Setup ==

To use this tutorial make sure that you've run the following:

{{{
sudo apt-get install ros-electric-pr2-arm-navigation
}}}

Also make sure that in each terminal you use you've run:

{{{
export ROBOT=sim
}}}

== Running in simulation ==
To use the arm planning stack, we first need to launch the simulator. 
{{{
roslaunch pr2_gazebo pr2_empty_world.launch
}}}

The simulator should come up. 

Now, launch the arm navigation stack:
{{{
roslaunch pr2_3dnav right_arm_navigation.launch
}}}

Next, run this executable from the pr2_arm_navigation_tutorials stack:
{{{
rosrun pr2_arm_navigation_tutorials get_ik
}}}

If it succeeds, you should see the following output
{{{
[ INFO] 0.000000000: Joint: r_shoulder_pan_joint -0.330230
[ INFO] 0.000000000: Joint: r_shoulder_lift_joint 0.008300
[ INFO] 0.000000000: Joint: r_upper_arm_roll_joint -1.550000
[ INFO] 0.000000000: Joint: r_elbow_flex_joint -0.859908
[ INFO] 0.000000000: Joint: r_forearm_roll_joint 3.139403
[ INFO] 0.000000000: Joint: r_wrist_flex_joint -0.529580
[ INFO] 0.000000000: Joint: r_wrist_roll_joint -1.591270
}}}

This may fail if the planning scene has not been set.  A blank call from the command line will fix this:

{{{
rosservice call /environment_server/set_planning_scene_diff '{}'
}}}

Alternatively, you can set the planning screen from your program that calls the kinematics service:
{{{

// Add before main()
#include <arm_navigation_msgs/SetPlanningSceneDiff.h>
static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";

// Add before you call any kinematics services 
// eg. before while (ros::ok()) {}    
ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
// CHANGE NODEHANDLE 'rh'
ros::ServiceClient set_planning_scene_diff_client = rh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);

arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;

if (!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) {
    ROS_WARN("Can't set planning scene");
    //return 0;
}
}}}


== The code ==

The code for this example can be found in the package pr2_arm_navigation_tutorials in the file src/get_ik.cpp , and is valid for the diamondback or electric distributions:

{{{
#!cplusplus block=ik_block
#include <ros/ros.h>
#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/GetPositionIK.h>

int main(int argc, char **argv){
  ros::init (argc, argv, "get_ik");
  ros::NodeHandle rh;

  ros::service::waitForService("pr2_right_arm_kinematics/get_ik_solver_info");
  ros::service::waitForService("pr2_right_arm_kinematics/get_ik");

  ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("pr2_right_arm_kinematics/get_ik_solver_info");
  ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::GetPositionIK>("pr2_right_arm_kinematics/get_ik");

  // define the service messages
  kinematics_msgs::GetKinematicSolverInfo::Request request;
  kinematics_msgs::GetKinematicSolverInfo::Response response;

  if(query_client.call(request,response))
  {
    for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
    {
      ROS_DEBUG("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
    }
  }
  else
  {
    ROS_ERROR("Could not call query service");
    ros::shutdown();
    exit(1);
  }
  // define the service messages
  kinematics_msgs::GetPositionIK::Request  gpik_req;
  kinematics_msgs::GetPositionIK::Response gpik_res;
  gpik_req.timeout = ros::Duration(5.0);
  gpik_req.ik_request.ik_link_name = "r_wrist_roll_link";

  gpik_req.ik_request.pose_stamped.header.frame_id = "torso_lift_link";
  gpik_req.ik_request.pose_stamped.pose.position.x = 0.75;
  gpik_req.ik_request.pose_stamped.pose.position.y = -0.188;
  gpik_req.ik_request.pose_stamped.pose.position.z = 0.0;

  gpik_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
  gpik_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
  gpik_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
  gpik_req.ik_request.pose_stamped.pose.orientation.w = 1.0;
  gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
  gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
  for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
  {
    gpik_req.ik_request.ik_seed_state.joint_state.position[i] = (response.kinematic_solver_info.limits[i].min_position + response.kinematic_solver_info.limits[i].max_position)/2.0;
  }
  if(ik_client.call(gpik_req, gpik_res))
  {
    if(gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
      for(unsigned int i=0; i < gpik_res.solution.joint_state.name.size(); i ++)
        ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state.position[i]);
    else
      ROS_ERROR("Inverse kinematics failed");
  }
  else
    ROS_ERROR("Inverse kinematics service call failed");
  ros::shutdown();
}
}}}

=== Populating the inverse kinematics service request ===

The inverse kinematics for the PR2 is (currently) available only for the end-effector links on the two arms - i.e. the '''r_wrist_roll_link''' and the '''l_wrist_roll_link'''. To populate the inverse kinematics request, you need to fill up three pieces of information:
 * A timeout
 * The desired pose for the link that you want compute IK for and 
 * A suggested/seed/hint set of positions for the joints used for the IK computation. This set of states will be used to '''seed''' the inverse kinematics solver. 

=== Setting a timeout ===

Setting a timeout is '''VERY IMPORTANT'''. If you set a timeout of 0.0, the kinematics node will reject the service request. The inverse kinematics solver is very fast, but we will set a timeout of 5.0 seconds.
<<CodeRef(ik_block, 35, 35)>>

=== Setting the desired pose ===

We will first set the desired cartesian pose for the link that we want to compute IK for. In this case, its the '''r_wrist_roll_link''' of the PR2 robot. We will set a desired cartesian pose for this link in the '''torso_lift_link''' frame. 
<<CodeRef(ik_block, 36, 46)>>

=== Seeding the IK solver ===

The ik_seed_state field is intended for use to seed the IK solver with an initial guess for inverse kinematics. It is important to (at the very least) seed the joints that the kinematics node uses in its computation, i.e. the vector of joints returned by the query service. In this tutorial, we will seed the joint states with values at the middle of the joint limits.
<<CodeRef(ik_block, 47, 52)>>

=== Calling the IK service ===
We can now call the IK service. Note that since we asked for the forward kinematics for two links, the response will contain a vector of poses of size 2. 
<<CodeRef(ik_block, 53, 53)>>

=== Interpreting the response ===

The response to the IK request is contained within a motion_planning_msgs/RobotState message in the service response. For arm inverse kinematics, it is of the form sensor_msgs/JointState. It contains both the joint values for the IK solution and the corresponding joint names. 
<<CodeRef(ik_block, 55, 59)>>

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