37 #ifndef OMPL_CONTROL_PLANNERS_SYCLOP_SYCLOP_
38 #define OMPL_CONTROL_PLANNERS_SYCLOP_SYCLOP_
40 #include <boost/graph/astar_search.hpp>
41 #include <boost/graph/graph_traits.hpp>
42 #include <boost/graph/adjacency_list.hpp>
43 #include <boost/unordered_map.hpp>
44 #include "ompl/control/planners/PlannerIncludes.h"
45 #include "ompl/control/planners/syclop/Decomposition.h"
46 #include "ompl/control/planners/syclop/GridDecomposition.h"
47 #include "ompl/datastructures/PDF.h"
91 typedef boost::function<void(int, int, std::vector<int>&)>
LeadComputeFn;
124 virtual void setup(
void);
126 virtual void clear(
void);
232 static const int NUM_FREEVOL_SAMPLES = 100000;
233 static const int COVGRID_LENGTH = 128;
234 static const int NUM_REGION_EXPANSIONS = 100;
235 static const int NUM_TREE_SELECTIONS = 1;
239 static const double PROB_ABANDON_LEAD_EARLY ;
240 static const double PROB_KEEP_ADDING_TO_AVAIL ;
241 static const double PROB_SHORTEST_PATH ;
246 #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
273 #pragma pack (pop) // Restoring default byte alignment
275 #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
314 #pragma pack (pop) // Restoring default byte alignment
316 #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
350 #pragma pack (pop) // Restoring default byte alignment
362 return graph_[boost::vertex(rid,graph_)];
402 virtual ~CoverageGrid()
408 virtual void project(
const base::State* s, std::vector<double>& coord)
const
422 typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, Region, Adjacency> RegionGraph;
423 typedef boost::graph_traits<RegionGraph>::vertex_descriptor Vertex;
424 typedef boost::graph_traits<RegionGraph>::vertex_iterator VertexIter;
425 typedef boost::property_map<RegionGraph, boost::vertex_index_t>::type VertexIndexMap;
426 typedef boost::graph_traits<RegionGraph>::edge_iterator EdgeIter;
429 friend class DecompositionHeuristic;
431 class DecompositionHeuristic :
public boost::astar_heuristic<RegionGraph, double>
434 DecompositionHeuristic(
const Syclop* s,
const Region& goal) : syclop(s), goalRegion(goal)
438 double operator()(Vertex v)
440 const Region& region = syclop->getRegionFromIndex(v);
441 return region.alpha*goalRegion.alpha;
445 const Region& goalRegion;
448 struct found_goal {};
450 class GoalVisitor :
public boost::default_astar_visitor
453 GoalVisitor(
const unsigned int goal) : goalRegion(goal)
456 void examine_vertex(Vertex v,
const RegionGraph& )
462 const unsigned int goalRegion;
470 int sampleUniform(
void)
474 return regions.sample(rng.uniform01());
476 void insert(
const int r)
478 if (regToElem.count(r) == 0)
479 regToElem[r] = regions.add(r, 1);
482 PDF<int>::Element* elem = regToElem[r];
483 regions.update(elem, regions.getWeight(elem)+1);
491 std::size_t size(
void)
const
493 return regions.size();
497 return regions.empty();
502 boost::unordered_map<const int, PDF<int>::Element*> regToElem;
507 void initRegion(Region& r);
510 void setupRegionEstimates(
void);
513 void updateRegion(Region& r);
516 void initEdge(Adjacency& a,
const Region* source,
const Region* target);
519 void setupEdgeEstimates(
void);
522 void updateEdge(Adjacency& a);
526 bool updateCoverageEstimate(Region& r,
const base::State* s);
530 bool updateConnectionEstimate(
const Region& c,
const Region& d,
const base::State* s);
534 void buildGraph(
void);
537 void clearGraphDetails(
void);
540 int selectRegion(
void);
543 void computeAvailableRegions(
void);
546 void defaultComputeLead(
int startRegion,
int goalRegion, std::vector<int>& lead);
549 double defaultEdgeCost(
int r,
int s);
554 std::vector<int> lead_;
558 std::vector<EdgeCostFactorFn> edgeCostFactors_;
560 CoverageGrid covGrid_;
566 boost::unordered_map<std::pair<int,int>, Adjacency*> regionsToEdge_;
568 unsigned int numMotions_;
570 RegionSet startRegions_;
572 RegionSet goalRegions_;
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Planner(const SpaceInformationPtr &si, const std::string &name)
Constructor.
Synergistic Combination of Layers of Planning.
double alpha
The coefficient contributed by this region to edge weights in lead computations.
base::State * state
The state contained by the motion.
Representation of an adjacency (a directed edge) between two regions in the Decomposition assigned to...
std::set< int > covGridCells
The cells of the underlying coverage grid that contain tree motions from this region.
int numLeadInclusions
The number of times this adjacency has been included in a lead.
int numSelections
The number of times the low-level tree planner has selected motions from the source region when attem...
Definition of an abstract control.
int index
The index of the graph node corresponding to this region.
RNG rng_
Random number generator.
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.
double volume
The volume of this region.
Representation of a region in the Decomposition assigned to Syclop.
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
std::vector< Motion * > motions
The tree motions contained in this region.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const SpaceInformation * siC_
Handle to the control::SpaceInformation object.
DecompositionPtr decomp_
The high level decomposition used to focus tree expansion.
A GridDecomposition is a Decomposition implemented using a grid.
Representation of a motion.
void clearEdgeCostFactors(void)
Clears all edge cost factors, making all edge weights equivalent to 1.
void clear(void)
Clears motions and coverage information from this region.
int numTreeSelections_
The number of calls to selectAndExtend() in the low-level tree planner for a given lead and region...
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Control * control
The control contained by the motion.
int numRegionExpansions_
The number of times a new region will be chosen and promoted for expansion from a given lead...
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 container that supports probabilistic sampling over weighted data.
int getNumRegionExpansions(void) const
Get the number of times a new region will be chosen and promoted for expansion from a given lead...
const Region & getRegionFromIndex(const int rid) const
Returns a reference to the Region object with the given index. Assumes the index is valid...
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Continues solving until a solution is found or a given planner termination condition is met...
Syclop(const SpaceInformationPtr &si, const DecompositionPtr &d, const std::string &plannerName)
Constructor. Requires a Decomposition, which Syclop uses to create high-level leads.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
double getProbShortestPathLead(void) const
Get the probability [0,1] that a lead will be computed as a shortest-path instead of a random-DFS...
Base class for a planner.
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...
std::set< int > covGridCells
The cells of the underlying coverage grid that contain tree motions originating from direct connectio...
const Region * source
The source region of this adjacency edge.
void setProbAbandonLeadEarly(double probability)
The probability that a lead will be abandoned early, before a new region is chosen for expansion...
double getProbAddingToAvailableRegions(void) const
Get the probability [0,1] that the set of available regions will be augmented.
double cost
The cost of this adjacency edge, used in lead computations.
A class to store the exit status of Planner::solve()
const Region * target
The target region of this adjacency edge.
double getProbAbandonLeadEarly(void) const
Get the probability [0,1] that a lead will be abandoned early, before a new region is chosen for expa...
void clear(void)
Clears coverage information from this adjacency.
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.
double percentValidCells
The percent of free volume of this region.
virtual void project(const base::State *s, std::vector< double > &coord) const =0
Project a given State to a set of coordinates in R^k, where k is the dimension of this Decomposition...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
double probShortestPath_
The probability that a lead will be computed as a shortest-path instead of a random-DFS.
Contains default values for Syclop parameters.
void setLeadComputeFn(const LeadComputeFn &compute)
Allows the user to override the lead computation function.
unsigned int numSelections
The number of times this region has been selected for expansion.
void setNumFreeVolumeSamples(int numSamples)
Set the number of states to sample when estimating free volume in the Decomposition.
void setProbShortestPathLead(double probability)
Set the probability [0,1] that a lead will be computed as a shortest-path instead of a random-DFS...
int getNumFreeVolumeSamples(void) const
Get the number of states to sample when estimating free volume in the Decomposition.
double probKeepAddingToAvail_
The probability that the set of available regions will be augmented.
int getNumTreeExpansions(void) const
Get the number of calls to selectAndExtend() in the low-level tree planner for a given lead and regio...
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...
A boost shared pointer wrapper for ompl::control::Decomposition.
const Motion * parent
The parent motion in the tree.
double freeVolume
The free volume of this region.
int numFreeVolSamples_
The number of states to sample to estimate free volume in the Decomposition.
void setNumTreeExpansions(int treeExpansions)
Set the number of calls to selectAndExtend() in the low-level tree planner for a given lead and regio...
double probAbandonLeadEarly_
The probability that a lead will be abandoned early, before a new region is chosen for expansion...
void setNumRegionExpansions(int regionExpansions)
Set the number of times a new region will be chosen and promoted for expansion from a given lead...
double weight
The probabilistic weight of this region, used when sampling from PDF.
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...
void setProbAddingToAvailableRegions(double probability)
Set the probability [0,1] that the set of available regions will be augmented.
unsigned int steps
The number of steps for which the control is applied.