37 #include "ompl/control/planners/syclop/Syclop.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/base/ProblemDefinition.h"
44 const double ompl::control::Syclop::Defaults::PROB_ABANDON_LEAD_EARLY = 0.25;
45 const double ompl::control::Syclop::Defaults::PROB_KEEP_ADDING_TO_AVAIL = 0.50;
46 const double ompl::control::Syclop::Defaults::PROB_SHORTEST_PATH = 0.95;
52 setLeadComputeFn([
this](
int startRegion,
int goalRegion, std::vector<int> &lead)
54 defaultComputeLead(startRegion, goalRegion, lead);
59 return defaultEdgeCost(r, s);
68 clearEdgeCostFactors();
70 startRegions_.clear();
80 setupRegionEstimates();
86 const int region = decomp_->locateRegion(s);
87 startRegions_.insert(region);
88 Motion *startMotion = addRoot(s);
89 graph_[boost::vertex(region, graph_)].motions.push_back(startMotion);
91 updateCoverageEstimate(graph_[boost::vertex(region, graph_)], s);
93 if (startRegions_.empty())
95 OMPL_ERROR(
"%s: There are no valid start states", getName().c_str());
100 if (goalRegions_.empty())
103 goalRegions_.insert(decomp_->locateRegion(g));
106 OMPL_ERROR(
"%s: Unable to sample a valid goal state", getName().c_str());
111 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure", getName().c_str(), numMotions_);
113 std::vector<Motion *> newMotions;
114 const Motion *solution =
nullptr;
116 double goalDist = std::numeric_limits<double>::infinity();
118 while (!ptc && !solved)
120 const int chosenStartRegion = startRegions_.sampleUniform();
121 int chosenGoalRegion = -1;
124 if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_ / 2)
128 chosenGoalRegion = decomp_->locateRegion(g);
129 goalRegions_.insert(chosenGoalRegion);
132 if (chosenGoalRegion == -1)
133 chosenGoalRegion = goalRegions_.sampleUniform();
135 leadComputeFn(chosenStartRegion, chosenGoalRegion, lead_);
136 computeAvailableRegions();
137 for (
int i = 0; i < numRegionExpansions_ && !solved && !ptc; ++i)
139 const int region = selectRegion();
140 bool improved =
false;
141 for (
int j = 0; j < numTreeSelections_ && !solved && !ptc; ++j)
144 selectAndExtend(graph_[boost::vertex(region, graph_)], newMotions);
145 for (std::vector<Motion *>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc; ++m)
158 if (distance < goalDist)
163 const int newRegion = decomp_->locateRegion(motion->
state);
164 graph_[boost::vertex(newRegion, graph_)].motions.push_back(motion);
166 Region &newRegionObj = graph_[boost::vertex(newRegion, graph_)];
167 improved |= updateCoverageEstimate(newRegionObj, motion->
state);
171 if (newRegion != region && regionsToEdge_.count(std::pair<int, int>(region, newRegion)) > 0)
173 Adjacency *adj = regionsToEdge_[std::pair<int, int>(region, newRegion)];
176 improved |= updateConnectionEstimate(graph_[boost::vertex(region, graph_)], newRegionObj,
181 if (newRegionObj.
pdfElem !=
nullptr)
182 availDist_.update(newRegionObj.
pdfElem, newRegionObj.
weight);
185 else if (std::find(lead_.begin(), lead_.end(), newRegion) != lead_.end())
192 if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
196 bool addedSolution =
false;
197 if (solution !=
nullptr)
199 std::vector<const Motion *> mpath;
200 while (solution !=
nullptr)
202 mpath.push_back(solution);
203 solution = solution->
parent;
205 auto path(std::make_shared<PathControl>(si_));
206 for (
int i = mpath.size() - 1; i >= 0; --i)
207 if (mpath[i]->parent !=
nullptr)
208 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
210 path->append(mpath[i]->state);
211 pdef_->addSolutionPath(path, !solved, goalDist, getName());
212 addedSolution =
true;
219 leadComputeFn = compute;
224 edgeCostFactors_.push_back(factor);
229 edgeCostFactors_.clear();
232 void ompl::control::Syclop::initRegion(Region &r)
236 r.percentValidCells = 1.0;
241 void ompl::control::Syclop::setupRegionEstimates()
243 std::vector<int> numTotal(decomp_->getNumRegions(), 0);
244 std::vector<int> numValid(decomp_->getNumRegions(), 0);
245 base::StateValidityCheckerPtr checker = si_->getStateValidityChecker();
246 base::StateSamplerPtr sampler = si_->allocStateSampler();
249 for (
int i = 0; i < numFreeVolSamples_; ++i)
251 sampler->sampleUniform(s);
252 int rid = decomp_->locateRegion(s);
255 if (checker->isValid(s))
262 for (
int i = 0; i < decomp_->getNumRegions(); ++i)
264 Region &r = graph_[boost::vertex(i, graph_)];
265 r.volume = decomp_->getRegionVolume(i);
266 if (numTotal[i] == 0)
267 r.percentValidCells = 1.0;
269 r.percentValidCells = ((double)numValid[i]) / (double)numTotal[i];
270 r.freeVolume = r.percentValidCells * r.volume;
271 if (r.freeVolume < std::numeric_limits<double>::epsilon())
272 r.freeVolume = std::numeric_limits<double>::epsilon();
277 void ompl::control::Syclop::updateRegion(Region &r)
279 const double f = r.freeVolume * r.freeVolume * r.freeVolume * r.freeVolume;
280 r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f);
281 r.weight = f / ((1 + r.covGridCells.size()) * (1 + r.numSelections * r.numSelections));
284 void ompl::control::Syclop::initEdge(Adjacency &adj,
const Region *source,
const Region *target)
289 regionsToEdge_[std::pair<int, int>(source->index, target->index)] = &adj;
292 void ompl::control::Syclop::setupEdgeEstimates()
295 for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
297 Adjacency &adj = graph_[*ei];
299 adj.numLeadInclusions = 0;
300 adj.numSelections = 0;
305 void ompl::control::Syclop::updateEdge(Adjacency &a)
308 for (
const auto &factor : edgeCostFactors_)
310 a.cost *= factor(a.source->index, a.target->index);
314 bool ompl::control::Syclop::updateCoverageEstimate(Region &r,
const base::State *s)
316 const int covCell = covGrid_.locateRegion(s);
317 if (r.covGridCells.count(covCell) == 1)
319 r.covGridCells.insert(covCell);
324 bool ompl::control::Syclop::updateConnectionEstimate(
const Region &c,
const Region &d,
const base::State *s)
326 Adjacency &adj = *regionsToEdge_[std::pair<int, int>(c.index, d.index)];
327 const int covCell = covGrid_.locateRegion(s);
328 if (adj.covGridCells.count(covCell) == 1)
330 adj.covGridCells.insert(covCell);
335 void ompl::control::Syclop::buildGraph()
337 VertexIndexMap index = get(boost::vertex_index, graph_);
338 std::vector<int> neighbors;
339 for (
int i = 0; i < decomp_->getNumRegions(); ++i)
341 const RegionGraph::vertex_descriptor v = boost::add_vertex(graph_);
342 Region &r = graph_[boost::vertex(v, graph_)];
347 for (boost::tie(vi, vend) = boost::vertices(graph_); vi != vend; ++vi)
351 decomp_->getNeighbors(index[*vi], neighbors);
352 for (
const auto &j : neighbors)
354 RegionGraph::edge_descriptor edge;
356 boost::tie(edge, ignore) = boost::add_edge(*vi, boost::vertex(j, graph_), graph_);
357 initEdge(graph_[edge], &graph_[*vi], &graph_[boost::vertex(j, graph_)]);
363 void ompl::control::Syclop::clearGraphDetails()
366 for (boost::tie(vi, vend) = boost::vertices(graph_); vi != vend; ++vi)
369 for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
374 int ompl::control::Syclop::selectRegion()
376 const int index = availDist_.sample(rng_.uniform01());
377 Region ®ion = graph_[boost::vertex(index, graph_)];
378 ++region.numSelections;
379 updateRegion(region);
383 void ompl::control::Syclop::computeAvailableRegions()
385 for (
unsigned int i = 0; i < availDist_.size(); ++i)
386 graph_[boost::vertex(availDist_[i], graph_)].pdfElem =
nullptr;
388 for (
int i = lead_.size() - 1; i >= 0; --i)
390 Region &r = graph_[boost::vertex(lead_[i], graph_)];
391 if (!r.motions.empty())
393 r.pdfElem = availDist_.add(lead_[i], r.weight);
394 if (rng_.uniform01() >= probKeepAddingToAvail_)
400 void ompl::control::Syclop::defaultComputeLead(
int startRegion,
int goalRegion, std::vector<int> &lead)
403 if (startRegion == goalRegion)
405 lead.push_back(startRegion);
409 if (rng_.uniform01() < probShortestPath_)
411 std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
412 std::vector<double> distances(decomp_->getNumRegions());
416 boost::astar_search(graph_, boost::vertex(startRegion, graph_),
417 DecompositionHeuristic(
this, getRegionFromIndex(goalRegion)),
418 boost::weight_map(get(&Adjacency::cost, graph_))
419 .distance_map(boost::make_iterator_property_map(distances.begin(),
420 get(boost::vertex_index, graph_)))
421 .predecessor_map(boost::make_iterator_property_map(
422 parents.begin(), get(boost::vertex_index, graph_)))
423 .visitor(GoalVisitor(goalRegion)));
425 catch (found_goal fg)
427 int region = goalRegion;
430 while (region != startRegion)
432 region = parents[region];
435 lead.resize(leadLength);
437 for (
int i = leadLength - 1; i >= 0; --i)
440 region = parents[region];
448 VertexIndexMap index = get(boost::vertex_index, graph_);
449 std::stack<int> nodesToProcess;
450 std::vector<int> parents(decomp_->getNumRegions(), -1);
451 parents[startRegion] = startRegion;
452 nodesToProcess.push(startRegion);
453 bool goalFound =
false;
454 while (!goalFound && !nodesToProcess.empty())
456 const int v = nodesToProcess.top();
457 nodesToProcess.pop();
458 std::vector<int> neighbors;
459 boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend;
460 for (boost::tie(ai, aend) = adjacent_vertices(boost::vertex(v, graph_), graph_); ai != aend; ++ai)
462 if (parents[index[*ai]] < 0)
464 neighbors.push_back(index[*ai]);
465 parents[index[*ai]] = v;
468 for (std::size_t i = 0; i < neighbors.size(); ++i)
470 const int choice = rng_.uniformInt(i, neighbors.size() - 1);
471 if (neighbors[choice] == goalRegion)
473 int region = goalRegion;
475 while (region != startRegion)
477 region = parents[region];
480 lead.resize(leadLength);
482 for (
int j = leadLength - 1; j >= 0; --j)
485 region = parents[region];
490 nodesToProcess.push(neighbors[choice]);
491 std::swap(neighbors[i], neighbors[choice]);
497 for (std::size_t i = 0; i < lead.size() - 1; ++i)
499 Adjacency &adj = *regionsToEdge_[std::pair<int, int>(lead[i], lead[i + 1])];
502 ++adj.numLeadInclusions;
508 double ompl::control::Syclop::defaultEdgeCost(
int r,
int s)
510 const Adjacency &a = *regionsToEdge_[std::pair<int, int>(r, s)];
512 const int nsel = (a.empty ? a.numLeadInclusions : a.numSelections);
513 factor = (1.0 + (double)nsel * nsel) / (1.0 + (double)a.covGridCells.size() * a.covGridCells.size());
514 factor *= (a.source->alpha * a.target->alpha);