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