Note: This tutorial assumes that you have completed the previous tutorials: Getting kinematic solver info from a kinematics node.
(!) 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.

Forward kinematics for the PR2 arms

Description: This tutorial will show you how to use a kinematics node to solve the forward kinematics and get the cartesian positions for the links on a PR2 arm.

Keywords: kinematics forward direct cartesian

Tutorial Level: INTERMEDIATE

Next Tutorial: Inverse kinematics for the PR2 arms

In this tutorial, you will learn how to use the PR2 kinematics node to generate forward kinematics positions for all the links 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_fk

If it succeeds, you should see the following output

[ INFO] 1025.270999983: Link    : r_wrist_roll_link
[ INFO] 1025.270999983: Position: 0.520578,0.180466,-0.445349
[ INFO] 1025.270999983: Orientation: 0.436766 0.736298 -0.027756 0.516073
[ INFO] 1025.270999983: Link    : r_elbow_flex_link
[ INFO] 1025.270999983: Position: 0.395819,0.0282368,-0.19177
[ INFO] 1025.270999983: Orientation: 0.124788 0.511289 0.210368 0.823867

The code

The code for this example can be found in the package pr2_arm_navigation_tutorials in the file src/get_fk.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/GetPositionFK.h>
   4 
   5 int main(int argc, char **argv){
   6   ros::init (argc, argv, "get_fk");
   7   ros::NodeHandle rh;
   8 
   9   ros::service::waitForService("pr2_right_arm_kinematics/get_fk_solver_info");
  10   ros::service::waitForService("pr2_right_arm_kinematics/get_fk");
  11 
  12   ros::ServiceClient query_client = 
  13     rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>
  14     ("pr2_right_arm_kinematics/get_fk_solver_info");
  15   ros::ServiceClient fk_client = rh.serviceClient
  16     <kinematics_msgs::GetPositionFK>("pr2_right_arm_kinematics/get_fk");
  17 
  18   // define the service messages
  19   kinematics_msgs::GetKinematicSolverInfo::Request request;
  20   kinematics_msgs::GetKinematicSolverInfo::Response response;
  21   if(query_client.call(request,response))
  22   {
  23     for(unsigned int i=0; 
  24         i< response.kinematic_solver_info.joint_names.size(); i++)
  25     {
  26       ROS_DEBUG("Joint: %d %s", i,
  27        response.kinematic_solver_info.joint_names[i].c_str());
  28     }
  29   }
  30   else
  31   {
  32     ROS_ERROR("Could not call query service");
  33     ros::shutdown();
  34     exit(1);
  35   }
  36   // define the service messages
  37   kinematics_msgs::GetPositionFK::Request  fk_request;
  38   kinematics_msgs::GetPositionFK::Response fk_response;
  39   fk_request.header.frame_id = "torso_lift_link";
  40   fk_request.fk_link_names.resize(2);
  41   fk_request.fk_link_names[0] = "r_wrist_roll_link";
  42   fk_request.fk_link_names[1] = "r_elbow_flex_link";
  43   fk_request.robot_state.joint_state.position.resize
  44     (response.kinematic_solver_info.joint_names.size());
  45   fk_request.robot_state.joint_state.name = 
  46     response.kinematic_solver_info.joint_names;
  47   for(unsigned int i=0; 
  48       i< response.kinematic_solver_info.joint_names.size(); i++)
  49   {
  50     fk_request.robot_state.joint_state.position[i] = 0.5;
  51   }
  52   if(fk_client.call(fk_request, fk_response))
  53   {
  54     if(fk_response.error_code.val == fk_response.error_code.SUCCESS)
  55     {
  56       for(unsigned int i=0; i < fk_response.pose_stamped.size(); i ++)
  57       {
  58         ROS_INFO_STREAM("Link    : " << fk_response.fk_link_names[i].c_str());
  59         ROS_INFO_STREAM("Position: " << 
  60           fk_response.pose_stamped[i].pose.position.x << "," <<  
  61           fk_response.pose_stamped[i].pose.position.y << "," << 
  62           fk_response.pose_stamped[i].pose.position.z);
  63         ROS_INFO("Orientation: %f %f %f %f",
  64           fk_response.pose_stamped[i].pose.orientation.x,
  65           fk_response.pose_stamped[i].pose.orientation.y,
  66           fk_response.pose_stamped[i].pose.orientation.z,
  67           fk_response.pose_stamped[i].pose.orientation.w);
  68       } 
  69     }
  70     else
  71     {
  72       ROS_ERROR("Forward kinematics failed");
  73     }
  74   }
  75   else
  76   {
  77     ROS_ERROR("Forward kinematics service call failed");
  78   }
  79   ros::shutdown();
  80 }

Populating the GetPositionFK service call request

In the previous tutorial, you learned how to get information from a kinematics solver. We will now use this information to compute the forward kinematics for a PR2 arm. The components of the query response that we will use are the joint names.

Let us assume that you want to compute the cartesian positions for two links on the right arm: the r_wrist_roll_link and the r_elbow_flex_link. Also assume that you need these cartesian positions in the torso_lift_link. We will fill up the GetPositionFK request appropriately. Note that the frame_id in the header is the frame_id in which we want the result cartesian position. Thus, we set it to torso_lift_link.

  39   fk_request.header.frame_id = "torso_lift_link";
  40   fk_request.fk_link_names.resize(2);
  41   fk_request.fk_link_names[0] = "r_wrist_roll_link";
  42   fk_request.fk_link_names[1] = "r_elbow_flex_link";
  43   fk_request.robot_state.joint_state.position.resize
  44     (response.kinematic_solver_info.joint_names.size());
  45   fk_request.robot_state.joint_state.name = 
  46     response.kinematic_solver_info.joint_names;

Note that the two desired links that we want to compute forward kinematics for must exist in the vector of link names returned in the query response.

Setting the desired joint state for FK

Next, we will fill up the joint names and joint positions vectors with the desired joint state for which we want to compute the forward kinematics. If the size of the joint values vector is not the same as the size of the joint names vector, the kinematics node will reject the request.

  47   for(unsigned int i=0; 
  48       i< response.kinematic_solver_info.joint_names.size(); i++)
  49   {
  50     fk_request.robot_state.joint_state.position[i] = 0.5;
  51   }
  52   if(fk_client.call(fk_request, fk_response))
  53   {

Calling the FK service

We can now call the FK service. Note that since we asked for the forward kinematics for two links, the response will contain a vector of poses of size 2.

  54     if(fk_response.error_code.val == fk_response.error_code.SUCCESS)
  55     {
  56       for(unsigned int i=0; i < fk_response.pose_stamped.size(); i ++)
  57       {
  58         ROS_INFO_STREAM("Link    : " << fk_response.fk_link_names[i].c_str());
  59         ROS_INFO_STREAM("Position: " << 
  60           fk_response.pose_stamped[i].pose.position.x << "," <<  
  61           fk_response.pose_stamped[i].pose.position.y << "," << 
  62           fk_response.pose_stamped[i].pose.position.z);
  63         ROS_INFO("Orientation: %f %f %f %f",
  64           fk_response.pose_stamped[i].pose.orientation.x,
  65           fk_response.pose_stamped[i].pose.orientation.y,
  66           fk_response.pose_stamped[i].pose.orientation.z,
  67           fk_response.pose_stamped[i].pose.orientation.w);
  68       } 
  69     }
  70     else
  71     {
  72       ROS_ERROR("Forward kinematics failed");
  73     }
  74   }
  75   else
  76   {
  77     ROS_ERROR("Forward kinematics service call failed");
  78   }
  79   ros::shutdown();
  80 }

Wiki: pr2_kinematics/Tutorials/Tutorial 3 (last edited 2011-11-08 04:04:37 by EGilJones)