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_environment/models/collision_models.h>
00038 #include <planning_environment/models/collision_models_interface.h>
00039 #include <planning_models/kinematic_state.h>
00040 #include <ros/time.h>
00041 #include <gtest/gtest.h>
00042 #include <iostream>
00043 #include <sstream>
00044 #include <fstream>
00045 #include <ros/package.h>
00046 #include <planning_environment/models/model_utils.h>
00047
00048 static const std::string rel_path = "/test_urdf/robot.xml";
00049 static const double VERY_SMALL = .0001;
00050
00051 class TestCollisionModels : public testing::Test
00052 {
00053 protected:
00054
00055 virtual void SetUp() {
00056
00057 full_path_ = ros::package::getPath("planning_models")+rel_path;
00058
00059 std::string com = "rosparam set robot_description -t "+full_path_;
00060
00061 int ok = system(com.c_str());
00062
00063 if(ok != 0) {
00064 ROS_WARN_STREAM("Setting parameter system call not ok");
00065 }
00066
00067 static_object_1_.header.stamp = ros::Time::now();
00068 static_object_1_.header.frame_id = "odom_combined";
00069 static_object_1_.id = "object_1";
00070 static_object_1_.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00071 static_object_1_.shapes.resize(1);
00072 static_object_1_.shapes[0].type = arm_navigation_msgs::Shape::CYLINDER;
00073 static_object_1_.shapes[0].dimensions.resize(2);
00074 static_object_1_.shapes[0].dimensions[0] = .1;
00075 static_object_1_.shapes[0].dimensions[1] = 1.5;
00076 static_object_1_.poses.resize(1);
00077 static_object_1_.poses[0].position.x = .75;
00078 static_object_1_.poses[0].position.y = -.37;
00079 static_object_1_.poses[0].position.z = .81;
00080 static_object_1_.poses[0].orientation.w = 1.0;
00081
00082 static_object_2_.header.stamp = ros::Time::now();
00083 static_object_2_.header.frame_id = "odom_combined";
00084 static_object_2_.id = "object_2";
00085 static_object_2_.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00086 static_object_2_.shapes.resize(2);
00087 static_object_2_.shapes[0].type = arm_navigation_msgs::Shape::BOX;
00088 static_object_2_.shapes[0].dimensions.resize(3);
00089 static_object_2_.shapes[0].dimensions[0] = 1.0;
00090 static_object_2_.shapes[0].dimensions[1] = 1.0;
00091 static_object_2_.shapes[0].dimensions[2] = .05;
00092 static_object_2_.shapes[1].type = arm_navigation_msgs::Shape::BOX;
00093 static_object_2_.shapes[1].dimensions.resize(3);
00094 static_object_2_.shapes[1].dimensions[0] = 1.0;
00095 static_object_2_.shapes[1].dimensions[1] = 1.0;
00096 static_object_2_.shapes[1].dimensions[2] = .05;
00097 static_object_2_.poses.resize(2);
00098 static_object_2_.poses[0].position.x = 1.0;
00099 static_object_2_.poses[0].position.y = 0;
00100 static_object_2_.poses[0].position.z = .5;
00101 static_object_2_.poses[0].orientation.x = 0;
00102 static_object_2_.poses[0].orientation.y = 0;
00103 static_object_2_.poses[0].orientation.z = 0;
00104 static_object_2_.poses[0].orientation.w = 1;
00105 static_object_2_.poses[1].position.x = 1.0;
00106 static_object_2_.poses[1].position.y = 0;
00107 static_object_2_.poses[1].position.z = .75;
00108 static_object_2_.poses[1].orientation.x = 0;
00109 static_object_2_.poses[1].orientation.y = 0;
00110 static_object_2_.poses[1].orientation.z = 0;
00111 static_object_2_.poses[1].orientation.w = 1;
00112
00113 static_object_3_.header.stamp = ros::Time::now();
00114 static_object_3_.header.frame_id = "odom_combined";
00115 static_object_3_.id = "object_3";
00116 static_object_3_.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00117 static_object_3_.shapes.resize(1);
00118 static_object_3_.shapes[0].type = arm_navigation_msgs::Shape::BOX;
00119 static_object_3_.shapes[0].dimensions.resize(3);
00120 static_object_3_.shapes[0].dimensions[0] = 1.0;
00121 static_object_3_.shapes[0].dimensions[1] = 1.0;
00122 static_object_3_.shapes[0].dimensions[2] = .05;
00123 static_object_3_.poses.resize(1);
00124 static_object_3_.poses[0].position.x = .15;
00125 static_object_3_.poses[0].position.y = 0;
00126 static_object_3_.poses[0].position.z = .5;
00127 static_object_3_.poses[0].orientation.x = 0;
00128 static_object_3_.poses[0].orientation.y = 0;
00129 static_object_3_.poses[0].orientation.z = 0;
00130 static_object_3_.poses[0].orientation.w = 1;
00131
00132 att_object_1_.object.header.stamp = ros::Time::now();
00133 att_object_1_.object.header.frame_id = "odom_combined";
00134 att_object_1_.link_name = "r_gripper_r_finger_tip_link";
00135 att_object_1_.object.id = "object_4";
00136 att_object_1_.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00137 att_object_1_.object.shapes.resize(1);
00138 att_object_1_.object.shapes[0].type = arm_navigation_msgs::Shape::BOX;
00139 att_object_1_.object.shapes[0].dimensions.resize(3);
00140 att_object_1_.object.shapes[0].dimensions[0] = 1.0;
00141 att_object_1_.object.shapes[0].dimensions[1] = 1.0;
00142 att_object_1_.object.shapes[0].dimensions[2] = .05;
00143 att_object_1_.object.poses.resize(1);
00144 att_object_1_.object.poses[0].position.x = .15;
00145 att_object_1_.object.poses[0].position.y = 0;
00146 att_object_1_.object.poses[0].position.z = .5;
00147 att_object_1_.object.poses[0].orientation.x = 0;
00148 att_object_1_.object.poses[0].orientation.y = 0;
00149 att_object_1_.object.poses[0].orientation.z = 0;
00150 att_object_1_.object.poses[0].orientation.w = 1;
00151
00152 }
00153
00154 protected:
00155
00156 arm_navigation_msgs::CollisionObject static_object_1_;
00157 arm_navigation_msgs::CollisionObject static_object_2_;
00158 arm_navigation_msgs::CollisionObject static_object_3_;
00159
00160 arm_navigation_msgs::AttachedCollisionObject att_object_1_;
00161
00162 ros::NodeHandle nh_;
00163 std::string full_path_;
00164 };
00165
00166
00167 TEST_F(TestCollisionModels, NotInCollisionByDefault)
00168 {
00169
00170 planning_environment::CollisionModels cm("robot_description");
00171
00172 planning_models::KinematicState state(cm.getKinematicModel());
00173
00174 state.setKinematicStateToDefault();
00175
00176 EXPECT_FALSE(cm.isKinematicStateInCollision(state));
00177
00178 std::vector<arm_navigation_msgs::ContactInformation> contacts;
00179 cm.getAllCollisionsForState(state, contacts,1);
00180
00181 EXPECT_EQ(contacts.size(),0);
00182
00183 arm_navigation_msgs::OrderedCollisionOperations ord;
00184 ord.collision_operations.resize(1);
00185 ord.collision_operations[0].object1 = ord.collision_operations[0].COLLISION_SET_ALL;
00186 ord.collision_operations[0].object2 = ord.collision_operations[0].COLLISION_SET_ALL;
00187 ord.collision_operations[0].operation = ord.collision_operations[0].ENABLE;
00188
00189 cm.applyOrderedCollisionOperationsToCollisionSpace(ord);
00190
00191 EXPECT_TRUE(cm.isKinematicStateInCollision(state));
00192
00193 cm.getAllCollisionsForState(state, contacts,1);
00194
00195 EXPECT_GE(contacts.size(),0);
00196
00197 }
00198
00199
00200 TEST_F(TestCollisionModels, TestModelUtils)
00201 {
00202
00203 planning_environment::CollisionModels cm("robot_description");
00204
00205 planning_models::KinematicState state(cm.getKinematicModel());
00206
00207 state.setKinematicStateToDefault();
00208
00209 arm_navigation_msgs::RobotState rs;
00210 planning_environment::convertKinematicStateToRobotState(state,
00211 ros::Time::now(),
00212 cm.getWorldFrameId(),
00213 rs);
00214
00215 ASSERT_EQ(rs.multi_dof_joint_state.poses[0].position.x, 0.0);
00216 ASSERT_EQ(rs.multi_dof_joint_state.poses[0].position.y, 0.0);
00217 ASSERT_EQ(rs.multi_dof_joint_state.poses[0].position.z, 0.0);
00218 ASSERT_EQ(rs.multi_dof_joint_state.poses[0].orientation.x, 0.0);
00219 ASSERT_EQ(rs.multi_dof_joint_state.poses[0].orientation.y, 0.0);
00220 ASSERT_EQ(rs.multi_dof_joint_state.poses[0].orientation.z, 0.0);
00221 ASSERT_EQ(rs.multi_dof_joint_state.poses[0].orientation.w, 1.0);
00222 ASSERT_EQ(rs.multi_dof_joint_state.joint_names[0], std::string("base_joint"));
00223 ASSERT_EQ(rs.multi_dof_joint_state.frame_ids[0], cm.getWorldFrameId());
00224 ASSERT_EQ(rs.multi_dof_joint_state.child_frame_ids[0], cm.getRobotFrameId());
00225
00226 ASSERT_TRUE(planning_environment::setRobotStateAndComputeTransforms(rs,state));
00227
00228 EXPECT_EQ(rs.joint_state.name[0],std::string("floating_trans_x"));
00229 rs.joint_state.name.erase(rs.joint_state.name.begin());
00230 rs.joint_state.position.erase(rs.joint_state.position.begin());
00231
00232 EXPECT_NE(rs.joint_state.name[0],std::string("floating_trans_x"));
00233
00234
00235 ASSERT_TRUE(planning_environment::setRobotStateAndComputeTransforms(rs,state));
00236
00237
00238 rs.multi_dof_joint_state.frame_ids.clear();
00239 ASSERT_FALSE(planning_environment::setRobotStateAndComputeTransforms(rs,state));
00240 }
00241
00242
00243 TEST_F(TestCollisionModels,TestCollisionObjects)
00244 {
00245 planning_environment::CollisionModels cm("robot_description");
00246
00247 cm.addStaticObject(static_object_2_);
00248 cm.addStaticObject(static_object_3_);
00249
00250 std::vector<arm_navigation_msgs::CollisionObject> space_objs;
00251 cm.getCollisionSpaceCollisionObjects(space_objs);
00252
00253 std::vector<arm_navigation_msgs::AttachedCollisionObject> space_atts;
00254 cm.getCollisionSpaceAttachedCollisionObjects(space_atts);
00255
00256 ASSERT_EQ(space_objs.size(),2);
00257 ASSERT_EQ(space_atts.size(),0);
00258
00259 std::vector<std::string> touch_links;
00260 tf::Transform ident;
00261 ident.setIdentity();
00262 cm.convertStaticObjectToAttachedObject("object_3", "base_link", ident, touch_links);
00263
00264 cm.getCollisionSpaceCollisionObjects(space_objs);
00265 cm.getCollisionSpaceAttachedCollisionObjects(space_atts);
00266
00267 ASSERT_EQ(space_objs.size(),1);
00268 ASSERT_EQ(space_atts.size(),1);
00269
00270 const collision_space::EnvironmentModel::AllowedCollisionMatrix& acm = cm.getCollisionSpace()->getCurrentAllowedCollisionMatrix();
00271
00272 bool allowed;
00273 EXPECT_TRUE(acm.getAllowedCollision("object_3", "base_link", allowed));
00274 EXPECT_TRUE(allowed);
00275
00276 cm.addAttachedObject(att_object_1_);
00277
00278 cm.getCollisionSpaceCollisionObjects(space_objs);
00279 cm.getCollisionSpaceAttachedCollisionObjects(space_atts);
00280
00281 ASSERT_EQ(space_objs.size(),1);
00282 ASSERT_EQ(space_atts.size(),2);
00283
00284 cm.deleteAllAttachedObjects("base_link");
00285
00286 cm.getCollisionSpaceAttachedCollisionObjects(space_atts);
00287 ASSERT_EQ(space_atts.size(),1);
00288 ASSERT_EQ(space_atts[0].object.id, "object_4");
00289
00290
00291 cm.convertAttachedObjectToStaticObject("object_4","r_gripper_finger_tip_link", ident);
00292
00293 cm.getCollisionSpaceCollisionObjects(space_objs);
00294 cm.getCollisionSpaceAttachedCollisionObjects(space_atts);
00295
00296 ASSERT_EQ(space_objs.size(),1);
00297 ASSERT_EQ(space_atts.size(),1);
00298
00299
00300 cm.convertAttachedObjectToStaticObject("object_4","r_gripper_r_finger_tip_link", ident);
00301
00302 cm.getCollisionSpaceCollisionObjects(space_objs);
00303 cm.getCollisionSpaceAttachedCollisionObjects(space_atts);
00304
00305 ASSERT_EQ(space_objs.size(),2);
00306 ASSERT_EQ(space_atts.size(),0);
00307 }
00308
00309
00310 TEST_F(TestCollisionModels,TestAlterLinkPadding)
00311 {
00312 planning_environment::CollisionModels cm("robot_description");
00313
00314 planning_models::KinematicState state(cm.getKinematicModel());
00315
00316 state.setKinematicStateToDefault();
00317
00318 EXPECT_FALSE(cm.isKinematicStateInCollision(state));
00319
00320 cm.addStaticObject(static_object_1_);
00321
00322
00323 EXPECT_FALSE(cm.isKinematicStateInCollision(state));
00324
00325 std::vector<arm_navigation_msgs::LinkPadding> padd_vec;
00326 padd_vec.resize(3);
00327 padd_vec[0].link_name = "r_gripper_palm_link";
00328 padd_vec[0].padding = .1;
00329 padd_vec[1].link_name = "r_gripper_r_finger_link";
00330 padd_vec[1].padding = .1;
00331 padd_vec[2].link_name = "r_gripper_l_finger_link";
00332 padd_vec[2].padding = .1;
00333
00334
00335 cm.applyLinkPaddingToCollisionSpace(padd_vec);
00336
00337 EXPECT_TRUE(cm.isKinematicStateInCollision(state));
00338 EXPECT_FALSE(cm.isKinematicStateInSelfCollision(state));
00339 EXPECT_TRUE(cm.isKinematicStateInEnvironmentCollision(state));
00340
00341 std::vector<arm_navigation_msgs::ContactInformation> contacts;
00342 cm.getAllCollisionsForState(state, contacts,1);
00343
00344 EXPECT_GE(contacts.size(),1);
00345
00346 for(unsigned int i = 0; i < contacts.size(); i++) {
00347 EXPECT_TRUE(contacts[i].contact_body_1 == "object_1" || contacts[i].contact_body_2 == "object_1") << contacts[i].contact_body_1 << " " << contacts[i].contact_body_2;
00348 EXPECT_TRUE(contacts[i].contact_body_1 != "object_1" || contacts[i].contact_body_2 != "object_1");
00349 EXPECT_TRUE(contacts[i].body_type_1 == arm_navigation_msgs::ContactInformation::ROBOT_LINK ||
00350 contacts[i].body_type_2 == arm_navigation_msgs::ContactInformation::ROBOT_LINK);
00351 EXPECT_TRUE(contacts[i].body_type_1 == arm_navigation_msgs::ContactInformation::OBJECT ||
00352 contacts[i].body_type_2 == arm_navigation_msgs::ContactInformation::OBJECT);
00353 }
00354
00355 cm.revertCollisionSpacePaddingToDefault();
00356
00357 EXPECT_FALSE(cm.isKinematicStateInCollision(state));
00358 }
00359
00360
00361 TEST_F(TestCollisionModels,TestAllowedCollisions)
00362 {
00363 planning_environment::CollisionModels cm("robot_description");
00364
00365 {
00366 planning_models::KinematicState state(cm.getKinematicModel());
00367
00368 state.setKinematicStateToDefault();
00369 }
00370 collision_space::EnvironmentModel::AllowedCollisionMatrix check_acm = cm.getDefaultAllowedCollisionMatrix();
00371
00372 bool allowed;
00373 ASSERT_TRUE(check_acm.getAllowedCollision("r_gripper_palm_link","l_gripper_palm_link", allowed));
00374 EXPECT_FALSE(allowed);
00375
00376 arm_navigation_msgs::OrderedCollisionOperations ord;
00377 ord.collision_operations.resize(1);
00378 ord.collision_operations[0].object1 = "r_gripper_palm_link";
00379 ord.collision_operations[0].object2 = "l_gripper_palm_link";
00380 ord.collision_operations[0].operation = ord.collision_operations[0].DISABLE;
00381
00382 cm.applyOrderedCollisionOperationsToCollisionSpace(ord);
00383
00384 check_acm = cm.getCurrentAllowedCollisionMatrix();
00385
00386 ASSERT_TRUE(check_acm.getAllowedCollision("r_gripper_palm_link","l_gripper_palm_link", allowed));
00387 EXPECT_TRUE(allowed);
00388
00389 cm.revertAllowedCollisionToDefault();
00390
00391 check_acm = cm.getCurrentAllowedCollisionMatrix();
00392
00393 ASSERT_TRUE(check_acm.getAllowedCollision("r_gripper_palm_link","l_gripper_palm_link", allowed));
00394 EXPECT_FALSE(allowed);
00395
00396 check_acm.changeEntry("r_gripper_palm_link", "l_gripper_palm_link", true);
00397
00398 cm.setAlteredAllowedCollisionMatrix(check_acm);
00399
00400 ASSERT_TRUE(check_acm.getAllowedCollision("r_gripper_palm_link","l_gripper_palm_link", allowed));
00401 EXPECT_TRUE(allowed);
00402
00403 cm.revertAllowedCollisionToDefault();
00404
00405
00406
00407 cm.addStaticObject(static_object_1_);
00408
00409 check_acm = cm.getCurrentAllowedCollisionMatrix();
00410
00411 ASSERT_TRUE(check_acm.getAllowedCollision("r_gripper_palm_link","object_1", allowed));
00412 EXPECT_FALSE(allowed);
00413
00414 ord.collision_operations[0].object1 = "r_gripper_palm_link";
00415 ord.collision_operations[0].object2 = ord.collision_operations[0].COLLISION_SET_OBJECTS;
00416 ord.collision_operations[0].operation = ord.collision_operations[0].DISABLE;
00417
00418 cm.applyOrderedCollisionOperationsToCollisionSpace(ord);
00419
00420 check_acm = cm.getCurrentAllowedCollisionMatrix();
00421
00422 ASSERT_TRUE(check_acm.getAllowedCollision("r_gripper_palm_link","object_1", allowed));
00423 EXPECT_TRUE(allowed);
00424
00425
00426 att_object_1_.touch_links.push_back("r_gripper_palm_link");
00427 cm.addAttachedObject(att_object_1_);
00428
00429
00430 cm.revertAllowedCollisionToDefault();
00431
00432 check_acm = cm.getCurrentAllowedCollisionMatrix();
00433
00434 ASSERT_TRUE(check_acm.getAllowedCollision("r_gripper_palm_link","object_1", allowed));
00435 EXPECT_FALSE(allowed);
00436
00437 check_acm = cm.getCurrentAllowedCollisionMatrix();
00438
00439 ASSERT_TRUE(check_acm.getAllowedCollision("r_gripper_palm_link","object_4", allowed));
00440 EXPECT_TRUE(allowed);
00441
00442 ASSERT_TRUE(check_acm.getAllowedCollision("r_gripper_r_finger_tip_link","object_4", allowed));
00443 EXPECT_TRUE(allowed);
00444
00445 ASSERT_TRUE(check_acm.getAllowedCollision("r_gripper_l_finger_tip_link","object_4", allowed));
00446 EXPECT_FALSE(allowed);
00447
00448 ord.collision_operations[0].object1 = "r_gripper_palm_link";
00449 ord.collision_operations[0].object2 = ord.collision_operations[0].COLLISION_SET_ATTACHED_OBJECTS;
00450 ord.collision_operations[0].operation = ord.collision_operations[0].DISABLE;
00451
00452 cm.applyOrderedCollisionOperationsToCollisionSpace(ord);
00453
00454 check_acm = cm.getCurrentAllowedCollisionMatrix();
00455
00456
00457
00458 ASSERT_TRUE(check_acm.getAllowedCollision("r_gripper_palm_link","object_4", allowed));
00459 EXPECT_TRUE(allowed);
00460
00461 }
00462
00463
00464 TEST_F(TestCollisionModels,TestAttachedObjectCollisions)
00465 {
00466 planning_environment::CollisionModels cm("robot_description");
00467
00468 arm_navigation_msgs::CollisionObject table;
00469
00470 table.header.stamp = ros::Time::now();
00471 table.header.frame_id = "odom_combined";
00472 table.id = "wall";
00473 table.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00474 table.shapes.resize(1);
00475 table.shapes[0].type = arm_navigation_msgs::Shape::BOX;
00476 table.shapes[0].dimensions.resize(3);
00477 table.shapes[0].dimensions[0] = 0.05;
00478 table.shapes[0].dimensions[1] = 4.0;
00479 table.shapes[0].dimensions[2] = 4.0;
00480 table.poses.resize(1);
00481 table.poses[0].position.x = .7;
00482 table.poses[0].position.y = 0;
00483 table.poses[0].position.z = 1.0;
00484 table.poses[0].orientation.w = 1.0;
00485
00486 cm.addStaticObject(table);
00487
00488 arm_navigation_msgs::AttachedCollisionObject pole;
00489
00490 pole.object.header.stamp = ros::Time::now();
00491 pole.object.header.frame_id = "r_gripper_r_finger_tip_link";
00492 pole.link_name = "r_gripper_r_finger_tip_link";
00493 pole.object.id = "pole";
00494 pole.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00495 pole.object.shapes.resize(1);
00496 pole.object.shapes[0].type = arm_navigation_msgs::Shape::CYLINDER;
00497 pole.object.shapes[0].dimensions.resize(2);
00498 pole.object.shapes[0].dimensions[0] = .05;
00499 pole.object.shapes[0].dimensions[1] = .4;
00500 pole.object.poses.resize(1);
00501 pole.object.poses[0].position.x = 0;
00502 pole.object.poses[0].position.y = 0;
00503 pole.object.poses[0].position.z = 0;
00504 pole.object.poses[0].orientation.x = 0;
00505 pole.object.poses[0].orientation.y = 0;
00506 pole.object.poses[0].orientation.z = 0;
00507 pole.object.poses[0].orientation.w = 1;
00508
00509 std::vector<std::string> touch_links;
00510
00511 touch_links.push_back("r_gripper_palm_link");
00512 touch_links.push_back("r_gripper_r_finger_link");
00513 touch_links.push_back("r_gripper_l_finger_link");
00514 touch_links.push_back("r_gripper_l_finger_tip_link");
00515
00516 cm.addAttachedObject(pole);
00517
00518
00519 {
00520 planning_models::KinematicState state(cm.getKinematicModel());
00521
00522 state.setKinematicStateToDefault();
00523
00524 EXPECT_TRUE(cm.isKinematicStateInCollision(state));
00525
00526 std::vector<arm_navigation_msgs::ContactInformation> contacts;
00527 cm.getAllCollisionsForState(state, contacts,1);
00528
00529 EXPECT_GE(contacts.size(),1);
00530
00531
00532 bool got_object = false;
00533 std::map<std::string, bool> touch_links_map;
00534 for(unsigned int i = 0; i < touch_links.size(); i++) {
00535 touch_links_map[touch_links[i]] = false;
00536 }
00537 for(unsigned int i = 0; i < contacts.size(); i++) {
00538 std::string other_body_name;
00539 char other_body_type;
00540 if(contacts[i].body_type_1 == arm_navigation_msgs::ContactInformation::ATTACHED_BODY) {
00541 other_body_name = contacts[i].contact_body_2;
00542 other_body_type = contacts[i].body_type_2;
00543 } else if(contacts[i].body_type_2 == arm_navigation_msgs::ContactInformation::ATTACHED_BODY) {
00544 other_body_name = contacts[i].contact_body_1;
00545 other_body_type = contacts[i].body_type_1;
00546 } else {
00547 ASSERT_TRUE(false) << "Collision other than with attached object " << contacts[i].contact_body_1 << " and " << contacts[i].contact_body_2;
00548 }
00549 if(other_body_type == arm_navigation_msgs::ContactInformation::ROBOT_LINK) {
00550 EXPECT_FALSE(touch_links_map.find(other_body_name) == touch_links_map.end()) << contacts[i].contact_body_1 << " and " << contacts[i].contact_body_2;
00551 touch_links_map[other_body_name] = true;
00552 } else {
00553 got_object = true;
00554 }
00555 }
00556 EXPECT_TRUE(got_object);
00557 for(std::map<std::string, bool>::iterator it = touch_links_map.begin();
00558 it != touch_links_map.end();
00559 it++) {
00560 EXPECT_TRUE(it->second) << it->first << " not in collision";
00561 }
00562 }
00563 pole.touch_links = touch_links;
00564 cm.addAttachedObject(pole);
00565
00566 {
00567 planning_models::KinematicState state(cm.getKinematicModel());
00568
00569 state.setKinematicStateToDefault();
00570
00571 EXPECT_TRUE(cm.isKinematicStateInCollision(state));
00572 EXPECT_TRUE(cm.isKinematicStateInEnvironmentCollision(state));
00573 EXPECT_FALSE(cm.isKinematicStateInSelfCollision(state));
00574 }
00575
00576
00577 pole.touch_links.clear();
00578 pole.touch_links.push_back("r_end_effector");
00579 cm.addAttachedObject(pole);
00580
00581 {
00582 planning_models::KinematicState state(cm.getKinematicModel());
00583
00584 state.setKinematicStateToDefault();
00585
00586 EXPECT_TRUE(cm.isKinematicStateInCollision(state));
00587 EXPECT_TRUE(cm.isKinematicStateInEnvironmentCollision(state));
00588 EXPECT_FALSE(cm.isKinematicStateInSelfCollision(state));
00589 }
00590
00591 }
00592
00593 TEST_F(TestCollisionModels, TestTrajectoryValidity)
00594 {
00595
00596 planning_environment::CollisionModels cm("robot_description");
00597
00598 static_object_1_.poses[0].position.x = .45;
00599 static_object_1_.poses[0].position.y = -.5;
00600 cm.addStaticObject(static_object_1_);
00601
00602 planning_models::KinematicState kin_state(cm.getKinematicModel());
00603 kin_state.setKinematicStateToDefault();
00604
00605 std::map<std::string, double> jm;
00606 jm["r_shoulder_pan_joint"] = -2.0;
00607 kin_state.setKinematicState(jm);
00608 ASSERT_FALSE(cm.isKinematicStateInCollision(kin_state));
00609
00610 arm_navigation_msgs::Constraints goal_constraints;
00611 goal_constraints.joint_constraints.resize(1);
00612 goal_constraints.joint_constraints[0].joint_name = "r_shoulder_pan_joint";
00613 goal_constraints.joint_constraints[0].position = -2.0;
00614 goal_constraints.joint_constraints[0].tolerance_below = 0.1;
00615 goal_constraints.joint_constraints[0].tolerance_above = 0.1;
00616
00617
00618 arm_navigation_msgs::Constraints path_constraints;
00619
00620
00621 trajectory_msgs::JointTrajectory trajectory;
00622 trajectory.joint_names.push_back("r_shoulder_pan_joint");
00623 trajectory.points.resize(1);
00624 trajectory.points[0].positions.resize(1);
00625 trajectory.points[0].positions[0] = -2.0;
00626
00627 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> trajectory_error_codes;
00628 arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00629 ASSERT_TRUE(cm.isJointTrajectoryValid(kin_state, trajectory, goal_constraints, path_constraints,
00630 error_code, trajectory_error_codes, false)) << error_code.val;
00631 EXPECT_EQ(error_code.val, error_code.SUCCESS);
00632
00633
00634 trajectory.points[0].positions[0] = -1.8;
00635
00636 ASSERT_FALSE(cm.isJointTrajectoryValid(kin_state, trajectory, goal_constraints, path_constraints,
00637 error_code, trajectory_error_codes, false));
00638 EXPECT_EQ(error_code.val, error_code.GOAL_CONSTRAINTS_VIOLATED);
00639
00640
00641 goal_constraints.joint_constraints[0].position = -2.3;
00642 trajectory.points[0].positions[0] = -2.0;
00643
00644 ASSERT_FALSE(cm.isJointTrajectoryValid(kin_state, trajectory, goal_constraints, path_constraints,
00645 error_code, trajectory_error_codes, false));
00646 EXPECT_EQ(error_code.val, error_code.INVALID_GOAL_JOINT_CONSTRAINTS);
00647
00648
00649
00650 goal_constraints.joint_constraints[0].position = -2.0;
00651
00652 unsigned int num_points = fabs(100.0/-2.0);
00653 trajectory.points.resize(num_points);
00654 for(unsigned int i = 1; i <= num_points; i++) {
00655 trajectory.points[i-1].positions.resize(1);
00656 trajectory.points[i-1].positions[0] = -2.0*((i*1.0)/(1.0*num_points));
00657 }
00658 EXPECT_EQ(trajectory.points.back().positions[0], -2.0);
00659
00660 ASSERT_FALSE(cm.isJointTrajectoryValid(kin_state, trajectory, goal_constraints, path_constraints,
00661 error_code, trajectory_error_codes, false));
00662 EXPECT_EQ(error_code.val, error_code.COLLISION_CONSTRAINTS_VIOLATED);
00663 }
00664
00665 TEST_F(TestCollisionModels, TestConversionFunctionsForObjects)
00666 {
00667
00668 planning_environment::CollisionModels cm("robot_description");
00669
00670 planning_models::KinematicState kin_state(cm.getKinematicModel());
00671 kin_state.setKinematicStateToDefault();
00672
00673 arm_navigation_msgs::RobotState robot_state;
00674
00675 planning_environment::convertKinematicStateToRobotState(kin_state,
00676 ros::Time::now(),
00677 cm.getWorldFrameId(),
00678 robot_state);
00679
00680 robot_state.multi_dof_joint_state.poses[0].position.x = 4.0;
00681 robot_state.multi_dof_joint_state.poses[0].orientation.w = 1.0;
00682
00683 planning_environment::setRobotStateAndComputeTransforms(robot_state, kin_state);
00684
00685 arm_navigation_msgs::CollisionObject obj1;
00686 obj1.header.stamp = ros::Time::now();
00687 obj1.header.frame_id = "base_footprint";
00688 obj1.id = "obj1";
00689 obj1.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00690 obj1.shapes.resize(1);
00691 obj1.shapes[0].type = arm_navigation_msgs::Shape::BOX;
00692 obj1.shapes[0].dimensions.resize(3);
00693 obj1.shapes[0].dimensions[0] = .1;
00694 obj1.shapes[0].dimensions[1] = .1;
00695 obj1.shapes[0].dimensions[2] = .75;
00696 obj1.poses.resize(1);
00697 obj1.poses[0].position.x = .5;
00698 obj1.poses[0].position.y = .5;
00699 obj1.poses[0].position.z = 0;
00700 obj1.poses[0].orientation.w = 1.0;
00701
00702 arm_navigation_msgs::AttachedCollisionObject att_obj;
00703 att_obj.object = obj1;
00704 att_obj.object.header.stamp = ros::Time::now();
00705 att_obj.object.header.frame_id = "r_gripper_r_finger_tip_link";
00706 att_obj.link_name = "r_gripper_palm_link";
00707 att_obj.touch_links.push_back("r_end_effector");
00708 att_obj.touch_links.push_back("r_wrist_roll_link");
00709 att_obj.touch_links.push_back("r_wrist_flex_link");
00710 att_obj.touch_links.push_back("r_forearm_link");
00711 att_obj.object.id = "obj2";
00712 att_obj.object.shapes[0].type = arm_navigation_msgs::Shape::CYLINDER;
00713 att_obj.object.shapes[0].dimensions.resize(2);
00714 att_obj.object.shapes[0].dimensions[0] = .025;
00715 att_obj.object.shapes[0].dimensions[1] = .5;
00716 att_obj.object.poses.resize(1);
00717 att_obj.object.poses[0].position.x = 0.0;
00718 att_obj.object.poses[0].position.y = 0.0;
00719 att_obj.object.poses[0].position.z = 0.0;
00720 att_obj.object.poses[0].orientation.w = 1.0;
00721
00722 cm.convertAttachedCollisionObjectToNewWorldFrame(kin_state,
00723 att_obj);
00724
00725 EXPECT_EQ(att_obj.object.header.frame_id, "r_gripper_palm_link");
00726
00727 cm.convertAttachedCollisionObjectToNewWorldFrame(kin_state,
00728 att_obj);
00729
00730 att_obj.link_name = "base_footprint";
00731 att_obj.object.header.frame_id = "base_footprint";
00732 att_obj.object.id = "obj3";
00733 att_obj.object.poses[0].position.x = 0.12;
00734
00735 cm.convertAttachedCollisionObjectToNewWorldFrame(kin_state,
00736 att_obj);
00737
00738 EXPECT_GE(att_obj.object.poses[0].position.x,0.0);
00739 EXPECT_LE(fabs(att_obj.object.poses[0].position.x-.12), VERY_SMALL);
00740
00741 cm.convertCollisionObjectToNewWorldFrame(kin_state,
00742 obj1);
00743
00744 EXPECT_EQ(obj1.header.frame_id, "odom_combined");
00745 EXPECT_LE(fabs(obj1.poses[0].position.x-4.5),VERY_SMALL);
00746 EXPECT_LE(fabs(obj1.poses[0].position.y-.5),VERY_SMALL);
00747 }
00748
00749 TEST_F(TestCollisionModels, TestConversionFunctionsForConstraints)
00750 {
00751
00752 planning_environment::CollisionModels cm("robot_description");
00753
00754 planning_models::KinematicState kin_state(cm.getKinematicModel());
00755 kin_state.setKinematicStateToDefault();
00756
00757 arm_navigation_msgs::RobotState robot_state;
00758
00759 robot_state.multi_dof_joint_state.stamp = ros::Time::now();
00760 robot_state.multi_dof_joint_state.joint_names.push_back("base_joint");
00761 robot_state.multi_dof_joint_state.frame_ids.push_back("odom_combined");
00762 robot_state.multi_dof_joint_state.child_frame_ids.push_back("base_footprint");
00763 robot_state.multi_dof_joint_state.poses.resize(1);
00764 robot_state.multi_dof_joint_state.poses[0].position.x = 4.0;
00765 robot_state.multi_dof_joint_state.poses[0].orientation.w = 1.0;
00766
00767 planning_environment::setRobotStateAndComputeTransforms(robot_state, kin_state);
00768
00769 arm_navigation_msgs::Constraints goal_constraints;
00770 arm_navigation_msgs::PositionConstraint pos;
00771 pos.header.frame_id = "base_footprint";
00772 pos.header.stamp = ros::Time::now();
00773 pos.link_name = "r_wrist_roll_link";
00774 pos.position.x = .5;
00775 pos.constraint_region_orientation.w = 1.0;
00776
00777 goal_constraints.position_constraints.push_back(pos);
00778
00779 cm.convertConstraintsGivenNewWorldTransform(kin_state,
00780 goal_constraints);
00781
00782 EXPECT_TRUE(goal_constraints.position_constraints[0].header.frame_id == "odom_combined");
00783 EXPECT_LE(fabs(goal_constraints.position_constraints[0].position.x-4.5), VERY_SMALL) ;
00784
00785
00786
00787 tf::Quaternion turn(tf::Vector3(0,0,1), M_PI);
00788
00789 robot_state.multi_dof_joint_state.poses[0].orientation.x = turn.x();
00790 robot_state.multi_dof_joint_state.poses[0].orientation.y = turn.y();
00791 robot_state.multi_dof_joint_state.poses[0].orientation.z = turn.z();
00792 robot_state.multi_dof_joint_state.poses[0].orientation.w = turn.w();
00793
00794 planning_environment::setRobotStateAndComputeTransforms(robot_state, kin_state);
00795
00796 goal_constraints.position_constraints.clear();
00797 goal_constraints.position_constraints.push_back(pos);
00798
00799 cm.convertConstraintsGivenNewWorldTransform(kin_state,
00800 goal_constraints);
00801
00802
00803 EXPECT_TRUE(goal_constraints.position_constraints[0].header.frame_id == "odom_combined");
00804 EXPECT_LE(fabs(goal_constraints.position_constraints[0].position.x-3.5), VERY_SMALL) ;
00805 }
00806
00807 int main(int argc, char **argv)
00808 {
00809 testing::InitGoogleTest(&argc, argv);
00810 ros::init(argc, argv, "test_collision_models");
00811
00812 return RUN_ALL_TESTS();
00813 }
00814