$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 #include <collision_space/environmentODE.h> 00044 #include <boost/thread.hpp> 00045 #include <boost/bind.hpp> 00046 00047 //urdf location relative to the planning_models path 00048 static const std::string rel_path = "/test_urdf/robot.xml"; 00049 00050 class TestCollisionSpace : public testing::Test { 00051 public: 00052 00053 void spinThread() { 00054 lock_.lock(); 00055 coll_space_->isCollision(); 00056 lock_.unlock(); 00057 } 00058 00059 protected: 00060 00061 virtual void SetUp() { 00062 00063 full_path_ = ros::package::getPath("planning_models")+rel_path; 00064 00065 urdf_ok_ = urdf_model_.initFile(full_path_); 00066 00067 std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs; 00068 //now this should work with an identity transform 00069 planning_models::KinematicModel::MultiDofConfig config("base_joint"); 00070 config.type = "Planar"; 00071 config.parent_frame_id = "base_footprint"; 00072 config.child_frame_id = "base_footprint"; 00073 multi_dof_configs.push_back(config); 00074 00075 std::vector<planning_models::KinematicModel::GroupConfig> gcs; 00076 planning_models::KinematicModel::GroupConfig left_arm("left_arm", 00077 "torso_lift_link", 00078 "l_wrist_roll_link"); 00079 00080 planning_models::KinematicModel::GroupConfig right_arm("right_arm", 00081 "torso_lift_link", 00082 "r_wrist_roll_link"); 00083 00084 kinematic_model_ = new planning_models::KinematicModel(urdf_model_, 00085 gcs, 00086 multi_dof_configs); 00087 coll_space_ = new collision_space::EnvironmentModelODE(); 00088 }; 00089 00090 virtual void TearDown() { 00091 delete kinematic_model_; 00092 delete coll_space_; 00093 } 00094 protected: 00095 00096 boost::mutex lock_; 00097 00098 urdf::Model urdf_model_; 00099 bool urdf_ok_; 00100 std::string full_path_; 00101 collision_space::EnvironmentModelODE* coll_space_; 00102 planning_models::KinematicModel* kinematic_model_; 00103 }; 00104 00105 00106 TEST_F(TestCollisionSpace, TestInit) { 00107 std::vector<std::string> links; 00108 kinematic_model_->getLinkModelNames(links); 00109 std::map<std::string, double> link_padding_map; 00110 00111 { 00112 collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links,true); 00113 00114 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00115 00116 //all AllowedCollisions set to true, so no collision 00117 ASSERT_FALSE(coll_space_->isCollision()); 00118 } 00119 00120 { 00121 collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links,false); 00122 00123 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00124 00125 //now we are in collision with nothing disabled 00126 ASSERT_TRUE(coll_space_->isCollision()); 00127 } 00128 00129 //one more time for good measure 00130 { 00131 collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links,false); 00132 00133 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00134 00135 //now we are in collision with nothing disabled 00136 ASSERT_TRUE(coll_space_->isCollision()); 00137 } 00138 } 00139 00140 00141 TEST_F(TestCollisionSpace, TestACM) { 00142 std::vector<std::string> links; 00143 kinematic_model_->getLinkModelNames(links); 00144 std::map<std::string, double> link_padding_map; 00145 00146 //first we get 00147 { 00148 collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links, false); 00149 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00150 00151 planning_models::KinematicState state(kinematic_model_); 00152 state.setKinematicStateToDefault(); 00153 00154 coll_space_->updateRobotModel(&state); 00155 00156 //at default state in collision 00157 ASSERT_TRUE(coll_space_->isCollision()); 00158 00159 //now we get the full set of collisions in the default state 00160 std::vector<collision_space::EnvironmentModel::Contact> contacts; 00161 00162 coll_space_->getAllCollisionContacts(contacts, 1); 00163 00164 ASSERT_TRUE(contacts.size() > 1); 00165 //now we change all these pairwise to true 00166 for(unsigned int i = 0; i < contacts.size(); i++) { 00167 ASSERT_TRUE(contacts[i].body_type_1 == collision_space::EnvironmentModel::LINK); 00168 ASSERT_TRUE(contacts[i].body_type_2 == collision_space::EnvironmentModel::LINK); 00169 ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true)); 00170 } 00171 00172 coll_space_->setAlteredCollisionMatrix(acm); 00173 00174 //with all of these disabled, no more collisions 00175 ASSERT_FALSE(coll_space_->isCollision()); 00176 00177 coll_space_->revertAlteredCollisionMatrix(); 00178 ASSERT_TRUE(coll_space_->isCollision()); 00179 } 00180 } 00181 00182 TEST_F(TestCollisionSpace, TestAttachedObjects) 00183 { 00184 std::vector<std::string> links; 00185 kinematic_model_->getLinkModelNames(links); 00186 std::map<std::string, double> link_padding_map; 00187 00188 collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links, false); 00189 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00190 00191 { 00192 //indented cause the state needs to cease to exist before we add the attached body 00193 planning_models::KinematicState state(kinematic_model_); 00194 state.setKinematicStateToDefault(); 00195 00196 coll_space_->updateRobotModel(&state); 00197 00198 //now we get the full set of collisions in the default state 00199 std::vector<collision_space::EnvironmentModel::Contact> contacts; 00200 00201 coll_space_->getAllCollisionContacts(contacts, 1); 00202 00203 //now we change all these pairwise to true 00204 for(unsigned int i = 0; i < contacts.size(); i++) { 00205 ASSERT_TRUE(contacts[i].body_type_1 == collision_space::EnvironmentModel::LINK); 00206 ASSERT_TRUE(contacts[i].body_type_2 == collision_space::EnvironmentModel::LINK); 00207 ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true)); 00208 } 00209 00210 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00211 coll_space_->updateRobotModel(&state); 00212 } 00213 00214 //now we shouldn't be in collision 00215 ASSERT_FALSE(coll_space_->isCollision()); 00216 00217 const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link"); 00218 00219 //first a single box 00220 shapes::Sphere* sphere1 = new shapes::Sphere(); 00221 sphere1->radius = .1; 00222 00223 shapes::Box* box2 = new shapes::Box(); 00224 box2->size[0] = .05; 00225 box2->size[1] = .05; 00226 box2->size[2] = .05; 00227 00228 std::vector<shapes::Shape*> shape_vector; 00229 shape_vector.push_back(sphere1); 00230 00231 btTransform pose; 00232 pose.setIdentity(); 00233 00234 std::vector<btTransform> poses; 00235 poses.push_back(pose); 00236 00237 std::vector<std::string> touch_links; 00238 00239 planning_models::KinematicModel::AttachedBodyModel* ab1 = 00240 new planning_models::KinematicModel::AttachedBodyModel(link, "box_1", 00241 poses, 00242 touch_links, 00243 shape_vector); 00244 00245 kinematic_model_->addAttachedBodyModel(link->getName(), ab1); 00246 coll_space_->updateAttachedBodies(); 00247 00248 const collision_space::EnvironmentModel::AllowedCollisionMatrix& aft_attached 00249 = coll_space_->getDefaultAllowedCollisionMatrix(); 00250 00251 ASSERT_TRUE(aft_attached.hasEntry("box_1")); 00252 bool allowed; 00253 EXPECT_TRUE(aft_attached.getAllowedCollision("box_1", link->getName(), allowed)); 00254 EXPECT_FALSE(allowed); 00255 00256 { 00257 //indented cause the state needs to cease to exist before we add the attached body 00258 planning_models::KinematicState state(kinematic_model_); 00259 state.setKinematicStateToDefault(); 00260 00261 coll_space_->updateRobotModel(&state); 00262 } 00263 00264 //now it collides 00265 ASSERT_TRUE(coll_space_->isCollision()); 00266 00267 kinematic_model_->clearLinkAttachedBodyModel(link->getName(), "box_1"); 00268 coll_space_->updateAttachedBodies(); 00269 00270 ASSERT_FALSE(aft_attached.hasEntry("box_1")); 00271 00272 //now adding an attached object with two boxes, this time with two objects 00273 shape_vector.clear(); 00274 shape_vector.push_back(box2); 00275 pose.getOrigin().setX(.1); 00276 poses.clear(); 00277 poses.push_back(pose); 00278 touch_links.push_back("r_gripper_palm_link"); 00279 touch_links.push_back("r_gripper_r_finger_link"); 00280 touch_links.push_back("r_gripper_l_finger_link"); 00281 touch_links.push_back("r_gripper_r_finger_tip_link"); 00282 touch_links.push_back("r_gripper_l_finger_tip_link"); 00283 touch_links.push_back("base_link"); 00284 00285 planning_models::KinematicModel::AttachedBodyModel* ab2 = 00286 new planning_models::KinematicModel::AttachedBodyModel(link, "box_2", 00287 poses, 00288 touch_links, 00289 shape_vector); 00290 kinematic_model_->addAttachedBodyModel(link->getName(), ab2); 00291 coll_space_->updateAttachedBodies(); 00292 00293 ASSERT_TRUE(aft_attached.hasEntry("box_2")); 00294 EXPECT_TRUE(aft_attached.getAllowedCollision("box_2", link->getName(), allowed)); 00295 EXPECT_TRUE(allowed); 00296 00297 { 00298 //indented cause the state needs to cease to exist before we add the attached body 00299 planning_models::KinematicState state(kinematic_model_); 00300 state.setKinematicStateToDefault(); 00301 00302 coll_space_->updateRobotModel(&state); 00303 } 00304 00305 //now it doesn't collide 00306 ASSERT_FALSE(coll_space_->isCollision()); 00307 } 00308 00309 TEST_F(TestCollisionSpace, TestStaticObjects) 00310 { 00311 std::vector<std::string> links; 00312 kinematic_model_->getLinkModelNames(links); 00313 std::map<std::string, double> link_padding_map; 00314 00315 collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links, false); 00316 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00317 00318 { 00319 //indented cause the state needs to cease to exist before we add the attached body 00320 planning_models::KinematicState state(kinematic_model_); 00321 state.setKinematicStateToDefault(); 00322 00323 coll_space_->updateRobotModel(&state); 00324 } 00325 00326 ASSERT_FALSE(coll_space_->isEnvironmentCollision()); 00327 00328 shapes::Sphere* sphere1 = new shapes::Sphere(); 00329 sphere1->radius = .2; 00330 00331 btTransform pose; 00332 pose.setIdentity(); 00333 00334 std::vector<btTransform> poses; 00335 poses.push_back(pose); 00336 00337 std::vector<shapes::Shape*> shape_vector; 00338 shape_vector.push_back(sphere1); 00339 00340 coll_space_->addObjects("obj1", shape_vector, poses); 00341 00342 ASSERT_TRUE(coll_space_->isEnvironmentCollision()); 00343 00344 00345 00346 //Now test interactions between static and attached objects 00347 00348 const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link"); 00349 00350 shapes::Box* att_box = new shapes::Box(); 00351 att_box->size[0] = .05; 00352 att_box->size[1] = .05; 00353 att_box->size[2] = .05; 00354 00355 std::vector<shapes::Shape*> att_shapes; 00356 att_shapes.push_back(att_box); 00357 00358 btTransform att_pose; 00359 att_pose.setIdentity(); 00360 00361 std::vector<btTransform> att_poses; 00362 att_poses.push_back(att_pose); 00363 00364 std::vector<std::string> touch_links; 00365 touch_links.push_back("base_link"); 00366 touch_links.push_back("base_footprint"); 00367 00368 planning_models::KinematicModel::AttachedBodyModel* ab1 = 00369 new planning_models::KinematicModel::AttachedBodyModel(link, "att1", 00370 att_poses, 00371 touch_links, 00372 att_shapes); 00373 00374 kinematic_model_->addAttachedBodyModel(link->getName(), ab1); 00375 coll_space_->updateAttachedBodies(); 00376 00377 { 00378 //indented cause the state needs to cease to exist before we add the attached body 00379 planning_models::KinematicState state(kinematic_model_); 00380 state.setKinematicStateToDefault(); 00381 00382 coll_space_->updateRobotModel(&state); 00383 } 00384 00385 ASSERT_TRUE(coll_space_->isEnvironmentCollision()); 00386 00387 //now we get the full set of collisions in the default state 00388 std::vector<collision_space::EnvironmentModel::Contact> contacts; 00389 00390 coll_space_->getAllCollisionContacts(contacts, 1); 00391 00392 //now we change all these pairwise to true 00393 for(unsigned int i = 0; i < contacts.size(); i++) { 00394 if(contacts[i].body_type_1 == collision_space::EnvironmentModel::OBJECT) { 00395 ASSERT_TRUE(contacts[i].body_name_1 == "obj1"); 00396 } 00397 if(contacts[i].body_type_2 == collision_space::EnvironmentModel::OBJECT) { 00398 ASSERT_TRUE(contacts[i].body_name_2 == "obj1"); 00399 } 00400 } 00401 00402 acm = coll_space_->getDefaultAllowedCollisionMatrix(); 00403 bool allowed; 00404 ASSERT_TRUE(acm.getAllowedCollision("obj1","att1",allowed)); 00405 EXPECT_FALSE(allowed); 00406 00407 ASSERT_TRUE(acm.changeEntry(link->getName(), "obj1", true)); 00408 ASSERT_TRUE(acm.changeEntry("base_footprint", "obj1", true)); 00409 coll_space_->setAlteredCollisionMatrix(acm); 00410 00411 { 00412 //indented cause the state needs to cease to exist before we add the attached body 00413 planning_models::KinematicState state(kinematic_model_); 00414 state.setKinematicStateToDefault(); 00415 00416 coll_space_->updateRobotModel(&state); 00417 } 00418 00419 EXPECT_TRUE(coll_space_->isEnvironmentCollision()); 00420 00421 ASSERT_TRUE(acm.changeEntry("att1", "obj1", true)); 00422 coll_space_->setAlteredCollisionMatrix(acm); 00423 00424 allowed = false; 00425 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("att1","obj1", allowed)); 00426 EXPECT_TRUE(allowed); 00427 00428 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","att1", allowed)); 00429 EXPECT_TRUE(allowed); 00430 00431 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("base_link","obj1", allowed)); 00432 EXPECT_TRUE(allowed); 00433 00434 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_link", allowed)); 00435 EXPECT_TRUE(allowed); 00436 00437 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_footprint", allowed)); 00438 EXPECT_TRUE(allowed); 00439 00440 EXPECT_FALSE(coll_space_->isEnvironmentCollision()); 00441 contacts.clear(); 00442 00443 coll_space_->getAllCollisionContacts(contacts, 1); 00444 00445 //now we change all these pairwise to true 00446 for(unsigned int i = 0; i < contacts.size(); i++) { 00447 if(contacts[i].body_type_1 == collision_space::EnvironmentModel::OBJECT) { 00448 ASSERT_TRUE(contacts[i].body_name_1 == "obj1"); 00449 ROS_INFO_STREAM(contacts[i].body_name_2); 00450 } 00451 if(contacts[i].body_type_2 == collision_space::EnvironmentModel::OBJECT) { 00452 ASSERT_TRUE(contacts[i].body_name_2 == "obj1"); 00453 ROS_INFO_STREAM(contacts[i].body_name_1); 00454 } 00455 } 00456 00457 } 00458 00459 TEST_F(TestCollisionSpace, TestAllowedContacts) 00460 { 00461 std::vector<std::string> links; 00462 kinematic_model_->getLinkModelNames(links); 00463 std::map<std::string, double> link_padding_map; 00464 00465 collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links, true); 00466 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map); 00467 00468 { 00469 //indented cause the state needs to cease to exist before we add the attached body 00470 planning_models::KinematicState state(kinematic_model_); 00471 state.setKinematicStateToDefault(); 00472 00473 coll_space_->updateRobotModel(&state); 00474 } 00475 00476 ASSERT_FALSE(coll_space_->isEnvironmentCollision()); 00477 00478 shapes::Sphere* sphere1 = new shapes::Sphere(.2); 00479 shapes::Box* box1 = new shapes::Box(.4, .4, .4); 00480 shapes::Box* box1a = new shapes::Box(.4, .4, .4); 00481 00482 btTransform pose; 00483 pose.setIdentity(); 00484 00485 std::vector<btTransform> poses; 00486 poses.push_back(pose); 00487 00488 std::vector<shapes::Shape*> shape_vector_1; 00489 shape_vector_1.push_back(sphere1); 00490 00491 coll_space_->addObjects("obj1", shape_vector_1, poses); 00492 00493 std::vector<shapes::Shape*> shape_vector_2; 00494 shape_vector_2.push_back(box1); 00495 shape_vector_2.push_back(box1a); 00496 pose.getOrigin().setX(.25); 00497 poses.push_back(pose); 00498 00499 coll_space_->addObjects("obj2", shape_vector_2, poses); 00500 00501 ASSERT_TRUE(coll_space_->isEnvironmentCollision()); 00502 00503 std::vector<collision_space::EnvironmentModel::Contact> contacts; 00504 ASSERT_TRUE(coll_space_->getAllCollisionContacts(contacts)); 00505 00506 std::vector<collision_space::EnvironmentModel::AllowedContact> allowed; 00507 00508 for(unsigned int i = 0; i < contacts.size(); i++) { 00509 //now we place an allowed contact region around this sphere for each contact 00510 00511 if(contacts[i].body_name_1 == "obj1" || contacts[i].body_name_2 == "obj1") { 00512 00513 shapes::Sphere* sphere2 = new shapes::Sphere(.3); 00514 00515 btTransform trans(btQuaternion(0,0,0,1.0), contacts[i].pos); 00516 00517 boost::shared_ptr<bodies::Sphere> bodysp(new bodies::Sphere(sphere2)); 00518 bodysp->setPose(trans); 00519 00520 delete sphere2; 00521 00522 collision_space::EnvironmentModel::AllowedContact allc; 00523 00524 allc.bound = bodysp; 00525 allc.body_name_1 = contacts[i].body_name_1; 00526 allc.body_name_2 = contacts[i].body_name_2; 00527 allc.depth = 100.0;//contacts[i].depth; 00528 00529 allowed.push_back(allc); 00530 } 00531 if(contacts[i].body_name_1 == "obj2" || contacts[i].body_name_2 == "obj2") { 00532 00533 shapes::Box* box2 = new shapes::Box(.01, .01, .01); 00534 00535 btTransform trans(btQuaternion(0,0,0,1.0), contacts[i].pos); 00536 00537 ROS_DEBUG_STREAM("Making allowed contact for " << contacts[i].body_name_1 00538 << " and " << contacts[i].body_name_2 00539 << " at " << contacts[i].pos.x() << " " 00540 << contacts[i].pos.y() << " " << contacts[i].pos.z()); 00541 00542 00543 boost::shared_ptr<bodies::Box> bodysp(new bodies::Box(box2)); 00544 bodysp->setPose(trans); 00545 00546 delete box2; 00547 00548 collision_space::EnvironmentModel::AllowedContact allc; 00549 00550 allc.bound = bodysp; 00551 allc.body_name_1 = contacts[i].body_name_1; 00552 allc.body_name_2 = contacts[i].body_name_2; 00553 allc.depth = 100.0;//contacts[i].depth; 00554 00555 allowed.push_back(allc); 00556 } 00557 } 00558 coll_space_->setAllowedContacts(allowed); 00559 00560 ASSERT_FALSE(coll_space_->isEnvironmentCollision()); 00561 00562 coll_space_->clearAllowedContacts(); 00563 } 00564 00565 TEST_F(TestCollisionSpace, TestThreading) 00566 { 00567 boost::thread thread1(boost::bind(&TestCollisionSpace::spinThread, this)); 00568 boost::thread thread2(boost::bind(&TestCollisionSpace::spinThread, this)); 00569 boost::thread thread3(boost::bind(&TestCollisionSpace::spinThread, this)); 00570 boost::thread thread4(boost::bind(&TestCollisionSpace::spinThread, this)); 00571 thread1.join(); 00572 thread2.join(); 00573 thread3.join(); 00574 thread4.join(); 00575 } 00576 00577 00578 int main(int argc, char **argv) 00579 { 00580 testing::InitGoogleTest(&argc, argv); 00581 return RUN_ALL_TESTS(); 00582 } 00583