get_ik.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00003 #include <kinematics_msgs/GetPositionIK.h>
00004 
00005 int main(int argc, char **argv){
00006   ros::init (argc, argv, "get_ik");
00007   ros::NodeHandle rh;
00008 
00009   ros::service::waitForService("pr2_right_arm_kinematics/get_ik_solver_info");
00010   ros::service::waitForService("pr2_right_arm_kinematics/get_ik");
00011 
00012   ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("pr2_right_arm_kinematics/get_ik_solver_info");
00013   ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::GetPositionIK>("pr2_right_arm_kinematics/get_ik");
00014 
00015   // define the service messages
00016   kinematics_msgs::GetKinematicSolverInfo::Request request;
00017   kinematics_msgs::GetKinematicSolverInfo::Response response;
00018 
00019   if(query_client.call(request,response))
00020   {
00021     for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
00022     {
00023       ROS_DEBUG("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
00024     }
00025   }
00026   else
00027   {
00028     ROS_ERROR("Could not call query service");
00029     ros::shutdown();
00030     exit(1);
00031   }
00032   // define the service messages
00033   kinematics_msgs::GetPositionIK::Request  gpik_req;
00034   kinematics_msgs::GetPositionIK::Response gpik_res;
00035   gpik_req.timeout = ros::Duration(5.0);
00036   gpik_req.ik_request.ik_link_name = "r_wrist_roll_link";
00037 
00038   gpik_req.ik_request.pose_stamped.header.frame_id = "torso_lift_link";
00039   gpik_req.ik_request.pose_stamped.pose.position.x = 0.75;
00040   gpik_req.ik_request.pose_stamped.pose.position.y = -0.188;
00041   gpik_req.ik_request.pose_stamped.pose.position.z = 0.0;
00042 
00043   gpik_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
00044   gpik_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
00045   gpik_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
00046   gpik_req.ik_request.pose_stamped.pose.orientation.w = 1.0;
00047   gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
00048   gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
00049   for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
00050   {
00051     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;
00052   }
00053   if(ik_client.call(gpik_req, gpik_res))
00054   {
00055     if(gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
00056       for(unsigned int i=0; i < gpik_res.solution.joint_state.name.size(); i ++)
00057         ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state.position[i]);
00058     else
00059       ROS_ERROR("Inverse kinematics failed");
00060   }
00061   else
00062     ROS_ERROR("Inverse kinematics service call failed");
00063   ros::shutdown();
00064 }


pr2_arm_navigation_tutorials
Author(s): Sachin Chitta
autogenerated on Tue Apr 22 2014 19:29:47