$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00037 #include <kinematics_msgs/GetConstraintAwarePositionIK.h> 00038 #include <planning_environment/models/collision_models.h> 00039 #include <ros/time.h> 00040 #include <gtest/gtest.h> 00041 #include <iostream> 00042 #include <sstream> 00043 #include <fstream> 00044 #include <ros/package.h> 00045 #include <kinematics_msgs/GetPositionIK.h> 00046 #include <kinematics_msgs/GetPositionFK.h> 00047 #include <kinematics_msgs/GetKinematicSolverInfo.h> 00048 #include <kinematics_msgs/GetConstraintAwarePositionIK.h> 00049 #include <arm_navigation_msgs/SetPlanningSceneDiff.h> 00050 #include <std_srvs/Empty.h> 00051 #include <planning_environment/models/model_utils.h> 00052 00053 static const std::string ARM_FK_NAME = "/pr2_right_arm_kinematics/get_fk"; 00054 static const std::string ARM_IK_NAME = "/pr2_right_arm_kinematics/get_ik"; 00055 static const std::string ARM_COLLISION_IK_NAME = "/pr2_right_arm_kinematics/get_constraint_aware_ik"; 00056 static const std::string ARM_QUERY_NAME = "/pr2_right_arm_kinematics/get_ik_solver_info"; 00057 static const std::string SET_PLANNING_SCENE_DIFF_SERVICE="/environment_server/set_planning_scene_diff"; 00058 00059 double gen_rand(double min, double max) 00060 { 00061 int rand_num = rand()%100+1; 00062 double result = min + (double)((max-min)*rand_num)/101.0; 00063 return result; 00064 } 00065 00066 std::vector<double> generateRandomValues(std::vector<double> min_values, 00067 std::vector<double> max_values) { 00068 std::vector<double> ret_vec; 00069 for(unsigned int i = 0; i < min_values.size(); i++) { 00070 ret_vec.push_back(gen_rand(min_values[i], max_values[i])); 00071 } 00072 return ret_vec; 00073 } 00074 00075 class TestConstraintAwareKinematics : public testing::Test 00076 { 00077 protected: 00078 00079 virtual void SetUp() { 00080 collision_models_ = new planning_environment::CollisionModels("robot_description"); 00081 00082 ready_ = false; 00083 done_ = false; 00084 00085 srand(time(NULL)); 00086 00087 ros::service::waitForService(ARM_QUERY_NAME); 00088 ros::service::waitForService(ARM_FK_NAME); 00089 ros::service::waitForService(ARM_COLLISION_IK_NAME); 00090 ros::service::waitForService(SET_PLANNING_SCENE_DIFF_SERVICE); 00091 00092 query_client_ = nh_.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(ARM_QUERY_NAME); 00093 fk_client_ = nh_.serviceClient<kinematics_msgs::GetPositionFK>(ARM_FK_NAME); 00094 ik_with_collision_client_ = nh_.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK>(ARM_COLLISION_IK_NAME); 00095 ik_client_ = nh_.serviceClient<kinematics_msgs::GetPositionIK>(ARM_IK_NAME); 00096 set_planning_scene_diff_client_ = nh_.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_SERVICE); 00097 00098 //first getting limits 00099 kinematics_msgs::GetKinematicSolverInfo::Request request; 00100 kinematics_msgs::GetKinematicSolverInfo::Response response; 00101 00102 bool service_call = query_client_.call(request, response); 00103 ASSERT_TRUE(service_call); 00104 ASSERT_TRUE(response.kinematic_solver_info.joint_names.size() == response.kinematic_solver_info.limits.size()); 00105 ASSERT_TRUE(!response.kinematic_solver_info.link_names.empty()); 00106 00107 std::vector<double> min_limits, max_limits; 00108 joint_names_ = response.kinematic_solver_info.joint_names; 00109 for(unsigned int i=0; i< joint_names_.size(); i++) { 00110 min_limits_.push_back(response.kinematic_solver_info.limits[i].min_position); 00111 max_limits_.push_back(response.kinematic_solver_info.limits[i].max_position); 00112 } 00113 ik_link_name_ = response.kinematic_solver_info.link_names[0]; 00114 00115 arm_navigation_msgs::SetPlanningSceneDiff::Request set_req; 00116 00117 //now dealing with planning scene 00118 arm_navigation_msgs::CollisionObject table; 00119 table.header.stamp = ros::Time::now(); 00120 table.header.frame_id = "odom_combined"; 00121 table.id = "table"; 00122 table.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD; 00123 table.shapes.resize(1); 00124 table.shapes[0].type = arm_navigation_msgs::Shape::BOX; 00125 table.shapes[0].dimensions.resize(3); 00126 table.shapes[0].dimensions[0] = 1.0; 00127 table.shapes[0].dimensions[1] = 1.0; 00128 table.shapes[0].dimensions[1] = .05; 00129 table.poses.resize(1); 00130 table.poses[0].position.x = 1.0; 00131 table.poses[0].position.y = 0; 00132 table.poses[0].position.z = .5; 00133 table.poses[0].orientation.w = 1.0; 00134 00135 set_req.planning_scene_diff.collision_objects.push_back(table); 00136 00137 arm_navigation_msgs::AttachedCollisionObject att_box; 00138 00139 att_box.object.header.stamp = ros::Time::now(); 00140 att_box.object.header.frame_id = "r_gripper_palm_link"; 00141 att_box.link_name = "r_gripper_palm_link"; 00142 att_box.object.id = "att_box"; 00143 att_box.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD; 00144 att_box.object.shapes.resize(1); 00145 att_box.object.shapes[0].type = arm_navigation_msgs::Shape::BOX; 00146 att_box.object.shapes[0].dimensions.resize(3); 00147 att_box.object.shapes[0].dimensions[0] = .04; 00148 att_box.object.shapes[0].dimensions[1] = .04; 00149 att_box.object.shapes[0].dimensions[2] = .2; 00150 att_box.object.poses.resize(1); 00151 att_box.object.poses[0].position.x = .12; 00152 att_box.object.poses[0].position.y = 0; 00153 att_box.object.poses[0].position.z = 0; 00154 att_box.object.poses[0].orientation.x = 0; 00155 att_box.object.poses[0].orientation.y = 0; 00156 att_box.object.poses[0].orientation.z = 0; 00157 att_box.object.poses[0].orientation.w = 1; 00158 00159 std::vector<std::string> touch_links; 00160 00161 touch_links.push_back("r_gripper_palm_link"); 00162 touch_links.push_back("r_gripper_l_finger_link"); 00163 touch_links.push_back("r_gripper_r_finger_link"); 00164 touch_links.push_back("r_gripper_l_finger_link"); 00165 touch_links.push_back("r_gripper_l_finger_tip_link"); 00166 att_box.touch_links = touch_links; 00167 00168 set_req.planning_scene_diff.attached_collision_objects.push_back(att_box); 00169 00170 arm_navigation_msgs::SetPlanningSceneDiff::Response set_res; 00171 ASSERT_TRUE(set_planning_scene_diff_client_.call(set_req, set_res)); 00172 00173 planning_scene_ = set_res.planning_scene; 00174 } 00175 00176 virtual void TearDown() 00177 { 00178 delete collision_models_; 00179 } 00180 00181 protected: 00182 00183 ros::NodeHandle nh_; 00184 00185 ros::ServiceClient query_client_; 00186 ros::ServiceClient fk_client_; 00187 ros::ServiceClient ik_with_collision_client_; 00188 ros::ServiceClient ik_client_; 00189 ros::ServiceClient set_planning_scene_diff_client_; 00190 00191 arm_navigation_msgs::PlanningScene planning_scene_; 00192 00193 std::vector<std::string> joint_names_; 00194 std::vector<double> min_limits_, max_limits_; 00195 std::string ik_link_name_; 00196 00197 planning_environment::CollisionModels* collision_models_; 00198 00199 bool ready_; 00200 bool done_; 00201 }; 00202 00203 TEST_F(TestConstraintAwareKinematics, TestCollisions) 00204 { 00205 kinematics_msgs::GetConstraintAwarePositionIK::Request ik_request; 00206 kinematics_msgs::GetConstraintAwarePositionIK::Response ik_response; 00207 00208 ik_request.ik_request.ik_seed_state.joint_state.name = joint_names_; 00209 ik_request.ik_request.ik_seed_state.joint_state.position.resize(joint_names_.size()); 00210 ik_request.ik_request.ik_link_name = ik_link_name_; 00211 ik_request.timeout = ros::Duration(2.0); 00212 00213 ik_request.ik_request.ik_link_name = "r_wrist_roll_link"; 00214 ik_request.ik_request.pose_stamped.header.frame_id = "base_link"; 00215 ik_request.ik_request.pose_stamped.pose.position.x = 0.52; 00216 ik_request.ik_request.pose_stamped.pose.position.y = -.2; 00217 ik_request.ik_request.pose_stamped.pose.position.z = .8; 00218 00219 ik_request.ik_request.pose_stamped.pose.orientation.x = 0.0; 00220 ik_request.ik_request.pose_stamped.pose.orientation.y = 0.7071; 00221 ik_request.ik_request.pose_stamped.pose.orientation.z = 0.0; 00222 ik_request.ik_request.pose_stamped.pose.orientation.w = 0.7071; 00223 00224 planning_models::KinematicState* kin_state = collision_models_->setPlanningScene(planning_scene_); 00225 00226 for(unsigned int i = 0; i < 25; i++) { 00227 00228 std::map<std::string, double> joint_value_map; 00229 00230 //first finding a valid starting robot state 00231 while(true) { 00232 std::vector<double> vals = generateRandomValues(min_limits_, max_limits_); 00233 for(unsigned int i = 0; i < vals.size(); i++) { 00234 joint_value_map[joint_names_[i]] = vals[i]; 00235 } 00236 kin_state->setKinematicState(joint_value_map); 00237 if(!collision_models_->isKinematicStateInCollision(*kin_state)) { 00238 break; 00239 } 00240 } 00241 00242 planning_environment::convertKinematicStateToRobotState(*kin_state, ros::Time::now(), collision_models_->getWorldFrameId(), 00243 planning_scene_.robot_state); 00244 00245 ik_request.ik_request.ik_seed_state.joint_state.position = generateRandomValues(min_limits_, max_limits_); 00246 ASSERT_TRUE(ik_with_collision_client_.call(ik_request, ik_response)); 00247 00248 EXPECT_EQ(ik_response.error_code.val, ik_response.error_code.SUCCESS); 00249 } 00250 collision_models_->revertPlanningScene(kin_state); 00251 } 00252 00253 int main(int argc, char **argv) 00254 { 00255 testing::InitGoogleTest(&argc, argv); 00256 ros::init(argc, argv, "test_constraint_aware_pr2_kinematics"); 00257 00258 return RUN_ALL_TESTS(); 00259 }