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