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