test_ik_node_regress.cpp

Go to the documentation of this file.

//Software License Agreement (BSD License)

//Copyright (c) 2008, Willow Garage, Inc.
//All rights reserved.

//Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions
//are met:

// * Redistributions of source code must retain the above copyright
//   notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above
//   copyright notice, this list of conditions and the following
//   disclaimer in the documentation and/or other materials provided
//   with the distribution.
// * Neither the name of Willow Garage, Inc. nor the names of its
//   contributors may be used to endorse or promote products derived
//   from this software without specific prior written permission.

//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
//"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
//LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
//FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
//COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
//INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
//BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
//CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
//LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
//ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//POSSIBILITY OF SUCH DAMAGE.

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

#include <gtest/gtest.h>

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

static const std::string ARM_FK_NAME = "/pr2_right_arm_kinematics/get_fk";
static const std::string ARM_IK_NAME = "/pr2_right_arm_kinematics/get_ik";
static const std::string FK_SOLVER_INFO = "/pr2_right_arm_kinematics/get_fk_solver_info";
static const std::string IK_SOLVER_INFO = "/pr2_right_arm_kinematics/get_ik_solver_info";

static const int NUM_TESTS = 1000;

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;
}

TEST(PR2ArmIKNode, queryKinematics)
{
  ros::NodeHandle rh;
  ros::service::waitForService(FK_SOLVER_INFO);
  ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(FK_SOLVER_INFO);

  // define the service messages
  kinematics_msgs::GetKinematicSolverInfo::Request request;
  kinematics_msgs::GetKinematicSolverInfo::Response response;
  bool service_call = query_client.call(request, response);
  ASSERT_TRUE(service_call);
  ASSERT_TRUE(!response.kinematic_solver_info.link_names.empty());
  ASSERT_TRUE(!response.kinematic_solver_info.joint_names.empty());
  ros::shutdown();
}

TEST(PR2ArmIKNode, forwardKinematics)
{
  ros::NodeHandle rh;
  ros::service::waitForService(FK_SOLVER_INFO);
  ros::service::waitForService(ARM_FK_NAME);
  ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(FK_SOLVER_INFO);
  ros::ServiceClient fk_client = rh.serviceClient<kinematics_msgs::GetPositionFK>(ARM_FK_NAME);

  // define the service messages
  kinematics_msgs::GetKinematicSolverInfo::Request request;
  kinematics_msgs::GetKinematicSolverInfo::Response response;
            
  bool service_call = query_client.call(request, response);
  ASSERT_TRUE(service_call);
  ASSERT_TRUE(!response.kinematic_solver_info.link_names.empty());

  int num_joints;
  std::vector<double> min_limits, max_limits;
  num_joints = (int) response.kinematic_solver_info.joint_names.size();
  for(int i=0; i< num_joints; i++)
  {
    min_limits.push_back(-M_PI);
    max_limits.push_back(M_PI);
  }
  // define the service messages
  kinematics_msgs::GetPositionFK::Request  fk_request;
  kinematics_msgs::GetPositionFK::Response fk_response;
  fk_request.header.frame_id = "torso_lift_link";
  fk_request.fk_link_names.resize(2);
  fk_request.fk_link_names[0] = "r_wrist_roll_link";
  fk_request.fk_link_names[1] = "r_elbow_flex_link";
  fk_request.robot_state.joint_state.position.resize(num_joints);
  fk_request.robot_state.joint_state.name = response.kinematic_solver_info.joint_names;
  for(int i=0; i< num_joints; i++)
  {
    fk_request.robot_state.joint_state.position[i] = gen_rand(min_limits[i],max_limits[i]);
  }
  bool fk_service_call = fk_client.call(fk_request, fk_response);
  EXPECT_TRUE(fk_service_call);
  ASSERT_TRUE(!fk_response.pose_stamped.empty());
  ASSERT_TRUE(fk_response.pose_stamped.size()==2);
  ASSERT_TRUE(fk_response.fk_link_names.size()==2);
  ros::shutdown();
}

TEST(PR2ArmIKNode, inverseKinematics)
{
  srand ( time(NULL) ); // initialize random seed
  ros::NodeHandle rh;
  ros::service::waitForService(IK_SOLVER_INFO);
  ros::service::waitForService(ARM_FK_NAME);
  ros::service::waitForService(ARM_IK_NAME);
  ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(IK_SOLVER_INFO);
  ros::ServiceClient fk_client = rh.serviceClient<kinematics_msgs::GetPositionFK>(ARM_FK_NAME);
  ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::GetPositionIK>(ARM_IK_NAME);

  // define the service messages
  kinematics_msgs::GetKinematicSolverInfo::Request request;
  kinematics_msgs::GetKinematicSolverInfo::Response response;
            
  bool service_call = query_client.call(request, response);
  ASSERT_TRUE(service_call);
  ASSERT_TRUE(response.kinematic_solver_info.joint_names.size() == response.kinematic_solver_info.limits.size());
  ASSERT_TRUE(!response.kinematic_solver_info.link_names.empty());

  int num_joints;
  std::vector<double> min_limits, max_limits;
  num_joints = (int) response.kinematic_solver_info.joint_names.size();
  for(int i=0; i< num_joints; i++)
  {
    min_limits.push_back(response.kinematic_solver_info.limits[i].min_position);
    max_limits.push_back(response.kinematic_solver_info.limits[i].max_position);
  }

  // define the service messages
  kinematics_msgs::GetPositionFK::Request  fk_request;
  kinematics_msgs::GetPositionFK::Response fk_response;
  kinematics_msgs::GetPositionIK::Request  ik_request;
  kinematics_msgs::GetPositionIK::Response ik_response;

  fk_request.header.frame_id = "torso_lift_link";
  fk_request.fk_link_names.resize(1);
  fk_request.fk_link_names[0] = "r_wrist_roll_link";
  fk_request.robot_state.joint_state.name = response.kinematic_solver_info.joint_names;
  fk_request.robot_state.joint_state.position.resize(num_joints);

  int num_solutions = 0;

  for(int kk =0 ; kk < NUM_TESTS; kk++)
  {
    for(int i=0; i< num_joints; i++)
    {
      fk_request.robot_state.joint_state.position[i] =  gen_rand(std::max(min_limits[i],-M_PI),std::min(max_limits[i],M_PI));
    }
    bool fk_service_call = fk_client.call(fk_request, fk_response);

    ik_request.ik_request.pose_stamped = fk_response.pose_stamped[0];
    ik_request.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
    ik_request.ik_request.ik_seed_state.joint_state.position.resize(num_joints);
    ik_request.ik_request.ik_link_name = response.kinematic_solver_info.link_names[0];
    ik_request.timeout = ros::Duration(2.0);
    for(int i=0; i< num_joints; i++)
    {
      ik_request.ik_request.ik_seed_state.joint_state.position[i] = gen_rand(std::max(min_limits[i],-M_PI),std::min(max_limits[i],M_PI));
    }
    bool ik_service_call = ik_client.call(ik_request,ik_response);
    if(!ik_service_call)
    {
      ROS_ERROR("IK service call failed: %d",kk);
      continue;
    }
    if(ik_response.error_code.val != ik_response.error_code.SUCCESS)
    {
      ROS_ERROR("IK failed: %d",kk);
      continue;
    }

    ROS_DEBUG("IK response: num positions %d, num solution joints %d",(int)ik_response.solution.joint_state.position.size(),(int)ik_response.solution.joint_state.name.size());

    fk_request.robot_state.joint_state.position = ik_response.solution.joint_state.position;
    fk_request.robot_state.joint_state.name = ik_response.solution.joint_state.name;
    fk_service_call = fk_client.call(fk_request, fk_response);

    ASSERT_TRUE(!fk_response.pose_stamped.empty());

    if(NOT_NEAR(ik_request.ik_request.pose_stamped.pose.position.x,fk_response.pose_stamped[0].pose.position.x,IK_NEAR_TRANSLATE) || 
       NOT_NEAR(ik_request.ik_request.pose_stamped.pose.position.y,fk_response.pose_stamped[0].pose.position.y,IK_NEAR_TRANSLATE) ||
       NOT_NEAR(ik_request.ik_request.pose_stamped.pose.position.z,fk_response.pose_stamped[0].pose.position.z,IK_NEAR_TRANSLATE) ||

       NOT_NEAR(ik_request.ik_request.pose_stamped.pose.orientation.x,fk_response.pose_stamped[0].pose.orientation.x,IK_NEAR) ||
       NOT_NEAR(ik_request.ik_request.pose_stamped.pose.orientation.y,fk_response.pose_stamped[0].pose.orientation.y,IK_NEAR) ||
       NOT_NEAR(ik_request.ik_request.pose_stamped.pose.orientation.z,fk_response.pose_stamped[0].pose.orientation.z,IK_NEAR) ||
       NOT_NEAR(ik_request.ik_request.pose_stamped.pose.orientation.w,fk_response.pose_stamped[0].pose.orientation.w,IK_NEAR))
    {
      ROS_DEBUG("IK solution incorrect: %d",kk);
      ROS_ERROR("ik request: %s, %f, %f, %f :: %f %f %f %f",ik_request.ik_request.pose_stamped.header.frame_id.c_str(),ik_request.ik_request.pose_stamped.pose.position.x,
                ik_request.ik_request.pose_stamped.pose.position.y,
                ik_request.ik_request.pose_stamped.pose.position.z,
                ik_request.ik_request.pose_stamped.pose.orientation.x,
                ik_request.ik_request.pose_stamped.pose.orientation.y,
                ik_request.ik_request.pose_stamped.pose.orientation.z,
                ik_request.ik_request.pose_stamped.pose.orientation.w);
      ROS_ERROR("fk response: %s, %f, %f, %f :: %f %f %f %f\n",fk_response.pose_stamped[0].header.frame_id.c_str(),fk_response.pose_stamped[0].pose.position.x,
                fk_response.pose_stamped[0].pose.position.y,
                fk_response.pose_stamped[0].pose.position.z,
                fk_response.pose_stamped[0].pose.orientation.x,
                fk_response.pose_stamped[0].pose.orientation.y,
                fk_response.pose_stamped[0].pose.orientation.z,
                fk_response.pose_stamped[0].pose.orientation.w);
      continue;
    }
    num_solutions++;
  }
  double percent_success = ((double) num_solutions)/((double) NUM_TESTS);
  bool success = percent_success > 0.99;
  EXPECT_TRUE(success); 
  ROS_INFO("Num tests: %d, num solutions: %d",NUM_TESTS,num_solutions); 
  ros::shutdown();
}

int main(int argc, char **argv){
  testing::InitGoogleTest(&argc, argv);
  ros::init (argc, argv, "pr2_ik_node_regression_test");
  return RUN_ALL_TESTS();
}