Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2112] pkg/trunk
|
From: <is...@us...> - 2008-07-25 01:29:20
|
Revision: 2112
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2112&view=rev
Author: isucan
Date: 2008-07-25 01:29:28 +0000 (Fri, 25 Jul 2008)
Log Message:
-----------
motion planning without obstacles seems to be running....
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
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/src/SpaceInformationKinematic.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/util/collision_space/src/test/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 01:25:49 UTC (rev 2111)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-25 01:29:28 UTC (rev 2112)
@@ -199,6 +199,7 @@
goal->state = new ompl::SpaceInformationKinematic::StateKinematic(dim);
for (int i = 0 ; i < dim ; ++i)
goal->state->values[i] = req.goal_state.vals[i];
+ goal->threshold = 1e-6;
p.si->setGoal(goal);
/* do the planning */
@@ -218,7 +219,7 @@
res.path.states[i].vals[j] = path->states[i]->values[j];
}
}
-
+
/* cleanup */
p.si->clearGoal();
p.si->clearStartStates(true);
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-07-25 01:29:28 UTC (rev 2112)
@@ -145,7 +145,7 @@
m_isValidStateFnData = data;
}
- virtual void printState(const StateKinematic_t state, FILE* out = stdout);
+ virtual void printState(const StateKinematic_t state, FILE* out = stdout) const;
virtual void copyState(StateKinematic_t destination, const StateKinematic_t source)
{
memcpy(destination->values, source->values, sizeof(double) * m_stateDimension);
@@ -172,6 +172,8 @@
return m_isValidStateFn(state, m_isValidStateFnData);
}
+ virtual void printSettings(FILE *out = stdout) const;
+
protected:
unsigned int m_stateDimension;
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 01:25:49 UTC (rev 2111)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp 2008-07-25 01:29:28 UTC (rev 2112)
@@ -7,6 +7,9 @@
SpaceInformationKinematic::GoalStateKinematic_t goal_s = dynamic_cast<SpaceInformationKinematic::GoalStateKinematic_t>(si->getGoal());
unsigned int dim = si->getStateDimension();
+ printf("Received motion planning request for %f seconds.\n", solveTime);
+ si->printSettings();
+
ros::Time endTime = ros::Time::now() + ros::Duration(solveTime);
for (unsigned int i = 0 ; i < m_si->getStartStateCount() ; ++i)
@@ -63,7 +66,6 @@
if (si->checkMotion(nmotion->state, motion->state))
{
m_nn.add(motion);
-
if (goal_r->distanceGoal(motion->state) < goal_r->threshold)
{
solution = motion;
@@ -82,10 +84,14 @@
solution = solution->parent;
}
- /*set the solution path */
+ /* set the solution path */
SpaceInformationKinematic::PathKinematic_t path = new SpaceInformationKinematic::PathKinematic(m_si);
- for (int i = mpath.size() - 1 ; i >= 0 ; ++i)
- path->states.push_back(mpath[i]->state);
+ for (int i = mpath.size() - 1 ; i >= 0 ; --i)
+ {
+ SpaceInformationKinematic::StateKinematic_t st = new SpaceInformationKinematic::StateKinematic(dim);
+ si->copyState(st, mpath[i]->state);
+ path->states.push_back(st);
+ }
goal_r->setSolutionPath(path);
}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2008-07-25 01:29:28 UTC (rev 2112)
@@ -2,7 +2,7 @@
#include <algorithm>
#include <queue>
-void ompl::SpaceInformationKinematic::printState(const StateKinematic_t state, FILE* out)
+void ompl::SpaceInformationKinematic::printState(const StateKinematic_t state, FILE* out) const
{
for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
fprintf(out, "%0.6f ", state->values[i]);
@@ -126,3 +126,17 @@
}
}
}
+
+void ompl::SpaceInformationKinematic::printSettings(FILE *out) const
+{
+ fprintf(out, "Kinematic state space settings:\n");
+ fprintf(out, " - dimension = %u\n", m_stateDimension);
+ fprintf(out, " - start states:\n");
+ for (unsigned int i = 0 ; i < getStartStateCount() ; ++i)
+ printState(dynamic_cast<const StateKinematic_t>(getStartState(i)), out);
+ fprintf(out, " - goal = %p\n", m_goal);
+ fprintf(out, " - bounding box:\n");
+ for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ fprintf(out, "[%f, %f](%f) ", m_stateComponent[i].minValue, m_stateComponent[i].maxValue, m_stateComponent[i].resolution);
+ fprintf(out, "\n");
+}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-07-25 01:29:28 UTC (rev 2112)
@@ -207,6 +207,7 @@
<axis> 1 0 0 </axis> <!-- direction of the joint in a global coordinate frame -->
<anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
+ <limit>-1 1</limit>
</define>
<define template="pr2_forearm_roll_inertial">
@@ -228,6 +229,7 @@
<axis> 0 1 0 </axis> <!-- direction of the joint in a global coordinate frame -->
<anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
+ <limit>-1 1</limit>
</define>
<define template="pr2_wrist_pitch_inertial">
@@ -250,6 +252,7 @@
<axis> 0 1 0 </axis> <!-- direction of the joint in a global coordinate frame -->
<anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
+ <limit>-1 1</limit>
</define>
<define template="pr2_wrist_roll_inertial">
Modified: pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environmentODE.h 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/util/collision_space/include/collision_space/environmentODE.h 2008-07-25 01:29:28 UTC (rev 2112)
@@ -67,6 +67,7 @@
}
dSpaceID getODESpace(void) const;
+ dSpaceID getModelODESpace(unsigned int model_id) const;
/** Check if a model is in collision */
virtual bool isCollision(unsigned int 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 01:25:49 UTC (rev 2111)
+++ pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-25 01:29:28 UTC (rev 2112)
@@ -173,6 +173,12 @@
return m_space;
}
+dSpaceID collision_space::EnvironmentModelODE::getModelODESpace(unsigned int model_id) const
+{
+ return m_kgeoms[model_id].s;
+}
+
+
struct CollisionData
{
bool collides;
Modified: pkg/trunk/util/collision_space/src/test/test_kinematic_ode.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/test/test_kinematic_ode.cpp 2008-07-25 01:25:49 UTC (rev 2111)
+++ pkg/trunk/util/collision_space/src/test/test_kinematic_ode.cpp 2008-07-25 01:29:28 UTC (rev 2112)
@@ -146,8 +146,6 @@
int main(int argc, char **argv)
{
- dInitODE();
-
robot_desc::URDF model;
model.loadFile("/u/isucan/ros/ros-pkg/robot_descriptions/wg_robot_description/pr2/pr2.xml");
@@ -169,7 +167,7 @@
m->computeTransforms(param, m->getGroupID("pr2::leftArm"));
km->updateRobotModel(0);
- robotSpace = dynamic_cast<EnvironmentModelODE*>(km)->getODESpace();
+ robotSpace = dynamic_cast<EnvironmentModelODE*>(km)->getModelODESpace(0);
dsFunctions fn;
fn.version = DS_VERSION;
@@ -184,7 +182,6 @@
delete km;
delete[] param;
- dCloseODE();
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
