test_collision_space.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, 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 the Willow Garage 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/environmentODE.h>
00044 #include <boost/thread.hpp>
00045 
00046 //urdf location relative to the planning_models path
00047 static const std::string rel_path = "/test_urdf/robot.xml";
00048 
00049 class TestCollisionSpace : public testing::Test {
00050 public:
00051 
00052   void spinThread() {
00053     lock_.lock();
00054     coll_space_->isCollision();
00055     lock_.unlock();
00056   }
00057 
00058 protected:
00059   
00060   virtual void SetUp() {
00061 
00062     full_path_ = ros::package::getPath("planning_models")+rel_path;
00063     
00064     urdf_ok_ = urdf_model_.initFile(full_path_);
00065 
00066     std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00067     //now this should work with an identity transform
00068     planning_models::KinematicModel::MultiDofConfig config("base_joint");
00069     config.type = "Planar";
00070     config.parent_frame_id = "base_footprint";
00071     config.child_frame_id = "base_footprint";
00072     multi_dof_configs.push_back(config);
00073 
00074     std::vector<planning_models::KinematicModel::GroupConfig> gcs;
00075     planning_models::KinematicModel::GroupConfig left_arm("left_arm",
00076                                                           "torso_lift_link",
00077                                                           "l_wrist_roll_link");
00078 
00079     planning_models::KinematicModel::GroupConfig right_arm("right_arm",
00080                                                            "torso_lift_link",
00081                                                            "r_wrist_roll_link");
00082 
00083     kinematic_model_ = new planning_models::KinematicModel(urdf_model_,
00084                                                            gcs,
00085                                                            multi_dof_configs);
00086     coll_space_ = new collision_space::EnvironmentModelODE();
00087   };
00088 
00089   virtual void TearDown() {
00090     delete kinematic_model_;
00091     delete coll_space_;
00092   }
00093 protected:
00094 
00095   boost::mutex lock_;
00096 
00097   urdf::Model urdf_model_;
00098   bool urdf_ok_;
00099   std::string full_path_;
00100   collision_space::EnvironmentModelODE* coll_space_;
00101   planning_models::KinematicModel* kinematic_model_;
00102 };
00103 
00104 
00105 TEST_F(TestCollisionSpace, TestInit) {
00106   std::vector<std::string> links;
00107   kinematic_model_->getLinkModelNames(links);
00108   std::map<std::string, double> link_padding_map;
00109 
00110   {
00111     collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links,true);
00112     
00113     coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00114     
00115     //all AllowedCollisions set to true, so no collision
00116     ASSERT_FALSE(coll_space_->isCollision());
00117   }
00118 
00119   {
00120     collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links,false);
00121 
00122     coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00123   
00124     //now we are in collision with nothing disabled
00125     ASSERT_TRUE(coll_space_->isCollision());
00126   }
00127 
00128   //one more time for good measure
00129   {
00130     collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links,false);
00131 
00132     coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00133   
00134     //now we are in collision with nothing disabled
00135     ASSERT_TRUE(coll_space_->isCollision());
00136   }
00137 }
00138 
00139 
00140 TEST_F(TestCollisionSpace, TestACM) {
00141   std::vector<std::string> links;
00142   kinematic_model_->getLinkModelNames(links);
00143   std::map<std::string, double> link_padding_map;
00144   
00145   //first we get
00146   {
00147     collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
00148     coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00149 
00150     planning_models::KinematicState state(kinematic_model_);
00151     state.setKinematicStateToDefault();
00152 
00153     coll_space_->updateRobotModel(&state);
00154 
00155     //at default state in collision
00156     ASSERT_TRUE(coll_space_->isCollision());
00157 
00158     //now we get the full set of collisions in the default state
00159     std::vector<collision_space::EnvironmentModel::Contact> contacts;
00160       
00161     coll_space_->getAllCollisionContacts(contacts, 1);
00162 
00163     ASSERT_TRUE(contacts.size() > 1);
00164     //now we change all these pairwise to true
00165     for(unsigned int i = 0; i < contacts.size(); i++) {
00166       ASSERT_TRUE(contacts[i].body_type_1 == collision_space::EnvironmentModel::LINK);
00167       ASSERT_TRUE(contacts[i].body_type_2 == collision_space::EnvironmentModel::LINK);
00168       ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true));
00169     }
00170 
00171     coll_space_->setAlteredCollisionMatrix(acm);
00172     
00173     //with all of these disabled, no more collisions
00174     ASSERT_FALSE(coll_space_->isCollision());
00175 
00176     coll_space_->revertAlteredCollisionMatrix();
00177     ASSERT_TRUE(coll_space_->isCollision());
00178   }
00179 }
00180 
00181 TEST_F(TestCollisionSpace, TestAttachedObjects)
00182 {
00183   std::vector<std::string> links;
00184   kinematic_model_->getLinkModelNames(links);
00185   std::map<std::string, double> link_padding_map;
00186   
00187   collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
00188   coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);  
00189 
00190   {
00191     //indented cause the state needs to cease to exist before we add the attached body
00192     planning_models::KinematicState state(kinematic_model_);
00193     state.setKinematicStateToDefault();
00194     
00195     coll_space_->updateRobotModel(&state);
00196 
00197     //now we get the full set of collisions in the default state
00198     std::vector<collision_space::EnvironmentModel::Contact> contacts;
00199       
00200     coll_space_->getAllCollisionContacts(contacts, 1);
00201 
00202     //now we change all these pairwise to true
00203     for(unsigned int i = 0; i < contacts.size(); i++) {
00204       ASSERT_TRUE(contacts[i].body_type_1 == collision_space::EnvironmentModel::LINK);
00205       ASSERT_TRUE(contacts[i].body_type_2 == collision_space::EnvironmentModel::LINK);
00206       ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true));
00207     }
00208 
00209     coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
00210     coll_space_->updateRobotModel(&state);
00211   }
00212   
00213   //now we shouldn't be in collision
00214   ASSERT_FALSE(coll_space_->isCollision());  
00215 
00216   const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link");
00217 
00218   //first a single box
00219   shapes::Sphere* sphere1 = new shapes::Sphere();
00220   sphere1->radius = .1;
00221 
00222   shapes::Box* box2 = new shapes::Box();
00223   box2->size[0] = .05;
00224   box2->size[1] = .05;
00225   box2->size[2] = .05;
00226   
00227   std::vector<shapes::Shape*> shape_vector;
00228   shape_vector.push_back(sphere1);
00229 
00230   tf::Transform pose;
00231   pose.setIdentity();
00232 
00233   std::vector<tf::Transform> poses;
00234   poses.push_back(pose);
00235   
00236   std::vector<std::string> touch_links;
00237 
00238   planning_models::KinematicModel::AttachedBodyModel* ab1 = 
00239     new planning_models::KinematicModel::AttachedBodyModel(link, "box_1",
00240                                                            poses,
00241                                                            touch_links,
00242                                                            shape_vector);
00243 
00244   kinematic_model_->addAttachedBodyModel(link->getName(), ab1);
00245   coll_space_->updateAttachedBodies();
00246 
00247   const collision_space::EnvironmentModel::AllowedCollisionMatrix& aft_attached 
00248     = coll_space_->getDefaultAllowedCollisionMatrix();
00249   
00250   ASSERT_TRUE(aft_attached.hasEntry("box_1"));
00251   bool allowed;
00252   EXPECT_TRUE(aft_attached.getAllowedCollision("box_1", link->getName(), allowed));
00253   EXPECT_FALSE(allowed);
00254 
00255   {
00256     //indented cause the state needs to cease to exist before we add the attached body
00257     planning_models::KinematicState state(kinematic_model_);
00258     state.setKinematicStateToDefault();
00259     
00260     coll_space_->updateRobotModel(&state);
00261   }
00262 
00263   //now it collides
00264   ASSERT_TRUE(coll_space_->isCollision());  
00265 
00266   kinematic_model_->clearLinkAttachedBodyModel(link->getName(), "box_1");
00267   coll_space_->updateAttachedBodies();
00268 
00269   ASSERT_FALSE(aft_attached.hasEntry("box_1"));
00270 
00271   //now adding an attached object with two boxes, this time with two objects
00272   shape_vector.clear();
00273   shape_vector.push_back(box2);
00274   pose.getOrigin().setX(.1);
00275   poses.clear();
00276   poses.push_back(pose);
00277   touch_links.push_back("r_gripper_palm_link");
00278   touch_links.push_back("r_gripper_r_finger_link");
00279   touch_links.push_back("r_gripper_l_finger_link");
00280   touch_links.push_back("r_gripper_r_finger_tip_link");
00281   touch_links.push_back("r_gripper_l_finger_tip_link");
00282   touch_links.push_back("base_link");
00283   
00284   planning_models::KinematicModel::AttachedBodyModel* ab2 = 
00285     new planning_models::KinematicModel::AttachedBodyModel(link, "box_2",
00286                                                            poses,
00287                                                            touch_links,
00288                                                            shape_vector);
00289   kinematic_model_->addAttachedBodyModel(link->getName(), ab2);
00290   coll_space_->updateAttachedBodies();
00291 
00292   ASSERT_TRUE(aft_attached.hasEntry("box_2"));
00293   EXPECT_TRUE(aft_attached.getAllowedCollision("box_2", link->getName(), allowed));
00294   EXPECT_TRUE(allowed);
00295 
00296   {
00297     //indented cause the state needs to cease to exist before we add the attached body
00298     planning_models::KinematicState state(kinematic_model_);
00299     state.setKinematicStateToDefault();
00300     
00301     coll_space_->updateRobotModel(&state);
00302   }
00303 
00304   //now it doesn't collide
00305   ASSERT_FALSE(coll_space_->isCollision());  
00306 }
00307 
00308 TEST_F(TestCollisionSpace, TestStaticObjects)
00309 {
00310   std::vector<std::string> links;
00311   kinematic_model_->getLinkModelNames(links);
00312   std::map<std::string, double> link_padding_map;
00313   
00314   collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
00315   coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);  
00316   
00317   {
00318     //indented cause the state needs to cease to exist before we add the attached body
00319     planning_models::KinematicState state(kinematic_model_);
00320     state.setKinematicStateToDefault();
00321     
00322     coll_space_->updateRobotModel(&state);
00323   }
00324 
00325   ASSERT_FALSE(coll_space_->isEnvironmentCollision());
00326 
00327   shapes::Sphere* sphere1 = new shapes::Sphere();
00328   sphere1->radius = .2;
00329 
00330   tf::Transform pose;
00331   pose.setIdentity();
00332 
00333   std::vector<tf::Transform> poses;
00334   poses.push_back(pose);
00335 
00336   std::vector<shapes::Shape*> shape_vector;
00337   shape_vector.push_back(sphere1);
00338 
00339   coll_space_->addObjects("obj1", shape_vector, poses);
00340 
00341   ASSERT_TRUE(coll_space_->isEnvironmentCollision());
00342 
00343 
00344 
00345   //Now test interactions between static and attached objects
00346 
00347   const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link");
00348 
00349   shapes::Box* att_box = new shapes::Box();
00350   att_box->size[0] = .05;
00351   att_box->size[1] = .05;
00352   att_box->size[2] = .05;
00353   
00354   std::vector<shapes::Shape*> att_shapes;
00355   att_shapes.push_back(att_box);
00356 
00357   tf::Transform att_pose;
00358   att_pose.setIdentity();
00359 
00360   std::vector<tf::Transform> att_poses;
00361   att_poses.push_back(att_pose);
00362   
00363   std::vector<std::string> touch_links;
00364   touch_links.push_back("base_link");
00365   touch_links.push_back("base_footprint");
00366 
00367   planning_models::KinematicModel::AttachedBodyModel* ab1 = 
00368     new planning_models::KinematicModel::AttachedBodyModel(link, "att1",
00369                                                            att_poses,
00370                                                            touch_links,
00371                                                            att_shapes);
00372 
00373   kinematic_model_->addAttachedBodyModel(link->getName(), ab1);
00374   coll_space_->updateAttachedBodies();
00375 
00376   {
00377     //indented cause the state needs to cease to exist before we add the attached body
00378     planning_models::KinematicState state(kinematic_model_);
00379     state.setKinematicStateToDefault();
00380     
00381     coll_space_->updateRobotModel(&state);
00382   }
00383 
00384   ASSERT_TRUE(coll_space_->isEnvironmentCollision());
00385 
00386   //now we get the full set of collisions in the default state
00387   std::vector<collision_space::EnvironmentModel::Contact> contacts;
00388   
00389   coll_space_->getAllCollisionContacts(contacts, 1);
00390 
00391   //now we change all these pairwise to true
00392   for(unsigned int i = 0; i < contacts.size(); i++) {
00393     if(contacts[i].body_type_1 == collision_space::EnvironmentModel::OBJECT) {
00394       ASSERT_TRUE(contacts[i].body_name_1 == "obj1");
00395     }
00396     if(contacts[i].body_type_2 == collision_space::EnvironmentModel::OBJECT) {
00397       ASSERT_TRUE(contacts[i].body_name_2 == "obj1");
00398     }
00399   }
00400 
00401   acm = coll_space_->getDefaultAllowedCollisionMatrix();
00402   bool allowed;
00403   ASSERT_TRUE(acm.getAllowedCollision("obj1","att1",allowed));
00404   EXPECT_FALSE(allowed);
00405   
00406   ASSERT_TRUE(acm.changeEntry(link->getName(), "obj1", true));
00407   ASSERT_TRUE(acm.changeEntry("base_footprint", "obj1", true));
00408   coll_space_->setAlteredCollisionMatrix(acm);
00409 
00410   {
00411     //indented cause the state needs to cease to exist before we add the attached body
00412     planning_models::KinematicState state(kinematic_model_);
00413     state.setKinematicStateToDefault();
00414     
00415     coll_space_->updateRobotModel(&state);
00416   }
00417 
00418   EXPECT_TRUE(coll_space_->isEnvironmentCollision());
00419   
00420   ASSERT_TRUE(acm.changeEntry("att1", "obj1", true));
00421   coll_space_->setAlteredCollisionMatrix(acm);
00422   
00423   allowed = false;
00424   ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("att1","obj1", allowed));
00425   EXPECT_TRUE(allowed);
00426 
00427   ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","att1", allowed));
00428   EXPECT_TRUE(allowed);
00429 
00430   ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("base_link","obj1", allowed));
00431   EXPECT_TRUE(allowed);
00432 
00433   ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_link", allowed));
00434   EXPECT_TRUE(allowed);
00435 
00436   ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_footprint", allowed));
00437   EXPECT_TRUE(allowed);
00438   
00439   EXPECT_FALSE(coll_space_->isEnvironmentCollision());
00440   contacts.clear();
00441 
00442   coll_space_->getAllCollisionContacts(contacts, 1);
00443 
00444   //now we change all these pairwise to true
00445   for(unsigned int i = 0; i < contacts.size(); i++) {
00446     if(contacts[i].body_type_1 == collision_space::EnvironmentModel::OBJECT) {
00447       ASSERT_TRUE(contacts[i].body_name_1 == "obj1");
00448       ROS_INFO_STREAM(contacts[i].body_name_2);
00449     }
00450     if(contacts[i].body_type_2 == collision_space::EnvironmentModel::OBJECT) {
00451       ASSERT_TRUE(contacts[i].body_name_2 == "obj1");
00452       ROS_INFO_STREAM(contacts[i].body_name_1);
00453     }
00454   }
00455 
00456 }
00457 
00458 TEST_F(TestCollisionSpace, TestAllowedContacts)
00459 {
00460   std::vector<std::string> links;
00461   kinematic_model_->getLinkModelNames(links);
00462   std::map<std::string, double> link_padding_map;
00463   
00464   collision_space::EnvironmentModel::AllowedCollisionMatrix acm(links, true);
00465   coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);  
00466   
00467   {
00468     //indented cause the state needs to cease to exist before we add the attached body
00469     planning_models::KinematicState state(kinematic_model_);
00470     state.setKinematicStateToDefault();
00471     
00472     coll_space_->updateRobotModel(&state);
00473   }
00474 
00475   ASSERT_FALSE(coll_space_->isEnvironmentCollision());
00476 
00477   shapes::Sphere* sphere1 = new shapes::Sphere(.2);
00478   shapes::Box* box1 = new shapes::Box(.4, .4, .4);
00479   shapes::Box* box1a = new shapes::Box(.4, .4, .4);
00480 
00481   tf::Transform pose;
00482   pose.setIdentity();
00483 
00484   std::vector<tf::Transform> poses;
00485   poses.push_back(pose);
00486 
00487   std::vector<shapes::Shape*> shape_vector_1;
00488   shape_vector_1.push_back(sphere1);
00489 
00490   coll_space_->addObjects("obj1", shape_vector_1, poses);
00491 
00492   std::vector<shapes::Shape*> shape_vector_2;
00493   shape_vector_2.push_back(box1);
00494   shape_vector_2.push_back(box1a);
00495   pose.getOrigin().setX(.25);
00496   poses.push_back(pose);
00497 
00498   coll_space_->addObjects("obj2", shape_vector_2, poses);
00499 
00500   ASSERT_TRUE(coll_space_->isEnvironmentCollision());
00501 
00502   std::vector<collision_space::EnvironmentModel::Contact> contacts;
00503   ASSERT_TRUE(coll_space_->getAllCollisionContacts(contacts));
00504 
00505   std::vector<collision_space::EnvironmentModel::AllowedContact> allowed;
00506 
00507   for(unsigned int i = 0; i < contacts.size(); i++) {
00508     //now we place an allowed contact region around this sphere for each contact
00509 
00510     if(contacts[i].body_name_1 == "obj1" || contacts[i].body_name_2 == "obj1") {
00511 
00512       shapes::Sphere* sphere2 = new shapes::Sphere(.3);
00513 
00514       tf::Transform trans(tf::Quaternion(0,0,0,1.0), contacts[i].pos);
00515       
00516       boost::shared_ptr<bodies::Sphere> bodysp(new bodies::Sphere(sphere2));
00517       bodysp->setPose(trans);
00518 
00519       delete sphere2;
00520 
00521       collision_space::EnvironmentModel::AllowedContact allc;
00522 
00523       allc.bound = bodysp;
00524       allc.body_name_1 = contacts[i].body_name_1;
00525       allc.body_name_2 = contacts[i].body_name_2;
00526       allc.depth = 100.0;//contacts[i].depth;
00527 
00528       allowed.push_back(allc);
00529     }
00530     if(contacts[i].body_name_1 == "obj2" || contacts[i].body_name_2 == "obj2") {
00531 
00532       shapes::Box* box2 = new shapes::Box(.01, .01, .01);
00533 
00534       tf::Transform trans(tf::Quaternion(0,0,0,1.0), contacts[i].pos);
00535 
00536       ROS_DEBUG_STREAM("Making allowed contact for " << contacts[i].body_name_1 
00537                        << " and " << contacts[i].body_name_2 
00538                        << " at " << contacts[i].pos.x() << " " 
00539                        << contacts[i].pos.y() << " " << contacts[i].pos.z());      
00540 
00541       
00542       boost::shared_ptr<bodies::Box> bodysp(new bodies::Box(box2));
00543       bodysp->setPose(trans);
00544 
00545       delete box2;
00546 
00547       collision_space::EnvironmentModel::AllowedContact allc;
00548 
00549       allc.bound = bodysp;
00550       allc.body_name_1 = contacts[i].body_name_1;
00551       allc.body_name_2 = contacts[i].body_name_2;
00552       allc.depth = 100.0;//contacts[i].depth;
00553 
00554       allowed.push_back(allc);
00555     }
00556   }
00557   coll_space_->setAllowedContacts(allowed);
00558 
00559   ASSERT_FALSE(coll_space_->isEnvironmentCollision());
00560 
00561   coll_space_->clearAllowedContacts();
00562 }
00563 
00564 TEST_F(TestCollisionSpace, TestThreading)
00565 {
00566   boost::thread thread1(boost::bind(&TestCollisionSpace::spinThread, this));
00567   boost::thread thread2(boost::bind(&TestCollisionSpace::spinThread, this));
00568   boost::thread thread3(boost::bind(&TestCollisionSpace::spinThread, this));
00569   boost::thread thread4(boost::bind(&TestCollisionSpace::spinThread, this));
00570   thread1.join();
00571   thread2.join();
00572   thread3.join();
00573   thread4.join();
00574 }
00575 
00576 
00577 int main(int argc, char **argv)
00578 {
00579   testing::InitGoogleTest(&argc, argv);
00580   return RUN_ALL_TESTS();
00581 }
00582 


collision_space
Author(s): Ioan Sucan/isucan@willowgarage.com
autogenerated on Thu Dec 12 2013 11:09:16