Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3424] pkg/trunk
|
From: <is...@us...> - 2008-08-21 20:28:16
|
Revision: 3424
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3424&view=rev
Author: isucan
Date: 2008-08-21 20:28:21 +0000 (Thu, 21 Aug 2008)
Log Message:
-----------
printing goals added to ompl, multiple runs option in kinematic planning
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/base/General.h
pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-08-21 20:28:18 UTC (rev 3423)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-08-21 20:28:21 UTC (rev 3424)
@@ -7,4 +7,5 @@
<depend package="robot_srvs" />
<depend package="planning_node_util" />
<depend package="ompl" />
+ <depend package="gtest" />
</package>
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-21 20:28:18 UTC (rev 3423)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-21 20:28:21 UTC (rev 3424)
@@ -112,7 +112,12 @@
planning_node_util::NodeCollisionModel(dynamic_cast<ros::node*>(this),
robot_model)
{
- advertise_service("plan_kinematic_path", &KinematicPlanning::plan);
+ advertise_service("plan_kinematic_path_state", &KinematicPlanning::plan);
+
+
+ double sphere[3] = {0.7, 0.5, 0.7 };
+ m_collisionSpace->addPointCloud(1, sphere, 0.2);
+
}
~KinematicPlanning(void)
@@ -125,7 +130,7 @@
{
if (m_models.find(req.model_id) == m_models.end())
{
- fprintf(stderr, "Cannot plan for '%s'. Model does not exist\n", req.model_id.c_str());
+ std::cerr << "Cannot plan for '" << req.model_id << "'. Model does not exist" << std::endl;
return false;
}
@@ -134,28 +139,35 @@
if (m->kmodel->stateDimension != req.start_state.get_vals_size())
{
- fprintf(stderr, "Dimension of start state expected to be %u but was received as %u\n", m->kmodel->stateDimension, req.start_state.get_vals_size());
+ std::cerr << "Dimension of start state expected to be " << m->kmodel->stateDimension << " but was received as " << req.start_state.get_vals_size() << std::endl;
return false;
}
-
+
const int dim = req.goal_state.get_vals_size();
if ((int)p.si->getStateDimension() != dim)
{
- fprintf(stderr, "Dimension of goal state expected to be %u but was received as %d\n", p.si->getStateDimension(), dim);
+ std::cerr << "Dimension of start goal expected to be " << p.si->getStateDimension() << " but was received as " << dim << std::endl;
return false;
}
+ if (req.times <= 0)
+ {
+ std::cerr << "Request specifies motion plan should be computed " << req.times << " times" << std::endl;
+ return false;
+ }
+
+
/* set the workspace volume for planning */
// only area or volume should go through... not sure what happens on really complicated models where
// we have both multiple planar and multiple floating joints...
static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningArea(req.volumeMin.x, req.volumeMin.y,
req.volumeMax.x, req.volumeMax.y);
- static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningVolume(req.volumeMin.x, req.volumeMin.y, req.volumeMin.z,
+ static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningVolume(req.volumeMin.x, req.volumeMin.y, req.volumeMin.z,
req.volumeMax.x, req.volumeMax.y, req.volumeMax.z);
-
+
/* set the starting state */
- ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
-
+ ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
+
if (m->groupID >= 0)
{
/* set the pose of the whole robot */
@@ -168,6 +180,7 @@
}
else
{
+ /* the start state is the complete state */
for (int i = 0 ; i < dim ; ++i)
start->values[i] = req.start_state.vals[i];
}
@@ -186,41 +199,60 @@
p.si->printSettings();
printf("=======================================\n");
+
/* do the planning */
+ ompl::SpaceInformationKinematic::PathKinematic_t bestPath = NULL;
+ double bestDifference = 0.0;
+ double totalTime = 0.0;
+
m_collisionSpace->lock();
- ros::Time startTime = ros::Time::now();
- bool ok = p.mp->solve(req.allowed_time);
- double tsolve = (ros::Time::now() - startTime).to_double();
- printf("Motion planner spent %f seconds\n", tsolve);
-
- /* do path smoothing */
- if (ok)
+ for (int i = 0 ; i < req.times ; ++i)
{
ros::Time startTime = ros::Time::now();
- ompl::SpaceInformationKinematic::PathKinematic_t path = static_cast<ompl::SpaceInformationKinematic::PathKinematic_t>(goal->getSolutionPath());
- p.smoother->smoothVertices(path);
- double tsmooth = (ros::Time::now() - startTime).to_double();
- printf("Smoother spent %f seconds (%f seconds in total)\n", tsmooth, tsmooth + tsolve);
- if (req.interpolate)
- p.si->interpolatePath(path);
- }
+ bool ok = p.mp->solve(req.allowed_time);
+ double tsolve = (ros::Time::now() - startTime).to_double();
+ std::cout << (ok ? "[Success]" : "[Failure]") << " Motion planner spent " << tsolve << " seconds" << std::endl;
+ totalTime += tsolve;
+
+ /* do path smoothing */
+ if (ok)
+ {
+ ros::Time startTime = ros::Time::now();
+ ompl::SpaceInformationKinematic::PathKinematic_t path = static_cast<ompl::SpaceInformationKinematic::PathKinematic_t>(goal->getSolutionPath());
+ p.smoother->smoothVertices(path);
+ double tsmooth = (ros::Time::now() - startTime).to_double();
+ std::cout << " Smoother spent " << tsmooth << " seconds (" << (tsmooth + tsolve) << " seconds in total)" << std::endl;
+ if (req.interpolate)
+ p.si->interpolatePath(path);
+ if (bestPath == NULL || bestDifference > goal->getDifference() ||
+ (bestPath && bestDifference == goal->getDifference() && bestPath->states.size() > path->states.size()))
+ {
+ if (bestPath)
+ delete bestPath;
+ bestPath = path;
+ bestDifference = goal->getDifference();
+ goal->forgetSolutionPath();
+ std::cout << " Obtained better solution" << std::endl;
+ }
+ }
+ }
m_collisionSpace->unlock();
-
+ std::cout << std::endl << "Total planning time: " << totalTime << "; Average planning time: " << (totalTime / (double)req.times) << " (seconds)" << std::endl;
+
/* copy the solution to the result */
- if (ok)
+ if (bestPath)
{
- ompl::SpaceInformationKinematic::PathKinematic_t path = static_cast<ompl::SpaceInformationKinematic::PathKinematic_t>(goal->getSolutionPath());
- res.path.set_states_size(path->states.size());
- for (unsigned int i = 0 ; i < path->states.size() ; ++i)
+ res.path.set_states_size(bestPath->states.size());
+ for (unsigned int i = 0 ; i < bestPath->states.size() ; ++i)
{
res.path.states[i].set_vals_size(dim);
for (int j = 0 ; j < dim ; ++j)
- res.path.states[i].vals[j] = path->states[i]->values[j];
+ res.path.states[i].vals[j] = bestPath->states[i]->values[j];
}
- res.distance = goal->getDifference();
+ res.distance = bestDifference;
}
else
{
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/General.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/General.h 2008-08-21 20:28:18 UTC (rev 3423)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/General.h 2008-08-21 20:28:21 UTC (rev 3424)
@@ -45,6 +45,8 @@
#include <climits>
#include <cmath>
+#include <iostream>
+
#include "ompl/base/util/types.h"
#include "ompl/base/util/time.h"
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h 2008-08-21 20:28:18 UTC (rev 3423)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h 2008-08-21 20:28:21 UTC (rev 3424)
@@ -278,6 +278,12 @@
return m_approximate;
}
+ /** Print information about the goal */
+ virtual void print(std::ostream &out = std::cout) const
+ {
+ out << "Goal memory address " << reinterpret_cast<const void*>(this) << std::endl;
+ }
+
protected:
/** solution path, if found */
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-08-21 20:28:18 UTC (rev 3423)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-08-21 20:28:21 UTC (rev 3424)
@@ -106,7 +106,12 @@
}
virtual double distanceGoal(StateKinematic_t s) = 0;
-
+
+ virtual void print(std::ostream &out = std::cout) const
+ {
+ out << "Goal region, threshold = " << threshold << ", memory address = " << reinterpret_cast<const void*>(this) << std::endl;
+ }
+
double threshold;
};
@@ -131,6 +136,12 @@
return static_cast<SpaceInformationKinematic_t>(m_si)->distance(s, state);
}
+ virtual void print(std::ostream &out = std::cout) const
+ {
+ out << "Goal state, threshold = " << threshold << ", memory address = " << reinterpret_cast<const void*>(this) << ", state = ";
+ static_cast<SpaceInformationKinematic_t>(m_si)->printState(state, out);
+ }
+
StateKinematic_t state;
};
@@ -189,7 +200,7 @@
double resolution;
};
- virtual void printState(const StateKinematic_t state, FILE* out = stdout) const;
+ virtual void printState(const StateKinematic_t state, std::ostream &out = std::cout) const;
virtual void copyState(StateKinematic_t destination, const StateKinematic_t source)
{
memcpy(destination->values, source->values, sizeof(double) * m_stateDimension);
@@ -222,7 +233,7 @@
return (*m_stateValidityChecker)(static_cast<const State_t>(state));
}
- virtual void printSettings(FILE *out = stdout) const;
+ virtual void printSettings(std::ostream &out = std::cout) const;
virtual void setup(void)
{
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-08-21 20:28:18 UTC (rev 3423)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2008-08-21 20:28:21 UTC (rev 3424)
@@ -56,12 +56,18 @@
return dist;
}
-void ompl::SpaceInformationKinematic::printState(const StateKinematic_t state, FILE* out) const
+void ompl::SpaceInformationKinematic::printState(const StateKinematic_t state, std::ostream &out) const
{
- for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
- fprintf(out, "%0.6f ", state->values[i]);
- fprintf(out, "\n");
+ if (state)
+ {
+ for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ out << state->values[i] << " ";
+ out << std::endl;
+ }
+ else
+ out << "NULL" << std::endl;
}
+
void ompl::SpaceInformationKinematic::sample(StateKinematic_t state)
{
for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
@@ -216,16 +222,19 @@
path->states.swap(newStates);
}
-void ompl::SpaceInformationKinematic::printSettings(FILE *out) const
+void ompl::SpaceInformationKinematic::printSettings(std::ostream &out) const
{
- fprintf(out, "Kinematic state space settings:\n");
- fprintf(out, " - dimension = %u\n", m_stateDimension);
- fprintf(out, " - start states:\n");
+ out << "Kinematic state space settings:" << std::endl;
+ out << " - dimension = " << m_stateDimension << std::endl;
+ out << " - start states:" << std::endl;
for (unsigned int i = 0 ; i < getStartStateCount() ; ++i)
- printState(dynamic_cast<const StateKinematic_t>(getStartState(i)), out);
- fprintf(out, " - goal = %p\n", reinterpret_cast<void*>(m_goal));
- fprintf(out, " - bounding box:\n");
+ printState(dynamic_cast<const StateKinematic_t>(getStartState(i)), out);
+ if (m_goal)
+ m_goal->print(out);
+ else
+ out << " - goal = NULL" << std::endl;
+ out << " - bounding box:" << std::endl;
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");
+ out << "[" << m_stateComponent[i].minValue << ", " << m_stateComponent[i].maxValue << "](" << m_stateComponent[i].resolution << ") ";
+ out << std::endl;
}
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-21 20:28:18 UTC (rev 3423)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-21 20:28:21 UTC (rev 3424)
@@ -79,6 +79,7 @@
req.threshold = 0.01;
req.distance_metric = "L2Square";
req.interpolate = 1;
+ req.times = 10;
initialState(req.start_state);
@@ -103,13 +104,14 @@
req.threshold = 0.01;
req.distance_metric = "L2Square";
req.interpolate = 1;
+ req.times = 1;
initialState(req.start_state);
req.goal_state.set_vals_size(7);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
req.goal_state.vals[i] = 0.0;
- req.goal_state.vals[0] = -1.0;
+ req.goal_state.vals[0] = 1.0;
req.allowed_time = 10.0;
@@ -128,6 +130,7 @@
req.threshold = 0.01;
req.distance_metric = "L2Square";
req.interpolate = 1;
+ req.times = 100;
initialState(req.start_state);
@@ -149,7 +152,7 @@
robot_srvs::KinematicPlanState::response res;
robot_msgs::NamedKinematicPath dpath;
- if (ros::service::call("plan_kinematic_path", req, res))
+ if (ros::service::call("plan_kinematic_path_state", req, res))
{
unsigned int nstates = res.path.get_states_size();
printf("Obtained %ssolution path with %u states, distance to goal = %f\n",
@@ -190,11 +193,11 @@
ros::init(argc, argv);
PlanKinematicPath plan;
-
+ /*
ros::Duration dur(0.1);
while (!plan.haveBasePos())
dur.sleep();
-
+ */
char test = (argc < 2) ? 'b' : argv[1][0];
switch (test)
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-21 20:28:18 UTC (rev 3423)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-21 20:28:21 UTC (rev 3424)
@@ -112,7 +112,9 @@
m_displayRobot = true;
m_displayObstacles = true;
m_checkCollision = false;
-
+
+ double sphere[3] = {0.7, 0.5, 0.7 };
+ m_collisionSpace->addPointCloud(1, sphere, 0.2);
}
~PlanningWorldViewer(void)
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv 2008-08-21 20:28:18 UTC (rev 3423)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv 2008-08-21 20:28:21 UTC (rev 3424)
@@ -25,9 +25,13 @@
# No state in the produced motion plan will violate these constraints
robot_msgs/KinematicConstraint[] constraints
+
+# The number of times this plan is to be computed. Shortest solution
+# will be reported.
+int32 times
+
# Boolean flag: if true, the returned path will contain interpolated
# states as well
-
byte interpolate
# The maximum amount of time the motion planner is allowed to plan for
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2008-08-21 20:28:18 UTC (rev 3423)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2008-08-21 20:28:21 UTC (rev 3424)
@@ -24,13 +24,15 @@
# constraints is correct
robot_msgs/KinematicState goal_state
-
# No state in the produced motion plan will violate these constraints
robot_msgs/KinematicConstraint[] constraints
+# The number of times this plan is to be computed. Shortest solution
+# will be reported.
+int32 times
+
# Boolean flag: if true, the returned path will contain interpolated
# states as well
-
byte interpolate
# The maximum amount of time the motion planner is allowed to plan for
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
