Syclop.cpp
68 ompl::base::PlannerStatus ompl::control::Syclop::solve(const base::PlannerTerminationCondition &ptc)
105 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), numMotions_);
139 for (std::vector<Motion*>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc; ++m)
171 improved |= updateConnectionEstimate(graph_[boost::vertex(region, graph_)], newRegionObj, motion->state);
202 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
278 void ompl::control::Syclop::initEdge(Adjacency &adj, const Region *source, const Region *target)
302 for (std::vector<EdgeCostFactorFn>::const_iterator i = edgeCostFactors_.begin(); i != edgeCostFactors_.end(); ++i)
319 bool ompl::control::Syclop::updateConnectionEstimate(const Region &c, const Region &d, const base::State *s)
395 void ompl::control::Syclop::defaultComputeLead(int startRegion, int goalRegion, std::vector<int>& lead)
411 boost::astar_search(graph_, boost::vertex(startRegion, graph_), DecompositionHeuristic(this, getRegionFromIndex(goalRegion)),
454 for (boost::tie(ai,aend) = adjacent_vertices(boost::vertex(v, graph_), graph_); ai != aend; ++ai)
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Syclop.cpp:48
double alpha
The coefficient contributed by this region to edge weights in lead computations.
Definition: Syclop.h:315
Representation of an adjacency (a directed edge) between two regions in the Decomposition assigned to...
Definition: Syclop.h:328
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
Definition: SpaceInformation.h:207
std::set< int > covGridCells
The cells of the underlying coverage grid that contain tree motions from this region.
Definition: Syclop.h:303
int numLeadInclusions
The number of times this adjacency has been included in a lead.
Definition: Syclop.h:352
The planner failed to find a solution.
Definition: PlannerStatus.h:62
int numSelections
The number of times the low-level tree planner has selected motions from the source region when attem...
Definition: Syclop.h:355
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...
Definition: PathControl.cpp:266
PDF< int >::Element * pdfElem
The Element corresponding to this region in the PDF of available regions.
Definition: Syclop.h:321
A boost shared pointer wrapper for ompl::base::StateSampler.
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or NULL if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:271
Representation of a region in the Decomposition assigned to Syclop.
Definition: Syclop.h:277
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
const SpaceInformation * siC_
Handle to the control::SpaceInformation object.
Definition: Syclop.h:393
DecompositionPtr decomp_
The high level decomposition used to focus tree expansion.
Definition: Syclop.h:396
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:112
int numTreeSelections_
The number of calls to selectAndExtend() in the low-level tree planner for a given lead and region...
Definition: Syclop.h:387
int numRegionExpansions_
The number of times a new region will be chosen and promoted for expansion from a given lead...
Definition: Syclop.h:384
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...
Definition: Syclop.h:91
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...
Definition: Syclop.cpp:57
void clearEdgeCostFactors()
Clears all edge cost factors, making all edge weights equivalent to 1.
Definition: Syclop.cpp:221
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Continues solving until a solution is found or a given planner termination condition is met...
Definition: Syclop.cpp:68
void addEdgeCostFactor(const EdgeCostFactorFn &factor)
Adds an edge cost factor to be used for edge weights between adjacent regions.
Definition: Syclop.cpp:216
bool empty
This value is true if and only if this adjacency's source and target regions both contain zero tree m...
Definition: Syclop.h:357
The planner found an exact solution.
Definition: PlannerStatus.h:66
std::set< int > covGridCells
The cells of the underlying coverage grid that contain tree motions originating from direct connectio...
Definition: Syclop.h:344
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:86
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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: Syclop.h:88
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:100
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
const Region & getRegionFromIndex(const int rid) const
Returns a reference to the Region object with the given index. Assumes the index is valid...
Definition: Syclop.h:369
const State * nextStart()
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:230
double probShortestPath_
The probability that a lead will be computed as a shortest-path instead of a random-DFS.
Definition: Syclop.h:378
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
Definition: PDF.h:155
_T & sample(double r) const
Returns a piece of data from the PDF according to the input sampling value, which must be between 0 a...
Definition: PDF.h:132
void setLeadComputeFn(const LeadComputeFn &compute)
Allows the user to override the lead computation function.
Definition: Syclop.cpp:211
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.
Definition: PDF.h:97
unsigned int numSelections
The number of times this region has been selected for expansion.
Definition: Syclop.h:319
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:352
double probKeepAddingToAvail_
The probability that the set of available regions will be augmented.
Definition: Syclop.h:381
int numFreeVolSamples_
The number of states to sample to estimate free volume in the Decomposition.
Definition: Syclop.h:375
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:75
double probAbandonLeadEarly_
The probability that a lead will be abandoned early, before a new region is chosen for expansion...
Definition: Syclop.h:390
virtual void selectAndExtend(Region ®ion, std::vector< Motion *> &newMotions)=0
Select a Motion from the given Region, and extend the tree from the Motion. Add any new motions creat...
double weight
The probabilistic weight of this region, used when sampling from PDF.
Definition: Syclop.h:313
A boost shared pointer wrapper for ompl::base::Path.
virtual Motion * addRoot(const base::State *s)=0
Add State s as a new root in the low-level tree, and return the Motion corresponding to s...