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.95;
46 const double ompl::control::Syclop::Defaults::PROB_SHORTEST_PATH = 0.95;
52 setLeadComputeFn(boost::bind(&ompl::control::Syclop::defaultComputeLead,
this, _1, _2, _3));
54 addEdgeCostFactor(boost::bind(&ompl::control::Syclop::defaultEdgeCost,
this, _1, _2));
63 startRegions_.clear();
73 setupRegionEstimates();
79 const int region = decomp_->locateRegion(s);
80 startRegions_.insert(region);
81 Motion* startMotion = addRoot(s);
82 graph_[boost::vertex(region,graph_)].motions.push_back(startMotion);
84 updateCoverageEstimate(graph_[boost::vertex(region,graph_)], s);
86 if (startRegions_.empty())
88 logError(
"There are no valid start states");
93 if (goalRegions_.empty())
96 goalRegions_.insert(decomp_->locateRegion(g));
99 logError(
"Unable to sample a valid goal state");
104 logInform(
"Starting with %u states", numMotions_);
106 std::vector<Motion*> newMotions;
107 const Motion* solution = NULL;
109 double goalDist = std::numeric_limits<double>::infinity();
111 while (!ptc() && !solved)
113 const int chosenStartRegion = startRegions_.sampleUniform();
114 int chosenGoalRegion = -1;
117 if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_/2)
121 chosenGoalRegion = decomp_->locateRegion(g);
122 goalRegions_.insert(chosenGoalRegion);
125 if (chosenGoalRegion == -1)
126 chosenGoalRegion = goalRegions_.sampleUniform();
128 leadComputeFn(chosenStartRegion, chosenGoalRegion, lead_);
129 computeAvailableRegions();
130 for (
int i = 0; i < numRegionExpansions_ && !solved && !ptc(); ++i)
132 const int region = selectRegion();
133 bool improved =
false;
134 for (
int j = 0; j < numTreeSelections_ && !solved && !ptc(); ++j)
137 selectAndExtend(graph_[boost::vertex(region,graph_)], newMotions);
138 for (std::vector<Motion*>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc(); ++m)
151 if (distance < goalDist)
156 const int newRegion = decomp_->locateRegion(motion->
state);
157 graph_[boost::vertex(newRegion,graph_)].motions.push_back(motion);
159 Region& newRegionObj = graph_[boost::vertex(newRegion, graph_)];
160 improved |= updateCoverageEstimate(newRegionObj, motion->
state);
161 if (newRegion != region)
164 if (newRegionObj.
motions.size() == 1)
165 availDist_.add(newRegion, newRegionObj.
weight);
168 if (regionsToEdge_.count(std::pair<int,int>(region, newRegion)) > 0)
170 Adjacency* adj = regionsToEdge_[std::pair<int,int>(region,newRegion)];
173 improved |= updateConnectionEstimate(graph_[boost::vertex(region,graph_)], newRegionObj, motion->
state);
178 if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
182 bool addedSolution =
false;
183 if (solution != NULL)
185 std::vector<const Motion*> mpath;
186 while (solution != NULL)
188 mpath.push_back(solution);
189 solution = solution->
parent;
192 for (
int i = mpath.size()-1; i >= 0; --i)
193 if (mpath[i]->parent)
194 path->
append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
196 path->
append(mpath[i]->state);
197 pdef_->addSolutionPath(
base::PathPtr(path), !solved, goalDist);
198 addedSolution =
true;
205 leadComputeFn = compute;
210 edgeCostFactors_.push_back(factor);
215 edgeCostFactors_.clear();
218 void ompl::control::Syclop::initRegion(Region& r)
222 r.percentValidCells = 1.0;
226 void ompl::control::Syclop::setupRegionEstimates(
void)
228 std::vector<int> numTotal(decomp_->getNumRegions(), 0);
229 std::vector<int> numValid(decomp_->getNumRegions(), 0);
234 for (
int i = 0; i < numFreeVolSamples_; ++i)
236 sampler->sampleUniform(s);
237 int rid = decomp_->locateRegion(s);
238 if (checker->isValid(s))
244 for (
int i = 0; i < decomp_->getNumRegions(); ++i)
246 Region& r = graph_[boost::vertex(i, graph_)];
247 r.volume = decomp_->getRegionVolume(i);
248 if (numTotal[i] == 0)
249 r.percentValidCells = 1.0;
251 r.percentValidCells = ((double) numValid[i]) / (double)numTotal[i];
252 r.freeVolume = r.percentValidCells * r.volume;
253 if (r.freeVolume < std::numeric_limits<double>::epsilon())
254 r.freeVolume = std::numeric_limits<double>::epsilon();
259 void ompl::control::Syclop::updateRegion(Region& r)
261 const double f = r.freeVolume*r.freeVolume*r.freeVolume*r.freeVolume;
262 r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f);
263 r.weight = f / ((1 + r.covGridCells.size())*(1 + r.numSelections*r.numSelections));
266 void ompl::control::Syclop::initEdge(Adjacency& adj,
const Region* source,
const Region* target)
271 regionsToEdge_[std::pair<int,int>(source->index, target->index)] = &adj;
274 void ompl::control::Syclop::setupEdgeEstimates(
void)
277 for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei)
279 Adjacency& adj = graph_[*ei];
281 adj.numLeadInclusions = 0;
282 adj.numSelections = 0;
287 void ompl::control::Syclop::updateEdge(Adjacency& a)
290 for (std::vector<EdgeCostFactorFn>::const_iterator i = edgeCostFactors_.begin(); i != edgeCostFactors_.end(); ++i)
292 const EdgeCostFactorFn& factor = *i;
293 a.cost *= factor(a.source->index, a.target->index);
297 bool ompl::control::Syclop::updateCoverageEstimate(Region& r,
const base::State *s)
299 const int covCell = covGrid_.locateRegion(s);
300 if (r.covGridCells.count(covCell) == 1)
302 r.covGridCells.insert(covCell);
307 bool ompl::control::Syclop::updateConnectionEstimate(
const Region& c,
const Region& d,
const base::State *s)
309 Adjacency& adj = *regionsToEdge_[std::pair<int,int>(c.index,d.index)];
310 const int covCell = covGrid_.locateRegion(s);
311 if (adj.covGridCells.count(covCell) == 1)
313 adj.covGridCells.insert(covCell);
318 void ompl::control::Syclop::buildGraph(
void)
320 VertexIndexMap index =
get(boost::vertex_index, graph_);
321 std::vector<int> neighbors;
322 for (
int i = 0; i < decomp_->getNumRegions(); ++i)
324 const RegionGraph::vertex_descriptor v = boost::add_vertex(graph_);
325 Region& r = graph_[boost::vertex(v,graph_)];
330 for (boost::tie(vi,vend) = boost::vertices(graph_); vi != vend; ++vi)
334 decomp_->getNeighbors(index[*vi], neighbors);
335 for (std::vector<int>::const_iterator j = neighbors.begin(); j != neighbors.end(); ++j)
337 RegionGraph::edge_descriptor edge;
339 boost::tie(edge, ignore) = boost::add_edge(*vi, boost::vertex(*j,graph_), graph_);
340 initEdge(graph_[edge], &graph_[*vi], &graph_[boost::vertex(*j,graph_)]);
346 void ompl::control::Syclop::clearGraphDetails(
void)
349 for (boost::tie(vi,vend) = boost::vertices(graph_); vi != vend; ++vi)
352 for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei)
357 int ompl::control::Syclop::selectRegion(
void)
359 const int index = availDist_.sample(rng_.uniform01());
360 Region& region = graph_[boost::vertex(index,graph_)];
361 ++region.numSelections;
362 updateRegion(region);
366 void ompl::control::Syclop::computeAvailableRegions(
void)
369 for (
int i = lead_.size()-1; i >= 0; --i)
371 Region& r = graph_[boost::vertex(lead_[i],graph_)];
372 if (!r.motions.empty())
374 availDist_.add(lead_[i], r.weight);
375 if (rng_.uniform01() >= probKeepAddingToAvail_)
381 void ompl::control::Syclop::defaultComputeLead(
int startRegion,
int goalRegion, std::vector<int>& lead)
384 if (startRegion == goalRegion)
386 lead.push_back(startRegion);
390 else if (rng_.uniform01() < probShortestPath_)
392 std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
393 std::vector<double> distances(decomp_->getNumRegions());
397 boost::astar_search(graph_, boost::vertex(startRegion, graph_), DecompositionHeuristic(
this, getRegionFromIndex(goalRegion)),
398 boost::weight_map(
get(&Adjacency::cost, graph_)).distance_map(
399 boost::make_iterator_property_map(distances.begin(),
get(boost::vertex_index, graph_)
401 boost::make_iterator_property_map(parents.begin(),
get(boost::vertex_index, graph_))
402 ).visitor(GoalVisitor(goalRegion))
405 catch (found_goal fg)
407 int region = goalRegion;
410 while (region != startRegion)
412 region = parents[region];
415 lead.resize(leadLength);
417 for (
int i = leadLength-1; i >= 0; --i)
420 region = parents[region];
428 VertexIndexMap index =
get(boost::vertex_index, graph_);
429 std::stack<int> nodesToProcess;
430 std::vector<int> parents(decomp_->getNumRegions(), -1);
431 parents[startRegion] = startRegion;
432 nodesToProcess.push(startRegion);
433 bool goalFound =
false;
434 while (!goalFound && !nodesToProcess.empty())
436 const int v = nodesToProcess.top();
437 nodesToProcess.pop();
438 std::vector<int> neighbors;
439 boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend;
440 for (boost::tie(ai,aend) = adjacent_vertices(boost::vertex(v,graph_),graph_); ai != aend; ++ai)
442 if (parents[index[*ai]] < 0)
444 neighbors.push_back(index[*ai]);
445 parents[index[*ai]] = v;
448 for (std::size_t i = 0; i < neighbors.size(); ++i)
450 const int choice = rng_.uniformInt(i, neighbors.size()-1);
451 if (neighbors[choice] == goalRegion)
453 int region = goalRegion;
455 while (region != startRegion)
457 region = parents[region];
460 lead.resize(leadLength);
462 for (
int j = leadLength-1; j >= 0; --j)
465 region = parents[region];
470 nodesToProcess.push(neighbors[choice]);
471 std::swap(neighbors[i], neighbors[choice]);
477 for (std::size_t i = 0; i < lead.size()-1; ++i)
479 Adjacency& adj = *regionsToEdge_[std::pair<int,int>(lead[i], lead[i+1])];
482 ++adj.numLeadInclusions;
488 double ompl::control::Syclop::defaultEdgeCost(
int r,
int s)
490 const Adjacency& a = *regionsToEdge_[std::pair<int,int>(r,s)];
492 const int nsel = (a.empty ? a.numLeadInclusions : a.numSelections);
493 factor = (double)(1 + nsel*nsel) / (double)(1 + a.covGridCells.size()*a.covGridCells.size());
494 factor *= (a.source->alpha * a.target->alpha);