environmentODE.h
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 #ifndef COLLISION_SPACE_ENVIRONMENT_MODEL_ODE_
00038 #define COLLISION_SPACE_ENVIRONMENT_MODEL_ODE_
00039 
00040 #include "collision_space/environment.h"
00041 #include <ode/ode.h>
00042 #include <map>
00043 
00044 namespace collision_space
00045 {
00046         
00048 class EnvironmentModelODE : public EnvironmentModel
00049 {     
00050   
00051   friend void nearCallbackFn(void *data, dGeomID o1, dGeomID o2);
00052         
00053 public:
00054                 
00055   EnvironmentModelODE(void);
00056   virtual ~EnvironmentModelODE(void);
00057 
00059   virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total = 1, unsigned int max_per_pair = 1) const;
00060 
00062   virtual bool getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_per_contact = 1) const;
00063 
00065   virtual bool isCollision(void) const;
00066 
00068   virtual bool isSelfCollision(void) const;
00069 
00071   virtual bool isEnvironmentCollision(void) const;
00072 
00074   virtual bool isObjectRobotCollision(const std::string& object_name) const;    
00075 
00077   virtual bool isObjectObjectCollision(const std::string& object1_name, 
00078                                        const std::string& object2_name) const;
00079 
00081   virtual bool isObjectInEnvironmentCollision(const std::string& object_name) const;
00082 
00083   virtual bool getAllObjectEnvironmentCollisionContacts (const std::string& object_name, 
00084                                                          std::vector<Contact> &contacts,
00085                                                          unsigned int num_contacts_per_pair) const;
00086         
00088   virtual void clearObjects(void);
00089         
00091   virtual void clearObjects(const std::string &ns);
00092 
00094   virtual bool hasObject(const std::string& ns) const;
00095                 
00097   virtual void addObject(const std::string &ns, shapes::StaticShape *shape);
00098 
00100   virtual void addObject(const std::string &ns, shapes::Shape* shape, const tf::Transform &pose);
00101 
00103   virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<tf::Transform> &poses);
00104 
00105   virtual void getAttachedBodyPoses(std::map<std::string, std::vector<tf::Transform> >& pose_map) const;
00106 
00113   virtual void setRobotModel(const planning_models::KinematicModel* model, 
00114                              const AllowedCollisionMatrix& allowed_collision_matrix,
00115                              const std::map<std::string, double>& link_padding_map,
00116                              double default_padding = 0.0,
00117                              double scale = 1.0); 
00118 
00120   virtual void updateRobotModel(const planning_models::KinematicState* state);
00121 
00123   virtual void updateAttachedBodies(void);
00124 
00126   virtual void updateAttachedBodies(const std::map<std::string, double>& link_padding_map);
00127 
00129   virtual void setAlteredLinkPadding(const std::map<std::string, double>& link_padding_map);
00130 
00132   virtual void revertAlteredLinkPadding();
00133 
00135   virtual EnvironmentModel* clone(void) const;
00136 
00137 protected:
00138 
00140   struct ODEStorage
00141   {     
00142     struct Element
00143     {
00144       double *vertices;
00145       dTriIndex *indices;
00146       dTriMeshDataID data;
00147       int n_indices;
00148       int n_vertices;
00149     };
00150             
00151     ~ODEStorage(void)
00152     {
00153       clear();
00154     }
00155 
00156     void remove(dGeomID id) {
00157       
00158       if(meshes.find(id) == meshes.end()) {
00159         return;
00160       }
00161 
00162       Element& e = meshes[id];
00163       delete[] e.indices;
00164       delete[] e.vertices;
00165       dGeomTriMeshDataDestroy(e.data);
00166       meshes.erase(id);
00167     }
00168             
00169     void clear(void)
00170     {
00171       for(std::map<dGeomID, Element>::iterator it = meshes.begin();
00172           it != meshes.end();
00173           it++)
00174       {
00175         delete[] it->second.indices;
00176         delete[] it->second.vertices;
00177         dGeomTriMeshDataDestroy(it->second.data);
00178       }
00179       meshes.clear();
00180     }
00181             
00182     /* Pointers for ODE indices; we need this around in ODE's assumed datatype */
00183     std::map<dGeomID, Element> meshes;
00184   };
00185         
00186   class ODECollide2
00187   {
00188   public:
00189             
00190     ODECollide2(dSpaceID space = NULL)
00191     {   
00192       setup_ = false;
00193       if (space)
00194         registerSpace(space);
00195     }
00196             
00197     ~ODECollide2(void)
00198     {
00199       clear();
00200     }
00201             
00202     void registerSpace(dSpaceID space);
00203     void registerGeom(dGeomID geom);
00204     void unregisterGeom(dGeomID geom);
00205     void clear(void);
00206     void setup(void);
00207     void collide(dGeomID geom, void *data, dNearCallback *nearCallback) const;
00208     bool empty(void) const;
00209     void getGeoms(std::vector<dGeomID> &geoms) const;
00210             
00211   private:
00212             
00213     struct Geom
00214     {
00215       dGeomID id;
00216       dReal   aabb[6];
00217     };
00218             
00219     struct SortByXLow
00220     {
00221       bool operator()(const Geom *a, const Geom *b) const
00222       {
00223         if (a->aabb[0] < b->aabb[0])
00224           return true;
00225         return false;
00226       }
00227     };
00228             
00229     struct SortByYLow
00230     {
00231       bool operator()(const Geom *a, const Geom *b) const
00232       {
00233         if (a->aabb[2] < b->aabb[2])
00234           return true;
00235         return false;
00236       }
00237     };
00238             
00239     struct SortByZLow
00240     {
00241       bool operator()(const Geom *a, const Geom *b) const
00242       {
00243         if (a->aabb[4] < b->aabb[4])
00244           return true;
00245         return false;
00246       }
00247     };
00248             
00249     struct SortByXTest
00250     {
00251       bool operator()(const Geom *a, const Geom *b) const
00252       {
00253         if (a->aabb[1] < b->aabb[0])
00254           return true;
00255         return false;
00256       }
00257     };
00258             
00259     struct SortByYTest
00260     {
00261       bool operator()(const Geom *a, const Geom *b) const
00262       {
00263         if (a->aabb[3] < b->aabb[2])
00264           return true;
00265         return false;
00266       }
00267     };
00268             
00269     struct SortByZTest
00270     {
00271       bool operator()(const Geom *a, const Geom *b) const
00272       {
00273         if (a->aabb[5] < b->aabb[4])
00274           return true;
00275         return false;
00276       }
00277     };
00278             
00279     bool setup_;
00280     std::vector<Geom*> geoms_x;
00281     std::vector<Geom*> geoms_y;
00282     std::vector<Geom*> geoms_z;
00283             
00284     void checkColl(std::vector<Geom*>::const_iterator posStart, std::vector<Geom*>::const_iterator posEnd,
00285                    Geom *g, void *data, dNearCallback *nearCallback) const;
00286   };
00287 
00288   struct AttGeom
00289   {
00290     AttGeom(ODEStorage& s) : storage(s){
00291     }
00292 
00293     ~AttGeom() {
00294 
00295       for(unsigned int i = 0; i < geom.size(); i++) {
00296         dGeomDestroy(geom[i]);
00297         storage.remove(geom[i]);
00298       }
00299       for(unsigned int i = 0; i < padded_geom.size(); i++) {
00300         dGeomDestroy(padded_geom[i]);
00301         storage.remove(padded_geom[i]);
00302       }
00303     }
00304 
00305     ODEStorage& storage;
00306     std::vector<dGeomID> geom;
00307     std::vector<dGeomID> padded_geom;
00308     const planning_models::KinematicModel::AttachedBodyModel *att;
00309     unsigned int index;
00310   };
00311 
00312   struct LinkGeom
00313   {
00314     LinkGeom(ODEStorage& s) : storage(s){
00315     }
00316 
00317     ~LinkGeom() {
00318       for(unsigned int i = 0; i < geom.size(); i++) {
00319         dGeomDestroy(geom[i]);
00320       }
00321       for(unsigned int i = 0; i < padded_geom.size(); i++) {
00322         dGeomDestroy(padded_geom[i]);
00323       }
00324       deleteAttachedBodies();
00325     }
00326     
00327     void deleteAttachedBodies()
00328     {
00329       for(unsigned int i = 0; i < att_bodies.size(); i++) {
00330         delete att_bodies[i];
00331       }
00332       att_bodies.clear();
00333     }
00334 
00335     ODEStorage& storage;
00336     std::vector<dGeomID> geom;
00337     std::vector<dGeomID> padded_geom;
00338     std::vector<AttGeom*> att_bodies;
00339     const planning_models::KinematicModel::LinkModel *link;
00340     unsigned int index;
00341   };
00342         
00343   struct ModelInfo
00344   {
00345     ~ModelInfo() {
00346       storage.clear();
00347     }
00348 
00349     std::vector< LinkGeom* > link_geom;
00350     dSpaceID env_space;
00351     dSpaceID self_space;
00352     ODEStorage storage;
00353   };
00354         
00355   struct CollisionNamespace
00356   {
00357     CollisionNamespace(const std::string &nm) : name(nm)
00358     {
00359       space = dHashSpaceCreate(0);
00360     }
00361             
00362     virtual ~CollisionNamespace(void)
00363     {
00364       if (space) {
00365         dSpaceDestroy(space);
00366       }
00367     }
00368             
00369     void clear(void)
00370     {
00371       if (space) { 
00372         dSpaceDestroy(space);
00373       }
00374       space = dHashSpaceCreate(0);
00375       geoms.clear();
00376       collide2.clear();
00377       storage.clear();
00378     }
00379             
00380     std::string name;
00381     dSpaceID space;
00382     std::vector<dGeomID> geoms;
00383     ODECollide2 collide2;
00384     ODEStorage storage;
00385   };
00386     
00387   struct CollisionData
00388   {
00389     CollisionData(void)
00390     {
00391       done = false;
00392       collides = false;
00393       max_contacts_total = 0;
00394       max_contacts_pair = 0;
00395       contacts = NULL;
00396       allowed_collision_matrix = NULL;
00397       allowed = NULL;
00398     }
00399 
00400     //these are parameters
00401     unsigned int max_contacts_total;
00402     unsigned int max_contacts_pair;
00403     const AllowedCollisionMatrix *allowed_collision_matrix;
00404     const std::map<dGeomID, std::pair<std::string, BodyType> >* geom_lookup_map;
00405     const std::map<std::string, dSpaceID>* dspace_lookup_map;
00406     const AllowedContactMap *allowed;
00407             
00408     //these are for return info
00409     bool done;
00410     bool collides;
00411     std::vector<EnvironmentModelODE::Contact> *contacts;
00412     
00413     //for the last collision found
00414     std::string body_name_1;
00415     BodyType body_type_1;
00416 
00417     std::string body_name_2;
00418     BodyType body_type_2;
00419 
00420 
00421 
00422   };
00423         
00424         
00426   void testCollision(CollisionData *data) const;
00427 
00429   void testSelfCollision(CollisionData *data) const;
00430 
00432   void testEnvironmentCollision(CollisionData *data) const;
00433 
00435   void testObjectObjectCollision(CollisionData *cdata, 
00436                                  const std::string& object1_name, 
00437                                  const std::string& object2_name) const;
00438 
00440   void testObjectEnvironmentCollision(CollisionData *cdata, 
00441                                       const std::string& object_name) const;
00442 
00444   void testObjectCollision(CollisionNamespace *cn, CollisionData *data) const;
00445   
00446   dGeomID copyGeom(dSpaceID space, ODEStorage &storage, dGeomID geom, ODEStorage &sourceStorage) const;
00447   void createODERobotModel();   
00448   dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::Shape *shape, double scale, double padding);
00449   dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::StaticShape *shape);
00450   void updateGeom(dGeomID geom, const tf::Transform &pose) const;       
00451 
00452   void addAttachedBody(LinkGeom* lg, const planning_models::KinematicModel::AttachedBodyModel* attm,
00453                        double padd);
00454 
00455   std::map<std::string, bool> attached_bodies_in_collision_matrix_;
00456 
00457   void setAttachedBodiesLinkPadding();
00458   void revertAttachedBodiesLinkPadding();
00459 
00460   void freeMemory(void);        
00461         
00462   ModelInfo model_geom_;
00463   std::map<std::string, CollisionNamespace*> coll_namespaces_;
00464 
00465   std::map<dGeomID, std::pair<std::string, BodyType> > geom_lookup_map_;
00466   std::map<std::string, dSpaceID> dspace_lookup_map_;
00467 
00468   bool previous_set_robot_model_;
00469 
00470   void checkThreadInit(void) const;     
00471 };
00472 }
00473 
00474 #endif


collision_space
Author(s): Ioan Sucan/isucan@willowgarage.com
autogenerated on Mon Dec 2 2013 12:34:20