ompl::geometric::SPARS Class Reference

SPArse Roadmap Spanner technique. More...

#include <ompl/geometric/planners/prm/SPARS.h>

Inheritance diagram for ompl::geometric::SPARS:

List of all members.

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 DenseGraphgetDenseGraph () const
 Retrieve the underlying dense graph structure. This is built as a PRM* and asymptotically approximates best paths through the space.
const SpannerGraphgetRoadmap () 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< SparseVertexstartM_
 Array of start guards.
std::vector< SparseVertexgoalM_
 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)

Definition at line 80 of file SPARS.h.


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.

Definition at line 187 of file SPARS.h.

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.

Definition at line 154 of file SPARS.h.


Member Function Documentation

Thread that checks for solution

Definition at line 212 of file SPARS.cpp.


The documentation for this class was generated from the following files:
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines