$search
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 #ifndef COLLISION_SPACE_ENVIRONMENT_MODEL_ 00038 #define COLLISION_SPACE_ENVIRONMENT_MODEL_ 00039 00040 #include "collision_space/environment_objects.h" 00041 #include <planning_models/kinematic_model.h> 00042 #include <planning_models/kinematic_state.h> 00043 #include <geometric_shapes/bodies.h> 00044 #include <LinearMath/btVector3.h> 00045 #include <boost/thread/recursive_mutex.hpp> 00046 #include <boost/shared_ptr.hpp> 00047 #include <vector> 00048 #include <string> 00049 00050 00052 namespace collision_space 00053 { 00054 00061 class EnvironmentModel 00062 { 00063 public: 00064 00065 enum BodyType { 00066 LINK, 00067 ATTACHED, 00068 OBJECT 00069 }; 00070 00072 struct Contact 00073 { 00075 btVector3 pos; 00077 btVector3 normal; 00079 double depth; 00080 00082 std::string body_name_1; 00083 BodyType body_type_1; 00084 00086 std::string body_name_2; 00087 BodyType body_type_2; 00088 }; 00089 00091 struct AllowedContact 00092 { 00094 boost::shared_ptr<bodies::Body> bound; 00095 00096 std::string body_name_1; 00097 std::string body_name_2; 00098 00100 double depth; 00101 }; 00102 00103 typedef std::map<std::string, std::map<std::string, std::vector<AllowedContact> > > AllowedContactMap; 00104 00106 /* False means that no collisions are allowed, true means ok */ 00107 class AllowedCollisionMatrix 00108 { 00109 public: 00110 00111 AllowedCollisionMatrix(){ 00112 valid_ = true; 00113 } 00114 00115 AllowedCollisionMatrix(const std::vector<std::string>& names, 00116 bool allowed = false); 00117 00118 AllowedCollisionMatrix(const std::vector<std::vector<bool> >& all_coll_vectors, 00119 const std::map<std::string, unsigned int>& all_coll_indices); 00120 00121 AllowedCollisionMatrix(const AllowedCollisionMatrix& acm); 00122 00123 bool getAllowedCollision(const std::string& name1, const std::string& name2, 00124 bool& allowed_collision) const; 00125 00126 bool getAllowedCollision(unsigned int i, unsigned int j, 00127 bool& allowed_collision) const; 00128 00129 bool hasEntry(const std::string& name) const; 00130 00131 bool getEntryIndex(const std::string& name, 00132 unsigned int& index) const; 00133 00134 bool getEntryName(const unsigned int ind, 00135 std::string& name) const; 00136 00137 bool removeEntry(const std::string& name); 00138 00139 bool addEntry(const std::string& name, bool allowed); 00140 00141 bool changeEntry(bool allowed); 00142 00143 bool changeEntry(const std::string& name, bool allowed); 00144 00145 bool changeEntry(const std::string& name1, 00146 const std::string& name2, 00147 bool allowed); 00148 00149 bool changeEntry(const unsigned int ind_1, 00150 const unsigned int ind_2, 00151 bool allowed); 00152 00153 bool changeEntry(const std::string& name, 00154 const std::vector<std::string>& change_names, 00155 bool allowed); 00156 00157 bool changeEntry(const std::vector<std::string>& change_names_1, 00158 const std::vector<std::string>& change_names_2, 00159 bool allowed); 00160 00161 void getAllEntryNames(std::vector<std::string>& names) const; 00162 00163 bool getValid() const { 00164 return valid_; 00165 }; 00166 00167 unsigned int getSize() const { 00168 return allowed_entries_.size(); 00169 } 00170 00171 typedef boost::bimap<std::string, unsigned int> entry_type; 00172 00173 const entry_type& getEntriesBimap() const { 00174 return allowed_entries_bimap_; 00175 } 00176 00177 void print(std::ostream& out) const; 00178 00179 private: 00180 00181 bool valid_; 00182 std::vector<std::vector<bool> > allowed_entries_; 00183 00184 entry_type allowed_entries_bimap_; 00185 }; 00186 00187 EnvironmentModel(void) 00188 { 00189 verbose_ = false; 00190 objects_ = new EnvironmentObjects(); 00191 use_altered_collision_matrix_ = false; 00192 } 00193 00194 virtual ~EnvironmentModel(void) 00195 { 00196 if (objects_) 00197 delete objects_; 00198 } 00199 00200 /**********************************************************************/ 00201 /* Collision Environment Configuration */ 00202 /**********************************************************************/ 00203 00210 virtual void setRobotModel(const planning_models::KinematicModel* model, 00211 const AllowedCollisionMatrix& acm, 00212 const std::map<std::string, double>& link_padding_map, 00213 double default_padding = 0.0, 00214 double scale = 1.0); 00215 00217 double getRobotScale(void) const; 00218 00220 double getRobotPadding(void) const; 00221 00223 virtual void updateRobotModel(const planning_models::KinematicState* state) = 0; 00224 00226 virtual void updateAttachedBodies() = 0; 00227 00229 const planning_models::KinematicModel* getRobotModel(void) const; 00230 00231 /**********************************************************************/ 00232 /* Collision Checking Routines */ 00233 /**********************************************************************/ 00234 00235 00237 virtual bool isCollision(void) const = 0; 00238 00240 virtual bool isSelfCollision(void) const = 0; 00241 00243 virtual bool isEnvironmentCollision(void) const = 0; 00244 00246 virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total = 1, unsigned int max_per_pair = 1) const = 0; 00247 00249 virtual bool getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_per_contact = 1) const = 0; 00250 00251 /**********************************************************************/ 00252 /* Collision Bodies */ 00253 /**********************************************************************/ 00254 00256 virtual void clearObjects(void) = 0; 00257 00259 virtual void clearObjects(const std::string &ns) = 0; 00260 00262 virtual bool hasObject(const std::string& ns) const = 0; 00263 00265 virtual void addObject(const std::string &ns, shapes::StaticShape *shape) = 0; 00266 00268 virtual void addObject(const std::string &ns, shapes::Shape* shape, const btTransform &pose) = 0; 00269 00271 virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses) = 0; 00272 00273 virtual void getAttachedBodyPoses(std::map<std::string, std::vector<btTransform> >& pose_map) const = 0; 00274 00276 virtual void setAlteredLinkPadding(const std::map<std::string, double>& link_padding_map); 00277 00279 virtual void revertAlteredLinkPadding(); 00280 00281 const std::map<std::string,double>& getDefaultLinkPaddingMap() const; 00282 00283 std::map<std::string,double> getCurrentLinkPaddingMap() const; 00284 00285 double getCurrentLinkPadding(std::string name) const; 00286 00288 const EnvironmentObjects* getObjects(void) const; 00289 00290 const AllowedCollisionMatrix& getDefaultAllowedCollisionMatrix() const; 00291 00292 const AllowedCollisionMatrix& getCurrentAllowedCollisionMatrix() const; 00293 00295 virtual void setAlteredCollisionMatrix(const AllowedCollisionMatrix& acm); 00296 00298 virtual void revertAlteredCollisionMatrix(); 00299 00301 virtual void setAllowedContacts(const std::vector<AllowedContact>& allowed_contacts); 00302 00304 const std::vector<AllowedContact>& getAllowedContacts() const; 00305 00307 void clearAllowedContacts() { 00308 allowed_contact_map_.clear(); 00309 allowed_contacts_.clear(); 00310 } 00311 00312 /**********************************************************************/ 00313 /* Miscellaneous Routines */ 00314 /**********************************************************************/ 00315 00317 void lock(void) const; 00318 00320 void unlock(void) const; 00321 00323 void setVerbose(bool verbose); 00324 00326 bool getVerbose(void) const; 00327 00329 virtual EnvironmentModel* clone(void) const = 0; 00330 00331 protected: 00332 00334 mutable boost::recursive_mutex lock_; 00335 00337 bool verbose_; 00338 00340 const planning_models::KinematicModel* robot_model_; 00341 00343 EnvironmentObjects *objects_; 00344 00346 double robot_scale_; 00347 00349 double default_robot_padding_; 00350 00351 AllowedCollisionMatrix default_collision_matrix_; 00352 AllowedCollisionMatrix altered_collision_matrix_; 00353 00354 bool use_altered_collision_matrix_; 00355 00356 std::map<std::string, double> default_link_padding_map_; 00357 std::map<std::string, double> altered_link_padding_map_; 00358 00359 bool use_altered_link_padding_map_; 00360 00361 AllowedContactMap allowed_contact_map_; 00362 std::vector<AllowedContact> allowed_contacts_; 00363 00364 }; 00365 } 00366 00367 #endif 00368