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)
53 { defaultComputeLead(startRegion, goalRegion, lead); });
63 clearEdgeCostFactors();
65 startRegions_.clear();
75 setupRegionEstimates();
81 const int region = decomp_->locateRegion(s);
82 startRegions_.insert(region);
83 Motion *startMotion = addRoot(s);
84 graph_[boost::vertex(region, graph_)].motions.push_back(startMotion);
86 updateCoverageEstimate(graph_[boost::vertex(region, graph_)], s);
88 if (startRegions_.empty())
90 OMPL_ERROR(
"%s: There are no valid start states", getName().c_str());
95 if (goalRegions_.empty())
98 goalRegions_.insert(decomp_->locateRegion(g));
101 OMPL_ERROR(
"%s: Unable to sample a valid goal state", getName().c_str());
106 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure", getName().c_str(), numMotions_);
108 std::vector<Motion *> newMotions;
109 const Motion *solution =
nullptr;
111 double goalDist = std::numeric_limits<double>::infinity();
113 while (!ptc && !solved)
115 const int chosenStartRegion = startRegions_.sampleUniform();
116 int chosenGoalRegion = -1;
119 if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_ / 2)
123 chosenGoalRegion = decomp_->locateRegion(g);
124 goalRegions_.insert(chosenGoalRegion);
127 if (chosenGoalRegion == -1)
128 chosenGoalRegion = goalRegions_.sampleUniform();
130 leadComputeFn(chosenStartRegion, chosenGoalRegion, lead_);
131 computeAvailableRegions();
132 for (
int i = 0; i < numRegionExpansions_ && !solved && !ptc; ++i)
134 const int region = selectRegion();
135 bool improved =
false;
136 for (
int j = 0; j < numTreeSelections_ && !solved && !ptc; ++j)
139 selectAndExtend(graph_[boost::vertex(region, graph_)], newMotions);
140 for (std::vector<Motion *>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc; ++m)
144 solved = goal->isSatisfied(motion->
state, &distance);
153 if (distance < goalDist)
158 const int newRegion = decomp_->locateRegion(motion->
state);
159 graph_[boost::vertex(newRegion, graph_)].motions.push_back(motion);
161 Region &newRegionObj = graph_[boost::vertex(newRegion, graph_)];
162 improved |= updateCoverageEstimate(newRegionObj, motion->
state);
166 if (newRegion != region && regionsToEdge_.count(std::pair<int, int>(region, newRegion)) > 0)
168 Adjacency *adj = regionsToEdge_[std::pair<int, int>(region, newRegion)];
171 improved |= updateConnectionEstimate(graph_[boost::vertex(region, graph_)], newRegionObj,
176 if (newRegionObj.
pdfElem !=
nullptr)
177 availDist_.update(newRegionObj.
pdfElem, newRegionObj.
weight);
180 else if (std::find(lead_.begin(), lead_.end(), newRegion) != lead_.end())
187 if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
191 bool addedSolution =
false;
192 if (solution !=
nullptr)
194 std::vector<const Motion *> mpath;
195 while (solution !=
nullptr)
197 mpath.push_back(solution);
198 solution = solution->
parent;
200 auto path(std::make_shared<PathControl>(si_));
201 for (
int i = mpath.size() - 1; i >= 0; --i)
202 if (mpath[i]->parent !=
nullptr)
203 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
205 path->append(mpath[i]->state);
206 pdef_->addSolutionPath(path, !solved, goalDist, getName());
207 addedSolution =
true;
214 leadComputeFn = compute;
219 edgeCostFactors_.push_back(factor);
224 edgeCostFactors_.clear();
227 void ompl::control::Syclop::initRegion(Region &r)
231 r.percentValidCells = 1.0;
236 void ompl::control::Syclop::setupRegionEstimates()
238 std::vector<int> numTotal(decomp_->getNumRegions(), 0);
239 std::vector<int> numValid(decomp_->getNumRegions(), 0);
240 base::StateValidityCheckerPtr checker = si_->getStateValidityChecker();
241 base::StateSamplerPtr sampler = si_->allocStateSampler();
244 for (
int i = 0; i < numFreeVolSamples_; ++i)
246 sampler->sampleUniform(s);
247 int rid = decomp_->locateRegion(s);
250 if (checker->isValid(s))
257 for (
int i = 0; i < decomp_->getNumRegions(); ++i)
259 Region &r = graph_[boost::vertex(i, graph_)];
260 r.volume = decomp_->getRegionVolume(i);
261 if (numTotal[i] == 0)
262 r.percentValidCells = 1.0;
264 r.percentValidCells = ((double)numValid[i]) / (double)numTotal[i];
265 r.freeVolume = r.percentValidCells * r.volume;
266 if (r.freeVolume < std::numeric_limits<double>::epsilon())
267 r.freeVolume = std::numeric_limits<double>::epsilon();
272 void ompl::control::Syclop::updateRegion(Region &r)
274 const double f = r.freeVolume * r.freeVolume * r.freeVolume * r.freeVolume;
275 r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f);
276 r.weight = f / ((1 + r.covGridCells.size()) * (1 + r.numSelections * r.numSelections));
279 void ompl::control::Syclop::initEdge(Adjacency &adj,
const Region *source,
const Region *target)
284 regionsToEdge_[std::pair<int, int>(source->index, target->index)] = &adj;
287 void ompl::control::Syclop::setupEdgeEstimates()
289 for (
auto [ei, eend] = boost::edges(graph_); ei != eend; ++ei)
291 Adjacency &adj = graph_[*ei];
293 adj.numLeadInclusions = 0;
294 adj.numSelections = 0;
299 void ompl::control::Syclop::updateEdge(Adjacency &a)
302 for (
const auto &factor : edgeCostFactors_)
304 a.cost *= factor(a.source->index, a.target->index);
308 bool ompl::control::Syclop::updateCoverageEstimate(Region &r,
const base::State *s)
310 const int covCell = covGrid_.locateRegion(s);
311 if (r.covGridCells.count(covCell) == 1)
313 r.covGridCells.insert(covCell);
318 bool ompl::control::Syclop::updateConnectionEstimate(
const Region &c,
const Region &d,
const base::State *s)
320 Adjacency &adj = *regionsToEdge_[std::pair<int, int>(c.index, d.index)];
321 const int covCell = covGrid_.locateRegion(s);
322 if (adj.covGridCells.count(covCell) == 1)
324 adj.covGridCells.insert(covCell);
329 void ompl::control::Syclop::buildGraph()
331 VertexIndexMap index = get(boost::vertex_index, graph_);
332 std::vector<int> neighbors;
333 for (
int i = 0; i < decomp_->getNumRegions(); ++i)
335 const RegionGraph::vertex_descriptor v = boost::add_vertex(graph_);
336 Region &r = graph_[boost::vertex(v, graph_)];
341 for (boost::tie(vi, vend) = boost::vertices(graph_); vi != vend; ++vi)
345 decomp_->getNeighbors(index[*vi], neighbors);
346 for (
const auto &j : neighbors)
348 RegionGraph::edge_descriptor edge;
350 boost::tie(edge, ignore) = boost::add_edge(*vi, boost::vertex(j, graph_), graph_);
351 initEdge(graph_[edge], &graph_[*vi], &graph_[boost::vertex(j, graph_)]);
357 void ompl::control::Syclop::clearGraphDetails()
360 for (boost::tie(vi, vend) = boost::vertices(graph_); vi != vend; ++vi)
362 for (
auto [ei, eend] = boost::edges(graph_); ei != eend; ++ei)
367 int ompl::control::Syclop::selectRegion()
369 const int index = availDist_.sample(rng_.uniform01());
370 Region ®ion = graph_[boost::vertex(index, graph_)];
371 ++region.numSelections;
372 updateRegion(region);
376 void ompl::control::Syclop::computeAvailableRegions()
378 for (
unsigned int i = 0; i < availDist_.size(); ++i)
379 graph_[boost::vertex(availDist_[i], graph_)].pdfElem =
nullptr;
381 for (
int i = lead_.size() - 1; i >= 0; --i)
383 Region &r = graph_[boost::vertex(lead_[i], graph_)];
384 if (!r.motions.empty())
386 r.pdfElem = availDist_.add(lead_[i], r.weight);
387 if (rng_.uniform01() >= probKeepAddingToAvail_)
393 void ompl::control::Syclop::defaultComputeLead(
int startRegion,
int goalRegion, std::vector<int> &lead)
396 if (startRegion == goalRegion)
398 lead.push_back(startRegion);
402 if (rng_.uniform01() < probShortestPath_)
404 std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
405 std::vector<double> distances(decomp_->getNumRegions());
409 boost::astar_search(graph_, boost::vertex(startRegion, graph_),
410 DecompositionHeuristic(
this, getRegionFromIndex(goalRegion)),
411 boost::weight_map(get(&Adjacency::cost, graph_))
412 .distance_map(boost::make_iterator_property_map(distances.begin(),
413 get(boost::vertex_index, graph_)))
414 .predecessor_map(boost::make_iterator_property_map(
415 parents.begin(), get(boost::vertex_index, graph_)))
416 .visitor(GoalVisitor(goalRegion)));
418 catch (found_goal fg)
420 int region = goalRegion;
423 while (region != startRegion)
425 region = parents[region];
428 lead.resize(leadLength);
430 for (
int i = leadLength - 1; i >= 0; --i)
433 region = parents[region];
441 VertexIndexMap index = get(boost::vertex_index, graph_);
442 std::stack<int> nodesToProcess;
443 std::vector<int> parents(decomp_->getNumRegions(), -1);
444 parents[startRegion] = startRegion;
445 nodesToProcess.push(startRegion);
446 bool goalFound =
false;
447 while (!goalFound && !nodesToProcess.empty())
449 const int v = nodesToProcess.top();
450 nodesToProcess.pop();
451 std::vector<int> neighbors;
452 boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend;
453 for (boost::tie(ai, aend) = adjacent_vertices(boost::vertex(v, graph_), graph_); ai != aend; ++ai)
455 if (parents[index[*ai]] < 0)
457 neighbors.push_back(index[*ai]);
458 parents[index[*ai]] = v;
461 for (std::size_t i = 0; i < neighbors.size(); ++i)
463 const int choice = rng_.uniformInt(i, neighbors.size() - 1);
464 if (neighbors[choice] == goalRegion)
466 int region = goalRegion;
468 while (region != startRegion)
470 region = parents[region];
473 lead.resize(leadLength);
475 for (
int j = leadLength - 1; j >= 0; --j)
478 region = parents[region];
483 nodesToProcess.push(neighbors[choice]);
484 std::swap(neighbors[i], neighbors[choice]);
490 for (std::size_t i = 0; i < lead.size() - 1; ++i)
492 Adjacency &adj = *regionsToEdge_[std::pair<int, int>(lead[i], lead[i + 1])];
495 ++adj.numLeadInclusions;
501 double ompl::control::Syclop::defaultEdgeCost(
int r,
int s)
503 const Adjacency &a = *regionsToEdge_[std::pair<int, int>(r, s)];
505 const int nsel = (a.empty ? a.numLeadInclusions : a.numSelections);
506 factor = (1.0 + (double)nsel * nsel) / (1.0 + (double)a.covGridCells.size() * a.covGridCells.size());
507 factor *= (a.source->alpha * a.target->alpha);