Classes |
Public Types |
Public Member Functions |
Protected Member Functions |
Protected Attributes
ompl::geometric::SPARS Class Reference
SPArse Roadmap Spanner technique. More...
#include <ompl/geometric/planners/prm/SPARS.h>
Inheritance diagram for ompl::geometric::SPARS:

Classes | |
struct | InterfaceHashStruct |
struct | vertex_color_t |
struct | vertex_interface_list_t |
struct | vertex_list_t |
struct | vertex_representative_t |
struct | vertex_state_t |
Public Types | |
enum | GuardType { START, GOAL, COVERAGE, CONNECTIVITY, INTERFACE, QUALITY } |
Enumeration which specifies the reason a guard is added to the spanner. | |
typedef unsigned long int | VertexIndexType |
The type used internally for representing vertex IDs. | |
typedef boost::unordered_map < VertexIndexType, std::set < VertexIndexType > , boost::hash< VertexIndexType > > | InterfaceHash |
Hash for storing interface information. | |
typedef std::deque< base::State * > | DensePath |
Internal representation of a dense path. | |
typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::undirectedS, boost::property < vertex_state_t, base::State *, boost::property < boost::vertex_predecessor_t, VertexIndexType, boost::property < boost::vertex_rank_t, VertexIndexType, boost::property < vertex_color_t, GuardType, boost::property< vertex_list_t, std::set< VertexIndexType > , boost::property < vertex_interface_list_t, InterfaceHashStruct > > > > > >, boost::property < boost::edge_weight_t, base::Cost > > | SpannerGraph |
The constructed roadmap spanner. | |
typedef boost::graph_traits < SpannerGraph > ::vertex_descriptor | SparseVertex |
A vertex in the sparse roadmap that is constructed. | |
typedef boost::graph_traits < SpannerGraph > ::edge_descriptor | SparseEdge |
An edge in the sparse roadmap that is constructed. | |
typedef boost::shared_ptr < NearestNeighbors < SparseVertex > > | SparseNeighbors |
Nearest neighbor structure which works over the SpannerGraph. | |
typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::undirectedS, boost::property < vertex_state_t, base::State *, boost::property < boost::vertex_predecessor_t, VertexIndexType, boost::property < boost::vertex_rank_t, VertexIndexType, boost::property < vertex_representative_t, SparseVertex > > > >, boost::property < boost::edge_weight_t, double > > | DenseGraph |
The underlying roadmap graph. | |
typedef boost::graph_traits < DenseGraph > ::vertex_descriptor | DenseVertex |
A vertex in DenseGraph. | |
typedef boost::graph_traits < DenseGraph > ::edge_descriptor | DenseEdge |
An edge in DenseGraph. | |
typedef boost::shared_ptr < NearestNeighbors < DenseVertex > > | DenseNeighbors |
Nearest neighbor structure which works over the DenseGraph. | |
Public Member Functions | |
SPARS (const base::SpaceInformationPtr &si) | |
Constructor. | |
virtual | ~SPARS () |
Destructor. | |
virtual void | setProblemDefinition (const base::ProblemDefinitionPtr &pdef) |
virtual void | getPlannerData (base::PlannerData &data) const |
Get information about the current run of the motion planner. Repeated calls to this function will update data (only additions are made). This is useful to see what changed in the exploration datastructure, between calls to solve(), for example (without calling clear() in between). | |
void | constructRoadmap (const base::PlannerTerminationCondition &ptc) |
While the termination condition permits, construct the spanner graph. | |
void | constructRoadmap (const base::PlannerTerminationCondition &ptc, bool stopOnMaxFail) |
While the termination condition permits, construct the spanner graph. If stopOnMaxFail is true, the function also terminates when the failure limit set by setMaxFailures() is reached. | |
virtual base::PlannerStatus | solve (const base::PlannerTerminationCondition &ptc) |
Function that can solve the motion planning problem. This function can be called multiple times on the same problem, without calling clear() in between. This allows the planner to continue work for more time on an unsolved problem, for example. Start and goal states from the currently specified ProblemDefinition are cached. This means that between calls to solve(), input states are only added, not removed. When using PRM as a multi-query planner, the input states should be however cleared, without clearing the roadmap itself. This can be done using the clearQuery() function. | |
void | clearQuery () |
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse the previously computed roadmap, but will clear the set of input states constructed by the previous call to solve(). This enables multi-query functionality for PRM. | |
virtual void | clear () |
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() will ignore all previous work. | |
template<template< typename T > class NN> | |
void | setDenseNeighbors () |
Set a different nearest neighbors datastructure for the roadmap graph. This nearest neighbor structure contains only information on the nodes existing in the underlying dense roadmap. This structure is used for near-neighbor queries for the construction of that graph as well as for determining which dense samples the sparse roadmap nodes should represent. | |
template<template< typename T > class NN> | |
void | setSparseNeighbors () |
Set a different nearest neighbors datastructure for the spanner graph. This structure is stores only nodes in the roadmap spanner, and is used in the construction of the spanner. It can also be queried to determine which node in the spanner should represent a given state. | |
void | setMaxFailures (unsigned int m) |
Set the maximum consecutive failures to augment the spanner before termination. In general, if the algorithm fails to add to the spanner for M consecutive iterations, then we can probabilistically estimate how close to attaining the desired properties the SPARS spanner is. | |
void | setDenseDeltaFraction (double d) |
Set the delta fraction for interface detection. If two nodes in the dense graph are more than a delta fraction of the max. extent apart, then the algorithm cannot consider them to have accurately approximated the location of an interface. | |
void | setSparseDeltaFraction (double d) |
Set the delta fraction for connection distance on the sparse spanner. This value represents the visibility range of sparse samples. A sparse node represents all dense nodes within a delta fraction of the max. extent if it is also the closest sparse node to that dense node. | |
void | setStretchFactor (double t) |
Set the roadmap spanner stretch factor. This value represents a multiplicative upper bound on path quality that should be produced by the roadmap spanner. The produced sparse graph with solutions that are less than t times the optimap path length. It does not make sense to make this parameter more than 3. | |
unsigned | getMaxFailures () const |
Retrieve the maximum consecutive failure limit. | |
double | getDenseDeltaFraction () const |
Retrieve the dense graph interface support delta fraction. | |
double | getSparseDeltaFraction () const |
Retrieve the sparse graph visibility range delta fraction. | |
double | getStretchFactor () const |
Retrieve the spanner's set stretch factor. | |
virtual void | setup () |
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceInformation::setup() if needed. This must be called before solving. | |
const DenseGraph & | getDenseGraph () const |
Retrieve the underlying dense graph structure. This is built as a PRM* and asymptotically approximates best paths through the space. | |
const SpannerGraph & | getRoadmap () const |
Retrieve the sparse roadmap structure. This is the structure which answers given queries, and has the desired property of asymptotic near-optimality. | |
unsigned int | milestoneCount () const |
Returns the number of milestones added to D. | |
unsigned int | guardCount () const |
Returns the number of guards added to S. | |
double | averageValence () const |
Returns the average valence of the spanner graph. | |
void | printDebug (std::ostream &out=std::cout) const |
Print debug information about planner. | |
bool | reachedFailureLimit () const |
Returns true if we have reached the iteration failures limit, maxFailures_. | |
std::string | getIterationCount () const |
std::string | getBestCost () const |
Protected Member Functions | |
DenseVertex | addSample (base::State *workState, const base::PlannerTerminationCondition &ptc) |
Attempt to add a single sample to the roadmap. | |
void | checkQueryStateInitialization () |
Check that the query vertex is initialized (used for internal nearest neighbor searches) | |
bool | sameComponent (SparseVertex m1, SparseVertex m2) |
Check that two vertices are in the same connected component. | |
DenseVertex | addMilestone (base::State *state) |
Construct a milestone for a given state (state) and store it in the nearest neighbors data structure. | |
SparseVertex | addGuard (base::State *state, GuardType type) |
Construct a node with the given state (state) for the spanner and store it in the nn structure. | |
void | connectSparsePoints (SparseVertex v, SparseVertex vp) |
Convenience function for creating an edge in the Spanner Roadmap. | |
void | connectDensePoints (DenseVertex v, DenseVertex vp) |
Connects points in the dense graph. | |
bool | checkAddCoverage (const base::State *lastState, const std::vector< SparseVertex > &neigh) |
Checks the latest dense sample for the coverage property, and adds appropriately. | |
bool | checkAddConnectivity (const base::State *lastState, const std::vector< SparseVertex > &neigh) |
Checks the latest dense sample for connectivity, and adds appropriately. | |
bool | checkAddInterface (const std::vector< DenseVertex > &graphNeighborhood, const std::vector< DenseVertex > &visibleNeighborhood, DenseVertex q) |
Checks the latest dense sample for bridging an edge-less interface. | |
bool | checkAddPath (DenseVertex q, const std::vector< DenseVertex > &neigh) |
Checks for adding an entire dense path to the Sparse Roadmap. | |
DenseVertex | getInterfaceNeighbor (DenseVertex q, SparseVertex rep) |
Get the first neighbor of q who has representative rep and is within denseDelta_. | |
bool | addPathToSpanner (const DensePath &p, SparseVertex vp, SparseVertex vpp) |
Method for actually adding a dense path to the Roadmap Spanner, S. | |
void | updateRepresentatives (SparseVertex v) |
Automatically updates the representatives of all dense samplse within sparseDelta_ of v. | |
void | calculateRepresentative (DenseVertex q) |
Calculates the representative for a dense sample. | |
void | addToRepresentatives (DenseVertex q, SparseVertex rep, const std::set< SparseVertex > &oreps) |
Adds a dense sample to the appropriate lists of its representative. | |
void | removeFromRepresentatives (DenseVertex q, SparseVertex rep) |
Removes the node from its representative's lists. | |
void | computeVPP (DenseVertex v, DenseVertex vp, std::vector< SparseVertex > &VPPs) |
Computes all nodes which qualify as a candidate v" for v and vp. | |
void | computeX (DenseVertex v, DenseVertex vp, DenseVertex vpp, std::vector< SparseVertex > &Xs) |
Computes all nodes which qualify as a candidate x for v, v', and v". | |
void | resetFailures () |
A reset function for resetting the failures count. | |
void | checkForSolution (const base::PlannerTerminationCondition &ptc, base::PathPtr &solution) |
bool | haveSolution (const std::vector< DenseVertex > &start, const std::vector< DenseVertex > &goal, base::PathPtr &solution) |
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in start and the second is in goal, and the two milestones are in the same connected component. If a solution is found, the path is saved. | |
bool | reachedTerminationCriterion () const |
Returns true if we have reached the iteration failures limit, maxFailures_ or if a solution was added. | |
base::PathPtr | constructSolution (const SparseVertex start, const SparseVertex goal) const |
Given two milestones from the same connected component, construct a path connecting them and set it as the solution. | |
void | computeDensePath (const DenseVertex start, const DenseVertex goal, DensePath &path) const |
Constructs the dense path between the start and goal vertices (if connected) | |
void | freeMemory () |
Free all the memory allocated by the planner. | |
void | getSparseNeighbors (base::State *inState, std::vector< SparseVertex > &graphNeighborhood) |
Get all nodes in the sparse graph which are within sparseDelta_ of the given state. | |
void | filterVisibleNeighbors (base::State *inState, const std::vector< SparseVertex > &graphNeighborhood, std::vector< SparseVertex > &visibleNeighborhood) const |
Get the visible neighbors. | |
void | getInterfaceNeighborRepresentatives (DenseVertex q, std::set< SparseVertex > &interfaceRepresentatives) |
Gets the representatives of all interfaces that q supports. | |
void | getInterfaceNeighborhood (DenseVertex q, std::vector< DenseVertex > &interfaceNeighborhood) |
Gets the neighbors of q who help it support an interface. | |
double | distanceFunction (const DenseVertex a, const DenseVertex b) const |
Compute distance between two milestones (this is simply distance between the states of the milestones) | |
double | sparseDistanceFunction (const SparseVertex a, const SparseVertex b) const |
Compute distance between two nodes in the sparse roadmap spanner. | |
base::Cost | costHeuristic (SparseVertex u, SparseVertex v) const |
Given two vertices, returns a heuristic on the cost of the path connecting them. This method wraps OptimizationObjective::motionCostHeuristic. | |
Protected Attributes | |
base::ValidStateSamplerPtr | sampler_ |
Sampler user for generating valid samples in the state space. | |
base::StateSamplerPtr | simpleSampler_ |
Sampler user for generating random in the state space. | |
DenseNeighbors | nn_ |
Nearest neighbors data structure. | |
SparseNeighbors | snn_ |
Nearest Neighbors structure for the sparse roadmap. | |
DenseGraph | g_ |
The dense graph, D. | |
SpannerGraph | s_ |
The sparse roadmap, S. | |
std::vector< SparseVertex > | startM_ |
Array of start guards. | |
std::vector< SparseVertex > | goalM_ |
Array of goal guards. | |
DenseVertex | sparseQueryVertex_ |
DenseVertex for performing nearest neighbor queries on the SPARSE roadmap. | |
DenseVertex | queryVertex_ |
Vertex for performing nearest neighbor queries on the DENSE graph. | |
PathGeometric | geomPath_ |
Geometric Path variable used for smoothing out paths. | |
boost::property_map < DenseGraph, vertex_state_t > ::type | stateProperty_ |
Access to the internal base::state at each DenseVertex. | |
boost::property_map < SpannerGraph, vertex_state_t > ::type | sparseStateProperty_ |
Access to the internal base::State for each SparseVertex of S. | |
boost::property_map < SpannerGraph, vertex_color_t > ::type | sparseColorProperty_ |
Access to draw colors for the SparseVertexs of S, to indicate addition type. | |
boost::property_map < DenseGraph, vertex_representative_t > ::type | representativesProperty_ |
Access to the representatives of the Dense vertices. | |
boost::property_map < SpannerGraph, vertex_list_t > ::type | nonInterfaceListsProperty_ |
Access to all non-interface supporting vertices of the sparse nodes. | |
boost::property_map < SpannerGraph, vertex_interface_list_t > ::type | interfaceListsProperty_ |
Access to the interface-supporting vertice hashes of the sparse nodes. | |
PathSimplifierPtr | psimp_ |
A path simplifier used to simplify dense paths added to S. | |
boost::property_map < DenseGraph, boost::edge_weight_t >::type | weightProperty_ |
Access to the weights of each DenseEdge. | |
boost::disjoint_sets < boost::property_map < SpannerGraph, boost::vertex_rank_t >::type, boost::property_map < SpannerGraph, boost::vertex_predecessor_t > ::type > | sparseDJSets_ |
Data structure that maintains the connected components of S. | |
boost::function< const std::vector< DenseVertex > &(const DenseVertex)> | connectionStrategy_ |
Function that returns the milestones to attempt connections with. | |
unsigned int | consecutiveFailures_ |
A counter for the number of consecutive failed iterations of the algorithm. | |
double | stretchFactor_ |
The stretch factor in terms of graph spanners for SPARS to check against. | |
unsigned int | maxFailures_ |
The maximum number of failures before terminating the algorithm. | |
bool | addedSolution_ |
A flag indicating that a solution has been added during solve() | |
double | denseDeltaFraction_ |
SPARS parameter for dense graph connection distance as a fraction of max. extent. | |
double | sparseDeltaFraction_ |
SPARS parameter for Sparse Roadmap connection distance as a fraction of max. extent. | |
double | denseDelta_ |
SPARS parameter for dense graph connection distance. | |
double | sparseDelta_ |
SPARS parameter for Sparse Roadmap connection distance. | |
RNG | rng_ |
Random number generator. | |
boost::mutex | graphMutex_ |
Mutex to guard access to the graphs. | |
base::OptimizationObjectivePtr | opt_ |
Objective cost function for PRM graph edges. | |
long unsigned int | iterations_ |
A counter for the number of iterations of the algorithm. | |
base::Cost | bestCost_ |
Best cost found so far by algorithm. |
Detailed Description
SPArse Roadmap Spanner technique.
- Short description
- SPARS is an algorithm which operates similarly to the Visibility-based PRM. It has several desirable properties, including asymptotic near-optimality, and a meaningful stopping criterion.
- External documentation
- A. Dobson, A. Krontiris, K. Bekris, Sparse Roadmap Spanners, Workshop on the Algorithmic Foundations of Robotics (WAFR) 2012. [[PDF]](http://www.cs.rutgers.edu/~kb572/pubs/sparse_roadmap_spanner.pdf)
Member Typedef Documentation
typedef boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, boost::property < vertex_state_t, base::State*, boost::property < boost::vertex_predecessor_t, VertexIndexType, boost::property < boost::vertex_rank_t, VertexIndexType, boost::property < vertex_representative_t, SparseVertex > > > >, boost::property < boost::edge_weight_t, double > > ompl::geometric::SPARS::DenseGraph |
The underlying roadmap graph.
- Any BGL graph representation could be used here. Because we
- expect the roadmap to be sparse (m<n^2), an adjacency_list is more appropriate than an adjacency_matrix.
- Obviously, a ompl::base::State* vertex property is required.
- The incremental connected components algorithm requires vertex_predecessor_t and vertex_rank_t properties. If boost::vecS is not used for vertex storage, then there must also be a boost:vertex_index_t property manually added.
- DenseEdges should be undirected and have a weight property.
typedef boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, boost::property < vertex_state_t, base::State*, boost::property < boost::vertex_predecessor_t, VertexIndexType, boost::property < boost::vertex_rank_t, VertexIndexType, boost::property < vertex_color_t, GuardType, boost::property < vertex_list_t, std::set<VertexIndexType>, boost::property < vertex_interface_list_t, InterfaceHashStruct > > > > > >, boost::property < boost::edge_weight_t, base::Cost > > ompl::geometric::SPARS::SpannerGraph |
The constructed roadmap spanner.
- Any BGL graph representation could be used here, but the
- spanner should be very sparse (m<n^2), so we use an adjacency_list.
- Nodes in the spanner contain extra information needed by the
- spanner technique, including nodes in the dense graph which nodes in the spanner represent.
- SparseEdges should be undirected and have a weight property.
Member Function Documentation
void ompl::geometric::SPARS::checkForSolution | ( | const base::PlannerTerminationCondition & | ptc, |
base::PathPtr & | solution | ||
) | [protected] |
The documentation for this class was generated from the following files: