00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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 void clearObjects(void);
00075
00077 virtual void clearObjects(const std::string &ns);
00078
00080 virtual bool hasObject(const std::string& ns) const;
00081
00083 virtual void addObject(const std::string &ns, shapes::StaticShape *shape);
00084
00086 virtual void addObject(const std::string &ns, shapes::Shape* shape, const btTransform &pose);
00087
00089 virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses);
00090
00091 virtual void getAttachedBodyPoses(std::map<std::string, std::vector<btTransform> >& pose_map) const;
00092
00099 virtual void setRobotModel(const planning_models::KinematicModel* model,
00100 const AllowedCollisionMatrix& allowed_collision_matrix,
00101 const std::map<std::string, double>& link_padding_map,
00102 double default_padding = 0.0,
00103 double scale = 1.0);
00104
00106 virtual void updateRobotModel(const planning_models::KinematicState* state);
00107
00109 virtual void updateAttachedBodies(void);
00110
00112 virtual void updateAttachedBodies(const std::map<std::string, double>& link_padding_map);
00113
00115 virtual void setAlteredLinkPadding(const std::map<std::string, double>& link_padding_map);
00116
00118 virtual void revertAlteredLinkPadding();
00119
00121 virtual EnvironmentModel* clone(void) const;
00122
00123 protected:
00124
00126 struct ODEStorage
00127 {
00128 struct Element
00129 {
00130 double *vertices;
00131 dTriIndex *indices;
00132 dTriMeshDataID data;
00133 int n_indices;
00134 int n_vertices;
00135 };
00136
00137 ~ODEStorage(void)
00138 {
00139 clear();
00140 }
00141
00142 void remove(dGeomID id) {
00143
00144 if(meshes.find(id) == meshes.end()) {
00145 return;
00146 }
00147
00148 Element& e = meshes[id];
00149 delete[] e.indices;
00150 delete[] e.vertices;
00151 dGeomTriMeshDataDestroy(e.data);
00152 meshes.erase(id);
00153 }
00154
00155 void clear(void)
00156 {
00157 for(std::map<dGeomID, Element>::iterator it = meshes.begin();
00158 it != meshes.end();
00159 it++)
00160 {
00161 delete[] it->second.indices;
00162 delete[] it->second.vertices;
00163 dGeomTriMeshDataDestroy(it->second.data);
00164 }
00165 meshes.clear();
00166 }
00167
00168
00169 std::map<dGeomID, Element> meshes;
00170 };
00171
00172 class ODECollide2
00173 {
00174 public:
00175
00176 ODECollide2(dSpaceID space = NULL)
00177 {
00178 setup_ = false;
00179 if (space)
00180 registerSpace(space);
00181 }
00182
00183 ~ODECollide2(void)
00184 {
00185 clear();
00186 }
00187
00188 void registerSpace(dSpaceID space);
00189 void registerGeom(dGeomID geom);
00190 void unregisterGeom(dGeomID geom);
00191 void clear(void);
00192 void setup(void);
00193 void collide(dGeomID geom, void *data, dNearCallback *nearCallback) const;
00194 bool empty(void) const;
00195 void getGeoms(std::vector<dGeomID> &geoms) const;
00196
00197 private:
00198
00199 struct Geom
00200 {
00201 dGeomID id;
00202 dReal aabb[6];
00203 };
00204
00205 struct SortByXLow
00206 {
00207 bool operator()(const Geom *a, const Geom *b) const
00208 {
00209 if (a->aabb[0] < b->aabb[0])
00210 return true;
00211 return false;
00212 }
00213 };
00214
00215 struct SortByYLow
00216 {
00217 bool operator()(const Geom *a, const Geom *b) const
00218 {
00219 if (a->aabb[2] < b->aabb[2])
00220 return true;
00221 return false;
00222 }
00223 };
00224
00225 struct SortByZLow
00226 {
00227 bool operator()(const Geom *a, const Geom *b) const
00228 {
00229 if (a->aabb[4] < b->aabb[4])
00230 return true;
00231 return false;
00232 }
00233 };
00234
00235 struct SortByXTest
00236 {
00237 bool operator()(const Geom *a, const Geom *b) const
00238 {
00239 if (a->aabb[1] < b->aabb[0])
00240 return true;
00241 return false;
00242 }
00243 };
00244
00245 struct SortByYTest
00246 {
00247 bool operator()(const Geom *a, const Geom *b) const
00248 {
00249 if (a->aabb[3] < b->aabb[2])
00250 return true;
00251 return false;
00252 }
00253 };
00254
00255 struct SortByZTest
00256 {
00257 bool operator()(const Geom *a, const Geom *b) const
00258 {
00259 if (a->aabb[5] < b->aabb[4])
00260 return true;
00261 return false;
00262 }
00263 };
00264
00265 bool setup_;
00266 std::vector<Geom*> geoms_x;
00267 std::vector<Geom*> geoms_y;
00268 std::vector<Geom*> geoms_z;
00269
00270 void checkColl(std::vector<Geom*>::const_iterator posStart, std::vector<Geom*>::const_iterator posEnd,
00271 Geom *g, void *data, dNearCallback *nearCallback) const;
00272 };
00273
00274 struct AttGeom
00275 {
00276 AttGeom(ODEStorage& s) : storage(s){
00277 }
00278
00279 ~AttGeom() {
00280
00281 for(unsigned int i = 0; i < geom.size(); i++) {
00282 dGeomDestroy(geom[i]);
00283 storage.remove(geom[i]);
00284 }
00285 for(unsigned int i = 0; i < padded_geom.size(); i++) {
00286 dGeomDestroy(padded_geom[i]);
00287 storage.remove(padded_geom[i]);
00288 }
00289 }
00290
00291 ODEStorage& storage;
00292 std::vector<dGeomID> geom;
00293 std::vector<dGeomID> padded_geom;
00294 const planning_models::KinematicModel::AttachedBodyModel *att;
00295 unsigned int index;
00296 };
00297
00298 struct LinkGeom
00299 {
00300 LinkGeom(ODEStorage& s) : storage(s){
00301 }
00302
00303 ~LinkGeom() {
00304 for(unsigned int i = 0; i < geom.size(); i++) {
00305 dGeomDestroy(geom[i]);
00306 }
00307 for(unsigned int i = 0; i < padded_geom.size(); i++) {
00308 dGeomDestroy(padded_geom[i]);
00309 }
00310 deleteAttachedBodies();
00311 }
00312
00313 void deleteAttachedBodies()
00314 {
00315 for(unsigned int i = 0; i < att_bodies.size(); i++) {
00316 delete att_bodies[i];
00317 }
00318 att_bodies.clear();
00319 }
00320
00321 ODEStorage& storage;
00322 std::vector<dGeomID> geom;
00323 std::vector<dGeomID> padded_geom;
00324 std::vector<AttGeom*> att_bodies;
00325 const planning_models::KinematicModel::LinkModel *link;
00326 unsigned int index;
00327 };
00328
00329 struct ModelInfo
00330 {
00331 ~ModelInfo() {
00332 storage.clear();
00333 }
00334
00335 std::vector< LinkGeom* > link_geom;
00336 dSpaceID env_space;
00337 dSpaceID self_space;
00338 ODEStorage storage;
00339 };
00340
00341 struct CollisionNamespace
00342 {
00343 CollisionNamespace(const std::string &nm) : name(nm)
00344 {
00345 space = dHashSpaceCreate(0);
00346 }
00347
00348 virtual ~CollisionNamespace(void)
00349 {
00350 if (space) {
00351 dSpaceDestroy(space);
00352 }
00353 }
00354
00355 void clear(void)
00356 {
00357 if (space) {
00358 dSpaceDestroy(space);
00359 }
00360 space = dHashSpaceCreate(0);
00361 geoms.clear();
00362 collide2.clear();
00363 storage.clear();
00364 }
00365
00366 std::string name;
00367 dSpaceID space;
00368 std::vector<dGeomID> geoms;
00369 ODECollide2 collide2;
00370 ODEStorage storage;
00371 };
00372
00373 struct CollisionData
00374 {
00375 CollisionData(void)
00376 {
00377 done = false;
00378 collides = false;
00379 max_contacts_total = 0;
00380 max_contacts_pair = 0;
00381 contacts = NULL;
00382 allowed_collision_matrix = NULL;
00383 allowed = NULL;
00384 }
00385
00386
00387 unsigned int max_contacts_total;
00388 unsigned int max_contacts_pair;
00389 const AllowedCollisionMatrix *allowed_collision_matrix;
00390 const std::map<dGeomID, std::pair<std::string, BodyType> >* geom_lookup_map;
00391 const std::map<std::string, dSpaceID>* dspace_lookup_map;
00392 const AllowedContactMap *allowed;
00393
00394
00395 bool done;
00396 bool collides;
00397 std::vector<EnvironmentModelODE::Contact> *contacts;
00398
00399
00400 std::string body_name_1;
00401 BodyType body_type_1;
00402
00403 std::string body_name_2;
00404 BodyType body_type_2;
00405
00406
00407
00408 };
00409
00410
00412 void testCollision(CollisionData *data) const;
00413
00415 void testSelfCollision(CollisionData *data) const;
00416
00418 void testEnvironmentCollision(CollisionData *data) const;
00419
00421 void testObjectCollision(CollisionNamespace *cn, CollisionData *data) const;
00422
00423 dGeomID copyGeom(dSpaceID space, ODEStorage &storage, dGeomID geom, ODEStorage &sourceStorage) const;
00424 void createODERobotModel();
00425 dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::Shape *shape, double scale, double padding);
00426 dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::StaticShape *shape);
00427 void updateGeom(dGeomID geom, const btTransform &pose) const;
00428
00429 void addAttachedBody(LinkGeom* lg, const planning_models::KinematicModel::AttachedBodyModel* attm,
00430 double padd);
00431
00432 std::map<std::string, bool> attached_bodies_in_collision_matrix_;
00433
00434 void setAttachedBodiesLinkPadding();
00435 void revertAttachedBodiesLinkPadding();
00436
00437 void freeMemory(void);
00438
00439 ModelInfo model_geom_;
00440 std::map<std::string, CollisionNamespace*> coll_namespaces_;
00441
00442 std::map<dGeomID, std::pair<std::string, BodyType> > geom_lookup_map_;
00443 std::map<std::string, dSpaceID> dspace_lookup_map_;
00444
00445 bool previous_set_robot_model_;
00446
00447 void checkThreadInit(void) const;
00448 };
00449 }
00450
00451 #endif
