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.

Getting kinematic solver info from a kinematics node

In this tutorial, you will learn how to get information about a kinematic solver from a solver node. This information will later be used to structure forward and inverse position kinematics requests. We will get this information by querying the kinematics node using a GetKinematicSolverInfo service call.

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_solver_info

If it succeeds, you should see the following output

[ INFO] 0.000000000: Joint: 0 r_shoulder_pan_joint
[ INFO] 0.000000000: Joint: 1 r_shoulder_lift_joint
[ INFO] 0.000000000: Joint: 2 r_upper_arm_roll_joint
[ INFO] 0.000000000: Joint: 3 r_elbow_flex_joint
[ INFO] 0.000000000: Joint: 4 r_forearm_roll_joint
[ INFO] 0.000000000: Joint: 5 r_wrist_flex_joint
[ INFO] 0.000000000: Joint: 6 r_wrist_roll_joint

The code

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

   1 #include <ros/ros.h>
   2 #include <kinematics_msgs/GetKinematicSolverInfo.h>
   3 int main(int argc, char **argv){
   4   ros::init (argc, argv, "get_ik_solver_info");
   5   ros::NodeHandle rh;
   6   ros::service::waitForService("pr2_right_arm_kinematics/get_ik_solver_info");
   7   ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("pr2_right_arm_kinematics/get_ik_solver_info");
   8 
   9   // define the service messages
  10   kinematics_msgs::GetKinematicSolverInfo::Request request;
  11   kinematics_msgs::GetKinematicSolverInfo::Response response;
  12 
  13   if(query_client.call(request,response))
  14   {
  15     for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
  16     {
  17       ROS_INFO("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
  18     }
  19   }
  20   else
  21   {
  22     ROS_ERROR("Could not call query service");
  23   }
  24   ros::shutdown();
  25 }

The service call

   2 #include <kinematics_msgs/GetKinematicSolverInfo.h>
   3 

The service we will use to query the kinematics node is the kinematics_msgs/GetKinematicSolverInfo message. The response to this service contains the following information:

• joint_names - this is the vector of names of joints that the kinematics node uses for its kinematics computations
• limits - this is a vector of joint limits for the joints. In particular, it specifies the internal joint limits used by the kinematics node.
• link_names - this is a list of links that the kinematics can perform computations for.