pr2_arm_ik_tests: pr2_arm_ik_tests: test_ik

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 <pr2_arm_kinematics/pr2_arm_ik_solver.h>
00038 #include <pr2_arm_kinematics/pr2_arm_kinematics_utils.h>
00039 #include <kdl/chainfksolverpos_recursive.hpp>
00040 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
00041 
00042 #include <gtest/gtest.h>
00043 
00044 
00045 
00046 #define IK_NEAR 1e-4
00047 #define IK_NEAR_TRANSLATE 1e-5
00048 using namespace pr2_arm_kinematics;
00049 using namespace KDL;
00050 
00051 double gen_rand(double min, double max)
00052 {
00053   int rand_num = rand()%100+1;
00054   double result = min + (double)((max-min)*rand_num)/101.0;
00055   return result;
00056 }
00057 
00058 TEST(PR2ArmIK, initialize)
00059 {
00060   ros::NodeHandle nh("pr2_ik_regression_test");
00061   urdf::Model robot_model;
00062   std::string tip_name,root_name,xml_string;
00063   int free_angle;
00064   double sd;
00065   ROS_INFO("Running test");
00066   EXPECT_TRUE(loadRobotModel(nh,robot_model,root_name,tip_name,xml_string));
00067   nh.param<int>("free_angle",free_angle,2);
00068   nh.param<double>("search_discretization",sd,0.01);
00069 }
00070 
00071 bool NOT_NEAR(const double &v1, const double &v2, const double &NEAR)
00072 {
00073    if(fabs(v1-v2) > NEAR)
00074       return true;
00075    return false;
00076 }
00077 
00078 TEST(PR2ArmIK, inverseKinematics)
00079 {
00080   ros::NodeHandle nh("/pr2_ik_regression_test");
00081   urdf::Model robot_model;
00082   std::string tip_name,root_name,xml_string;
00083   int free_angle;
00084   double sd;
00085 
00086   EXPECT_TRUE(loadRobotModel(nh,robot_model,root_name,tip_name,xml_string));
00087   nh.param<int>("free_angle",free_angle,2);
00088   nh.param<double>("search_discretization",sd,0.01);
00089   pr2_arm_kinematics::PR2ArmIKSolver ik(robot_model,root_name,tip_name,sd,free_angle);
00090 
00091   KDL::Chain kdl_chain;
00092   EXPECT_TRUE(pr2_arm_kinematics::getKDLChain(xml_string,root_name,tip_name,kdl_chain));
00093 
00094   int num_tests = 10000;
00095   EXPECT_TRUE(ik.active_);
00096 
00097   KDL::ChainFkSolverPos_recursive *jnt_to_pose_solver;
00098   KDL::JntArray jnt_pos_in;
00099   KDL::JntArray jnt_pos_out;
00100   KDL::Frame p_out;
00101   KDL::Frame p_ik;
00102 
00103   srand ( time(NULL) ); // initialize random seed: 
00104 
00105   jnt_to_pose_solver = new KDL::ChainFkSolverPos_recursive(kdl_chain);
00106   jnt_pos_in.resize(7);
00107   jnt_pos_out.resize(7);
00108 
00109   kinematics_msgs::KinematicSolverInfo chain_info;
00110   ik.getSolverInfo(chain_info);
00111 
00112   int num_solutions(0);
00113 
00114   for(int kk = 0; kk < num_tests; kk++)
00115   {
00116     for(int i=0; i < 7; i++)
00117     {
00118       jnt_pos_in(i) = gen_rand(std::max(chain_info.limits[i].min_position,-M_PI),std::min(chain_info.limits[i].max_position,M_PI));
00119       EXPECT_TRUE((jnt_pos_in(i) <= chain_info.limits[i].max_position));
00120       EXPECT_TRUE((jnt_pos_in(i) >= chain_info.limits[i].min_position));
00121     }
00122 
00123     if(jnt_to_pose_solver->JntToCart(jnt_pos_in,p_out) >=0)
00124     {
00125       bool ik_valid = (ik.CartToJnt(jnt_pos_in,p_out,jnt_pos_out) >= 0);
00126       if(ik_valid)
00127       {
00128         num_solutions++;
00129         jnt_to_pose_solver->JntToCart(jnt_pos_out,p_ik);
00130         for(int j=0; j< 3; j++)
00131         {
00132           EXPECT_NEAR(p_ik.M(j,0),p_out.M(j,0),IK_NEAR);
00133           EXPECT_NEAR(p_ik.M(j,1),p_out.M(j,1),IK_NEAR); 
00134           EXPECT_NEAR(p_ik.M(j,2),p_out.M(j,2),IK_NEAR); 
00135           EXPECT_NEAR(p_ik.p(j),p_out.p(j),IK_NEAR_TRANSLATE);
00136         }
00137       }
00138     }
00139   }
00140 
00141   bool success = ((double)num_solutions)/((double) num_tests) > 0.99;
00142   ROS_INFO("Success rate is %f",((double)num_solutions)/((double) num_tests));
00143 //  EXPECT_EQ(num_solutions,num_tests);
00144   EXPECT_TRUE(success);
00145   delete jnt_to_pose_solver;
00146 }
00147 
00148 
00149 TEST(PR2ArmIK, inverseKinematicsSearch)
00150 {
00151   ros::NodeHandle nh("/pr2_ik_regression_test");
00152   urdf::Model robot_model;
00153   std::string tip_name,root_name,xml_string;
00154   int free_angle;
00155   double sd;
00156 
00157   EXPECT_TRUE(loadRobotModel(nh,robot_model,root_name,tip_name,xml_string));
00158   nh.param<int>("free_angle",free_angle,2);
00159   nh.param<double>("search_discretization",sd,0.001);
00160   pr2_arm_kinematics::PR2ArmIKSolver ik(robot_model,root_name,tip_name,sd,free_angle);
00161 
00162   KDL::Chain kdl_chain;
00163   EXPECT_TRUE(pr2_arm_kinematics::getKDLChain(xml_string,root_name,tip_name,kdl_chain));
00164 
00165   kinematics_msgs::KinematicSolverInfo chain_info;
00166   ik.getSolverInfo(chain_info);
00167 
00168   int num_tests = 10000;
00169   KDL::ChainFkSolverPos_recursive *jnt_to_pose_solver;
00170   KDL::JntArray jnt_pos_in;
00171   KDL::JntArray jnt_pos_test;
00172   std::vector<KDL::JntArray> jnt_pos_out;
00173   KDL::Frame p_out;
00174   KDL::Frame p_ik;
00175 
00176   srand ( time(NULL) );
00177 
00178   jnt_to_pose_solver = new KDL::ChainFkSolverPos_recursive(kdl_chain);
00179   jnt_pos_in.resize(7);
00180   jnt_pos_test.resize(7);
00181 
00182   int num_solutions(0);
00183 
00184   for(int kk = 0; kk < num_tests; kk++)
00185   {
00186     ROS_INFO("Test: %d",kk);
00187     for(int i=0; i < 7; i++)
00188     {
00189       jnt_pos_in(i) = gen_rand(std::max(chain_info.limits[i].min_position,-M_PI),std::min(chain_info.limits[i].max_position,M_PI));
00190       EXPECT_TRUE((jnt_pos_in(i) <= chain_info.limits[i].max_position));
00191       EXPECT_TRUE((jnt_pos_in(i) >= chain_info.limits[i].min_position));
00192     }
00193 //   jnt_pos_test = jnt_pos_in;
00194     for(int i=0; i < 7; i++)
00195     {
00196       jnt_pos_test(i) = gen_rand(std::max(chain_info.limits[i].min_position,-M_PI),std::min(chain_info.limits[i].max_position,M_PI));
00197       EXPECT_TRUE((jnt_pos_test(i) <= chain_info.limits[i].max_position));
00198       EXPECT_TRUE((jnt_pos_test(i) >= chain_info.limits[i].min_position));
00199       }
00200 // 
00201     if(jnt_to_pose_solver->JntToCart(jnt_pos_in,p_out) >=0)
00202     {
00203       arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00204 //      jnt_pos_test(free_angle) = gen_rand(std::max(chain_info.limits[free_angle].min_position,-M_PI),std::min(chain_info.limits[free_angle].max_position,M_PI));
00205       bool ik_valid = (ik.CartToJntSearch(jnt_pos_test,p_out,jnt_pos_out,10) >= 0);
00206       if(ik_valid)
00207       {
00208         num_solutions++;
00209         for(int k=0; k < (int)jnt_pos_out.size(); k++)
00210         {
00211           jnt_to_pose_solver->JntToCart(jnt_pos_out[k],p_ik);
00212           for(int j=0; j< 3; j++)
00213           {
00214             EXPECT_NEAR(p_ik.M(j,0),p_out.M(j,0),IK_NEAR);
00215             EXPECT_NEAR(p_ik.M(j,1),p_out.M(j,1),IK_NEAR); 
00216             EXPECT_NEAR(p_ik.M(j,2),p_out.M(j,2),IK_NEAR); 
00217             EXPECT_NEAR(p_ik.p(j),p_out.p(j),IK_NEAR_TRANSLATE);
00218           }
00219 //          std::cout << p_ik << std::endl;
00220 //          std::cout << p_out << std::endl;
00221         }
00222       }
00223       else
00224       {
00225         ROS_INFO("Failed solution");
00226         for(int m=0; m < 3; m++)
00227         {
00228           printf("%f %f %f %f\n",p_out.M(m,0),p_out.M(m,1),p_out.M(m,2),p_out.p(m));
00229         }
00230         printf("\n");
00231         ROS_INFO("Original joint values");
00232           for(int n = 0; n< 7; n++)
00233           {
00234             printf("%f ",jnt_pos_in(n));
00235           }
00236           printf("\n");
00237           ROS_INFO("Guess: %f",jnt_pos_test(2));
00238           /*        ROS_INFO("Solution");
00239         for(int m=0; m < (int) ik.pr2_arm_ik_.solution_ik_.size(); m++)
00240         {
00241           for(int n = 0; n< 7; n++)
00242           {
00243             printf("%f ",ik.pr2_arm_ik_.solution_ik_[m][n]);
00244           }
00245           printf("\n");
00246           }*/
00247         ROS_INFO("\n\n\n");
00248       }
00249     }
00250   }
00251   bool success = ((double)num_solutions)/((double) num_tests) > 0.99;
00252   ROS_INFO("Success rate is %f",((double)num_solutions)/((double) num_tests));
00253 //  EXPECT_EQ(num_solutions,num_tests);
00254   EXPECT_TRUE(success);
00255   delete jnt_to_pose_solver;
00256 }
00257 
00258 
00259 int main(int argc, char **argv){
00260   testing::InitGoogleTest(&argc, argv);
00261   ros::init (argc, argv, "pr2_ik_regression_test");
00262 //  ros::Node n("pr2_ik_regression_test");
00263   return RUN_ALL_TESTS();
00264 }
test_ik_regress.cpp
