ompl/geometric/planners/prm/src/SPARS.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick 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 Rutgers 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: Andrew Dobson */ 00036 00037 #include "ompl/geometric/planners/prm/SPARS.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/tools/config/SelfConfig.h" 00042 #include "ompl/tools/config/MagicConstants.h" 00043 #include <boost/bind.hpp> 00044 #include <boost/graph/astar_search.hpp> 00045 #include <boost/graph/incremental_components.hpp> 00046 #include <boost/property_map/vector_property_map.hpp> 00047 #include <boost/foreach.hpp> 00048 00049 #include "GoalVisitor.hpp" 00050 00051 #define foreach BOOST_FOREACH 00052 #define foreach_reverse BOOST_REVERSE_FOREACH 00053 00054 ompl::geometric::SPARS::SPARS(const base::SpaceInformationPtr &si) : 00055 base::Planner(si, "SPARS"), 00056 geomPath_(si), 00057 stateProperty_(boost::get(vertex_state_t(), g_)), 00058 sparseStateProperty_(boost::get(vertex_state_t(), s_)), 00059 sparseColorProperty_(boost::get(vertex_color_t(), s_)), 00060 representativesProperty_(boost::get(vertex_representative_t(), g_)), 00061 nonInterfaceListsProperty_(boost::get(vertex_list_t(), s_)), 00062 interfaceListsProperty_(boost::get(vertex_interface_list_t(), s_)), 00063 weightProperty_(boost::get(boost::edge_weight, g_)), 00064 sparseDJSets_(boost::get(boost::vertex_rank, s_), 00065 boost::get(boost::vertex_predecessor, s_)), 00066 consecutiveFailures_(0), 00067 stretchFactor_(3.), 00068 maxFailures_(1000), 00069 addedSolution_(false), 00070 denseDeltaFraction_(.001), 00071 sparseDeltaFraction_(.25), 00072 denseDelta_(0.), 00073 sparseDelta_(0.), 00074 iterations_(0), 00075 bestCost_(std::numeric_limits<double>::quiet_NaN()) 00076 { 00077 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION; 00078 specs_.approximateSolutions = false; 00079 specs_.optimizingPaths = true; 00080 specs_.multithreaded = true; 00081 00082 psimp_.reset(new PathSimplifier(si_)); 00083 psimp_->freeStates(false); 00084 00085 Planner::declareParam<double>("stretch_factor", this, &SPARS::setStretchFactor, &SPARS::getStretchFactor, "1.1:0.1:3.0"); 00086 Planner::declareParam<double>("sparse_delta_fraction", this, &SPARS::setSparseDeltaFraction, &SPARS::getSparseDeltaFraction, "0.0:0.01:1.0"); 00087 Planner::declareParam<double>("dense_delta_fraction", this, &SPARS::setDenseDeltaFraction, &SPARS::getDenseDeltaFraction, "0.0:0.0001:0.1"); 00088 Planner::declareParam<unsigned int>("max_failures", this, &SPARS::setMaxFailures, &SPARS::getMaxFailures, "100:10:3000"); 00089 00090 addPlannerProgressProperty("iterations INTEGER", 00091 boost::bind(&SPARS::getIterationCount, this)); 00092 addPlannerProgressProperty("best cost REAL", 00093 boost::bind(&SPARS::getBestCost, this)); 00094 } 00095 00096 ompl::geometric::SPARS::~SPARS() 00097 { 00098 freeMemory(); 00099 } 00100 00101 void ompl::geometric::SPARS::setup() 00102 { 00103 Planner::setup(); 00104 if (!nn_) 00105 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<DenseVertex>(si_->getStateSpace())); 00106 nn_->setDistanceFunction(boost::bind(&SPARS::distanceFunction, this, _1, _2)); 00107 if (!snn_) 00108 snn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<SparseVertex>(si_->getStateSpace())); 00109 snn_->setDistanceFunction(boost::bind(&SPARS::sparseDistanceFunction, this, _1, _2)); 00110 if (!connectionStrategy_) 00111 connectionStrategy_ = KStarStrategy<DenseVertex>(boost::bind(&SPARS::milestoneCount, this), nn_, si_->getStateDimension()); 00112 double maxExt = si_->getMaximumExtent(); 00113 sparseDelta_ = sparseDeltaFraction_ * maxExt; 00114 denseDelta_ = denseDeltaFraction_ * maxExt; 00115 00116 // Setup optimization objective 00117 // 00118 // If no optimization objective was specified, then default to 00119 // optimizing path length as computed by the distance() function 00120 // in the state space. 00121 if (pdef_) 00122 { 00123 if (pdef_->hasOptimizationObjective()) 00124 { 00125 opt_ = pdef_->getOptimizationObjective(); 00126 if (!dynamic_cast<base::PathLengthOptimizationObjective*>(opt_.get())) 00127 OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence for other optimizaton objectives is not guaranteed.", getName().c_str()); 00128 } 00129 else 00130 opt_.reset(new base::PathLengthOptimizationObjective(si_)); 00131 } 00132 else 00133 { 00134 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str()); 00135 setup_ = false; 00136 } 00137 } 00138 00139 void ompl::geometric::SPARS::setProblemDefinition(const base::ProblemDefinitionPtr &pdef) 00140 { 00141 Planner::setProblemDefinition(pdef); 00142 clearQuery(); 00143 } 00144 00145 void ompl::geometric::SPARS::resetFailures() 00146 { 00147 consecutiveFailures_ = 0; 00148 } 00149 00150 void ompl::geometric::SPARS::clearQuery() 00151 { 00152 startM_.clear(); 00153 goalM_.clear(); 00154 pis_.restart(); 00155 } 00156 00157 void ompl::geometric::SPARS::clear() 00158 { 00159 Planner::clear(); 00160 sampler_.reset(); 00161 simpleSampler_.reset(); 00162 freeMemory(); 00163 if (nn_) 00164 nn_->clear(); 00165 if (snn_) 00166 snn_->clear(); 00167 clearQuery(); 00168 resetFailures(); 00169 iterations_ = 0; 00170 bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN()); 00171 } 00172 00173 void ompl::geometric::SPARS::freeMemory() 00174 { 00175 foreach (DenseVertex v, boost::vertices(g_)) 00176 if( stateProperty_[v] != NULL ) 00177 { 00178 si_->freeState(stateProperty_[v]); 00179 stateProperty_[v] = NULL; 00180 } 00181 foreach (SparseVertex n, boost::vertices(s_)) 00182 if( sparseStateProperty_[n] != NULL ) 00183 { 00184 si_->freeState(sparseStateProperty_[n]); 00185 sparseStateProperty_[n] = NULL; 00186 } 00187 s_.clear(); 00188 g_.clear(); 00189 } 00190 00191 ompl::geometric::SPARS::DenseVertex ompl::geometric::SPARS::addSample(base::State *workState, const base::PlannerTerminationCondition &ptc) 00192 { 00193 DenseVertex result = boost::graph_traits<DenseGraph>::null_vertex(); 00194 00195 // search for a valid state 00196 bool found = false; 00197 while (!found && ptc == false) 00198 { 00199 unsigned int attempts = 0; 00200 do 00201 { 00202 found = sampler_->sample(workState); 00203 attempts++; 00204 } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found); 00205 } 00206 00207 if (found) 00208 result = addMilestone(si_->cloneState(workState)); 00209 return result; 00210 } 00211 00212 void ompl::geometric::SPARS::checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution) 00213 { 00214 base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get()); 00215 while (!ptc && !addedSolution_) 00216 { 00217 // Check for any new goal states 00218 if (goal->maxSampleCount() > goalM_.size()) 00219 { 00220 const base::State *st = pis_.nextGoal(); 00221 if (st) 00222 { 00223 addMilestone(si_->cloneState(st)); 00224 goalM_.push_back(addGuard(si_->cloneState(st), GOAL)); 00225 } 00226 } 00227 00228 // Check for a solution 00229 addedSolution_ = haveSolution(startM_, goalM_, solution); 00230 // Sleep for 1ms 00231 if (!addedSolution_) 00232 boost::this_thread::sleep(boost::posix_time::milliseconds(1)); 00233 } 00234 } 00235 00236 bool ompl::geometric::SPARS::haveSolution(const std::vector<DenseVertex> &starts, const std::vector<DenseVertex> &goals, base::PathPtr &solution) 00237 { 00238 base::Goal *g = pdef_->getGoal().get(); 00239 base::Cost sol_cost(opt_->infiniteCost()); 00240 foreach (DenseVertex start, starts) 00241 { 00242 foreach (DenseVertex goal, goals) 00243 { 00244 // we lock because the connected components algorithm is incremental and may change disjointSets_ 00245 graphMutex_.lock(); 00246 bool same_component = sameComponent(start, goal); 00247 graphMutex_.unlock(); 00248 00249 if (same_component && g->isStartGoalPairValid(sparseStateProperty_[goal], sparseStateProperty_[start])) 00250 { 00251 base::PathPtr p = constructSolution(start, goal); 00252 if (p) 00253 { 00254 base::Cost pathCost = p->cost(opt_); 00255 if (opt_->isCostBetterThan(pathCost, bestCost_)) 00256 bestCost_ = pathCost; 00257 // Check if optimization objective is satisfied 00258 if (opt_->isSatisfied(pathCost)) 00259 { 00260 solution = p; 00261 return true; 00262 } 00263 else if (opt_->isCostBetterThan(pathCost, sol_cost)) 00264 { 00265 solution = p; 00266 sol_cost = pathCost; 00267 } 00268 } 00269 } 00270 } 00271 } 00272 00273 return false; 00274 } 00275 00276 bool ompl::geometric::SPARS::reachedTerminationCriterion() const 00277 { 00278 return consecutiveFailures_ >= maxFailures_ || addedSolution_; 00279 } 00280 00281 bool ompl::geometric::SPARS::reachedFailureLimit() const 00282 { 00283 return consecutiveFailures_ >= maxFailures_; 00284 } 00285 00286 void ompl::geometric::SPARS::checkQueryStateInitialization() 00287 { 00288 boost::mutex::scoped_lock _(graphMutex_); 00289 if (boost::num_vertices(g_) < 1) 00290 { 00291 sparseQueryVertex_ = boost::add_vertex(s_); 00292 queryVertex_ = boost::add_vertex(g_); 00293 sparseStateProperty_[sparseQueryVertex_] = NULL; 00294 stateProperty_[queryVertex_] = NULL; 00295 } 00296 } 00297 00298 bool ompl::geometric::SPARS::sameComponent(SparseVertex m1, SparseVertex m2) 00299 { 00300 return boost::same_component(m1, m2, sparseDJSets_); 00301 } 00302 00303 ompl::base::PlannerStatus ompl::geometric::SPARS::solve(const base::PlannerTerminationCondition &ptc) 00304 { 00305 checkValidity(); 00306 checkQueryStateInitialization(); 00307 00308 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get()); 00309 00310 if (!goal) 00311 { 00312 OMPL_ERROR("%s: Unknown type of goal", getName().c_str()); 00313 return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE; 00314 } 00315 00316 // Add the valid start states as milestones 00317 while (const base::State *st = pis_.nextStart()) 00318 { 00319 addMilestone(si_->cloneState(st)); 00320 startM_.push_back(addGuard(si_->cloneState(st), START )); 00321 } 00322 if (startM_.empty()) 00323 { 00324 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); 00325 return base::PlannerStatus::INVALID_START; 00326 } 00327 00328 if (goalM_.empty() && !goal->couldSample()) 00329 { 00330 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str()); 00331 return base::PlannerStatus::INVALID_GOAL; 00332 } 00333 00334 // Add the valid goal states as milestones 00335 while (const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal())) 00336 { 00337 addMilestone(si_->cloneState(st)); 00338 goalM_.push_back(addGuard(si_->cloneState(st), GOAL )); 00339 } 00340 if (goalM_.empty()) 00341 { 00342 OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str()); 00343 return base::PlannerStatus::INVALID_GOAL; 00344 } 00345 00346 unsigned int nrStartStatesDense = boost::num_vertices(g_) - 1; // don't count query vertex 00347 unsigned int nrStartStatesSparse = boost::num_vertices(s_) - 1; // don't count query vertex 00348 OMPL_INFORM("%s: Starting planning with %u dense states, %u sparse states", getName().c_str(), nrStartStatesDense, nrStartStatesSparse); 00349 00350 // Reset addedSolution_ member 00351 addedSolution_ = false; 00352 resetFailures(); 00353 base::PathPtr sol; 00354 base::PlannerTerminationCondition ptcOrFail = 00355 base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARS::reachedFailureLimit, this))); 00356 boost::thread slnThread(boost::bind(&SPARS::checkForSolution, this, ptcOrFail, boost::ref(sol))); 00357 00358 // Construct planner termination condition which also takes maxFailures_ and addedSolution_ into account 00359 base::PlannerTerminationCondition ptcOrStop = 00360 base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARS::reachedTerminationCriterion, this))); 00361 constructRoadmap(ptcOrStop); 00362 00363 // Ensure slnThread is ceased before exiting solve 00364 slnThread.join(); 00365 00366 if (sol) 00367 pdef_->addSolutionPath(sol, false, -1.0, getName()); 00368 00369 OMPL_INFORM("%s: Created %u dense states, %u sparse states", getName().c_str(), 00370 (unsigned int)(boost::num_vertices(g_) - nrStartStatesDense), 00371 (unsigned int)(boost::num_vertices(s_) - nrStartStatesSparse)); 00372 00373 // Return true if any solution was found. 00374 return sol ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; 00375 } 00376 00377 void ompl::geometric::SPARS::constructRoadmap(const base::PlannerTerminationCondition &ptc, bool stopOnMaxFail) 00378 { 00379 if (stopOnMaxFail) 00380 { 00381 resetFailures(); 00382 base::PlannerTerminationCondition ptcOrFail = 00383 base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARS::reachedFailureLimit, this))); 00384 constructRoadmap(ptcOrFail); 00385 } 00386 else 00387 constructRoadmap(ptc); 00388 } 00389 00390 void ompl::geometric::SPARS::constructRoadmap(const base::PlannerTerminationCondition &ptc) 00391 { 00392 checkQueryStateInitialization(); 00393 00394 if (!isSetup()) 00395 setup(); 00396 if (!sampler_) 00397 sampler_ = si_->allocValidStateSampler(); 00398 if (!simpleSampler_) 00399 simpleSampler_ = si_->allocStateSampler(); 00400 00401 base::State *workState = si_->allocState(); 00402 00403 /* The whole neighborhood set which has been most recently computed */ 00404 std::vector<SparseVertex> graphNeighborhood; 00405 00406 /* The visible neighborhood set which has been most recently computed */ 00407 std::vector<SparseVertex> visibleNeighborhood; 00408 00409 /* Storage for the interface neighborhood, populated by getInterfaceNeighborhood() */ 00410 std::vector<DenseVertex> interfaceNeighborhood; 00411 00412 bestCost_ = opt_->infiniteCost(); 00413 while (ptc == false) 00414 { 00415 iterations_++; 00416 00417 // Generate a single sample, and attempt to connect it to nearest neighbors. 00418 DenseVertex q = addSample(workState, ptc); 00419 if (q == boost::graph_traits<DenseGraph>::null_vertex()) 00420 continue; 00421 00422 //Now that we've added to D, try adding to S 00423 //Start by figuring out who our neighbors are 00424 getSparseNeighbors(workState, graphNeighborhood); 00425 filterVisibleNeighbors(workState, graphNeighborhood, visibleNeighborhood); 00426 //Check for addition for Coverage 00427 if( !checkAddCoverage(workState, graphNeighborhood)) 00428 //If not for Coverage, then Connectivity 00429 if( !checkAddConnectivity(workState, graphNeighborhood)) 00430 //Check for the existence of an interface 00431 if( !checkAddInterface(graphNeighborhood, visibleNeighborhood, q)) 00432 { 00433 // Then check to see if it's on an interface 00434 getInterfaceNeighborhood(q, interfaceNeighborhood); 00435 if (interfaceNeighborhood.size() > 0) 00436 { 00437 //Check for addition for spanner prop 00438 if (!checkAddPath(q, interfaceNeighborhood)) 00439 //All of the tests have failed. Report failure for the sample 00440 ++consecutiveFailures_; 00441 } 00442 else 00443 //There's no interface here, so drop it 00444 ++consecutiveFailures_; 00445 } 00446 } 00447 00448 si_->freeState(workState); 00449 } 00450 00451 ompl::geometric::SPARS::DenseVertex ompl::geometric::SPARS::addMilestone(base::State *state) 00452 { 00453 boost::mutex::scoped_lock _(graphMutex_); 00454 00455 DenseVertex m = boost::add_vertex(g_); 00456 stateProperty_[m] = state; 00457 00458 // Which milestones will we attempt to connect to? 00459 const std::vector<DenseVertex>& neighbors = connectionStrategy_(m); 00460 00461 foreach (DenseVertex n, neighbors) 00462 if (si_->checkMotion(stateProperty_[m], stateProperty_[n])) 00463 { 00464 const double weight = distanceFunction(m, n); 00465 const DenseGraph::edge_property_type properties(weight); 00466 00467 boost::add_edge(m, n, properties, g_); 00468 } 00469 00470 nn_->add(m); 00471 00472 //Need to update representative information here... 00473 calculateRepresentative(m); 00474 00475 std::vector<DenseVertex> interfaceNeighborhood; 00476 std::set<SparseVertex> interfaceRepresentatives; 00477 00478 getInterfaceNeighborRepresentatives(m, interfaceRepresentatives); 00479 getInterfaceNeighborhood(m, interfaceNeighborhood); 00480 addToRepresentatives(m, representativesProperty_[m], interfaceRepresentatives); 00481 foreach (DenseVertex qp, interfaceNeighborhood) 00482 { 00483 removeFromRepresentatives( qp, representativesProperty_[qp] ); 00484 getInterfaceNeighborRepresentatives( qp, interfaceRepresentatives ); 00485 addToRepresentatives( qp, representativesProperty_[qp], interfaceRepresentatives ); 00486 } 00487 00488 return m; 00489 } 00490 00491 ompl::geometric::SPARS::SparseVertex ompl::geometric::SPARS::addGuard(base::State *state, GuardType type) 00492 { 00493 boost::mutex::scoped_lock _(graphMutex_); 00494 00495 SparseVertex v = boost::add_vertex(s_); 00496 sparseStateProperty_[v] = state; 00497 sparseColorProperty_[v] = type; 00498 00499 sparseDJSets_.make_set(v); 00500 00501 snn_->add(v); 00502 updateRepresentatives(v); 00503 00504 resetFailures(); 00505 return v; 00506 } 00507 00508 void ompl::geometric::SPARS::connectSparsePoints(SparseVertex v, SparseVertex vp) 00509 { 00510 const base::Cost weight(costHeuristic(v, vp)); 00511 const SpannerGraph::edge_property_type properties(weight); 00512 boost::mutex::scoped_lock _(graphMutex_); 00513 boost::add_edge(v, vp, properties, s_); 00514 sparseDJSets_.union_set(v, vp); 00515 } 00516 00517 void ompl::geometric::SPARS::connectDensePoints( DenseVertex v, DenseVertex vp ) 00518 { 00519 const double weight = distanceFunction(v, vp); 00520 const DenseGraph::edge_property_type properties(weight); 00521 boost::mutex::scoped_lock _(graphMutex_); 00522 boost::add_edge(v, vp, properties, g_); 00523 } 00524 00525 bool ompl::geometric::SPARS::checkAddCoverage(const base::State *lastState, const std::vector<SparseVertex>& neigh ) 00526 { 00527 //For each of these neighbors, 00528 foreach (SparseVertex n, neigh) 00529 //If path between is free 00530 if (si_->checkMotion( lastState, sparseStateProperty_[n])) 00531 //Abort out and return false 00532 return false; 00533 //No free paths means we add for coverage 00534 addGuard(si_->cloneState(lastState), COVERAGE); 00535 return true; 00536 } 00537 00538 bool ompl::geometric::SPARS::checkAddConnectivity( const base::State *lastState, const std::vector<SparseVertex>& neigh ) 00539 { 00540 std::vector< SparseVertex > links; 00541 //For each neighbor 00542 for (std::size_t i = 0; i < neigh.size(); ++i ) 00543 //For each other neighbor 00544 for (std::size_t j = i + 1; j < neigh.size(); ++j ) 00545 //If they are in different components 00546 if (!sameComponent(neigh[i], neigh[j])) 00547 //If the paths between are collision free 00548 if( si_->checkMotion( lastState, sparseStateProperty_[neigh[i]] ) && si_->checkMotion( lastState, sparseStateProperty_[neigh[j]] ) ) 00549 { 00550 links.push_back( neigh[i] ); 00551 links.push_back( neigh[j] ); 00552 } 00553 00554 if( links.size() != 0 ) 00555 { 00556 //Add the node 00557 SparseVertex g = addGuard( si_->cloneState(lastState), CONNECTIVITY ); 00558 00559 for (std::size_t i = 0; i < links.size(); ++i ) 00560 //If there's no edge 00561 if (!boost::edge(g, links[i], s_).second) 00562 //And the components haven't been united by previous links 00563 if (!sameComponent(links[i], g)) 00564 connectSparsePoints( g, links[i] ); 00565 return true; 00566 } 00567 return false; 00568 } 00569 00570 bool ompl::geometric::SPARS::checkAddInterface(const std::vector<SparseVertex>& graphNeighborhood, const std::vector<SparseVertex>& visibleNeighborhood, DenseVertex q ) 00571 { 00572 //If we have more than 1 neighbor 00573 if( visibleNeighborhood.size() > 1 ) 00574 //If our closest neighbors are also visible 00575 if( graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1] ) 00576 //If our two closest neighbors don't share an edge 00577 if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], s_).second) 00578 { 00579 //If they can be directly connected 00580 if( si_->checkMotion( sparseStateProperty_[visibleNeighborhood[0]], sparseStateProperty_[visibleNeighborhood[1]] ) ) 00581 { 00582 //Connect them 00583 connectSparsePoints( visibleNeighborhood[0], visibleNeighborhood[1] ); 00584 //And report that we added to the roadmap 00585 resetFailures(); 00586 //Report success 00587 return true; 00588 } 00589 else 00590 { 00591 //Add the new node to the graph, to bridge the interface 00592 SparseVertex v = addGuard( si_->cloneState( stateProperty_[q] ), INTERFACE ); 00593 connectSparsePoints( v, visibleNeighborhood[0] ); 00594 connectSparsePoints( v, visibleNeighborhood[1] ); 00595 //Report success 00596 return true; 00597 } 00598 } 00599 return false; 00600 } 00601 00602 bool ompl::geometric::SPARS::checkAddPath(DenseVertex q, const std::vector<DenseVertex>& neigh) 00603 { 00604 bool result = false; 00605 00606 //Get q's representative => v 00607 SparseVertex v = representativesProperty_[q]; 00608 00609 //Extract the representatives of neigh => n_rep 00610 std::set<SparseVertex> n_rep; 00611 foreach( DenseVertex qp, neigh ) 00612 n_rep.insert(representativesProperty_[qp]); 00613 00614 std::vector<SparseVertex> Xs; 00615 //for each v' in n_rep 00616 for (std::set<SparseVertex>::iterator it = n_rep.begin() ; it != n_rep.end() && !result ; ++it) 00617 { 00618 SparseVertex vp = *it; 00619 //Identify appropriate v" candidates => vpps 00620 std::vector<SparseVertex> VPPs; 00621 computeVPP(v, vp, VPPs); 00622 00623 foreach( SparseVertex vpp, VPPs ) 00624 { 00625 double s_max = 0; 00626 //Find the X nodes to test 00627 computeX(v, vp, vpp, Xs); 00628 00629 //For each x in xs 00630 foreach( SparseVertex x, Xs ) 00631 { 00632 //Compute/Retain MAXimum distance path thorugh S 00633 double dist = (si_->distance(sparseStateProperty_[x], sparseStateProperty_[v]) 00634 + si_->distance(sparseStateProperty_[v], sparseStateProperty_[vp])) / 2.0; 00635 if( dist > s_max ) 00636 s_max = dist; 00637 } 00638 00639 DensePath bestDPath; 00640 DenseVertex best_qpp = boost::graph_traits<DenseGraph>::null_vertex(); 00641 double d_min = std::numeric_limits<double>::infinity(); //Insanely big number 00642 //For each vpp in vpps 00643 for (std::size_t j = 0; j < VPPs.size() && !result; ++j) 00644 { 00645 SparseVertex vpp = VPPs[j]; 00646 //For each q", which are stored interface nodes on v for i(vpp,v) 00647 foreach( DenseVertex qpp, interfaceListsProperty_[v].interfaceHash[vpp] ) 00648 { 00649 // check that representatives are consistent 00650 assert(representativesProperty_[qpp] == v); 00651 00652 //If they happen to be the one and same node 00653 if (q == qpp) 00654 { 00655 bestDPath.push_front( stateProperty_[q] ); 00656 best_qpp = qpp; 00657 d_min = 0; 00658 } 00659 else 00660 { 00661 //Compute/Retain MINimum distance path on D through q, q" 00662 DensePath dPath; 00663 computeDensePath(q, qpp, dPath); 00664 if (dPath.size() > 0) 00665 { 00666 // compute path length 00667 double length = 0.0; 00668 DensePath::const_iterator jt = dPath.begin(); 00669 for (DensePath::const_iterator it = jt + 1 ; it != dPath.end() ; ++it) 00670 { 00671 length += si_->distance(*jt, *it); 00672 jt = it; 00673 } 00674 00675 if (length < d_min) 00676 { 00677 d_min = length; 00678 bestDPath.swap(dPath); 00679 best_qpp = qpp; 00680 } 00681 } 00682 } 00683 } 00684 00685 //If the spanner property is violated for these paths 00686 if (s_max > stretchFactor_* d_min) 00687 { 00688 //Need to augment this path with the appropriate neighbor information 00689 DenseVertex na = getInterfaceNeighbor(q, vp); 00690 DenseVertex nb = getInterfaceNeighbor(best_qpp, vpp); 00691 00692 bestDPath.push_front( stateProperty_[na] ); 00693 bestDPath.push_back( stateProperty_[nb] ); 00694 00695 // check consistency of representatives 00696 assert(representativesProperty_[na] == vp && representativesProperty_[nb] == vpp); 00697 00698 //Add the dense path to the spanner 00699 addPathToSpanner( bestDPath, vpp, vp ); 00700 00701 //Report success 00702 result = true; 00703 } 00704 } 00705 } 00706 } 00707 return result; 00708 } 00709 00710 double ompl::geometric::SPARS::averageValence() const 00711 { 00712 double degree = 0.0; 00713 foreach (DenseVertex v, boost::vertices(s_)) 00714 degree += (double)boost::out_degree(v, s_); 00715 degree /= (double)boost::num_vertices(s_); 00716 return degree; 00717 } 00718 00719 void ompl::geometric::SPARS::printDebug(std::ostream &out) const 00720 { 00721 out << "SPARS Debug Output: " << std::endl; 00722 out << " Settings: " << std::endl; 00723 out << " Max Failures: " << getMaxFailures() << std::endl; 00724 out << " Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl; 00725 out << " Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl; 00726 out << " Stretch Factor: " << getStretchFactor() << std::endl; 00727 out << " Status: " << std::endl; 00728 out << " Milestone Count: " << milestoneCount() << std::endl; 00729 out << " Guard Count: " << guardCount() << std::endl; 00730 out << " Iterations: " << getIterationCount() << std::endl; 00731 out << " Average Valence: " << averageValence() << std::endl; 00732 out << " Consecutive Failures: " << consecutiveFailures_ << std::endl; 00733 } 00734 00735 void ompl::geometric::SPARS::getSparseNeighbors(base::State *inState, std::vector<SparseVertex> &graphNeighborhood) 00736 { 00737 sparseStateProperty_[sparseQueryVertex_] = inState; 00738 00739 graphNeighborhood.clear(); 00740 snn_->nearestR(sparseQueryVertex_, sparseDelta_, graphNeighborhood); 00741 00742 sparseStateProperty_[sparseQueryVertex_] = NULL; 00743 } 00744 00745 void ompl::geometric::SPARS::filterVisibleNeighbors(base::State *inState, const std::vector<SparseVertex> &graphNeighborhood, 00746 std::vector<SparseVertex> &visibleNeighborhood) const 00747 { 00748 visibleNeighborhood.clear(); 00749 //Now that we got the neighbors from the NN, we must remove any we can't see 00750 for (std::size_t i = 0; i < graphNeighborhood.size(); ++i) 00751 if (si_->checkMotion(inState, sparseStateProperty_[graphNeighborhood[i]])) 00752 visibleNeighborhood.push_back(graphNeighborhood[i]); 00753 } 00754 00755 ompl::geometric::SPARS::DenseVertex ompl::geometric::SPARS::getInterfaceNeighbor(DenseVertex q, SparseVertex rep) 00756 { 00757 foreach (DenseVertex vp, boost::adjacent_vertices( q, g_ )) 00758 if (representativesProperty_[vp] == rep ) 00759 if (distanceFunction( q, vp ) <= denseDelta_) 00760 return vp; 00761 throw Exception(name_, "Vertex has no interface neighbor with given representative"); 00762 } 00763 00764 bool ompl::geometric::SPARS::addPathToSpanner( const DensePath &dense_path, SparseVertex vp, SparseVertex vpp ) 00765 { 00766 // First, check to see that the path has length 00767 if (dense_path.size() <= 1) 00768 { 00769 // The path is 0 length, so simply link the representatives 00770 connectSparsePoints( vp, vpp ); 00771 resetFailures(); 00772 } 00773 else 00774 { 00775 //We will need to construct a PathGeometric to do this. 00776 geomPath_.getStates().resize( dense_path.size() ); 00777 std::copy( dense_path.begin(), dense_path.end(), geomPath_.getStates().begin() ); 00778 00779 //Attempt to simplify the path 00780 psimp_->reduceVertices( geomPath_, geomPath_.getStateCount() * 2); 00781 00782 // we are sure there are at least 2 points left on geomPath_ 00783 00784 std::vector< SparseVertex > added_nodes; 00785 added_nodes.reserve(geomPath_.getStateCount()); 00786 for (std::size_t i = 0; i < geomPath_.getStateCount(); ++i ) 00787 { 00788 //Add each guard 00789 SparseVertex ng = addGuard( si_->cloneState(geomPath_.getState(i)), QUALITY ); 00790 added_nodes.push_back( ng ); 00791 } 00792 //Link them up 00793 for (std::size_t i = 1; i < added_nodes.size() ; ++i ) 00794 { 00795 connectSparsePoints(added_nodes[i - 1], added_nodes[i]); 00796 } 00797 //Don't forget to link them to their representatives 00798 connectSparsePoints( added_nodes[0], vp ); 00799 connectSparsePoints( added_nodes[added_nodes.size()-1], vpp ); 00800 } 00801 geomPath_.getStates().clear(); 00802 return true; 00803 } 00804 00805 void ompl::geometric::SPARS::updateRepresentatives(SparseVertex v) 00806 { 00807 //Get all of the dense samples which may be affected by adding this node 00808 std::vector< DenseVertex > dense_points; 00809 00810 stateProperty_[ queryVertex_ ] = sparseStateProperty_[ v ]; 00811 00812 nn_->nearestR( queryVertex_, sparseDelta_ + denseDelta_, dense_points ); 00813 00814 stateProperty_[ queryVertex_ ] = NULL; 00815 00816 //For each of those points 00817 for (std::size_t i = 0 ; i < dense_points.size() ; ++i) 00818 { 00819 //Remove that point from the old representative's list(s) 00820 removeFromRepresentatives( dense_points[i], representativesProperty_[dense_points[i]] ); 00821 //Update that point's representative 00822 calculateRepresentative( dense_points[i] ); 00823 } 00824 00825 std::set<SparseVertex> interfaceRepresentatives; 00826 //For each of the points 00827 for (std::size_t i = 0 ; i < dense_points.size(); ++i) 00828 { 00829 //Get it's representative 00830 SparseVertex rep = representativesProperty_[dense_points[i]]; 00831 //Extract the representatives of any interface-sharing neighbors 00832 getInterfaceNeighborRepresentatives( dense_points[i], interfaceRepresentatives ); 00833 //For sanity's sake, make sure we clear ourselves out of what this new rep might think of us 00834 removeFromRepresentatives( dense_points[i], rep ); 00835 //Add this vertex to it's representative's list for the other representatives 00836 addToRepresentatives( dense_points[i], rep, interfaceRepresentatives ); 00837 } 00838 } 00839 00840 void ompl::geometric::SPARS::calculateRepresentative(DenseVertex q) 00841 { 00842 //Get the nearest neighbors within sparseDelta_ 00843 std::vector<SparseVertex> graphNeighborhood; 00844 getSparseNeighbors(stateProperty_[q], graphNeighborhood); 00845 00846 //For each neighbor 00847 for (std::size_t i = 0; i < graphNeighborhood.size(); ++i) 00848 if (si_->checkMotion(stateProperty_[q], sparseStateProperty_[graphNeighborhood[i]])) 00849 { 00850 //update the representative 00851 representativesProperty_[q] = graphNeighborhood[i]; 00852 //abort 00853 break; 00854 } 00855 } 00856 00857 void ompl::geometric::SPARS::addToRepresentatives(DenseVertex q, SparseVertex rep, const std::set<SparseVertex> &oreps) 00858 { 00859 //If this node supports no interfaces 00860 if (oreps.size() == 0) 00861 { 00862 //Add it to the pool of non-interface nodes 00863 bool new_insert = nonInterfaceListsProperty_[rep].insert(q).second; 00864 00865 // we expect this was not previously tracked 00866 if (!new_insert) 00867 assert(false); 00868 } 00869 else 00870 { 00871 //otherwise, for every neighbor representative 00872 foreach( SparseVertex v, oreps ) 00873 { 00874 assert(rep == representativesProperty_[q]); 00875 bool new_insert = interfaceListsProperty_[rep].interfaceHash[v].insert(q).second; 00876 if (!new_insert) 00877 assert(false); 00878 } 00879 } 00880 } 00881 00882 void ompl::geometric::SPARS::removeFromRepresentatives(DenseVertex q, SparseVertex rep) 00883 { 00884 // Remove the node from the non-interface points (if there) 00885 nonInterfaceListsProperty_[rep].erase(q); 00886 00887 // From each of the interfaces 00888 foreach (SparseVertex vpp, interfaceListsProperty_[rep].interfaceHash | boost::adaptors::map_keys) 00889 { 00890 // Remove this node from that list 00891 interfaceListsProperty_[rep].interfaceHash[vpp].erase( q ); 00892 } 00893 } 00894 00895 void ompl::geometric::SPARS::computeVPP(SparseVertex v, SparseVertex vp, std::vector<SparseVertex> &VPPs) 00896 { 00897 foreach( SparseVertex cvpp, boost::adjacent_vertices( v, s_ ) ) 00898 if( cvpp != vp ) 00899 if( !boost::edge( cvpp, vp, s_ ).second ) 00900 VPPs.push_back( cvpp ); 00901 } 00902 00903 void ompl::geometric::SPARS::computeX(SparseVertex v, SparseVertex vp, SparseVertex vpp, std::vector<SparseVertex> &Xs) 00904 { 00905 Xs.clear(); 00906 foreach( SparseVertex cx, boost::adjacent_vertices( vpp, s_ ) ) 00907 if( boost::edge( cx, v, s_ ).second && !boost::edge( cx, vp, s_ ).second ) 00908 if (interfaceListsProperty_[vpp].interfaceHash[cx].size() > 0) 00909 Xs.push_back( cx ); 00910 Xs.push_back( vpp ); 00911 } 00912 00913 void ompl::geometric::SPARS::getInterfaceNeighborRepresentatives(DenseVertex q, std::set<SparseVertex> &interfaceRepresentatives) 00914 { 00915 interfaceRepresentatives.clear(); 00916 00917 // Get our representative 00918 SparseVertex rep = representativesProperty_[q]; 00919 // For each neighbor we are connected to 00920 foreach( DenseVertex n, boost::adjacent_vertices( q, g_ ) ) 00921 { 00922 // Get his representative 00923 SparseVertex orep = representativesProperty_[n]; 00924 // If that representative is not our own 00925 if (orep != rep) 00926 // If he is within denseDelta_ 00927 if (distanceFunction( q, n ) < denseDelta_) 00928 // Include his rep in the set 00929 interfaceRepresentatives.insert(orep); 00930 } 00931 } 00932 00933 void ompl::geometric::SPARS::getInterfaceNeighborhood(DenseVertex q, std::vector<DenseVertex> &interfaceNeighborhood) 00934 { 00935 interfaceNeighborhood.clear(); 00936 00937 // Get our representative 00938 SparseVertex rep = representativesProperty_[q]; 00939 00940 // For each neighbor we are connected to 00941 foreach( DenseVertex n, boost::adjacent_vertices( q, g_ ) ) 00942 // If neighbor representative is not our own 00943 if( representativesProperty_[n] != rep ) 00944 // If he is within denseDelta_ 00945 if( distanceFunction( q, n ) < denseDelta_ ) 00946 // Append him to the list 00947 interfaceNeighborhood.push_back( n ); 00948 } 00949 00950 ompl::base::PathPtr ompl::geometric::SPARS::constructSolution(const SparseVertex start, const SparseVertex goal) const 00951 { 00952 boost::mutex::scoped_lock _(graphMutex_); 00953 00954 boost::vector_property_map<SparseVertex> prev(boost::num_vertices(s_)); 00955 00956 try 00957 { 00958 // Consider using a persistent distance_map if it's slow 00959 boost::astar_search(s_, start, 00960 boost::bind(&SPARS::costHeuristic, this, _1, goal), 00961 boost::predecessor_map(prev). 00962 distance_compare(boost::bind(&base::OptimizationObjective:: 00963 isCostBetterThan, opt_.get(), _1, _2)). 00964 distance_combine(boost::bind(&base::OptimizationObjective:: 00965 combineCosts, opt_.get(), _1, _2)). 00966 distance_inf(opt_->infiniteCost()). 00967 distance_zero(opt_->identityCost()). 00968 visitor(AStarGoalVisitor<SparseVertex>(goal))); 00969 } 00970 catch (AStarFoundGoal&) 00971 { 00972 } 00973 00974 if (prev[goal] == goal) 00975 throw Exception(name_, "Could not find solution path"); 00976 else 00977 { 00978 PathGeometric *p = new PathGeometric(si_); 00979 00980 for (SparseVertex pos = goal; prev[pos] != pos; pos = prev[pos]) 00981 p->append(sparseStateProperty_[pos]); 00982 p->append(sparseStateProperty_[start]); 00983 p->reverse(); 00984 00985 return base::PathPtr(p); 00986 } 00987 } 00988 00989 void ompl::geometric::SPARS::computeDensePath(const DenseVertex start, const DenseVertex goal, DensePath &path) const 00990 { 00991 path.clear(); 00992 00993 boost::vector_property_map<DenseVertex> prev(boost::num_vertices(g_)); 00994 00995 try 00996 { 00997 boost::astar_search(g_, start, 00998 boost::bind(&SPARS::distanceFunction, this, _1, goal), 00999 boost::predecessor_map(prev). 01000 visitor(AStarGoalVisitor<DenseVertex>(goal))); 01001 } 01002 catch (AStarFoundGoal&) 01003 { 01004 } 01005 01006 if (prev[goal] == goal) 01007 OMPL_WARN("%s: No dense path was found?", getName().c_str()); 01008 else 01009 { 01010 for (DenseVertex pos = goal; prev[pos] != pos; pos = prev[pos]) 01011 path.push_front( stateProperty_[pos] ); 01012 path.push_front( stateProperty_[start] ); 01013 } 01014 } 01015 01016 void ompl::geometric::SPARS::getPlannerData(base::PlannerData &data) const 01017 { 01018 Planner::getPlannerData(data); 01019 01020 // Explicitly add start and goal states: 01021 for (std::size_t i = 0; i < startM_.size(); ++i) 01022 data.addStartVertex(base::PlannerDataVertex(sparseStateProperty_[startM_[i]], (int)START)); 01023 01024 for (std::size_t i = 0; i < goalM_.size(); ++i) 01025 data.addGoalVertex(base::PlannerDataVertex(sparseStateProperty_[goalM_[i]], (int)GOAL)); 01026 01027 // Adding edges and all other vertices simultaneously 01028 foreach (const SparseEdge e, boost::edges(s_)) 01029 { 01030 const SparseVertex v1 = boost::source(e, s_); 01031 const SparseVertex v2 = boost::target(e, s_); 01032 data.addEdge(base::PlannerDataVertex(sparseStateProperty_[v1], (int)sparseColorProperty_[v1]), 01033 base::PlannerDataVertex(sparseStateProperty_[v2], (int)sparseColorProperty_[v2])); 01034 01035 // Add the reverse edge, since we're constructing an undirected roadmap 01036 data.addEdge(base::PlannerDataVertex(sparseStateProperty_[v2], (int)sparseColorProperty_[v1]), 01037 base::PlannerDataVertex(sparseStateProperty_[v1], (int)sparseColorProperty_[v2])); 01038 } 01039 01040 // Make sure to add edge-less nodes as well 01041 foreach (const SparseVertex n, boost::vertices(s_)) 01042 if (boost::out_degree( n, s_ ) == 0) 01043 data.addVertex(base::PlannerDataVertex(sparseStateProperty_[n], (int)sparseColorProperty_[n])); 01044 } 01045 01046 ompl::base::Cost ompl::geometric::SPARS::costHeuristic(SparseVertex u, SparseVertex v) const 01047 { 01048 return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]); 01049 }