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 armnavigation. 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: ikclient.call(gpikreq, 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