Class SPARSdb

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Class Documentation

class SPARSdb : public ompl::base::Planner

SPArse Roadmap Spanner Version 2.0

This version has been modified for use with Thunder

Short description

SPARSdb 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
enum EdgeCollisionState

Possible collision states of an edge.

Values:

enumerator NOT_CHECKED
enumerator IN_COLLISION
enumerator FREE
using VertexIndexType = unsigned long int

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 VertexProperties = 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, InterfaceHashStruct>>>>>

The underlying roadmap graph.

Properties of vertices*

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. Edges are undirected.

Note: If boost::vecS is not used for vertex storage, then there must also be a boost:vertex_index_t property manually added.

Properties of edges*

  • edge_weight_t - cost/distance between two vertices

  • edge_collision_state_t - used for lazy collision checking, determines if an edge has been checked already for collision. 0 = not checked/unknown, 1 = in collision, 2 = free Wrapper for the vertex’s multiple as its property.

using EdgeProperties = boost::property<boost::edge_weight_t, double, boost::property<edge_collision_state_t, int>>

Wrapper for the double assigned to an edge as its weight property.

using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, VertexProperties, EdgeProperties>

The underlying boost graph type (undirected weighted-edge adjacency list with above properties).

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

Vertex in Graph.

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

Edge in Graph.

using EdgeCollisionStateMap = boost::property_map<Graph, edge_collision_state_t>::type

Access map that stores the lazy collision checking status of each edge.

Public Functions

SPARSdb(const base::SpaceInformationPtr &si)

Constructor.

~SPARSdb() 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.

bool getGuardSpacingFactor(double pathLength, double &numGuards, double &spacingFactor)
bool getGuardSpacingFactor(double pathLength, int &numGuards, double &spacingFactor)

Calculate the distance that should be used in inserting nodes into the db.

Parameters:

  • path – length - from the trajectory

  • num – guards - the output result

  • spacing – factor - what fraction of the sparsedelta should be used in placing guards

Returns:

bool addPathToRoadmap(const base::PlannerTerminationCondition &ptc, ompl::geometric::PathGeometric &solutionPath)
bool checkStartGoalConnection(ompl::geometric::PathGeometric &solutionPath)
bool addStateToRoadmap(const base::PlannerTerminationCondition &ptc, base::State *newState)
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.

bool getSimilarPaths(int nearestK, const base::State *start, const base::State *goal, CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)

Search the roadmap for the best path close to the given start and goal states that is valid.

Parameters:

  • nearestK – - unused

  • start –

  • goal –

  • geometricSolution – - the resulting path

Returns:

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 getNumVertices() const

Get the number of vertices in the sparse roadmap.

inline unsigned int getNumEdges() const

Get the number of edges in the sparse roadmap.

inline unsigned int getNumConnectedComponents() const

Get the number of disjoint sets in the sparse roadmap.

inline unsigned int getNumPathInsertionFailed() const

Get the number of times a path was inserted into the database and it failed to have connectivity.

inline unsigned int getNumConsecutiveFailures() const

description

inline long unsigned int getIterations() const

Get the number of iterations the algorithm performed.

bool convertVertexPathToStatePath(std::vector<Vertex> &vertexPath, const base::State *actualStart, const base::State *actualGoal, CandidateSolution &candidateSolution, bool disableCollisionWarning = false)

Convert astar results to correctly ordered path.

Parameters:

  • vertexPath – - in reverse

  • start – - actual start that is probably not included in new path

  • goal – - actual goal that is probably not included in new path

  • path – - returned solution

  • disableCollisionWarning – - if the func should ignore edges that are not checked

Returns:

true on success

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 setPlannerData(const base::PlannerData &data)

Set the sparse graph from file.

Parameters:

a – pre-built graph

bool reachedFailureLimit() const

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

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

Print debug information about planner.

void clearEdgeCollisionStates()

Clear all past edge state information about in collision or not.

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 *state, std::vector<Vertex> &graphNeighborhood, std::vector<Vertex> &visibleNeighborhood)

Finds visible nodes in the graph near state.

bool findGraphNeighbors(const base::State *state, std::vector<Vertex> &graphNeighborhood)

Finds nodes in the graph near state NOTE: note tested for visibility.

Parameters:

  • state – - vertex to find neighbors around

  • result –

Returns:

false is no neighbors found

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 *workState, 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 getPaths(const std::vector<Vertex> &candidateStarts, const std::vector<Vertex> &candidateGoals, const base::State *actualStart, const base::State *actualGoal, CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)

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 lazyCollisionSearch(const Vertex &start, const Vertex &goal, const base::State *actualStart, const base::State *actualGoal, CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)

Repeatidly search through graph for connection then check for collisions then repeat.

Returns:

true if a valid path is found

bool lazyCollisionCheck(std::vector<Vertex> &vertexPath, const base::PlannerTerminationCondition &ptc)

Check recalled path for collision and disable as needed.

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

Thread that checks for solution

bool constructSolution(Vertex start, Vertex goal, std::vector<Vertex> &vertexPath) const

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

Parameters:

  • start –

  • goal –

  • vertexPath –

Returns:

true if candidate solution found

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)

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_ = {5000u}

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

unsigned int numPathInsertionFailures_ = {0u}

Track how many solutions fail to have connectivity at end.

unsigned int nearSamplePoints_

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

PathSimplifierPtr psimp_

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

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

Access to the weights of each Edge.

EdgeCollisionStateMap edgeCollisionStateProperty_

Access to the collision checking state of each Edge.

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

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

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_ = {0u}

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

long unsigned int iterations_ = {0ul}

A counter for the number of 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.

std::vector<Vertex> startVertexCandidateNeighbors_

Used by getSimilarPaths.

std::vector<Vertex> goalVertexCandidateNeighbors_
bool verbose_ = {false}

Option to enable debugging output.

struct CandidateSolution

Struct for passing around partially solved solutions.

Public Functions

inline std::size_t getStateCount()
inline ompl::geometric::PathGeometric &getGeometricPath()

Public Members

bool isApproximate_
base::PathPtr path_
std::vector<EdgeCollisionState> edgeCollisionStatus_
class CustomVisitor : public AStarVisitorConcept, public boost::default_astar_visitor

Vertex visitor to check if A* search is finished.

Public Functions

CustomVisitor(Vertex goal)

Construct a visitor for a given search.

Parameters:

goal – goal vertex of the search

void examine_vertex(Vertex u, const Graph &g) const

Check if we have arrived at the goal.

Parameters:

  • u – current vertex

  • g – graph we are searching on

Throws:

foundGoalException – if u is the goal

struct edge_collision_state_t

Public Types

using kind = boost::edge_property_tag
class edgeWeightMap : public ReadablePropertyMapConcept

Used to artifically supress edges during A* search.

Public Types

using key_type = Edge

Map key type.

using value_type = double

Map value type.

using reference = double&

Map auxiliary value type.

using category = boost::readable_property_map_tag

Map type.

Public Functions

edgeWeightMap(const Graph &graph, const EdgeCollisionStateMap &collisionStates)

Construct map for certain constraints.

Parameters:

graph – Graph to use

double get(Edge e) const

Get the weight of an edge.

Parameters:

e – the edge

Returns:

infinity if e lies in a forbidden neighborhood; actual weight of e otherwise

class foundGoalException

Thrown to stop the A* search when finished.

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 InterfaceHashStruct

Public Members

InterfaceHash interfaceHash
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