ompl/control/planners/syclop/src/Syclop.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 the 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: Matt Maly */ 00036 00037 #include "ompl/control/planners/syclop/Syclop.h" 00038 #include "ompl/base/goals/GoalSampleableRegion.h" 00039 #include "ompl/base/ProblemDefinition.h" 00040 #include <limits> 00041 #include <stack> 00042 #include <algorithm> 00043 00044 const double ompl::control::Syclop::Defaults::PROB_ABANDON_LEAD_EARLY = 0.25; 00045 const double ompl::control::Syclop::Defaults::PROB_KEEP_ADDING_TO_AVAIL = 0.50; 00046 const double ompl::control::Syclop::Defaults::PROB_SHORTEST_PATH = 0.95; 00047 00048 void ompl::control::Syclop::setup() 00049 { 00050 base::Planner::setup(); 00051 if (!leadComputeFn) 00052 setLeadComputeFn(boost::bind(&ompl::control::Syclop::defaultComputeLead, this, _1, _2, _3)); 00053 buildGraph(); 00054 addEdgeCostFactor(boost::bind(&ompl::control::Syclop::defaultEdgeCost, this, _1, _2)); 00055 } 00056 00057 void ompl::control::Syclop::clear() 00058 { 00059 base::Planner::clear(); 00060 lead_.clear(); 00061 availDist_.clear(); 00062 clearEdgeCostFactors(); 00063 clearGraphDetails(); 00064 startRegions_.clear(); 00065 goalRegions_.clear(); 00066 } 00067 00068 ompl::base::PlannerStatus ompl::control::Syclop::solve(const base::PlannerTerminationCondition &ptc) 00069 { 00070 checkValidity(); 00071 if (!graphReady_) 00072 { 00073 numMotions_ = 0; 00074 setupRegionEstimates(); 00075 setupEdgeEstimates(); 00076 graphReady_ = true; 00077 } 00078 while (const base::State *s = pis_.nextStart()) 00079 { 00080 const int region = decomp_->locateRegion(s); 00081 startRegions_.insert(region); 00082 Motion *startMotion = addRoot(s); 00083 graph_[boost::vertex(region, graph_)].motions.push_back(startMotion); 00084 ++numMotions_; 00085 updateCoverageEstimate(graph_[boost::vertex(region, graph_)], s); 00086 } 00087 if (startRegions_.empty()) 00088 { 00089 OMPL_ERROR("%s: There are no valid start states", getName().c_str()); 00090 return base::PlannerStatus::INVALID_START; 00091 } 00092 00093 //We need at least one valid goal sample so that we can find the goal region 00094 if (goalRegions_.empty()) 00095 { 00096 if (const base::State *g = pis_.nextGoal(ptc)) 00097 goalRegions_.insert(decomp_->locateRegion(g)); 00098 else 00099 { 00100 OMPL_ERROR("%s: Unable to sample a valid goal state", getName().c_str()); 00101 return base::PlannerStatus::INVALID_GOAL; 00102 } 00103 } 00104 00105 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), numMotions_); 00106 00107 std::vector<Motion*> newMotions; 00108 const Motion *solution = NULL; 00109 base::Goal *goal = pdef_->getGoal().get(); 00110 double goalDist = std::numeric_limits<double>::infinity(); 00111 bool solved = false; 00112 while (!ptc && !solved) 00113 { 00114 const int chosenStartRegion = startRegions_.sampleUniform(); 00115 int chosenGoalRegion = -1; 00116 00117 // if we have not sampled too many goal regions already 00118 if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_/2) 00119 { 00120 if (const base::State *g = pis_.nextGoal()) 00121 { 00122 chosenGoalRegion = decomp_->locateRegion(g); 00123 goalRegions_.insert(chosenGoalRegion); 00124 } 00125 } 00126 if (chosenGoalRegion == -1) 00127 chosenGoalRegion = goalRegions_.sampleUniform(); 00128 00129 leadComputeFn(chosenStartRegion, chosenGoalRegion, lead_); 00130 computeAvailableRegions(); 00131 for (int i = 0; i < numRegionExpansions_ && !solved && !ptc; ++i) 00132 { 00133 const int region = selectRegion(); 00134 bool improved = false; 00135 for (int j = 0; j < numTreeSelections_ && !solved && !ptc; ++j) 00136 { 00137 newMotions.clear(); 00138 selectAndExtend(graph_[boost::vertex(region, graph_)], newMotions); 00139 for (std::vector<Motion*>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc; ++m) 00140 { 00141 Motion *motion = *m; 00142 double distance; 00143 solved = goal->isSatisfied(motion->state, &distance); 00144 if (solved) 00145 { 00146 goalDist = distance; 00147 solution = motion; 00148 break; 00149 } 00150 00151 // Check for approximate (best-so-far) solution 00152 if (distance < goalDist) 00153 { 00154 goalDist = distance; 00155 solution = motion; 00156 } 00157 const int newRegion = decomp_->locateRegion(motion->state); 00158 graph_[boost::vertex(newRegion, graph_)].motions.push_back(motion); 00159 ++numMotions_; 00160 Region &newRegionObj = graph_[boost::vertex(newRegion, graph_)]; 00161 improved |= updateCoverageEstimate(newRegionObj, motion->state); 00162 /* If tree has just crossed from one region to its neighbor, 00163 update the connection estimates. If the tree has crossed an entire region, 00164 then region and newRegion are not adjacent, and so we do not update estimates. */ 00165 if (newRegion != region 00166 && regionsToEdge_.count(std::pair<int,int>(region,newRegion)) > 0) 00167 { 00168 Adjacency *adj = regionsToEdge_[std::pair<int,int>(region,newRegion)]; 00169 adj->empty = false; 00170 ++adj->numSelections; 00171 improved |= updateConnectionEstimate(graph_[boost::vertex(region, graph_)], newRegionObj, motion->state); 00172 } 00173 00174 /* If this region already exists in availDist, update its weight. */ 00175 if (newRegionObj.pdfElem != NULL) 00176 availDist_.update(newRegionObj.pdfElem, newRegionObj.weight); 00177 /* Otherwise, only add this region to availDist 00178 if it already exists in the lead. */ 00179 else if (std::find(lead_.begin(), lead_.end(), newRegion) != lead_.end()) 00180 { 00181 PDF<int>::Element *elem = availDist_.add(newRegion, newRegionObj.weight); 00182 newRegionObj.pdfElem = elem; 00183 } 00184 } 00185 } 00186 if (!improved && rng_.uniform01() < probAbandonLeadEarly_) 00187 break; 00188 } 00189 } 00190 bool addedSolution = false; 00191 if (solution != NULL) 00192 { 00193 std::vector<const Motion*> mpath; 00194 while (solution != NULL) 00195 { 00196 mpath.push_back(solution); 00197 solution = solution->parent; 00198 } 00199 PathControl *path = new PathControl(si_); 00200 for (int i = mpath.size()-1; i >= 0; --i) 00201 if (mpath[i]->parent) 00202 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize()); 00203 else 00204 path->append(mpath[i]->state); 00205 pdef_->addSolutionPath(base::PathPtr(path), !solved, goalDist, getName()); 00206 addedSolution = true; 00207 } 00208 return addedSolution ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; 00209 } 00210 00211 void ompl::control::Syclop::setLeadComputeFn(const LeadComputeFn& compute) 00212 { 00213 leadComputeFn = compute; 00214 } 00215 00216 void ompl::control::Syclop::addEdgeCostFactor(const EdgeCostFactorFn& factor) 00217 { 00218 edgeCostFactors_.push_back(factor); 00219 } 00220 00221 void ompl::control::Syclop::clearEdgeCostFactors() 00222 { 00223 edgeCostFactors_.clear(); 00224 } 00225 00226 void ompl::control::Syclop::initRegion(Region &r) 00227 { 00228 r.numSelections = 0; 00229 r.volume = 1.0; 00230 r.percentValidCells = 1.0; 00231 r.freeVolume = 1.0; 00232 r.pdfElem = NULL; 00233 } 00234 00235 void ompl::control::Syclop::setupRegionEstimates() 00236 { 00237 std::vector<int> numTotal(decomp_->getNumRegions(), 0); 00238 std::vector<int> numValid(decomp_->getNumRegions(), 0); 00239 base::StateValidityCheckerPtr checker = si_->getStateValidityChecker(); 00240 base::StateSamplerPtr sampler = si_->allocStateSampler(); 00241 base::State *s = si_->allocState(); 00242 00243 for (int i = 0; i < numFreeVolSamples_; ++i) 00244 { 00245 sampler->sampleUniform(s); 00246 int rid = decomp_->locateRegion(s); 00247 if (rid >= 0) 00248 { 00249 if (checker->isValid(s)) 00250 ++numValid[rid]; 00251 ++numTotal[rid]; 00252 } 00253 } 00254 si_->freeState(s); 00255 00256 for (int i = 0; i < decomp_->getNumRegions(); ++i) 00257 { 00258 Region &r = graph_[boost::vertex(i, graph_)]; 00259 r.volume = decomp_->getRegionVolume(i); 00260 if (numTotal[i] == 0) 00261 r.percentValidCells = 1.0; 00262 else 00263 r.percentValidCells = ((double) numValid[i]) / (double)numTotal[i]; 00264 r.freeVolume = r.percentValidCells * r.volume; 00265 if (r.freeVolume < std::numeric_limits<double>::epsilon()) 00266 r.freeVolume = std::numeric_limits<double>::epsilon(); 00267 updateRegion(r); 00268 } 00269 } 00270 00271 void ompl::control::Syclop::updateRegion(Region &r) 00272 { 00273 const double f = r.freeVolume*r.freeVolume*r.freeVolume*r.freeVolume; 00274 r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f); 00275 r.weight = f / ((1 + r.covGridCells.size())*(1 + r.numSelections*r.numSelections)); 00276 } 00277 00278 void ompl::control::Syclop::initEdge(Adjacency &adj, const Region *source, const Region *target) 00279 { 00280 adj.source = source; 00281 adj.target = target; 00282 updateEdge(adj); 00283 regionsToEdge_[std::pair<int,int>(source->index, target->index)] = &adj; 00284 } 00285 00286 void ompl::control::Syclop::setupEdgeEstimates() 00287 { 00288 EdgeIter ei, eend; 00289 for (boost::tie(ei,eend) = boost::edges(graph_) ; ei != eend; ++ei) 00290 { 00291 Adjacency &adj = graph_[*ei]; 00292 adj.empty = true; 00293 adj.numLeadInclusions = 0; 00294 adj.numSelections = 0; 00295 updateEdge(adj); 00296 } 00297 } 00298 00299 void ompl::control::Syclop::updateEdge(Adjacency &a) 00300 { 00301 a.cost = 1.0; 00302 for (std::vector<EdgeCostFactorFn>::const_iterator i = edgeCostFactors_.begin(); i != edgeCostFactors_.end(); ++i) 00303 { 00304 const EdgeCostFactorFn& factor = *i; 00305 a.cost *= factor(a.source->index, a.target->index); 00306 } 00307 } 00308 00309 bool ompl::control::Syclop::updateCoverageEstimate(Region &r, const base::State *s) 00310 { 00311 const int covCell = covGrid_.locateRegion(s); 00312 if (r.covGridCells.count(covCell) == 1) 00313 return false; 00314 r.covGridCells.insert(covCell); 00315 updateRegion(r); 00316 return true; 00317 } 00318 00319 bool ompl::control::Syclop::updateConnectionEstimate(const Region &c, const Region &d, const base::State *s) 00320 { 00321 Adjacency &adj = *regionsToEdge_[std::pair<int,int>(c.index,d.index)]; 00322 const int covCell = covGrid_.locateRegion(s); 00323 if (adj.covGridCells.count(covCell) == 1) 00324 return false; 00325 adj.covGridCells.insert(covCell); 00326 updateEdge(adj); 00327 return true; 00328 } 00329 00330 void ompl::control::Syclop::buildGraph() 00331 { 00332 VertexIndexMap index = get(boost::vertex_index, graph_); 00333 std::vector<int> neighbors; 00334 for (int i = 0; i < decomp_->getNumRegions(); ++i) 00335 { 00336 const RegionGraph::vertex_descriptor v = boost::add_vertex(graph_); 00337 Region &r = graph_[boost::vertex(v, graph_)]; 00338 initRegion(r); 00339 r.index = index[v]; 00340 } 00341 VertexIter vi, vend; 00342 for (boost::tie(vi,vend) = boost::vertices(graph_); vi != vend; ++vi) 00343 { 00344 /* Create an edge between this vertex and each of its neighboring regions in the decomposition, 00345 and initialize the edge's Adjacency object. */ 00346 decomp_->getNeighbors(index[*vi], neighbors); 00347 for (std::vector<int>::const_iterator j = neighbors.begin(); j != neighbors.end(); ++j) 00348 { 00349 RegionGraph::edge_descriptor edge; 00350 bool ignore; 00351 boost::tie(edge, ignore) = boost::add_edge(*vi, boost::vertex(*j, graph_), graph_); 00352 initEdge(graph_[edge], &graph_[*vi], &graph_[boost::vertex(*j, graph_)]); 00353 } 00354 neighbors.clear(); 00355 } 00356 } 00357 00358 void ompl::control::Syclop::clearGraphDetails() 00359 { 00360 VertexIter vi, vend; 00361 for (boost::tie(vi,vend) = boost::vertices(graph_); vi != vend; ++vi) 00362 graph_[*vi].clear(); 00363 EdgeIter ei, eend; 00364 for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei) 00365 graph_[*ei].clear(); 00366 graphReady_ = false; 00367 } 00368 00369 int ompl::control::Syclop::selectRegion() 00370 { 00371 const int index = availDist_.sample(rng_.uniform01()); 00372 Region ®ion = graph_[boost::vertex(index, graph_)]; 00373 ++region.numSelections; 00374 updateRegion(region); 00375 return index; 00376 } 00377 00378 void ompl::control::Syclop::computeAvailableRegions() 00379 { 00380 for (unsigned int i = 0; i < availDist_.size(); ++i) 00381 graph_[boost::vertex(availDist_[i],graph_)].pdfElem = NULL; 00382 availDist_.clear(); 00383 for (int i = lead_.size()-1; i >= 0; --i) 00384 { 00385 Region &r = graph_[boost::vertex(lead_[i], graph_)]; 00386 if (!r.motions.empty()) 00387 { 00388 r.pdfElem = availDist_.add(lead_[i], r.weight); 00389 if (rng_.uniform01() >= probKeepAddingToAvail_) 00390 break; 00391 } 00392 } 00393 } 00394 00395 void ompl::control::Syclop::defaultComputeLead(int startRegion, int goalRegion, std::vector<int>& lead) 00396 { 00397 lead.clear(); 00398 if (startRegion == goalRegion) 00399 { 00400 lead.push_back(startRegion); 00401 return; 00402 } 00403 00404 else if (rng_.uniform01() < probShortestPath_) 00405 { 00406 std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions()); 00407 std::vector<double> distances(decomp_->getNumRegions()); 00408 00409 try 00410 { 00411 boost::astar_search(graph_, boost::vertex(startRegion, graph_), DecompositionHeuristic(this, getRegionFromIndex(goalRegion)), 00412 boost::weight_map(get(&Adjacency::cost, graph_)).distance_map( 00413 boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, graph_) 00414 )).predecessor_map( 00415 boost::make_iterator_property_map(parents.begin(), get(boost::vertex_index, graph_)) 00416 ).visitor(GoalVisitor(goalRegion)) 00417 ); 00418 } 00419 catch (found_goal fg) 00420 { 00421 int region = goalRegion; 00422 int leadLength = 1; 00423 00424 while (region != startRegion) 00425 { 00426 region = parents[region]; 00427 ++leadLength; 00428 } 00429 lead.resize(leadLength); 00430 region = goalRegion; 00431 for (int i = leadLength-1; i >= 0; --i) 00432 { 00433 lead[i] = region; 00434 region = parents[region]; 00435 } 00436 } 00437 } 00438 else 00439 { 00440 /* Run a random-DFS over the decomposition graph from the start region to the goal region. 00441 There may be a way to do this using boost::depth_first_search. */ 00442 VertexIndexMap index = get(boost::vertex_index, graph_); 00443 std::stack<int> nodesToProcess; 00444 std::vector<int> parents(decomp_->getNumRegions(), -1); 00445 parents[startRegion] = startRegion; 00446 nodesToProcess.push(startRegion); 00447 bool goalFound = false; 00448 while (!goalFound && !nodesToProcess.empty()) 00449 { 00450 const int v = nodesToProcess.top(); 00451 nodesToProcess.pop(); 00452 std::vector<int> neighbors; 00453 boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend; 00454 for (boost::tie(ai,aend) = adjacent_vertices(boost::vertex(v, graph_), graph_); ai != aend; ++ai) 00455 { 00456 if (parents[index[*ai]] < 0) 00457 { 00458 neighbors.push_back(index[*ai]); 00459 parents[index[*ai]] = v; 00460 } 00461 } 00462 for (std::size_t i = 0; i < neighbors.size(); ++i) 00463 { 00464 const int choice = rng_.uniformInt(i, neighbors.size()-1); 00465 if (neighbors[choice] == goalRegion) 00466 { 00467 int region = goalRegion; 00468 int leadLength = 1; 00469 while (region != startRegion) 00470 { 00471 region = parents[region]; 00472 ++leadLength; 00473 } 00474 lead.resize(leadLength); 00475 region = goalRegion; 00476 for (int j = leadLength-1; j >= 0; --j) 00477 { 00478 lead[j] = region; 00479 region = parents[region]; 00480 } 00481 goalFound = true; 00482 break; 00483 } 00484 nodesToProcess.push(neighbors[choice]); 00485 std::swap(neighbors[i], neighbors[choice]); 00486 } 00487 } 00488 } 00489 00490 //Now that we have a lead, update the edge weights. 00491 for (std::size_t i = 0; i < lead.size()-1; ++i) 00492 { 00493 Adjacency &adj = *regionsToEdge_[std::pair<int,int>(lead[i], lead[i+1])]; 00494 if (adj.empty) 00495 { 00496 ++adj.numLeadInclusions; 00497 updateEdge(adj); 00498 } 00499 } 00500 } 00501 00502 double ompl::control::Syclop::defaultEdgeCost(int r, int s) 00503 { 00504 const Adjacency &a = *regionsToEdge_[std::pair<int,int>(r,s)]; 00505 double factor = 1.0; 00506 const int nsel = (a.empty ? a.numLeadInclusions : a.numSelections); 00507 factor = (double)(1 + nsel*nsel) / (double)(1 + a.covGridCells.size()*a.covGridCells.size()); 00508 factor *= (a.source->alpha * a.target->alpha); 00509 return factor; 00510 }