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 #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
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
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
00075
00076
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
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);
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
00206 for (unsigned int k = 1 ; k < kg->geom.size() ; ++k)
00207 {
00208 m_world->removeCollisionObject(kg->geom[k]);
00209
00210
00211 }
00212
00213 kg->geom.resize(1);
00214
00215
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
00361
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 }