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 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
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
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
00409 bool done;
00410 bool collides;
00411 std::vector<EnvironmentModelODE::Contact> *contacts;
00412
00413
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