Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16815] pkg/trunk
|
From: <is...@us...> - 2009-06-07 00:04:56
|
Revision: 16815
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16815&view=rev
Author: isucan
Date: 2009-06-07 00:04:10 +0000 (Sun, 07 Jun 2009)
Log Message:
-----------
removed support for multiple robots from collision space since the planning model can support that as well
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequest.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_validator.cpp
pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
pkg/trunk/util/self_watch/self_watch.cpp
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/collision_space/environment.cpp
pkg/trunk/world_models/collision_space/src/collision_space/environmentODE.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequest.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequest.h 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequest.h 2009-06-07 00:04:10 UTC (rev 16815)
@@ -290,7 +290,7 @@
{
/* set the pose of the whole robot */
psetup->model->kmodel->computeTransforms(&start_state.vals[0]);
- psetup->model->collisionSpace->updateRobotModel(psetup->model->collisionSpaceID);
+ psetup->model->collisionSpace->updateRobotModel();
/* extract the components needed for the start state of the desired group */
for (unsigned int i = 0 ; i < dim ; ++i)
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-06-07 00:04:10 UTC (rev 16815)
@@ -126,7 +126,7 @@
void update(const ompl::sb::State *state) const
{
m_model->kmodel->computeTransformsGroup(state->values, m_model->groupID);
- m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
+ m_model->collisionSpace->updateRobotModel();
}
mutable RKPModelBase *m_model;
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h 2009-06-07 00:04:10 UTC (rev 16815)
@@ -51,8 +51,7 @@
public:
RKPModelBase(void)
{
- groupID = -1;
- collisionSpaceID = 0;
+ groupID = -1;
}
virtual ~RKPModelBase(void)
@@ -61,7 +60,6 @@
boost::mutex lock;
boost::shared_ptr<collision_space::EnvironmentModel> collisionSpace;
- unsigned int collisionSpaceID;
boost::shared_ptr<planning_models::KinematicModel> kmodel;
std::string groupName;
int groupID;
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h 2009-06-07 00:04:10 UTC (rev 16815)
@@ -59,9 +59,9 @@
{
m_model->kmodel->lock();
m_model->kmodel->computeTransformsGroup(static_cast<const ompl::sb::State*>(state)->values, m_model->groupID);
- m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
+ m_model->collisionSpace->updateRobotModel();
- bool valid = !m_model->collisionSpace->isCollision(m_model->collisionSpaceID);
+ bool valid = !m_model->collisionSpace->isCollision();
if (valid)
valid = m_kce.decide();
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -54,8 +54,7 @@
void kinematic_planning::CollisionSpaceMonitor::attachObjectCallback(const robot_msgs::AttachedObjectConstPtr &attachedObject)
{
m_collisionSpace->lock();
- int model_id = m_collisionSpace->getModelID(attachedObject->robot_name);
- planning_models::KinematicModel::Link *link = model_id >= 0 ? m_kmodel->getLink(attachedObject->link_name) : NULL;
+ planning_models::KinematicModel::Link *link = m_kmodel->getLink(attachedObject->link_name);
if (link)
{
@@ -89,7 +88,7 @@
}
// update the collision model
- m_collisionSpace->updateAttachedBodies(model_id);
+ m_collisionSpace->updateAttachedBodies();
ROS_INFO("Link '%s' on '%s' has %d objects attached", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str(), n);
}
else
@@ -102,11 +101,7 @@
bool kinematic_planning::CollisionSpaceMonitor::setCollisionState(motion_planning_srvs::CollisionCheckState::Request &req, motion_planning_srvs::CollisionCheckState::Response &res)
{
m_collisionSpace->lock();
- int model_id = m_collisionSpace->getModelID(req.robot_name);
- if (model_id >= 0)
- res.value = m_collisionSpace->setCollisionCheck(model_id, req.link_name, req.value ? true : false);
- else
- res.value = -1;
+ res.value = m_collisionSpace->setCollisionCheck(req.link_name, req.value ? true : false);
m_collisionSpace->unlock();
if (res.value == -1)
ROS_WARN("Unable to change collision checking state for link '%s' on '%s'", req.link_name.c_str(), req.robot_name.c_str());
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -499,7 +499,6 @@
/* set the data for the model */
RKPModel *model = new RKPModel();
- model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
model->groupName = m_kmodel->getModelName();
@@ -515,7 +514,6 @@
for (unsigned int i = 0 ; i < groups.size() ; ++i)
{
RKPModel *model = new RKPModel();
- model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
model->groupID = m_kmodel->getGroupID(groups[i]);
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_validator.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_validator.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -163,7 +163,7 @@
{
/* set the pose of the whole robot to the current state */
model->kmodel->computeTransforms(m_robotState->getParams());
- model->collisionSpace->updateRobotModel(model->collisionSpaceID);
+ model->collisionSpace->updateRobotModel();
/* extract the components needed for the start state of the desired group */
for (unsigned int i = 0 ; i < dim ; ++i)
@@ -229,7 +229,7 @@
{
/* set the pose of the whole robot */
model->kmodel->computeTransforms(&req.start_state.vals[0]);
- model->collisionSpace->updateRobotModel(model->collisionSpaceID);
+ model->collisionSpace->updateRobotModel();
/* extract the components needed for the start state of the desired group */
for (unsigned int i = 0 ; i < dim ; ++i)
@@ -281,7 +281,6 @@
/* set the data for the model */
myModel *model = new myModel();
- model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
model->groupName = m_kmodel->getModelName();
@@ -297,7 +296,6 @@
for (unsigned int i = 0 ; i < groups.size() ; ++i)
{
myModel *model = new myModel();
- model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
model->groupID = m_kmodel->getGroupID(groups[i]);
Modified: pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -127,12 +127,12 @@
{
CollisionSpaceMonitor::stateUpdate();
- if (m_collisionSpace && m_collisionSpace->getModelCount() == 1)
+ if (m_collisionSpace)
{
m_collisionSpace->lock();
m_kmodel->computeTransforms(m_robotState->getParams());
- m_collisionSpace->updateRobotModel(0);
- bool invalid = m_collisionSpace->isCollision(0);
+ m_collisionSpace->updateRobotModel();
+ bool invalid = m_collisionSpace->isCollision();
m_collisionSpace->unlock();
std_msgs::Byte msg;
msg.data = invalid ? 0 : 1;
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -44,10 +44,10 @@
if (urdf_.get() && kmodel_.get())
{
ode_collision_model_->lock();
- unsigned int cid = ode_collision_model_->addRobotModel(kmodel_, collision_check_links_, scale_, padd_);
+ ode_collision_model_->addRobotModel(kmodel_, collision_check_links_, scale_, padd_);
for (unsigned int i = 0 ; i < self_collision_check_groups_.size() ; ++i)
- ode_collision_model_->addSelfCollisionGroup(cid, self_collision_check_groups_[i]);
- ode_collision_model_->updateRobotModel(cid);
+ ode_collision_model_->addSelfCollisionGroup(self_collision_check_groups_[i]);
+ ode_collision_model_->updateRobotModel();
ode_collision_model_->unlock();
}
}
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -155,11 +155,11 @@
m_kmodel->computeTransforms(m_robotState->getParams());
// ask the collision space to look at the updates
- m_collisionSpace->updateRobotModel(0);
+ m_collisionSpace->updateRobotModel();
// get the first contact point
std::vector<collision_space::EnvironmentModel::Contact> contacts;
- m_collisionSpace->getCollisionContacts(0, contacts, 1);
+ m_collisionSpace->getCollisionContacts(contacts, 1);
if (contacts.size() > 0)
{
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-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-06-07 00:04:10 UTC (rev 16815)
@@ -53,10 +53,9 @@
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 set of kinematic
- robots. The class provides functionality for checking whether a
+ can change (removed by clearObstacles()) and a kinematic
+ robot model. The class provides functionality for checking whether a
given robot is in collision.
-
*/
@@ -99,10 +98,10 @@
bool getSelfCollision(void) const;
/** Add a group of links to be checked for self collision */
- virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links) = 0;
+ virtual void addSelfCollisionGroup(std::vector<std::string> &links) = 0;
/** Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
- virtual int setCollisionCheck(unsigned int model_id, const std::string &link, bool state) = 0;
+ virtual int setCollisionCheck(const std::string &link, bool state) = 0;
/** Add a robot model. Ignore robot links if their name is not
specified in the string vector. The scale argument can be
@@ -110,34 +109,28 @@
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual unsigned int 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 addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** Update the positions of the geometry used in collision detection */
- virtual void updateRobotModel(unsigned int model_id) = 0;
+ virtual void updateRobotModel(void) = 0;
/** Update the set of bodies that are attached to the robot (re-creates them) */
- virtual void updateAttachedBodies(unsigned int model_id) = 0;
+ virtual void updateAttachedBodies(void) = 0;
- /** Get the number of loaded models */
- unsigned int getModelCount(void) const;
-
- /** Get a specific model */
- boost::shared_ptr<planning_models::KinematicModel> getRobotModel(unsigned int model_id) const;
+ /** Get the robot model */
+ boost::shared_ptr<planning_models::KinematicModel> getRobotModel(void) const;
- /** Get the model ID based on the model (robot) name; returns -1 if model not found. */
- int getModelID(const std::string& robot_name) const;
-
/**********************************************************************/
/* Collision Checking Routines */
/**********************************************************************/
- /** Check if a model is in collision */
- virtual bool isCollision(unsigned int model_id) = 0;
+ /** Check if a model is in collision. Contacts are not computed */
+ virtual bool isCollision(void) = 0;
/** Get the list of contacts (collisions) */
- virtual bool getCollisionContacts(unsigned int model_id, std::vector<Contact> &contacts, unsigned int max_count = 1) = 0;
+ virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1) = 0;
/**********************************************************************/
@@ -172,14 +165,13 @@
protected:
- boost::mutex m_lock;
- bool m_selfCollision;
- bool m_verbose;
- msg::Interface m_msg;
+ boost::mutex m_lock;
+ bool m_selfCollision;
+ bool m_verbose;
+ msg::Interface m_msg;
/** List of loaded robot models */
- std::vector< boost::shared_ptr<planning_models::KinematicModel> >
- m_models;
+ boost::shared_ptr<planning_models::KinematicModel> m_robotModel;
};
}
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-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-06-07 00:04:10 UTC (rev 16815)
@@ -73,14 +73,14 @@
/** Return the space ID for the space in which static objects are added */
dSpaceID getODEBasicGeomSpace(void) const;
- /** Return the space ID for the space in which the particular model is instanciated */
- dSpaceID getModelODESpace(unsigned int model_id) const;
+ /** Return the space ID for the space in which the robot model is instanciated */
+ dSpaceID getModelODESpace(void) const;
/** Get the list of contacts (collisions) */
- virtual bool getCollisionContacts(unsigned int model_id, std::vector<Contact> &contacts, unsigned int max_count = 1);
+ virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1);
/** Check if a model is in collision */
- virtual bool isCollision(unsigned int model_id);
+ virtual bool isCollision(void);
/** Remove all obstacles from collision model */
virtual void clearObstacles(void);
@@ -97,24 +97,24 @@
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual unsigned int 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 addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** Update the positions of the geometry used in collision detection */
- virtual void updateRobotModel(unsigned int model_id);
+ virtual void updateRobotModel(void);
/** Update the set of bodies that are attached to the robot (re-creates them) */
- virtual void updateAttachedBodies(unsigned int model_id);
+ virtual void updateAttachedBodies(void);
/** Add a group of links to be checked for self collision */
- virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links);
+ 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(unsigned int model_id, const std::string &link, bool state);
+ virtual int setCollisionCheck(const std::string &link, bool state);
protected:
/** Internal function for collision detection */
- void testCollision(unsigned int model_id, void *data);
+ void testCollision(void *data);
class ODECollide2
{
@@ -235,15 +235,15 @@
void updateGeom(dGeomID geom, btTransform &pose) const;
void freeMemory(void);
- std::vector<ModelInfo> m_modelsGeom;
- dSpaceID m_space;
- dSpaceID m_spaceBasicGeoms;
+ ModelInfo m_modelGeom;
+ dSpaceID m_space;
+ dSpaceID m_spaceBasicGeoms;
/* 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;
+ ODECollide2 m_collide2;
/* 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;
+ std::vector<dGeomID> m_basicGeoms;
};
}
Modified: pkg/trunk/world_models/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/collision_space/environment.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/world_models/collision_space/src/collision_space/environment.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -41,11 +41,9 @@
m_verbose = verbose;
}
-unsigned int 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::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
- unsigned int pos = m_models.size();
- m_models.push_back(model);
- return pos;
+ m_robotModel = model;
}
void collision_space::EnvironmentModel::lock(void)
@@ -67,22 +65,3 @@
{
return m_selfCollision;
}
-
-
-unsigned int collision_space::EnvironmentModel::getModelCount(void) const
-{
- return m_models.size();
-}
-
-int collision_space::EnvironmentModel::getModelID(const std::string& robot_name) const
-{
- for (unsigned int i = 0 ; i < m_models.size() ; ++i)
- if (m_models[i]->getModelName() == robot_name)
- return i;
- return -1;
-}
-
-boost::shared_ptr<planning_models::KinematicModel> collision_space::EnvironmentModel::getRobotModel(unsigned int model_id) const
-{
- return m_models[model_id];
-}
Modified: pkg/trunk/world_models/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/collision_space/environmentODE.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/world_models/collision_space/src/collision_space/environmentODE.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -42,37 +42,31 @@
void collision_space::EnvironmentModelODE::freeMemory(void)
{
- for (unsigned int i = 0 ; i < m_modelsGeom.size() ; ++i)
- {
- for (unsigned int j = 0 ; j < m_modelsGeom[i].linkGeom.size() ; ++j)
- delete m_modelsGeom[i].linkGeom[j];
- dSpaceDestroy(m_modelsGeom[i].space);
- }
+ for (unsigned int j = 0 ; j < m_modelGeom.linkGeom.size() ; ++j)
+ 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);
}
-unsigned int 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::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
- unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links, scale, padding);
+ collision_space::EnvironmentModel::addRobotModel(model, links, scale, padding);
- if (m_modelsGeom.size() <= id)
- {
- m_modelsGeom.resize(id + 1);
- m_modelsGeom[id].space = dHashSpaceCreate(0);
- m_modelsGeom[id].scale = scale;
- m_modelsGeom[id].padding = padding;
- }
+ m_modelGeom.space = dHashSpaceCreate(0);
+ 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_models[id]->getRobotCount() ; ++j)
+ for (unsigned int j = 0 ; j < m_robotModel->getRobotCount() ; ++j)
{
- planning_models::KinematicModel::Robot *robot = m_models[id]->getRobot(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
@@ -86,19 +80,18 @@
kGeom *kg = new kGeom();
kg->link = robot->links[i];
kg->enabled = true;
- dGeomID g = createODEGeom(m_modelsGeom[id].space, robot->links[i]->shape, scale, padding);
+ 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_modelsGeom[id].space, kg->link->attachedBodies[k]->shape, scale, padding);
+ dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, scale, padding);
assert(ga);
kg->geom.push_back(ga);
}
- m_modelsGeom[id].linkGeom.push_back(kg);
+ m_modelGeom.linkGeom.push_back(kg);
}
}
- return id;
}
dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, planning_models::shapes::Shape *shape, double scale, double padding) const
@@ -139,12 +132,12 @@
dGeomSetQuaternion(geom, q);
}
-void collision_space::EnvironmentModelODE::updateAttachedBodies(unsigned int model_id)
+void collision_space::EnvironmentModelODE::updateAttachedBodies(void)
{
- const unsigned int n = m_modelsGeom[model_id].linkGeom.size();
+ const unsigned int n = m_modelGeom.linkGeom.size();
for (unsigned int i = 0 ; i < n ; ++i)
{
- kGeom *kg = m_modelsGeom[model_id].linkGeom[i];
+ kGeom *kg = m_modelGeom.linkGeom[i];
/* clear previosly attached bodies */
for (unsigned int k = 1 ; k < kg->geom.size() ; ++k)
@@ -155,23 +148,23 @@
const unsigned int nab = kg->link->attachedBodies.size();
for (unsigned int k = 0 ; k < nab ; ++k)
{
- dGeomID ga = createODEGeom(m_modelsGeom[model_id].space, kg->link->attachedBodies[k]->shape, m_modelsGeom[model_id].scale, m_modelsGeom[model_id].padding);
+ dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, m_modelGeom.scale, m_modelGeom.padding);
assert(ga);
kg->geom.push_back(ga);
}
}
}
-void collision_space::EnvironmentModelODE::updateRobotModel(unsigned int model_id)
+void collision_space::EnvironmentModelODE::updateRobotModel(void)
{
- const unsigned int n = m_modelsGeom[model_id].linkGeom.size();
+ const unsigned int n = m_modelGeom.linkGeom.size();
for (unsigned int i = 0 ; i < n ; ++i)
{
- updateGeom(m_modelsGeom[model_id].linkGeom[i]->geom[0], m_modelsGeom[model_id].linkGeom[i]->link->globalTrans);
- const unsigned int nab = m_modelsGeom[model_id].linkGeom[i]->link->attachedBodies.size();
+ updateGeom(m_modelGeom.linkGeom[i]->geom[0], m_modelGeom.linkGeom[i]->link->globalTrans);
+ const unsigned int nab = m_modelGeom.linkGeom[i]->link->attachedBodies.size();
for (unsigned int k = 0 ; k < nab ; ++k)
- updateGeom(m_modelsGeom[model_id].linkGeom[i]->geom[k + 1], m_modelsGeom[model_id].linkGeom[i]->link->attachedBodies[k]->globalTrans);
+ updateGeom(m_modelGeom.linkGeom[i]->geom[k + 1], m_modelGeom.linkGeom[i]->link->attachedBodies[k]->globalTrans);
}
}
@@ -301,9 +294,9 @@
return m_spaceBasicGeoms;
}
-dSpaceID collision_space::EnvironmentModelODE::getModelODESpace(unsigned int model_id) const
+dSpaceID collision_space::EnvironmentModelODE::getModelODESpace(void) const
{
- return m_modelsGeom[model_id].space;
+ return m_modelGeom.space;
}
struct CollisionData
@@ -385,39 +378,39 @@
}
}
-bool collision_space::EnvironmentModelODE::getCollisionContacts(unsigned int model_id, std::vector<Contact> &contacts, unsigned int max_count)
+bool collision_space::EnvironmentModelODE::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count)
{
CollisionData cdata;
cdata.contacts = &contacts;
cdata.max_contacts = max_count;
contacts.clear();
- testCollision(model_id, reinterpret_cast<void*>(&cdata));
+ testCollision(reinterpret_cast<void*>(&cdata));
return cdata.collides;
}
-bool collision_space::EnvironmentModelODE::isCollision(unsigned int model_id)
+bool collision_space::EnvironmentModelODE::isCollision(void)
{
CollisionData cdata;
- testCollision(model_id, reinterpret_cast<void*>(&cdata));
+ testCollision(reinterpret_cast<void*>(&cdata));
return cdata.collides;
}
-void collision_space::EnvironmentModelODE::testCollision(unsigned int model_id, void *data)
+void collision_space::EnvironmentModelODE::testCollision(void *data)
{
CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
/* check self collision */
if (m_selfCollision)
{
- for (int i = m_modelsGeom[model_id].selfCollision.size() - 1 ; i >= 0 && !cdata->done ; --i)
+ for (int i = m_modelGeom.selfCollision.size() - 1 ; i >= 0 && !cdata->done ; --i)
{
- const std::vector<unsigned int> &vec = m_modelsGeom[model_id].selfCollision[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_modelsGeom[model_id].linkGeom[vec[j]]->link;
- const unsigned int njg = m_modelsGeom[model_id].linkGeom[vec[j]]->geom.size();
+ 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)
{
@@ -426,15 +419,15 @@
// get the data anyway, we attempt to speed things
// up using it.
- const unsigned int nkg = m_modelsGeom[model_id].linkGeom[vec[k]]->geom.size();
- cdata->link2 = m_modelsGeom[model_id].linkGeom[vec[k]]->link;
+ 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_modelsGeom[model_id].linkGeom[vec[j]]->geom[jg];
- dGeomID g2 = m_modelsGeom[model_id].linkGeom[vec[k]]->geom[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);
@@ -448,7 +441,7 @@
if (cdata->collides && m_verbose)
m_msg.inform("Self-collision between '%s' and '%s'\n",
- m_modelsGeom[model_id].linkGeom[vec[j]]->link->name.c_str(), m_modelsGeom[model_id].linkGeom[vec[k]]->link->name.c_str());
+ m_modelGeom.linkGeom[vec[j]]->link->name.c_str(), m_modelGeom.linkGeom[vec[k]]->link->name.c_str());
}
}
}
@@ -460,17 +453,17 @@
if (!cdata->done)
{
cdata->link2 = NULL;
- for (int i = m_modelsGeom[model_id].linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
+ for (int i = m_modelGeom.linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
{
/* skip disabled bodies */
- if (!m_modelsGeom[model_id].linkGeom[i]->enabled)
+ if (!m_modelGeom.linkGeom[i]->enabled)
continue;
- const unsigned int ng = m_modelsGeom[model_id].linkGeom[i]->geom.size();
- cdata->link1 = m_modelsGeom[model_id].linkGeom[i]->link;
+ 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)
{
- dGeomID g1 = m_modelsGeom[model_id].linkGeom[i]->geom[ig];
+ dGeomID g1 = m_modelGeom.linkGeom[i]->geom[ig];
dReal aabb1[6];
dGeomGetAABB(g1, aabb1);
for (int j = m_basicGeoms.size() - 1 ; j >= 0 ; --j)
@@ -487,7 +480,7 @@
if (cdata->collides && m_verbose)
m_msg.inform("Collision between static body and link '%s'\n",
- m_modelsGeom[model_id].linkGeom[i]->link->name.c_str());
+ m_modelGeom.linkGeom[i]->link->name.c_str());
}
}
}
@@ -499,17 +492,17 @@
{
cdata->link2 = NULL;
m_collide2.setup();
- for (int i = m_modelsGeom[model_id].linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
- if (m_modelsGeom[model_id].linkGeom[i]->enabled)
+ for (int i = m_modelGeom.linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
+ if (m_modelGeom.linkGeom[i]->enabled)
{
- const unsigned int ng = m_modelsGeom[model_id].linkGeom[i]->geom.size();
- cdata->link1 = m_modelsGeom[model_id].linkGeom[i]->link;
+ 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_modelsGeom[model_id].linkGeom[i]->geom[ig], data, nearCallbackFn);
+ 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_modelsGeom[model_id].linkGeom[i]->link->name.c_str());
+ m_modelGeom.linkGeom[i]->link->name.c_str());
}
}
}
@@ -544,35 +537,31 @@
m_collide2.setup();
}
-void collision_space::EnvironmentModelODE::addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links)
+void collision_space::EnvironmentModelODE::addSelfCollisionGroup(std::vector<std::string> &links)
{
- if (model_id < m_modelsGeom.size())
+ unsigned int pos = m_modelGeom.selfCollision.size();
+ m_modelGeom.selfCollision.resize(pos + 1);
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
{
- unsigned int pos = m_modelsGeom[model_id].selfCollision.size();
- m_modelsGeom[model_id].selfCollision.resize(pos + 1);
- for (unsigned int i = 0 ; i < links.size() ; ++i)
- {
- for (unsigned int j = 0 ; j < m_modelsGeom[model_id].linkGeom.size() ; ++j)
- if (m_modelsGeom[model_id].linkGeom[j]->link->name == links[i])
- m_modelsGeom[model_id].selfCollision[pos].push_back(j);
- }
+ for (unsigned int j = 0 ; j < m_modelGeom.linkGeom.size() ; ++j)
+ if (m_modelGeom.linkGeom[j]->link->name == links[i])
+ m_modelGeom.selfCollision[pos].push_back(j);
}
+
}
-int collision_space::EnvironmentModelODE::setCollisionCheck(unsigned int model_id, const std::string &link, bool state)
+int collision_space::EnvironmentModelODE::setCollisionCheck(const std::string &link, bool state)
{
int result = -1;
- if (model_id < m_modelsGeom.size())
+ for (unsigned int j = 0 ; j < m_modelGeom.linkGeom.size() ; ++j)
{
- for (unsigned int j = 0 ; j < m_modelsGeom[model_id].linkGeom.size() ; ++j)
+ if (m_modelGeom.linkGeom[j]->link->name == link)
{
- if (m_modelsGeom[model_id].linkGeom[j]->link->name == link)
- {
- result = m_modelsGeom[model_id].linkGeom[j]->enabled ? 1 : 0;
- m_modelsGeom[model_id].linkGeom[j]->enabled = state;
- break;
- }
+ result = m_modelGeom.linkGeom[j]->enabled ? 1 : 0;
+ m_modelGeom.linkGeom[j]->enabled = state;
+ break;
}
}
+
return result;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
