collision_space_ccd: collision_space_ccd: test_collision_space

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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 //urdf location relative to the planning_models path
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     //now this should work with an identity transform
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     //all AllowedCollisions set to true, so no collision
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     //now we are in collision with nothing disabled
00116     ASSERT_TRUE(coll_space_->isCollision());
00117   }
00118 
00119   //one more time for good measure
00120   {
00121     collision_space_ccd::EnvironmentModel::AllowedCollisionMatrix acm(links,false);
00122 
00123     coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00124   
00125     //now we are in collision with nothing disabled
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   //first we get
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     //at default state in collision
00147     ASSERT_TRUE(coll_space_->isCollision());
00148 
00149     //now we get the full set of collisions in the default state
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     //now we change all these pairwise to true
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     //with all of these disabled, no more collisions
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     //indented cause the state needs to cease to exist before we add the attached body
00184     planning_models::KinematicState state(kinematic_model_);
00185     state.setKinematicStateToDefault();
00186     
00187     coll_space_->updateRobotModel(&state);
00188 
00189     //now we get the full set of collisions in the default state
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     //now we change all these pairwise to true
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   //now we shouldn't be in collision
00207   ASSERT_FALSE(coll_space_->isCollision());  
00208 
00209   const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link");
00210 
00211   //first a single box
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     //indented cause the state needs to cease to exist before we add the attached body
00250     planning_models::KinematicState state(kinematic_model_);
00251     state.setKinematicStateToDefault();
00252     
00253     coll_space_->updateRobotModel(&state);
00254   }
00255 
00256   //now it collides
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   //now adding an attached object with two boxes, this time with two objects
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     //indented cause the state needs to cease to exist before we add the attached body
00291     planning_models::KinematicState state(kinematic_model_);
00292     state.setKinematicStateToDefault();
00293     
00294     coll_space_->updateRobotModel(&state);
00295   }
00296 
00297   //now it doesn't collide
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     //indented cause the state needs to cease to exist before we add the attached body
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   //Now test interactions between static and attached objects
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     //indented cause the state needs to cease to exist before we add the attached body
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   //now we get the full set of collisions in the default state
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   //now we change all these pairwise to true
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     //indented cause the state needs to cease to exist before we add the attached body
00407     planning_models::KinematicState state(kinematic_model_);
00408     state.setKinematicStateToDefault();
00409     
00410     coll_space_->updateRobotModel(&state);
00411   }
00412 
00413   /* Remove because sphere-box collision does not work for mesh */
00414   // EXPECT_TRUE(coll_space_->isEnvironmentCollision());
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   //now we change all these pairwise to true
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
test_collision_space_ccd.cpp
