pr2_arm_ik_tests: pr2_arm_ik_tests: test_ik_with

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 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00041 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00042 
00043 #include <gtest/gtest.h>
00044 
00045 #define IK_NEAR 1e-4
00046 #define IK_NEAR_TRANSLATE 1e-5
00047 
00048 static const std::string ARM_FK_NAME = "/pr2_right_arm_kinematics/get_fk";
00049 static const std::string ARM_IK_NAME = "/pr2_right_arm_kinematics/get_ik";
00050 static const std::string ARM_COLLISION_IK_NAME = "/pr2_right_arm_kinematics/get_constraint_aware_ik";
00051 static const std::string ARM_QUERY_NAME = "/pr2_right_arm_kinematics/get_ik_solver_info";
00052 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
00053 
00054 
00055 static const int NUM_TESTS = 1000;
00056 
00057 double gen_rand(double min, double max)
00058 {
00059   int rand_num = rand()%100+1;
00060   double result = min + (double)((max-min)*rand_num)/101.0;
00061   return result;
00062 }
00063 
00064 bool NOT_NEAR(const double &v1, const double &v2, const double &NEAR)
00065 {
00066   if(fabs(v1-v2) > NEAR)
00067     return true;
00068   return false;
00069 }
00070 
00071 TEST(PR2ArmIKWithCollisionNode, inverseKinematics)
00072 {
00073   srand ( time(NULL) ); // initialize random seed
00074   ros::NodeHandle rh;
00075   ros::service::waitForService(ARM_QUERY_NAME);
00076   ros::service::waitForService(ARM_FK_NAME);
00077   ros::service::waitForService(ARM_COLLISION_IK_NAME);
00078   ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
00079 
00080   ros::ServiceClient set_planning_scene_diff_client = rh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);
00081   ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(ARM_QUERY_NAME);
00082   ros::ServiceClient fk_client = rh.serviceClient<kinematics_msgs::GetPositionFK>(ARM_FK_NAME);
00083   ros::ServiceClient ik_with_collision_client = rh.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK>(ARM_COLLISION_IK_NAME);
00084   ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::GetPositionIK>(ARM_IK_NAME);
00085 
00086   // define the service messages
00087   kinematics_msgs::GetKinematicSolverInfo::Request request;
00088   kinematics_msgs::GetKinematicSolverInfo::Response response;
00089 
00090   arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
00091   arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
00092 
00093   if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) {
00094     ROS_WARN("Can't set planning scene");
00095     return;
00096   }
00097 
00098   bool service_call = query_client.call(request, response);
00099   ASSERT_TRUE(service_call);
00100   ASSERT_TRUE(response.kinematic_solver_info.joint_names.size() == response.kinematic_solver_info.limits.size());
00101   ASSERT_TRUE(!response.kinematic_solver_info.link_names.empty());
00102 
00103   int num_joints;
00104   std::vector<double> min_limits, max_limits;
00105   num_joints = (int) response.kinematic_solver_info.joint_names.size();
00106   for(int i=0; i< num_joints; i++)
00107   {
00108     min_limits.push_back(response.kinematic_solver_info.limits[i].min_position);
00109     max_limits.push_back(response.kinematic_solver_info.limits[i].max_position);
00110   }
00111 
00112   kinematics_msgs::GetConstraintAwarePositionIK::Request  ik_request;
00113   kinematics_msgs::GetConstraintAwarePositionIK::Response ik_response;
00114 
00115   ik_request.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
00116   ik_request.ik_request.ik_seed_state.joint_state.position.resize(num_joints);
00117   ik_request.ik_request.ik_link_name = response.kinematic_solver_info.link_names[0];
00118   ik_request.timeout = ros::Duration(2.0);
00119   for(int i=0; i< num_joints; i++)
00120   {
00121     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));
00122   }
00123 
00124   ik_request.ik_request.ik_link_name = "r_wrist_roll_link";
00125   ik_request.ik_request.pose_stamped.header.frame_id = "torso_lift_link";
00126   ik_request.ik_request.pose_stamped.pose.position.x = 0.45;
00127   ik_request.ik_request.pose_stamped.pose.position.y = 0.0;
00128   ik_request.ik_request.pose_stamped.pose.position.z = 0.0;
00129 
00130   ik_request.ik_request.pose_stamped.pose.orientation.x = 0.0;
00131   ik_request.ik_request.pose_stamped.pose.orientation.y = 0.0;
00132   ik_request.ik_request.pose_stamped.pose.orientation.z = 0.0;
00133   ik_request.ik_request.pose_stamped.pose.orientation.w = 1.0;
00134 
00135 
00136   //Setup visibility constraints  
00137   arm_navigation_msgs::PositionConstraint pc;
00138   pc.header.frame_id = "torso_lift_link";
00139   pc.header.stamp = ros::Time();
00140   pc.link_name = "r_elbow_flex_link";
00141   pc.position.x = 0.45;
00142   pc.position.y = 0.0;
00143   pc.position.z = 1.0;
00144 
00145   pc.constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
00146   pc.constraint_region_shape.dimensions.push_back(1.8);
00147   pc.constraint_region_shape.dimensions.push_back(1.8);
00148   pc.constraint_region_shape.dimensions.push_back(1.8);
00149   pc.constraint_region_orientation.w = 1.0;
00150   ik_request.constraints.position_constraints.push_back(pc);
00151 
00152 
00153   bool ik_service_call = ik_with_collision_client.call(ik_request,ik_response);
00154   if(!ik_service_call)
00155   {
00156     ROS_ERROR("IK service call failed");
00157   }
00158   ROS_INFO("IK response code: %d",(int)ik_response.error_code.val);
00159   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());
00160   ASSERT_TRUE(ik_response.error_code.val == ik_response.error_code.SUCCESS);
00161 
00162 
00163 
00164   kinematics_msgs::GetPositionFK::Request  fk_request;
00165   kinematics_msgs::GetPositionFK::Response fk_response;
00166   fk_request.header.frame_id = "torso_lift_link";
00167   fk_request.fk_link_names.resize(1);
00168   fk_request.fk_link_names[0] = "r_elbow_flex_link";
00169   fk_request.robot_state.joint_state.position = ik_response.solution.joint_state.position;
00170   fk_request.robot_state.joint_state.name = ik_response.solution.joint_state.name;
00171   if(fk_client.call(fk_request, fk_response))
00172   {
00173     if(fk_response.error_code.val == fk_response.error_code.SUCCESS)
00174     {
00175       for(unsigned int i=0; i < fk_response.pose_stamped.size(); i ++)
00176       {
00177         ROS_INFO_STREAM("Link    : " << fk_response.fk_link_names[i].c_str());
00178         ROS_INFO_STREAM("Position: " << 
00179           fk_response.pose_stamped[i].pose.position.x << "," <<  
00180           fk_response.pose_stamped[i].pose.position.y << "," << 
00181           fk_response.pose_stamped[i].pose.position.z);
00182         ROS_INFO("Orientation: %f %f %f %f",
00183           fk_response.pose_stamped[i].pose.orientation.x,
00184           fk_response.pose_stamped[i].pose.orientation.y,
00185           fk_response.pose_stamped[i].pose.orientation.z,
00186           fk_response.pose_stamped[i].pose.orientation.w);
00187       } 
00188     }
00189     else
00190     {
00191       ROS_ERROR("Forward kinematics failed");
00192     }
00193   }
00194   else
00195   {
00196     ROS_ERROR("Forward kinematics service call failed");
00197   }
00198 
00199   kinematics_msgs::GetPositionIK::Request  pos_ik_request;
00200   kinematics_msgs::GetPositionIK::Response pos_ik_response;
00201   pos_ik_request.ik_request = ik_request.ik_request;
00202   pos_ik_request.timeout = ik_request.timeout;
00203   bool ik_call = ik_client.call(pos_ik_request,pos_ik_response);
00204   ASSERT_TRUE(ik_call);
00205   ASSERT_TRUE(pos_ik_response.error_code.val == pos_ik_response.error_code.SUCCESS);
00206   ros::shutdown();
00207 }
00208 
00209 int main(int argc, char **argv){
00210   testing::InitGoogleTest(&argc, argv);
00211   ros::init (argc, argv, "pr2_ik_with_collision_regression_test");
00212   return RUN_ALL_TESTS();
00213 }
test_ik_with_constraints.cpp
