environmentBullet.cpp
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 #include "collision_space/environmentBullet.h"
00038 
00039 void collision_space::EnvironmentModelBullet::freeMemory(void)
00040 {
00041   for (unsigned int j = 0 ; j < m_modelGeom.linkGeom.size() ; ++j)
00042     delete m_modelGeom.linkGeom[j];
00043   /*
00044     if (m_world)
00045     {
00046     for(int i = m_world->getNumCollisionObjects() - 1; i >= 0; --i)
00047     {
00048     btCollisionObject* obj = m_world->getCollisionObjectArray()[i];
00049     delete obj->getCollisionShape();
00050     delete obj;
00051     }
00052         
00053     delete m_world->getBroadphase();
00054     delete m_world->getDispatcher();
00055     delete m_world;
00056     }   
00057     
00058     if (m_config)
00059     delete m_config; */
00060 }
00061 
00062 void collision_space::EnvironmentModelBullet::setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, 
00063                                                             const std::vector<std::string> &links,
00064                                                             const std::map<std::string, double>& link_padding_map,
00065                                                             double default_padding,
00066                                                             double scale)
00067 {
00068   collision_space::EnvironmentModel::setRobotModel(model, links, link_padding_map, default_padding, scale);
00069   m_modelGeom.scale = scale;
00070   m_modelGeom.padding = default_padding;
00071 
00072   for (unsigned int i = 0 ; i < links.size() ; ++i)
00073   {
00074     /* skip this link if we have no geometry or if the link
00075        name is not specified as enabled for collision
00076        checking */
00077     const planning_models::KinematicModel::LinkModel *link = m_robotModel->getLinkModel(links[i]);
00078     if (!link || !link->getLinkShape())
00079             continue;
00080         
00081     kGeom *kg = new kGeom();
00082     kg->link = link;
00083     kg->enabled = true;
00084     kg->index = i;
00085     btCollisionObject *obj = createCollisionBody(link->getLinkShape(), scale, default_padding);
00086     assert(obj);
00087     m_world->addCollisionObject(obj);
00088     obj->setUserPointer(reinterpret_cast<void*>(kg));
00089     kg->geom.push_back(obj);
00090     for (unsigned int k = 0 ; k < kg->link->getAttachedBodyModels().size() ; ++k)
00091     {
00092             btCollisionObject *obja = createCollisionBody(kg->link->attached_bodies[k]->shapes[0], scale, default_padding);
00093             assert(obja);       
00094             m_world->addCollisionObject(obja);
00095             obja->setUserPointer(reinterpret_cast<void*>(kg));
00096             kg->geom.push_back(obja);
00097     }
00098     m_modelGeom.linkGeom.push_back(kg);
00099   }
00100 }
00101 
00102 const std::vector<const planning_models::KinematicModel::AttachedBody*> collision_space::EnvironmentModelBullet::getAttachedBodies(void) const 
00103 {
00104   std::vector<const planning_models::KinematicModel::AttachedBody*> ret_vec;
00105 
00106   const unsigned int n = m_modelGeom.linkGeom.size();    
00107   for (unsigned int i = 0 ; i < n ; ++i)
00108   {
00109     kGeom *kg = m_modelGeom.linkGeom[i];
00110         
00111     /* create new set of attached bodies */     
00112     const unsigned int nab = kg->link->attached_bodies.size();
00113     for (unsigned int k = 0 ; k < nab ; ++k)
00114     {
00115       ret_vec.push_back(kg->link->attached_bodies[k]);
00116     }
00117   }
00118   return ret_vec;
00119 }
00120 
00121 btCollisionObject* collision_space::EnvironmentModelBullet::createCollisionBody(const shapes::Shape *shape, double scale, double padding)
00122 {
00123   btCollisionShape *btshape = NULL;
00124     
00125   switch (shape->type)
00126   {
00127   case shapes::SPHERE:
00128     {
00129             btshape = dynamic_cast<btCollisionShape*>(new btSphereShape(static_cast<const shapes::Sphere*>(shape)->radius * scale + padding));
00130     }
00131     break;
00132   case shapes::BOX:
00133     {
00134             const double *size = static_cast<const shapes::Box*>(shape)->size;
00135             btshape = dynamic_cast<btCollisionShape*>(new btBoxShape(tf::Vector3(size[0] * scale / 2.0 + padding, size[1] * scale / 2.0 + padding, size[2] * scale / 2.0 + padding)));
00136     }   
00137     break;
00138   case shapes::CYLINDER:
00139     {
00140             double r2 = static_cast<const shapes::Cylinder*>(shape)->radius * scale + padding;
00141             btshape = dynamic_cast<btCollisionShape*>(new btCylinderShapeZ(tf::Vector3(r2, r2, static_cast<const shapes::Cylinder*>(shape)->length * scale / 2.0 + padding)));
00142     }
00143     break;
00144   case shapes::MESH:
00145     {
00146             const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
00147             btConvexHullShape *btmesh = new btConvexHullShape();
00148             
00149             for (unsigned int i = 0 ; i < mesh->vertexCount ; ++i)
00150         btmesh->addPoint(tf::Vector3(mesh->vertices[3*i], mesh->vertices[3*i + 1], mesh->vertices[3*i + 2]));
00151             
00152             btmesh->setLocalScaling(tf::Vector3(scale, scale, scale));
00153             btmesh->setMargin(padding + 0.0001); // we need this to be positive
00154             btshape = dynamic_cast<btCollisionShape*>(btmesh);
00155     }
00156         
00157   default:
00158     break;
00159   }
00160     
00161   if (btshape)
00162   {
00163     btCollisionObject *object = new btCollisionObject();
00164     object->setCollisionShape(btshape);
00165     return object;
00166   }
00167   else
00168     return NULL;
00169 }
00170 
00171 btCollisionObject* collision_space::EnvironmentModelBullet::createCollisionBody(const shapes::StaticShape *shape)
00172 {
00173   btCollisionShape *btshape = NULL;
00174     
00175   switch (shape->type)
00176   {
00177   case shapes::PLANE:
00178     {
00179             const shapes::Plane *p = static_cast<const shapes::Plane*>(shape);
00180             btshape = new btStaticPlaneShape(tf::Vector3(p->a, p->b, p->c), p->d);
00181     }
00182     break;
00183 
00184   default:
00185     break;
00186   }
00187     
00188   if (btshape)
00189   {
00190     btCollisionObject *object = new btCollisionObject();
00191     object->setCollisionShape(btshape);
00192     return object;
00193   }
00194   else
00195     return NULL;
00196 }
00197 
00198 void collision_space::EnvironmentModelBullet::updateAttachedBodies(void)
00199 {
00200   const unsigned int n = m_modelGeom.linkGeom.size();    
00201   for (unsigned int i = 0 ; i < n ; ++i)
00202   {
00203     kGeom *kg = m_modelGeom.linkGeom[i];
00204         
00205     /* clear previosly attached bodies */
00206     for (unsigned int k = 1 ; k < kg->geom.size() ; ++k)
00207     {
00208             m_world->removeCollisionObject(kg->geom[k]);
00209             //      delete kg->geom[k]->getCollisionShape();
00210             //      delete kg->geom[k];
00211     }
00212         
00213     kg->geom.resize(1);
00214         
00215     /* create new set of attached bodies */     
00216     const unsigned int nab = kg->link->attached_bodies.size();
00217     for (unsigned int k = 0 ; k < nab ; ++k)
00218     {
00219             btCollisionObject *obja = createCollisionBody(kg->link->attached_bodies[k]->shapes[0], m_modelGeom.scale, m_modelGeom.padding);
00220             assert(obja);
00221             m_world->addCollisionObject(obja);
00222             obja->setUserPointer(reinterpret_cast<void*>(kg));
00223             kg->geom.push_back(obja);
00224     }
00225   }
00226 }
00227 
00228 void collision_space::EnvironmentModelBullet::updateRobotModel(void)
00229 { 
00230   const unsigned int n = m_modelGeom.linkGeom.size();
00231   for (unsigned int i = 0 ; i < n ; ++i)
00232   {
00233     m_modelGeom.linkGeom[i]->geom[0]->setWorldTransform(m_modelGeom.linkGeom[i]->link->global_link_transform);
00234     const unsigned int nab = m_modelGeom.linkGeom[i]->link->attached_bodies.size();
00235     for (unsigned int k = 0 ; k < nab ; ++k)
00236             m_modelGeom.linkGeom[i]->geom[k + 1]->setWorldTransform(m_modelGeom.linkGeom[i]->link->attached_bodies[k]->global_collision_body_transform[0]);
00237   }
00238 }
00239 
00240 bool collision_space::EnvironmentModelBullet::isCollision(void)
00241 {   
00242   m_world->getPairCache()->setOverlapFilterCallback(&m_genericCollisionFilterCallback);
00243   m_world->performDiscreteCollisionDetection();
00244     
00245   unsigned int numManifolds = m_world->getDispatcher()->getNumManifolds();
00246   for(unsigned int i = 0; i < numManifolds; ++i)
00247   {
00248     btPersistentManifold* contactManifold = m_world->getDispatcher()->getManifoldByIndexInternal(i);
00249         
00250     if (contactManifold->getNumContacts() > 0)
00251             return true;
00252   }
00253     
00254   return false;
00255 }
00256 
00257 bool collision_space::EnvironmentModelBullet::getCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int max_count)
00258 {        
00259   m_world->getPairCache()->setOverlapFilterCallback(&m_genericCollisionFilterCallback);
00260   m_world->performDiscreteCollisionDetection();
00261   bool collision = false;
00262   contacts.clear();
00263     
00264   unsigned int numManifolds = m_world->getDispatcher()->getNumManifolds();
00265   for(unsigned int i = 0; i < numManifolds; ++i)
00266   {
00267     btPersistentManifold* contactManifold = m_world->getDispatcher()->getManifoldByIndexInternal(i);
00268     int nc = contactManifold->getNumContacts();
00269     if (nc > 0)
00270     {
00271             collision = true;
00272             for (int j = 0 ; j < nc && (contacts.size() < max_count || max_count == 0) ; ++j)
00273             {
00274         btManifoldPoint& pt = contactManifold->getContactPoint(j);
00275         collision_space::EnvironmentModelBullet::Contact add;
00276         add.pos = pt.getPositionWorldOnB();
00277         add.normal = pt.m_normalWorldOnB;
00278         add.depth = -pt.getDistance();
00279         add.link1 = NULL;
00280         add.link2 = NULL;
00281         btCollisionObject* b0 = reinterpret_cast<btCollisionObject*>(contactManifold->getBody0());
00282         btCollisionObject* b1 = reinterpret_cast<btCollisionObject*>(contactManifold->getBody1());
00283         if (b1 && b0)
00284         {
00285           kGeom* kg0 = reinterpret_cast<kGeom*>(b0->getUserPointer());
00286           kGeom* kg1 = reinterpret_cast<kGeom*>(b1->getUserPointer());
00287           if (kg0)
00288             add.link1 = kg0->link;
00289           if (kg1)
00290             add.link2 = kg1->link;
00291         }
00292         contacts.push_back(add);
00293             }
00294             if (max_count > 0 && contacts.size() >= max_count)
00295         break;
00296     }
00297   }
00298     
00299   return collision;
00300 }
00301 
00302 bool collision_space::EnvironmentModelBullet::isSelfCollision(void)
00303 {
00304   m_world->getPairCache()->setOverlapFilterCallback(&m_selfCollisionFilterCallback);
00305   m_world->performDiscreteCollisionDetection();
00306     
00307   unsigned int numManifolds = m_world->getDispatcher()->getNumManifolds();
00308   for(unsigned int i = 0; i < numManifolds; ++i)
00309   {
00310     btPersistentManifold* contactManifold = m_world->getDispatcher()->getManifoldByIndexInternal(i);
00311         
00312     if (contactManifold->getNumContacts() > 0)
00313             return true;
00314   }
00315     
00316   return false;
00317 }
00318 
00319 void collision_space::EnvironmentModelBullet::removeCollidingObjects(const shapes::StaticShape *shape)
00320 {
00321 }
00322 
00323 void collision_space::EnvironmentModelBullet::removeCollidingObjects(const shapes::Shape *shape, const tf::Transform &pose)
00324 {
00325 }
00326 
00327 void collision_space::EnvironmentModelBullet::addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<tf::Transform> &poses)
00328 {
00329   assert(shapes.size() == poses.size());
00330   for (unsigned int i = 0 ; i < shapes.size() ; ++i)
00331     addObject(ns, shapes[i], poses[i]);
00332 }
00333 
00334 void collision_space::EnvironmentModelBullet::addObject(const std::string &ns, shapes::StaticShape* shape)
00335 {
00336   btCollisionObject *obj = createCollisionBody(shape);
00337   m_world->addCollisionObject(obj);
00338   m_obstacles[ns].push_back(obj);
00339   m_objects->addObject(ns, shape);
00340 }
00341 
00342 void collision_space::EnvironmentModelBullet::addObject(const std::string &ns, shapes::Shape *shape, const tf::Transform &pose)
00343 {
00344   btCollisionObject *obj = createCollisionBody(shape, 1.0, 0.0);
00345   obj->setWorldTransform(pose);
00346   m_world->addCollisionObject(obj);
00347   m_obstacles[ns].push_back(obj);
00348   m_objects->addObject(ns, shape, pose);    
00349 }
00350 
00351 void collision_space::EnvironmentModelBullet::clearObjects(const std::string &ns)
00352 {
00353   if (m_obstacles.find(ns) != m_obstacles.end())
00354   {
00355     unsigned int n = m_obstacles[ns].size();
00356     for (unsigned int i = 0 ; i < n ; ++i)
00357     {
00358             btCollisionObject* obj = m_obstacles[ns][i];
00359             m_world->removeCollisionObject(obj);
00360             //  delete obj->getCollisionShape();
00361             //  delete obj;
00362     }
00363     m_obstacles.erase(ns);
00364   }   
00365   m_objects->clearObjects(ns);
00366 }
00367 
00368 void collision_space::EnvironmentModelBullet::clearObjects(void)
00369 {
00370   for (std::map<std::string, std::vector<btCollisionObject*> >::iterator it = m_obstacles.begin() ; it != m_obstacles.end() ; ++it)
00371     clearObjects(it->first);
00372 }
00373 
00374 int collision_space::EnvironmentModelBullet::setCollisionCheck(const std::string &link, bool state)
00375 { 
00376   int result = -1;
00377   for (unsigned int j = 0 ; j < m_modelGeom.linkGeom.size() ; ++j)
00378   {
00379     if (m_modelGeom.linkGeom[j]->link->getName() == link)
00380     {
00381             result = m_modelGeom.linkGeom[j]->enabled ? 1 : 0;
00382             m_modelGeom.linkGeom[j]->enabled = state;
00383             break;
00384     }
00385   }
00386 
00387   return result;    
00388 }
00389 
00390 void collision_space::EnvironmentModelBullet::setCollisionCheckLinks(const std::vector<std::string> &links, bool state)
00391 { 
00392   if(links.empty())
00393     return;
00394 
00395   for (unsigned int i = 0 ; i < m_modelGeom.linkGeom.size() ; ++i)
00396   {
00397     for (unsigned int j=0; j < links.size(); j++)
00398     {
00399       if (m_modelGeom.linkGeom[i]->link->getName() == links[j])
00400       {
00401         m_modelGeom.linkGeom[i]->enabled = state;
00402         break;
00403       }
00404     }
00405   }
00406 }
00407 
00408 void collision_space::EnvironmentModelBullet::setCollisionCheckOnlyLinks(const std::vector<std::string> &links, bool state)
00409 { 
00410   if(links.empty())
00411     return;
00412 
00413   for (unsigned int i = 0 ; i < m_modelGeom.linkGeom.size() ; ++i)
00414   {
00415     int result = -1;
00416     for (unsigned int j=0; j < links.size(); j++)
00417     {
00418       if (m_modelGeom.linkGeom[i]->link->getName() == links[j])
00419       {
00420         m_modelGeom.linkGeom[i]->enabled = state;
00421         result = j;
00422         break;
00423       }        
00424     }
00425     if(result < 0)
00426       m_modelGeom.linkGeom[i]->enabled = !state;      
00427   }
00428 }
00429 
00430 void collision_space::EnvironmentModelBullet::setCollisionCheckAll(bool state)
00431 { 
00432   for (unsigned int j = 0 ; j < m_modelGeom.linkGeom.size() ; ++j)
00433   {
00434     m_modelGeom.linkGeom[j]->enabled = state;
00435   }
00436 }
00437 
00438 collision_space::EnvironmentModel* collision_space::EnvironmentModelBullet::clone(void) const
00439 {
00440   return NULL;    
00441 }


collision_space
Author(s): Ioan Sucan/isucan@willowgarage.com
autogenerated on Thu Dec 12 2013 11:09:16