Class SPARStwo

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Class Documentation

class SPARStwo : public ompl::base::Planner

SPArse Roadmap Spanner Version 2.0

Short description

SPARStwo is a variant of the SPARS algorithm which removes the dependency on having the dense graph, D. It works through similar mechanics, but uses a different approach to identifying interfaces and computing shortest paths through said interfaces.

External documentation

A. Dobson, K. Bekris, Improving Sparse Roadmap Spanners, IEEE International Conference on Robotics and Automation (ICRA) May 2013. [PDF]

Public Types

enum GuardType

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

Values:

enumerator START
enumerator GOAL
enumerator COVERAGE
enumerator CONNECTIVITY
enumerator INTERFACE
enumerator QUALITY
using VertexIndexType = unsigned long

The type used internally for representing vertex IDs.

using VertexPair = std::pair<VertexIndexType, VertexIndexType>

Pair of vertices which support an interface.

using InterfaceHash = std::unordered_map<VertexPair, InterfaceData>

the hash which maps pairs of neighbor points to pairs of states

using Graph = 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_interface_data_t, InterfaceHash>>>>>, boost::property<boost::edge_weight_t, base::Cost>>

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.

Edges should be undirected and have a weight property.

using Vertex = boost::graph_traits<Graph>::vertex_descriptor

Vertex in Graph.

using Edge = boost::graph_traits<Graph>::edge_descriptor

Edge in Graph.

Public Functions

SPARStwo(const base::SpaceInformationPtr &si)

Constructor.

~SPARStwo() override

Destructor.

void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override
inline void setStretchFactor(double t)

Sets the stretch factor.

inline void setSparseDeltaFraction(double D)

Sets vertex visibility range as a fraction of max. extent.

inline void setDenseDeltaFraction(double d)

Sets interface support tolerance as a fraction of max. extent.

inline void setMaxFailures(unsigned int m)

Sets the maximum failures until termination.

inline unsigned int getMaxFailures() const

Retrieve the maximum consecutive failure limit.

inline double getDenseDeltaFraction() const

Retrieve the dense graph interface support delta.

inline double getSparseDeltaFraction() const

Retrieve the sparse graph visibility range delta.

inline double getStretchFactor() const

Retrieve the spanner’s set stretch factor.

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

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

virtual 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>
inline void setNearestNeighbors()

Set a different nearest neighbors datastructure.

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

inline const Graph &getRoadmap() const

Retrieve the computed roadmap.

inline unsigned int milestoneCount() const

Get the number of vertices in the sparse roadmap.

virtual 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 printDebug(std::ostream &out = std::cout) const

Print debug information about planner.

inline std::string getIterationCount() const
inline std::string getBestCost() const

Protected Functions

void freeMemory()

Free all the memory allocated by the planner.

void checkQueryStateInitialization()

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

bool checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)

Checks to see if the sample needs to be added to ensure coverage of the space.

bool checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)

Checks to see if the sample needs to be added to ensure connectivity.

bool checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood, std::vector<Vertex> &visibleNeighborhood)

Checks to see if the current sample reveals the existence of an interface, and if so, tries to bridge it.

bool checkAddPath(Vertex v)

Checks vertex v for short paths through its region and adds when appropriate.

void resetFailures()

A reset function for resetting the failures count.

void findGraphNeighbors(base::State *st, std::vector<Vertex> &graphNeighborhood, std::vector<Vertex> &visibleNeighborhood)

Finds visible nodes in the graph near st.

void approachGraph(Vertex v)

Approaches the graph from a given vertex.

Vertex findGraphRepresentative(base::State *st)

Finds the representative of the input state, st

void findCloseRepresentatives(base::State *workArea, const base::State *qNew, Vertex qRep, std::map<Vertex, base::State*> &closeRepresentatives, const base::PlannerTerminationCondition &ptc)

Finds representatives of samples near qNew_ which are not his representative.

void updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s)

High-level method which updates pair point information for repV_ with neighbor r.

void computeVPP(Vertex v, Vertex vp, std::vector<Vertex> &VPPs)

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

void computeX(Vertex v, Vertex vp, Vertex vpp, std::vector<Vertex> &Xs)

Computes all nodes which qualify as a candidate x for v, v’, and v”.

VertexPair index(Vertex vp, Vertex vpp)

Rectifies indexing order for accessing the vertex data.

InterfaceData &getData(Vertex v, Vertex vp, Vertex vpp)

Retrieves the Vertex data associated with v,vp,vpp.

void distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp)

Performs distance checking for the candidate new state, q against the current information.

void abandonLists(base::State *st)

When a new guard is added at state st, finds all guards who must abandon their interface information and deletes that information.

Vertex addGuard(base::State *state, GuardType type)

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

void connectGuards(Vertex v, Vertex vp)

Connect two guards in the roadmap.

bool haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &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.

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

Thread that checks for solution

bool reachedTerminationCriterion() const

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

bool reachedFailureLimit() const

Returns whether we have reached the iteration failures limit, maxFailures_.

base::PathPtr constructSolution(Vertex start, Vertex goal) const

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

bool sameComponent(Vertex m1, Vertex m2)

Check if two milestones (m1 and m2) are part of the same connected component. This is not a const function since we use incremental connected components from boost.

inline double distanceFunction(const Vertex a, const Vertex b) const

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

base::Cost costHeuristic(Vertex u, Vertex 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.

std::shared_ptr<NearestNeighbors<Vertex>> nn_

Nearest neighbors data structure.

Graph g_

Connectivity graph.

std::vector<Vertex> startM_

Array of start milestones.

std::vector<Vertex> goalM_

Array of goal milestones.

Vertex queryVertex_

Vertex for performing nearest neighbor queries.

double stretchFactor_ = {3.}

Stretch Factor as per graph spanner literature (multiplicative bound on path quality)

double sparseDeltaFraction_ = {.25}

Maximum visibility range for nodes in the graph as a fraction of maximum extent.

double denseDeltaFraction_ = {.001}

Maximum range for allowing two samples to support an interface as a fraction of maximum extent.

unsigned int maxFailures_ = {5000}

The number of consecutive failures to add to the graph before termination.

unsigned int nearSamplePoints_

Number of sample points to use when trying to detect interfaces.

boost::property_map<Graph, vertex_state_t>::type stateProperty_

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

PathSimplifierPtr psimp_

A path simplifier used to simplify dense paths added to the graph.

boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_

Access to the weights of each Edge.

boost::property_map<Graph, vertex_color_t>::type colorProperty_

Access to the colors for the vertices.

boost::property_map<Graph, vertex_interface_data_t>::type interfaceDataProperty_

Access to the interface pair information for the vertices.

boost::disjoint_sets<boost::property_map<Graph, boost::vertex_rank_t>::type, boost::property_map<Graph, boost::vertex_predecessor_t>::type> disjointSets_

Data structure that maintains the connected components.

RNG rng_

Random number generator.

bool addedSolution_ = {false}

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

unsigned int consecutiveFailures_ = {0}

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

double sparseDelta_ = {0.}

Maximum visibility range for nodes in the graph.

double denseDelta_ = {0.}

Maximum range for allowing two samples to support an interface.

mutable std::mutex graphMutex_

Mutex to guard access to the Graph member (g_)

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.

struct InterfaceData

Interface information storage class, which does bookkeeping for criterion four.

Public Functions

InterfaceData() = default

Constructor.

inline void clear(const base::SpaceInformationPtr &si)

Clears the given interface data.

inline void setFirst(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)

Sets information for the first interface (i.e. interface with smaller index vertex).

inline void setSecond(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)

Sets information for the second interface (i.e. interface with larger index vertex).

Public Members

base::State *pointA_ = {nullptr}

States which lie inside the visibility region of a vertex and support an interface.

base::State *pointB_ = {nullptr}
base::State *sigmaA_ = {nullptr}

States which lie just outside the visibility region of a vertex and support an interface.

base::State *sigmaB_ = {nullptr}
double d_ = {std::numeric_limits<double>::infinity()}

Last known distance between the two interfaces supported by points_ and sigmas.

struct vertex_color_t

Public Types

using kind = boost::vertex_property_tag
struct vertex_interface_data_t

Public Types

using kind = boost::vertex_property_tag
struct vertex_state_t

Public Types

using kind = boost::vertex_property_tag