ompl/geometric/planners/prm/src/SPARStwo.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/SPARStwo.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 <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 #include "GoalVisitor.hpp" 00049 00050 #define foreach BOOST_FOREACH 00051 #define foreach_reverse BOOST_REVERSE_FOREACH 00052 00053 ompl::geometric::SPARStwo::SPARStwo(const base::SpaceInformationPtr &si) : 00054 base::Planner(si, "SPARStwo"), 00055 stretchFactor_(3.), 00056 sparseDeltaFraction_(.25), 00057 denseDeltaFraction_(.001), 00058 maxFailures_(5000), 00059 nearSamplePoints_((2*si_->getStateDimension())), 00060 stateProperty_(boost::get(vertex_state_t(), g_)), 00061 weightProperty_(boost::get(boost::edge_weight, g_)), 00062 colorProperty_(boost::get(vertex_color_t(), g_)), 00063 interfaceDataProperty_(boost::get(vertex_interface_data_t(), g_)), 00064 disjointSets_(boost::get(boost::vertex_rank, g_), 00065 boost::get(boost::vertex_predecessor, g_)), 00066 addedSolution_(false), 00067 consecutiveFailures_(0), 00068 sparseDelta_(0.), 00069 denseDelta_(0.), 00070 iterations_(0), 00071 bestCost_(std::numeric_limits<double>::quiet_NaN()) 00072 { 00073 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION; 00074 specs_.approximateSolutions = false; 00075 specs_.optimizingPaths = true; 00076 specs_.multithreaded = true; 00077 00078 psimp_.reset(new PathSimplifier(si_)); 00079 00080 Planner::declareParam<double>("stretch_factor", this, &SPARStwo::setStretchFactor, &SPARStwo::getStretchFactor, "1.1:0.1:3.0"); 00081 Planner::declareParam<double>("sparse_delta_fraction", this, &SPARStwo::setSparseDeltaFraction, &SPARStwo::getSparseDeltaFraction, "0.0:0.01:1.0"); 00082 Planner::declareParam<double>("dense_delta_fraction", this, &SPARStwo::setDenseDeltaFraction, &SPARStwo::getDenseDeltaFraction, "0.0:0.0001:0.1"); 00083 Planner::declareParam<unsigned int>("max_failures", this, &SPARStwo::setMaxFailures, &SPARStwo::getMaxFailures, "100:10:3000"); 00084 00085 addPlannerProgressProperty("iterations INTEGER", 00086 boost::bind(&SPARStwo::getIterationCount, this)); 00087 addPlannerProgressProperty("best cost REAL", 00088 boost::bind(&SPARStwo::getBestCost, this)); 00089 } 00090 00091 ompl::geometric::SPARStwo::~SPARStwo() 00092 { 00093 freeMemory(); 00094 } 00095 00096 void ompl::geometric::SPARStwo::setup() 00097 { 00098 Planner::setup(); 00099 if (!nn_) 00100 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace())); 00101 nn_->setDistanceFunction(boost::bind(&SPARStwo::distanceFunction, this, _1, _2)); 00102 double maxExt = si_->getMaximumExtent(); 00103 sparseDelta_ = sparseDeltaFraction_ * maxExt; 00104 denseDelta_ = denseDeltaFraction_ * maxExt; 00105 00106 // Setup optimization objective 00107 // 00108 // If no optimization objective was specified, then default to 00109 // optimizing path length as computed by the distance() function 00110 // in the state space. 00111 if (pdef_) 00112 { 00113 if (pdef_->hasOptimizationObjective()) 00114 { 00115 opt_ = pdef_->getOptimizationObjective(); 00116 if (!dynamic_cast<base::PathLengthOptimizationObjective*>(opt_.get())) 00117 OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence for other optimizaton objectives is not guaranteed.", getName().c_str()); 00118 } 00119 else 00120 opt_.reset(new base::PathLengthOptimizationObjective(si_)); 00121 } 00122 else 00123 { 00124 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str()); 00125 setup_ = false; 00126 } 00127 } 00128 00129 void ompl::geometric::SPARStwo::setProblemDefinition(const base::ProblemDefinitionPtr &pdef) 00130 { 00131 Planner::setProblemDefinition(pdef); 00132 clearQuery(); 00133 } 00134 00135 void ompl::geometric::SPARStwo::clearQuery() 00136 { 00137 startM_.clear(); 00138 goalM_.clear(); 00139 pis_.restart(); 00140 } 00141 00142 void ompl::geometric::SPARStwo::clear() 00143 { 00144 Planner::clear(); 00145 clearQuery(); 00146 resetFailures(); 00147 iterations_ = 0; 00148 bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN()); 00149 freeMemory(); 00150 if (nn_) 00151 nn_->clear(); 00152 } 00153 00154 void ompl::geometric::SPARStwo::freeMemory() 00155 { 00156 Planner::clear(); 00157 sampler_.reset(); 00158 simpleSampler_.reset(); 00159 00160 foreach (Vertex v, boost::vertices(g_)) 00161 { 00162 foreach (InterfaceData &d, interfaceDataProperty_[v].interfaceHash | boost::adaptors::map_values) 00163 d.clear(si_); 00164 if( stateProperty_[v] != NULL ) 00165 si_->freeState(stateProperty_[v]); 00166 stateProperty_[v] = NULL; 00167 } 00168 g_.clear(); 00169 00170 if (nn_) 00171 nn_->clear(); 00172 } 00173 00174 bool ompl::geometric::SPARStwo::haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution) 00175 { 00176 base::Goal *g = pdef_->getGoal().get(); 00177 base::Cost sol_cost(opt_->infiniteCost()); 00178 foreach (Vertex start, starts) 00179 foreach (Vertex goal, goals) 00180 { 00181 // we lock because the connected components algorithm is incremental and may change disjointSets_ 00182 graphMutex_.lock(); 00183 bool same_component = sameComponent(start, goal); 00184 graphMutex_.unlock(); 00185 00186 if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start])) 00187 { 00188 base::PathPtr p = constructSolution(start, goal); 00189 if (p) 00190 { 00191 base::Cost pathCost = p->cost(opt_); 00192 if (opt_->isCostBetterThan(pathCost, bestCost_)) 00193 bestCost_ = pathCost; 00194 // Check if optimization objective is satisfied 00195 if (opt_->isSatisfied(pathCost)) 00196 { 00197 solution = p; 00198 return true; 00199 } 00200 else if (opt_->isCostBetterThan(pathCost, sol_cost)) 00201 { 00202 solution = p; 00203 sol_cost = pathCost; 00204 } 00205 } 00206 } 00207 } 00208 return false; 00209 } 00210 00211 bool ompl::geometric::SPARStwo::sameComponent(Vertex m1, Vertex m2) 00212 { 00213 return boost::same_component(m1, m2, disjointSets_); 00214 } 00215 00216 bool ompl::geometric::SPARStwo::reachedFailureLimit() const 00217 { 00218 return consecutiveFailures_ >= maxFailures_; 00219 } 00220 00221 bool ompl::geometric::SPARStwo::reachedTerminationCriterion() const 00222 { 00223 return consecutiveFailures_ >= maxFailures_ || addedSolution_; 00224 } 00225 00226 void ompl::geometric::SPARStwo::constructRoadmap(const base::PlannerTerminationCondition &ptc, bool stopOnMaxFail) 00227 { 00228 if (stopOnMaxFail) 00229 { 00230 resetFailures(); 00231 base::PlannerTerminationCondition ptcOrFail = 00232 base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARStwo::reachedFailureLimit, this))); 00233 constructRoadmap(ptcOrFail); 00234 } 00235 else 00236 constructRoadmap(ptc); 00237 } 00238 00239 void ompl::geometric::SPARStwo::constructRoadmap(const base::PlannerTerminationCondition &ptc) 00240 { 00241 checkQueryStateInitialization(); 00242 00243 if (!isSetup()) 00244 setup(); 00245 if (!sampler_) 00246 sampler_ = si_->allocValidStateSampler(); 00247 if (!simpleSampler_) 00248 simpleSampler_ = si_->allocStateSampler(); 00249 00250 base::State *qNew = si_->allocState(); 00251 base::State *workState = si_->allocState(); 00252 00253 /* The whole neighborhood set which has been most recently computed */ 00254 std::vector<Vertex> graphNeighborhood; 00255 /* The visible neighborhood set which has been most recently computed */ 00256 std::vector<Vertex> visibleNeighborhood; 00257 00258 bestCost_ = opt_->infiniteCost(); 00259 while (ptc == false) 00260 { 00261 ++iterations_; 00262 ++consecutiveFailures_; 00263 00264 //Generate a single sample, and attempt to connect it to nearest neighbors. 00265 if (!sampler_->sample(qNew)) 00266 continue; 00267 00268 findGraphNeighbors(qNew, graphNeighborhood, visibleNeighborhood); 00269 00270 if (!checkAddCoverage(qNew, visibleNeighborhood)) 00271 if (!checkAddConnectivity(qNew, visibleNeighborhood)) 00272 if (!checkAddInterface(qNew, graphNeighborhood, visibleNeighborhood)) 00273 { 00274 if (visibleNeighborhood.size() > 0) 00275 { 00276 std::map<Vertex, base::State*> closeRepresentatives; 00277 findCloseRepresentatives(workState, qNew, visibleNeighborhood[0], closeRepresentatives, ptc); 00278 for (std::map<Vertex, base::State*>::iterator it = closeRepresentatives.begin(); it != closeRepresentatives.end(); ++it) 00279 { 00280 updatePairPoints(visibleNeighborhood[0], qNew, it->first, it->second); 00281 updatePairPoints(it->first, it->second, visibleNeighborhood[0], qNew); 00282 } 00283 checkAddPath(visibleNeighborhood[0]); 00284 for (std::map<Vertex, base::State*>::iterator it = closeRepresentatives.begin(); it != closeRepresentatives.end(); ++it) 00285 { 00286 checkAddPath(it->first); 00287 si_->freeState(it->second); 00288 } 00289 } 00290 } 00291 } 00292 si_->freeState(workState); 00293 si_->freeState(qNew); 00294 } 00295 00296 void ompl::geometric::SPARStwo::checkQueryStateInitialization() 00297 { 00298 boost::mutex::scoped_lock _(graphMutex_); 00299 if (boost::num_vertices(g_) < 1) 00300 { 00301 queryVertex_ = boost::add_vertex( g_ ); 00302 stateProperty_[queryVertex_] = NULL; 00303 } 00304 } 00305 00306 ompl::base::PlannerStatus ompl::geometric::SPARStwo::solve(const base::PlannerTerminationCondition &ptc) 00307 { 00308 checkValidity(); 00309 checkQueryStateInitialization(); 00310 00311 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get()); 00312 00313 if (!goal) 00314 { 00315 OMPL_ERROR("%s: Unknown type of goal", getName().c_str()); 00316 return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE; 00317 } 00318 00319 // Add the valid start states as milestones 00320 while (const base::State *st = pis_.nextStart()) 00321 startM_.push_back(addGuard(si_->cloneState(st), START)); 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 (!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 goalM_.push_back(addGuard(si_->cloneState(st), GOAL)); 00337 if (goalM_.empty()) 00338 { 00339 OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str()); 00340 return base::PlannerStatus::INVALID_GOAL; 00341 } 00342 00343 unsigned int nrStartStates = boost::num_vertices(g_) - 1; // don't count query vertex 00344 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nrStartStates); 00345 00346 // Reset addedSolution_ member 00347 addedSolution_ = false; 00348 resetFailures(); 00349 base::PathPtr sol; 00350 base::PlannerTerminationCondition ptcOrFail = 00351 base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARStwo::reachedFailureLimit, this))); 00352 boost::thread slnThread(boost::bind(&SPARStwo::checkForSolution, this, ptcOrFail, boost::ref(sol))); 00353 00354 //Construct planner termination condition which also takes M into account 00355 base::PlannerTerminationCondition ptcOrStop = 00356 base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARStwo::reachedTerminationCriterion, this))); 00357 constructRoadmap(ptcOrStop); 00358 00359 // Ensure slnThread is ceased before exiting solve 00360 slnThread.join(); 00361 00362 OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates); 00363 00364 if (sol) 00365 pdef_->addSolutionPath(sol, false, -1.0, getName()); 00366 00367 // Return true if any solution was found. 00368 return sol ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; 00369 } 00370 00371 void ompl::geometric::SPARStwo::checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution) 00372 { 00373 base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get()); 00374 while (!ptc && !addedSolution_) 00375 { 00376 // Check for any new goal states 00377 if (goal->maxSampleCount() > goalM_.size()) 00378 { 00379 const base::State *st = pis_.nextGoal(); 00380 if (st) 00381 goalM_.push_back(addGuard(si_->cloneState(st), GOAL)); 00382 } 00383 00384 // Check for a solution 00385 addedSolution_ = haveSolution(startM_, goalM_, solution); 00386 // Sleep for 1ms 00387 if (!addedSolution_) 00388 boost::this_thread::sleep(boost::posix_time::milliseconds(1)); 00389 } 00390 } 00391 00392 bool ompl::geometric::SPARStwo::checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood) 00393 { 00394 if (visibleNeighborhood.size() > 0) 00395 return false; 00396 //No free paths means we add for coverage 00397 addGuard(si_->cloneState(qNew), COVERAGE); 00398 return true; 00399 } 00400 00401 bool ompl::geometric::SPARStwo::checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood) 00402 { 00403 std::vector<Vertex> links; 00404 if (visibleNeighborhood.size() > 1) 00405 { 00406 //For each neighbor 00407 for (std::size_t i = 0; i < visibleNeighborhood.size(); ++i) 00408 //For each other neighbor 00409 for (std::size_t j = i + 1; j < visibleNeighborhood.size(); ++j) 00410 //If they are in different components 00411 if (!sameComponent(visibleNeighborhood[i], visibleNeighborhood[j])) 00412 { 00413 links.push_back(visibleNeighborhood[i]); 00414 links.push_back(visibleNeighborhood[j]); 00415 } 00416 00417 if (links.size() > 0) 00418 { 00419 //Add the node 00420 Vertex g = addGuard(si_->cloneState(qNew), CONNECTIVITY); 00421 00422 for (std::size_t i = 0; i < links.size() ; ++i) 00423 //If there's no edge 00424 if (!boost::edge(g, links[i], g_).second) 00425 //And the components haven't been united by previous links 00426 if (!sameComponent(links[i], g)) 00427 connectGuards(g, links[i]); 00428 return true; 00429 } 00430 } 00431 return false; 00432 } 00433 00434 bool ompl::geometric::SPARStwo::checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood, std::vector<Vertex> &visibleNeighborhood) 00435 { 00436 //If we have more than 1 or 0 neighbors 00437 if (visibleNeighborhood.size() > 1) 00438 if (graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1]) 00439 //If our two closest neighbors don't share an edge 00440 if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], g_).second) 00441 { 00442 //If they can be directly connected 00443 if (si_->checkMotion(stateProperty_[visibleNeighborhood[0]], stateProperty_[visibleNeighborhood[1]])) 00444 { 00445 //Connect them 00446 connectGuards(visibleNeighborhood[0], visibleNeighborhood[1]); 00447 //And report that we added to the roadmap 00448 resetFailures(); 00449 //Report success 00450 return true; 00451 } 00452 else 00453 { 00454 //Add the new node to the graph, to bridge the interface 00455 Vertex v = addGuard(si_->cloneState(qNew), INTERFACE); 00456 connectGuards(v, visibleNeighborhood[0]); 00457 connectGuards(v, visibleNeighborhood[1]); 00458 //Report success 00459 return true; 00460 } 00461 } 00462 return false; 00463 } 00464 00465 bool ompl::geometric::SPARStwo::checkAddPath( Vertex v ) 00466 { 00467 bool ret = false; 00468 00469 std::vector< Vertex > rs; 00470 foreach( Vertex r, boost::adjacent_vertices( v, g_ ) ) 00471 rs.push_back(r); 00472 00473 /* Candidate x vertices as described in the method, filled by function computeX(). */ 00474 std::vector<Vertex> Xs; 00475 00476 /* Candidate v" vertices as described in the method, filled by function computeVPP(). */ 00477 std::vector<Vertex> VPPs; 00478 00479 for (std::size_t i = 0; i < rs.size() && !ret; ++i) 00480 { 00481 Vertex r = rs[i]; 00482 computeVPP(v, r, VPPs); 00483 foreach (Vertex rp, VPPs) 00484 { 00485 //First, compute the longest path through the graph 00486 computeX(v, r, rp, Xs); 00487 double rm_dist = 0.0; 00488 foreach( Vertex rpp, Xs) 00489 { 00490 double tmp_dist = (si_->distance( stateProperty_[r], stateProperty_[v] ) 00491 + si_->distance( stateProperty_[v], stateProperty_[rpp] ) )/2.0; 00492 if( tmp_dist > rm_dist ) 00493 rm_dist = tmp_dist; 00494 } 00495 00496 InterfaceData& d = getData( v, r, rp ); 00497 00498 //Then, if the spanner property is violated 00499 if (rm_dist > stretchFactor_ * d.d_) 00500 { 00501 ret = true; //Report that we added for the path 00502 if (si_->checkMotion(stateProperty_[r], stateProperty_[rp])) 00503 connectGuards(r, rp); 00504 else 00505 { 00506 PathGeometric *p = new PathGeometric( si_ ); 00507 if (r < rp) 00508 { 00509 p->append(d.sigmaA_); 00510 p->append(d.pointA_); 00511 p->append(stateProperty_[v]); 00512 p->append(d.pointB_); 00513 p->append(d.sigmaB_); 00514 } 00515 else 00516 { 00517 p->append(d.sigmaB_); 00518 p->append(d.pointB_); 00519 p->append(stateProperty_[v]); 00520 p->append(d.pointA_); 00521 p->append(d.sigmaA_); 00522 } 00523 00524 psimp_->reduceVertices(*p, 10); 00525 psimp_->shortcutPath(*p, 50); 00526 00527 if (p->checkAndRepair(100).second) 00528 { 00529 Vertex prior = r; 00530 Vertex vnew; 00531 std::vector<base::State*>& states = p->getStates(); 00532 00533 foreach (base::State *st, states) 00534 { 00535 // no need to clone st, since we will destroy p; we just copy the pointer 00536 vnew = addGuard(st , QUALITY); 00537 00538 connectGuards(prior, vnew); 00539 prior = vnew; 00540 } 00541 // clear the states, so memory is not freed twice 00542 states.clear(); 00543 connectGuards(prior, rp); 00544 } 00545 00546 delete p; 00547 } 00548 } 00549 } 00550 } 00551 00552 return ret; 00553 } 00554 00555 void ompl::geometric::SPARStwo::resetFailures() 00556 { 00557 consecutiveFailures_ = 0; 00558 } 00559 00560 void ompl::geometric::SPARStwo::findGraphNeighbors(base::State *st, std::vector<Vertex> &graphNeighborhood, std::vector<Vertex> &visibleNeighborhood) 00561 { 00562 visibleNeighborhood.clear(); 00563 stateProperty_[ queryVertex_ ] = st; 00564 nn_->nearestR( queryVertex_, sparseDelta_, graphNeighborhood); 00565 stateProperty_[ queryVertex_ ] = NULL; 00566 00567 //Now that we got the neighbors from the NN, we must remove any we can't see 00568 for (std::size_t i = 0; i < graphNeighborhood.size() ; ++i ) 00569 if (si_->checkMotion(st, stateProperty_[graphNeighborhood[i]])) 00570 visibleNeighborhood.push_back(graphNeighborhood[i]); 00571 } 00572 00573 void ompl::geometric::SPARStwo::approachGraph( Vertex v ) 00574 { 00575 std::vector< Vertex > hold; 00576 nn_->nearestR( v, sparseDelta_, hold ); 00577 00578 std::vector< Vertex > neigh; 00579 for (std::size_t i = 0; i < hold.size(); ++i) 00580 if (si_->checkMotion( stateProperty_[v], stateProperty_[hold[i]])) 00581 neigh.push_back( hold[i] ); 00582 00583 foreach (Vertex vp, neigh) 00584 connectGuards(v, vp); 00585 } 00586 00587 ompl::geometric::SPARStwo::Vertex ompl::geometric::SPARStwo::findGraphRepresentative(base::State *st) 00588 { 00589 std::vector<Vertex> nbh; 00590 stateProperty_[ queryVertex_ ] = st; 00591 nn_->nearestR( queryVertex_, sparseDelta_, nbh); 00592 stateProperty_[queryVertex_] = NULL; 00593 00594 Vertex result = boost::graph_traits<Graph>::null_vertex(); 00595 00596 for (std::size_t i = 0 ; i< nbh.size() ; ++i) 00597 if (si_->checkMotion(st, stateProperty_[nbh[i]])) 00598 { 00599 result = nbh[i]; 00600 break; 00601 } 00602 return result; 00603 } 00604 00605 void ompl::geometric::SPARStwo::findCloseRepresentatives(base::State *workArea, const base::State *qNew, const Vertex qRep, 00606 std::map<Vertex, base::State*> &closeRepresentatives, 00607 const base::PlannerTerminationCondition &ptc) 00608 { 00609 for (std::map<Vertex, base::State*>::iterator it = closeRepresentatives.begin(); it != closeRepresentatives.end(); ++it) 00610 si_->freeState(it->second); 00611 closeRepresentatives.clear(); 00612 00613 // Then, begin searching the space around him 00614 for (unsigned int i = 0 ; i < nearSamplePoints_ ; ++i) 00615 { 00616 do 00617 { 00618 sampler_->sampleNear(workArea, qNew, denseDelta_); 00619 } while ((!si_->isValid(workArea) || si_->distance(qNew, workArea) > denseDelta_ || !si_->checkMotion(qNew, workArea)) && ptc == false); 00620 00621 // if we were not successful at sampling a desirable state, we are out of time 00622 if (ptc == true) 00623 break; 00624 00625 // Compute who his graph neighbors are 00626 Vertex representative = findGraphRepresentative(workArea); 00627 00628 // Assuming this sample is actually seen by somebody (which he should be in all likelihood) 00629 if (representative != boost::graph_traits<Graph>::null_vertex()) 00630 { 00631 //If his representative is different than qNew 00632 if (qRep != representative) 00633 //And we haven't already tracked this representative 00634 if (closeRepresentatives.find(representative) == closeRepresentatives.end()) 00635 //Track the representative 00636 closeRepresentatives[representative] = si_->cloneState(workArea); 00637 } 00638 else 00639 { 00640 //This guy can't be seen by anybody, so we should take this opportunity to add him 00641 addGuard(si_->cloneState(workArea), COVERAGE); 00642 00643 //We should also stop our efforts to add a dense path 00644 for (std::map<Vertex, base::State*>::iterator it = closeRepresentatives.begin(); it != closeRepresentatives.end(); ++it) 00645 si_->freeState(it->second); 00646 closeRepresentatives.clear(); 00647 break; 00648 } 00649 } 00650 } 00651 00652 void ompl::geometric::SPARStwo::updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s) 00653 { 00654 //First of all, we need to compute all candidate r' 00655 std::vector<Vertex> VPPs; 00656 computeVPP(rep, r, VPPs); 00657 00658 //Then, for each pair Pv(r,r') 00659 foreach (Vertex rp, VPPs) 00660 //Try updating the pair info 00661 distanceCheck(rep, q, r, s, rp); 00662 } 00663 00664 void ompl::geometric::SPARStwo::computeVPP(Vertex v, Vertex vp, std::vector<Vertex> &VPPs) 00665 { 00666 VPPs.clear(); 00667 foreach( Vertex cvpp, boost::adjacent_vertices( v, g_ ) ) 00668 if( cvpp != vp ) 00669 if( !boost::edge( cvpp, vp, g_ ).second ) 00670 VPPs.push_back( cvpp ); 00671 } 00672 00673 void ompl::geometric::SPARStwo::computeX(Vertex v, Vertex vp, Vertex vpp, std::vector<Vertex> &Xs) 00674 { 00675 Xs.clear(); 00676 00677 foreach (Vertex cx, boost::adjacent_vertices(vpp, g_)) 00678 if (boost::edge(cx, v, g_).second && !boost::edge(cx, vp, g_).second) 00679 { 00680 InterfaceData& d = getData( v, vpp, cx ); 00681 if ((vpp < cx && d.pointA_) || (cx < vpp && d.pointB_)) 00682 Xs.push_back( cx ); 00683 } 00684 Xs.push_back(vpp); 00685 } 00686 00687 ompl::geometric::SPARStwo::VertexPair ompl::geometric::SPARStwo::index( Vertex vp, Vertex vpp ) 00688 { 00689 if( vp < vpp ) 00690 return VertexPair( vp, vpp ); 00691 else if( vpp < vp ) 00692 return VertexPair( vpp, vp ); 00693 else 00694 throw Exception( name_, "Trying to get an index where the pairs are the same point!"); 00695 } 00696 00697 ompl::geometric::SPARStwo::InterfaceData& ompl::geometric::SPARStwo::getData( Vertex v, Vertex vp, Vertex vpp ) 00698 { 00699 return interfaceDataProperty_[v].interfaceHash[index( vp, vpp )]; 00700 } 00701 00702 void ompl::geometric::SPARStwo::distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp) 00703 { 00704 //Get the info for the current representative-neighbors pair 00705 InterfaceData& d = getData( rep, r, rp ); 00706 00707 if (r < rp) // FIRST points represent r (the guy discovered through sampling) 00708 { 00709 if (d.pointA_ == NULL) // If the point we're considering replacing (P_v(r,.)) isn't there 00710 //Then we know we're doing better, so add it 00711 d.setFirst(q, s, si_); 00712 else //Otherwise, he is there, 00713 { 00714 if (d.pointB_ == NULL) //But if the other guy doesn't exist, we can't compare. 00715 { 00716 //Should probably keep the one that is further away from rep? Not known what to do in this case. 00717 // TODO: is this not part of the algorithm? 00718 } 00719 else //We know both of these points exist, so we can check some distances 00720 if (si_->distance(q, d.pointB_) < si_->distance(d.pointA_, d.pointB_)) 00721 //Distance with the new point is good, so set it. 00722 d.setFirst( q, s, si_ ); 00723 } 00724 } 00725 else // SECOND points represent r (the guy discovered through sampling) 00726 { 00727 if (d.pointB_ == NULL) //If the point we're considering replacing (P_V(.,r)) isn't there... 00728 //Then we must be doing better, so add it 00729 d.setSecond(q, s, si_); 00730 else //Otherwise, he is there 00731 { 00732 if (d.pointA_ == NULL) //But if the other guy doesn't exist, we can't compare. 00733 { 00734 //Should we be doing something cool here? 00735 } 00736 else 00737 if (si_->distance(q, d.pointA_) < si_->distance(d.pointB_, d.pointA_)) 00738 //Distance with the new point is good, so set it 00739 d.setSecond(q, s, si_); 00740 } 00741 } 00742 00743 // Lastly, save what we have discovered 00744 interfaceDataProperty_[rep].interfaceHash[index(r, rp)] = d; 00745 } 00746 00747 void ompl::geometric::SPARStwo::abandonLists(base::State *st) 00748 { 00749 stateProperty_[ queryVertex_ ] = st; 00750 00751 std::vector< Vertex > hold; 00752 nn_->nearestR( queryVertex_, sparseDelta_, hold ); 00753 00754 stateProperty_[queryVertex_] = NULL; 00755 00756 //For each of the vertices 00757 foreach (Vertex v, hold) 00758 { 00759 foreach (VertexPair r, interfaceDataProperty_[v].interfaceHash | boost::adaptors::map_keys) 00760 interfaceDataProperty_[v].interfaceHash[r].clear(si_); 00761 } 00762 } 00763 00764 ompl::geometric::SPARStwo::Vertex ompl::geometric::SPARStwo::addGuard(base::State *state, GuardType type) 00765 { 00766 boost::mutex::scoped_lock _(graphMutex_); 00767 00768 Vertex m = boost::add_vertex(g_); 00769 stateProperty_[m] = state; 00770 colorProperty_[m] = type; 00771 00772 assert(si_->isValid(state)); 00773 abandonLists(state); 00774 00775 disjointSets_.make_set(m); 00776 nn_->add(m); 00777 resetFailures(); 00778 00779 return m; 00780 } 00781 00782 void ompl::geometric::SPARStwo::connectGuards(Vertex v, Vertex vp) 00783 { 00784 assert(v <= milestoneCount()); 00785 assert(vp <= milestoneCount()); 00786 00787 const base::Cost weight(costHeuristic(v, vp)); 00788 const Graph::edge_property_type properties(weight); 00789 boost::mutex::scoped_lock _(graphMutex_); 00790 boost::add_edge(v, vp, properties, g_); 00791 disjointSets_.union_set(v, vp); 00792 } 00793 00794 ompl::base::PathPtr ompl::geometric::SPARStwo::constructSolution(const Vertex start, const Vertex goal) const 00795 { 00796 boost::mutex::scoped_lock _(graphMutex_); 00797 00798 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_)); 00799 00800 try 00801 { 00802 boost::astar_search(g_, start, 00803 boost::bind(&SPARStwo::costHeuristic, this, _1, goal), 00804 boost::predecessor_map(prev). 00805 distance_compare(boost::bind(&base::OptimizationObjective:: 00806 isCostBetterThan, opt_.get(), _1, _2)). 00807 distance_combine(boost::bind(&base::OptimizationObjective:: 00808 combineCosts, opt_.get(), _1, _2)). 00809 distance_inf(opt_->infiniteCost()). 00810 distance_zero(opt_->identityCost()). 00811 visitor(AStarGoalVisitor<Vertex>(goal))); 00812 } 00813 catch (AStarFoundGoal&) 00814 { 00815 } 00816 00817 if (prev[goal] == goal) 00818 throw Exception(name_, "Could not find solution path"); 00819 else 00820 { 00821 PathGeometric *p = new PathGeometric(si_); 00822 for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos]) 00823 p->append(stateProperty_[pos]); 00824 p->append(stateProperty_[start]); 00825 p->reverse(); 00826 00827 return base::PathPtr(p); 00828 } 00829 } 00830 00831 void ompl::geometric::SPARStwo::printDebug(std::ostream &out) const 00832 { 00833 out << "SPARStwo Debug Output: " << std::endl; 00834 out << " Settings: " << std::endl; 00835 out << " Max Failures: " << getMaxFailures() << std::endl; 00836 out << " Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl; 00837 out << " Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl; 00838 out << " Stretch Factor: " << getStretchFactor() << std::endl; 00839 out << " Status: " << std::endl; 00840 out << " Milestone Count: " << milestoneCount() << std::endl; 00841 out << " Iterations: " << getIterationCount() << std::endl; 00842 out << " Consecutive Failures: " << consecutiveFailures_ << std::endl; 00843 } 00844 00845 void ompl::geometric::SPARStwo::getPlannerData(base::PlannerData &data) const 00846 { 00847 Planner::getPlannerData(data); 00848 00849 // Explicitly add start and goal states: 00850 for (size_t i = 0; i < startM_.size(); ++i) 00851 data.addStartVertex(base::PlannerDataVertex(stateProperty_[startM_[i]], (int)START)); 00852 00853 for (size_t i = 0; i < goalM_.size(); ++i) 00854 data.addGoalVertex(base::PlannerDataVertex(stateProperty_[goalM_[i]], (int)GOAL)); 00855 00856 // If there are even edges here 00857 if (boost::num_edges( g_ ) > 0) 00858 { 00859 // Adding edges and all other vertices simultaneously 00860 foreach (const Edge e, boost::edges(g_)) 00861 { 00862 const Vertex v1 = boost::source(e, g_); 00863 const Vertex v2 = boost::target(e, g_); 00864 data.addEdge(base::PlannerDataVertex(stateProperty_[v1], (int)colorProperty_[v1]), 00865 base::PlannerDataVertex(stateProperty_[v2], (int)colorProperty_[v2])); 00866 00867 // Add the reverse edge, since we're constructing an undirected roadmap 00868 data.addEdge(base::PlannerDataVertex(stateProperty_[v2], (int)colorProperty_[v2]), 00869 base::PlannerDataVertex(stateProperty_[v1], (int)colorProperty_[v1])); 00870 } 00871 } 00872 else 00873 OMPL_INFORM("%s: There are no edges in the graph!", getName().c_str()); 00874 00875 // Make sure to add edge-less nodes as well 00876 foreach (const Vertex n, boost::vertices(g_)) 00877 if (boost::out_degree(n, g_) == 0) 00878 data.addVertex(base::PlannerDataVertex(stateProperty_[n], (int)colorProperty_[n])); 00879 } 00880 00881 ompl::base::Cost ompl::geometric::SPARStwo::costHeuristic(Vertex u, Vertex v) const 00882 { 00883 return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]); 00884 } 00885