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(boost::bind(&ompl::control::Syclop::defaultComputeLead,
this, _1, _2, _3));
54 addEdgeCostFactor(boost::bind(&ompl::control::Syclop::defaultEdgeCost,
this, _1, _2));
62 clearEdgeCostFactors();
64 startRegions_.clear();
74 setupRegionEstimates();
80 const int region = decomp_->locateRegion(s);
81 startRegions_.insert(region);
82 Motion *startMotion = addRoot(s);
83 graph_[boost::vertex(region, graph_)].motions.push_back(startMotion);
85 updateCoverageEstimate(graph_[boost::vertex(region, graph_)], s);
87 if (startRegions_.empty())
89 OMPL_ERROR(
"%s: There are no valid start states", getName().c_str());
94 if (goalRegions_.empty())
97 goalRegions_.insert(decomp_->locateRegion(g));
100 OMPL_ERROR(
"%s: Unable to sample a valid goal state", getName().c_str());
105 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure", getName().c_str(), numMotions_);
107 std::vector<Motion*> newMotions;
108 const Motion *solution = NULL;
110 double goalDist = std::numeric_limits<double>::infinity();
112 while (!ptc && !solved)
114 const int chosenStartRegion = startRegions_.sampleUniform();
115 int chosenGoalRegion = -1;
118 if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_/2)
122 chosenGoalRegion = decomp_->locateRegion(g);
123 goalRegions_.insert(chosenGoalRegion);
126 if (chosenGoalRegion == -1)
127 chosenGoalRegion = goalRegions_.sampleUniform();
129 leadComputeFn(chosenStartRegion, chosenGoalRegion, lead_);
130 computeAvailableRegions();
131 for (
int i = 0; i < numRegionExpansions_ && !solved && !ptc; ++i)
133 const int region = selectRegion();
134 bool improved =
false;
135 for (
int j = 0; j < numTreeSelections_ && !solved && !ptc; ++j)
138 selectAndExtend(graph_[boost::vertex(region, graph_)], newMotions);
139 for (std::vector<Motion*>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc; ++m)
152 if (distance < goalDist)
157 const int newRegion = decomp_->locateRegion(motion->
state);
158 graph_[boost::vertex(newRegion, graph_)].motions.push_back(motion);
160 Region &newRegionObj = graph_[boost::vertex(newRegion, graph_)];
161 improved |= updateCoverageEstimate(newRegionObj, motion->
state);
165 if (newRegion != region
166 && 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, motion->
state);
175 if (newRegionObj.
pdfElem != NULL)
176 availDist_.update(newRegionObj.
pdfElem, newRegionObj.
weight);
179 else if (std::find(lead_.begin(), lead_.end(), newRegion) != lead_.end())
186 if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
190 bool addedSolution =
false;
191 if (solution != NULL)
193 std::vector<const Motion*> mpath;
194 while (solution != NULL)
196 mpath.push_back(solution);
197 solution = solution->
parent;
200 for (
int i = mpath.size()-1; i >= 0; --i)
201 if (mpath[i]->parent)
202 path->
append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
204 path->
append(mpath[i]->state);
205 pdef_->addSolutionPath(
base::PathPtr(path), !solved, goalDist, getName());
206 addedSolution =
true;
213 leadComputeFn = compute;
218 edgeCostFactors_.push_back(factor);
223 edgeCostFactors_.clear();
226 void ompl::control::Syclop::initRegion(Region &r)
230 r.percentValidCells = 1.0;
235 void ompl::control::Syclop::setupRegionEstimates()
237 std::vector<int> numTotal(decomp_->getNumRegions(), 0);
238 std::vector<int> numValid(decomp_->getNumRegions(), 0);
243 for (
int i = 0; i < numFreeVolSamples_; ++i)
245 sampler->sampleUniform(s);
246 int rid = decomp_->locateRegion(s);
249 if (checker->isValid(s))
256 for (
int i = 0; i < decomp_->getNumRegions(); ++i)
258 Region &r = graph_[boost::vertex(i, graph_)];
259 r.volume = decomp_->getRegionVolume(i);
260 if (numTotal[i] == 0)
261 r.percentValidCells = 1.0;
263 r.percentValidCells = ((double) numValid[i]) / (double)numTotal[i];
264 r.freeVolume = r.percentValidCells * r.volume;
265 if (r.freeVolume < std::numeric_limits<double>::epsilon())
266 r.freeVolume = std::numeric_limits<double>::epsilon();
271 void ompl::control::Syclop::updateRegion(Region &r)
273 const double f = r.freeVolume*r.freeVolume*r.freeVolume*r.freeVolume;
274 r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f);
275 r.weight = f / ((1 + r.covGridCells.size())*(1 + r.numSelections*r.numSelections));
278 void ompl::control::Syclop::initEdge(Adjacency &adj,
const Region *source,
const Region *target)
283 regionsToEdge_[std::pair<int,int>(source->index, target->index)] = &adj;
286 void ompl::control::Syclop::setupEdgeEstimates()
289 for (boost::tie(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 (std::vector<EdgeCostFactorFn>::const_iterator i = edgeCostFactors_.begin(); i != edgeCostFactors_.end(); ++i)
304 const EdgeCostFactorFn& factor = *i;
305 a.cost *= factor(a.source->index, a.target->index);
309 bool ompl::control::Syclop::updateCoverageEstimate(Region &r,
const base::State *s)
311 const int covCell = covGrid_.locateRegion(s);
312 if (r.covGridCells.count(covCell) == 1)
314 r.covGridCells.insert(covCell);
319 bool ompl::control::Syclop::updateConnectionEstimate(
const Region &c,
const Region &d,
const base::State *s)
321 Adjacency &adj = *regionsToEdge_[std::pair<int,int>(c.index,d.index)];
322 const int covCell = covGrid_.locateRegion(s);
323 if (adj.covGridCells.count(covCell) == 1)
325 adj.covGridCells.insert(covCell);
330 void ompl::control::Syclop::buildGraph()
332 VertexIndexMap index =
get(boost::vertex_index, graph_);
333 std::vector<int> neighbors;
334 for (
int i = 0; i < decomp_->getNumRegions(); ++i)
336 const RegionGraph::vertex_descriptor v = boost::add_vertex(graph_);
337 Region &r = graph_[boost::vertex(v, graph_)];
342 for (boost::tie(vi,vend) = boost::vertices(graph_); vi != vend; ++vi)
346 decomp_->getNeighbors(index[*vi], neighbors);
347 for (std::vector<int>::const_iterator j = neighbors.begin(); j != neighbors.end(); ++j)
349 RegionGraph::edge_descriptor edge;
351 boost::tie(edge, ignore) = boost::add_edge(*vi, boost::vertex(*j, graph_), graph_);
352 initEdge(graph_[edge], &graph_[*vi], &graph_[boost::vertex(*j, graph_)]);
358 void ompl::control::Syclop::clearGraphDetails()
361 for (boost::tie(vi,vend) = boost::vertices(graph_); vi != vend; ++vi)
364 for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei)
369 int ompl::control::Syclop::selectRegion()
371 const int index = availDist_.sample(rng_.uniform01());
372 Region ®ion = graph_[boost::vertex(index, graph_)];
373 ++region.numSelections;
374 updateRegion(region);
378 void ompl::control::Syclop::computeAvailableRegions()
380 for (
unsigned int i = 0; i < availDist_.size(); ++i)
381 graph_[boost::vertex(availDist_[i],graph_)].pdfElem = NULL;
383 for (
int i = lead_.size()-1; i >= 0; --i)
385 Region &r = graph_[boost::vertex(lead_[i], graph_)];
386 if (!r.motions.empty())
388 r.pdfElem = availDist_.add(lead_[i], r.weight);
389 if (rng_.uniform01() >= probKeepAddingToAvail_)
395 void ompl::control::Syclop::defaultComputeLead(
int startRegion,
int goalRegion, std::vector<int>& lead)
398 if (startRegion == goalRegion)
400 lead.push_back(startRegion);
404 else if (rng_.uniform01() < probShortestPath_)
406 std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
407 std::vector<double> distances(decomp_->getNumRegions());
411 boost::astar_search(graph_, boost::vertex(startRegion, graph_), DecompositionHeuristic(
this, getRegionFromIndex(goalRegion)),
412 boost::weight_map(
get(&Adjacency::cost, graph_)).distance_map(
413 boost::make_iterator_property_map(distances.begin(),
get(boost::vertex_index, graph_)
415 boost::make_iterator_property_map(parents.begin(),
get(boost::vertex_index, graph_))
416 ).visitor(GoalVisitor(goalRegion))
419 catch (found_goal fg)
421 int region = goalRegion;
424 while (region != startRegion)
426 region = parents[region];
429 lead.resize(leadLength);
431 for (
int i = leadLength-1; i >= 0; --i)
434 region = parents[region];
442 VertexIndexMap index =
get(boost::vertex_index, graph_);
443 std::stack<int> nodesToProcess;
444 std::vector<int> parents(decomp_->getNumRegions(), -1);
445 parents[startRegion] = startRegion;
446 nodesToProcess.push(startRegion);
447 bool goalFound =
false;
448 while (!goalFound && !nodesToProcess.empty())
450 const int v = nodesToProcess.top();
451 nodesToProcess.pop();
452 std::vector<int> neighbors;
453 boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend;
454 for (boost::tie(ai,aend) = adjacent_vertices(boost::vertex(v, graph_), graph_); ai != aend; ++ai)
456 if (parents[index[*ai]] < 0)
458 neighbors.push_back(index[*ai]);
459 parents[index[*ai]] = v;
462 for (std::size_t i = 0; i < neighbors.size(); ++i)
464 const int choice = rng_.uniformInt(i, neighbors.size()-1);
465 if (neighbors[choice] == goalRegion)
467 int region = goalRegion;
469 while (region != startRegion)
471 region = parents[region];
474 lead.resize(leadLength);
476 for (
int j = leadLength-1; j >= 0; --j)
479 region = parents[region];
484 nodesToProcess.push(neighbors[choice]);
485 std::swap(neighbors[i], neighbors[choice]);
491 for (std::size_t i = 0; i < lead.size()-1; ++i)
493 Adjacency &adj = *regionsToEdge_[std::pair<int,int>(lead[i], lead[i+1])];
496 ++adj.numLeadInclusions;
502 double ompl::control::Syclop::defaultEdgeCost(
int r,
int s)
504 const Adjacency &a = *regionsToEdge_[std::pair<int,int>(r,s)];
506 const int nsel = (a.empty ? a.numLeadInclusions : a.numSelections);
507 factor = (double)(1 + nsel*nsel) / (double)(1 + a.covGridCells.size()*a.covGridCells.size());
508 factor *= (a.source->alpha * a.target->alpha);
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
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...
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
PDF< int >::Element * pdfElem
The Element corresponding to this region in the PDF of available regions.
A boost 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.
Definition of a control path.
boost::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 boost shared pointer wrapper for ompl::base::StateValidityChecker.
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
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.
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Continues solving until a solution is found or a given planner termination condition is met...
#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.
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
A class to store the exit status of Planner::solve()
boost::function< double(int, int)> EdgeCostFactorFn
Each edge weight between two adjacent regions in the Decomposition is defined as a product of edge co...
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.
const Motion * parent
The parent motion in the tree.
double weight
The probabilistic weight of this region, used when sampling from PDF.
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.