$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 #include "collision_space/environmentODE.h" 00038 #include <geometric_shapes/shape_operations.h> 00039 #include <ros/console.h> 00040 #include <cassert> 00041 #include <cstdio> 00042 #include <cmath> 00043 #include <algorithm> 00044 #include <map> 00045 00046 #include <boost/thread.hpp> 00047 static int ODEInitCount = 0; 00048 static boost::mutex ODEInitCountLock; 00049 00050 static std::map<boost::thread::id, int> ODEThreadMap; 00051 static boost::mutex ODEThreadMapLock; 00052 00053 //all the threading stuff is necessary to check collision from different threads 00054 00055 static const int MAX_ODE_CONTACTS = 128; 00056 static const int TEST_FOR_ALLOWED_NUM = 1; 00057 00058 static const std::string CONTACT_ONLY_NAME="contact_only"; 00059 00060 collision_space::EnvironmentModelODE::EnvironmentModelODE(void) : EnvironmentModel() 00061 { 00062 ODEInitCountLock.lock(); 00063 if (ODEInitCount == 0) 00064 { 00065 int res = dInitODE2(0); 00066 ROS_DEBUG_STREAM("Calling ODE Init res " << res); 00067 } 00068 ODEInitCount++; 00069 ODEInitCountLock.unlock(); 00070 00071 checkThreadInit(); 00072 00073 ROS_DEBUG("Initializing ODE"); 00074 00075 model_geom_.env_space = dSweepAndPruneSpaceCreate(0, dSAP_AXES_XZY); 00076 model_geom_.self_space = dSweepAndPruneSpaceCreate(0, dSAP_AXES_XZY); 00077 00078 previous_set_robot_model_ = false; 00079 } 00080 00081 collision_space::EnvironmentModelODE::~EnvironmentModelODE(void) 00082 { 00083 freeMemory(); 00084 ODEInitCountLock.lock(); 00085 ODEInitCount--; 00086 boost::thread::id id = boost::this_thread::get_id(); 00087 ODEThreadMapLock.lock(); 00088 ODEThreadMap.erase(id); 00089 ODEThreadMapLock.unlock(); 00090 if (ODEInitCount == 0) 00091 { 00092 ODEThreadMap.clear(); 00093 ROS_DEBUG("Closing ODE"); 00094 dCloseODE(); 00095 } 00096 ODEInitCountLock.unlock(); 00097 } 00098 00099 void collision_space::EnvironmentModelODE::freeMemory(void) 00100 { 00101 for (unsigned int j = 0 ; j < model_geom_.link_geom.size() ; ++j) 00102 delete model_geom_.link_geom[j]; 00103 model_geom_.link_geom.clear(); 00104 if (model_geom_.env_space) 00105 dSpaceDestroy(model_geom_.env_space); 00106 if (model_geom_.self_space) 00107 dSpaceDestroy(model_geom_.self_space); 00108 for (std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.begin() ; it != coll_namespaces_.end() ; ++it) { 00109 delete it->second; 00110 } 00111 model_geom_.storage.clear(); 00112 coll_namespaces_.clear(); 00113 } 00114 00115 void collision_space::EnvironmentModelODE::checkThreadInit(void) const 00116 { 00117 boost::thread::id id = boost::this_thread::get_id(); 00118 ODEThreadMapLock.lock(); 00119 if (ODEThreadMap.find(id) == ODEThreadMap.end()) 00120 { 00121 ODEThreadMap[id] = 1; 00122 ROS_DEBUG("Initializing new thread (%d total)", (int)ODEThreadMap.size()); 00123 int res = dAllocateODEDataForThread(dAllocateMaskAll); 00124 ROS_DEBUG_STREAM("Init says " << res); 00125 } 00126 ODEThreadMapLock.unlock(); 00127 } 00128 00129 void collision_space::EnvironmentModelODE::setRobotModel(const planning_models::KinematicModel* model, 00130 const AllowedCollisionMatrix& allowed_collision_matrix, 00131 const std::map<std::string, double>& link_padding_map, 00132 double default_padding, 00133 double scale) 00134 { 00135 collision_space::EnvironmentModel::setRobotModel(model, allowed_collision_matrix, link_padding_map, default_padding, scale); 00136 if(previous_set_robot_model_) { 00137 for (unsigned int j = 0 ; j < model_geom_.link_geom.size() ; ++j) 00138 delete model_geom_.link_geom[j]; 00139 model_geom_.link_geom.clear(); 00140 dSpaceDestroy(model_geom_.env_space); 00141 dSpaceDestroy(model_geom_.self_space); 00142 model_geom_.env_space = dSweepAndPruneSpaceCreate(0, dSAP_AXES_XZY); 00143 model_geom_.self_space = dSweepAndPruneSpaceCreate(0, dSAP_AXES_XZY); 00144 attached_bodies_in_collision_matrix_.clear(); 00145 geom_lookup_map_.clear(); 00146 } 00147 createODERobotModel(); 00148 previous_set_robot_model_ = true; 00149 } 00150 00151 void collision_space::EnvironmentModelODE::getAttachedBodyPoses(std::map<std::string, std::vector<btTransform> >& pose_map) const 00152 { 00153 pose_map.clear(); 00154 00155 const unsigned int n = model_geom_.link_geom.size(); 00156 for (unsigned int i = 0 ; i < n ; ++i) 00157 { 00158 LinkGeom *lg = model_geom_.link_geom[i]; 00159 00160 /* create new set of attached bodies */ 00161 const unsigned int nab = lg->att_bodies.size(); 00162 std::vector<btTransform> nbt; 00163 for (unsigned int j = 0 ; j < nab ; ++j) 00164 { 00165 for(unsigned int k = 0; k < lg->att_bodies[j]->geom.size(); k++) { 00166 const dReal *pos = dGeomGetPosition(lg->att_bodies[j]->geom[k]); 00167 dQuaternion q; 00168 dGeomGetQuaternion(lg->att_bodies[j]->geom[k], q); 00169 //note that ODE puts w first (w,x,y,z) 00170 nbt.push_back(btTransform(btQuaternion(q[1], q[2], q[3], q[0]), btVector3(pos[0], pos[1], pos[2]))); 00171 } 00172 pose_map[lg->att_bodies[j]->att->getName()] = nbt; 00173 } 00174 } 00175 } 00176 00177 void collision_space::EnvironmentModelODE::createODERobotModel() 00178 { 00179 for (unsigned int i = 0 ; i < robot_model_->getLinkModels().size() ; ++i) 00180 { 00181 /* skip this link if we have no geometry or if the link 00182 name is not specified as enabled for collision 00183 checking */ 00184 const planning_models::KinematicModel::LinkModel *link = robot_model_->getLinkModels()[i]; 00185 if (!link || !link->getLinkShape()) 00186 continue; 00187 00188 LinkGeom *lg = new LinkGeom(model_geom_.storage); 00189 lg->link = link; 00190 if(!default_collision_matrix_.getEntryIndex(link->getName(), lg->index)) { 00191 ROS_WARN_STREAM("Link " << link->getName() << " not in provided collision matrix"); 00192 } 00193 double padd = default_robot_padding_; 00194 if(default_link_padding_map_.find(link->getName()) != default_link_padding_map_.end()) { 00195 padd = default_link_padding_map_.find(link->getName())->second; 00196 } 00197 ROS_DEBUG_STREAM("Link " << link->getName() << " padding " << padd); 00198 00199 dGeomID unpadd_g = createODEGeom(model_geom_.self_space, model_geom_.storage, link->getLinkShape(), 1.0, 0.0); 00200 assert(unpadd_g); 00201 lg->geom.push_back(unpadd_g); 00202 geom_lookup_map_[unpadd_g] = std::pair<std::string, BodyType>(link->getName(), LINK); 00203 00204 dGeomID padd_g = createODEGeom(model_geom_.env_space, model_geom_.storage, link->getLinkShape(), robot_scale_, padd); 00205 assert(padd_g); 00206 lg->padded_geom.push_back(padd_g); 00207 geom_lookup_map_[padd_g] = std::pair<std::string, BodyType>(link->getName(), LINK); 00208 const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_bodies = link->getAttachedBodyModels(); 00209 for (unsigned int j = 0 ; j < attached_bodies.size() ; ++j) { 00210 padd = default_robot_padding_; 00211 if(default_link_padding_map_.find(attached_bodies[j]->getName()) != default_link_padding_map_.end()) { 00212 padd = default_link_padding_map_.find(attached_bodies[j]->getName())->second; 00213 } else if (default_link_padding_map_.find("attached") != default_link_padding_map_.end()) { 00214 padd = default_link_padding_map_.find("attached")->second; 00215 } 00216 addAttachedBody(lg, attached_bodies[j], padd); 00217 } 00218 model_geom_.link_geom.push_back(lg); 00219 } 00220 } 00221 00222 dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::StaticShape *shape) 00223 { 00224 dGeomID g = NULL; 00225 switch (shape->type) 00226 { 00227 case shapes::PLANE: 00228 { 00229 const shapes::Plane *p = static_cast<const shapes::Plane*>(shape); 00230 g = dCreatePlane(space, p->a, p->b, p->c, p->d); 00231 } 00232 break; 00233 default: 00234 break; 00235 } 00236 return g; 00237 } 00238 00239 dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::Shape *shape, double scale, double padding) 00240 { 00241 dGeomID g = NULL; 00242 switch (shape->type) 00243 { 00244 case shapes::SPHERE: 00245 { 00246 g = dCreateSphere(space, static_cast<const shapes::Sphere*>(shape)->radius * scale + padding); 00247 } 00248 break; 00249 case shapes::BOX: 00250 { 00251 const double *size = static_cast<const shapes::Box*>(shape)->size; 00252 g = dCreateBox(space, size[0] * scale + padding * 2.0, size[1] * scale + padding * 2.0, size[2] * scale + padding * 2.0); 00253 } 00254 break; 00255 case shapes::CYLINDER: 00256 { 00257 g = dCreateCylinder(space, static_cast<const shapes::Cylinder*>(shape)->radius * scale + padding, 00258 static_cast<const shapes::Cylinder*>(shape)->length * scale + padding * 2.0); 00259 } 00260 break; 00261 case shapes::MESH: 00262 { 00263 const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape); 00264 if (mesh->vertexCount > 0 && mesh->triangleCount > 0) 00265 { 00266 // copy indices for ODE 00267 int icount = mesh->triangleCount * 3; 00268 dTriIndex *indices = new dTriIndex[icount]; 00269 for (int i = 0 ; i < icount ; ++i) 00270 indices[i] = mesh->triangles[i]; 00271 00272 // copt vertices for ODE 00273 double *vertices = new double[mesh->vertexCount* 3]; 00274 double sx = 0.0, sy = 0.0, sz = 0.0; 00275 for (unsigned int i = 0 ; i < mesh->vertexCount ; ++i) 00276 { 00277 unsigned int i3 = i * 3; 00278 vertices[i3] = mesh->vertices[i3]; 00279 vertices[i3 + 1] = mesh->vertices[i3 + 1]; 00280 vertices[i3 + 2] = mesh->vertices[i3 + 2]; 00281 sx += vertices[i3]; 00282 sy += vertices[i3 + 1]; 00283 sz += vertices[i3 + 2]; 00284 } 00285 // the center of the mesh 00286 sx /= (double)mesh->vertexCount; 00287 sy /= (double)mesh->vertexCount; 00288 sz /= (double)mesh->vertexCount; 00289 00290 // scale the mesh 00291 for (unsigned int i = 0 ; i < mesh->vertexCount ; ++i) 00292 { 00293 unsigned int i3 = i * 3; 00294 00295 // vector from center to the vertex 00296 double dx = vertices[i3] - sx; 00297 double dy = vertices[i3 + 1] - sy; 00298 double dz = vertices[i3 + 2] - sz; 00299 00300 // length of vector 00301 //double norm = sqrt(dx * dx + dy * dy + dz * dz); 00302 00303 double ndx = ((dx > 0) ? dx+padding : dx-padding); 00304 double ndy = ((dy > 0) ? dy+padding : dy-padding); 00305 double ndz = ((dz > 0) ? dz+padding : dz-padding); 00306 00307 // the new distance of the vertex from the center 00308 //double fact = scale + padding/norm; 00309 vertices[i3] = sx + ndx; //dx * fact; 00310 vertices[i3 + 1] = sy + ndy; //dy * fact; 00311 vertices[i3 + 2] = sz + ndz; //dz * fact; 00312 } 00313 00314 dTriMeshDataID data = dGeomTriMeshDataCreate(); 00315 dGeomTriMeshDataBuildDouble(data, vertices, sizeof(double) * 3, mesh->vertexCount, indices, icount, sizeof(dTriIndex) * 3); 00316 g = dCreateTriMesh(space, data, NULL, NULL, NULL); 00317 ODEStorage::Element& e = storage.meshes[g]; 00318 e.vertices = vertices; 00319 e.indices = indices; 00320 e.data = data; 00321 e.n_vertices = mesh->vertexCount; 00322 e.n_indices = icount; 00323 } 00324 } 00325 00326 default: 00327 break; 00328 } 00329 return g; 00330 } 00331 00332 void collision_space::EnvironmentModelODE::updateGeom(dGeomID geom, const btTransform &pose) const 00333 { 00334 btVector3 pos = pose.getOrigin(); 00335 dGeomSetPosition(geom, pos.getX(), pos.getY(), pos.getZ()); 00336 btQuaternion quat = pose.getRotation(); 00337 dQuaternion q; 00338 q[0] = quat.getW(); q[1] = quat.getX(); q[2] = quat.getY(); q[3] = quat.getZ(); 00339 dGeomSetQuaternion(geom, q); 00340 } 00341 00342 void collision_space::EnvironmentModelODE::updateAttachedBodies() 00343 { 00344 updateAttachedBodies(default_link_padding_map_); 00345 } 00346 00347 void collision_space::EnvironmentModelODE::updateAttachedBodies(const std::map<std::string, double>& link_padding_map) 00348 { 00349 //getting rid of all entries associated with the current attached bodies 00350 for(std::map<std::string, bool>::iterator it = attached_bodies_in_collision_matrix_.begin(); 00351 it != attached_bodies_in_collision_matrix_.end(); 00352 it++) { 00353 if(!default_collision_matrix_.removeEntry(it->first)) { 00354 ROS_WARN_STREAM("No entry in default collision matrix for attached body " << it->first << 00355 " when there really should be."); 00356 } 00357 } 00358 attached_bodies_in_collision_matrix_.clear(); 00359 for (unsigned int i = 0 ; i < model_geom_.link_geom.size() ; ++i) { 00360 LinkGeom *lg = model_geom_.link_geom[i]; 00361 00362 for(unsigned int j = 0; j < lg->att_bodies.size(); j++) { 00363 for(unsigned int k = 0; k < lg->att_bodies[j]->geom.size(); k++) { 00364 geom_lookup_map_.erase(lg->att_bodies[j]->geom[k]); 00365 } 00366 for(unsigned int k = 0; k < lg->att_bodies[j]->padded_geom.size(); k++) { 00367 geom_lookup_map_.erase(lg->att_bodies[j]->padded_geom[k]); 00368 } 00369 } 00370 lg->deleteAttachedBodies(); 00371 00372 /* create new set of attached bodies */ 00373 const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_bodies = lg->link->getAttachedBodyModels(); 00374 for (unsigned int j = 0 ; j < attached_bodies.size(); ++j) { 00375 double padd = default_robot_padding_; 00376 if(link_padding_map.find(attached_bodies[j]->getName()) != link_padding_map.end()) { 00377 padd = link_padding_map.find(attached_bodies[j]->getName())->second; 00378 } else if (link_padding_map.find("attached") != link_padding_map.end()) { 00379 padd = link_padding_map.find("attached")->second; 00380 } 00381 ROS_DEBUG_STREAM("Updating attached body " << attached_bodies[j]->getName()); 00382 addAttachedBody(lg, attached_bodies[j], padd); 00383 } 00384 } 00385 } 00386 00387 void collision_space::EnvironmentModelODE::addAttachedBody(LinkGeom* lg, 00388 const planning_models::KinematicModel::AttachedBodyModel* attm, 00389 double padd) { 00390 00391 AttGeom* attg = new AttGeom(model_geom_.storage); 00392 attg->att = attm; 00393 00394 if(!default_collision_matrix_.addEntry(attm->getName(), false)) { 00395 ROS_WARN_STREAM("Must already have an entry in allowed collision matrix for " << attm->getName()); 00396 } else { 00397 ROS_DEBUG_STREAM("Adding entry for " << attm->getName()); 00398 } 00399 attached_bodies_in_collision_matrix_[attm->getName()] = true; 00400 default_collision_matrix_.getEntryIndex(attm->getName(), attg->index); 00401 //setting touch links 00402 for(unsigned int i = 0; i < attm->getTouchLinks().size(); i++) { 00403 if(default_collision_matrix_.hasEntry(attm->getTouchLinks()[i])) { 00404 if(!default_collision_matrix_.changeEntry(attm->getName(), attm->getTouchLinks()[i], true)) { 00405 ROS_WARN_STREAM("No entry in allowed collision matrix for " << attm->getName() << " and " << attm->getTouchLinks()[i]); 00406 } else { 00407 ROS_DEBUG_STREAM("Adding touch link for " << attm->getName() << " and " << attm->getTouchLinks()[i]); 00408 } 00409 } 00410 } 00411 for(unsigned int i = 0; i < attm->getShapes().size(); i++) { 00412 dGeomID ga = createODEGeom(model_geom_.self_space, model_geom_.storage, attm->getShapes()[i], 1.0, 0.0); 00413 assert(ga); 00414 attg->geom.push_back(ga); 00415 geom_lookup_map_[ga] = std::pair<std::string, BodyType>(attm->getName(), ATTACHED); 00416 00417 dGeomID padd_ga = createODEGeom(model_geom_.env_space, model_geom_.storage, attm->getShapes()[i], robot_scale_, padd); 00418 assert(padd_ga); 00419 attg->padded_geom.push_back(padd_ga); 00420 geom_lookup_map_[padd_ga] = std::pair<std::string, BodyType>(attm->getName(), ATTACHED); 00421 } 00422 lg->att_bodies.push_back(attg); 00423 } 00424 00425 void collision_space::EnvironmentModelODE::setAttachedBodiesLinkPadding() { 00426 for (unsigned int i = 0 ; i < model_geom_.link_geom.size() ; ++i) { 00427 LinkGeom *lg = model_geom_.link_geom[i]; 00428 00429 const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_bodies = lg->link->getAttachedBodyModels(); 00430 for (unsigned int j = 0 ; j < attached_bodies.size(); ++j) { 00431 double new_padd = -1.0; 00432 if(altered_link_padding_map_.find(attached_bodies[j]->getName()) != altered_link_padding_map_.end()) { 00433 new_padd = altered_link_padding_map_.find(attached_bodies[j]->getName())->second; 00434 } else if (altered_link_padding_map_.find("attached") != altered_link_padding_map_.end()) { 00435 new_padd = altered_link_padding_map_.find("attached")->second; 00436 } 00437 if(new_padd != -1.0) { 00438 for(unsigned int k = 0; k < attached_bodies[j]->getShapes().size(); k++) { 00439 geom_lookup_map_.erase(lg->att_bodies[j]->padded_geom[k]); 00440 dGeomDestroy(lg->att_bodies[j]->padded_geom[k]); 00441 model_geom_.storage.remove(lg->att_bodies[j]->padded_geom[k]); 00442 dGeomID padd_ga = createODEGeom(model_geom_.env_space, model_geom_.storage, attached_bodies[j]->getShapes()[k], robot_scale_, new_padd); 00443 assert(padd_ga); 00444 lg->att_bodies[j]->padded_geom[k] = padd_ga; 00445 geom_lookup_map_[padd_ga] = std::pair<std::string, BodyType>(attached_bodies[j]->getName(), ATTACHED); 00446 } 00447 } 00448 } 00449 } 00450 } 00451 00452 void collision_space::EnvironmentModelODE::revertAttachedBodiesLinkPadding() { 00453 for (unsigned int i = 0 ; i < model_geom_.link_geom.size() ; ++i) { 00454 LinkGeom *lg = model_geom_.link_geom[i]; 00455 00456 const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_bodies = lg->link->getAttachedBodyModels(); 00457 for (unsigned int j = 0 ; j < attached_bodies.size(); ++j) { 00458 double new_padd = -1.0; 00459 if(altered_link_padding_map_.find(attached_bodies[j]->getName()) != altered_link_padding_map_.end()) { 00460 new_padd = default_link_padding_map_.find(attached_bodies[j]->getName())->second; 00461 } else if (altered_link_padding_map_.find("attached") != altered_link_padding_map_.end()) { 00462 new_padd = default_link_padding_map_.find("attached")->second; 00463 } 00464 if(new_padd != -1.0) { 00465 for(unsigned int k = 0; k < attached_bodies[j]->getShapes().size(); k++) { 00466 geom_lookup_map_.erase(lg->att_bodies[j]->padded_geom[k]); 00467 dGeomDestroy(lg->att_bodies[j]->padded_geom[k]); 00468 model_geom_.storage.remove(lg->att_bodies[j]->padded_geom[k]); 00469 dGeomID padd_ga = createODEGeom(model_geom_.env_space, model_geom_.storage, attached_bodies[j]->getShapes()[k], robot_scale_, new_padd); 00470 assert(padd_ga); 00471 lg->att_bodies[j]->padded_geom[k] = padd_ga; 00472 geom_lookup_map_[padd_ga] = std::pair<std::string, BodyType>(attached_bodies[j]->getName(), ATTACHED); 00473 } 00474 } 00475 } 00476 } 00477 } 00478 00479 void collision_space::EnvironmentModelODE::updateRobotModel(const planning_models::KinematicState* state) 00480 { 00481 const unsigned int n = model_geom_.link_geom.size(); 00482 00483 for (unsigned int i = 0 ; i < n ; ++i) { 00484 const planning_models::KinematicState::LinkState* link_state = state->getLinkState(model_geom_.link_geom[i]->link->getName()); 00485 if(link_state == NULL) { 00486 ROS_WARN_STREAM("No link state for link " << model_geom_.link_geom[i]->link->getName()); 00487 continue; 00488 } 00489 updateGeom(model_geom_.link_geom[i]->geom[0], link_state->getGlobalCollisionBodyTransform()); 00490 updateGeom(model_geom_.link_geom[i]->padded_geom[0], link_state->getGlobalCollisionBodyTransform()); 00491 const std::vector<planning_models::KinematicState::AttachedBodyState*>& attached_bodies = link_state->getAttachedBodyStateVector(); 00492 for (unsigned int j = 0 ; j < attached_bodies.size(); ++j) { 00493 for(unsigned int k = 0; k < attached_bodies[j]->getGlobalCollisionBodyTransforms().size(); k++) { 00494 updateGeom(model_geom_.link_geom[i]->att_bodies[j]->geom[k], attached_bodies[j]->getGlobalCollisionBodyTransforms()[k]); 00495 updateGeom(model_geom_.link_geom[i]->att_bodies[j]->padded_geom[k], attached_bodies[j]->getGlobalCollisionBodyTransforms()[k]); 00496 } 00497 } 00498 } 00499 } 00500 00501 void collision_space::EnvironmentModelODE::setAlteredLinkPadding(const std::map<std::string, double>& new_link_padding) { 00502 00503 //updating altered map 00504 collision_space::EnvironmentModel::setAlteredLinkPadding(new_link_padding); 00505 00506 for(unsigned int i = 0; i < model_geom_.link_geom.size(); i++) { 00507 00508 LinkGeom *lg = model_geom_.link_geom[i]; 00509 00510 if(altered_link_padding_map_.find(lg->link->getName()) != altered_link_padding_map_.end()) { 00511 double new_padding = altered_link_padding_map_.find(lg->link->getName())->second; 00512 const planning_models::KinematicModel::LinkModel *link = lg->link; 00513 if (!link || !link->getLinkShape()) { 00514 ROS_WARN_STREAM("Can't get kinematic model for link " << link->getName() << " to make new padding"); 00515 continue; 00516 } 00517 ROS_DEBUG_STREAM("Setting padding for link " << lg->link->getName() << " from " 00518 << default_link_padding_map_[lg->link->getName()] 00519 << " to " << new_padding); 00520 //otherwise we clear out the data associated with the old one 00521 for (unsigned int j = 0 ; j < lg->padded_geom.size() ; ++j) { 00522 geom_lookup_map_.erase(lg->padded_geom[j]); 00523 dGeomDestroy(lg->padded_geom[j]); 00524 model_geom_.storage.remove(lg->padded_geom[j]); 00525 } 00526 lg->padded_geom.clear(); 00527 dGeomID g = createODEGeom(model_geom_.env_space, model_geom_.storage, link->getLinkShape(), robot_scale_, new_padding); 00528 assert(g); 00529 lg->padded_geom.push_back(g); 00530 geom_lookup_map_[g] = std::pair<std::string, BodyType>(link->getName(), LINK); 00531 } 00532 } 00533 //this does all the work 00534 setAttachedBodiesLinkPadding(); 00535 } 00536 00537 void collision_space::EnvironmentModelODE::revertAlteredLinkPadding() { 00538 for(unsigned int i = 0; i < model_geom_.link_geom.size(); i++) { 00539 00540 LinkGeom *lg = model_geom_.link_geom[i]; 00541 00542 if(altered_link_padding_map_.find(lg->link->getName()) != altered_link_padding_map_.end()) { 00543 double old_padding = default_link_padding_map_.find(lg->link->getName())->second; 00544 const planning_models::KinematicModel::LinkModel *link = lg->link; 00545 if (!link || !link->getLinkShape()) { 00546 ROS_WARN_STREAM("Can't get kinematic model for link " << link->getName() << " to revert to old padding"); 00547 continue; 00548 } 00549 //otherwise we clear out the data associated with the old one 00550 for (unsigned int j = 0 ; j < lg->padded_geom.size() ; ++j) { 00551 geom_lookup_map_.erase(lg->padded_geom[j]); 00552 dGeomDestroy(lg->padded_geom[j]); 00553 model_geom_.storage.remove(lg->padded_geom[j]); 00554 } 00555 ROS_DEBUG_STREAM("Reverting padding for link " << lg->link->getName() << " from " << altered_link_padding_map_[lg->link->getName()] 00556 << " to " << old_padding); 00557 lg->padded_geom.clear(); 00558 dGeomID g = createODEGeom(model_geom_.env_space, model_geom_.storage, link->getLinkShape(), robot_scale_, old_padding); 00559 assert(g); 00560 dGeomSetData(g, reinterpret_cast<void*>(lg)); 00561 lg->padded_geom.push_back(g); 00562 geom_lookup_map_[g] = std::pair<std::string, BodyType>(link->getName(), LINK); 00563 } 00564 } 00565 revertAttachedBodiesLinkPadding(); 00566 00567 //clears altered map 00568 collision_space::EnvironmentModel::revertAlteredLinkPadding(); 00569 } 00570 00571 bool collision_space::EnvironmentModelODE::ODECollide2::empty(void) const 00572 { 00573 return geoms_x.empty(); 00574 } 00575 00576 void collision_space::EnvironmentModelODE::ODECollide2::registerSpace(dSpaceID space) 00577 { 00578 int n = dSpaceGetNumGeoms(space); 00579 for (int i = 0 ; i < n ; ++i) 00580 registerGeom(dSpaceGetGeom(space, i)); 00581 } 00582 00583 void collision_space::EnvironmentModelODE::ODECollide2::unregisterGeom(dGeomID geom) 00584 { 00585 setup(); 00586 00587 Geom g; 00588 g.id = geom; 00589 dGeomGetAABB(geom, g.aabb); 00590 00591 Geom *found = NULL; 00592 00593 std::vector<Geom*>::iterator posStart1 = std::lower_bound(geoms_x.begin(), geoms_x.end(), &g, SortByXTest()); 00594 std::vector<Geom*>::iterator posEnd1 = std::upper_bound(posStart1, geoms_x.end(), &g, SortByXTest()); 00595 while (posStart1 < posEnd1) 00596 { 00597 if ((*posStart1)->id == geom) 00598 { 00599 found = *posStart1; 00600 geoms_x.erase(posStart1); 00601 break; 00602 } 00603 ++posStart1; 00604 } 00605 00606 std::vector<Geom*>::iterator posStart2 = std::lower_bound(geoms_y.begin(), geoms_y.end(), &g, SortByYTest()); 00607 std::vector<Geom*>::iterator posEnd2 = std::upper_bound(posStart2, geoms_y.end(), &g, SortByYTest()); 00608 while (posStart2 < posEnd2) 00609 { 00610 if ((*posStart2)->id == geom) 00611 { 00612 assert(found == *posStart2); 00613 geoms_y.erase(posStart2); 00614 break; 00615 } 00616 ++posStart2; 00617 } 00618 00619 std::vector<Geom*>::iterator posStart3 = std::lower_bound(geoms_z.begin(), geoms_z.end(), &g, SortByZTest()); 00620 std::vector<Geom*>::iterator posEnd3 = std::upper_bound(posStart3, geoms_z.end(), &g, SortByZTest()); 00621 while (posStart3 < posEnd3) 00622 { 00623 if ((*posStart3)->id == geom) 00624 { 00625 assert(found == *posStart3); 00626 geoms_z.erase(posStart3); 00627 break; 00628 } 00629 ++posStart3; 00630 } 00631 00632 assert(found); 00633 delete found; 00634 } 00635 00636 void collision_space::EnvironmentModelODE::ODECollide2::registerGeom(dGeomID geom) 00637 { 00638 Geom *g = new Geom(); 00639 g->id = geom; 00640 dGeomGetAABB(geom, g->aabb); 00641 geoms_x.push_back(g); 00642 geoms_y.push_back(g); 00643 geoms_z.push_back(g); 00644 setup_ = false; 00645 } 00646 00647 void collision_space::EnvironmentModelODE::ODECollide2::clear(void) 00648 { 00649 for (unsigned int i = 0 ; i < geoms_x.size() ; ++i) 00650 delete geoms_x[i]; 00651 geoms_x.clear(); 00652 geoms_y.clear(); 00653 geoms_z.clear(); 00654 setup_ = false; 00655 } 00656 00657 void collision_space::EnvironmentModelODE::ODECollide2::setup(void) 00658 { 00659 if (!setup_) 00660 { 00661 std::sort(geoms_x.begin(), geoms_x.end(), SortByXLow()); 00662 std::sort(geoms_y.begin(), geoms_y.end(), SortByYLow()); 00663 std::sort(geoms_z.begin(), geoms_z.end(), SortByZLow()); 00664 setup_ = true; 00665 } 00666 } 00667 00668 void collision_space::EnvironmentModelODE::ODECollide2::getGeoms(std::vector<dGeomID> &geoms) const 00669 { 00670 geoms.resize(geoms_x.size()); 00671 for (unsigned int i = 0 ; i < geoms.size() ; ++i) 00672 geoms[i] = geoms_x[i]->id; 00673 } 00674 00675 void collision_space::EnvironmentModelODE::ODECollide2::checkColl(std::vector<Geom*>::const_iterator posStart, std::vector<Geom*>::const_iterator posEnd, 00676 Geom *g, void *data, dNearCallback *nearCallback) const 00677 { 00678 /* posStart now identifies the first geom which has an AABB 00679 that could overlap the AABB of geom on the X axis. posEnd 00680 identifies the first one that cannot overlap. */ 00681 00682 while (posStart < posEnd) 00683 { 00684 /* if the boxes are not disjoint along Y, Z, check further */ 00685 if (!((*posStart)->aabb[2] > g->aabb[3] || 00686 (*posStart)->aabb[3] < g->aabb[2] || 00687 (*posStart)->aabb[4] > g->aabb[5] || 00688 (*posStart)->aabb[5] < g->aabb[4])) 00689 dSpaceCollide2(g->id, (*posStart)->id, data, nearCallback); 00690 posStart++; 00691 } 00692 } 00693 00694 void collision_space::EnvironmentModelODE::ODECollide2::collide(dGeomID geom, void *data, dNearCallback *nearCallback) const 00695 { 00696 static const int CUTOFF = 100; 00697 00698 assert(setup_); 00699 00700 Geom g; 00701 g.id = geom; 00702 dGeomGetAABB(geom, g.aabb); 00703 00704 std::vector<Geom*>::const_iterator posStart1 = std::lower_bound(geoms_x.begin(), geoms_x.end(), &g, SortByXTest()); 00705 if (posStart1 != geoms_x.end()) 00706 { 00707 std::vector<Geom*>::const_iterator posEnd1 = std::upper_bound(posStart1, geoms_x.end(), &g, SortByXTest()); 00708 int d1 = posEnd1 - posStart1; 00709 00710 /* Doing two binary searches on the sorted-by-y array takes 00711 log(n) time, which should be around 12 steps. Each step 00712 should be just a few ops, so a cut-off like 100 is 00713 appropriate. */ 00714 if (d1 > CUTOFF) 00715 { 00716 std::vector<Geom*>::const_iterator posStart2 = std::lower_bound(geoms_y.begin(), geoms_y.end(), &g, SortByYTest()); 00717 if (posStart2 != geoms_y.end()) 00718 { 00719 std::vector<Geom*>::const_iterator posEnd2 = std::upper_bound(posStart2, geoms_y.end(), &g, SortByYTest()); 00720 int d2 = posEnd2 - posStart2; 00721 00722 if (d2 > CUTOFF) 00723 { 00724 std::vector<Geom*>::const_iterator posStart3 = std::lower_bound(geoms_z.begin(), geoms_z.end(), &g, SortByZTest()); 00725 if (posStart3 != geoms_z.end()) 00726 { 00727 std::vector<Geom*>::const_iterator posEnd3 = std::upper_bound(posStart3, geoms_z.end(), &g, SortByZTest()); 00728 int d3 = posEnd3 - posStart3; 00729 if (d3 > CUTOFF) 00730 { 00731 if (d3 <= d2 && d3 <= d1) 00732 checkColl(posStart3, posEnd3, &g, data, nearCallback); 00733 else 00734 if (d2 <= d3 && d2 <= d1) 00735 checkColl(posStart2, posEnd2, &g, data, nearCallback); 00736 else 00737 checkColl(posStart1, posEnd1, &g, data, nearCallback); 00738 } 00739 else 00740 checkColl(posStart3, posEnd3, &g, data, nearCallback); 00741 } 00742 } 00743 else 00744 checkColl(posStart2, posEnd2, &g, data, nearCallback); 00745 } 00746 } 00747 else 00748 checkColl(posStart1, posEnd1, &g, data, nearCallback); 00749 } 00750 } 00751 00752 namespace collision_space 00753 { 00754 00755 void nearCallbackFn(void *data, dGeomID o1, dGeomID o2) 00756 { 00757 EnvironmentModelODE::CollisionData *cdata = reinterpret_cast<EnvironmentModelODE::CollisionData*>(data); 00758 00759 if (cdata->done) { 00760 return; 00761 } 00762 00763 //first figure out what check is happening 00764 bool check_in_allowed_collision_matrix = true; 00765 00766 std::string object_name; 00767 00768 std::map<dGeomID, std::pair<std::string, EnvironmentModelODE::BodyType> >::const_iterator it1 = cdata->geom_lookup_map->find(o1); 00769 std::map<dGeomID, std::pair<std::string, EnvironmentModelODE::BodyType> >::const_iterator it2 = cdata->geom_lookup_map->find(o2); 00770 00771 if(it1 != cdata->geom_lookup_map->end()) { 00772 cdata->body_name_1 = it1->second.first; 00773 cdata->body_type_1 = it1->second.second; 00774 } else { 00775 for(std::map<std::string, dSpaceID>::const_iterator it = cdata->dspace_lookup_map->begin(); 00776 it != cdata->dspace_lookup_map->end(); 00777 it++) { 00778 if(dSpaceQuery(it->second, o1)) { 00779 object_name = it->first; 00780 break; 00781 } 00782 } 00783 if(object_name == "") { 00784 ROS_WARN_STREAM("Object does not have entry in dspace map"); 00785 } 00786 cdata->body_name_1 = object_name; 00787 cdata->body_type_1 = EnvironmentModelODE::OBJECT; 00788 check_in_allowed_collision_matrix = false; 00789 } 00790 00791 if(it2 != cdata->geom_lookup_map->end()) { 00792 cdata->body_name_2 = it2->second.first; 00793 cdata->body_type_2 = it2->second.second; 00794 } else { 00795 for(std::map<std::string, dSpaceID>::const_iterator it = cdata->dspace_lookup_map->begin(); 00796 it != cdata->dspace_lookup_map->end(); 00797 it++) { 00798 if(dSpaceQuery(it->second, o2)) { 00799 object_name = it->first; 00800 break; 00801 } 00802 } 00803 if(object_name == "") { 00804 ROS_WARN_STREAM("Object does not have entry in dspace map"); 00805 } 00806 cdata->body_name_2 = object_name; 00807 cdata->body_type_2 = EnvironmentModelODE::OBJECT; 00808 check_in_allowed_collision_matrix = false; 00809 } 00810 00811 //determine whether or not this collision is allowed in the self_collision matrix 00812 if (cdata->allowed_collision_matrix && check_in_allowed_collision_matrix) { 00813 bool allowed; 00814 if(!cdata->allowed_collision_matrix->getAllowedCollision(cdata->body_name_1, cdata->body_name_2, allowed)) { 00815 ROS_WARN_STREAM("No entry in allowed collision matrix for " << cdata->body_name_1 << " and " << cdata->body_name_2); 00816 return; 00817 } 00818 if(allowed) { 00819 ROS_DEBUG_STREAM("Will not test for collision between " << cdata->body_name_1 << " and " << cdata->body_name_2); 00820 return; 00821 } else { 00822 ROS_DEBUG_STREAM("Will test for collision between " << cdata->body_name_1 << " and " << cdata->body_name_2); 00823 } 00824 } 00825 00826 //do the actual collision check to get the desired number of contacts 00827 int num_contacts = 1; 00828 if(cdata->contacts) { 00829 num_contacts = std::min(MAX_ODE_CONTACTS, (int) cdata->max_contacts_pair); 00830 } 00831 if(cdata->allowed) { 00832 num_contacts = std::max(num_contacts, TEST_FOR_ALLOWED_NUM); 00833 } 00834 num_contacts = std::max(num_contacts, 1); 00835 00836 ROS_DEBUG_STREAM("Testing " << cdata->body_name_1 00837 << " and " << cdata->body_name_2 << " contact size " << num_contacts); 00838 00839 dContactGeom contactGeoms[num_contacts]; 00840 int numc = dCollide(o1, o2, num_contacts, 00841 &(contactGeoms[0]), sizeof(dContactGeom)); 00842 00843 //no collisions, return 00844 if(!numc) 00845 return; 00846 00847 if(!cdata->contacts && !cdata->allowed) { 00848 //we don't care about contact information, so just set to true if there's been collision 00849 ROS_DEBUG_STREAM_NAMED(CONTACT_ONLY_NAME, "Detected collision between " << cdata->body_name_1 << " and " << cdata->body_name_2); 00850 cdata->collides = true; 00851 cdata->done = true; 00852 } else { 00853 unsigned int num_not_allowed = 0; 00854 if(numc != num_contacts) { 00855 ROS_INFO_STREAM("Asked for " << num_contacts << " but only got " << numc); 00856 } 00857 for (int i = 0 ; i < numc ; ++i) { 00858 00859 ROS_DEBUG_STREAM("Contact at " << contactGeoms[i].pos[0] << " " 00860 << contactGeoms[i].pos[1] << " " << contactGeoms[i].pos[2]); 00861 const dReal *pos1 = dGeomGetPosition(o1); 00862 dQuaternion quat1, quat2; 00863 dGeomGetQuaternion(o1, quat1); 00864 const dReal *pos2 = dGeomGetPosition(o2); 00865 dGeomGetQuaternion(o2, quat2); 00866 00867 ROS_DEBUG_STREAM("Body pos 1 " << pos1[0] << " " << pos1[1] << " " << pos1[2]); 00868 ROS_DEBUG_STREAM("Body quat 1 " << quat1[1] << " " << quat1[2] << " " << quat1[3] << " " << quat1[0]); 00869 ROS_DEBUG_STREAM("Body pos 2 " << pos2[0] << " " << pos2[1] << " " << pos2[2]); 00870 ROS_DEBUG_STREAM("Body quat 2 " << quat2[1] << " " << quat2[2] << " " << quat2[3] << " " << quat2[0]); 00871 00872 btVector3 pos(contactGeoms[i].pos[0], contactGeoms[i].pos[1], contactGeoms[i].pos[2]); 00873 00874 //figure out whether the contact is allowed 00875 //allowed contacts only allowed with objects for now 00876 bool allowed = false; 00877 if(cdata->allowed) { 00878 EnvironmentModel::AllowedContactMap::const_iterator it1 = cdata->allowed->find(cdata->body_name_1); 00879 if(it1 != cdata->allowed->end()) { 00880 std::map<std::string, std::vector<EnvironmentModel::AllowedContact> >::const_iterator it2 = it1->second.find(cdata->body_name_2); 00881 if(it2 != it1->second.end()) { 00882 ROS_DEBUG_STREAM("Testing allowed contact for " << cdata->body_name_1 << " and " << cdata->body_name_2 << " num " << i); 00883 ROS_DEBUG_STREAM("Contact at " << contactGeoms[i].pos[0] << " " 00884 << contactGeoms[i].pos[1] << " " << contactGeoms[i].pos[2]); 00885 00886 const std::vector<EnvironmentModel::AllowedContact>& av = it2->second; 00887 for(unsigned int j = 0; j < av.size(); j++) { 00888 if(av[j].bound->containsPoint(pos)) { 00889 if(av[j].depth >= fabs(contactGeoms[i].depth)) { 00890 allowed = true; 00891 ROS_DEBUG_STREAM("Contact allowed by allowed collision region"); 00892 break; 00893 } else { 00894 ROS_DEBUG_STREAM("Depth check failing " << av[j].depth << " detected " << contactGeoms[i].depth); 00895 } 00896 } 00897 } 00898 } 00899 } 00900 } 00901 if(!allowed) { 00902 00903 cdata->collides = true; 00904 num_not_allowed++; 00905 00906 ROS_DEBUG_STREAM_NAMED(CONTACT_ONLY_NAME, "Detected collision between " << cdata->body_name_1 << " and " << cdata->body_name_2); 00907 00908 if(cdata->contacts != NULL) { 00909 if(num_not_allowed <= cdata->max_contacts_pair) { 00910 collision_space::EnvironmentModelODE::Contact add; 00911 00912 add.pos = pos; 00913 00914 add.normal.setX(contactGeoms[i].normal[0]); 00915 add.normal.setY(contactGeoms[i].normal[1]); 00916 add.normal.setZ(contactGeoms[i].normal[2]); 00917 00918 add.depth = contactGeoms[i].depth; 00919 00920 add.body_name_1 = cdata->body_name_1; 00921 add.body_name_2 = cdata->body_name_2; 00922 add.body_type_1 = cdata->body_type_1; 00923 add.body_type_2 = cdata->body_type_2; 00924 00925 cdata->contacts->push_back(add); 00926 if(cdata->contacts->size() >= cdata->max_contacts_total) { 00927 cdata->done = true; 00928 } 00929 } 00930 } else { 00931 cdata->done = true; 00932 } 00933 } 00934 } 00935 } 00936 } 00937 } 00938 00939 bool collision_space::EnvironmentModelODE::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total, unsigned int max_per_pair) const 00940 { 00941 contacts.clear(); 00942 CollisionData cdata; 00943 cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix(); 00944 cdata.geom_lookup_map = &geom_lookup_map_; 00945 cdata.dspace_lookup_map = &dspace_lookup_map_; 00946 cdata.contacts = &contacts; 00947 cdata.max_contacts_total = max_total; 00948 cdata.max_contacts_pair = max_per_pair; 00949 if (!allowed_contacts_.empty()) 00950 cdata.allowed = &allowed_contact_map_; 00951 contacts.clear(); 00952 checkThreadInit(); 00953 testCollision(&cdata); 00954 return cdata.collides; 00955 } 00956 00957 bool collision_space::EnvironmentModelODE::getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_contacts_per_pair) const 00958 { 00959 contacts.clear(); 00960 CollisionData cdata; 00961 cdata.geom_lookup_map = &geom_lookup_map_; 00962 cdata.dspace_lookup_map = &dspace_lookup_map_; 00963 cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix(); 00964 cdata.contacts = &contacts; 00965 cdata.max_contacts_total = UINT_MAX; 00966 cdata.max_contacts_pair = num_contacts_per_pair; 00967 if (!allowed_contacts_.empty()) 00968 cdata.allowed = &allowed_contact_map_; 00969 contacts.clear(); 00970 checkThreadInit(); 00971 testCollision(&cdata); 00972 return cdata.collides; 00973 } 00974 00975 bool collision_space::EnvironmentModelODE::isCollision(void) const 00976 { 00977 CollisionData cdata; 00978 cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix(); 00979 cdata.geom_lookup_map = &geom_lookup_map_; 00980 cdata.dspace_lookup_map = &dspace_lookup_map_; 00981 if (!allowed_contacts_.empty()) { 00982 cdata.allowed = &allowed_contact_map_; 00983 ROS_DEBUG_STREAM("Got contacts size " << cdata.allowed->size()); 00984 } else { 00985 ROS_DEBUG_STREAM("No allowed contacts"); 00986 } 00987 checkThreadInit(); 00988 testCollision(&cdata); 00989 return cdata.collides; 00990 } 00991 00992 bool collision_space::EnvironmentModelODE::isSelfCollision(void) const 00993 { 00994 CollisionData cdata; 00995 cdata.geom_lookup_map = &geom_lookup_map_; 00996 cdata.dspace_lookup_map = &dspace_lookup_map_; 00997 cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix(); 00998 if (!allowed_contacts_.empty()) 00999 cdata.allowed = &allowed_contact_map_; 01000 checkThreadInit(); 01001 testSelfCollision(&cdata); 01002 return cdata.collides; 01003 } 01004 01005 bool collision_space::EnvironmentModelODE::isEnvironmentCollision(void) const 01006 { 01007 CollisionData cdata; 01008 cdata.geom_lookup_map = &geom_lookup_map_; 01009 cdata.dspace_lookup_map = &dspace_lookup_map_; 01010 cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix(); 01011 if (!allowed_contacts_.empty()) 01012 cdata.allowed = &allowed_contact_map_; 01013 checkThreadInit(); 01014 testEnvironmentCollision(&cdata); 01015 return cdata.collides; 01016 } 01017 01018 void collision_space::EnvironmentModelODE::testObjectCollision(CollisionNamespace *cn, CollisionData *cdata) const 01019 { 01020 if (cn->collide2.empty()) { 01021 ROS_WARN_STREAM("Problem - collide2 required for body collision for " << cn->name); 01022 return; 01023 } 01024 01025 cn->collide2.setup(); 01026 for (int i = model_geom_.link_geom.size() - 1 ; i >= 0 && !cdata->done; --i) { 01027 LinkGeom *lg = model_geom_.link_geom[i]; 01028 01029 bool allowed = false; 01030 if(cdata->allowed_collision_matrix) { 01031 if(!cdata->allowed_collision_matrix->getAllowedCollision(cn->name, lg->link->getName(), allowed)) { 01032 ROS_WARN_STREAM("No entry in cdata allowed collision matrix for " << cn->name << " and " << lg->link->getName()); 01033 return; 01034 } 01035 } 01036 01037 //have to test collisions with link 01038 if(!allowed) { 01039 ROS_DEBUG_STREAM("Will test for collision between object " << cn->name << " and link " << lg->link->getName()); 01040 for(unsigned int j = 0; j < lg->padded_geom.size(); j++) { 01041 //have to figure 01042 unsigned int current_contacts_size = 0; 01043 if(cdata->contacts) { 01044 current_contacts_size = cdata->contacts->size(); 01045 } 01046 cn->collide2.collide(lg->padded_geom[j], cdata, nearCallbackFn); 01047 if(cdata->contacts && cdata->contacts->size() > current_contacts_size) { 01048 //new contacts must mean collision 01049 for(unsigned int k = current_contacts_size; k < cdata->contacts->size(); k++) { 01050 if(cdata->contacts->at(k).body_type_1 == OBJECT) { 01051 cdata->contacts->at(k).body_name_1 = cn->name; 01052 } else if(cdata->contacts->at(k).body_type_2 == OBJECT) { 01053 cdata->contacts->at(k).body_name_2 = cn->name; 01054 } else { 01055 ROS_WARN_STREAM("New contacts really should have an object as one of the bodys"); 01056 } 01057 } 01058 } 01059 if(cdata->done) { 01060 return; 01061 } 01062 } 01063 } else { 01064 ROS_DEBUG_STREAM("Will not test for allowed collision between object " << cn->name << " and link " << lg->link->getName()); 01065 } 01066 //now we need to do the attached bodies 01067 for(unsigned int j = 0; j < lg->att_bodies.size(); j++) { 01068 std::string att_name = lg->att_bodies[j]->att->getName(); 01069 allowed = false; 01070 if(cdata->allowed_collision_matrix) { 01071 if(!cdata->allowed_collision_matrix->getAllowedCollision(cn->name, att_name, allowed)) { 01072 ROS_WARN_STREAM("No entry in current allowed collision matrix for " << cn->name << " and " << att_name); 01073 return; 01074 } 01075 } 01076 if(!allowed) { 01077 ROS_DEBUG_STREAM("Will test for collision between object " << cn->name << " and attached object " << att_name); 01078 for(unsigned int k = 0; k < lg->att_bodies[j]->padded_geom.size(); k++) { 01079 //have to figure 01080 unsigned int current_contacts_size = 0; 01081 if(cdata->contacts) { 01082 current_contacts_size = cdata->contacts->size(); 01083 } 01084 cn->collide2.collide(lg->att_bodies[j]->padded_geom[k], cdata, nearCallbackFn); 01085 if(cdata->contacts && cdata->contacts->size() > current_contacts_size) { 01086 //new contacts must mean collision 01087 for(unsigned int l = current_contacts_size; l < cdata->contacts->size(); l++) { 01088 if(cdata->contacts->at(l).body_type_1 == OBJECT) { 01089 cdata->contacts->at(l).body_name_1 = cn->name; 01090 } else if(cdata->contacts->at(l).body_type_2 == OBJECT) { 01091 cdata->contacts->at(l).body_name_2 = cn->name; 01092 } else { 01093 ROS_WARN_STREAM("New contacts really should have an object as one of the bodys"); 01094 } 01095 } 01096 } 01097 if(cdata->done) { 01098 return; 01099 } 01100 } 01101 } else { 01102 ROS_DEBUG_STREAM("Will not test for allowed collision between object " << cn->name << " and attached object " << att_name); 01103 } 01104 } 01105 } 01106 } 01107 01108 void collision_space::EnvironmentModelODE::testCollision(CollisionData *cdata) const 01109 { 01110 testSelfCollision(cdata); 01111 testEnvironmentCollision(cdata); 01112 } 01113 01114 void collision_space::EnvironmentModelODE::testSelfCollision(CollisionData *cdata) const 01115 { 01116 dSpaceCollide(model_geom_.self_space, cdata, nearCallbackFn); 01117 } 01118 01119 void collision_space::EnvironmentModelODE::testEnvironmentCollision(CollisionData *cdata) const 01120 { 01121 /* check collision with other ode bodies until done*/ 01122 for (std::map<std::string, CollisionNamespace*>::const_iterator it = coll_namespaces_.begin() ; it != coll_namespaces_.end() && !cdata->done ; ++it) { 01123 testObjectCollision(it->second, cdata); 01124 } 01125 } 01126 01127 bool collision_space::EnvironmentModelODE::hasObject(const std::string& ns) const 01128 { 01129 if(coll_namespaces_.find(ns) != coll_namespaces_.end()) { 01130 return true; 01131 } 01132 return false; 01133 } 01134 01135 void collision_space::EnvironmentModelODE::addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses) 01136 { 01137 assert(shapes.size() == poses.size()); 01138 std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.find(ns); 01139 CollisionNamespace* cn = NULL; 01140 if (it == coll_namespaces_.end()) 01141 { 01142 cn = new CollisionNamespace(ns); 01143 dspace_lookup_map_[ns] = cn->space; 01144 coll_namespaces_[ns] = cn; 01145 default_collision_matrix_.addEntry(ns, false); 01146 } 01147 else { 01148 cn = it->second; 01149 } 01150 01151 //we're going to create the namespace in objects_ even if it doesn't have anything in it 01152 objects_->addObjectNamespace(ns); 01153 01154 unsigned int n = shapes.size(); 01155 for (unsigned int i = 0 ; i < n ; ++i) 01156 { 01157 dGeomID g = createODEGeom(cn->space, cn->storage, shapes[i], 1.0, 0.0); 01158 assert(g); 01159 dGeomSetData(g, reinterpret_cast<void*>(shapes[i])); 01160 updateGeom(g, poses[i]); 01161 cn->collide2.registerGeom(g); 01162 objects_->addObject(ns, shapes[i], poses[i]); 01163 } 01164 cn->collide2.setup(); 01165 } 01166 01167 void collision_space::EnvironmentModelODE::addObject(const std::string &ns, shapes::Shape *shape, const btTransform &pose) 01168 { 01169 std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.find(ns); 01170 CollisionNamespace* cn = NULL; 01171 if (it == coll_namespaces_.end()) 01172 { 01173 cn = new CollisionNamespace(ns); 01174 dspace_lookup_map_[ns] = cn->space; 01175 coll_namespaces_[ns] = cn; 01176 default_collision_matrix_.addEntry(ns, false); 01177 } 01178 else 01179 cn = it->second; 01180 01181 dGeomID g = createODEGeom(cn->space, cn->storage, shape, 1.0, 0.0); 01182 assert(g); 01183 dGeomSetData(g, reinterpret_cast<void*>(shape)); 01184 01185 updateGeom(g, pose); 01186 cn->geoms.push_back(g); 01187 objects_->addObject(ns, shape, pose); 01188 } 01189 01190 void collision_space::EnvironmentModelODE::addObject(const std::string &ns, shapes::StaticShape* shape) 01191 { 01192 std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.find(ns); 01193 CollisionNamespace* cn = NULL; 01194 if (it == coll_namespaces_.end()) 01195 { 01196 cn = new CollisionNamespace(ns); 01197 dspace_lookup_map_[ns] = cn->space; 01198 coll_namespaces_[ns] = cn; 01199 default_collision_matrix_.addEntry(ns, false); 01200 } 01201 else 01202 cn = it->second; 01203 01204 dGeomID g = createODEGeom(cn->space, cn->storage, shape); 01205 assert(g); 01206 dGeomSetData(g, reinterpret_cast<void*>(shape)); 01207 cn->geoms.push_back(g); 01208 objects_->addObject(ns, shape); 01209 } 01210 01211 void collision_space::EnvironmentModelODE::clearObjects(void) 01212 { 01213 for (std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.begin() ; it != coll_namespaces_.end() ; ++it) { 01214 default_collision_matrix_.removeEntry(it->first); 01215 delete it->second; 01216 } 01217 dspace_lookup_map_.clear(); 01218 coll_namespaces_.clear(); 01219 objects_->clearObjects(); 01220 } 01221 01222 void collision_space::EnvironmentModelODE::clearObjects(const std::string &ns) 01223 { 01224 std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.find(ns); 01225 if (it != coll_namespaces_.end()) { 01226 default_collision_matrix_.removeEntry(ns); 01227 delete it->second; 01228 coll_namespaces_.erase(ns); 01229 dspace_lookup_map_.erase(ns); 01230 } 01231 objects_->clearObjects(ns); 01232 } 01233 01234 dGeomID collision_space::EnvironmentModelODE::copyGeom(dSpaceID space, ODEStorage &storage, dGeomID geom, ODEStorage &sourceStorage) const 01235 { 01236 int c = dGeomGetClass(geom); 01237 dGeomID ng = NULL; 01238 bool location = true; 01239 switch (c) 01240 { 01241 case dSphereClass: 01242 ng = dCreateSphere(space, dGeomSphereGetRadius(geom)); 01243 break; 01244 case dBoxClass: 01245 { 01246 dVector3 r; 01247 dGeomBoxGetLengths(geom, r); 01248 ng = dCreateBox(space, r[0], r[1], r[2]); 01249 } 01250 break; 01251 case dCylinderClass: 01252 { 01253 dReal r, l; 01254 dGeomCylinderGetParams(geom, &r, &l); 01255 ng = dCreateCylinder(space, r, l); 01256 } 01257 break; 01258 case dPlaneClass: 01259 { 01260 dVector4 p; 01261 dGeomPlaneGetParams(geom, p); 01262 ng = dCreatePlane(space, p[0], p[1], p[2], p[3]); 01263 location = false; 01264 } 01265 break; 01266 case dTriMeshClass: 01267 { 01268 dTriMeshDataID tdata = dGeomTriMeshGetData(geom); 01269 dTriMeshDataID cdata = dGeomTriMeshDataCreate(); 01270 for(std::map<dGeomID, ODEStorage::Element>::const_iterator it = sourceStorage.meshes.begin(); 01271 it != sourceStorage.meshes.end(); 01272 it++) { 01273 if (it->second.data == tdata) 01274 { 01275 ODEStorage::Element& e = storage.meshes[geom]; 01276 e.n_vertices = it->second.n_vertices; 01277 e.n_indices = it->second.n_indices; 01278 e.indices = new dTriIndex[e.n_indices]; 01279 for (int j = 0 ; j < e.n_indices ; ++j) 01280 e.indices[j] = it->second.indices[j]; 01281 e.vertices = new double[e.n_vertices]; 01282 for (int j = 0 ; j < e.n_vertices ; ++j) 01283 e.vertices[j] = e.vertices[j]; 01284 dGeomTriMeshDataBuildDouble(cdata, e.vertices, sizeof(double) * 3, e.n_vertices, e.indices, e.n_indices, sizeof(dTriIndex) * 3); 01285 e.data = cdata; 01286 break; 01287 } 01288 ng = dCreateTriMesh(space, cdata, NULL, NULL, NULL); 01289 } 01290 } 01291 break; 01292 default: 01293 assert(0); // this should never happen 01294 break; 01295 } 01296 01297 if (ng && location) 01298 { 01299 const dReal *pos = dGeomGetPosition(geom); 01300 dGeomSetPosition(ng, pos[0], pos[1], pos[2]); 01301 dQuaternion q; 01302 dGeomGetQuaternion(geom, q); 01303 dGeomSetQuaternion(ng, q); 01304 } 01305 01306 return ng; 01307 } 01308 01309 collision_space::EnvironmentModel* collision_space::EnvironmentModelODE::clone(void) const 01310 { 01311 EnvironmentModelODE *env = new EnvironmentModelODE(); 01312 env->default_collision_matrix_ = default_collision_matrix_; 01313 env->default_link_padding_map_ = default_link_padding_map_; 01314 env->verbose_ = verbose_; 01315 env->robot_scale_ = robot_scale_; 01316 env->default_robot_padding_ = default_robot_padding_; 01317 env->robot_model_ = new planning_models::KinematicModel(*robot_model_); 01318 env->createODERobotModel(); 01319 01320 for (std::map<std::string, CollisionNamespace*>::const_iterator it = coll_namespaces_.begin() ; it != coll_namespaces_.end() ; ++it) { 01321 // construct a map of the shape pointers we have; this points to the index positions where they are stored; 01322 std::map<void*, int> shapePtrs; 01323 const EnvironmentObjects::NamespaceObjects &ns = objects_->getObjects(it->first); 01324 unsigned int n = ns.static_shape.size(); 01325 for (unsigned int i = 0 ; i < n ; ++i) 01326 shapePtrs[ns.static_shape[i]] = -1 - i; 01327 n = ns.shape.size(); 01328 for (unsigned int i = 0 ; i < n ; ++i) 01329 shapePtrs[ns.shape[i]] = i; 01330 01331 // copy the collision namespace structure, geom by geom 01332 CollisionNamespace *cn = new CollisionNamespace(it->first); 01333 env->coll_namespaces_[it->first] = cn; 01334 env->dspace_lookup_map_[cn->name] = cn->space; 01335 n = it->second->geoms.size(); 01336 cn->geoms.reserve(n); 01337 for (unsigned int i = 0 ; i < n ; ++i) 01338 { 01339 dGeomID newGeom = copyGeom(cn->space, cn->storage, it->second->geoms[i], it->second->storage); 01340 int idx = shapePtrs[dGeomGetData(it->second->geoms[i])]; 01341 if (idx < 0) // static geom 01342 { 01343 shapes::StaticShape *newShape = shapes::cloneShape(ns.static_shape[-idx - 1]); 01344 dGeomSetData(newGeom, reinterpret_cast<void*>(newShape)); 01345 env->objects_->addObject(it->first, newShape); 01346 } 01347 else // movable geom 01348 { 01349 shapes::Shape *newShape = shapes::cloneShape(ns.shape[idx]); 01350 dGeomSetData(newGeom, reinterpret_cast<void*>(newShape)); 01351 env->objects_->addObject(it->first, newShape, ns.shape_pose[idx]); 01352 } 01353 cn->geoms.push_back(newGeom); 01354 } 01355 std::vector<dGeomID> geoms; 01356 it->second->collide2.getGeoms(geoms); 01357 n = geoms.size(); 01358 for (unsigned int i = 0 ; i < n ; ++i) 01359 { 01360 dGeomID newGeom = copyGeom(cn->space, cn->storage, geoms[i], it->second->storage); 01361 int idx = shapePtrs[dGeomGetData(geoms[i])]; 01362 if (idx < 0) // static geom 01363 { 01364 shapes::StaticShape *newShape = shapes::cloneShape(ns.static_shape[-idx - 1]); 01365 dGeomSetData(newGeom, reinterpret_cast<void*>(newShape)); 01366 env->objects_->addObject(it->first, newShape); 01367 } 01368 else // movable geom 01369 { 01370 shapes::Shape *newShape = shapes::cloneShape(ns.shape[idx]); 01371 dGeomSetData(newGeom, reinterpret_cast<void*>(newShape)); 01372 env->objects_->addObject(it->first, newShape, ns.shape_pose[idx]); 01373 } 01374 cn->collide2.registerGeom(newGeom); 01375 } 01376 } 01377 01378 return env; 01379 }