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	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.
using	VertexIndexType = unsigned long
	The type used internally for representing vertex IDs.
using	InterfaceHash = std::unordered_map< VertexIndexType, std::set< VertexIndexType > >
	Hash for storing interface information.
using	DensePath = std::deque< base::State * >
	Internal representation of a dense path.
using	SpannerGraph = 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, InterfaceHash > >> >> >, boost::property< boost::edge_weight_t, base::Cost > >
	The constructed roadmap spanner. More...
using	SparseVertex = boost::graph_traits< SpannerGraph >::vertex_descriptor
	A vertex in the sparse roadmap that is constructed.
using	SparseEdge = boost::graph_traits< SpannerGraph >::edge_descriptor
	An edge in the sparse roadmap that is constructed.
using	SparseNeighbors = std::shared_ptr< NearestNeighbors< SparseVertex > >
	Nearest neighbor structure which works over the SpannerGraph.
using	DenseGraph = 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 > >
	The underlying roadmap graph. More...
using	DenseVertex = boost::graph_traits< DenseGraph >::vertex_descriptor
	A vertex in DenseGraph.
using	DenseEdge = boost::graph_traits< DenseGraph >::edge_descriptor
	An edge in DenseGraph.
using	DenseNeighbors = std::shared_ptr< NearestNeighbors< DenseVertex > >
	Nearest neighbor structure which works over the DenseGraph.
Public Types inherited from ompl::base::Planner
using	PlannerProgressProperty = std::function< std::string()>
	Definition of a function which returns a property about the planner's progress that can be queried by a benchmarking routine.
using	PlannerProgressProperties = std::map< std::string, PlannerProgressProperty >
	A dictionary which maps the name of a progress property to the function to be used for querying that property.

Public Member Functions
	SPARS (const base::SpaceInformationPtr &si)
	Constructor.
	~SPARS () override
	Destructor.
void	setProblemDefinition (const base::ProblemDefinitionPtr &pdef) override
void	getPlannerData (base::PlannerData &data) const override
	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.
base::PlannerStatus	solve (const base::PlannerTerminationCondition &ptc) override
	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 () override
	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.
void	clear () override
	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.
void	setup () override
	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
Public Member Functions inherited from ompl::base::Planner
	Planner (const Planner &)=delete
Planner &	operator= (const Planner &)=delete
	Planner (SpaceInformationPtr si, std::string name)
	Constructor.
virtual	~Planner ()=default
	Destructor.
template<class T >
T *	as ()
	Cast this instance to a desired type. More...
template<class T >
const T *	as () const
	Cast this instance to a desired type. More...
const SpaceInformationPtr &	getSpaceInformation () const
	Get the space information this planner is using.
const ProblemDefinitionPtr &	getProblemDefinition () const
	Get the problem definition the planner is trying to solve.
ProblemDefinitionPtr &	getProblemDefinition ()
	Get the problem definition the planner is trying to solve.
const PlannerInputStates &	getPlannerInputStates () const
	Get the planner input states.
virtual void	setProblemDefinition (const ProblemDefinitionPtr &pdef)
	Set the problem definition for the planner. The problem needs to be set before calling solve(). Note: If this problem definition replaces a previous one, it may also be necessary to call clear() or clearQuery().
PlannerStatus	solve (const PlannerTerminationConditionFn &ptc, double checkInterval)
	Same as above except the termination condition is only evaluated at a specified interval.
PlannerStatus	solve (double solveTime)
	Same as above except the termination condition is solely a time limit: the number of seconds the algorithm is allowed to spend planning.
const std::string &	getName () const
	Get the name of the planner.
void	setName (const std::string &name)
	Set the name of the planner.
const PlannerSpecs &	getSpecs () const
	Return the specifications (capabilities of this planner)
virtual void	checkValidity ()
	Check to see if the planner is in a working state (setup has been called, a goal was set, the input states seem to be in order). In case of error, this function throws an exception.
bool	isSetup () const
	Check if setup() was called for this planner.
ParamSet &	params ()
	Get the parameters for this planner.
const ParamSet &	params () const
	Get the parameters for this planner.
const PlannerProgressProperties &	getPlannerProgressProperties () const
	Retrieve a planner's planner progress property map.
virtual void	printProperties (std::ostream &out) const
	Print properties of the motion planner.
virtual void	printSettings (std::ostream &out) const
	Print information about the motion planner's settings.

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 &dense_path, 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 > &starts, const std::vector< DenseVertex > &goals, 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 (SparseVertex start, SparseVertex goal) const
	Given two milestones from the same connected component, construct a path connecting them and set it as the solution.
void	computeDensePath (DenseVertex start, 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 Member Functions inherited from ompl::base::Planner
template<typename T , typename PlannerType , typename SetterType , typename GetterType >
void	declareParam (const std::string &name, const PlannerType &planner, const SetterType &setter, const GetterType &getter, const std::string &rangeSuggestion="")
	This function declares a parameter for this planner instance, and specifies the setter and getter functions.
template<typename T , typename PlannerType , typename SetterType >
void	declareParam (const std::string &name, const PlannerType &planner, const SetterType &setter, const std::string &rangeSuggestion="")
	This function declares a parameter for this planner instance, and specifies the setter function.
void	addPlannerProgressProperty (const std::string &progressPropertyName, const PlannerProgressProperty &prop)
	Add a planner progress property called progressPropertyName with a property querying function prop to this planner's progress property map.

Protected Attributes
base::ValidStateSamplerPtr	sampler_
	Sampler user for generating valid samples 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.
std::function< const std::vector< DenseVertex > &(const DenseVertex)>	connectionStrategy_
	Function that returns the milestones to attempt connections with.
unsigned int	consecutiveFailures_ {0u}
	A counter for the number of consecutive failed iterations of the algorithm.
double	stretchFactor_ {3.}
	The stretch factor in terms of graph spanners for SPARS to check against.
unsigned int	maxFailures_ {1000u}
	The maximum number of failures before terminating the algorithm.
bool	addedSolution_ {false}
	A flag indicating that a solution has been added during solve()
double	denseDeltaFraction_ {.001}
	SPARS parameter for dense graph connection distance as a fraction of max. extent.
double	sparseDeltaFraction_ {.25}
	SPARS parameter for Sparse Roadmap connection distance as a fraction of max. extent.
double	denseDelta_ {0.}
	SPARS parameter for dense graph connection distance.
double	sparseDelta_ {0.}
	SPARS parameter for Sparse Roadmap connection distance.
RNG	rng_
	Random number generator.
std::mutex	graphMutex_
	Mutex to guard access to the graphs.
base::OptimizationObjectivePtr	opt_
	Objective cost function for PRM graph edges.
long unsigned int	iterations_ {0ul}
	A counter for the number of iterations of the algorithm.
base::Cost	bestCost_ {std::numeric_limits<double>::quiet_NaN()}
	Best cost found so far by algorithm.
Protected Attributes inherited from ompl::base::Planner
SpaceInformationPtr	si_
	The space information for which planning is done.
ProblemDefinitionPtr	pdef_
	The user set problem definition.
PlannerInputStates	pis_
	Utility class to extract valid input states
std::string	name_
	The name of this planner.
PlannerSpecs	specs_
	The specifications of the planner (its capabilities)
ParamSet	params_
	A map from parameter names to parameter instances for this planner. This field is populated by the declareParam() function.
PlannerProgressProperties	plannerProgressProperties_
	A mapping between this planner's progress property names and the functions used for querying those progress properties.
bool	setup_
	Flag indicating whether setup() has been called.

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]

Definition at line 141 of file SPARS.h.

Member Typedef Documentation

DenseGraph

using ompl::geometric::SPARS::DenseGraph = 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> >

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 277 of file SPARS.h.

SpannerGraph

using ompl::geometric::SPARS::SpannerGraph = 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, InterfaceHash> >> >> >, boost::property<boost::edge_weight_t, base::Cost> >

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 244 of file SPARS.h.

Member Function Documentation

checkForSolution()

void ompl::geometric::SPARS::checkForSolution ( const base::PlannerTerminationCondition &  ptc, base::PathPtr &  solution  )

protected

Thread that checks for solution

Definition at line 211 of file SPARS.cpp.

---

The documentation for this class was generated from the following files:

• ompl/geometric/planners/prm/SPARS.h
• ompl/geometric/planners/prm/src/SPARS.cpp