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