QuotientSpaceGraph.cpp
59 ompl::geometric::QuotientSpaceGraph::QuotientSpaceGraph(const base::SpaceInformationPtr &si, QuotientSpace *parent_)
82 nearestDatastructure_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Configuration *>(this));
117 ompl::geometric::QuotientSpaceGraph::Configuration::Configuration(const base::SpaceInformationPtr &si)
121 ompl::geometric::QuotientSpaceGraph::Configuration::Configuration(const base::SpaceInformationPtr &si,
221 ompl::geometric::QuotientSpaceGraph::Vertex ompl::geometric::QuotientSpaceGraph::addConfiguration(Configuration *q)
242 const ompl::geometric::QuotientSpaceGraph::Graph &ompl::geometric::QuotientSpaceGraph::getGraph() const
271 double ompl::geometric::QuotientSpaceGraph::distance(const Configuration *a, const Configuration *b) const
319 ompl::base::PathPtr ompl::geometric::QuotientSpaceGraph::getPath(const Vertex &start, const Vertex &goal)
322 auto weight = boost::make_transform_value_property_map(std::mem_fn(&EdgeInternalState::getCost),
326 boost::astar_search(graph_, start, [this, goal](const Vertex v) { return costHeuristic(v, goal); },
394 << " --[QuotientSpaceGraph has " << getNumberOfVertices() << " vertices and " << getNumberOfEdges()
447 base::PlannerDataVertexAnnotated &v1a = static_cast<base::PlannerDataVertexAnnotated &>(data.getVertex(vi1));
448 base::PlannerDataVertexAnnotated &v2a = static_cast<base::PlannerDataVertexAnnotated &>(data.getVertex(vi2));
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:48
A shared pointer wrapper for ompl::base::Path.
An annotated vertex, adding information about its level in the quotient-space hiearchy,...
Definition: PlannerDataVertexAnnotated.h:52
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition: PlannerData.cpp:413
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition: PlannerData.cpp:422
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
Definition: PlannerData.cpp:212
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Definition: PlannerData.cpp:432
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
Definition: PlannerData.cpp:391
A configuration in quotient-space.
Definition: QuotientSpaceGraph.h:76
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
Definition: QuotientSpaceGraph.h:106
An edge in quotient-space.
Definition: QuotientSpaceGraph.h:111
virtual void getPlannerData(ompl::base::PlannerData &data) const override
Return plannerdata structure, whereby each vertex is marked depending to which component it belongs (...
Definition: QuotientSpaceGraph.cpp:403
ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal)
Shortest path on quotient-graph.
Definition: QuotientSpaceGraph.cpp:319
virtual double getImportance() const override
Importance of quotient-space depending on number of vertices in quotient-graph.
Definition: QuotientSpaceGraph.cpp:160
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: QuotientSpaceGraph.cpp:77
virtual void print(std::ostream &out) const override
Internal function implementing actual printing to stream.
Definition: QuotientSpaceGraph.cpp:390
void printConfiguration(const Configuration *) const
Print configuration to std::cout.
Definition: QuotientSpaceGraph.cpp:398
void init()
Initialization methods for the first iteration (adding start configuration and doing sanity checks)
Definition: QuotientSpaceGraph.cpp:166
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, Configuration *, EdgeInternalState, GraphBundle > Graph
A quotient-graph structure using boost::adjacency_list bundles.
Definition: QuotientSpaceGraph.h:143
void clearQuery() override
Clears internal datastructures of any query-specific information from the previous query....
Definition: QuotientSpaceGraph.cpp:155
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: QuotientSpaceGraph.cpp:106
QuotientSpace * getParent() const
Parent is a more simplified quotient-space (higher in abstraction hierarchy)
Definition: QuotientSpace.cpp:1360