collision_space: environmentODE.h Source 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 void addSelfCollisionGroup(const std::vector<std::string> &group1,
00060                                      const std::vector<std::string> &group2);
00061 
00063   virtual void removeSelfCollisionGroup(const std::vector<std::string> &group1,
00064                                         const std::vector<std::string> &group2);
00065 
00067   virtual bool getCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int max_count = 1);
00068 
00070   virtual bool isCollision(void);
00071 
00073   virtual bool isSelfCollision(void);
00074         
00076   virtual void clearObjects(void);
00077         
00079   virtual void clearObjects(const std::string &ns);
00080                 
00082   virtual void addObject(const std::string &ns, shapes::StaticShape *shape);
00083 
00085   virtual void addObject(const std::string &ns, shapes::Shape* shape, const btTransform &pose);
00086 
00088   virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses);
00089 
00091   virtual void removeCollidingObjects(const shapes::StaticShape *shape);
00092 
00094   virtual void removeCollidingObjects(const shapes::Shape *shape, const btTransform &pose);
00095 
00096   virtual const std::vector<const planning_models::KinematicModel::AttachedBodyModel*> getAttachedBodies(void) const;
00097 
00098   virtual const std::vector<const planning_models::KinematicModel::AttachedBodyModel*> getAttachedBodies(const std::string link_name) const;
00099         
00106   virtual void setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, 
00107                              const std::vector<std::string> &links, 
00108                              const std::map<std::string, double>& link_padding_map,
00109                              double default_padding = 0.0,
00110                              double scale = 1.0); 
00111 
00113   virtual void updateRobotModel(const planning_models::KinematicState* state);
00114 
00116   virtual void updateAttachedBodies(void);
00117 
00119   virtual void updateAttachedBodies(const std::map<std::string, double>& link_padding_map);
00120 
00122   virtual int setCollisionCheck(const std::string &link, bool state);
00123 
00125   virtual void setRobotLinkPadding(const std::map<std::string, double>& link_padding_map);
00126 
00128   virtual void revertRobotLinkPadding();
00129 
00131   void setCollisionCheckLinks(const std::vector<std::string> &links, bool state);
00132 
00134   void setCollisionCheckOnlyLinks(const std::vector<std::string> &links, bool state);
00135 
00137   void setCollisionCheckAll(bool state);
00138 
00140   virtual EnvironmentModel* clone(void) const;
00141 
00142   void updateAllowedTouch(const std::map<std::string, double>& link_padding_map);
00143 
00144   void updateAllowedTouch(void);
00145   
00146   virtual void getDefaultAllowedCollisionMatrix(std::vector<std::vector<bool> > &matrix,
00147                                                 std::map<std::string, unsigned int> &ind) const;
00148   
00150   virtual void setAllowedCollisionMatrix(const std::vector<std::vector<bool> > &matrix,
00151                                  const std::map<std::string, unsigned int > &ind);
00152 
00154   virtual void revertAllowedCollisionMatrix();
00155 
00156 protected:
00157         
00158   class ODECollide2
00159   {
00160   public:
00161             
00162     ODECollide2(dSpaceID space = NULL)
00163     {   
00164       m_setup = false;
00165       if (space)
00166         registerSpace(space);
00167     }
00168             
00169     ~ODECollide2(void)
00170     {
00171       clear();
00172     }
00173             
00174     void registerSpace(dSpaceID space);
00175     void registerGeom(dGeomID geom);
00176     void unregisterGeom(dGeomID geom);
00177     void clear(void);
00178     void setup(void);
00179     void collide(dGeomID geom, void *data, dNearCallback *nearCallback) const;
00180     bool empty(void) const;
00181     void getGeoms(std::vector<dGeomID> &geoms) const;
00182             
00183   private:
00184             
00185     struct Geom
00186     {
00187       dGeomID id;
00188       dReal   aabb[6];
00189     };
00190             
00191     struct SortByXLow
00192     {
00193       bool operator()(const Geom *a, const Geom *b) const
00194       {
00195         if (a->aabb[0] < b->aabb[0])
00196           return true;
00197         return false;
00198       }
00199     };
00200             
00201     struct SortByYLow
00202     {
00203       bool operator()(const Geom *a, const Geom *b) const
00204       {
00205         if (a->aabb[2] < b->aabb[2])
00206           return true;
00207         return false;
00208       }
00209     };
00210             
00211     struct SortByZLow
00212     {
00213       bool operator()(const Geom *a, const Geom *b) const
00214       {
00215         if (a->aabb[4] < b->aabb[4])
00216           return true;
00217         return false;
00218       }
00219     };
00220             
00221     struct SortByXTest
00222     {
00223       bool operator()(const Geom *a, const Geom *b) const
00224       {
00225         if (a->aabb[1] < b->aabb[0])
00226           return true;
00227         return false;
00228       }
00229     };
00230             
00231     struct SortByYTest
00232     {
00233       bool operator()(const Geom *a, const Geom *b) const
00234       {
00235         if (a->aabb[3] < b->aabb[2])
00236           return true;
00237         return false;
00238       }
00239     };
00240             
00241     struct SortByZTest
00242     {
00243       bool operator()(const Geom *a, const Geom *b) const
00244       {
00245         if (a->aabb[5] < b->aabb[4])
00246           return true;
00247         return false;
00248       }
00249     };
00250             
00251     bool               m_setup;
00252     std::vector<Geom*> m_geomsX;
00253     std::vector<Geom*> m_geomsY;
00254     std::vector<Geom*> m_geomsZ;
00255             
00256     void checkColl(std::vector<Geom*>::const_iterator posStart, std::vector<Geom*>::const_iterator posEnd,
00257                    Geom *g, void *data, dNearCallback *nearCallback) const;
00258   };
00259 
00260   struct kGeom
00261   {
00262     std::vector<dGeomID>                   geom;
00263     std::map<dGeomID, unsigned int> geomAttachedBodyMap;
00264     std::vector< std::vector<bool> >       allowedTouch;
00265     bool                                   enabled;
00266     const planning_models::KinematicModel::LinkModel *link;
00267     unsigned int                           index;
00268   };
00269 
00271   struct ODEStorage
00272   {     
00273     struct Element
00274     {
00275       double         *vertices;
00276       dTriIndex      *indices;
00277       dTriMeshDataID  data;
00278       int             nIndices;
00279       int             nVertices;
00280     };
00281             
00282     ~ODEStorage(void)
00283     {
00284       clear();
00285     }
00286             
00287     void clear(void)
00288     {
00289       for (unsigned int i = 0 ; i < mesh.size() ; ++i)
00290       {
00291         delete[] mesh[i].indices;
00292         delete[] mesh[i].vertices;
00293         dGeomTriMeshDataDestroy(mesh[i].data);
00294       }
00295       mesh.clear();
00296     }
00297             
00298     /* Pointers for ODE indices; we need this around in ODE's assumed datatype */
00299     std::vector<Element> mesh;
00300   };
00301         
00302   struct ModelInfo
00303   {
00304     std::vector< kGeom* > linkGeom;
00305     dSpaceID              space;
00306     ODEStorage            storage;
00307   };
00308         
00309   struct CollisionNamespace
00310   {
00311     CollisionNamespace(const std::string &nm) : name(nm)
00312     {
00313       space = dHashSpaceCreate(0);
00314     }
00315             
00316     virtual ~CollisionNamespace(void)
00317     {
00318       if (space)
00319         dSpaceDestroy(space);
00320     }
00321             
00322     void clear(void)
00323     {
00324       if (space)
00325         dSpaceDestroy(space);
00326       space = dHashSpaceCreate(0);
00327       geoms.clear();
00328       collide2.clear();
00329       storage.clear();
00330     }
00331             
00332     std::string          name;
00333     dSpaceID             space;
00334     std::vector<dGeomID> geoms;
00335     ODECollide2          collide2;
00336     ODEStorage           storage;
00337   };
00338     
00339   struct CollisionData
00340   {
00341     CollisionData(void)
00342     {
00343       done = false;
00344       collides = false;
00345       max_contacts = 0;
00346       contacts = NULL;
00347       selfCollisionTest = NULL;
00348       link1 = link2 = NULL;
00349       allowed = NULL;
00350     }
00351             
00352     bool                                       done;
00353             
00354     bool                                       collides;
00355     unsigned int                               max_contacts;
00356     std::vector<EnvironmentModelODE::Contact> *contacts;
00357     const std::vector< std::vector<bool> >    *selfCollisionTest;
00358     dSpaceID                                   selfSpace;
00359             
00360     const std::vector<AllowedContact>         *allowed;
00361             
00362     const planning_models::KinematicModel::LinkModel     *link1;
00363     const planning_models::KinematicModel::LinkModel     *link2;
00364   };
00365         
00366         
00368   void testCollision(CollisionData *data);
00369 
00371   void testSelfCollision(CollisionData *data);
00372 
00374   void testBodyCollision(CollisionNamespace *cn, CollisionData *data);
00375 
00376   dGeomID copyGeom(dSpaceID space, ODEStorage &storage, dGeomID geom, ODEStorage &sourceStorage) const;
00377   void    createODERobotModel();        
00378   dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::Shape *shape, double scale, double padding);
00379   dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::StaticShape *shape);
00380   void    updateGeom(dGeomID geom, const btTransform &pose) const;      
00381   void    removeCollidingObjects(const dGeomID geom);
00382 
00384   void    checkThreadInit(void) const;  
00385   void    freeMemory(void);     
00386         
00387   ModelInfo                                   m_modelGeom;
00388   std::map<std::string, CollisionNamespace*>  m_collNs;
00389   std::vector<bool> stored_collision_check;
00390         
00391 };
00392 }
00393 
00394 #endif