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