Loading...
Searching...
No Matches

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. More...
 
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.
 
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.
 
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 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
 
- Public Member Functions inherited from ompl::base::Planner
 Planner (const Planner &)=delete
 
Planneroperator= (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.
 
template<class T >
const T * as () const
 Cast this instance to a desired type.
 
const SpaceInformationPtrgetSpaceInformation () const
 Get the space information this planner is using.
 
const ProblemDefinitionPtrgetProblemDefinition () const
 Get the problem definition the planner is trying to solve.
 
ProblemDefinitionPtrgetProblemDefinition ()
 Get the problem definition the planner is trying to solve.
 
const PlannerInputStatesgetPlannerInputStates () 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().
 
virtual PlannerStatus solve (const PlannerTerminationCondition &ptc)=0
 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. If this option is used, it is assumed the problem definition is not changed (unpredictable results otherwise). The only change in the problem definition that is accounted for is the addition of starting or goal states (but not changing previously added start/goal states). If clearQuery() is called, the planner may retain prior datastructures generated from a previous query on a new problem definition. The function terminates if the call to ptc returns true.
 
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.
 
virtual void clear ()
 Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() will ignore all previous work.
 
virtual void clearQuery ()
 Clears internal datastructures of any query-specific information from the previous query. Planner settings are not affected. The planner, if able, should retain all datastructures generated from previous queries that can be used to help solve the next query. Note that clear() should also clear all query-specific information along with all other datastructures in the planner. By default clearQuery() calls clear().
 
virtual void getPlannerData (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).

 
const std::string & getName () const
 Get the name of the planner.
 
void setName (const std::string &name)
 Set the name of the planner.
 
const PlannerSpecsgetSpecs () const
 Return the specifications (capabilities of this planner)
 
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.
 
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.
 
ParamSetparams ()
 Get the parameters for this planner.
 
const ParamSetparams () const
 Get the parameters for this planner.
 
const PlannerProgressPropertiesgetPlannerProgressProperties () 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< 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.
 
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 77 of file SPARS.h.

Member Typedef Documentation

◆ DenseEdge

using ompl::geometric::SPARS::DenseEdge = boost::graph_traits<DenseGraph>::edge_descriptor

An edge in DenseGraph.

Definition at line 187 of file SPARS.h.

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

◆ DenseNeighbors

Nearest neighbor structure which works over the DenseGraph.

Definition at line 190 of file SPARS.h.

◆ DensePath

Internal representation of a dense path.

Definition at line 123 of file SPARS.h.

◆ DenseVertex

using ompl::geometric::SPARS::DenseVertex = boost::graph_traits<DenseGraph>::vertex_descriptor

A vertex in DenseGraph.

Definition at line 184 of file SPARS.h.

◆ InterfaceHash

Hash for storing interface information.

Definition at line 120 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 137 of file SPARS.h.

◆ SparseEdge

using ompl::geometric::SPARS::SparseEdge = boost::graph_traits<SpannerGraph>::edge_descriptor

An edge in the sparse roadmap that is constructed.

Definition at line 154 of file SPARS.h.

◆ SparseNeighbors

Nearest neighbor structure which works over the SpannerGraph.

Definition at line 157 of file SPARS.h.

◆ SparseVertex

using ompl::geometric::SPARS::SparseVertex = boost::graph_traits<SpannerGraph>::vertex_descriptor

A vertex in the sparse roadmap that is constructed.

Definition at line 151 of file SPARS.h.

◆ VertexIndexType

The type used internally for representing vertex IDs.

Definition at line 117 of file SPARS.h.

Member Enumeration Documentation

◆ GuardType

Enumeration which specifies the reason a guard is added to the spanner.

Definition at line 81 of file SPARS.h.

Constructor & Destructor Documentation

◆ SPARS()

ompl::geometric::SPARS::SPARS ( const base::SpaceInformationPtr &  si)

Constructor.

Definition at line 54 of file SPARS.cpp.

◆ ~SPARS()

ompl::geometric::SPARS::~SPARS ( )
override

Destructor.

Definition at line 93 of file SPARS.cpp.

Member Function Documentation

◆ addGuard()

ompl::geometric::SPARS::SparseVertex ompl::geometric::SPARS::addGuard ( base::State state,
GuardType  type 
)
protected

Construct a node with the given state (state) for the spanner and store it in the nn structure.

Definition at line 514 of file SPARS.cpp.

◆ addMilestone()

ompl::geometric::SPARS::DenseVertex ompl::geometric::SPARS::addMilestone ( base::State state)
protected

Construct a milestone for a given state (state) and store it in the nearest neighbors data structure.

Definition at line 474 of file SPARS.cpp.

◆ addPathToSpanner()

bool ompl::geometric::SPARS::addPathToSpanner ( const DensePath dense_path,
SparseVertex  vp,
SparseVertex  vpp 
)
protected

Method for actually adding a dense path to the Roadmap Spanner, S.

Definition at line 790 of file SPARS.cpp.

◆ addSample()

ompl::geometric::SPARS::DenseVertex ompl::geometric::SPARS::addSample ( base::State workState,
const base::PlannerTerminationCondition ptc 
)
protected

Attempt to add a single sample to the roadmap.

Definition at line 204 of file SPARS.cpp.

◆ addToRepresentatives()

void ompl::geometric::SPARS::addToRepresentatives ( DenseVertex  q,
SparseVertex  rep,
const std::set< SparseVertex > &  oreps 
)
protected

Adds a dense sample to the appropriate lists of its representative.

Definition at line 883 of file SPARS.cpp.

◆ averageValence()

double ompl::geometric::SPARS::averageValence ( ) const

Returns the average valence of the spanner graph.

Definition at line 735 of file SPARS.cpp.

◆ calculateRepresentative()

void ompl::geometric::SPARS::calculateRepresentative ( DenseVertex  q)
protected

Calculates the representative for a dense sample.

Definition at line 866 of file SPARS.cpp.

◆ checkAddConnectivity()

bool ompl::geometric::SPARS::checkAddConnectivity ( const base::State lastState,
const std::vector< SparseVertex > &  neigh 
)
protected

Checks the latest dense sample for connectivity, and adds appropriately.

Definition at line 561 of file SPARS.cpp.

◆ checkAddCoverage()

bool ompl::geometric::SPARS::checkAddCoverage ( const base::State lastState,
const std::vector< SparseVertex > &  neigh 
)
protected

Checks the latest dense sample for the coverage property, and adds appropriately.

Definition at line 548 of file SPARS.cpp.

◆ checkAddInterface()

bool ompl::geometric::SPARS::checkAddInterface ( const std::vector< DenseVertex > &  graphNeighborhood,
const std::vector< DenseVertex > &  visibleNeighborhood,
DenseVertex  q 
)
protected

Checks the latest dense sample for bridging an edge-less interface.

Definition at line 594 of file SPARS.cpp.

◆ checkAddPath()

bool ompl::geometric::SPARS::checkAddPath ( DenseVertex  q,
const std::vector< DenseVertex > &  neigh 
)
protected

Checks for adding an entire dense path to the Sparse Roadmap.

Definition at line 626 of file SPARS.cpp.

◆ checkForSolution()

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

Thread that checks for solution

Definition at line 226 of file SPARS.cpp.

◆ checkQueryStateInitialization()

void ompl::geometric::SPARS::checkQueryStateInitialization ( )
protected

Check that the query vertex is initialized (used for internal nearest neighbor searches)

Definition at line 301 of file SPARS.cpp.

◆ clear()

void ompl::geometric::SPARS::clear ( )
overridevirtual

Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() will ignore all previous work.

Reimplemented from ompl::base::Planner.

Definition at line 171 of file SPARS.cpp.

◆ clearQuery()

void ompl::geometric::SPARS::clearQuery ( )
overridevirtual

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.

Reimplemented from ompl::base::Planner.

Definition at line 160 of file SPARS.cpp.

◆ computeDensePath()

void ompl::geometric::SPARS::computeDensePath ( DenseVertex  start,
DenseVertex  goal,
DensePath path 
) const
protected

Constructs the dense path between the start and goal vertices (if connected)

Definition at line 1023 of file SPARS.cpp.

◆ computeVPP()

void ompl::geometric::SPARS::computeVPP ( DenseVertex  v,
DenseVertex  vp,
std::vector< SparseVertex > &  VPPs 
)
protected

Computes all nodes which qualify as a candidate v" for v and vp.

Definition at line 921 of file SPARS.cpp.

◆ computeX()

void ompl::geometric::SPARS::computeX ( DenseVertex  v,
DenseVertex  vp,
DenseVertex  vpp,
std::vector< SparseVertex > &  Xs 
)
protected

Computes all nodes which qualify as a candidate x for v, v', and v".

Definition at line 929 of file SPARS.cpp.

◆ connectDensePoints()

void ompl::geometric::SPARS::connectDensePoints ( DenseVertex  v,
DenseVertex  vp 
)
protected

Connects points in the dense graph.

Definition at line 540 of file SPARS.cpp.

◆ connectSparsePoints()

void ompl::geometric::SPARS::connectSparsePoints ( SparseVertex  v,
SparseVertex  vp 
)
protected

Convenience function for creating an edge in the Spanner Roadmap.

Definition at line 531 of file SPARS.cpp.

◆ constructRoadmap() [1/2]

void ompl::geometric::SPARS::constructRoadmap ( const base::PlannerTerminationCondition ptc)

While the termination condition permits, construct the spanner graph.

Definition at line 415 of file SPARS.cpp.

◆ constructRoadmap() [2/2]

void ompl::geometric::SPARS::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.

Definition at line 400 of file SPARS.cpp.

◆ constructSolution()

ompl::base::PathPtr ompl::geometric::SPARS::constructSolution ( SparseVertex  start,
SparseVertex  goal 
) const
protected

Given two milestones from the same connected component, construct a path connecting them and set it as the solution.

Definition at line 977 of file SPARS.cpp.

◆ costHeuristic()

ompl::base::Cost ompl::geometric::SPARS::costHeuristic ( SparseVertex  u,
SparseVertex  v 
) const
protected

Given two vertices, returns a heuristic on the cost of the path connecting them. This method wraps OptimizationObjective::motionCostHeuristic.

Definition at line 1082 of file SPARS.cpp.

◆ distanceFunction()

double ompl::geometric::SPARS::distanceFunction ( const DenseVertex  a,
const DenseVertex  b 
) const
inlineprotected

Compute distance between two milestones (this is simply distance between the states of the milestones)

Definition at line 471 of file SPARS.h.

◆ filterVisibleNeighbors()

void ompl::geometric::SPARS::filterVisibleNeighbors ( base::State inState,
const std::vector< SparseVertex > &  graphNeighborhood,
std::vector< SparseVertex > &  visibleNeighborhood 
) const
protected

Get the visible neighbors.

Definition at line 770 of file SPARS.cpp.

◆ freeMemory()

void ompl::geometric::SPARS::freeMemory ( )
protected

Free all the memory allocated by the planner.

Definition at line 186 of file SPARS.cpp.

◆ getBestCost()

std::string ompl::geometric::SPARS::getBestCost ( ) const
inline

Definition at line 364 of file SPARS.h.

◆ getDenseDeltaFraction()

double ompl::geometric::SPARS::getDenseDeltaFraction ( ) const
inline

Retrieve the dense graph interface support delta fraction.

Definition at line 304 of file SPARS.h.

◆ getDenseGraph()

const DenseGraph & ompl::geometric::SPARS::getDenseGraph ( ) const
inline

Retrieve the underlying dense graph structure. This is built as a PRM* and asymptotically approximates best paths through the space.

Definition at line 325 of file SPARS.h.

◆ getInterfaceNeighbor()

ompl::geometric::SPARS::DenseVertex ompl::geometric::SPARS::getInterfaceNeighbor ( DenseVertex  q,
SparseVertex  rep 
)
protected

Get the first neighbor of q who has representative rep and is within denseDelta_.

Definition at line 781 of file SPARS.cpp.

◆ getInterfaceNeighborhood()

void ompl::geometric::SPARS::getInterfaceNeighborhood ( DenseVertex  q,
std::vector< DenseVertex > &  interfaceNeighborhood 
)
protected

Gets the neighbors of q who help it support an interface.

Definition at line 960 of file SPARS.cpp.

◆ getInterfaceNeighborRepresentatives()

void ompl::geometric::SPARS::getInterfaceNeighborRepresentatives ( DenseVertex  q,
std::set< SparseVertex > &  interfaceRepresentatives 
)
protected

Gets the representatives of all interfaces that q supports.

Definition at line 939 of file SPARS.cpp.

◆ getIterationCount()

std::string ompl::geometric::SPARS::getIterationCount ( ) const
inline

Definition at line 360 of file SPARS.h.

◆ getMaxFailures()

unsigned ompl::geometric::SPARS::getMaxFailures ( ) const
inline

Retrieve the maximum consecutive failure limit.

Definition at line 298 of file SPARS.h.

◆ getPlannerData()

void ompl::geometric::SPARS::getPlannerData ( base::PlannerData data) const
overridevirtual

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).

Reimplemented from ompl::base::Planner.

Definition at line 1052 of file SPARS.cpp.

◆ getRoadmap()

const SpannerGraph & ompl::geometric::SPARS::getRoadmap ( ) const
inline

Retrieve the sparse roadmap structure. This is the structure which answers given queries, and has the desired property of asymptotic near-optimality.

Definition at line 332 of file SPARS.h.

◆ getSparseDeltaFraction()

double ompl::geometric::SPARS::getSparseDeltaFraction ( ) const
inline

Retrieve the sparse graph visibility range delta fraction.

Definition at line 310 of file SPARS.h.

◆ getSparseNeighbors()

void ompl::geometric::SPARS::getSparseNeighbors ( base::State inState,
std::vector< SparseVertex > &  graphNeighborhood 
)
protected

Get all nodes in the sparse graph which are within sparseDelta_ of the given state.

Definition at line 760 of file SPARS.cpp.

◆ getStretchFactor()

double ompl::geometric::SPARS::getStretchFactor ( ) const
inline

Retrieve the spanner's set stretch factor.

Definition at line 316 of file SPARS.h.

◆ guardCount()

unsigned int ompl::geometric::SPARS::guardCount ( ) const
inline

Returns the number of guards added to S.

Definition at line 344 of file SPARS.h.

◆ haveSolution()

bool ompl::geometric::SPARS::haveSolution ( const std::vector< DenseVertex > &  starts,
const std::vector< DenseVertex > &  goals,
base::PathPtr &  solution 
)
protected

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.

Definition at line 250 of file SPARS.cpp.

◆ milestoneCount()

unsigned int ompl::geometric::SPARS::milestoneCount ( ) const
inline

Returns the number of milestones added to D.

Definition at line 338 of file SPARS.h.

◆ printDebug()

void ompl::geometric::SPARS::printDebug ( std::ostream &  out = std::cout) const

Print debug information about planner.

Definition at line 744 of file SPARS.cpp.

◆ reachedFailureLimit()

bool ompl::geometric::SPARS::reachedFailureLimit ( ) const

Returns true if we have reached the iteration failures limit, maxFailures_

Definition at line 296 of file SPARS.cpp.

◆ reachedTerminationCriterion()

bool ompl::geometric::SPARS::reachedTerminationCriterion ( ) const
protected

Returns true if we have reached the iteration failures limit, maxFailures_ or if a solution was added.

Definition at line 291 of file SPARS.cpp.

◆ removeFromRepresentatives()

void ompl::geometric::SPARS::removeFromRepresentatives ( DenseVertex  q,
SparseVertex  rep 
)
protected

Removes the node from its representative's lists.

Definition at line 908 of file SPARS.cpp.

◆ resetFailures()

void ompl::geometric::SPARS::resetFailures ( )
protected

A reset function for resetting the failures count.

Definition at line 155 of file SPARS.cpp.

◆ sameComponent()

bool ompl::geometric::SPARS::sameComponent ( SparseVertex  m1,
SparseVertex  m2 
)
protected

Check that two vertices are in the same connected component.

Definition at line 313 of file SPARS.cpp.

◆ setDenseDeltaFraction()

void ompl::geometric::SPARS::setDenseDeltaFraction ( double  d)
inline

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.

Definition at line 269 of file SPARS.h.

◆ setDenseNeighbors()

template<template< typename T > class NN>
void ompl::geometric::SPARS::setDenseNeighbors ( )
inline

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.

Definition at line 237 of file SPARS.h.

◆ setMaxFailures()

void ompl::geometric::SPARS::setMaxFailures ( unsigned int  m)
inline

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.

Definition at line 261 of file SPARS.h.

◆ setProblemDefinition()

void ompl::geometric::SPARS::setProblemDefinition ( const base::ProblemDefinitionPtr &  pdef)
override

Definition at line 149 of file SPARS.cpp.

◆ setSparseDeltaFraction()

void ompl::geometric::SPARS::setSparseDeltaFraction ( double  d)
inline

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.

Definition at line 280 of file SPARS.h.

◆ setSparseNeighbors()

template<template< typename T > class NN>
void ompl::geometric::SPARS::setSparseNeighbors ( )
inline

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.

Definition at line 250 of file SPARS.h.

◆ setStretchFactor()

void ompl::geometric::SPARS::setStretchFactor ( double  t)
inline

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.

Definition at line 292 of file SPARS.h.

◆ setup()

void ompl::geometric::SPARS::setup ( )
overridevirtual

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.

Reimplemented from ompl::base::Planner.

Definition at line 98 of file SPARS.cpp.

◆ solve()

ompl::base::PlannerStatus ompl::geometric::SPARS::solve ( const base::PlannerTerminationCondition ptc)
overridevirtual

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.

Implements ompl::base::Planner.

Definition at line 318 of file SPARS.cpp.

◆ sparseDistanceFunction()

double ompl::geometric::SPARS::sparseDistanceFunction ( const SparseVertex  a,
const SparseVertex  b 
) const
inlineprotected

Compute distance between two nodes in the sparse roadmap spanner.

Definition at line 477 of file SPARS.h.

◆ updateRepresentatives()

void ompl::geometric::SPARS::updateRepresentatives ( SparseVertex  v)
protected

Automatically updates the representatives of all dense samplse within sparseDelta_ of v.

Definition at line 831 of file SPARS.cpp.

Member Data Documentation

◆ addedSolution_

bool ompl::geometric::SPARS::addedSolution_ {false}
protected

A flag indicating that a solution has been added during solve()

Definition at line 553 of file SPARS.h.

◆ bestCost_

base::Cost ompl::geometric::SPARS::bestCost_ {std::numeric_limits<double>::quiet_NaN()}
protected

Best cost found so far by algorithm.

Definition at line 585 of file SPARS.h.

◆ connectionStrategy_

std::function<const std::vector<DenseVertex> &(const DenseVertex)> ompl::geometric::SPARS::connectionStrategy_
protected

Function that returns the milestones to attempt connections with.

Definition at line 541 of file SPARS.h.

◆ consecutiveFailures_

unsigned int ompl::geometric::SPARS::consecutiveFailures_ {0u}
protected

A counter for the number of consecutive failed iterations of the algorithm.

Definition at line 544 of file SPARS.h.

◆ denseDelta_

double ompl::geometric::SPARS::denseDelta_ {0.}
protected

SPARS parameter for dense graph connection distance.

Definition at line 562 of file SPARS.h.

◆ denseDeltaFraction_

double ompl::geometric::SPARS::denseDeltaFraction_ {.001}
protected

SPARS parameter for dense graph connection distance as a fraction of max. extent.

Definition at line 556 of file SPARS.h.

◆ g_

DenseGraph ompl::geometric::SPARS::g_
protected

The dense graph, D.

Definition at line 492 of file SPARS.h.

◆ geomPath_

PathGeometric ompl::geometric::SPARS::geomPath_
protected

Geometric Path variable used for smoothing out paths.

Definition at line 510 of file SPARS.h.

◆ goalM_

std::vector<SparseVertex> ompl::geometric::SPARS::goalM_
protected

Array of goal guards.

Definition at line 501 of file SPARS.h.

◆ graphMutex_

std::mutex ompl::geometric::SPARS::graphMutex_
mutableprotected

Mutex to guard access to the graphs.

Definition at line 571 of file SPARS.h.

◆ interfaceListsProperty_

boost::property_map<SpannerGraph,vertex_interface_list_t>::type ompl::geometric::SPARS::interfaceListsProperty_
protected

Access to the interface-supporting vertice hashes of the sparse nodes.

Definition at line 528 of file SPARS.h.

◆ iterations_

long unsigned int ompl::geometric::SPARS::iterations_ {0ul}
protected

A counter for the number of iterations of the algorithm.

Definition at line 583 of file SPARS.h.

◆ maxFailures_

unsigned int ompl::geometric::SPARS::maxFailures_ {1000u}
protected

The maximum number of failures before terminating the algorithm.

Definition at line 550 of file SPARS.h.

◆ nn_

DenseNeighbors ompl::geometric::SPARS::nn_
protected

Nearest neighbors data structure.

Definition at line 486 of file SPARS.h.

◆ nonInterfaceListsProperty_

boost::property_map<SpannerGraph,vertex_list_t>::type ompl::geometric::SPARS::nonInterfaceListsProperty_
protected

Access to all non-interface supporting vertices of the sparse nodes.

Definition at line 525 of file SPARS.h.

◆ opt_

base::OptimizationObjectivePtr ompl::geometric::SPARS::opt_
protected

Objective cost function for PRM graph edges.

Definition at line 574 of file SPARS.h.

◆ psimp_

PathSimplifierPtr ompl::geometric::SPARS::psimp_
protected

A path simplifier used to simplify dense paths added to S.

Definition at line 531 of file SPARS.h.

◆ queryVertex_

DenseVertex ompl::geometric::SPARS::queryVertex_
protected

Vertex for performing nearest neighbor queries on the DENSE graph.

Definition at line 507 of file SPARS.h.

◆ representativesProperty_

boost::property_map<DenseGraph,vertex_representative_t>::type ompl::geometric::SPARS::representativesProperty_
protected

Access to the representatives of the Dense vertices.

Definition at line 522 of file SPARS.h.

◆ rng_

RNG ompl::geometric::SPARS::rng_
protected

Random number generator.

Definition at line 568 of file SPARS.h.

◆ s_

SpannerGraph ompl::geometric::SPARS::s_
protected

The sparse roadmap, S.

Definition at line 495 of file SPARS.h.

◆ sampler_

base::ValidStateSamplerPtr ompl::geometric::SPARS::sampler_
protected

Sampler user for generating valid samples in the state space.

Definition at line 483 of file SPARS.h.

◆ snn_

SparseNeighbors ompl::geometric::SPARS::snn_
protected

Nearest Neighbors structure for the sparse roadmap.

Definition at line 489 of file SPARS.h.

◆ sparseColorProperty_

boost::property_map<SpannerGraph,vertex_color_t>::type ompl::geometric::SPARS::sparseColorProperty_
protected

Access to draw colors for the SparseVertexs of S, to indicate addition type.

Definition at line 519 of file SPARS.h.

◆ sparseDelta_

double ompl::geometric::SPARS::sparseDelta_ {0.}
protected

SPARS parameter for Sparse Roadmap connection distance.

Definition at line 565 of file SPARS.h.

◆ sparseDeltaFraction_

double ompl::geometric::SPARS::sparseDeltaFraction_ {.25}
protected

SPARS parameter for Sparse Roadmap connection distance as a fraction of max. extent.

Definition at line 559 of file SPARS.h.

◆ sparseDJSets_

boost::disjoint_sets<boost::property_map<SpannerGraph, boost::vertex_rank_t>::type, boost::property_map<SpannerGraph, boost::vertex_predecessor_t>::type> ompl::geometric::SPARS::sparseDJSets_
protected

Data structure that maintains the connected components of S.

Definition at line 538 of file SPARS.h.

◆ sparseQueryVertex_

DenseVertex ompl::geometric::SPARS::sparseQueryVertex_
protected

DenseVertex for performing nearest neighbor queries on the SPARSE roadmap.

Definition at line 504 of file SPARS.h.

◆ sparseStateProperty_

boost::property_map<SpannerGraph,vertex_state_t>::type ompl::geometric::SPARS::sparseStateProperty_
protected

Access to the internal base::State for each SparseVertex of S.

Definition at line 516 of file SPARS.h.

◆ startM_

std::vector<SparseVertex> ompl::geometric::SPARS::startM_
protected

Array of start guards.

Definition at line 498 of file SPARS.h.

◆ stateProperty_

boost::property_map<DenseGraph,vertex_state_t>::type ompl::geometric::SPARS::stateProperty_
protected

Access to the internal base::state at each DenseVertex.

Definition at line 513 of file SPARS.h.

◆ stretchFactor_

double ompl::geometric::SPARS::stretchFactor_ {3.}
protected

The stretch factor in terms of graph spanners for SPARS to check against.

Definition at line 547 of file SPARS.h.

◆ weightProperty_

boost::property_map<DenseGraph,boost::edge_weight_t>::type ompl::geometric::SPARS::weightProperty_
protected

Access to the weights of each DenseEdge.

Definition at line 534 of file SPARS.h.


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