pr2_arm_ik_tests: pr2_arm_ik_tests: test_ik_node

00001 //Software License Agreement (BSD License)
00002 
00003 //Copyright (c) 2008, Willow Garage, Inc.
00004 //All rights reserved.
00005 
00006 //Redistribution and use in source and binary forms, with or without
00007 //modification, are permitted provided that the following conditions
00008 //are met:
00009 
00010 // * Redistributions of source code must retain the above copyright
00011 //   notice, this list of conditions and the following disclaimer.
00012 // * Redistributions in binary form must reproduce the above
00013 //   copyright notice, this list of conditions and the following
00014 //   disclaimer in the documentation and/or other materials provided
00015 //   with the distribution.
00016 // * Neither the name of Willow Garage, Inc. nor the names of its
00017 //   contributors may be used to endorse or promote products derived
00018 //   from this software without specific prior written permission.
00019 
00020 //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 //"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 //LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 //FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 //COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 //INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 //BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 //CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 //LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 //ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 //POSSIBILITY OF SUCH DAMAGE.
00032 
00033 #include <stdio.h>
00034 #include <stdlib.h>
00035 #include <time.h>
00036 #include <ros/ros.h>
00037 #include <kinematics_msgs/GetPositionIK.h>
00038 #include <kinematics_msgs/GetPositionFK.h>
00039 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00040 
00041 #include <gtest/gtest.h>
00042 
00043 #define IK_NEAR 1e-4
00044 #define IK_NEAR_TRANSLATE 1e-5
00045 
00046 static const std::string ARM_FK_NAME = "/pr2_right_arm_kinematics/get_fk";
00047 static const std::string ARM_IK_NAME = "/pr2_right_arm_kinematics/get_ik";
00048 static const std::string FK_SOLVER_INFO = "/pr2_right_arm_kinematics/get_fk_solver_info";
00049 static const std::string IK_SOLVER_INFO = "/pr2_right_arm_kinematics/get_ik_solver_info";
00050 
00051 static const int NUM_TESTS = 1000;
00052 
00053 double gen_rand(double min, double max)
00054 {
00055   int rand_num = rand()%100+1;
00056   double result = min + (double)((max-min)*rand_num)/101.0;
00057   return result;
00058 }
00059 
00060 bool NOT_NEAR(const double &v1, const double &v2, const double &NEAR)
00061 {
00062   if(fabs(v1-v2) > NEAR)
00063     return true;
00064   return false;
00065 }
00066 
00067 TEST(PR2ArmIKNode, queryKinematics)
00068 {
00069   ros::NodeHandle rh;
00070   ros::service::waitForService(FK_SOLVER_INFO);
00071   ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(FK_SOLVER_INFO);
00072 
00073   // define the service messages
00074   kinematics_msgs::GetKinematicSolverInfo::Request request;
00075   kinematics_msgs::GetKinematicSolverInfo::Response response;
00076   bool service_call = query_client.call(request, response);
00077   ASSERT_TRUE(service_call);
00078   ASSERT_TRUE(!response.kinematic_solver_info.link_names.empty());
00079   ASSERT_TRUE(!response.kinematic_solver_info.joint_names.empty());
00080   ros::shutdown();
00081 }
00082 
00083 TEST(PR2ArmIKNode, forwardKinematics)
00084 {
00085   ros::NodeHandle rh;
00086   ros::service::waitForService(FK_SOLVER_INFO);
00087   ros::service::waitForService(ARM_FK_NAME);
00088   ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(FK_SOLVER_INFO);
00089   ros::ServiceClient fk_client = rh.serviceClient<kinematics_msgs::GetPositionFK>(ARM_FK_NAME);
00090 
00091   // define the service messages
00092   kinematics_msgs::GetKinematicSolverInfo::Request request;
00093   kinematics_msgs::GetKinematicSolverInfo::Response response;
00094             
00095   bool service_call = query_client.call(request, response);
00096   ASSERT_TRUE(service_call);
00097   ASSERT_TRUE(!response.kinematic_solver_info.link_names.empty());
00098 
00099   int num_joints;
00100   std::vector<double> min_limits, max_limits;
00101   num_joints = (int) response.kinematic_solver_info.joint_names.size();
00102   for(int i=0; i< num_joints; i++)
00103   {
00104     min_limits.push_back(-M_PI);
00105     max_limits.push_back(M_PI);
00106   }
00107   // define the service messages
00108   kinematics_msgs::GetPositionFK::Request  fk_request;
00109   kinematics_msgs::GetPositionFK::Response fk_response;
00110   fk_request.header.frame_id = "torso_lift_link";
00111   fk_request.fk_link_names.resize(2);
00112   fk_request.fk_link_names[0] = "r_wrist_roll_link";
00113   fk_request.fk_link_names[1] = "r_elbow_flex_link";
00114   fk_request.robot_state.joint_state.position.resize(num_joints);
00115   fk_request.robot_state.joint_state.name = response.kinematic_solver_info.joint_names;
00116   for(int i=0; i< num_joints; i++)
00117   {
00118     fk_request.robot_state.joint_state.position[i] = gen_rand(min_limits[i],max_limits[i]);
00119   }
00120   bool fk_service_call = fk_client.call(fk_request, fk_response);
00121   EXPECT_TRUE(fk_service_call);
00122   ASSERT_TRUE(!fk_response.pose_stamped.empty());
00123   ASSERT_TRUE(fk_response.pose_stamped.size()==2);
00124   ASSERT_TRUE(fk_response.fk_link_names.size()==2);
00125   ros::shutdown();
00126 }
00127 
00128 TEST(PR2ArmIKNode, inverseKinematics)
00129 {
00130   srand ( time(NULL) ); // initialize random seed
00131   ros::NodeHandle rh;
00132   ros::service::waitForService(IK_SOLVER_INFO);
00133   ros::service::waitForService(ARM_FK_NAME);
00134   ros::service::waitForService(ARM_IK_NAME);
00135   ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(IK_SOLVER_INFO);
00136   ros::ServiceClient fk_client = rh.serviceClient<kinematics_msgs::GetPositionFK>(ARM_FK_NAME);
00137   ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::GetPositionIK>(ARM_IK_NAME);
00138 
00139   // define the service messages
00140   kinematics_msgs::GetKinematicSolverInfo::Request request;
00141   kinematics_msgs::GetKinematicSolverInfo::Response response;
00142             
00143   bool service_call = query_client.call(request, response);
00144   ASSERT_TRUE(service_call);
00145   ASSERT_TRUE(response.kinematic_solver_info.joint_names.size() == response.kinematic_solver_info.limits.size());
00146   ASSERT_TRUE(!response.kinematic_solver_info.link_names.empty());
00147 
00148   int num_joints;
00149   std::vector<double> min_limits, max_limits;
00150   num_joints = (int) response.kinematic_solver_info.joint_names.size();
00151   for(int i=0; i< num_joints; i++)
00152   {
00153     min_limits.push_back(response.kinematic_solver_info.limits[i].min_position);
00154     max_limits.push_back(response.kinematic_solver_info.limits[i].max_position);
00155   }
00156 
00157   // define the service messages
00158   kinematics_msgs::GetPositionFK::Request  fk_request;
00159   kinematics_msgs::GetPositionFK::Response fk_response;
00160   kinematics_msgs::GetPositionIK::Request  ik_request;
00161   kinematics_msgs::GetPositionIK::Response ik_response;
00162 
00163   fk_request.header.frame_id = "torso_lift_link";
00164   fk_request.fk_link_names.resize(1);
00165   fk_request.fk_link_names[0] = "r_wrist_roll_link";
00166   fk_request.robot_state.joint_state.name = response.kinematic_solver_info.joint_names;
00167   fk_request.robot_state.joint_state.position.resize(num_joints);
00168 
00169   int num_solutions = 0;
00170 
00171   for(int kk =0 ; kk < NUM_TESTS; kk++)
00172   {
00173     for(int i=0; i< num_joints; i++)
00174     {
00175       fk_request.robot_state.joint_state.position[i] =  gen_rand(std::max(min_limits[i],-M_PI),std::min(max_limits[i],M_PI));
00176     }
00177     bool fk_service_call = fk_client.call(fk_request, fk_response);
00178 
00179     ik_request.ik_request.pose_stamped = fk_response.pose_stamped[0];
00180     ik_request.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
00181     ik_request.ik_request.ik_seed_state.joint_state.position.resize(num_joints);
00182     ik_request.ik_request.ik_link_name = response.kinematic_solver_info.link_names[0];
00183     ik_request.timeout = ros::Duration(2.0);
00184     for(int i=0; i< num_joints; i++)
00185     {
00186       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));
00187     }
00188     bool ik_service_call = ik_client.call(ik_request,ik_response);
00189     if(!ik_service_call)
00190     {
00191       ROS_ERROR("IK service call failed: %d",kk);
00192       continue;
00193     }
00194     if(ik_response.error_code.val != ik_response.error_code.SUCCESS)
00195     {
00196       ROS_ERROR("IK failed: %d",kk);
00197       continue;
00198     }
00199 
00200     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());
00201 
00202     fk_request.robot_state.joint_state.position = ik_response.solution.joint_state.position;
00203     fk_request.robot_state.joint_state.name = ik_response.solution.joint_state.name;
00204     fk_service_call = fk_client.call(fk_request, fk_response);
00205 
00206     ASSERT_TRUE(!fk_response.pose_stamped.empty());
00207 
00208     if(NOT_NEAR(ik_request.ik_request.pose_stamped.pose.position.x,fk_response.pose_stamped[0].pose.position.x,IK_NEAR_TRANSLATE) || 
00209        NOT_NEAR(ik_request.ik_request.pose_stamped.pose.position.y,fk_response.pose_stamped[0].pose.position.y,IK_NEAR_TRANSLATE) ||
00210        NOT_NEAR(ik_request.ik_request.pose_stamped.pose.position.z,fk_response.pose_stamped[0].pose.position.z,IK_NEAR_TRANSLATE) ||
00211 
00212        NOT_NEAR(ik_request.ik_request.pose_stamped.pose.orientation.x,fk_response.pose_stamped[0].pose.orientation.x,IK_NEAR) ||
00213        NOT_NEAR(ik_request.ik_request.pose_stamped.pose.orientation.y,fk_response.pose_stamped[0].pose.orientation.y,IK_NEAR) ||
00214        NOT_NEAR(ik_request.ik_request.pose_stamped.pose.orientation.z,fk_response.pose_stamped[0].pose.orientation.z,IK_NEAR) ||
00215        NOT_NEAR(ik_request.ik_request.pose_stamped.pose.orientation.w,fk_response.pose_stamped[0].pose.orientation.w,IK_NEAR))
00216     {
00217       ROS_DEBUG("IK solution incorrect: %d",kk);
00218       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,
00219                 ik_request.ik_request.pose_stamped.pose.position.y,
00220                 ik_request.ik_request.pose_stamped.pose.position.z,
00221                 ik_request.ik_request.pose_stamped.pose.orientation.x,
00222                 ik_request.ik_request.pose_stamped.pose.orientation.y,
00223                 ik_request.ik_request.pose_stamped.pose.orientation.z,
00224                 ik_request.ik_request.pose_stamped.pose.orientation.w);
00225       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,
00226                 fk_response.pose_stamped[0].pose.position.y,
00227                 fk_response.pose_stamped[0].pose.position.z,
00228                 fk_response.pose_stamped[0].pose.orientation.x,
00229                 fk_response.pose_stamped[0].pose.orientation.y,
00230                 fk_response.pose_stamped[0].pose.orientation.z,
00231                 fk_response.pose_stamped[0].pose.orientation.w);
00232       continue;
00233     }
00234     num_solutions++;
00235   }
00236   double percent_success = ((double) num_solutions)/((double) NUM_TESTS);
00237   bool success = percent_success > 0.99;
00238   EXPECT_TRUE(success); 
00239   ROS_INFO("Num tests: %d, num solutions: %d",NUM_TESTS,num_solutions); 
00240   ros::shutdown();
00241 }
00242 
00243 int main(int argc, char **argv){
00244   testing::InitGoogleTest(&argc, argv);
00245   ros::init (argc, argv, "pr2_ik_node_regression_test");
00246   return RUN_ALL_TESTS();
00247 }
test_ik_node_regress.cpp
