<robot_name>_arm_navigation ik_services [closed]

asked 2013-02-12 12:52:51 -0500

MartinW gravatar image

Hello all,

I am have difficulty using the prebuilt services from the Planning Description Configuration Wizard. When you use this wizard it creates a package by the name of <robot_name>_arm_navigation. I used this with the Warehouse Viewer tutorial to make my robots launch file and now I want to access the services that come with this, specifically the Kinematics services.

To do this I have been doing the kinematics tutorials and I have the forward kinematics calculation working, sending a request from the client and getting a response from the server. Now I want to extend this to working in Cartesian coordinates and obtaining joint angles through the IK service. However, when I try to use this server I get a very generic error "Inverse kinematices service call failed"

It happens from this call: ik_client.call(gpik_req, gpik_res)

In the code:

#include <ros/ros.h>
#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/GetPositionIK.h>


int main(int argc, char **argv){
  ros::init (argc, argv, "get_ik");
  ros::NodeHandle rh;


  ros::service::waitForService("H20_robot_left_arm_kinematics/get_ik_solver_info");
  ros::service::waitForService("H20_robot_left_arm_kinematics/get_ik");

  ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("H20_robot_left_arm_kinematics/get_ik_solver_info");
  ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::GetPositionIK>("H20_robot_left_arm_kinematics/get_ik");



  // define the service message
  kinematics_msgs::GetKinematicSolverInfo::Request request;
  kinematics_msgs::GetKinematicSolverInfo::Response response;

  if(query_client.call(request,response))
  {
    for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
    {
      ROS_DEBUG("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
    }
  }
  else
  {
    ROS_ERROR("Could not call query service");
    ros::shutdown();
    exit(1);
  }
  // define the service messages
  kinematics_msgs::GetPositionIK::Request  gpik_req;
  kinematics_msgs::GetPositionIK::Response gpik_res;
  gpik_req.timeout = ros::Duration(5.0);
  gpik_req.ik_request.ik_link_name = "left_hand_link";

  gpik_req.ik_request.pose_stamped.header.frame_id = "base_link";
  gpik_req.ik_request.pose_stamped.pose.position.x = 0.235;
  gpik_req.ik_request.pose_stamped.pose.position.y = -0.0302972;
  gpik_req.ik_request.pose_stamped.pose.position.z = -0.656106;

  gpik_req.ik_request.pose_stamped.pose.orientation.x = 0.998944;
  gpik_req.ik_request.pose_stamped.pose.orientation.y = -0.0459534;
  gpik_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
  gpik_req.ik_request.pose_stamped.pose.orientation.w = 0.0;
  gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
  gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;

 for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
  {
    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;

}
ROS_INFO(" %s ",  gpik_req.ik_request.pose_stamped.header.frame_id.c_str());


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


    if(gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
     for(unsigned int i=0; i < gpik_res.solution.joint_state.name.size(); i ++)
        ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state.position[i]);
    else
      ROS_ERROR("Inverse kinematics failed");
  }
  else
    ROS_ERROR("Inverse kinematics service call failed");
  ros::shutdown();
}

This code is pretty basic and doesn't change much from people's use in other applications, so I am having a tough time finding a solution to this problem with not many questions like it. Any help would be greatly appreciated!

Kind Regards, Martin

edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2015-10-30 17:52:43.949846