$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 <planning_models/kinematic_model.h> 00038 #include <planning_models/kinematic_state.h> 00039 #include <gtest/gtest.h> 00040 #include <sstream> 00041 #include <ctype.h> 00042 #include <ros/package.h> 00043 00044 #include <collision_space/environmentODE.h> 00045 #include <collision_space_ccd/environmentBVH.h> 00046 00047 #include <tf/transform_datatypes.h> 00048 #include <arm_navigation_msgs/RobotState.h> 00049 00050 #include <geometric_shapes/shape_operations.h> 00051 00052 #define NUM_MESH_OBJECTS 10 00053 #define NUM_RANDOM_OBJECTS 100 00054 #define NUM_TEST_CONFIGURATIONS 1000 00055 00056 typedef collision_space_ccd::EnvironmentModelBVH<collision_checking::OBB> EnvironmentModelOBB; 00057 00058 //urdf location relative to the planning_models path 00059 static const std::string rel_path = "/test_urdf/robot.xml"; 00060 00061 inline double gen_rand(double min, double max) 00062 { 00063 int rand_num = rand()%100+1; 00064 double result = min + (double)((max-min)*rand_num)/101.0; 00065 return result; 00066 } 00067 00068 inline geometry_msgs::Quaternion generateRandomUnitQuaternion() { 00069 00070 geometry_msgs::Quaternion quat; 00071 quat.x = gen_rand(-1.0, 1.0); 00072 quat.w = gen_rand(-1.0, 1.0); 00073 quat.z = gen_rand(-1.0, 1.0); 00074 quat.w = gen_rand(-1.0, 1.0); 00075 00076 double mag = sqrt(pow(quat.x, 2.0)+pow(quat.y, 2.0)+pow(quat.z, 2.0)+pow(quat.w, 2.0)); 00077 00078 quat.x /= mag; 00079 quat.y /= mag; 00080 quat.z /= mag; 00081 quat.w /= mag; 00082 00083 return quat; 00084 } 00085 00086 inline std::string getNumberedString(const std::string &object, unsigned int num) 00087 { 00088 std::stringstream my_string; 00089 my_string << object << num; 00090 return my_string.str(); 00091 } 00092 00093 class TestCollisionSpaceCCD : public testing::Test { 00094 public: 00095 00096 void spinThread() { 00097 lock_.lock(); 00098 coll_space_->isCollision(); 00099 lock_.unlock(); 00100 } 00101 00102 protected: 00103 00104 virtual void SetUp() { 00105 00106 full_path_ = ros::package::getPath("planning_models")+rel_path; 00107 00108 urdf_ok_ = urdf_model_.initFile(full_path_); 00109 00110 std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs; 00111 //now this should work with an identity transform 00112 planning_models::KinematicModel::MultiDofConfig config("base_joint"); 00113 config.type = "Planar"; 00114 config.parent_frame_id = "base_footprint"; 00115 config.child_frame_id = "base_footprint"; 00116 multi_dof_configs.push_back(config); 00117 00118 std::vector<planning_models::KinematicModel::GroupConfig> gcs; 00119 planning_models::KinematicModel::GroupConfig left_arm("left_arm", 00120 "torso_lift_link", 00121 "l_wrist_roll_link"); 00122 00123 planning_models::KinematicModel::GroupConfig right_arm("right_arm", 00124 "torso_lift_link", 00125 "r_wrist_roll_link"); 00126 gcs.push_back(left_arm); 00127 gcs.push_back(right_arm); 00128 kinematic_model_ = new planning_models::KinematicModel(urdf_model_, 00129 gcs, 00130 multi_dof_configs); 00131 coll_space_ode_ = new collision_space::EnvironmentModelODE(); 00132 coll_space_ = new EnvironmentModelOBB(); 00133 00134 workspace_x_extents_ = 1.0; 00135 workspace_y_extents_ = 1.0; 00136 workspace_z_extents_ = 1.0; 00137 ros::Time::init(); 00138 }; 00139 00140 virtual void TearDown() { 00141 delete kinematic_model_; 00142 // delete coll_space_; 00143 // delete coll_space_ode_; 00144 } 00145 00146 void getJointBoundsMap(const std::string &group, std::map<std::string, std::pair<double, double> > &joint_bounds_map) 00147 { 00148 const planning_models::KinematicModel::JointModelGroup* joint_model_group = kinematic_model_->getModelGroup(group); 00149 if(joint_model_group == NULL) { 00150 ROS_WARN_STREAM("No joint group " << group); 00151 return; 00152 } 00153 00154 for(unsigned int i = 0; i < joint_model_group->getJointModels().size(); i++) { 00155 const planning_models::KinematicModel::JointModel* jm = joint_model_group->getJointModels()[i]; 00156 std::pair<double, double> bounds; 00157 jm->getVariableBounds(jm->getName(), bounds); 00158 joint_bounds_map[jm->getName()] = bounds; 00159 } 00160 } 00161 00162 void setupForRandomConfigurations() 00163 { 00164 srand ( time(NULL) ); // initialize random seed: 00165 std::vector<std::string> links; 00166 kinematic_model_->getLinkModelNames(links); 00167 std::map<std::string, double> link_padding_map; 00168 00169 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links, false); 00170 collision_space::EnvironmentModel::AllowedCollisionMatrix acm_ode(links, false); 00171 00172 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00173 coll_space_ode_->setRobotModel(kinematic_model_, acm_ode, link_padding_map); 00174 00175 { 00176 //indented cause the state needs to cease to exist before we add the attached body 00177 planning_models::KinematicState state(kinematic_model_); 00178 state.setKinematicStateToDefault(); 00179 00180 coll_space_->updateRobotModel(&state); 00181 coll_space_ode_->updateRobotModel(&state); 00182 } 00183 00184 ASSERT_FALSE(coll_space_->isEnvironmentCollision()); 00185 ASSERT_FALSE(coll_space_ode_->isEnvironmentCollision()); 00186 } 00187 00188 void testForRandomConfigurations() 00189 { 00190 getJointBoundsMap("right_arm",joint_bounds_map_right_); 00191 getJointBoundsMap("left_arm",joint_bounds_map_left_); 00192 00193 std::vector<arm_navigation_msgs::RobotState> robot_states_right; 00194 std::vector<arm_navigation_msgs::RobotState> robot_states_left; 00195 // Now generate random robot states and test them for collisions 00196 for(unsigned int i=0; i < NUM_TEST_CONFIGURATIONS; i++) 00197 { 00198 arm_navigation_msgs::RobotState robot_state_right = generateRandomRobotStateWithinLimits(joint_bounds_map_right_); 00199 arm_navigation_msgs::RobotState robot_state_left = generateRandomRobotStateWithinLimits(joint_bounds_map_left_); 00200 robot_states_right.push_back(robot_state_right); 00201 robot_states_left.push_back(robot_state_left); 00202 } 00203 00204 std::vector<bool> is_collision, is_collision_ode; 00205 is_collision.resize(robot_states_right.size()); 00206 is_collision_ode.resize(robot_states_right.size()); 00207 00208 { 00209 //indented cause the state needs to cease to exist before we add the attached body 00210 planning_models::KinematicState state(kinematic_model_); 00211 state.setKinematicStateToDefault(); 00212 planning_models::KinematicState::JointStateGroup *right_state = state.getJointStateGroup("right_arm"); 00213 planning_models::KinematicState::JointStateGroup *left_state = state.getJointStateGroup("left_arm"); 00214 00215 ros::Time start_time = ros::Time::now(); 00216 for(unsigned int i=0; i < robot_states_right.size(); i++) 00217 { 00218 right_state->setKinematicState(robot_states_right[i].joint_state.position); 00219 left_state->setKinematicState(robot_states_left[i].joint_state.position); 00220 coll_space_->updateRobotModel(&state); 00221 is_collision[i] = coll_space_->isEnvironmentCollision(); 00222 } 00223 ros::Duration duration = ros::Time::now()-start_time; 00224 ROS_INFO("CCD: Collision time: %f",duration.toSec()); 00225 00226 start_time = ros::Time::now(); 00227 for(unsigned int i=0; i < robot_states_right.size(); i++) 00228 { 00229 right_state->setKinematicState(robot_states_right[i].joint_state.position); 00230 left_state->setKinematicState(robot_states_left[i].joint_state.position); 00231 coll_space_ode_->updateRobotModel(&state); 00232 is_collision_ode[i] = coll_space_ode_->isEnvironmentCollision(); 00233 } 00234 duration = ros::Time::now()-start_time; 00235 ROS_INFO("ODE: Collision time: %f",duration.toSec()); 00236 00237 unsigned int counter = 0; 00238 for(unsigned int i=0; i < robot_states_right.size(); i++) 00239 if(is_collision_ode[i] != is_collision[i]) 00240 counter++; 00241 ROS_DEBUG("%d mismatches between ODE and CCD result (out of %d)",(int) counter,(int) robot_states_right.size()); 00242 00243 ASSERT_TRUE(counter == 0); 00244 } 00245 } 00246 00247 arm_navigation_msgs::RobotState generateRandomRobotStateWithinLimits(std::map<std::string, std::pair<double, double> > &joint_bounds_map) 00248 { 00249 arm_navigation_msgs::RobotState rs; 00250 rs.joint_state.header.frame_id = "torso_lift_link"; 00251 rs.joint_state.header.stamp = ros::Time::now(); 00252 00253 for(std::map<std::string,std::pair<double, double> >::iterator it = joint_bounds_map.begin(); 00254 it != joint_bounds_map.end(); 00255 it++) { 00256 rs.joint_state.name.push_back(it->first); 00257 rs.joint_state.position.push_back(gen_rand(it->second.first, it->second.second)); 00258 } 00259 return rs; 00260 } 00261 00262 geometry_msgs::Pose generateRandomPoseInWorkspace() { 00263 00264 geometry_msgs::Pose rp; 00265 rp.position.x = gen_rand(-workspace_x_extents_, workspace_x_extents_); 00266 rp.position.y = gen_rand(-workspace_y_extents_, workspace_y_extents_); 00267 rp.position.z = gen_rand(-workspace_z_extents_, workspace_z_extents_); 00268 00269 rp.orientation = generateRandomUnitQuaternion(); 00270 00271 return rp; 00272 00273 } 00274 00275 00276 protected: 00277 00278 boost::mutex lock_; 00279 urdf::Model urdf_model_; 00280 bool urdf_ok_; 00281 std::string full_path_; 00282 collision_space::EnvironmentModelODE* coll_space_ode_; 00283 EnvironmentModelOBB* coll_space_; 00284 planning_models::KinematicModel* kinematic_model_; 00285 std::map<std::string, std::pair<double, double> > joint_bounds_map_right_, joint_bounds_map_left_; 00286 double workspace_x_extents_, workspace_y_extents_, workspace_z_extents_; 00287 }; 00288 00289 00290 00291 TEST_F(TestCollisionSpaceCCD, TestInit) { 00292 std::vector<std::string> links; 00293 kinematic_model_->getLinkModelNames(links); 00294 std::map<std::string, double> link_padding_map; 00295 00296 { 00297 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links,true); 00298 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00299 //all AllowedCollisions set to true, so no collision 00300 ASSERT_FALSE(coll_space_->isCollision()); 00301 } 00302 00303 { 00304 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links,false); 00305 00306 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00307 00308 //now we are in collision with nothing disabled 00309 ASSERT_TRUE(coll_space_->isCollision()); 00310 } 00311 00312 //one more time for good measure 00313 { 00314 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links,false); 00315 00316 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00317 00318 //now we are in collision with nothing disabled 00319 ASSERT_TRUE(coll_space_->isCollision()); 00320 } 00321 } 00322 00323 00324 TEST_F(TestCollisionSpaceCCD, TestACM) { 00325 std::vector<std::string> links; 00326 kinematic_model_->getLinkModelNames(links); 00327 std::map<std::string, double> link_padding_map; 00328 00329 //first we get 00330 { 00331 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links, false); 00332 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00333 00334 planning_models::KinematicState state(kinematic_model_); 00335 state.setKinematicStateToDefault(); 00336 coll_space_->updateRobotModel(&state); 00337 00338 //at default state in collision 00339 ASSERT_TRUE(coll_space_->isCollision()); 00340 00341 //now we get the full set of collisions in the default state 00342 std::vector<collision_space_ccd::EnvironmentModel::AllowedContact> ac; 00343 std::vector<collision_space_ccd::EnvironmentModel::Contact> contacts; 00344 00345 coll_space_->getAllCollisionContacts(ac, contacts, 1); 00346 00347 ASSERT_TRUE(contacts.size() > 1); 00348 //now we change all these pairwise to true 00349 for(unsigned int i = 0; i < contacts.size(); i++) { 00350 ASSERT_TRUE(contacts[i].body_type_1 == collision_space_ccd::EnvironmentModel::LINK); 00351 ASSERT_TRUE(contacts[i].body_type_2 == collision_space_ccd::EnvironmentModel::LINK); 00352 ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true)); 00353 } 00354 00355 coll_space_->setAlteredCollisionMatrix(acm); 00356 00357 //with all of these disabled, no more collisions 00358 ASSERT_FALSE(coll_space_->isCollision()); 00359 00360 coll_space_->revertAlteredCollisionMatrix(); 00361 ASSERT_TRUE(coll_space_->isCollision()); 00362 } 00363 } 00364 00365 TEST_F(TestCollisionSpaceCCD, TestAttachedObjects) 00366 { 00367 std::vector<std::string> links; 00368 kinematic_model_->getLinkModelNames(links); 00369 std::map<std::string, double> link_padding_map; 00370 00371 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links, false); 00372 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00373 00374 { 00375 //indented cause the state needs to cease to exist before we add the attached body 00376 planning_models::KinematicState state(kinematic_model_); 00377 state.setKinematicStateToDefault(); 00378 00379 coll_space_->updateRobotModel(&state); 00380 00381 //now we get the full set of collisions in the default state 00382 std::vector<collision_space_ccd::EnvironmentModel::AllowedContact> ac; 00383 std::vector<collision_space_ccd::EnvironmentModel::Contact> contacts; 00384 00385 coll_space_->getAllCollisionContacts(ac, contacts, 1); 00386 00387 //now we change all these pairwise to true 00388 for(unsigned int i = 0; i < contacts.size(); i++) { 00389 ASSERT_TRUE(contacts[i].body_type_1 == collision_space_ccd::EnvironmentModel::LINK); 00390 ASSERT_TRUE(contacts[i].body_type_2 == collision_space_ccd::EnvironmentModel::LINK); 00391 ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true)); 00392 } 00393 00394 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00395 coll_space_->updateRobotModel(&state); 00396 } 00397 00398 //now we shouldn't be in collision 00399 ASSERT_FALSE(coll_space_->isCollision()); 00400 00401 const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link"); 00402 00403 //first a single box 00404 shapes::Sphere* sphere1 = new shapes::Sphere(); 00405 sphere1->radius = .1; 00406 00407 shapes::Box* box2 = new shapes::Box(); 00408 box2->size[0] = .05; 00409 box2->size[1] = .05; 00410 box2->size[2] = .05; 00411 00412 std::vector<shapes::Shape*> shape_vector; 00413 shape_vector.push_back(sphere1); 00414 00415 btTransform pose; 00416 pose.setIdentity(); 00417 00418 std::vector<btTransform> poses; 00419 poses.push_back(pose); 00420 00421 std::vector<std::string> touch_links; 00422 00423 planning_models::KinematicModel::AttachedBodyModel* ab1 = 00424 new planning_models::KinematicModel::AttachedBodyModel(link, "box_1", 00425 poses, 00426 touch_links, 00427 shape_vector); 00428 00429 kinematic_model_->addAttachedBodyModel(link->getName(), ab1); 00430 coll_space_->updateAttachedBodies(); 00431 00432 const collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix& aft_attached 00433 = coll_space_->getDefaultAllowedCollisionMatrix(); 00434 00435 ASSERT_TRUE(aft_attached.hasEntry("box_1")); 00436 bool allowed; 00437 EXPECT_TRUE(aft_attached.getAllowedCollision("box_1", link->getName(), allowed)); 00438 EXPECT_FALSE(allowed); 00439 00440 { 00441 //indented cause the state needs to cease to exist before we add the attached body 00442 planning_models::KinematicState state(kinematic_model_); 00443 state.setKinematicStateToDefault(); 00444 00445 coll_space_->updateRobotModel(&state); 00446 } 00447 00448 //now it collides 00449 ASSERT_TRUE(coll_space_->isCollision()); 00450 00451 kinematic_model_->clearLinkAttachedBodyModel(link->getName(), "box_1"); 00452 coll_space_->updateAttachedBodies(); 00453 00454 ASSERT_FALSE(aft_attached.hasEntry("box_1")); 00455 00456 //now adding an attached object with two boxes, this time with two objects 00457 shape_vector.clear(); 00458 shape_vector.push_back(box2); 00459 pose.getOrigin().setX(.1); 00460 poses.clear(); 00461 poses.push_back(pose); 00462 touch_links.push_back("r_gripper_palm_link"); 00463 touch_links.push_back("r_gripper_r_finger_link"); 00464 touch_links.push_back("r_gripper_l_finger_link"); 00465 touch_links.push_back("r_gripper_r_finger_tip_link"); 00466 touch_links.push_back("r_gripper_l_finger_tip_link"); 00467 touch_links.push_back("base_link"); 00468 00469 planning_models::KinematicModel::AttachedBodyModel* ab2 = 00470 new planning_models::KinematicModel::AttachedBodyModel(link, "box_2", 00471 poses, 00472 touch_links, 00473 shape_vector); 00474 kinematic_model_->addAttachedBodyModel(link->getName(), ab2); 00475 coll_space_->updateAttachedBodies(); 00476 00477 ASSERT_TRUE(aft_attached.hasEntry("box_2")); 00478 EXPECT_TRUE(aft_attached.getAllowedCollision("box_2", link->getName(), allowed)); 00479 EXPECT_TRUE(allowed); 00480 00481 { 00482 //indented cause the state needs to cease to exist before we add the attached body 00483 planning_models::KinematicState state(kinematic_model_); 00484 state.setKinematicStateToDefault(); 00485 00486 coll_space_->updateRobotModel(&state); 00487 } 00488 00489 //now it doesn't collide 00490 ASSERT_FALSE(coll_space_->isCollision()); 00491 } 00492 00493 TEST_F(TestCollisionSpaceCCD, TestStaticObjects) 00494 { 00495 std::vector<std::string> links; 00496 kinematic_model_->getLinkModelNames(links); 00497 std::map<std::string, double> link_padding_map; 00498 00499 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links, false); 00500 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00501 00502 { 00503 //indented cause the state needs to cease to exist before we add the attached body 00504 planning_models::KinematicState state(kinematic_model_); 00505 state.setKinematicStateToDefault(); 00506 00507 coll_space_->updateRobotModel(&state); 00508 } 00509 00510 ASSERT_FALSE(coll_space_->isEnvironmentCollision()); 00511 00512 shapes::Sphere* sphere1 = new shapes::Sphere(); 00513 sphere1->radius = .2; 00514 00515 btTransform pose; 00516 pose.setIdentity(); 00517 00518 std::vector<btTransform> poses; 00519 poses.push_back(pose); 00520 00521 std::vector<shapes::Shape*> shape_vector; 00522 shape_vector.push_back(sphere1); 00523 00524 coll_space_->addObjects("obj1", shape_vector, poses); 00525 00526 00527 ASSERT_TRUE(coll_space_->isEnvironmentCollision()); 00528 00529 00530 00531 //Now test interactions between static and attached objects 00532 00533 const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link"); 00534 00535 shapes::Box* att_box = new shapes::Box(); 00536 att_box->size[0] = .05; 00537 att_box->size[1] = .05; 00538 att_box->size[2] = .05; 00539 00540 std::vector<shapes::Shape*> att_shapes; 00541 att_shapes.push_back(att_box); 00542 00543 btTransform att_pose; 00544 att_pose.setIdentity(); 00545 00546 std::vector<btTransform> att_poses; 00547 att_poses.push_back(att_pose); 00548 00549 std::vector<std::string> touch_links; 00550 touch_links.push_back("base_link"); 00551 touch_links.push_back("base_footprint"); 00552 00553 planning_models::KinematicModel::AttachedBodyModel* ab1 = 00554 new planning_models::KinematicModel::AttachedBodyModel(link, "att1", 00555 att_poses, 00556 touch_links, 00557 att_shapes); 00558 00559 kinematic_model_->addAttachedBodyModel(link->getName(), ab1); 00560 coll_space_->updateAttachedBodies(); 00561 00562 { 00563 //indented cause the state needs to cease to exist before we add the attached body 00564 planning_models::KinematicState state(kinematic_model_); 00565 state.setKinematicStateToDefault(); 00566 00567 coll_space_->updateRobotModel(&state); 00568 } 00569 00570 ASSERT_TRUE(coll_space_->isEnvironmentCollision()); 00571 00572 //now we get the full set of collisions in the default state 00573 std::vector<collision_space_ccd::EnvironmentModel::AllowedContact> ac; 00574 std::vector<collision_space_ccd::EnvironmentModel::Contact> contacts; 00575 00576 coll_space_->getAllCollisionContacts(ac, contacts, 1); 00577 00578 //now we change all these pairwise to true 00579 for(unsigned int i = 0; i < contacts.size(); i++) { 00580 if(contacts[i].body_type_1 == collision_space_ccd::EnvironmentModel::OBJECT) { 00581 ASSERT_TRUE(contacts[i].body_name_1 == "obj1"); 00582 } 00583 if(contacts[i].body_type_2 == collision_space_ccd::EnvironmentModel::OBJECT) { 00584 ASSERT_TRUE(contacts[i].body_name_2 == "obj1"); 00585 } 00586 } 00587 00588 acm = coll_space_->getDefaultAllowedCollisionMatrix(); 00589 bool allowed; 00590 ASSERT_TRUE(acm.getAllowedCollision("obj1","att1",allowed)); 00591 EXPECT_FALSE(allowed); 00592 00593 ASSERT_TRUE(acm.changeEntry(link->getName(), "obj1", true)); 00594 ASSERT_TRUE(acm.changeEntry("base_footprint", "obj1", true)); 00595 coll_space_->setAlteredCollisionMatrix(acm); 00596 00597 { 00598 //indented cause the state needs to cease to exist before we add the attached body 00599 planning_models::KinematicState state(kinematic_model_); 00600 state.setKinematicStateToDefault(); 00601 00602 coll_space_->updateRobotModel(&state); 00603 } 00604 00605 /* Remove because sphere-box collision does not work for mesh */ 00606 // EXPECT_TRUE(coll_space_->isEnvironmentCollision()); 00607 00608 ASSERT_TRUE(acm.changeEntry("att1", "obj1", true)); 00609 coll_space_->setAlteredCollisionMatrix(acm); 00610 00611 allowed = false; 00612 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("att1","obj1", allowed)); 00613 EXPECT_TRUE(allowed); 00614 00615 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","att1", allowed)); 00616 EXPECT_TRUE(allowed); 00617 00618 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("base_link","obj1", allowed)); 00619 EXPECT_TRUE(allowed); 00620 00621 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_link", allowed)); 00622 EXPECT_TRUE(allowed); 00623 00624 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_footprint", allowed)); 00625 EXPECT_TRUE(allowed); 00626 00627 EXPECT_FALSE(coll_space_->isEnvironmentCollision()); 00628 contacts.clear(); 00629 00630 coll_space_->getAllCollisionContacts(ac, contacts, 1); 00631 00632 //now we change all these pairwise to true 00633 for(unsigned int i = 0; i < contacts.size(); i++) { 00634 if(contacts[i].body_type_1 == collision_space_ccd::EnvironmentModel::OBJECT) { 00635 ASSERT_TRUE(contacts[i].body_name_1 == "obj1"); 00636 ROS_INFO_STREAM(contacts[i].body_name_2); 00637 } 00638 if(contacts[i].body_type_2 == collision_space_ccd::EnvironmentModel::OBJECT) { 00639 ASSERT_TRUE(contacts[i].body_name_2 == "obj1"); 00640 ROS_INFO_STREAM(contacts[i].body_name_1); 00641 } 00642 } 00643 } 00644 00645 TEST_F(TestCollisionSpaceCCD, TestODEvsCCDSphere) 00646 { 00647 setupForRandomConfigurations(); 00648 for(unsigned int i=0; i < NUM_RANDOM_OBJECTS; i++) 00649 { 00650 btTransform pose; 00651 shapes::Sphere* sphere1 = new shapes::Sphere(); 00652 sphere1->radius = .1; 00653 00654 geometry_msgs::Pose pose_msg = generateRandomPoseInWorkspace(); 00655 tf::poseMsgToTF(pose_msg,pose); 00656 00657 std::vector<btTransform> poses; 00658 poses.push_back(pose); 00659 00660 std::vector<shapes::Shape*> shape_vector; 00661 shape_vector.push_back(sphere1); 00662 00663 std::string object_id = getNumberedString("obj",i); 00664 00665 coll_space_->addObjects(object_id, shape_vector, poses); 00666 coll_space_ode_->addObjects(object_id, shape_vector, poses); 00667 } 00668 testForRandomConfigurations(); 00669 } 00670 00671 TEST_F(TestCollisionSpaceCCD, TestODEvsCCDCylinder) 00672 { 00673 setupForRandomConfigurations(); 00674 for(unsigned int i=0; i < NUM_RANDOM_OBJECTS; i++) 00675 { 00676 btTransform pose; 00677 shapes::Cylinder* c1 = new shapes::Cylinder(); 00678 c1->radius = .1; 00679 c1->length = 0.3; 00680 00681 geometry_msgs::Pose pose_msg = generateRandomPoseInWorkspace(); 00682 tf::poseMsgToTF(pose_msg,pose); 00683 00684 std::vector<btTransform> poses; 00685 poses.push_back(pose); 00686 00687 std::vector<shapes::Shape*> shape_vector; 00688 shape_vector.push_back(c1); 00689 00690 std::string object_id = getNumberedString("obj",i); 00691 00692 coll_space_->addObjects(object_id, shape_vector, poses); 00693 coll_space_ode_->addObjects(object_id, shape_vector, poses); 00694 } 00695 testForRandomConfigurations(); 00696 } 00697 00698 TEST_F(TestCollisionSpaceCCD, TestODEvsCCDBox) 00699 { 00700 setupForRandomConfigurations(); 00701 for(unsigned int i=0; i < NUM_RANDOM_OBJECTS; i++) 00702 { 00703 btTransform pose; 00704 shapes::Box* box = new shapes::Box(); 00705 box->size[0] = 0.1; 00706 box->size[1] = 0.2; 00707 box->size[2] = 0.3; 00708 00709 geometry_msgs::Pose pose_msg = generateRandomPoseInWorkspace(); 00710 tf::poseMsgToTF(pose_msg,pose); 00711 00712 std::vector<btTransform> poses; 00713 poses.push_back(pose); 00714 00715 std::vector<shapes::Shape*> shape_vector; 00716 shape_vector.push_back(box); 00717 00718 std::string object_id = getNumberedString("obj",i); 00719 00720 coll_space_->addObjects(object_id, shape_vector, poses); 00721 coll_space_ode_->addObjects(object_id, shape_vector, poses); 00722 } 00723 testForRandomConfigurations(); 00724 } 00725 00726 TEST_F(TestCollisionSpaceCCD, TestODEvsCCDMesh) 00727 { 00728 setupForRandomConfigurations(); 00729 std::string full_path = "package://collision_space_ccd_test/objects/meshes/9300.stl"; 00730 for(unsigned int i=0; i < NUM_MESH_OBJECTS; i++) 00731 { 00732 shapes::Mesh* mesh = shapes::createMeshFromFilename(full_path.c_str()); 00733 ASSERT_TRUE(mesh); 00734 btTransform pose; 00735 geometry_msgs::Pose pose_msg = generateRandomPoseInWorkspace(); 00736 tf::poseMsgToTF(pose_msg,pose); 00737 00738 std::vector<btTransform> poses; 00739 poses.push_back(pose); 00740 00741 std::vector<shapes::Shape*> shape_vector; 00742 shape_vector.push_back(mesh); 00743 00744 std::string object_id = getNumberedString("obj",i); 00745 00746 coll_space_->addObjects(object_id, shape_vector, poses); 00747 coll_space_ode_->addObjects(object_id, shape_vector, poses); 00748 delete mesh; 00749 } 00750 testForRandomConfigurations(); 00751 // coll_space_->clearObjects(); 00752 // coll_space_ode_->clearObjects(); 00753 } 00754 00755 int main(int argc, char **argv) 00756 { 00757 testing::InitGoogleTest(&argc, argv); 00758 return RUN_ALL_TESTS(); 00759 } 00760