Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18679] pkg/trunk
|
From: <is...@us...> - 2009-07-12 21:39:43
|
Revision: 18679
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18679&view=rev
Author: isucan
Date: 2009-07-12 21:39:40 +0000 (Sun, 12 Jul 2009)
Log Message:
-----------
namespace model for bodies in collision space
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/src/environment.cpp
pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-07-12 21:39:40 UTC (rev 18679)
@@ -41,7 +41,7 @@
void planning_environment::CollisionModels::setupModel(boost::shared_ptr<collision_space::EnvironmentModel> &model, const std::vector<std::string> &links)
{
model->lock();
- model->addRobotModel(kmodel_, links, scale_, padd_);
+ model->setRobotModel(kmodel_, links, scale_, padd_);
// form all pairs of links that can collide and add them as self-collision groups
for (unsigned int i = 0 ; i < self_collision_check_groups_.size() ; ++i)
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-07-12 21:39:40 UTC (rev 18679)
@@ -82,7 +82,7 @@
else
for (unsigned int i = 0 ; i < planeValues.size() / 4 ; ++i)
{
- collisionSpace_->addStaticPlane(planeValues[i * 4], planeValues[i * 4 + 1], planeValues[i * 4 + 2], planeValues[i * 4 + 3]);
+ collisionSpace_->addPlane("bounds", planeValues[i * 4], planeValues[i * 4 + 1], planeValues[i * 4 + 2], planeValues[i * 4 + 3]);
ROS_INFO("Added static plane %fx + %fy + %fz + %f = 0", planeValues[i * 4], planeValues[i * 4 + 1], planeValues[i * 4 + 2], planeValues[i * 4 + 3]);
}
@@ -209,9 +209,9 @@
collisionSpace_->lock();
if (clear)
- collisionSpace_->clearObstacles();
+ collisionSpace_->clearObstacles("points");
if (n > 0)
- collisionSpace_->addPointCloud(n, data);
+ collisionSpace_->addPointCloudSpheres("points", n, data);
collisionSpace_->unlock();
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-12 21:39:40 UTC (rev 18679)
@@ -49,14 +49,11 @@
/** \brief Main namespace */
namespace collision_space
{
- /** \brief
- A class describing an environment for a kinematic robot. This is
- the base (abstract) definition. Different implementations are
- possible. The class is aware of a certain set of fixed
- (addStatic*) obstacles that never change, a set of obstacles that
- can change (removed by clearObstacles()) and a kinematic
- robot model. The class provides functionality for checking whether a
- given robot is in collision.
+ /** \brief A class describing an environment for a kinematic
+ robot. This is the base (abstract) definition. Different
+ implementations are possible. The class is aware of a set of
+ obstacles and a robot model. The obstacles are placed in different
+ namespaces so they can be added and removed selectively.
*/
class EnvironmentModel
{
@@ -109,7 +106,7 @@
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual void addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** \brief Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(void) = 0;
@@ -137,21 +134,20 @@
/**********************************************************************/
- /* Collision Bodies Definition (Dynamic) */
+ /* Collision Bodies */
/**********************************************************************/
/** \brief Remove all obstacles from collision model */
virtual void clearObstacles(void) = 0;
+ /** \brief Remove obstacles from a specific namespace in the collision model */
+ virtual void clearObstacles(const std::string &ns) = 0;
+
/** \brief Add a point cloud to the collision space */
- virtual void addPointCloud(unsigned int n, const double* points) = 0;
+ virtual void addPointCloudSpheres(const std::string &ns, unsigned int n, const double* points) = 0;
- /**********************************************************************/
- /* Collision Bodies Definition (Static) */
- /**********************************************************************/
-
/** \brief Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addStaticPlane(double a, double b, double c, double d) = 0;
+ virtual void addPlane(const std::string &ns, double a, double b, double c, double d) = 0;
/**********************************************************************/
/* Miscellaneous Routines */
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-12 21:39:40 UTC (rev 18679)
@@ -40,6 +40,7 @@
#include "collision_space/environment.h"
#include "btBulletCollisionCommon.h"
+#include <map>
namespace collision_space
{
@@ -76,11 +77,14 @@
/** \brief Remove all obstacles from collision model */
virtual void clearObstacles(void);
+ /** \brief Remove obstacles from a specific namespace in the collision model */
+ virtual void clearObstacles(const std::string &ns);
+
/** \brief Add a point cloud to the collision space */
- virtual void addPointCloud(unsigned int n, const double *points);
+ virtual void addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points);
/** \brief Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addStaticPlane(double a, double b, double c, double d);
+ virtual void addPlane(const std::string &ns, double a, double b, double c, double d);
/** \brief Add a robot model. Ignore robot links if their name is not
specified in the string vector. The scale argument can be
@@ -88,7 +92,7 @@
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual void addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** \brief Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(void);
@@ -224,13 +228,14 @@
btCollisionObject* createCollisionBody(shapes::Shape *shape, double scale, double padding);
void freeMemory(void);
- SelfCollisionFilterCallback m_selfCollisionFilterCallback;
- GenericCollisionFilterCallback m_genericCollisionFilterCallback;
+ SelfCollisionFilterCallback m_selfCollisionFilterCallback;
+ GenericCollisionFilterCallback m_genericCollisionFilterCallback;
- ModelInfo m_modelGeom;
- std::vector<btCollisionObject*> m_dynamicObstacles;
- btCollisionWorld *m_world;
- btDefaultCollisionConfiguration *m_config;
+ ModelInfo m_modelGeom;
+ std::map<std::string, std::vector<btCollisionObject*> >
+ m_obstacles;
+ btCollisionWorld *m_world;
+ btDefaultCollisionConfiguration *m_config;
};
}
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-12 21:39:40 UTC (rev 18679)
@@ -39,6 +39,7 @@
#include "collision_space/environment.h"
#include <ode/ode.h>
+#include <map>
namespace collision_space
{
@@ -54,8 +55,6 @@
EnvironmentModelODE(void) : EnvironmentModel()
{
dInitODE2(0);
- m_space = dHashSpaceCreate(0);
- m_spaceBasicGeoms = dHashSpaceCreate(0);
}
virtual ~EnvironmentModelODE(void)
@@ -63,19 +62,7 @@
freeMemory();
dCloseODE();
}
-
- /** \brief The space ID for the objects that can be changed in the
- map. clearObstacles will invalidate this ID. Collision
- checking on this space is optimized for many small
- objects. */
- dSpaceID getODESpace(void) const;
- /** \brief Return the space ID for the space in which static objects are added */
- dSpaceID getODEBasicGeomSpace(void) const;
-
- /** \brief Return the space ID for the space in which the robot model is instanciated */
- dSpaceID getModelODESpace(void) const;
-
/** \brief Get the list of contacts (collisions) */
virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1);
@@ -88,19 +75,22 @@
/** \brief Remove all obstacles from collision model */
virtual void clearObstacles(void);
+ /** \brief Remove obstacles from a specific namespace in the collision model */
+ virtual void clearObstacles(const std::string &ns);
+
/** \brief Add a point cloud to the collision space */
- virtual void addPointCloud(unsigned int n, const double *points);
+ virtual void addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points);
/** \brief Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addStaticPlane(double a, double b, double c, double d);
-
+ virtual void addPlane(const std::string &ns, double a, double b, double c, double d);
+
/** \brief Add a robot model. Ignore robot links if their name is not
specified in the string vector. The scale argument can be
used to increase or decrease the size of the robot's
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual void addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** \brief Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(void);
@@ -113,12 +103,6 @@
protected:
- /** \brief Internal function for collision detection */
- void testCollision(void *data);
- void testSelfCollision(void *data);
- void testStaticBodyCollision(void *data);
- void testDynamicBodyCollision(void *data);
-
class ODECollide2
{
public:
@@ -140,6 +124,7 @@
void clear(void);
void setup(void);
void collide(dGeomID geom, void *data, dNearCallback *nearCallback);
+ bool empty(void);
private:
@@ -226,33 +211,108 @@
unsigned int index;
};
+ /** \brief Structure for maintaining ODE temporary data */
+ struct ODEStorage
+ {
+ ~ODEStorage(void)
+ {
+ clear();
+ }
+
+ void clear(void)
+ {
+ for (unsigned int i = 0 ; i < meshIndices.size() ; ++i)
+ delete[] meshIndices[i];
+ meshIndices.clear();
+ for (unsigned int i = 0 ; i < meshData.size() ; ++i)
+ dGeomTriMeshDataDestroy(meshData[i]);
+ meshData.clear();
+ }
+
+ /* Pointers for ODE indices; we need this around in ODE's assumed datatype */
+ std::vector<dTriIndex*> meshIndices;
+ std::vector<dTriMeshDataID> meshData;
+ };
+
struct ModelInfo
{
- std::vector< kGeom* > linkGeom;
- double scale;
- double padding;
- dSpaceID space;
+ std::vector< kGeom* > linkGeom;
+ double scale;
+ double padding;
+ dSpaceID space;
+ ODEStorage storage;
};
- dGeomID createODEGeom(dSpaceID space, shapes::Shape *shape, double scale, double padding);
- void updateGeom(dGeomID geom, btTransform &pose) const;
- void freeMemory(void);
+ struct CollisionNamespace
+ {
+ CollisionNamespace(const std::string &nm) : name(nm)
+ {
+ space = dHashSpaceCreate(0);
+ }
+
+ virtual ~CollisionNamespace(void)
+ {
+ if (space)
+ dSpaceDestroy(space);
+ }
+
+ void clear(void)
+ {
+ if (space)
+ dSpaceDestroy(space);
+ space = dHashSpaceCreate(0);
+ geoms.clear();
+ collide2.clear();
+ storage.clear();
+ }
+
+ std::string name;
+ dSpaceID space;
+ std::vector<dGeomID> geoms;
+ ODECollide2 collide2;
+ ODEStorage storage;
+ };
- ModelInfo m_modelGeom;
- dSpaceID m_space;
- dSpaceID m_spaceBasicGeoms;
+ struct CollisionData
+ {
+ CollisionData(void)
+ {
+ done = false;
+ collides = false;
+ max_contacts = 0;
+ contacts = NULL;
+ selfCollisionTest = NULL;
+ link1 = link2 = NULL;
+ }
+
+ bool done;
+
+ bool collides;
+ unsigned int max_contacts;
+ std::vector<EnvironmentModelODE::Contact> *contacts;
+ std::vector< std::vector<bool> > *selfCollisionTest;
+
+ planning_models::KinematicModel::Link *link1;
+ planning_models::KinematicModel::Link *link2;
+ };
- /* This is where geoms from the world (that can be cleared and recreated) are added; the space for this is m_space */
- ODECollide2 m_collide2;
+
+ /** \brief Internal function for collision detection */
+ void testCollision(CollisionData *data);
- /* This is where static geoms from the world (that are not cleared) are added; the space for this is m_spaceBasicGeoms */
- std::vector<dGeomID> m_basicGeoms;
+ /** \brief Internal function for collision detection */
+ void testSelfCollision(CollisionData *data);
+
+ /** \brief Internal function for collision detection */
+ void testBodyCollision(CollisionNamespace *cn, CollisionData *data);
+
+ dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, shapes::Shape *shape, double scale, double padding);
+ void updateGeom(dGeomID geom, btTransform &pose) const;
+ void freeMemory(void);
- private:
+ ModelInfo m_modelGeom;
+ std::map<std::string, CollisionNamespace*> m_collNs;
- /* Pointers for ODE indices; we need this around in ODE's assumed datatype */
- std::vector<dTriIndex*> m_meshIndices;
- std::vector<dTriMeshDataID> m_meshData;
};
}
Modified: pkg/trunk/world_models/collision_space/src/environment.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment.cpp 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/collision_space/src/environment.cpp 2009-07-12 21:39:40 UTC (rev 18679)
@@ -46,7 +46,7 @@
m_verbose = verbose;
}
-void collision_space::EnvironmentModel::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+void collision_space::EnvironmentModel::setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
m_robotModel = model;
m_collisionLinks = links;
Modified: pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-07-12 21:39:40 UTC (rev 18679)
@@ -59,9 +59,9 @@
delete m_config; */
}
-void collision_space::EnvironmentModelBullet::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+void collision_space::EnvironmentModelBullet::setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
- collision_space::EnvironmentModel::addRobotModel(model, links, scale, padding);
+ collision_space::EnvironmentModel::setRobotModel(model, links, scale, padding);
m_modelGeom.scale = scale;
m_modelGeom.padding = padding;
@@ -266,7 +266,7 @@
return false;
}
-void collision_space::EnvironmentModelBullet::addPointCloud(unsigned int n, const double *points)
+void collision_space::EnvironmentModelBullet::addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points)
{
btTransform t;
t.setIdentity();
@@ -280,30 +280,41 @@
t.setOrigin(btVector3(points[i4], points[i4 + 1], points[i4 + 2]));
object->setWorldTransform(t);
m_world->addCollisionObject(object);
- m_dynamicObstacles.push_back(object);
+ m_obstacles[ns].push_back(object);
}
}
-void collision_space::EnvironmentModelBullet::addStaticPlane(double a, double b, double c, double d)
+void collision_space::EnvironmentModelBullet::addPlane(const std::string &ns, double a, double b, double c, double d)
{
btCollisionObject *object = new btCollisionObject();
btCollisionShape *shape = new btStaticPlaneShape(btVector3(a, b, c), d);
object->setCollisionShape(shape);
m_world->addCollisionObject(object);
+ m_obstacles[ns].push_back(object);
}
-void collision_space::EnvironmentModelBullet::clearObstacles(void)
+void collision_space::EnvironmentModelBullet::clearObstacles(const std::string &ns)
{
- for (unsigned int i = 0 ; i < m_dynamicObstacles.size() ; ++i)
+ if (m_obstacles.find(ns) != m_obstacles.end())
{
- btCollisionObject* obj = m_dynamicObstacles[i];
- m_world->removeCollisionObject(obj);
- // delete obj->getCollisionShape();
- // delete obj;
+ unsigned int n = m_obstacles[ns].size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ btCollisionObject* obj = m_obstacles[ns][i];
+ m_world->removeCollisionObject(obj);
+ // delete obj->getCollisionShape();
+ // delete obj;
+ }
+ m_obstacles.erase(ns);
}
- m_dynamicObstacles.clear();
}
+void collision_space::EnvironmentModelBullet::clearObstacles(void)
+{
+ for (std::map<std::string, std::vector<btCollisionObject*> >::iterator it = m_obstacles.begin() ; it != m_obstacles.end() ; ++it)
+ clearObstacles(it->first);
+}
+
int collision_space::EnvironmentModelBullet::setCollisionCheck(const std::string &link, bool state)
{
int result = -1;
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-12 21:39:40 UTC (rev 18679)
@@ -46,21 +46,13 @@
delete m_modelGeom.linkGeom[j];
if (m_modelGeom.space)
dSpaceDestroy(m_modelGeom.space);
- if (m_space)
- dSpaceDestroy(m_space);
- if (m_spaceBasicGeoms)
- dSpaceDestroy(m_spaceBasicGeoms);
- for (unsigned int i = 0 ; i < m_meshIndices.size() ; ++i)
- delete[] m_meshIndices[i];
- m_meshIndices.clear();
- for (unsigned int i = 0 ; i < m_meshData.size() ; ++i)
- dGeomTriMeshDataDestroy(m_meshData[i]);
- m_meshData.clear();
+ for (std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.begin() ; it != m_collNs.end() ; ++it)
+ delete it->second;
}
-void collision_space::EnvironmentModelODE::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+void collision_space::EnvironmentModelODE::setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
- collision_space::EnvironmentModel::addRobotModel(model, links, scale, padding);
+ collision_space::EnvironmentModel::setRobotModel(model, links, scale, padding);
m_modelGeom.space = dSweepAndPruneSpaceCreate(0, dSAP_AXES_XZY);
m_modelGeom.scale = scale;
@@ -79,13 +71,13 @@
kg->link = link;
kg->enabled = true;
kg->index = i;
- dGeomID g = createODEGeom(m_modelGeom.space, link->shape, scale, padding);
+ dGeomID g = createODEGeom(m_modelGeom.space, m_modelGeom.storage, link->shape, scale, padding);
assert(g);
dGeomSetData(g, reinterpret_cast<void*>(kg));
kg->geom.push_back(g);
for (unsigned int k = 0 ; k < kg->link->attachedBodies.size() ; ++k)
{
- dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, scale, padding);
+ dGeomID ga = createODEGeom(m_modelGeom.space, m_modelGeom.storage, kg->link->attachedBodies[k]->shape, scale, padding);
assert(ga);
dGeomSetData(ga, reinterpret_cast<void*>(kg));
kg->geom.push_back(ga);
@@ -94,7 +86,7 @@
}
}
-dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, shapes::Shape *shape, double scale, double padding)
+dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, ODEStorage &storage, shapes::Shape *shape, double scale, double padding)
{
dGeomID g = NULL;
switch (shape->type)
@@ -126,8 +118,8 @@
indices[i] = mesh->triangles[i];
dGeomTriMeshDataBuildDouble(data, mesh->vertices, sizeof(double) * 3, mesh->vertexCount, indices, icount, sizeof(dTriIndex) * 3);
g = dCreateTriMesh(space, data, NULL, NULL, NULL);
- m_meshIndices.push_back(indices);
- m_meshData.push_back(data);
+ storage.meshIndices.push_back(indices);
+ storage.meshData.push_back(data);
}
default:
@@ -162,7 +154,7 @@
const unsigned int nab = kg->link->attachedBodies.size();
for (unsigned int k = 0 ; k < nab ; ++k)
{
- dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, m_modelGeom.scale, m_modelGeom.padding);
+ dGeomID ga = createODEGeom(m_modelGeom.space, m_modelGeom.storage, kg->link->attachedBodies[k]->shape, m_modelGeom.scale, m_modelGeom.padding);
assert(ga);
dGeomSetData(ga, reinterpret_cast<void*>(kg));
kg->geom.push_back(ga);
@@ -183,6 +175,11 @@
}
}
+bool collision_space::EnvironmentModelODE::ODECollide2::empty(void)
+{
+ return m_geomsX.empty();
+}
+
void collision_space::EnvironmentModelODE::ODECollide2::registerSpace(dSpaceID space)
{
int n = dSpaceGetNumGeoms(space);
@@ -299,50 +296,13 @@
}
}
-dSpaceID collision_space::EnvironmentModelODE::getODESpace(void) const
-{
- return m_space;
-}
-
-dSpaceID collision_space::EnvironmentModelODE::getODEBasicGeomSpace(void) const
-{
- return m_spaceBasicGeoms;
-}
-
-dSpaceID collision_space::EnvironmentModelODE::getModelODESpace(void) const
-{
- return m_modelGeom.space;
-}
-
namespace collision_space
{
- struct CollisionData
- {
- CollisionData(void)
- {
- done = false;
- collides = false;
- max_contacts = 0;
- contacts = NULL;
- selfCollisionTest = NULL;
- link1 = link2 = NULL;
- }
-
- bool done;
-
- bool collides;
- unsigned int max_contacts;
- std::vector<EnvironmentModelODE::Contact> *contacts;
- std::vector< std::vector<bool> > *selfCollisionTest;
-
- planning_models::KinematicModel::Link *link1;
- planning_models::KinematicModel::Link *link2;
- };
-
+
void nearCallbackFn(void *data, dGeomID o1, dGeomID o2)
{
- CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
+ EnvironmentModelODE::CollisionData *cdata = reinterpret_cast<EnvironmentModelODE::CollisionData*>(data);
if (cdata->done)
return;
@@ -422,7 +382,7 @@
cdata.contacts = &contacts;
cdata.max_contacts = max_count;
contacts.clear();
- testCollision(reinterpret_cast<void*>(&cdata));
+ testCollision(&cdata);
return cdata.collides;
}
@@ -430,7 +390,7 @@
{
CollisionData cdata;
cdata.selfCollisionTest = &m_selfCollisionTest;
- testCollision(reinterpret_cast<void*>(&cdata));
+ testCollision(&cdata);
return cdata.collides;
}
@@ -438,119 +398,136 @@
{
CollisionData cdata;
cdata.selfCollisionTest = &m_selfCollisionTest;
- testSelfCollision(reinterpret_cast<void*>(&cdata));
+ testSelfCollision(&cdata);
return cdata.collides;
}
-void collision_space::EnvironmentModelODE::testSelfCollision(void *data)
+void collision_space::EnvironmentModelODE::testSelfCollision(CollisionData *cdata)
{
- dSpaceCollide(m_modelGeom.space, data, nearCallbackFn);
+ dSpaceCollide(m_modelGeom.space, cdata, nearCallbackFn);
}
-void collision_space::EnvironmentModelODE::testStaticBodyCollision(void *data)
-{
- CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
+void collision_space::EnvironmentModelODE::testBodyCollision(CollisionNamespace *cn, CollisionData *cdata)
+{
cdata->link2 = NULL;
- for (int i = m_modelGeom.linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
+
+ if (cn->collide2.empty())
{
- /* skip disabled bodies */
- if (!m_modelGeom.linkGeom[i]->enabled)
- continue;
- const unsigned int ng = m_modelGeom.linkGeom[i]->geom.size();
- cdata->link1 = m_modelGeom.linkGeom[i]->link;
-
- for (unsigned int ig = 0 ; ig < ng && !cdata->done ; ++ig)
+ // if there is no collide2 structure, then there is a list of geoms
+ for (int i = m_modelGeom.linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
{
- dGeomID g1 = m_modelGeom.linkGeom[i]->geom[ig];
- dReal aabb1[6];
- dGeomGetAABB(g1, aabb1);
- for (int j = m_basicGeoms.size() - 1 ; j >= 0 ; --j)
- {
- dGeomID g2 = m_basicGeoms[j];
- dReal aabb2[6];
- dGeomGetAABB(g2, aabb2);
-
- if (!(aabb1[2] > aabb2[3] ||
- aabb1[3] < aabb2[2] ||
- aabb1[4] > aabb2[5] ||
- aabb1[5] < aabb2[4]))
- dSpaceCollide2(g1, g2, data, nearCallbackFn);
-
- if (cdata->collides && m_verbose)
- m_msg.inform("Collision between static body and link '%s'\n",
- m_modelGeom.linkGeom[i]->link->name.c_str());
- }
- }
- }
-}
-
-void collision_space::EnvironmentModelODE::testDynamicBodyCollision(void *data)
-{
- CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
- cdata->link2 = NULL;
- m_collide2.setup();
- for (int i = m_modelGeom.linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
- if (m_modelGeom.linkGeom[i]->enabled)
- {
+ /* skip disabled bodies */
+ if (!m_modelGeom.linkGeom[i]->enabled)
+ continue;
const unsigned int ng = m_modelGeom.linkGeom[i]->geom.size();
cdata->link1 = m_modelGeom.linkGeom[i]->link;
+
for (unsigned int ig = 0 ; ig < ng && !cdata->done ; ++ig)
{
- m_collide2.collide(m_modelGeom.linkGeom[i]->geom[ig], data, nearCallbackFn);
- if (cdata->collides && m_verbose)
- m_msg.inform("Collision between dynamic body and link '%s'\n",
- m_modelGeom.linkGeom[i]->link->name.c_str());
+ dGeomID g1 = m_modelGeom.linkGeom[i]->geom[ig];
+ dReal aabb1[6];
+ dGeomGetAABB(g1, aabb1);
+ for (int j = cn->geoms.size() - 1 ; j >= 0 ; --j)
+ {
+ dGeomID g2 = cn->geoms[j];
+ dReal aabb2[6];
+ dGeomGetAABB(g2, aabb2);
+
+ if (!(aabb1[2] > aabb2[3] ||
+ aabb1[3] < aabb2[2] ||
+ aabb1[4] > aabb2[5] ||
+ aabb1[5] < aabb2[4]))
+ dSpaceCollide2(g1, g2, cdata, nearCallbackFn);
+
+ if (cdata->collides && m_verbose)
+ m_msg.inform("Collision between body in namespace '%s' and link '%s'\n",
+ cn->name.c_str(), m_modelGeom.linkGeom[i]->link->name.c_str());
+ }
}
}
+ }
+ else
+ {
+ cn->collide2.setup();
+ for (int i = m_modelGeom.linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
+ if (m_modelGeom.linkGeom[i]->enabled)
+ {
+ const unsigned int ng = m_modelGeom.linkGeom[i]->geom.size();
+ cdata->link1 = m_modelGeom.linkGeom[i]->link;
+ for (unsigned int ig = 0 ; ig < ng && !cdata->done ; ++ig)
+ {
+ cn->collide2.collide(m_modelGeom.linkGeom[i]->geom[ig], cdata, nearCallbackFn);
+ if (cdata->collides && m_verbose)
+ m_msg.inform("Collision between body in namespace '%s' and link '%s'\n",
+ cn->name.c_str(), m_modelGeom.linkGeom[i]->link->name.c_str());
+ }
+ }
+ }
}
-
-void collision_space::EnvironmentModelODE::testCollision(void *data)
+void collision_space::EnvironmentModelODE::testCollision(CollisionData *cdata)
{
- CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
-
/* check self collision */
if (m_selfCollision)
- testSelfCollision(data);
+ testSelfCollision(cdata);
- /* check collision with standalone ode bodies */
- if (!cdata->done)
- testStaticBodyCollision(data);
+ /* check collision with other ode bodies */
+ for (std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.begin() ; it != m_collNs.end() && !cdata->done ; ++it)
+ testBodyCollision(it->second, cdata);
- /* check collision with pointclouds */
- if (!cdata->done)
- testDynamicBodyCollision(data);
-
cdata->done = true;
}
-void collision_space::EnvironmentModelODE::addPointCloud(unsigned int n, const double *points)
+void collision_space::EnvironmentModelODE::addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points)
{
+ std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.find(ns);
+ CollisionNamespace* cn = NULL;
+ if (it == m_collNs.end())
+ {
+ cn = new CollisionNamespace(ns);
+ m_collNs[ns] = cn;
+ }
+ else
+ cn = it->second;
for (unsigned int i = 0 ; i < n ; ++i)
{
unsigned int i4 = i * 4;
- dGeomID g = dCreateSphere(m_space, points[i4 + 3]);
+ dGeomID g = dCreateSphere(cn->space, points[i4 + 3]);
dGeomSetPosition(g, points[i4], points[i4 + 1], points[i4 + 2]);
- m_collide2.registerGeom(g);
+ cn->collide2.registerGeom(g);
}
- m_collide2.setup();
+ cn->collide2.setup();
}
-void collision_space::EnvironmentModelODE::addStaticPlane(double a, double b, double c, double d)
-{
- dGeomID g = dCreatePlane(m_spaceBasicGeoms, a, b, c, d);
- m_basicGeoms.push_back(g);
+void collision_space::EnvironmentModelODE::addPlane(const std::string &ns, double a, double b, double c, double d)
+{
+ std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.find(ns);
+ CollisionNamespace* cn = NULL;
+ if (it == m_collNs.end())
+ {
+ cn = new CollisionNamespace(ns);
+ m_collNs[ns] = cn;
+ }
+ else
+ cn = it->second;
+ dGeomID g = dCreatePlane(cn->space, a, b, c, d);
+ cn->geoms.push_back(g);
}
void collision_space::EnvironmentModelODE::clearObstacles(void)
{
- m_collide2.clear();
- if (m_space)
- dSpaceDestroy(m_space);
- m_space = dHashSpaceCreate(0);
- m_collide2.setup();
+ for (std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.begin() ; it != m_collNs.end() ; ++it)
+ delete it->second;
+ m_collNs.clear();
}
+void collision_space::EnvironmentModelODE::clearObstacles(const std::string &ns)
+{
+ std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.find(ns);
+ if (it != m_collNs.end())
+ it->second->clear();
+}
+
int collision_space::EnvironmentModelODE::setCollisionCheck(const std::string &link, bool state)
{
int result = -1;
Modified: pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
===================================================================
--- pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp 2009-07-12 21:39:40 UTC (rev 18679)
@@ -77,13 +77,13 @@
data[i4 + 2] = uniform(1.5);
data[i4 + 3] = 0.02;
em->clearObstacles();
- em->addPointCloud(1, data + i4);
+ em->addPointCloudSpheres("points", 1, data + i4);
}
while(em->isCollision());
}
em->clearObstacles();
- em->addPointCloud(n, data);
+ em->addPointCloudSpheres("points", n, data);
ROS_INFO("Added %d points", n);
delete[] data;
@@ -133,7 +133,7 @@
data[i4 + 2] = uniform(1.5);
data[i4 + 3] = 0.02;
em->clearObstacles();
- em->addPointCloud(1, data + i4);
+ em->addPointCloudSpheres("points", 1, data + i4);
}
while(!em->isCollision());
sendPoint(data[i4], data[i4 + 1], data[i4 + 2]);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
