planning_environment: test_collision

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_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   //this mostly tests that the planning_description file is correct
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 //Testing a couple important functions in model_utils.h
00200 TEST_F(TestCollisionModels, TestModelUtils) 
00201 {
00202   //this mostly tests that the planning_description file is correct
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   //should still be ok because the multi-dof is set
00235   ASSERT_TRUE(planning_environment::setRobotStateAndComputeTransforms(rs,state));
00236 
00237   //now we mess with the multi-dof, and now something's not getting set
00238   rs.multi_dof_joint_state.frame_ids.clear();
00239   ASSERT_FALSE(planning_environment::setRobotStateAndComputeTransforms(rs,state));
00240 }
00241 
00242 //Functional equivalent of test_collision_objects
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   //non-existent second link has no effect
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   //now we do it right
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 //Functional equivalent of test_alter_padding
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   //with 0.01 padding, shouldn't be in collision
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 //Functional equivalent of test_allowed_collision_operations
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   //now testing with objects
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   //now with attached objects
00426   att_object_1_.touch_links.push_back("r_gripper_palm_link");
00427   cm.addAttachedObject(att_object_1_);
00428 
00429   //this should also revert the allowed collision matrix
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   //this should override touch_links
00457 
00458   ASSERT_TRUE(check_acm.getAllowedCollision("r_gripper_palm_link","object_4", allowed));
00459   EXPECT_TRUE(allowed);
00460 
00461 }
00462 
00463 //Functional equivalent of test_allowed_collision_operations
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   //with no touch links should be in collision
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     //should get contacts between pole and touch_links and pole and table
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   //Testing group touch links
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   //empty path_constraints
00618   arm_navigation_msgs::Constraints path_constraints;
00619 
00620   //just testing goal constraints
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   //should be out of bounds
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   //valid goal constraint, but out of joint limits
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   //now we discretize and check collisions
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   //last one should replace other
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   //checking that if we turn around then we should subtract .5 in x
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