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 <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_ccd/environmentBVH.h>
00044
00045
00046 static const std::string rel_path = "/test_urdf/robot.xml";
00047
00048 typedef collision_space_ccd::EnvironmentModelBVH<collision_checking::OBB> EnvironmentModelOBB;
00049
00050 class TestCollisionSpaceCCD : public testing::Test {
00051 protected:
00052
00053 virtual void SetUp() {
00054
00055 full_path_ = ros::package::getPath("planning_models")+rel_path;
00056
00057 urdf_ok_ = urdf_model_.initFile(full_path_);
00058
00059 std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00060
00061 planning_models::KinematicModel::MultiDofConfig config("base_joint");
00062 config.type = "Planar";
00063 config.parent_frame_id = "base_footprint";
00064 config.child_frame_id = "base_footprint";
00065 multi_dof_configs.push_back(config);
00066
00067 std::vector<planning_models::KinematicModel::GroupConfig> gcs;
00068 planning_models::KinematicModel::GroupConfig left_arm("left_arm",
00069 "torso_lift_link",
00070 "l_wrist_roll_link");
00071
00072 planning_models::KinematicModel::GroupConfig right_arm("right_arm",
00073 "torso_lift_link",
00074 "r_wrist_roll_link");
00075
00076 kinematic_model_ = new planning_models::KinematicModel(urdf_model_,
00077 gcs,
00078 multi_dof_configs);
00079
00080 coll_space_ = new EnvironmentModelOBB();
00081 };
00082
00083 virtual void TearDown() {
00084 delete kinematic_model_;
00085 delete coll_space_;
00086 }
00087
00088 protected:
00089
00090 urdf::Model urdf_model_;
00091 bool urdf_ok_;
00092 std::string full_path_;
00093 EnvironmentModelOBB* coll_space_;
00094 planning_models::KinematicModel* kinematic_model_;
00095 };
00096
00097
00098 TEST_F(TestCollisionSpaceCCD, TestInit) {
00099 std::vector<std::string> links;
00100 kinematic_model_->getLinkModelNames(links);
00101 std::map<std::string, double> link_padding_map;
00102
00103 {
00104 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links,true);
00105 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00106
00107 ASSERT_FALSE(coll_space_->isCollision());
00108 }
00109
00110 {
00111 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links,false);
00112
00113 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00114
00115
00116 ASSERT_TRUE(coll_space_->isCollision());
00117 }
00118
00119
00120 {
00121 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links,false);
00122
00123 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00124
00125
00126 ASSERT_TRUE(coll_space_->isCollision());
00127 }
00128 }
00129
00130
00131 TEST_F(TestCollisionSpaceCCD, TestACM) {
00132 std::vector<std::string> links;
00133 kinematic_model_->getLinkModelNames(links);
00134 std::map<std::string, double> link_padding_map;
00135
00136
00137 {
00138 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
00139 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00140
00141 planning_models::KinematicState state(kinematic_model_);
00142 state.setKinematicStateToDefault();
00143
00144 coll_space_->updateRobotModel(&state);
00145
00146
00147 ASSERT_TRUE(coll_space_->isCollision());
00148
00149
00150 std::vector<collision_space_ccd::EnvironmentModel::AllowedContact> ac;
00151 std::vector<collision_space_ccd::EnvironmentModel::Contact> contacts;
00152
00153 coll_space_->getAllCollisionContacts(ac, contacts, 1);
00154
00155 ASSERT_TRUE(contacts.size() > 1);
00156
00157 for(unsigned int i = 0; i < contacts.size(); i++) {
00158 ASSERT_TRUE(contacts[i].body_type_1 == collision_space_ccd::EnvironmentModel::LINK);
00159 ASSERT_TRUE(contacts[i].body_type_2 == collision_space_ccd::EnvironmentModel::LINK);
00160 ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true));
00161 }
00162
00163 coll_space_->setAlteredCollisionMatrix(acm);
00164
00165
00166 ASSERT_FALSE(coll_space_->isCollision());
00167
00168 coll_space_->revertAlteredCollisionMatrix();
00169 ASSERT_TRUE(coll_space_->isCollision());
00170 }
00171 }
00172
00173 TEST_F(TestCollisionSpaceCCD, TestAttachedObjects)
00174 {
00175 std::vector<std::string> links;
00176 kinematic_model_->getLinkModelNames(links);
00177 std::map<std::string, double> link_padding_map;
00178
00179 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
00180 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00181
00182 {
00183
00184 planning_models::KinematicState state(kinematic_model_);
00185 state.setKinematicStateToDefault();
00186
00187 coll_space_->updateRobotModel(&state);
00188
00189
00190 std::vector<collision_space_ccd::EnvironmentModel::AllowedContact> ac;
00191 std::vector<collision_space_ccd::EnvironmentModel::Contact> contacts;
00192
00193 coll_space_->getAllCollisionContacts(ac, contacts, 1);
00194
00195
00196 for(unsigned int i = 0; i < contacts.size(); i++) {
00197 ASSERT_TRUE(contacts[i].body_type_1 == collision_space_ccd::EnvironmentModel::LINK);
00198 ASSERT_TRUE(contacts[i].body_type_2 == collision_space_ccd::EnvironmentModel::LINK);
00199 ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true));
00200 }
00201
00202 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00203 coll_space_->updateRobotModel(&state);
00204 }
00205
00206
00207 ASSERT_FALSE(coll_space_->isCollision());
00208
00209 const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link");
00210
00211
00212 shapes::Sphere* sphere1 = new shapes::Sphere();
00213 sphere1->radius = .1;
00214
00215 shapes::Box* box2 = new shapes::Box();
00216 box2->size[0] = .05;
00217 box2->size[1] = .05;
00218 box2->size[2] = .05;
00219
00220 std::vector<shapes::Shape*> shape_vector;
00221 shape_vector.push_back(sphere1);
00222
00223 btTransform pose;
00224 pose.setIdentity();
00225
00226 std::vector<btTransform> poses;
00227 poses.push_back(pose);
00228
00229 std::vector<std::string> touch_links;
00230
00231 planning_models::KinematicModel::AttachedBodyModel* ab1 =
00232 new planning_models::KinematicModel::AttachedBodyModel(link, "box_1",
00233 poses,
00234 touch_links,
00235 shape_vector);
00236
00237 kinematic_model_->addAttachedBodyModel(link->getName(), ab1);
00238 coll_space_->updateAttachedBodies();
00239
00240 const collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix& aft_attached
00241 = coll_space_->getDefaultAllowedCollisionMatrix();
00242
00243 ASSERT_TRUE(aft_attached.hasEntry("box_1"));
00244 bool allowed;
00245 EXPECT_TRUE(aft_attached.getAllowedCollision("box_1", link->getName(), allowed));
00246 EXPECT_FALSE(allowed);
00247
00248 {
00249
00250 planning_models::KinematicState state(kinematic_model_);
00251 state.setKinematicStateToDefault();
00252
00253 coll_space_->updateRobotModel(&state);
00254 }
00255
00256
00257 ASSERT_TRUE(coll_space_->isCollision());
00258
00259 kinematic_model_->clearLinkAttachedBodyModel(link->getName(), "box_1");
00260 coll_space_->updateAttachedBodies();
00261
00262 ASSERT_FALSE(aft_attached.hasEntry("box_1"));
00263
00264
00265 shape_vector.clear();
00266 shape_vector.push_back(box2);
00267 pose.getOrigin().setX(.1);
00268 poses.clear();
00269 poses.push_back(pose);
00270 touch_links.push_back("r_gripper_palm_link");
00271 touch_links.push_back("r_gripper_r_finger_link");
00272 touch_links.push_back("r_gripper_l_finger_link");
00273 touch_links.push_back("r_gripper_r_finger_tip_link");
00274 touch_links.push_back("r_gripper_l_finger_tip_link");
00275 touch_links.push_back("base_link");
00276
00277 planning_models::KinematicModel::AttachedBodyModel* ab2 =
00278 new planning_models::KinematicModel::AttachedBodyModel(link, "box_2",
00279 poses,
00280 touch_links,
00281 shape_vector);
00282 kinematic_model_->addAttachedBodyModel(link->getName(), ab2);
00283 coll_space_->updateAttachedBodies();
00284
00285 ASSERT_TRUE(aft_attached.hasEntry("box_2"));
00286 EXPECT_TRUE(aft_attached.getAllowedCollision("box_2", link->getName(), allowed));
00287 EXPECT_TRUE(allowed);
00288
00289 {
00290
00291 planning_models::KinematicState state(kinematic_model_);
00292 state.setKinematicStateToDefault();
00293
00294 coll_space_->updateRobotModel(&state);
00295 }
00296
00297
00298 ASSERT_FALSE(coll_space_->isCollision());
00299 }
00300
00301 TEST_F(TestCollisionSpaceCCD, TestStaticObjects)
00302 {
00303 std::vector<std::string> links;
00304 kinematic_model_->getLinkModelNames(links);
00305 std::map<std::string, double> link_padding_map;
00306
00307 collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
00308 coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00309
00310 {
00311
00312 planning_models::KinematicState state(kinematic_model_);
00313 state.setKinematicStateToDefault();
00314
00315 coll_space_->updateRobotModel(&state);
00316 }
00317
00318 ASSERT_FALSE(coll_space_->isEnvironmentCollision());
00319
00320 shapes::Sphere* sphere1 = new shapes::Sphere();
00321 sphere1->radius = .2;
00322
00323 btTransform pose;
00324 pose.setIdentity();
00325
00326 std::vector<btTransform> poses;
00327 poses.push_back(pose);
00328
00329 std::vector<shapes::Shape*> shape_vector;
00330 shape_vector.push_back(sphere1);
00331
00332 coll_space_->addObjects("obj1", shape_vector, poses);
00333
00334
00335 ASSERT_TRUE(coll_space_->isEnvironmentCollision());
00336
00337
00338
00339
00340
00341 const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link");
00342
00343 shapes::Box* att_box = new shapes::Box();
00344 att_box->size[0] = .05;
00345 att_box->size[1] = .05;
00346 att_box->size[2] = .05;
00347
00348 std::vector<shapes::Shape*> att_shapes;
00349 att_shapes.push_back(att_box);
00350
00351 btTransform att_pose;
00352 att_pose.setIdentity();
00353
00354 std::vector<btTransform> att_poses;
00355 att_poses.push_back(att_pose);
00356
00357 std::vector<std::string> touch_links;
00358 touch_links.push_back("base_link");
00359 touch_links.push_back("base_footprint");
00360
00361 planning_models::KinematicModel::AttachedBodyModel* ab1 =
00362 new planning_models::KinematicModel::AttachedBodyModel(link, "att1",
00363 att_poses,
00364 touch_links,
00365 att_shapes);
00366
00367 kinematic_model_->addAttachedBodyModel(link->getName(), ab1);
00368 coll_space_->updateAttachedBodies();
00369
00370 {
00371
00372 planning_models::KinematicState state(kinematic_model_);
00373 state.setKinematicStateToDefault();
00374
00375 coll_space_->updateRobotModel(&state);
00376 }
00377
00378 ASSERT_TRUE(coll_space_->isEnvironmentCollision());
00379
00380
00381 std::vector<collision_space_ccd::EnvironmentModel::AllowedContact> ac;
00382 std::vector<collision_space_ccd::EnvironmentModel::Contact> contacts;
00383
00384 coll_space_->getAllCollisionContacts(ac, contacts, 1);
00385
00386
00387 for(unsigned int i = 0; i < contacts.size(); i++) {
00388 if(contacts[i].body_type_1 == collision_space_ccd::EnvironmentModel::OBJECT) {
00389 ASSERT_TRUE(contacts[i].body_name_1 == "obj1");
00390 }
00391 if(contacts[i].body_type_2 == collision_space_ccd::EnvironmentModel::OBJECT) {
00392 ASSERT_TRUE(contacts[i].body_name_2 == "obj1");
00393 }
00394 }
00395
00396 acm = coll_space_->getDefaultAllowedCollisionMatrix();
00397 bool allowed;
00398 ASSERT_TRUE(acm.getAllowedCollision("obj1","att1",allowed));
00399 EXPECT_FALSE(allowed);
00400
00401 ASSERT_TRUE(acm.changeEntry(link->getName(), "obj1", true));
00402 ASSERT_TRUE(acm.changeEntry("base_footprint", "obj1", true));
00403 coll_space_->setAlteredCollisionMatrix(acm);
00404
00405 {
00406
00407 planning_models::KinematicState state(kinematic_model_);
00408 state.setKinematicStateToDefault();
00409
00410 coll_space_->updateRobotModel(&state);
00411 }
00412
00413
00414
00415
00416 ASSERT_TRUE(acm.changeEntry("att1", "obj1", true));
00417 coll_space_->setAlteredCollisionMatrix(acm);
00418
00419 allowed = false;
00420 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("att1","obj1", allowed));
00421 EXPECT_TRUE(allowed);
00422
00423 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","att1", allowed));
00424 EXPECT_TRUE(allowed);
00425
00426 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("base_link","obj1", allowed));
00427 EXPECT_TRUE(allowed);
00428
00429 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_link", allowed));
00430 EXPECT_TRUE(allowed);
00431
00432 ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_footprint", allowed));
00433 EXPECT_TRUE(allowed);
00434
00435 EXPECT_FALSE(coll_space_->isEnvironmentCollision());
00436 contacts.clear();
00437
00438 coll_space_->getAllCollisionContacts(ac, contacts, 1);
00439
00440
00441 for(unsigned int i = 0; i < contacts.size(); i++) {
00442 if(contacts[i].body_type_1 == collision_space_ccd::EnvironmentModel::OBJECT) {
00443 ASSERT_TRUE(contacts[i].body_name_1 == "obj1");
00444 ROS_INFO_STREAM(contacts[i].body_name_2);
00445 }
00446 if(contacts[i].body_type_2 == collision_space_ccd::EnvironmentModel::OBJECT) {
00447 ASSERT_TRUE(contacts[i].body_name_2 == "obj1");
00448 ROS_INFO_STREAM(contacts[i].body_name_1);
00449 }
00450 }
00451
00452 }
00453
00454 int main(int argc, char **argv)
00455 {
00456 testing::InitGoogleTest(&argc, argv);
00457 return RUN_ALL_TESTS();
00458 }
00459