Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3576] pkg/trunk
|
From: <is...@us...> - 2008-08-25 21:34:53
|
Revision: 3576
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3576&view=rev
Author: isucan
Date: 2008-08-25 21:34:51 +0000 (Mon, 25 Aug 2008)
Log Message:
-----------
added self collision checking
Modified Paths:
--------------
pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h
pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h
pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h
pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h 2008-08-25 21:34:51 UTC (rev 3576)
@@ -40,6 +40,7 @@
#include <planning_models/kinematic.h>
#include <rosthread/mutex.h>
#include <vector>
+#include <string>
/** @htmlinclude ../../manifest.html
@@ -78,10 +79,13 @@
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id) = 0;
+ /** Add a group of links to be checked for self collision */
+ virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links) = 0;
+
/** Get the number of loaded models */
unsigned int getModelCount(void) const;
/** Get a specific model */
- planning_models::KinematicModel* getModel(unsigned int model_id) const;
+ planning_models::KinematicModel* getRobotModel(unsigned int model_id) const;
/** Provide interface to a lock. Use carefully! */
void lock(void);
@@ -93,8 +97,8 @@
void setSelfCollision(bool selfCollision);
/** Check if self collision is enabled */
- bool getSelfCollision(void) const;
-
+ bool getSelfCollision(void) const;
+
protected:
ros::thread::mutex m_lock;
@@ -103,6 +107,8 @@
/** List of loaded robot models */
std::vector<planning_models::KinematicModel*> m_models;
+
+
};
}
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-08-25 21:34:51 UTC (rev 3576)
@@ -87,6 +87,9 @@
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id);
+ /** Add a group of links to be checked for self collision */
+ virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links);
+
protected:
class ODECollide2
@@ -161,8 +164,9 @@
struct ModelInfo
{
- std::vector< kGeom* > g;
- dSpaceID s;
+ std::vector< kGeom* > geom;
+ dSpaceID space;
+ std::vector< std::vector<unsigned int> > selfCollision;
};
dGeomID createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape) const;
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-08-25 21:34:51 UTC (rev 3576)
@@ -59,13 +59,13 @@
m_octree(0.0f, 0.0f, 0.0f, 0.02f, 0.02f, 0.02f, 1, OCTREE_CELL_EMPTY)
{
m_octree.setAutoExpand(true);
+ m_selfCollision = false;
}
virtual ~EnvironmentModelOctree(void)
{
}
-
/** Check if a model is in collision */
virtual bool isCollision(unsigned int model_id);
@@ -81,6 +81,9 @@
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id);
+ /** Add a group of links to be checked for self collision */
+ virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links);
+
const scan_utils::Octree<char>* getOctree(void) const;
protected:
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp 2008-08-25 21:34:51 UTC (rev 3576)
@@ -69,7 +69,7 @@
return m_models.size();
}
-planning_models::KinematicModel* collision_space::EnvironmentModel::getModel(unsigned int model_id) const
+planning_models::KinematicModel* collision_space::EnvironmentModel::getRobotModel(unsigned int model_id) const
{
return m_models[model_id];
}
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-08-25 21:34:51 UTC (rev 3576)
@@ -40,9 +40,9 @@
{
for (unsigned int i = 0 ; i < m_kgeoms.size() ; ++i)
{
- for (unsigned int j = 0 ; j < m_kgeoms[i].g.size() ; ++j)
- delete m_kgeoms[i].g[j];
- dSpaceDestroy(m_kgeoms[i].s);
+ for (unsigned int j = 0 ; j < m_kgeoms[i].geom.size() ; ++j)
+ delete m_kgeoms[i].geom[j];
+ dSpaceDestroy(m_kgeoms[i].space);
}
if (m_space)
dSpaceDestroy(m_space);
@@ -55,7 +55,7 @@
if (m_kgeoms.size() <= id)
{
m_kgeoms.resize(id + 1);
- m_kgeoms[id].s = dHashSpaceCreate(0);
+ m_kgeoms[id].space = dHashSpaceCreate(0);
}
for (unsigned int j = 0 ; j < m_models[id]->getRobotCount() ; ++j)
@@ -65,11 +65,11 @@
{
kGeom *kg = new kGeom();
kg->link = robot->links[i];
- dGeomID g = createODEGeom(m_kgeoms[id].s, robot->links[i]->shape);
+ dGeomID g = createODEGeom(m_kgeoms[id].space, robot->links[i]->shape);
if (g)
{
kg->geom = g;
- m_kgeoms[id].g.push_back(kg);
+ m_kgeoms[id].geom.push_back(kg);
}
else
delete kg;
@@ -108,12 +108,12 @@
void collision_space::EnvironmentModelODE::updateRobotModel(unsigned int model_id)
{
- const unsigned int n = m_kgeoms[model_id].g.size();
+ const unsigned int n = m_kgeoms[model_id].geom.size();
for (unsigned int i = 0 ; i < n ; ++i)
{
- libTF::Pose3D &pose = m_kgeoms[model_id].g[i]->link->globalTrans;
- dGeomID geom = m_kgeoms[model_id].g[i]->geom;
+ libTF::Pose3D &pose = m_kgeoms[model_id].geom[i]->link->globalTrans;
+ dGeomID geom = m_kgeoms[model_id].geom[i]->geom;
libTF::Pose3D::Position pos = pose.getPosition();
dGeomSetPosition(geom, pos.x, pos.y, pos.z);
@@ -190,7 +190,7 @@
dSpaceID collision_space::EnvironmentModelODE::getModelODESpace(unsigned int model_id) const
{
- return m_kgeoms[model_id].s;
+ return m_kgeoms[model_id].space;
}
struct CollisionData
@@ -216,12 +216,48 @@
{
CollisionData cdata;
cdata.collides = false;
- m_collide2.setup();
- for (int i = m_kgeoms[model_id].g.size() - 1 ; i >= 0 && !cdata.collides ; --i)
- m_collide2.collide(m_kgeoms[model_id].g[i]->geom, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+
+ if (m_selfCollision)
+ {
+ for (int i = m_kgeoms[model_id].selfCollision.size() - 1 ; i >= 0 ; --i)
+ {
+ const std::vector<unsigned int> &vec = m_kgeoms[model_id].selfCollision[i];
+ unsigned int n = vec.size();
+
+ for (unsigned int j = 0 ; j < n ; ++j)
+ for (unsigned int k = j + 1 ; k < n ; ++k)
+ {
+ // dSpaceCollide2 expects AABBs to be computed, so
+ // we force that by calling dGeomGetAABB. Since we
+ // get the data anyway, we attempt to speed things
+ // up using it.
+ dGeomID g1 = m_kgeoms[model_id].geom[vec[j]]->geom;
+ dGeomID g2 = m_kgeoms[model_id].geom[vec[k]]->geom;
+ dReal aabb1[6], aabb2[6];
+ dGeomGetAABB(g1, aabb1);
+ dGeomGetAABB(g2, aabb2);
+ if (!(aabb1[2] > aabb2[3] ||
+ aabb1[3] < aabb2[2] ||
+ aabb1[4] > aabb2[5] ||
+ aabb1[5] < aabb2[4]))
+ dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ if (cdata.collides)
+ {
+ std::cout << m_kgeoms[model_id].geom[vec[j]]->link->name << " intersects with " << m_kgeoms[model_id].geom[vec[k]]->link->name << std::endl;
+ goto OUT;
+ }
+ }
+ }
+ }
- if (m_selfCollision && !cdata.collides)
- dSpaceCollide(m_kgeoms[model_id].s, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ OUT:
+
+ if (!cdata.collides)
+ {
+ m_collide2.setup();
+ for (int i = m_kgeoms[model_id].geom.size() - 1 ; i >= 0 && !cdata.collides ; --i)
+ m_collide2.collide(m_kgeoms[model_id].geom[i]->geom, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ }
return cdata.collides;
}
@@ -246,3 +282,18 @@
m_space = dHashSpaceCreate(0);
m_collide2.setup();
}
+
+void collision_space::EnvironmentModelODE::addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links)
+{
+ if (model_id < m_kgeoms.size())
+ {
+ unsigned int pos = m_kgeoms[model_id].selfCollision.size();
+ m_kgeoms[model_id].selfCollision.resize(pos + 1);
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ {
+ for (unsigned int j = 0 ; j < m_kgeoms[model_id].geom.size() ; ++j)
+ if (m_kgeoms[model_id].geom[j]->link->name == links[i])
+ m_kgeoms[model_id].selfCollision[pos].push_back(j);
+ }
+ }
+}
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-08-25 21:34:51 UTC (rev 3576)
@@ -137,3 +137,8 @@
{
return &m_octree;
}
+
+void collision_space::EnvironmentModelOctree::addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links)
+{
+ fprintf(stderr, "Octree collision checking does not support self collision\n");
+}
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-25 21:34:51 UTC (rev 3576)
@@ -93,7 +93,7 @@
m_collisionSpace = collisionSpace;
else
m_collisionSpace = new collision_space::EnvironmentModelODE();
- m_collisionSpace->setSelfCollision(false);
+ m_collisionSpace->setSelfCollision(true);
m_sphereSize = 0.03;
@@ -115,8 +115,9 @@
if (m_kmodel)
{
m_collisionSpace->lock();
- m_collisionSpace->addRobotModel(m_kmodel);
+ unsigned int cid = m_collisionSpace->addRobotModel(m_kmodel);
m_collisionSpace->unlock();
+ addSelfCollisionGroups(cid, file);
}
}
@@ -133,6 +134,18 @@
collision_space::EnvironmentModel *m_collisionSpace;
double m_sphereSize;
+ void addSelfCollisionGroups(unsigned int cid, robot_desc::URDF *model)
+ {
+ std::vector<robot_desc::URDF::Group*> groups;
+ model->getGroups(groups);
+
+ m_collisionSpace->lock();
+ for (unsigned int i = 0 ; i < groups.size() ; ++i)
+ if (groups[i]->hasFlag("self_collision"))
+ m_collisionSpace->addSelfCollisionGroup(cid, groups[i]->linkNames);
+ m_collisionSpace->unlock();
+ }
+
void worldMapCallback(void)
{
unsigned int n = m_worldCloud.get_pts_size();
Modified: pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-25 21:34:51 UTC (rev 3576)
@@ -157,9 +157,9 @@
if (m_collisionSpace && m_collisionSpace->getModelCount() == 1 && m_follow)
{
- int group = m_collisionSpace->getModel(0)->getGroupID("pr2::base");
+ int group = m_collisionSpace->getRobotModel(0)->getGroupID("pr2::base");
m_collisionSpace->lock();
- m_collisionSpace->getModel(0)->computeTransforms(m_basePos, group);
+ m_collisionSpace->getRobotModel(0)->computeTransforms(m_basePos, group);
m_collisionSpace->updateRobotModel(0);
m_collisionSpace->unlock();
checkCollision();
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-25 21:34:51 UTC (rev 3576)
@@ -22,8 +22,8 @@
</group>
<!-- Define groups to check for self collision -->
-
- <group name="left_arm" flags="self_collision">
+
+ <group name="collisions1" flags="self_collision">
elbow_flex_left
base
</group>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
