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));
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 OMPL_ERROR(
"%s: There are no valid start states", getName().c_str());
93 if (goalRegions_.empty())
96 goalRegions_.insert(decomp_->locateRegion(g));
99 OMPL_ERROR(
"%s: Unable to sample a valid goal state", getName().c_str());
104 OMPL_INFORM(
"%s: Starting with %u states", getName().c_str(), 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);
164 if (newRegion != region
165 && regionsToEdge_.count(std::pair<int,int>(region,newRegion)) > 0)
167 Adjacency* adj = regionsToEdge_[std::pair<int,int>(region,newRegion)];
170 improved |= updateConnectionEstimate(graph_[boost::vertex(region, graph_)], newRegionObj, motion->
state);
174 if (newRegionObj.
pdfElem != NULL)
175 availDist_.update(newRegionObj.
pdfElem, newRegionObj.
weight);
178 else if (std::find(lead_.begin(), lead_.end(), newRegion) != lead_.end())
185 if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
189 bool addedSolution =
false;
190 if (solution != NULL)
192 std::vector<const Motion*> mpath;
193 while (solution != NULL)
195 mpath.push_back(solution);
196 solution = solution->
parent;
199 for (
int i = mpath.size()-1; i >= 0; --i)
200 if (mpath[i]->parent)
201 path->
append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
203 path->
append(mpath[i]->state);
204 pdef_->addSolutionPath(
base::PathPtr(path), !solved, goalDist);
205 addedSolution =
true;
212 leadComputeFn = compute;
217 edgeCostFactors_.push_back(factor);
222 edgeCostFactors_.clear();
225 void ompl::control::Syclop::initRegion(Region& r)
229 r.percentValidCells = 1.0;
234 void ompl::control::Syclop::setupRegionEstimates(
void)
236 std::vector<int> numTotal(decomp_->getNumRegions(), 0);
237 std::vector<int> numValid(decomp_->getNumRegions(), 0);
242 for (
int i = 0; i < numFreeVolSamples_; ++i)
244 sampler->sampleUniform(s);
245 int rid = decomp_->locateRegion(s);
248 if (checker->isValid(s))
255 for (
unsigned int i = 0; i < decomp_->getNumRegions(); ++i)
257 Region& r = graph_[boost::vertex(i, graph_)];
258 r.volume = decomp_->getRegionVolume(i);
259 if (numTotal[i] == 0)
260 r.percentValidCells = 1.0;
262 r.percentValidCells = ((double) numValid[i]) / (double)numTotal[i];
263 r.freeVolume = r.percentValidCells * r.volume;
264 if (r.freeVolume < std::numeric_limits<double>::epsilon())
265 r.freeVolume = std::numeric_limits<double>::epsilon();
270 void ompl::control::Syclop::updateRegion(Region& r)
272 const double f = r.freeVolume*r.freeVolume*r.freeVolume*r.freeVolume;
273 r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f);
274 r.weight = f / ((1 + r.covGridCells.size())*(1 + r.numSelections*r.numSelections));
277 void ompl::control::Syclop::initEdge(Adjacency& adj,
const Region* source,
const Region* target)
282 regionsToEdge_[std::pair<int,int>(source->index, target->index)] = &adj;
285 void ompl::control::Syclop::setupEdgeEstimates(
void)
288 for (boost::tie(ei,eend) = boost::edges(graph_) ; ei != eend; ++ei)
290 Adjacency& adj = graph_[*ei];
292 adj.numLeadInclusions = 0;
293 adj.numSelections = 0;
298 void ompl::control::Syclop::updateEdge(Adjacency& a)
301 for (std::vector<EdgeCostFactorFn>::const_iterator i = edgeCostFactors_.begin(); i != edgeCostFactors_.end(); ++i)
303 const EdgeCostFactorFn& factor = *i;
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(
void)
331 VertexIndexMap index =
get(boost::vertex_index, graph_);
332 std::vector<unsigned int> neighbors;
333 for (
unsigned 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 (std::vector<unsigned int>::const_iterator j = neighbors.begin(); j != neighbors.end(); ++j)
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(
void)
360 for (boost::tie(vi,vend) = boost::vertices(graph_); vi != vend; ++vi)
363 for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei)
368 int ompl::control::Syclop::selectRegion(
void)
370 const int index = availDist_.sample(rng_.uniform01());
371 Region& region = graph_[boost::vertex(index, graph_)];
372 ++region.numSelections;
373 updateRegion(region);
377 void ompl::control::Syclop::computeAvailableRegions(
void)
379 for (
unsigned int i = 0; i < availDist_.size(); ++i)
380 graph_[boost::vertex(availDist_[i],graph_)].pdfElem = NULL;
382 for (
int i = lead_.size()-1; i >= 0; --i)
384 Region& r = graph_[boost::vertex(lead_[i], graph_)];
385 if (!r.motions.empty())
387 r.pdfElem = availDist_.add(lead_[i], r.weight);
388 if (rng_.uniform01() >= probKeepAddingToAvail_)
394 void ompl::control::Syclop::defaultComputeLead(
int startRegion,
int goalRegion, std::vector<int>& lead)
397 if (startRegion == goalRegion)
399 lead.push_back(startRegion);
403 else if (rng_.uniform01() < probShortestPath_)
405 std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
406 std::vector<double> distances(decomp_->getNumRegions());
410 boost::astar_search(graph_, boost::vertex(startRegion, graph_), DecompositionHeuristic(
this, getRegionFromIndex(goalRegion)),
411 boost::weight_map(
get(&Adjacency::cost, graph_)).distance_map(
412 boost::make_iterator_property_map(distances.begin(),
get(boost::vertex_index, graph_)
414 boost::make_iterator_property_map(parents.begin(),
get(boost::vertex_index, graph_))
415 ).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 = (double)(1 + nsel*nsel) / (double)(1 + a.covGridCells.size()*a.covGridCells.size());
507 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...
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
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...
Representation of a motion.
Definition of a control path.
void clearEdgeCostFactors(void)
Clears all edge cost factors, making all edge weights equivalent to 1.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
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.
A container that supports probabilistic sampling over weighted data.
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.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
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.
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.
virtual void clear(void)
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.
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.