Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2139] pkg/trunk
|
From: <is...@us...> - 2008-07-25 20:41:31
|
Revision: 2139
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2139&view=rev
Author: isucan
Date: 2008-07-25 20:41:38 +0000 (Fri, 25 Jul 2008)
Log Message:
-----------
made some collision space data private
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
pkg/trunk/util/collision_space/include/collision_space/environment.h
pkg/trunk/util/collision_space/src/collision_space/environment.cpp
pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-25 20:33:21 UTC (rev 2138)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-25 20:41:38 UTC (rev 2139)
@@ -254,7 +254,7 @@
unsigned int cid = m_collisionSpace->addRobotModel(*file);
model->collisionSpaceID = cid;
model->collisionSpace = m_collisionSpace;
- model->kmodel = m_collisionSpace->models[cid];
+ model->kmodel = m_collisionSpace->getModel(cid);
m_models[name] = model;
createMotionPlanningInstances(model);
@@ -265,7 +265,7 @@
std::string gname = name + "::" + groups[i];
model->collisionSpaceID = cid;
model->collisionSpace = m_collisionSpace;
- model->kmodel = m_collisionSpace->models[cid];
+ model->kmodel = m_collisionSpace->getModel(cid);
model->groupID = model->kmodel->getGroupID(gname);
m_models[gname] = model;
createMotionPlanningInstances(model);
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp 2008-07-25 20:33:21 UTC (rev 2138)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp 2008-07-25 20:41:38 UTC (rev 2139)
@@ -97,6 +97,9 @@
delete xstate;
delete rmotion;
-
+
+ double tsolve = solveTime - (endTime - ros::Time::now()).to_double();
+ printf("Solved in %f seconds\n", tsolve);
+
return goal_r->isAchieved();
}
Modified: pkg/trunk/util/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-25 20:33:21 UTC (rev 2138)
+++ pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-25 20:41:38 UTC (rev 2139)
@@ -57,8 +57,8 @@
virtual ~EnvironmentModel(void)
{
- for (unsigned int i = 0 ; i < models.size() ; ++i)
- delete models[i];
+ for (unsigned int i = 0 ; i < m_models.size() ; ++i)
+ delete m_models[i];
}
/** Check if a model is in collision */
@@ -78,6 +78,11 @@
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id) = 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;
+
/** Provide interface to a lock. Use carefully! */
void lock(void);
@@ -89,15 +94,15 @@
/** Check if self collision is enabled */
bool getSelfCollision(void) const;
-
- /** List of loaded robot models */
- std::vector<planning_models::KinematicModel*> models;
protected:
ros::thread::mutex m_lock;
bool m_selfCollision;
+ /** List of loaded robot models */
+ std::vector<planning_models::KinematicModel*> m_models;
+
};
}
Modified: pkg/trunk/util/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environment.cpp 2008-07-25 20:33:21 UTC (rev 2138)
+++ pkg/trunk/util/collision_space/src/collision_space/environment.cpp 2008-07-25 20:41:38 UTC (rev 2139)
@@ -43,8 +43,8 @@
unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model)
{
- unsigned int pos = models.size();
- models.push_back(model);
+ unsigned int pos = m_models.size();
+ m_models.push_back(model);
return pos;
}
@@ -67,3 +67,14 @@
{
return m_selfCollision;
}
+
+
+unsigned int collision_space::EnvironmentModel::getModelCount(void) const
+{
+ return m_models.size();
+}
+
+planning_models::KinematicModel* collision_space::EnvironmentModel::getModel(unsigned int model_id) const
+{
+ return m_models[model_id];
+}
Modified: pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-25 20:33:21 UTC (rev 2138)
+++ pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-25 20:41:38 UTC (rev 2139)
@@ -56,9 +56,9 @@
m_kgeoms[id].s = dHashSpaceCreate(0);
}
- for (unsigned int j = 0 ; j < models[id]->getRobotCount() ; ++j)
+ for (unsigned int j = 0 ; j < m_models[id]->getRobotCount() ; ++j)
{
- planning_models::KinematicModel::Robot *robot = models[id]->getRobot(j);
+ planning_models::KinematicModel::Robot *robot = m_models[id]->getRobot(j);
for (unsigned int i = 0 ; i < robot->links.size() ; ++i)
{
kGeom *kg = new kGeom();
Modified: pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
===================================================================
--- pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-25 20:33:21 UTC (rev 2138)
+++ pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-25 20:41:38 UTC (rev 2139)
@@ -95,7 +95,7 @@
EnvironmentModel *km = new EnvironmentModelODE();
km->addRobotModel(model);
- planning_models::KinematicModel *m = km->models[0];
+ planning_models::KinematicModel *m = km->getModel(0);
printModelInfo(m);
double *param = new double[m->stateDimension];
@@ -110,9 +110,13 @@
param[i] = 0.1;
m->computeTransforms(param, m->getGroupID("pr2::leftArm"));
km->updateRobotModel(0);
-
- spaces.addSpace(dynamic_cast<EnvironmentModelODE*>(km)->getModelODESpace(0));
+ EnvironmentModelODE* okm = dynamic_cast<EnvironmentModelODE*>(km);
+ spaces.addSpace(okm->getODESpace());
+ for (unsigned int i = 0 ; i < okm->getModelCount() ; ++i)
+ spaces.addSpace(okm->getModelODESpace(i));
+
+
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
