00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/geometric/planners/prm/PRM.h"
00038 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
00039 #include "ompl/base/GoalSampleableRegion.h"
00040 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00041 #include "ompl/datastructures/PDF.h"
00042 #include <boost/lambda/bind.hpp>
00043 #include <boost/graph/astar_search.hpp>
00044 #include <boost/graph/incremental_components.hpp>
00045 #include <boost/property_map/vector_property_map.hpp>
00046 #include <boost/foreach.hpp>
00047
00048 #define foreach BOOST_FOREACH
00049 #define foreach_reverse BOOST_REVERSE_FOREACH
00050
00051 namespace ompl
00052 {
00053 namespace magic
00054 {
00055
00059 static const unsigned int FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK = 2;
00060
00063 static const unsigned int MAX_RANDOM_BOUNCE_STEPS = 5;
00064
00067 static const unsigned int DEFAULT_NEAREST_NEIGHBORS = 10;
00068 }
00069 }
00070
00071 ompl::geometric::PRM::PRM(const base::SpaceInformationPtr &si, bool starStrategy) :
00072 base::Planner(si, "PRM"),
00073 starStrategy_(starStrategy),
00074 stateProperty_(boost::get(vertex_state_t(), g_)),
00075 totalConnectionAttemptsProperty_(boost::get(vertex_total_connection_attempts_t(), g_)),
00076 successfulConnectionAttemptsProperty_(boost::get(vertex_successful_connection_attempts_t(), g_)),
00077 weightProperty_(boost::get(boost::edge_weight, g_)),
00078 edgeIDProperty_(boost::get(boost::edge_index, g_)),
00079 disjointSets_(boost::get(boost::vertex_rank, g_),
00080 boost::get(boost::vertex_predecessor, g_)),
00081 maxEdgeID_(0),
00082 userSetConnectionStrategy_(false)
00083 {
00084 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
00085 specs_.approximateSolutions = true;
00086 specs_.optimizingPaths = true;
00087 }
00088
00089 void ompl::geometric::PRM::setup(void)
00090 {
00091 Planner::setup();
00092 if (!nn_)
00093 nn_.reset(new NearestNeighborsGNAT<Vertex>());
00094 nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2));
00095 if (!connectionStrategy_)
00096 {
00097 if (starStrategy_)
00098 connectionStrategy_ = KStarStrategy<Vertex>(boost::bind(&PRM::milestoneCount, this), nn_, si_->getStateDimension());
00099 else
00100 connectionStrategy_ = KStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS, nn_);
00101 }
00102 if (!connectionFilter_)
00103 connectionFilter_ = boost::lambda::constant(true);
00104 }
00105
00106 void ompl::geometric::PRM::setMaxNearestNeighbors(unsigned int k)
00107 {
00108 if (!setup_)
00109 setup();
00110 connectionStrategy_ = KStrategy<Vertex>(k, nn_);
00111 }
00112
00113 void ompl::geometric::PRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
00114 {
00115 Planner::setProblemDefinition(pdef);
00116 startM_.clear();
00117 goalM_.clear();
00118 }
00119
00120 void ompl::geometric::PRM::clear(void)
00121 {
00122 Planner::clear();
00123 sampler_.reset();
00124 simpleSampler_.reset();
00125 freeMemory();
00126 if (nn_)
00127 nn_->clear();
00128 startM_.clear();
00129 goalM_.clear();
00130 maxEdgeID_ = 0;
00131 }
00132
00133 void ompl::geometric::PRM::freeMemory(void)
00134 {
00135 foreach (Vertex v, boost::vertices(g_))
00136 si_->freeState(stateProperty_[v]);
00137 g_.clear();
00138 }
00139
00140 void ompl::geometric::PRM::expandRoadmap(double expandTime)
00141 {
00142 if (!simpleSampler_)
00143 simpleSampler_ = si_->allocStateSampler();
00144
00145 std::vector<Vertex> empty;
00146 std::vector<base::State*> states(magic::MAX_RANDOM_BOUNCE_STEPS);
00147 si_->allocStates(states);
00148 expandRoadmap(empty, empty, base::timedPlannerTerminationCondition(expandTime), states);
00149 si_->freeStates(states);
00150 }
00151
00152 void ompl::geometric::PRM::expandRoadmap(const std::vector<Vertex> &starts,
00153 const std::vector<Vertex> &goals,
00154 const base::PlannerTerminationCondition &ptc,
00155 std::vector<base::State*> &workStates)
00156 {
00157
00158
00159
00160
00161
00162 PDF<Vertex> pdf;
00163 foreach (Vertex v, boost::vertices(g_))
00164 {
00165 const unsigned int t = totalConnectionAttemptsProperty_[v];
00166 pdf.add(v, (double)(t - successfulConnectionAttemptsProperty_[v]) / (double)t);
00167 }
00168
00169 if (pdf.empty())
00170 return;
00171
00172 while (ptc() == false)
00173 {
00174 Vertex v = pdf.sample(rng_.uniform01());
00175 unsigned int s = si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false);
00176 if (s > 0)
00177 {
00178 s--;
00179 Vertex last = addMilestone(si_->cloneState(workStates[s]));
00180
00181 for (unsigned int i = 0 ; i < s ; ++i)
00182 {
00183
00184 Vertex m = boost::add_vertex(g_);
00185 stateProperty_[m] = si_->cloneState(workStates[i]);
00186 totalConnectionAttemptsProperty_[m] = 1;
00187 successfulConnectionAttemptsProperty_[m] = 0;
00188 disjointSets_.make_set(m);
00189
00190
00191 const double weight = distanceFunction(v, m);
00192 const unsigned int id = maxEdgeID_++;
00193 const Graph::edge_property_type properties(weight, id);
00194 boost::add_edge(v, m, properties, g_);
00195 uniteComponents(v, m);
00196
00197
00198 nn_->add(m);
00199 v = m;
00200 }
00201
00202
00203
00204 if (s > 0 || !boost::same_component(v, last, disjointSets_))
00205 {
00206
00207 const double weight = distanceFunction(v, last);
00208 const unsigned int id = maxEdgeID_++;
00209 const Graph::edge_property_type properties(weight, id);
00210 boost::add_edge(v, last, properties, g_);
00211 uniteComponents(v, last);
00212 }
00213 if (haveSolution(starts, goals))
00214 break;
00215 }
00216 }
00217 }
00218
00219 void ompl::geometric::PRM::growRoadmap(double growTime)
00220 {
00221 if (!isSetup())
00222 setup();
00223 if (!sampler_)
00224 sampler_ = si_->allocValidStateSampler();
00225
00226 time::point endTime = time::now() + time::seconds(growTime);
00227 base::State *workState = si_->allocState();
00228 while (time::now() < endTime)
00229 {
00230
00231 bool found = false;
00232 while (!found && time::now() < endTime)
00233 {
00234 unsigned int attempts = 0;
00235 do
00236 {
00237 found = sampler_->sample(workState);
00238 attempts++;
00239 } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK && !found);
00240 }
00241
00242 if (found)
00243 addMilestone(si_->cloneState(workState));
00244 }
00245 si_->freeState(workState);
00246 }
00247
00248 void ompl::geometric::PRM::growRoadmap(const std::vector<Vertex> &start,
00249 const std::vector<Vertex> &goal,
00250 const base::PlannerTerminationCondition &ptc,
00251 base::State *workState)
00252 {
00253 while (ptc() == false)
00254 {
00255
00256 bool found = false;
00257 while (!found && ptc() == false)
00258 {
00259 unsigned int attempts = 0;
00260 do
00261 {
00262 found = sampler_->sample(workState);
00263 attempts++;
00264 } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK && !found);
00265 }
00266
00267 if (found)
00268 {
00269 addMilestone(si_->cloneState(workState));
00270 if (haveSolution(start, goal))
00271 break;
00272 }
00273 }
00274 }
00275
00276 bool ompl::geometric::PRM::haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, std::pair<Vertex, Vertex> *endpoints)
00277 {
00278 base::Goal *g = pdef_->getGoal().get();
00279 foreach (Vertex start, starts)
00280 foreach (Vertex goal, goals)
00281 {
00282
00283 if (boost::same_component(start, goal, disjointSets_) &&
00284 g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
00285 {
00286 bool sol = true;
00287
00288
00289 if (g->getMaximumPathLength() < std::numeric_limits<double>::infinity())
00290 {
00291 base::PathPtr p = constructSolution(start, goal);
00292 double pl = p->length();
00293 if (pl > g->getMaximumPathLength())
00294 {
00295 sol = false;
00296
00297 if (approxlen_ < 0.0 || approxlen_ > pl)
00298 {
00299 approxsol_ = p;
00300 approxlen_ = pl;
00301 }
00302 }
00303 }
00304
00305 if (sol)
00306 {
00307 if (endpoints)
00308 {
00309 endpoints->first = start;
00310 endpoints->second = goal;
00311 }
00312 return true;
00313 }
00314 }
00315 }
00316 return false;
00317 }
00318
00319 bool ompl::geometric::PRM::solve(const base::PlannerTerminationCondition &ptc)
00320 {
00321 checkValidity();
00322 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00323
00324 if (!goal)
00325 {
00326 msg_.error("Goal undefined or unknown type of goal");
00327 return false;
00328 }
00329
00330 unsigned int nrStartStates = boost::num_vertices(g_);
00331
00332
00333 while (const base::State *st = pis_.nextStart())
00334 startM_.push_back(addMilestone(si_->cloneState(st)));
00335
00336 if (startM_.size() == 0)
00337 {
00338 msg_.error("There are no valid initial states!");
00339 return false;
00340 }
00341
00342 if (!goal->canSample())
00343 {
00344 msg_.error("Insufficient states in sampleable goal region");
00345 return false;
00346 }
00347
00348 if (!sampler_)
00349 sampler_ = si_->allocValidStateSampler();
00350 if (!simpleSampler_)
00351 simpleSampler_ = si_->allocStateSampler();
00352
00353 approxsol_.reset();
00354 approxlen_ = -1.0;
00355
00356 msg_.inform("Starting with %u states", nrStartStates);
00357
00358 std::vector<base::State*> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
00359 si_->allocStates(xstates);
00360 std::pair<Vertex, Vertex> solEndpoints;
00361 unsigned int steps = 0;
00362
00363 while (ptc() == false)
00364 {
00365
00366 if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
00367 {
00368 const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
00369 if (st)
00370 goalM_.push_back(addMilestone(si_->cloneState(st)));
00371 if (goalM_.empty())
00372 {
00373 msg_.error("Unable to find any valid goal states");
00374 break;
00375 }
00376 }
00377
00378
00379 if (haveSolution(startM_, goalM_, &solEndpoints))
00380 {
00381 goal->setSolutionPath(constructSolution(solEndpoints.first, solEndpoints.second));
00382 break;
00383 }
00384
00385 else
00386 {
00387
00388
00389 if (steps < 4)
00390 {
00391 growRoadmap(startM_, goalM_, base::PlannerOrTerminationCondition(ptc, base::timedPlannerTerminationCondition(0.1)), xstates[0]);
00392 steps++;
00393 }
00394 else
00395 {
00396 expandRoadmap(startM_, goalM_, base::PlannerOrTerminationCondition(ptc, base::timedPlannerTerminationCondition(0.2)), xstates);
00397 steps = 0;
00398 }
00399
00400
00401 if (haveSolution(startM_, goalM_, &solEndpoints))
00402 {
00403 goal->setSolutionPath(constructSolution(solEndpoints.first, solEndpoints.second));
00404 break;
00405 }
00406 }
00407 }
00408 si_->freeStates(xstates);
00409
00410 msg_.inform("Created %u states", boost::num_vertices(g_) - nrStartStates);
00411
00412 if (!goal->getSolutionPath() && approxsol_)
00413 {
00414 goal->setSolutionPath(approxsol_, true);
00415
00416 goal->setDifference(0.0);
00417 }
00418
00419 return goal->isAchieved();
00420 }
00421
00422 ompl::geometric::PRM::Vertex ompl::geometric::PRM::addMilestone(base::State *state)
00423 {
00424 Vertex m = boost::add_vertex(g_);
00425 stateProperty_[m] = state;
00426 totalConnectionAttemptsProperty_[m] = 1;
00427 successfulConnectionAttemptsProperty_[m] = 0;
00428
00429
00430 disjointSets_.make_set(m);
00431
00432
00433 if (!connectionStrategy_)
00434 throw Exception(name_, "No connection strategy!");
00435
00436 const std::vector<Vertex>& neighbors = connectionStrategy_(m);
00437
00438 foreach (Vertex n, neighbors)
00439 if ((boost::same_component(m, n, disjointSets_) || connectionFilter_(m, n)))
00440 {
00441 totalConnectionAttemptsProperty_[m]++;
00442 totalConnectionAttemptsProperty_[n]++;
00443 if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
00444 {
00445 successfulConnectionAttemptsProperty_[m]++;
00446 successfulConnectionAttemptsProperty_[n]++;
00447 const double weight = distanceFunction(m, n);
00448 const unsigned int id = maxEdgeID_++;
00449 const Graph::edge_property_type properties(weight, id);
00450 boost::add_edge(m, n, properties, g_);
00451 uniteComponents(n, m);
00452 }
00453 }
00454
00455 nn_->add(m);
00456 return m;
00457 }
00458
00459 void ompl::geometric::PRM::uniteComponents(Vertex m1, Vertex m2)
00460 {
00461 disjointSets_.union_set(m1, m2);
00462 }
00463
00464 ompl::base::PathPtr ompl::geometric::PRM::constructSolution(const Vertex start, const Vertex goal)
00465 {
00466 PathGeometric *p = new PathGeometric(si_);
00467
00468 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
00469
00470 boost::astar_search(g_, start,
00471 boost::bind(&PRM::distanceFunction, this, _1, goal),
00472 boost::predecessor_map(prev));
00473
00474 if (prev[goal] == goal)
00475 throw Exception(name_, "Could not find solution path");
00476 else
00477 for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
00478 p->states.push_back(si_->cloneState(stateProperty_[pos]));
00479 p->states.push_back(si_->cloneState(stateProperty_[start]));
00480 p->reverse();
00481
00482 return base::PathPtr(p);
00483 }
00484
00485 void ompl::geometric::PRM::getPlannerData(base::PlannerData &data) const
00486 {
00487 Planner::getPlannerData(data);
00488
00489 foreach(const Edge e, boost::edges(g_))
00490 {
00491 const Vertex v1 = boost::source(e, g_);
00492 const Vertex v2 = boost::target(e, g_);
00493 data.recordEdge(stateProperty_[v1], stateProperty_[v2]);
00494 }
00495 }