$search
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 }