ompl/geometric/planners/prm/src/PRM.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan, James D. Marble, Ryan Luna */ 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/base/objectives/PathLengthOptimizationObjective.h" 00041 #include "ompl/datastructures/PDF.h" 00042 #include "ompl/tools/config/SelfConfig.h" 00043 #include "ompl/tools/config/MagicConstants.h" 00044 #include <boost/lambda/bind.hpp> 00045 #include <boost/graph/astar_search.hpp> 00046 #include <boost/graph/incremental_components.hpp> 00047 #include <boost/property_map/vector_property_map.hpp> 00048 #include <boost/foreach.hpp> 00049 #include <boost/thread.hpp> 00050 00051 #include "GoalVisitor.hpp" 00052 00053 #define foreach BOOST_FOREACH 00054 00055 namespace ompl 00056 { 00057 namespace magic 00058 { 00059 00062 static const unsigned int MAX_RANDOM_BOUNCE_STEPS = 5; 00063 00065 static const double ROADMAP_BUILD_TIME = 0.2; 00066 00069 static const unsigned int DEFAULT_NEAREST_NEIGHBORS = 10; 00070 } 00071 } 00072 00073 ompl::geometric::PRM::PRM(const base::SpaceInformationPtr &si, bool starStrategy) : 00074 base::Planner(si, "PRM"), 00075 starStrategy_(starStrategy), 00076 stateProperty_(boost::get(vertex_state_t(), g_)), 00077 totalConnectionAttemptsProperty_(boost::get(vertex_total_connection_attempts_t(), g_)), 00078 successfulConnectionAttemptsProperty_(boost::get(vertex_successful_connection_attempts_t(), g_)), 00079 weightProperty_(boost::get(boost::edge_weight, g_)), 00080 disjointSets_(boost::get(boost::vertex_rank, g_), 00081 boost::get(boost::vertex_predecessor, g_)), 00082 userSetConnectionStrategy_(false), 00083 addedNewSolution_(false), 00084 iterations_(0), 00085 bestCost_(std::numeric_limits<double>::quiet_NaN()) 00086 { 00087 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION; 00088 specs_.approximateSolutions = false; 00089 specs_.optimizingPaths = true; 00090 00091 Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &PRM::setMaxNearestNeighbors, std::string("8:1000")); 00092 00093 addPlannerProgressProperty("iterations INTEGER", 00094 boost::bind(&PRM::getIterationCount, this)); 00095 addPlannerProgressProperty("best cost REAL", 00096 boost::bind(&PRM::getBestCost, this)); 00097 addPlannerProgressProperty("milestone count INTEGER", 00098 boost::bind(&PRM::getMilestoneCountString, this)); 00099 addPlannerProgressProperty("edge count INTEGER", 00100 boost::bind(&PRM::getEdgeCountString, this)); 00101 } 00102 00103 ompl::geometric::PRM::~PRM() 00104 { 00105 freeMemory(); 00106 } 00107 00108 void ompl::geometric::PRM::setup() 00109 { 00110 Planner::setup(); 00111 if (!nn_) 00112 { 00113 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace())); 00114 nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2)); 00115 } 00116 if (!connectionStrategy_) 00117 { 00118 if (starStrategy_) 00119 connectionStrategy_ = KStarStrategy<Vertex>(boost::bind(&PRM::milestoneCount, this), nn_, si_->getStateDimension()); 00120 else 00121 connectionStrategy_ = KStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS, nn_); 00122 } 00123 if (!connectionFilter_) 00124 connectionFilter_ = boost::lambda::constant(true); 00125 00126 // Setup optimization objective 00127 // 00128 // If no optimization objective was specified, then default to 00129 // optimizing path length as computed by the distance() function 00130 // in the state space. 00131 if (pdef_) 00132 { 00133 if (pdef_->hasOptimizationObjective()) 00134 opt_ = pdef_->getOptimizationObjective(); 00135 else 00136 { 00137 opt_.reset(new base::PathLengthOptimizationObjective(si_)); 00138 if (!starStrategy_) 00139 opt_->setCostThreshold(opt_->infiniteCost()); 00140 } 00141 } 00142 else 00143 { 00144 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str()); 00145 setup_ = false; 00146 } 00147 } 00148 00149 void ompl::geometric::PRM::setMaxNearestNeighbors(unsigned int k) 00150 { 00151 if (starStrategy_) 00152 throw Exception("Cannot set the maximum nearest neighbors for " + getName()); 00153 if (!nn_) 00154 { 00155 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace())); 00156 nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2)); 00157 } 00158 if (!userSetConnectionStrategy_) 00159 connectionStrategy_.clear(); 00160 if (isSetup()) 00161 setup(); 00162 } 00163 00164 void ompl::geometric::PRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef) 00165 { 00166 Planner::setProblemDefinition(pdef); 00167 clearQuery(); 00168 } 00169 00170 void ompl::geometric::PRM::clearQuery() 00171 { 00172 startM_.clear(); 00173 goalM_.clear(); 00174 pis_.restart(); 00175 } 00176 00177 void ompl::geometric::PRM::clear() 00178 { 00179 Planner::clear(); 00180 sampler_.reset(); 00181 simpleSampler_.reset(); 00182 freeMemory(); 00183 if (nn_) 00184 nn_->clear(); 00185 clearQuery(); 00186 00187 iterations_ = 0; 00188 bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN()); 00189 } 00190 00191 void ompl::geometric::PRM::freeMemory() 00192 { 00193 foreach (Vertex v, boost::vertices(g_)) 00194 si_->freeState(stateProperty_[v]); 00195 g_.clear(); 00196 } 00197 00198 void ompl::geometric::PRM::expandRoadmap(double expandTime) 00199 { 00200 expandRoadmap(base::timedPlannerTerminationCondition(expandTime)); 00201 } 00202 00203 void ompl::geometric::PRM::expandRoadmap(const base::PlannerTerminationCondition &ptc) 00204 { 00205 if (!simpleSampler_) 00206 simpleSampler_ = si_->allocStateSampler(); 00207 00208 std::vector<base::State*> states(magic::MAX_RANDOM_BOUNCE_STEPS); 00209 si_->allocStates(states); 00210 expandRoadmap(ptc, states); 00211 si_->freeStates(states); 00212 } 00213 00214 void ompl::geometric::PRM::expandRoadmap(const base::PlannerTerminationCondition &ptc, 00215 std::vector<base::State*> &workStates) 00216 { 00217 // construct a probability distribution over the vertices in the roadmap 00218 // as indicated in 00219 // "Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces" 00220 // Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars 00221 00222 PDF<Vertex> pdf; 00223 foreach (Vertex v, boost::vertices(g_)) 00224 { 00225 const unsigned long int t = totalConnectionAttemptsProperty_[v]; 00226 pdf.add(v, (double)(t - successfulConnectionAttemptsProperty_[v]) / (double)t); 00227 } 00228 00229 if (pdf.empty()) 00230 return; 00231 00232 while (ptc == false) 00233 { 00234 iterations_++; 00235 Vertex v = pdf.sample(rng_.uniform01()); 00236 unsigned int s = si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false); 00237 if (s > 0) 00238 { 00239 s--; 00240 Vertex last = addMilestone(si_->cloneState(workStates[s])); 00241 00242 graphMutex_.lock(); 00243 for (unsigned int i = 0 ; i < s ; ++i) 00244 { 00245 // add the vertex along the bouncing motion 00246 Vertex m = boost::add_vertex(g_); 00247 stateProperty_[m] = si_->cloneState(workStates[i]); 00248 totalConnectionAttemptsProperty_[m] = 1; 00249 successfulConnectionAttemptsProperty_[m] = 0; 00250 disjointSets_.make_set(m); 00251 00252 // add the edge to the parent vertex 00253 const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[m]); 00254 const Graph::edge_property_type properties(weight); 00255 boost::add_edge(v, m, properties, g_); 00256 uniteComponents(v, m); 00257 00258 // add the vertex to the nearest neighbors data structure 00259 nn_->add(m); 00260 v = m; 00261 } 00262 00263 // if there are intermediary states or the milestone has not been connected to the initially sampled vertex, 00264 // we add an edge 00265 if (s > 0 || !sameComponent(v, last)) 00266 { 00267 // add the edge to the parent vertex 00268 const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[last]); 00269 const Graph::edge_property_type properties(weight); 00270 boost::add_edge(v, last, properties, g_); 00271 uniteComponents(v, last); 00272 } 00273 graphMutex_.unlock(); 00274 } 00275 } 00276 } 00277 00278 void ompl::geometric::PRM::growRoadmap(double growTime) 00279 { 00280 growRoadmap(base::timedPlannerTerminationCondition(growTime)); 00281 } 00282 00283 void ompl::geometric::PRM::growRoadmap(const base::PlannerTerminationCondition &ptc) 00284 { 00285 if (!isSetup()) 00286 setup(); 00287 if (!sampler_) 00288 sampler_ = si_->allocValidStateSampler(); 00289 00290 base::State *workState = si_->allocState(); 00291 growRoadmap (ptc, workState); 00292 si_->freeState(workState); 00293 } 00294 00295 void ompl::geometric::PRM::growRoadmap(const base::PlannerTerminationCondition &ptc, 00296 base::State *workState) 00297 { 00298 /* grow roadmap in the regular fashion -- sample valid states, add them to the roadmap, add valid connections */ 00299 while (ptc == false) 00300 { 00301 iterations_++; 00302 // search for a valid state 00303 bool found = false; 00304 while (!found && ptc == false) 00305 { 00306 unsigned int attempts = 0; 00307 do 00308 { 00309 found = sampler_->sample(workState); 00310 attempts++; 00311 } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found); 00312 } 00313 // add it as a milestone 00314 if (found) 00315 addMilestone(si_->cloneState(workState)); 00316 } 00317 } 00318 00319 void ompl::geometric::PRM::checkForSolution(const base::PlannerTerminationCondition &ptc, 00320 base::PathPtr &solution) 00321 { 00322 base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get()); 00323 while (!ptc && !addedNewSolution_) 00324 { 00325 // Check for any new goal states 00326 if (goal->maxSampleCount() > goalM_.size()) 00327 { 00328 const base::State *st = pis_.nextGoal(); 00329 if (st) 00330 goalM_.push_back(addMilestone(si_->cloneState(st))); 00331 } 00332 00333 // Check for a solution 00334 addedNewSolution_ = maybeConstructSolution(startM_, goalM_, solution); 00335 // Sleep for 1ms 00336 if (!addedNewSolution_) 00337 boost::this_thread::sleep(boost::posix_time::milliseconds(1)); 00338 } 00339 } 00340 00341 bool ompl::geometric::PRM::maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution) 00342 { 00343 base::Goal *g = pdef_->getGoal().get(); 00344 base::Cost sol_cost(opt_->infiniteCost()); 00345 foreach (Vertex start, starts) 00346 { 00347 foreach (Vertex goal, goals) 00348 { 00349 // we lock because the connected components algorithm is incremental and may change disjointSets_ 00350 graphMutex_.lock(); 00351 bool same_component = sameComponent(start, goal); 00352 graphMutex_.unlock(); 00353 00354 if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start])) 00355 { 00356 base::PathPtr p = constructSolution(start, goal); 00357 if (p) 00358 { 00359 base::Cost pathCost = p->cost(opt_); 00360 if (opt_->isCostBetterThan(pathCost, bestCost_)) 00361 bestCost_ = pathCost; 00362 // Check if optimization objective is satisfied 00363 if (opt_->isSatisfied(pathCost)) 00364 { 00365 solution = p; 00366 return true; 00367 } 00368 else if (opt_->isCostBetterThan(pathCost, sol_cost)) 00369 { 00370 solution = p; 00371 sol_cost = pathCost; 00372 } 00373 } 00374 } 00375 } 00376 } 00377 00378 return false; 00379 } 00380 00381 bool ompl::geometric::PRM::addedNewSolution() const 00382 { 00383 return addedNewSolution_; 00384 } 00385 00386 ompl::base::PlannerStatus ompl::geometric::PRM::solve(const base::PlannerTerminationCondition &ptc) 00387 { 00388 checkValidity(); 00389 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get()); 00390 00391 if (!goal) 00392 { 00393 OMPL_ERROR("%s: Unknown type of goal", getName().c_str()); 00394 return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE; 00395 } 00396 00397 // Add the valid start states as milestones 00398 while (const base::State *st = pis_.nextStart()) 00399 startM_.push_back(addMilestone(si_->cloneState(st))); 00400 00401 if (startM_.size() == 0) 00402 { 00403 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); 00404 return base::PlannerStatus::INVALID_START; 00405 } 00406 00407 if (!goal->couldSample()) 00408 { 00409 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str()); 00410 return base::PlannerStatus::INVALID_GOAL; 00411 } 00412 00413 // Ensure there is at least one valid goal state 00414 if (goal->maxSampleCount() > goalM_.size() || goalM_.empty()) 00415 { 00416 const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal(); 00417 if (st) 00418 goalM_.push_back(addMilestone(si_->cloneState(st))); 00419 00420 if (goalM_.empty()) 00421 { 00422 OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str()); 00423 return base::PlannerStatus::INVALID_GOAL; 00424 } 00425 } 00426 00427 unsigned long int nrStartStates = boost::num_vertices(g_); 00428 OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates); 00429 00430 // Reset addedNewSolution_ member and create solution checking thread 00431 addedNewSolution_ = false; 00432 base::PathPtr sol; 00433 boost::thread slnThread(boost::bind(&PRM::checkForSolution, this, ptc, boost::ref(sol))); 00434 00435 // construct new planner termination condition that fires when the given ptc is true, or a solution is found 00436 base::PlannerTerminationCondition ptcOrSolutionFound = 00437 base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&PRM::addedNewSolution, this))); 00438 00439 constructRoadmap(ptcOrSolutionFound); 00440 00441 // Ensure slnThread is ceased before exiting solve 00442 slnThread.join(); 00443 00444 OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates); 00445 00446 if (sol) 00447 { 00448 base::PlannerSolution psol(sol); 00449 psol.setPlannerName(getName()); 00450 // if the solution was optimized, we mark it as such 00451 psol.setOptimized(opt_, bestCost_, addedNewSolution()); 00452 pdef_->addSolutionPath(psol); 00453 } 00454 00455 return sol ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; 00456 } 00457 00458 void ompl::geometric::PRM::constructRoadmap(const base::PlannerTerminationCondition &ptc) 00459 { 00460 if (!isSetup()) 00461 setup(); 00462 if (!sampler_) 00463 sampler_ = si_->allocValidStateSampler(); 00464 if (!simpleSampler_) 00465 simpleSampler_ = si_->allocStateSampler(); 00466 00467 std::vector<base::State*> xstates(magic::MAX_RANDOM_BOUNCE_STEPS); 00468 si_->allocStates(xstates); 00469 bool grow = true; 00470 00471 bestCost_ = opt_->infiniteCost(); 00472 while (ptc() == false) 00473 { 00474 // maintain a 2:1 ratio for growing/expansion of roadmap 00475 // call growRoadmap() twice as long for every call of expandRoadmap() 00476 if (grow) 00477 growRoadmap(base::plannerOrTerminationCondition(ptc, base::timedPlannerTerminationCondition(2.0 * magic::ROADMAP_BUILD_TIME)), xstates[0]); 00478 else 00479 expandRoadmap(base::plannerOrTerminationCondition(ptc, base::timedPlannerTerminationCondition(magic::ROADMAP_BUILD_TIME)), xstates); 00480 grow = !grow; 00481 } 00482 00483 si_->freeStates(xstates); 00484 } 00485 00486 ompl::geometric::PRM::Vertex ompl::geometric::PRM::addMilestone(base::State *state) 00487 { 00488 boost::mutex::scoped_lock _(graphMutex_); 00489 00490 Vertex m = boost::add_vertex(g_); 00491 stateProperty_[m] = state; 00492 totalConnectionAttemptsProperty_[m] = 1; 00493 successfulConnectionAttemptsProperty_[m] = 0; 00494 00495 // Initialize to its own (dis)connected component. 00496 disjointSets_.make_set(m); 00497 00498 // Which milestones will we attempt to connect to? 00499 const std::vector<Vertex>& neighbors = connectionStrategy_(m); 00500 00501 foreach (Vertex n, neighbors) 00502 if (connectionFilter_(n, m)) 00503 { 00504 totalConnectionAttemptsProperty_[m]++; 00505 totalConnectionAttemptsProperty_[n]++; 00506 if (si_->checkMotion(stateProperty_[n], stateProperty_[m])) 00507 { 00508 successfulConnectionAttemptsProperty_[m]++; 00509 successfulConnectionAttemptsProperty_[n]++; 00510 const base::Cost weight = opt_->motionCost(stateProperty_[n], stateProperty_[m]); 00511 const Graph::edge_property_type properties(weight); 00512 boost::add_edge(n, m, properties, g_); 00513 uniteComponents(n, m); 00514 } 00515 } 00516 00517 nn_->add(m); 00518 00519 return m; 00520 } 00521 00522 void ompl::geometric::PRM::uniteComponents(Vertex m1, Vertex m2) 00523 { 00524 disjointSets_.union_set(m1, m2); 00525 } 00526 00527 bool ompl::geometric::PRM::sameComponent(Vertex m1, Vertex m2) 00528 { 00529 return boost::same_component(m1, m2, disjointSets_); 00530 } 00531 00532 ompl::base::PathPtr ompl::geometric::PRM::constructSolution(const Vertex &start, const Vertex &goal) 00533 { 00534 boost::mutex::scoped_lock _(graphMutex_); 00535 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_)); 00536 00537 try 00538 { 00539 // Consider using a persistent distance_map if it's slow 00540 boost::astar_search(g_, start, 00541 boost::bind(&PRM::costHeuristic, this, _1, goal), 00542 boost::predecessor_map(prev). 00543 distance_compare(boost::bind(&base::OptimizationObjective:: 00544 isCostBetterThan, opt_.get(), _1, _2)). 00545 distance_combine(boost::bind(&base::OptimizationObjective:: 00546 combineCosts, opt_.get(), _1, _2)). 00547 distance_inf(opt_->infiniteCost()). 00548 distance_zero(opt_->identityCost()). 00549 visitor(AStarGoalVisitor<Vertex>(goal))); 00550 } 00551 catch (AStarFoundGoal&) 00552 { 00553 } 00554 00555 if (prev[goal] == goal) 00556 throw Exception(name_, "Could not find solution path"); 00557 00558 PathGeometric *p = new PathGeometric(si_); 00559 for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos]) 00560 p->append(stateProperty_[pos]); 00561 p->append(stateProperty_[start]); 00562 p->reverse(); 00563 00564 return base::PathPtr(p); 00565 } 00566 00567 void ompl::geometric::PRM::getPlannerData(base::PlannerData &data) const 00568 { 00569 Planner::getPlannerData(data); 00570 00571 // Explicitly add start and goal states: 00572 for (size_t i = 0; i < startM_.size(); ++i) 00573 data.addStartVertex(base::PlannerDataVertex(stateProperty_[startM_[i]], const_cast<PRM*>(this)->disjointSets_.find_set(startM_[i]))); 00574 00575 for (size_t i = 0; i < goalM_.size(); ++i) 00576 data.addGoalVertex(base::PlannerDataVertex(stateProperty_[goalM_[i]], const_cast<PRM*>(this)->disjointSets_.find_set(goalM_[i]))); 00577 00578 // Adding edges and all other vertices simultaneously 00579 foreach(const Edge e, boost::edges(g_)) 00580 { 00581 const Vertex v1 = boost::source(e, g_); 00582 const Vertex v2 = boost::target(e, g_); 00583 data.addEdge(base::PlannerDataVertex(stateProperty_[v1]), 00584 base::PlannerDataVertex(stateProperty_[v2])); 00585 00586 // Add the reverse edge, since we're constructing an undirected roadmap 00587 data.addEdge(base::PlannerDataVertex(stateProperty_[v2]), 00588 base::PlannerDataVertex(stateProperty_[v1])); 00589 00590 // Add tags for the newly added vertices 00591 data.tagState(stateProperty_[v1], const_cast<PRM*>(this)->disjointSets_.find_set(v1)); 00592 data.tagState(stateProperty_[v2], const_cast<PRM*>(this)->disjointSets_.find_set(v2)); 00593 } 00594 } 00595 00596 ompl::base::Cost ompl::geometric::PRM::costHeuristic(Vertex u, Vertex v) const 00597 { 00598 return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]); 00599 }