<robot_name>_arm_navigation ik_services [closed]
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