Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17448] pkg/trunk
|
From: <is...@us...> - 2009-06-22 19:05:41
|
Revision: 17448
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17448&view=rev
Author: isucan
Date: 2009-06-22 19:05:40 +0000 (Mon, 22 Jun 2009)
Log Message:
-----------
speedup for self collision checking
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_both_arms.yaml
pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_left_arm.yaml
pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_right_arm.yaml
pkg/trunk/world_models/collision_space/include/collision_space/environment.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/environmentODE.cpp
pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-22 19:05:40 UTC (rev 17448)
@@ -128,7 +128,7 @@
}
}
else
- ROS_ERROR("Robot model '%s' not found!", description_.c_str());
+ ROS_ERROR("Robot model '%s' not found! Did you remap 'robot_description'?", description_.c_str());
}
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_both_arms.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_both_arms.yaml 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_both_arms.yaml 2009-06-22 19:05:40 UTC (rev 17448)
@@ -7,6 +7,8 @@
torso_lift_link
head_pan_link
head_tilt_link
+ laser_tilt_mount_link
+ base_laser
r_shoulder_pan_link
r_shoulder_lift_link
r_upper_arm_roll_link
@@ -42,12 +44,12 @@
## -- for right arm; self-collision if any link in 'a' collides with some link in 'b'
scg_r:
a: r_forearm_link r_gripper_palm_link r_gripper_l_finger_link r_gripper_r_finger_link r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link
- b: base_link torso_lift_link laser_tilt_mount_link head_tilt_link
+ b: base_link base_laser torso_lift_link laser_tilt_mount_link head_tilt_link
## -- for left arm; self-collision if any link in 'a' collides with some link in 'b'
scg_l:
a: l_forearm_link l_gripper_palm_link l_gripper_l_finger_link l_gripper_r_finger_link l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link
- b: base_link torso_lift_link laser_tilt_mount_link head_tilt_link
+ b: base_link base_laser torso_lift_link laser_tilt_mount_link head_tilt_link
## for arms with each other; self-collision if any link in 'a' collides with some link in 'b'
scg_lr:
@@ -84,6 +86,7 @@
r_shoulder_pan_link
r_shoulder_lift_link
torso_lift_link
+ base_laser
base_link
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_left_arm.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_left_arm.yaml 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_left_arm.yaml 2009-06-22 19:05:40 UTC (rev 17448)
@@ -6,7 +6,9 @@
base_link
torso_lift_link
head_pan_link
- head_tilt_link
+ head_tilt_link
+ laser_tilt_mount_link
+ base_laser
l_shoulder_pan_link
l_shoulder_lift_link
l_upper_arm_roll_link
@@ -28,7 +30,7 @@
## -- for left arm; self-collision if any link in 'a' collides with some link in 'b'
scg_l:
a: l_forearm_link l_gripper_palm_link l_gripper_l_finger_link l_gripper_r_finger_link l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link
- b: base_link torso_lift_link laser_tilt_mount_link head_tilt_link
+ b: base_link base_laser torso_lift_link laser_tilt_mount_link head_tilt_link
## list of links that the robot can see with its sensors (used to remove
## parts of the robot from scanned data)
@@ -47,6 +49,7 @@
l_shoulder_pan_link
l_shoulder_lift_link
torso_lift_link
+ base_laser
base_link
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_right_arm.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_right_arm.yaml 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_right_arm.yaml 2009-06-22 19:05:40 UTC (rev 17448)
@@ -7,6 +7,8 @@
torso_lift_link
head_pan_link
head_tilt_link
+ laser_tilt_mount_link
+ base_laser
r_shoulder_pan_link
r_shoulder_lift_link
r_upper_arm_roll_link
@@ -28,7 +30,7 @@
## -- for right arm; self-collision if any link in 'a' collides with some link in 'b'
scg_r:
a: r_forearm_link r_gripper_palm_link r_gripper_l_finger_link r_gripper_r_finger_link r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link
- b: base_link torso_lift_link laser_tilt_mount_link head_tilt_link
+ b: base_link base_laser torso_lift_link laser_tilt_mount_link head_tilt_link
## list of links that the robot can see with its sensors (used to remove
## parts of the robot from scanned data)
@@ -47,6 +49,7 @@
r_shoulder_pan_link
r_shoulder_lift_link
torso_lift_link
+ base_laser
base_link
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-06-22 19:05:40 UTC (rev 17448)
@@ -93,7 +93,7 @@
bool getSelfCollision(void) const;
/** Add a group of links to be checked for self collision */
- virtual void addSelfCollisionGroup(std::vector<std::string> &links) = 0;
+ virtual void addSelfCollisionGroup(std::vector<std::string> &links);
/** Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
virtual int setCollisionCheck(const std::string &link, bool state) = 0;
@@ -123,7 +123,10 @@
/** Check if a model is in collision. Contacts are not computed */
virtual bool isCollision(void) = 0;
-
+
+ /** Check for self collision. Contacts are not computed */
+ virtual bool isSelfCollision(void) = 0;
+
/** Get the list of contacts (collisions) */
virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1) = 0;
@@ -161,6 +164,10 @@
protected:
boost::mutex m_lock;
+ std::vector<std::string> m_collisionLinks;
+ std::map<std::string, unsigned int> m_collisionLinkIndex;
+ std::vector< std::vector<bool> > m_selfCollisionTest;
+
bool m_selfCollision;
bool m_verbose;
planning_models::msg::Interface m_msg;
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-06-22 19:05:40 UTC (rev 17448)
@@ -42,10 +42,13 @@
namespace collision_space
{
-
+
/** A class describing an environment for a kinematic robot using ODE */
class EnvironmentModelODE : public EnvironmentModel
- {
+ {
+
+ friend void nearCallbackFn(void *data, dGeomID o1, dGeomID o2);
+
public:
EnvironmentModelODE(void) : EnvironmentModel()
@@ -78,7 +81,10 @@
/** Check if a model is in collision */
virtual bool isCollision(void);
-
+
+ /** Check if a model is in self collision */
+ virtual bool isSelfCollision(void);
+
/** Remove all obstacles from collision model */
virtual void clearObstacles(void);
@@ -220,8 +226,9 @@
std::vector<dGeomID> geom;
bool enabled;
planning_models::KinematicModel::Link *link;
+ unsigned int index;
};
-
+
struct ModelInfo
{
std::vector< kGeom* > linkGeom;
Modified: pkg/trunk/world_models/collision_space/src/environment.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment.cpp 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/world_models/collision_space/src/environment.cpp 2009-06-22 19:05:40 UTC (rev 17448)
@@ -44,8 +44,37 @@
void collision_space::EnvironmentModel::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
m_robotModel = model;
+ m_collisionLinks = links;
+ m_selfCollisionTest.resize(links.size());
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ {
+ m_selfCollisionTest[i].resize(links.size(), false);
+ m_collisionLinkIndex[links[i]] = i;
+ }
}
+void collision_space::EnvironmentModel::addSelfCollisionGroup(std::vector<std::string> &links)
+{
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ {
+ if (m_collisionLinkIndex.find(links[i]) == m_collisionLinkIndex.end())
+ {
+ m_msg.error("Unknown link '%s'", links[i].c_str());
+ continue;
+ }
+ for (unsigned int j = i + 1 ; j < links.size() ; ++j)
+ {
+ if (m_collisionLinkIndex.find(links[j]) == m_collisionLinkIndex.end())
+ {
+ m_msg.error("Unknown link '%s'", links[j].c_str());
+ continue;
+ }
+ m_selfCollisionTest[m_collisionLinkIndex[links[i]]][m_collisionLinkIndex[links[j]]] = true;
+ m_selfCollisionTest[m_collisionLinkIndex[links[j]]][m_collisionLinkIndex[links[i]]] = true;
+ }
+ }
+}
+
void collision_space::EnvironmentModel::lock(void)
{
m_lock.lock();
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-06-22 19:05:40 UTC (rev 17448)
@@ -56,41 +56,35 @@
{
collision_space::EnvironmentModel::addRobotModel(model, links, scale, padding);
- m_modelGeom.space = dHashSpaceCreate(0);
+ m_modelGeom.space = dSweepAndPruneSpaceCreate(0, dSAP_AXES_XZY);
m_modelGeom.scale = scale;
m_modelGeom.padding = padding;
- std::map<std::string, bool> exists;
for (unsigned int i = 0 ; i < links.size() ; ++i)
- exists[links[i]] = true;
-
- for (unsigned int j = 0 ; j < m_robotModel->getRobotCount() ; ++j)
{
- planning_models::KinematicModel::Robot *robot = m_robotModel->getRobot(j);
- for (unsigned int i = 0 ; i < robot->links.size() ; ++i)
+ /* skip this link if we have no geometry or if the link
+ name is not specified as enabled for collision
+ checking */
+ planning_models::KinematicModel::Link *link = m_robotModel->getLink(links[i]);
+ if (!link || !link->shape)
+ continue;
+
+ kGeom *kg = new kGeom();
+ kg->link = link;
+ kg->enabled = true;
+ kg->index = i;
+ dGeomID g = createODEGeom(m_modelGeom.space, 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)
{
- /* skip this link if we have no geometry or if the link
- name is not specified as enabled for collision
- checking */
- if (!robot->links[i]->shape)
- continue;
- if (exists.find(robot->links[i]->name) == exists.end())
- continue;
-
- kGeom *kg = new kGeom();
- kg->link = robot->links[i];
- kg->enabled = true;
- dGeomID g = createODEGeom(m_modelGeom.space, robot->links[i]->shape, scale, padding);
- assert(g);
- 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);
- assert(ga);
- kg->geom.push_back(ga);
- }
- m_modelGeom.linkGeom.push_back(kg);
+ dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, scale, padding);
+ assert(ga);
+ dGeomSetData(ga, NULL);
+ kg->geom.push_back(ga);
}
+ m_modelGeom.linkGeom.push_back(kg);
}
}
@@ -150,6 +144,7 @@
{
dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, m_modelGeom.scale, m_modelGeom.padding);
assert(ga);
+ dGeomSetData(ga, NULL);
kg->geom.push_back(ga);
}
}
@@ -299,81 +294,96 @@
return m_modelGeom.space;
}
-struct CollisionData
+namespace collision_space
{
- CollisionData(void)
+
+ struct CollisionData
{
- done = false;
- collides = false;
- max_contacts = 0;
- contacts = NULL;
- link1 = link2 = NULL;
- }
+ 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;
+ };
- bool done;
-
- bool collides;
- unsigned int max_contacts;
- std::vector<collision_space::EnvironmentModelODE::Contact> *contacts;
-
- planning_models::KinematicModel::Link *link1;
- planning_models::KinematicModel::Link *link2;
-};
-
-static void nearCallbackFn(void *data, dGeomID o1, dGeomID o2)
-{
- CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
-
- if (cdata->done)
- return;
-
- if (cdata->contacts)
+ void nearCallbackFn(void *data, dGeomID o1, dGeomID o2)
{
- static const int MAX_CONTACTS = 3;
- dContact contact[MAX_CONTACTS];
- int numc = dCollide (o1, o2, MAX_CONTACTS,
- &contact[0].geom, sizeof(dContact));
- if (numc)
+ CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
+
+ if (cdata->done)
+ return;
+
+ if (cdata->selfCollisionTest)
{
- cdata->collides = true;
- for (int i = 0 ; i < numc ; ++i)
+ EnvironmentModelODE::kGeom* kg1 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o1));
+ EnvironmentModelODE::kGeom* kg2 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o2));
+ if (kg1 && kg2)
+ if (cdata->selfCollisionTest->at(kg1->index)[kg2->index] == false)
+ return;
+ }
+
+ if (cdata->contacts)
+ {
+ static const int MAX_CONTACTS = 3;
+ dContact contact[MAX_CONTACTS];
+ int numc = dCollide (o1, o2, MAX_CONTACTS,
+ &contact[0].geom, sizeof(dContact));
+ if (numc)
{
- if (cdata->contacts->size() < cdata->max_contacts)
+ cdata->collides = true;
+ for (int i = 0 ; i < numc ; ++i)
{
- collision_space::EnvironmentModelODE::Contact add;
-
- add.pos.setX(contact[i].geom.pos[0]);
- add.pos.setY(contact[i].geom.pos[1]);
- add.pos.setZ(contact[i].geom.pos[2]);
-
- add.normal.setX(contact[i].geom.normal[0]);
- add.normal.setY(contact[i].geom.normal[1]);
- add.normal.setZ(contact[i].geom.normal[2]);
-
- add.depth = contact[i].geom.depth;
-
- add.link1 = cdata->link1;
- add.link2 = cdata->link2;
-
- cdata->contacts->push_back(add);
+ if (cdata->contacts->size() < cdata->max_contacts)
+ {
+ collision_space::EnvironmentModelODE::Contact add;
+
+ add.pos.setX(contact[i].geom.pos[0]);
+ add.pos.setY(contact[i].geom.pos[1]);
+ add.pos.setZ(contact[i].geom.pos[2]);
+
+ add.normal.setX(contact[i].geom.normal[0]);
+ add.normal.setY(contact[i].geom.normal[1]);
+ add.normal.setZ(contact[i].geom.normal[2]);
+
+ add.depth = contact[i].geom.depth;
+
+ add.link1 = cdata->link1;
+ add.link2 = cdata->link2;
+
+ cdata->contacts->push_back(add);
+ }
+ else
+ break;
}
- else
- break;
}
+ if (cdata->contacts->size() >= cdata->max_contacts)
+ cdata->done = true;
}
- if (cdata->contacts->size() >= cdata->max_contacts)
- cdata->done = true;
- }
- else
- {
- static const int MAX_CONTACTS = 1;
- dContact contact[MAX_CONTACTS];
- int numc = dCollide (o1, o2, MAX_CONTACTS,
- &contact[0].geom, sizeof(dContact));
- if (numc)
+ else
{
- cdata->collides = true;
- cdata->done = true;
+ static const int MAX_CONTACTS = 1;
+ dContact contact[MAX_CONTACTS];
+ int numc = dCollide (o1, o2, MAX_CONTACTS,
+ &contact[0].geom, sizeof(dContact));
+ if (numc)
+ {
+ cdata->collides = true;
+ cdata->done = true;
+ }
}
}
}
@@ -381,6 +391,7 @@
bool collision_space::EnvironmentModelODE::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count)
{
CollisionData cdata;
+ cdata.selfCollisionTest = &m_selfCollisionTest;
cdata.contacts = &contacts;
cdata.max_contacts = max_count;
contacts.clear();
@@ -391,58 +402,24 @@
bool collision_space::EnvironmentModelODE::isCollision(void)
{
CollisionData cdata;
+ cdata.selfCollisionTest = &m_selfCollisionTest;
testCollision(reinterpret_cast<void*>(&cdata));
return cdata.collides;
}
+bool collision_space::EnvironmentModelODE::isSelfCollision(void)
+{
+ CollisionData cdata;
+ cdata.selfCollisionTest = &m_selfCollisionTest;
+ testSelfCollision(reinterpret_cast<void*>(&cdata));
+ return cdata.collides;
+}
+
void collision_space::EnvironmentModelODE::testSelfCollision(void *data)
{
- CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
- for (int i = m_modelGeom.selfCollision.size() - 1 ; i >= 0 && !cdata->done ; --i)
- {
- const std::vector<unsigned int> &vec = m_modelGeom.selfCollision[i];
- unsigned int n = vec.size();
-
- for (unsigned int j = 0 ; j < n && !cdata->done ; ++j)
- {
- cdata->link1 = m_modelGeom.linkGeom[vec[j]]->link;
- const unsigned int njg = m_modelGeom.linkGeom[vec[j]]->geom.size();
-
- for (unsigned int k = j + 1 ; k < n && !cdata->done; ++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.
-
- const unsigned int nkg = m_modelGeom.linkGeom[vec[k]]->geom.size();
- cdata->link2 = m_modelGeom.linkGeom[vec[k]]->link;
-
- /* this will account for attached bodies as well */
- for (unsigned int jg = 0 ; jg < njg && !cdata->done; ++jg)
- for (unsigned int kg = 0 ; kg < nkg && !cdata->done; ++kg)
- {
- dGeomID g1 = m_modelGeom.linkGeom[vec[j]]->geom[jg];
- dGeomID g2 = m_modelGeom.linkGeom[vec[k]]->geom[kg];
-
- 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, data, nearCallbackFn);
-
- if (cdata->collides && m_verbose)
- m_msg.inform("Self-collision between '%s' and '%s'\n",
- m_modelGeom.linkGeom[vec[j]]->link->name.c_str(), m_modelGeom.linkGeom[vec[k]]->link->name.c_str());
- }
- }
- }
- }
+ dSpaceCollide(m_modelGeom.space, data, nearCallbackFn);
}
+
void collision_space::EnvironmentModelODE::testStaticBodyCollision(void *data)
{
CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
@@ -549,6 +526,8 @@
void collision_space::EnvironmentModelODE::addSelfCollisionGroup(std::vector<std::string> &links)
{
+ EnvironmentModel::addSelfCollisionGroup(links);
+
unsigned int pos = m_modelGeom.selfCollision.size();
m_modelGeom.selfCollision.resize(pos + 1);
for (unsigned int i = 0 ; i < links.size() ; ++i)
@@ -557,7 +536,6 @@
if (m_modelGeom.linkGeom[j]->link->name == links[i])
m_modelGeom.selfCollision[pos].push_back(j);
}
-
}
int collision_space::EnvironmentModelODE::setCollisionCheck(const std::string &link, bool state)
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-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp 2009-06-22 19:05:40 UTC (rev 17448)
@@ -103,6 +103,11 @@
for (unsigned int i = 0 ; i < K ; ++i)
em->isCollision();
ROS_INFO("%f collision tests per second (with self collision checking)", (double)K/(ros::WallTime::now() - tm).toSec());
+
+ tm = ros::WallTime::now();
+ for (unsigned int i = 0 ; i < K ; ++i)
+ em->isSelfCollision();
+ ROS_INFO("%f collision tests per second (only self collision checking)", (double)K/(ros::WallTime::now() - tm).toSec());
}
protected:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
