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