00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include <moveit/test_resources/config.h>
00038 #include <moveit/robot_model/robot_model.h>
00039 #include <moveit/robot_state/robot_state.h>
00040 #include <moveit/collision_detection_fcl/collision_world_fcl.h>
00041 #include <moveit/collision_detection_fcl/collision_robot_fcl.h>
00042
00043 #include <urdf_parser/urdf_parser.h>
00044 #include <geometric_shapes/shape_operations.h>
00045
00046 #include <gtest/gtest.h>
00047 #include <sstream>
00048 #include <algorithm>
00049 #include <ctype.h>
00050 #include <fstream>
00051
00052 #include <boost/filesystem.hpp>
00053
00054 typedef collision_detection::CollisionWorldFCL DefaultCWorldType;
00055 typedef collision_detection::CollisionRobotFCL DefaultCRobotType;
00056
00057 static std::string urdf_file = (boost::filesystem::path(MOVEIT_TEST_RESOURCES_DIR) / "urdf/robot.xml").string();
00058 static std::string srdf_file = (boost::filesystem::path(MOVEIT_TEST_RESOURCES_DIR) / "srdf/robot.xml").string();
00059 static std::string kinect_dae_file = (boost::filesystem::path(MOVEIT_TEST_RESOURCES_DIR) / "urdf/meshes/sensors/kinect_v0/kinect.dae").string();
00060
00061 class FclCollisionDetectionTester : public testing::Test
00062 {
00063
00064 protected:
00065
00066 virtual void SetUp()
00067 {
00068 srdf_model_.reset(new srdf::Model());
00069 std::string xml_string;
00070 std::fstream xml_file(urdf_file.c_str(), std::fstream::in);
00071
00072 if (xml_file.is_open())
00073 {
00074 while ( xml_file.good() )
00075 {
00076 std::string line;
00077 std::getline( xml_file, line);
00078 xml_string += (line + "\n");
00079 }
00080 xml_file.close();
00081 urdf_model_ = urdf::parseURDF(xml_string);
00082 urdf_ok_ = urdf_model_;
00083 }
00084 else
00085 {
00086 EXPECT_EQ("FAILED TO OPEN FILE", urdf_file);
00087 urdf_ok_ = false;
00088 }
00089 srdf_ok_ = srdf_model_->initFile(*urdf_model_, srdf_file);
00090
00091 kmodel_.reset(new robot_model::RobotModel(urdf_model_, srdf_model_));
00092
00093 acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));
00094
00095 crobot_.reset(new DefaultCRobotType(kmodel_));
00096 cworld_.reset(new DefaultCWorldType());
00097 }
00098
00099 virtual void TearDown()
00100 {
00101
00102 }
00103
00104 protected:
00105
00106 bool urdf_ok_;
00107 bool srdf_ok_;
00108
00109 boost::shared_ptr<urdf::ModelInterface> urdf_model_;
00110 boost::shared_ptr<srdf::Model> srdf_model_;
00111
00112 robot_model::RobotModelPtr kmodel_;
00113
00114 boost::shared_ptr<collision_detection::CollisionRobot> crobot_;
00115 boost::shared_ptr<collision_detection::CollisionWorld> cworld_;
00116
00117 collision_detection::AllowedCollisionMatrixPtr acm_;
00118
00119 };
00120
00121
00122 TEST_F(FclCollisionDetectionTester, InitOK)
00123 {
00124 ASSERT_TRUE(urdf_ok_);
00125 ASSERT_TRUE(srdf_ok_);
00126 }
00127
00128 TEST_F(FclCollisionDetectionTester, DefaultNotInCollision)
00129 {
00130 robot_state::RobotState kstate(kmodel_);
00131 kstate.setToDefaultValues();
00132
00133 collision_detection::CollisionRequest req;
00134 collision_detection::CollisionResult res;
00135 crobot_->checkSelfCollision(req, res, kstate, *acm_);
00136 ASSERT_FALSE(res.collision);
00137 }
00138
00139
00140 TEST_F(FclCollisionDetectionTester, LinksInCollision)
00141 {
00142 collision_detection::CollisionRequest req;
00143 collision_detection::CollisionResult res1;
00144 collision_detection::CollisionResult res2;
00145 collision_detection::CollisionResult res3;
00146
00147
00148
00149 robot_state::RobotState kstate(kmodel_);
00150 kstate.setToDefaultValues();
00151
00152 Eigen::Affine3d offset = Eigen::Affine3d::Identity();
00153 offset.translation().x() = .01;
00154
00155
00156
00157 kstate.updateStateWithLinkAt("base_link", Eigen::Affine3d::Identity());
00158 kstate.updateStateWithLinkAt("base_bellow_link", offset);
00159
00160 acm_->setEntry("base_link", "base_bellow_link", false);
00161 crobot_->checkSelfCollision(req, res1, kstate, *acm_);
00162 ASSERT_TRUE(res1.collision);
00163
00164 acm_->setEntry("base_link", "base_bellow_link", true);
00165 crobot_->checkSelfCollision(req, res2, kstate, *acm_);
00166 ASSERT_FALSE(res2.collision);
00167
00168
00169
00170
00171 kstate.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Affine3d::Identity());
00172 kstate.updateStateWithLinkAt("l_gripper_palm_link", offset);
00173
00174 acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
00175 crobot_->checkSelfCollision(req, res3, kstate, *acm_);
00176 ASSERT_TRUE(res3.collision);
00177 }
00178
00179
00180 TEST_F(FclCollisionDetectionTester, ContactReporting)
00181 {
00182 collision_detection::CollisionRequest req;
00183 req.contacts = true;
00184 req.max_contacts = 1;
00185
00186 robot_state::RobotState kstate(kmodel_);
00187 kstate.setToDefaultValues();
00188
00189 Eigen::Affine3d offset = Eigen::Affine3d::Identity();
00190 offset.translation().x() = .01;
00191
00192
00193
00194
00195
00196
00197 kstate.updateStateWithLinkAt("base_link", Eigen::Affine3d::Identity());
00198 kstate.updateStateWithLinkAt("base_bellow_link", offset);
00199 kstate.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Affine3d::Identity());
00200 kstate.updateStateWithLinkAt("l_gripper_palm_link", offset);
00201
00202 acm_->setEntry("base_link", "base_bellow_link", false);
00203 acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
00204
00205 collision_detection::CollisionResult res;
00206 crobot_->checkSelfCollision(req, res, kstate, *acm_);
00207 ASSERT_TRUE(res.collision);
00208 EXPECT_EQ(res.contacts.size(),1);
00209 EXPECT_EQ(res.contacts.begin()->second.size(),1);
00210
00211 res.clear();
00212 req.max_contacts = 2;
00213 req.max_contacts_per_pair = 1;
00214
00215 crobot_->checkSelfCollision(req, res, kstate, *acm_);
00216 ASSERT_TRUE(res.collision);
00217 EXPECT_EQ(res.contacts.size(), 2);
00218 EXPECT_EQ(res.contacts.begin()->second.size(),1);
00219
00220 res.contacts.clear();
00221 res.contact_count = 0;
00222
00223 req.max_contacts = 10;
00224 req.max_contacts_per_pair = 2;
00225 acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), false));
00226 crobot_->checkSelfCollision(req, res, kstate, *acm_);
00227 ASSERT_TRUE(res.collision);
00228 EXPECT_LE(res.contacts.size(), 10);
00229 EXPECT_LE(res.contact_count, 10);
00230
00231 }
00232
00233 TEST_F(FclCollisionDetectionTester, ContactPositions)
00234 {
00235 collision_detection::CollisionRequest req;
00236 req.contacts = true;
00237 req.max_contacts = 1;
00238
00239 robot_state::RobotState kstate(kmodel_);
00240 kstate.setToDefaultValues();
00241
00242 Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
00243 Eigen::Affine3d pos2 = Eigen::Affine3d::Identity();
00244
00245 pos1.translation().x() = 5.0;
00246 pos2.translation().x() = 5.01;
00247
00248
00249
00250 kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
00251 kstate.updateStateWithLinkAt("l_gripper_palm_link", pos2);
00252
00253 acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
00254
00255 collision_detection::CollisionResult res;
00256 crobot_->checkSelfCollision(req, res, kstate, *acm_);
00257 ASSERT_TRUE(res.collision);
00258 ASSERT_EQ(res.contacts.size(),1);
00259 ASSERT_EQ(res.contacts.begin()->second.size(),1);
00260
00261 for(collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
00262 it != res.contacts.end();
00263 it++) {
00264 EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33);
00265 }
00266
00267 pos1 = Eigen::Affine3d(Eigen::Translation3d(3.0,0.0,0.0)*Eigen::Quaterniond::Identity());
00268 pos2 = Eigen::Affine3d(Eigen::Translation3d(3.0,0.0,0.0)*Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0));
00269
00270
00271 kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
00272 kstate.updateStateWithLinkAt("l_gripper_palm_link", pos2);
00273
00274 collision_detection::CollisionResult res2;
00275 crobot_->checkSelfCollision(req, res2, kstate, *acm_);
00276 ASSERT_TRUE(res2.collision);
00277 ASSERT_EQ(res2.contacts.size(),1);
00278 ASSERT_EQ(res2.contacts.begin()->second.size(),1);
00279
00280 for(collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin();
00281 it != res2.contacts.end();
00282 it++) {
00283 EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33);
00284 }
00285
00286 pos1 = Eigen::Affine3d(Eigen::Translation3d(3.0,0.0,0.0)*Eigen::Quaterniond::Identity());
00287 pos2 = Eigen::Affine3d(Eigen::Translation3d(3.0,0.0,0.0)*Eigen::Quaterniond(M_PI/4.0, 0.0, M_PI/4.0, 0.0));
00288
00289
00290 kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
00291 kstate.updateStateWithLinkAt("l_gripper_palm_link", pos2);
00292
00293 collision_detection::CollisionResult res3;
00294 crobot_->checkSelfCollision(req, res2, kstate, *acm_);
00295 ASSERT_FALSE(res3.collision);
00296 }
00297
00298 TEST_F(FclCollisionDetectionTester, AttachedBodyTester) {
00299 collision_detection::CollisionRequest req;
00300 collision_detection::CollisionResult res;
00301
00302 acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));
00303
00304 robot_state::RobotState kstate(kmodel_);
00305 kstate.setToDefaultValues();
00306
00307 Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
00308 pos1.translation().x() = 5.0;
00309
00310
00311 kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
00312 crobot_->checkSelfCollision(req, res, kstate, *acm_);
00313 ASSERT_FALSE(res.collision);
00314
00315 shapes::Shape* shape = new shapes::Box(.1,.1,.1);
00316 cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
00317
00318 res = collision_detection::CollisionResult();
00319 cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
00320 ASSERT_TRUE(res.collision);
00321
00322
00323 cworld_->getWorld()->removeObject("box");
00324
00325 shape = new shapes::Box(.1,.1,.1);
00326 std::vector<shapes::ShapeConstPtr> shapes;
00327 EigenSTL::vector_Affine3d poses;
00328 shapes.push_back(shapes::ShapeConstPtr(shape));
00329 poses.push_back(Eigen::Affine3d::Identity());
00330 std::vector<std::string> touch_links;
00331 kstate.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
00332
00333 res = collision_detection::CollisionResult();
00334 crobot_->checkSelfCollision(req, res, kstate, *acm_);
00335 ASSERT_TRUE(res.collision);
00336
00337
00338 kstate.clearAttachedBody("box");
00339
00340 touch_links.push_back("r_gripper_palm_link");
00341 shapes[0].reset(new shapes::Box(.1,.1,.1));
00342 kstate.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
00343
00344 res = collision_detection::CollisionResult();
00345 crobot_->checkSelfCollision(req, res, kstate, *acm_);
00346 ASSERT_FALSE(res.collision);
00347
00348 pos1.translation().x() = 5.01;
00349 shapes::Shape* coll = new shapes::Box(.1, .1, .1);
00350 cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
00351 res = collision_detection::CollisionResult();
00352 cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
00353 ASSERT_TRUE(res.collision);
00354
00355 acm_->setEntry("coll", "r_gripper_palm_link", true);
00356 res = collision_detection::CollisionResult();
00357 cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
00358 ASSERT_TRUE(res.collision);
00359 }
00360
00361 TEST_F(FclCollisionDetectionTester, DiffSceneTester)
00362 {
00363 robot_state::RobotState kstate(kmodel_);
00364 kstate.setToDefaultValues();
00365
00366 collision_detection::CollisionRequest req;
00367 collision_detection::CollisionResult res;
00368
00369 collision_detection::CollisionRobotFCL new_crobot(*(dynamic_cast<collision_detection::CollisionRobotFCL*>(crobot_.get())));
00370
00371 ros::WallTime before = ros::WallTime::now();
00372 new_crobot.checkSelfCollision(req,res,kstate);
00373 double first_check = (ros::WallTime::now()-before).toSec();
00374 before = ros::WallTime::now();
00375 new_crobot.checkSelfCollision(req,res,kstate);
00376 double second_check = (ros::WallTime::now()-before).toSec();
00377
00378 EXPECT_LT(fabs(first_check-second_check), .05);
00379
00380 std::vector<shapes::ShapeConstPtr> shapes;
00381 shapes.resize(1);
00382
00383 boost::filesystem::path path(boost::filesystem::current_path());
00384
00385 shapes[0].reset(shapes::createMeshFromResource("file://"+path.string()+"/"+kinect_dae_file));
00386
00387 EigenSTL::vector_Affine3d poses;
00388 poses.push_back(Eigen::Affine3d::Identity());
00389
00390 std::vector<std::string> touch_links;
00391 kstate.attachBody("kinect", shapes, poses, touch_links, "r_gripper_palm_link");
00392
00393 before = ros::WallTime::now();
00394 new_crobot.checkSelfCollision(req,res,kstate);
00395 first_check = (ros::WallTime::now()-before).toSec();
00396 before = ros::WallTime::now();
00397 new_crobot.checkSelfCollision(req,res,kstate);
00398 second_check = (ros::WallTime::now()-before).toSec();
00399
00400
00401 EXPECT_LT(second_check, .1);
00402
00403 collision_detection::CollisionRobotFCL other_new_crobot(*(dynamic_cast<collision_detection::CollisionRobotFCL*>(crobot_.get())));
00404 before = ros::WallTime::now();
00405 new_crobot.checkSelfCollision(req,res,kstate);
00406 first_check = (ros::WallTime::now()-before).toSec();
00407 before = ros::WallTime::now();
00408 new_crobot.checkSelfCollision(req,res,kstate);
00409 second_check = (ros::WallTime::now()-before).toSec();
00410
00411 EXPECT_LT(fabs(first_check-second_check), .05);
00412
00413 }
00414
00415 TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached)
00416 {
00417 collision_detection::CollisionRequest req;
00418 collision_detection::CollisionResult res;
00419
00420 boost::filesystem::path path(boost::filesystem::current_path());
00421 shapes::ShapeConstPtr shape(shapes::createMeshFromResource("file://"+path.string()+"/"+kinect_dae_file));
00422 Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
00423 Eigen::Affine3d pos2 = Eigen::Affine3d::Identity();
00424 pos2.translation().x() = 10.0;
00425
00426 cworld_->getWorld()->addToObject("kinect", shape, pos1);
00427
00428 robot_state::RobotState kstate(kmodel_);
00429 kstate.setToDefaultValues();
00430
00431 ros::WallTime before = ros::WallTime::now();
00432 cworld_->checkRobotCollision(req, res, *crobot_, kstate);
00433 double first_check = (ros::WallTime::now()-before).toSec();
00434 before = ros::WallTime::now();
00435 cworld_->checkRobotCollision(req, res, *crobot_, kstate);
00436 double second_check = (ros::WallTime::now()-before).toSec();
00437
00438 EXPECT_LT(second_check, .05);
00439
00440 collision_detection::CollisionWorld::ObjectPtr object = cworld_->getWorld()->getObject("kinect");
00441 cworld_->getWorld()->removeObject("kinect");
00442
00443 robot_state::RobotState kstate1(kmodel_);
00444 robot_state::RobotState kstate2(kmodel_);
00445 kstate1.setToDefaultValues();
00446 kstate2.setToDefaultValues();
00447
00448 std::vector<std::string> touch_links;
00449 kstate1.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link");
00450
00451 EigenSTL::vector_Affine3d other_poses;
00452 other_poses.push_back(pos2);
00453
00454
00455 kstate2.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link");
00456
00457
00458 res = collision_detection::CollisionResult();
00459 crobot_->checkSelfCollision(req,res,kstate1);
00460
00461 EXPECT_TRUE(res.collision);
00462
00463 before = ros::WallTime::now();
00464 crobot_->checkSelfCollision(req,res,kstate1, *acm_);
00465 first_check = (ros::WallTime::now()-before).toSec();
00466 before = ros::WallTime::now();
00467 req.verbose = true;
00468 res = collision_detection::CollisionResult();
00469 crobot_->checkSelfCollision(req,res,kstate2, *acm_);
00470 second_check = (ros::WallTime::now()-before).toSec();
00471
00472 EXPECT_LT(first_check, .05);
00473 EXPECT_LT(fabs(first_check-second_check), .1);
00474 }
00475
00476
00477
00478 TEST_F(FclCollisionDetectionTester, TestCollisionMapAdditionSpeed)
00479 {
00480 EigenSTL::vector_Affine3d poses;
00481 std::vector<shapes::ShapeConstPtr> shapes;
00482 for(unsigned int i = 0; i < 10000; i++) {
00483 poses.push_back(Eigen::Affine3d::Identity());
00484 shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01)));
00485 }
00486 ros::WallTime start = ros::WallTime::now();
00487 cworld_->getWorld()->addToObject("map", shapes, poses);
00488 double t = (ros::WallTime::now()-start).toSec();
00489 EXPECT_GE(1.0, t);
00490
00491
00492 logInform("Adding boxes took %g", t);
00493 }
00494
00495
00496 TEST_F(FclCollisionDetectionTester, MoveMesh)
00497 {
00498 robot_state::RobotState kstate1(kmodel_);
00499 kstate1.setToDefaultValues();
00500
00501 Eigen::Affine3d kinect_pose;
00502 kinect_pose.setIdentity();
00503 shapes::ShapePtr kinect_shape;
00504 boost::filesystem::path path(boost::filesystem::current_path());
00505 kinect_shape.reset(shapes::createMeshFromResource("file://"+path.string()+"/"+kinect_dae_file));
00506
00507 cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
00508
00509 Eigen::Affine3d np;
00510 for(unsigned int i = 0; i < 5 ; i++)
00511 {
00512 np = Eigen::Translation3d(i*.001, i*.001, i*.001)*Eigen::Quaterniond::Identity();
00513 cworld_->getWorld()->moveShapeInObject("kinect", kinect_shape, np);
00514 collision_detection::CollisionRequest req;
00515 collision_detection::CollisionResult res;
00516 cworld_->checkCollision(req, res, *crobot_, kstate1, *acm_);
00517 }
00518
00519
00520 }
00521
00522 TEST_F(FclCollisionDetectionTester, TestChangingShapeSize)
00523 {
00524 robot_state::RobotState kstate1(kmodel_);
00525 kstate1.setToDefaultValues();
00526
00527 collision_detection::CollisionRequest req1;
00528 collision_detection::CollisionResult res1;
00529
00530
00531 ASSERT_FALSE(res1.collision);
00532
00533 EigenSTL::vector_Affine3d poses;
00534 std::vector<shapes::ShapeConstPtr> shapes;
00535 for(unsigned int i = 0; i < 5; i++)
00536 {
00537 cworld_->getWorld()->removeObject("shape");
00538 shapes.clear();
00539 poses.clear();
00540 shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1+i*.0001, 1+i*.0001, 1+i*.0001)));
00541 poses.push_back(Eigen::Affine3d::Identity());
00542 cworld_->getWorld()->addToObject("shape", shapes, poses);
00543 collision_detection::CollisionRequest req;
00544 collision_detection::CollisionResult res;
00545 cworld_->checkCollision(req, res, *crobot_, kstate1, *acm_);
00546 ASSERT_TRUE(res.collision);
00547 }
00548
00549 Eigen::Affine3d kinect_pose;
00550 shapes::ShapePtr kinect_shape;
00551 boost::filesystem::path path(boost::filesystem::current_path());
00552 kinect_shape.reset(shapes::createMeshFromResource("file://"+path.string()+"/"+kinect_dae_file));
00553 cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
00554 collision_detection::CollisionRequest req2;
00555 collision_detection::CollisionResult res2;
00556 cworld_->checkCollision(req2, res2, *crobot_, kstate1, *acm_);
00557 ASSERT_TRUE(res2.collision);
00558 for(unsigned int i = 0; i < 5; i++) {
00559 cworld_->getWorld()->removeObject("shape");
00560 shapes.clear();
00561 poses.clear();
00562 shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1+i*.0001, 1+i*.0001, 1+i*.0001)));
00563 poses.push_back(Eigen::Affine3d::Identity());
00564 cworld_->getWorld()->addToObject("shape", shapes, poses);
00565 collision_detection::CollisionRequest req;
00566 collision_detection::CollisionResult res;
00567 cworld_->checkCollision(req, res, *crobot_, kstate1, *acm_);
00568 ASSERT_TRUE(res.collision);
00569 }
00570 }
00571
00572
00573 int main(int argc, char **argv)
00574 {
00575 testing::InitGoogleTest(&argc, argv);
00576 return RUN_ALL_TESTS();
00577 }