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);
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);
base::State * state
The state contained by the motion.
Representation of an adjacency (a directed edge) between two regions in the Decomposition assigned to...
The planner failed to find a solution.
int numSelections
The number of times the low-level tree planner has selected motions from the source region when attem...
PDF< int >::Element * pdfElem
The Element corresponding to this region in the PDF of available regions.
A shared pointer wrapper for ompl::base::StateSampler.
Representation of a region in the Decomposition assigned to Syclop.
Abstract definition of goals.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Representation of a motion.
std::function< void(int, int, std::vector< int > &)> LeadComputeFn
Leads should consist of a path of adjacent regions in the decomposition that start with the start reg...
A shared pointer wrapper for ompl::base::StateValidityChecker.
A container that supports probabilistic sampling over weighted data.
void clearEdgeCostFactors()
Clears all edge cost factors, making all edge weights equivalent to 1.
Invalid start state or no start state specified.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
void addEdgeCostFactor(const EdgeCostFactorFn &factor)
Adds an edge cost factor to be used for edge weights between adjacent regions.
bool empty
This value is true if and only if this adjacency's source and target regions both contain zero tree m...
The planner found an exact solution.
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continues solving until a solution is found or a given planner termination condition is met...
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
std::function< double(int, int)> EdgeCostFactorFn
Each edge weight between two adjacent regions in the Decomposition is defined as a product of edge co...
A class to store the exit status of Planner::solve()
Definition of an abstract state.
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
void setLeadComputeFn(const LeadComputeFn &compute)
Allows the user to override the lead computation function.
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element, which can be used to subsequently update or remove the data from the PDF.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
const Motion * parent
The parent motion in the tree.
double weight
The probabilistic weight of this region, used when sampling from PDF.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.