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