ik_test.cpp

Go to the documentation of this file.

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <ros/ros.h>
#include <kinematics_msgs/GetKinematicTreeInfo.h>
#include <kinematics_msgs/GetPositionIK.h>
#include <kinematics_msgs/GetPositionFK.h>
#include <vector>

#define IK_NEAR 1e-4
#define IK_NEAR_TRANSLATE 1e-5

static const std::string ARM_FK_NAME = "/pr2_ik_right_arm/get_fk";
static const std::string ARM_IK_NAME = "/pr2_ik_right_arm/get_ik";
static const std::string ARM_QUERY_NAME = "/pr2_ik_right_arm/get_kinematic_tree_info";


double gen_rand(double min, double max)
{
  int rand_num = rand()%100+1;
  double result = min + (double)((max-min)*rand_num)/101.0;
  return result;
}

bool NOT_NEAR(const double &v1, const double &v2, const double &NEAR)
{
   if(fabs(v1-v2) > NEAR)
      return true;
   return false;
}


class TestIKandFK{

private:
  ros::NodeHandle node;
  ros::ServiceClient ik_client;
  ros::ServiceClient fk_client;
  ros::ServiceClient query_client;

public:
  TestIKandFK(){

    //create a client function for the IK, FK, and joint query services
    ik_client = node.serviceClient<kinematics_msgs::GetPositionIK>
      (ARM_IK_NAME, true);    
    fk_client = node.serviceClient<kinematics_msgs::GetPositionFK>
      (ARM_FK_NAME, true);
    query_client = node.serviceClient<kinematics_msgs::GetKinematicTreeInfo>
      (ARM_QUERY_NAME, true);

    //wait for the various services to be ready
    ROS_INFO("Waiting for services to be ready");
    ros::service::waitForService(ARM_IK_NAME);
    ros::service::waitForService(ARM_FK_NAME);
    ros::service::waitForService(ARM_QUERY_NAME);
    ROS_INFO("Services ready");
  }

  //run inverse kinematics on a PoseStamped (7-dof pose 
  //(position + quaternion orientation) + header specifying the 
  //frame of the pose)
  //tries to stay close to double start_angles[7]
  //returns the solution angles in double solution[7] 
  bool run_ik(geometry_msgs::PoseStamped pose, double start_angles[7], 
              double solution[7], std::string link_name){

    kinematics_msgs::GetPositionIK::Request  ik_request;
    kinematics_msgs::GetPositionIK::Response ik_response;  
 
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_shoulder_pan_joint");
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_shoulder_lift_joint");
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_upper_arm_roll_joint");
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_elbow_flex_joint");
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_forearm_roll_joint");
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_wrist_flex_joint");
    ik_request.ik_request.ik_seed_state.joint_state.name.push_back("r_wrist_roll_joint");

    ik_request.ik_request.ik_link_name = link_name; 

    ik_request.ik_request.pose_stamped = pose;
    ik_request.ik_request.ik_seed_state.joint_state.position.resize(7);

    for(int i=0; i<7; i++) ik_request.ik_request.ik_seed_state.joint_state.position[i] = start_angles[i];

    ROS_INFO("request pose: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);

    bool ik_service_call = ik_client.call(ik_request,ik_response);
    if(!ik_service_call){
      ROS_ERROR("IK service call failed!");  
      return 0;
    }
    for(int i=0; i<7; i++){
      solution[i] = ik_response.solution.joint_state.position[i];
    }
    ROS_INFO("solution angles: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", 
             solution[0],solution[1], solution[2],solution[3],
             solution[4],solution[5],solution[6]);
    ROS_INFO("IK service call succeeded");
    return 1;
  }

  //run forward kinematics on a set of 7 joint angles 
  bool run_fk(double angles[7], geometry_msgs::PoseStamped &return_pose, std::string link_name){
    int i;
    kinematics_msgs::GetPositionFK::Request  fk_request;
    kinematics_msgs::GetPositionFK::Response fk_response;
    fk_request.header.stamp = ros::Time::now();
    fk_request.header.frame_id = "base_link";

    fk_request.robot_state.joint_state.name.push_back("r_shoulder_pan_joint");
    fk_request.robot_state.joint_state.name.push_back("r_shoulder_lift_joint");
    fk_request.robot_state.joint_state.name.push_back("r_upper_arm_roll_joint");
    fk_request.robot_state.joint_state.name.push_back("r_elbow_flex_joint");
    fk_request.robot_state.joint_state.name.push_back("r_forearm_roll_joint");
    fk_request.robot_state.joint_state.name.push_back("r_wrist_flex_joint");
    fk_request.robot_state.joint_state.name.push_back("r_wrist_roll_joint");

    fk_request.fk_link_names.push_back(link_name);

    fk_request.robot_state.joint_state.position.resize(7);
    for(i=0; i<7; i++){
      fk_request.robot_state.joint_state.position[i] = angles[i];
    }
    bool success = fk_client.call(fk_request, fk_response);
    if(!success){
      ROS_INFO("FK call failed!");
      return 0;
    }
    return_pose = fk_response.pose_stamped[0];
    return 1;
  }

  //test the ik and fk functions
  bool run_test(){
    srand ( time(NULL) ); // initialize random seed

    // use IKQuery to get the min and max joint angles
    kinematics_msgs::GetKinematicTreeInfo::Request request;
    kinematics_msgs::GetKinematicTreeInfo::Response response;
    query_client.call(request, response);
    std::vector<double> min_limits, max_limits;
    for(int i=0; i<7; i++){
      min_limits.push_back(response.kinematic_tree_info.limits[i].min_position);
      max_limits.push_back(response.kinematic_tree_info.limits[i].max_position);
    }

    //run forward kinematics on a random set of joints to get 
    //a corresponding Cartesian pose
    double angles[7];
    for(int i=0; i<7; i++){
      angles[i] =  gen_rand(std::max(min_limits[i],-M_PI),
                            std::min(max_limits[i],M_PI));
    }
    geometry_msgs::PoseStamped fk_response;
    run_fk(angles, fk_response, "r_wrist_roll_link");

    //run inverse kinematics on the resulting Cartesian pose to get 
    //another set of joint angles that makes that pose
    double solutionangles[7];
    double startangles[7] = {0,0,0,0,0,0,0};
    bool success = 1;
    success = run_ik(fk_response, startangles, solutionangles, "r_wrist_roll_link");
    if(!success){
      ROS_INFO("IK call failed!\n");
      return 0;
    }

    //run forward kinematics on the resulting joint angles to see 
    //if they match the first Cartesian pose
    geometry_msgs::PoseStamped fk_response2;
    fk_response2.header.frame_id = "base_link";
    run_fk(solutionangles, fk_response2, "r_wrist_roll_link");

    //check to see if the two poses are pretty close to each other
    success = 1;
    if(NOT_NEAR(fk_response.pose.position.x,
                fk_response2.pose.position.x,IK_NEAR_TRANSLATE) || 
       NOT_NEAR(fk_response.pose.position.y,
                fk_response2.pose.position.y,IK_NEAR_TRANSLATE) ||
       NOT_NEAR(fk_response.pose.position.z,
                fk_response2.pose.position.z,IK_NEAR_TRANSLATE) ||

       NOT_NEAR(fk_response.pose.orientation.x,
                fk_response2.pose.orientation.x,IK_NEAR) ||
       NOT_NEAR(fk_response.pose.orientation.y,
                fk_response2.pose.orientation.y,IK_NEAR) ||
       NOT_NEAR(fk_response.pose.orientation.z,
                fk_response2.pose.orientation.z,IK_NEAR) ||
       NOT_NEAR(fk_response.pose.orientation.w,
                fk_response2.pose.orientation.w,IK_NEAR))
      {
        ROS_INFO("IK solution incorrect\n");
        ROS_INFO("ik request: %f, %f, %f :: %f %f %f %f",
                 fk_response.pose.position.x,
                 fk_response.pose.position.y,
                 fk_response.pose.position.z,
                 fk_response.pose.orientation.x,
                 fk_response.pose.orientation.y,
                 fk_response.pose.orientation.z,
                 fk_response.pose.orientation.w);
        ROS_INFO("fk response: %f, %f, %f :: %f %f %f %f\n",
                 fk_response2.pose.position.x,
                 fk_response2.pose.position.y,
                 fk_response2.pose.position.z,
                 fk_response2.pose.orientation.x,
                 fk_response2.pose.orientation.y,
                 fk_response2.pose.orientation.z,
                 fk_response2.pose.orientation.w);
        success = 0;
      }
    if(success){
      ROS_INFO("IK solution good\n");
    }
    else{
      ROS_INFO("IK solution bad\n");
    }
    return success;
  }

};

int main(int argc, char** argv){

  //init ROS node
  ros::init(argc, argv, "test_ik_and_fk");

  //make the class object
  TestIKandFK test_ik_and_fk = TestIKandFK();

  //run the test
  test_ik_and_fk.run_test();

  return 0;
}