Note: This tutorial assumes that you have completed the previous tutorials: Forward kinematics for the PR2 arms.
(!) 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.

Inverse kinematics for the PR2 arms

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.

Tutorial Level: INTERMEDIATE

Next Tutorial: Collision free inverse kinematics for the PR2 arms

Contents

  1. ROS Setup
  2. Running in simulation
  3. The code
    1. Populating the inverse kinematics service request
    2. Setting a timeout
    3. Setting the desired pose
    4. Seeding the IK solver
    5. Calling the IK service
    6. Interpreting the response

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: 

   1 #include <ros/ros.h>
   2 #include <kinematics_msgs/GetKinematicSolverInfo.h>
   3 #include <kinematics_msgs/GetPositionIK.h>
   4 
   5 int main(int argc, char **argv){
   6   ros::init (argc, argv, "get_ik");
   7   ros::NodeHandle rh;
   8 
   9   ros::service::waitForService("pr2_right_arm_kinematics/get_ik_solver_info");
  10   ros::service::waitForService("pr2_right_arm_kinematics/get_ik");
  11 
  12   ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("pr2_right_arm_kinematics/get_ik_solver_info");
  13   ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::GetPositionIK>("pr2_right_arm_kinematics/get_ik");
  14 
  15   // define the service messages
  16   kinematics_msgs::GetKinematicSolverInfo::Request request;
  17   kinematics_msgs::GetKinematicSolverInfo::Response response;
  18 
  19   if(query_client.call(request,response))
  20   {
  21     for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
  22     {
  23       ROS_DEBUG("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
  24     }
  25   }
  26   else
  27   {
  28     ROS_ERROR("Could not call query service");
  29     ros::shutdown();
  30     exit(1);
  31   }
  32   // define the service messages
  33   kinematics_msgs::GetPositionIK::Request  gpik_req;
  34   kinematics_msgs::GetPositionIK::Response gpik_res;
  35   gpik_req.timeout = ros::Duration(5.0);
  36   gpik_req.ik_request.ik_link_name = "r_wrist_roll_link";
  37 
  38   gpik_req.ik_request.pose_stamped.header.frame_id = "torso_lift_link";
  39   gpik_req.ik_request.pose_stamped.pose.position.x = 0.75;
  40   gpik_req.ik_request.pose_stamped.pose.position.y = -0.188;
  41   gpik_req.ik_request.pose_stamped.pose.position.z = 0.0;
  42 
  43   gpik_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
  44   gpik_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
  45   gpik_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
  46   gpik_req.ik_request.pose_stamped.pose.orientation.w = 1.0;
  47   gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
  48   gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
  49   for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
  50   {
  51     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;
  52   }
  53   if(ik_client.call(gpik_req, gpik_res))
  54   {
  55     if(gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
  56       for(unsigned int i=0; i < gpik_res.solution.joint_state.name.size(); i ++)
  57         ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state.position[i]);
  58     else
  59       ROS_ERROR("Inverse kinematics failed");
  60   }
  61   else
  62     ROS_ERROR("Inverse kinematics service call failed");
  63   ros::shutdown();
  64 }

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. 

  35   gpik_req.timeout = ros::Duration(5.0);

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. 

  36   gpik_req.ik_request.ik_link_name = "r_wrist_roll_link";
  37 
  38   gpik_req.ik_request.pose_stamped.header.frame_id = "torso_lift_link";
  39   gpik_req.ik_request.pose_stamped.pose.position.x = 0.75;
  40   gpik_req.ik_request.pose_stamped.pose.position.y = -0.188;
  41   gpik_req.ik_request.pose_stamped.pose.position.z = 0.0;
  42 
  43   gpik_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
  44   gpik_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
  45   gpik_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
  46   gpik_req.ik_request.pose_stamped.pose.orientation.w = 1.0;

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. 

  47   gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
  48   gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
  49   for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
  50   {
  51     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;
  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. 

  53   if(ik_client.call(gpik_req, gpik_res))

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. 

  55     if(gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
  56       for(unsigned int i=0; i < gpik_res.solution.joint_state.name.size(); i ++)
  57         ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state.position[i]);
  58     else
  59       ROS_ERROR("Inverse kinematics failed");

Wiki: pr2_kinematics/Tutorials/Tutorial 4 (last edited 2012-11-24 11:13:26 by DavidButterworth)

Except where otherwise noted, the ROS wiki is licensed under the
Creative Commons Attribution 3.0