Class BundleSpaceGraph

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Derived Types

Class Documentation

class BundleSpaceGraph : public ompl::multilevel::BundleSpace

A graph on a Bundle-space.

Subclassed by ompl::multilevel::QMPImpl, ompl::multilevel::QRRTImpl, ompl::multilevel::QRRTStarImpl

Public Types

using normalized_index_type = int
using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, Configuration*, EdgeInternalState, GraphMetaData>

A Bundle-graph structure using boost::adjacency_list bundles.

https://www.boost.org/doc/libs/1_71_0/libs/graph/doc/adjacency_list.html

using BGT = boost::graph_traits<Graph>
using Vertex = BGT::vertex_descriptor
using Edge = BGT::edge_descriptor
using VertexIndex = BGT::vertices_size_type
using IEIterator = BGT::in_edge_iterator
using OEIterator = BGT::out_edge_iterator
using VertexParent = Vertex
using VertexRank = VertexIndex
using RoadmapNeighborsPtr = std::shared_ptr<NearestNeighbors<Configuration*>>
using PDF = ompl::PDF<Configuration*>
using PDF_Element = PDF::Element

Public Functions

BundleSpaceGraph(const ompl::base::SpaceInformationPtr &si, BundleSpace *parent = nullptr)
virtual ~BundleSpaceGraph()
virtual unsigned int getNumberOfVertices() const
virtual unsigned int getNumberOfEdges() const
Vertex nullVertex() const
virtual void grow() override = 0

Perform an iteration of the planner.

virtual void sampleFromDatastructure(ompl::base::State*) override
virtual void sampleBundleGoalBias(ompl::base::State *xRandom)
virtual bool getSolution(ompl::base::PathPtr &solution) override

Return best solution.

virtual ompl::base::PathPtr &getSolutionPathByReference()
virtual void getPlannerData(ompl::base::PlannerData &data) const override

Return plannerdata structure, whereby each vertex is marked depending to which component it belongs (start/goal/non-connected)

void getPlannerDataGraph(ompl::base::PlannerData &data, const Graph &graph, const Vertex vStart) const
virtual double getImportance() const override

Importance of Bundle-space depending on number of vertices in Bundle-graph.

virtual void init()

Initialization methods for the first iteration (adding start configuration and doing sanity checks)

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.

virtual void clear() override

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

virtual void clearVertices()
virtual void deleteConfiguration(Configuration *q)
template<template<typename T> class NN>
void setNearestNeighbors()
void uniteComponents(Vertex m1, Vertex m2)
bool sameComponent(Vertex m1, Vertex m2)
virtual const Configuration *nearest(const Configuration *s) const
virtual void setMetric(const std::string &sMetric) override
virtual void setPropagator(const std::string &sPropagator) override
virtual void setImportance(const std::string &sImportance)
virtual void setGraphSampler(const std::string &sGraphSampler)
virtual void setFindSectionStrategy(FindSectionType type)
BundleSpaceGraphSamplerPtr getGraphSampler()
virtual Graph &getGraphNonConst()

Get underlying boost graph representation (non const)

virtual const Graph &getGraph() const

Get underlying boost graph representation.

const RoadmapNeighborsPtr &getRoadmapNeighborsPtr() const
virtual void print(std::ostream &out) const override

Print class to ostream.

void writeToGraphviz(std::string filename) const

Write class to graphviz.

virtual void printConfiguration(const Configuration*) const

Print configuration to std::cout.

void setGoalBias(double goalBias)
double getGoalBias() const
void setRange(double distance)
double getRange() const
ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal)

Shortest path on Bundle-graph.

ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal, Graph &graph)
virtual double distance(const Configuration *a, const Configuration *b) const

Distance between two configurations using the current metric.

virtual bool checkMotion(const Configuration *a, const Configuration *b) const

Check if we can move from configuration a to configuration b using the current metric.

bool connect(const Configuration *from, const Configuration *to)

Try to connect configuration a to configuration b using the current metric.

Configuration *steerTowards(const Configuration *from, const Configuration *to)

Steer system at Configuration *from to Configuration *to.

Configuration *steerTowards_Range(const Configuration *from, Configuration *to)

Steer system at Configuration *from to Configuration *to, stopping if maxdistance is reached.

Configuration *extendGraphTowards_Range(const Configuration *from, Configuration *to)

Steer system at Configuration *from to Configuration *to while system is valid, stopping if maxDistance is reached.

virtual void interpolate(const Configuration *a, const Configuration *b, Configuration *dest) const

Interpolate from configuration a to configuration b and store results in dest.

virtual Configuration *addBundleConfiguration(base::State*)

Add ompl::base::State to graph. Return its configuration.

virtual Vertex addConfiguration(Configuration *q)

Add configuration to graph. Return its vertex in boost graph.

void addGoalConfiguration(Configuration *x)

Add configuration to graph as goal vertex.

virtual void addBundleEdge(const Configuration *a, const Configuration *b)

Add edge between configuration a and configuration b to graph.

virtual const std::pair<Edge, bool> addEdge(const Vertex a, const Vertex b)

Add edge between Vertex a and Vertex b to graph.

virtual Vertex getStartIndex() const

Get vertex representing the start.

virtual Vertex getGoalIndex() const

Get vertex representing the goal.

virtual void setStartIndex(Vertex)

Set vertex representing the start.

virtual bool findSection() override

Call algorithm to solve the find section problem.

Public Members

std::map<Vertex, VertexRank> vrank
std::map<Vertex, Vertex> vparent
boost::disjoint_sets<boost::associative_property_map<std::map<Vertex, VertexRank>>, boost::associative_property_map<std::map<Vertex, Vertex>>> disjointSets_ = {boost::make_assoc_property_map(vrank), boost::make_assoc_property_map(vparent)}
base::Cost bestCost_ = {+base::dInf}

Best cost found so far by algorithm.

std::vector<Vertex> shortestVertexPath_

Protected Types

using RNGType = boost::minstd_rand

Protected Functions

ompl::base::Cost costHeuristic(Vertex u, Vertex v) const

Protected Attributes

ompl::base::PathPtr solutionPath_
Vertex vStart_
RoadmapNeighborsPtr nearestDatastructure_

Nearest neighbor structure for Bundle space configurations.

Graph graph_
unsigned int numVerticesWhenComputingSolutionPath_ = {0}
RNG rng_
RNGType rng_boost
double graphLength_ = {0.0}

Length of graph (useful for determing importance of Bundle-space.

double maxDistance_ = {-1.0}

Maximum expansion step.

double goalBias_ = {.1}

Goal bias.

Configuration *xRandom_ = {nullptr}

Temporary random configuration.

BundleSpaceImportancePtr importanceCalculator_ = {nullptr}

Pointer to strategy to compute importance of this bundle space (which is used to decide which bundle space to grow next)

BundleSpaceGraphSamplerPtr graphSampler_ = {nullptr}

Pointer to strategy to sample from graph.

PathRestrictionPtr pathRestriction_ = {nullptr}

Pointer to current path restriction (the set of points which project onto the best cost path on the base space if any). This only exists if there exists a base space and there exists a base space path.

ompl::geometric::PathSimplifierPtr optimizer_

A path optimizer.

Configuration *qStart_ = {nullptr}

Start configuration.

Configuration *qGoal_ = {nullptr}

The (best) goal configuration.

std::vector<Configuration*> startConfigurations_

List of configurations that satisfy the start condition.

std::vector<Configuration*> goalConfigurations_

List of configurations that satisfy the goal condition.

class Configuration

A configuration in Bundle-space.

Public Functions

Configuration() = delete
Configuration(const ompl::base::SpaceInformationPtr &si)
Configuration(const ompl::base::SpaceInformationPtr &si, const ompl::base::State *state_)
inline void setPDFElement(void *element_)
inline void *getPDFElement()

Public Members

ompl::base::State *state = {nullptr}
ompl::control::Control *control = {nullptr}
unsigned int total_connection_attempts = {0}
unsigned int successful_connection_attempts = {0}
bool on_shortest_path = {false}
void *pdf_element

Element of Probability Density Function (needed to update probability)

bool isStart = {false}
bool isGoal = {false}
Configuration *parent = {nullptr}

parent index for {qrrt*}

base::Cost cost

cost to reach until current vertex in {qrrt*}

base::Cost lineCost

same as rrt*, connection cost with parent {qrrt*}

std::vector<Configuration*> children

The set of motions descending from the current motion {qrrt*}.

normalized_index_type index = {-1}

Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)], but if vertices are deleted or graphs are copied, we sometimes need to map them back to [0, num_vertices(graph)]

normalized_index_type representativeIndex = {-1}

Access to the representatives (Sparse Vertex) of the Dense vertices For Sparse Graph: Store index of Sparse Vertex which is represtative of Dense Graph Vertex.

std::set<normalized_index_type> nonInterfaceIndexList

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

std::unordered_map<normalized_index_type, std::set<normalized_index_type>> interfaceIndexList

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

std::vector<Configuration*> reachableSet
class EdgeInternalState

An edge in Bundle-space.

Public Functions

EdgeInternalState() = default
inline EdgeInternalState(ompl::base::Cost cost_)
inline EdgeInternalState(const EdgeInternalState &eis)
inline EdgeInternalState &operator=(const EdgeInternalState &eis)
inline void setWeight(double d)
inline ompl::base::Cost getCost()
struct GraphMetaData

Public Members

std::string name = {"BundleSpaceGraph"}