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