37 #include "ompl/control/planners/syclop/Syclop.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/base/ProblemDefinition.h"
40 #include "ompl/util/DisableCompilerWarning.h"
45 const double ompl::control::Syclop::Defaults::PROB_ABANDON_LEAD_EARLY = 0.25;
46 const double ompl::control::Syclop::Defaults::PROB_KEEP_ADDING_TO_AVAIL = 0.50;
47 const double ompl::control::Syclop::Defaults::PROB_SHORTEST_PATH = 0.95;
53 setLeadComputeFn([
this](
int startRegion,
int goalRegion, std::vector<int> &lead)
55 defaultComputeLead(startRegion, goalRegion, lead);
60 return defaultEdgeCost(r, s);
69 clearEdgeCostFactors();
71 startRegions_.clear();
81 setupRegionEstimates();
87 const int region = decomp_->locateRegion(s);
88 startRegions_.insert(region);
89 Motion *startMotion = addRoot(s);
90 graph_[boost::vertex(region, graph_)].motions.push_back(startMotion);
92 updateCoverageEstimate(graph_[boost::vertex(region, graph_)], s);
94 if (startRegions_.empty())
96 OMPL_ERROR(
"%s: There are no valid start states", getName().c_str());
101 if (goalRegions_.empty())
104 goalRegions_.insert(decomp_->locateRegion(g));
107 OMPL_ERROR(
"%s: Unable to sample a valid goal state", getName().c_str());
112 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure", getName().c_str(), numMotions_);
114 std::vector<Motion *> newMotions;
115 const Motion *solution =
nullptr;
117 double goalDist = std::numeric_limits<double>::infinity();
119 while (!ptc && !solved)
121 const int chosenStartRegion = startRegions_.sampleUniform();
122 int chosenGoalRegion = -1;
125 if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_ / 2)
129 chosenGoalRegion = decomp_->locateRegion(g);
130 goalRegions_.insert(chosenGoalRegion);
133 if (chosenGoalRegion == -1)
134 chosenGoalRegion = goalRegions_.sampleUniform();
136 leadComputeFn(chosenStartRegion, chosenGoalRegion, lead_);
137 computeAvailableRegions();
138 for (
int i = 0; i < numRegionExpansions_ && !solved && !ptc; ++i)
140 const int region = selectRegion();
141 bool improved =
false;
142 for (
int j = 0; j < numTreeSelections_ && !solved && !ptc; ++j)
145 selectAndExtend(graph_[boost::vertex(region, graph_)], newMotions);
146 for (std::vector<Motion *>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc; ++m)
150 solved = goal->isSatisfied(motion->
state, &distance);
159 if (distance < goalDist)
164 const int newRegion = decomp_->locateRegion(motion->
state);
165 graph_[boost::vertex(newRegion, graph_)].motions.push_back(motion);
167 Region &newRegionObj = graph_[boost::vertex(newRegion, graph_)];
168 improved |= updateCoverageEstimate(newRegionObj, motion->
state);
172 if (newRegion != region && regionsToEdge_.count(std::pair<int, int>(region, newRegion)) > 0)
174 Adjacency *adj = regionsToEdge_[std::pair<int, int>(region, newRegion)];
177 improved |= updateConnectionEstimate(graph_[boost::vertex(region, graph_)], newRegionObj,
182 if (newRegionObj.
pdfElem !=
nullptr)
183 availDist_.update(newRegionObj.
pdfElem, newRegionObj.
weight);
186 else if (std::find(lead_.begin(), lead_.end(), newRegion) != lead_.end())
193 if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
197 bool addedSolution =
false;
198 if (solution !=
nullptr)
200 std::vector<const Motion *> mpath;
201 while (solution !=
nullptr)
203 mpath.push_back(solution);
204 solution = solution->
parent;
206 auto path(std::make_shared<PathControl>(si_));
207 for (
int i = mpath.size() - 1; i >= 0; --i)
208 if (mpath[i]->parent !=
nullptr)
209 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
211 path->append(mpath[i]->state);
212 pdef_->addSolutionPath(path, !solved, goalDist, getName());
213 addedSolution =
true;
220 leadComputeFn = compute;
225 edgeCostFactors_.push_back(factor);
230 edgeCostFactors_.clear();
233 void ompl::control::Syclop::initRegion(Region &r)
237 r.percentValidCells = 1.0;
242 void ompl::control::Syclop::setupRegionEstimates()
244 std::vector<int> numTotal(decomp_->getNumRegions(), 0);
245 std::vector<int> numValid(decomp_->getNumRegions(), 0);
246 base::StateValidityCheckerPtr checker = si_->getStateValidityChecker();
247 base::StateSamplerPtr sampler = si_->allocStateSampler();
250 for (
int i = 0; i < numFreeVolSamples_; ++i)
252 sampler->sampleUniform(s);
253 int rid = decomp_->locateRegion(s);
256 if (checker->isValid(s))
263 for (
int i = 0; i < decomp_->getNumRegions(); ++i)
265 Region &r = graph_[boost::vertex(i, graph_)];
266 r.volume = decomp_->getRegionVolume(i);
267 if (numTotal[i] == 0)
268 r.percentValidCells = 1.0;
270 r.percentValidCells = ((double)numValid[i]) / (double)numTotal[i];
271 r.freeVolume = r.percentValidCells * r.volume;
272 if (r.freeVolume < std::numeric_limits<double>::epsilon())
273 r.freeVolume = std::numeric_limits<double>::epsilon();
278 void ompl::control::Syclop::updateRegion(Region &r)
280 const double f = r.freeVolume * r.freeVolume * r.freeVolume * r.freeVolume;
281 r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f);
282 r.weight = f / ((1 + r.covGridCells.size()) * (1 + r.numSelections * r.numSelections));
285 void ompl::control::Syclop::initEdge(Adjacency &adj,
const Region *source,
const Region *target)
290 regionsToEdge_[std::pair<int, int>(source->index, target->index)] = &adj;
293 void ompl::control::Syclop::setupEdgeEstimates()
295 OMPL_PUSH_DISABLE_GCC_WARNING(-Wmaybe-uninitialized)
298 for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
300 Adjacency &adj = graph_[*ei];
302 adj.numLeadInclusions = 0;
303 adj.numSelections = 0;
308 void ompl::control::Syclop::updateEdge(Adjacency &a)
311 for (
const auto &factor : edgeCostFactors_)
313 a.cost *= factor(a.source->index, a.target->index);
317 bool ompl::control::Syclop::updateCoverageEstimate(Region &r,
const base::State *s)
319 const int covCell = covGrid_.locateRegion(s);
320 if (r.covGridCells.count(covCell) == 1)
322 r.covGridCells.insert(covCell);
327 bool ompl::control::Syclop::updateConnectionEstimate(
const Region &c,
const Region &d,
const base::State *s)
329 Adjacency &adj = *regionsToEdge_[std::pair<int, int>(c.index, d.index)];
330 const int covCell = covGrid_.locateRegion(s);
331 if (adj.covGridCells.count(covCell) == 1)
333 adj.covGridCells.insert(covCell);
338 void ompl::control::Syclop::buildGraph()
340 VertexIndexMap index = get(boost::vertex_index, graph_);
341 std::vector<int> neighbors;
342 for (
int i = 0; i < decomp_->getNumRegions(); ++i)
344 const RegionGraph::vertex_descriptor v = boost::add_vertex(graph_);
345 Region &r = graph_[boost::vertex(v, graph_)];
350 for (boost::tie(vi, vend) = boost::vertices(graph_); vi != vend; ++vi)
354 decomp_->getNeighbors(index[*vi], neighbors);
355 for (
const auto &j : neighbors)
357 RegionGraph::edge_descriptor edge;
359 boost::tie(edge, ignore) = boost::add_edge(*vi, boost::vertex(j, graph_), graph_);
360 initEdge(graph_[edge], &graph_[*vi], &graph_[boost::vertex(j, graph_)]);
366 void ompl::control::Syclop::clearGraphDetails()
369 for (boost::tie(vi, vend) = boost::vertices(graph_); vi != vend; ++vi)
371 OMPL_PUSH_DISABLE_GCC_WARNING(-Wmaybe-uninitialized)
374 for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
379 int ompl::control::Syclop::selectRegion()
381 const int index = availDist_.sample(rng_.uniform01());
382 Region ®ion = graph_[boost::vertex(index, graph_)];
383 ++region.numSelections;
384 updateRegion(region);
388 void ompl::control::Syclop::computeAvailableRegions()
390 for (
unsigned int i = 0; i < availDist_.size(); ++i)
391 graph_[boost::vertex(availDist_[i], graph_)].pdfElem =
nullptr;
393 for (
int i = lead_.size() - 1; i >= 0; --i)
395 Region &r = graph_[boost::vertex(lead_[i], graph_)];
396 if (!r.motions.empty())
398 r.pdfElem = availDist_.add(lead_[i], r.weight);
399 if (rng_.uniform01() >= probKeepAddingToAvail_)
405 void ompl::control::Syclop::defaultComputeLead(
int startRegion,
int goalRegion, std::vector<int> &lead)
408 if (startRegion == goalRegion)
410 lead.push_back(startRegion);
414 if (rng_.uniform01() < probShortestPath_)
416 std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
417 std::vector<double> distances(decomp_->getNumRegions());
421 boost::astar_search(graph_, boost::vertex(startRegion, graph_),
422 DecompositionHeuristic(
this, getRegionFromIndex(goalRegion)),
423 boost::weight_map(get(&Adjacency::cost, graph_))
424 .distance_map(boost::make_iterator_property_map(distances.begin(),
425 get(boost::vertex_index, graph_)))
426 .predecessor_map(boost::make_iterator_property_map(
427 parents.begin(), get(boost::vertex_index, graph_)))
428 .visitor(GoalVisitor(goalRegion)));
430 catch (found_goal fg)
432 int region = goalRegion;
435 while (region != startRegion)
437 region = parents[region];
440 lead.resize(leadLength);
442 for (
int i = leadLength - 1; i >= 0; --i)
445 region = parents[region];
453 VertexIndexMap index = get(boost::vertex_index, graph_);
454 std::stack<int> nodesToProcess;
455 std::vector<int> parents(decomp_->getNumRegions(), -1);
456 parents[startRegion] = startRegion;
457 nodesToProcess.push(startRegion);
458 bool goalFound =
false;
459 while (!goalFound && !nodesToProcess.empty())
461 const int v = nodesToProcess.top();
462 nodesToProcess.pop();
463 std::vector<int> neighbors;
464 boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend;
465 for (boost::tie(ai, aend) = adjacent_vertices(boost::vertex(v, graph_), graph_); ai != aend; ++ai)
467 if (parents[index[*ai]] < 0)
469 neighbors.push_back(index[*ai]);
470 parents[index[*ai]] = v;
473 for (std::size_t i = 0; i < neighbors.size(); ++i)
475 const int choice = rng_.uniformInt(i, neighbors.size() - 1);
476 if (neighbors[choice] == goalRegion)
478 int region = goalRegion;
480 while (region != startRegion)
482 region = parents[region];
485 lead.resize(leadLength);
487 for (
int j = leadLength - 1; j >= 0; --j)
490 region = parents[region];
495 nodesToProcess.push(neighbors[choice]);
496 std::swap(neighbors[i], neighbors[choice]);
502 for (std::size_t i = 0; i < lead.size() - 1; ++i)
504 Adjacency &adj = *regionsToEdge_[std::pair<int, int>(lead[i], lead[i + 1])];
507 ++adj.numLeadInclusions;
513 double ompl::control::Syclop::defaultEdgeCost(
int r,
int s)
515 const Adjacency &a = *regionsToEdge_[std::pair<int, int>(r, s)];
517 const int nsel = (a.empty ? a.numLeadInclusions : a.numSelections);
518 factor = (1.0 + (double)nsel * nsel) / (1.0 + (double)a.covGridCells.size() * a.covGridCells.size());
519 factor *= (a.source->alpha * a.target->alpha);