Loading...
Searching...
No Matches
QuotientSpaceGraph.h
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, University of Stuttgart
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the University of Stuttgart nor the names
18 * of its contributors may be used to endorse or promote products
19 * derived from this software without specific prior written
20 * permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35
36/* Author: Andreas Orthey */
37
38#ifndef OMPL_GEOMETRIC_PLANNERS_QUOTIENTSPACE_QUOTIENTGRAPH_
39#define OMPL_GEOMETRIC_PLANNERS_QUOTIENTSPACE_QUOTIENTGRAPH_
40
41#include "QuotientSpace.h"
42#include <limits>
43#include <ompl/geometric/planners/PlannerIncludes.h>
44#include <ompl/datastructures/NearestNeighbors.h>
45#include <ompl/base/Cost.h>
46#include <ompl/datastructures/PDF.h>
47#include <ompl/base/spaces/RealVectorStateSpace.h>
48#include <boost/pending/disjoint_sets.hpp>
49#include <boost/graph/graph_traits.hpp>
50#include <boost/graph/adjacency_list.hpp>
51#include <boost/graph/random.hpp>
52#include <boost/graph/subgraph.hpp>
53#include <boost/graph/properties.hpp>
54#include <boost/random/linear_congruential.hpp>
55#include <boost/random/variate_generator.hpp>
56
57namespace ompl
58{
59 namespace base
60 {
61 const double dInf = std::numeric_limits<double>::infinity();
62 OMPL_CLASS_FORWARD(OptimizationObjective);
63 } // namespace base
64 namespace geometric
65 {
68 {
69 using BaseT = QuotientSpace;
70
71 public:
72 using normalized_index_type = int;
73
76 {
77 public:
78 Configuration() = delete;
81 ompl::base::State *state{nullptr};
82 unsigned int total_connection_attempts{0};
83 unsigned int successful_connection_attempts{0};
84 bool on_shortest_path{false};
85
89 void setPDFElement(void *element_)
90 {
91 pdf_element = element_;
92 }
93 void *getPDFElement()
94 {
95 return pdf_element;
96 }
97
98 bool isStart{false};
99 bool isGoal{false};
100
106 normalized_index_type index{-1};
107 };
108
111 {
112 public:
113 EdgeInternalState() = default;
114 EdgeInternalState(ompl::base::Cost cost_) : cost(cost_){};
116 {
117 cost = eis.cost;
118 }
119 void setWeight(double d)
120 {
121 cost = ompl::base::Cost(d);
122 }
123 ompl::base::Cost getCost()
124 {
125 return cost;
126 }
127
128 private:
129 ompl::base::Cost cost{+ompl::base::dInf};
130 };
131
133 {
134 std::string name{"quotient_graph"};
135 };
137 using Graph = boost::adjacency_list<boost::vecS,
138 boost::vecS,
139 boost::undirectedS,
143 >;
144
145 using BGT = boost::graph_traits<Graph>;
146 using Vertex = BGT::vertex_descriptor;
147 using Edge = BGT::edge_descriptor;
148 using VertexIndex = BGT::vertices_size_type;
149 using IEIterator = BGT::in_edge_iterator;
150 using OEIterator = BGT::out_edge_iterator;
151 // typedef Vertex *VertexParent;
152 // typedef VertexIndex *VertexRank;
153 using VertexParent = Vertex;
154 using VertexRank = VertexIndex;
155 using RoadmapNeighborsPtr = std::shared_ptr<NearestNeighbors<Configuration *>>;
158
159 public:
162
163 virtual unsigned int getNumberOfVertices() const;
164 virtual unsigned int getNumberOfEdges() const;
165
166 virtual bool sampleQuotient(ompl::base::State *) override;
167 virtual bool getSolution(ompl::base::PathPtr &solution) override;
168
171 virtual void getPlannerData(ompl::base::PlannerData &data) const override;
172
175 virtual double getImportance() const override;
176
179 void init();
180
181 virtual void setup() override;
182 virtual void clear() override;
183 void clearQuery() override;
184 virtual void clearVertices();
185 virtual void deleteConfiguration(Configuration *q);
186
187 template <template <typename T> class NN>
188 void setNearestNeighbors();
189 void uniteComponents(Vertex m1, Vertex m2);
190 bool sameComponent(Vertex m1, Vertex m2);
191 std::map<Vertex, VertexRank> vrank;
192 std::map<Vertex, Vertex> vparent;
193 boost::disjoint_sets<boost::associative_property_map<std::map<Vertex, VertexRank>>,
194 boost::associative_property_map<std::map<Vertex, Vertex>>>
195 disjointSets_{boost::make_assoc_property_map(vrank), boost::make_assoc_property_map(vparent)};
196
197 const Configuration *nearest(const Configuration *s) const;
198
199 ompl::base::Cost bestCost_{+ompl::base::dInf};
200 Configuration *qStart_{nullptr};
201 Configuration *qGoal_{nullptr};
202 Vertex vStart_;
203 Vertex vGoal_;
204 std::vector<Vertex> shortestVertexPath_;
205 std::vector<Vertex> startGoalVertexPath_;
206
207 const Graph &getGraph() const;
208 double getGraphLength() const;
209 const RoadmapNeighborsPtr &getRoadmapNeighborsPtr() const;
210
211 virtual void print(std::ostream &out) const override;
213 void printConfiguration(const Configuration *) const;
214
215 protected:
216 virtual double distance(const Configuration *a, const Configuration *b) const;
217
218 virtual Vertex addConfiguration(Configuration *q);
219 void addEdge(const Vertex a, const Vertex b);
220
221 ompl::base::Cost costHeuristic(Vertex u, Vertex v) const;
222
224 ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal);
225
227 RoadmapNeighborsPtr nearestDatastructure_;
228 Graph graph_;
229 ompl::base::PathPtr solutionPath_;
230 RNG rng_;
231 using RNGType = boost::minstd_rand;
232 RNGType rng_boost;
233
236 double graphLength_{0.0};
237 };
238 } // namespace geometric
239} // namespace ompl
240
241#endif
A class that will hold data contained in the PDF.
Definition PDF.h:53
A container that supports probabilistic sampling over weighted data.
Definition PDF.h:49
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
A shared pointer wrapper for ompl::base::Path.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition State.h:50
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
void * pdf_element
Element of Probability Density Function (needed to update probability)
A graph on a quotient-space.
virtual void getPlannerData(ompl::base::PlannerData &data) const override
Return plannerdata structure, whereby each vertex is marked depending to which component it belongs (...
ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal)
Shortest path on quotient-graph.
virtual double getImportance() const override
Importance of quotient-space depending on number of vertices in quotient-graph.
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
virtual void print(std::ostream &out) const override
Internal function implementing actual printing to stream.
void printConfiguration(const Configuration *) const
Print configuration to std::cout.
void init()
Initialization methods for the first iteration (adding start configuration and doing sanity checks)
double graphLength_
Length of graph (useful for determing importance of quotient-space.
RoadmapNeighborsPtr nearestDatastructure_
Nearest neighbor structure for quotient space configurations.
void clearQuery() override
Clears internal datastructures of any query-specific information from the previous query....
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, Configuration *, EdgeInternalState, GraphBundle > Graph
A quotient-graph structure using boost::adjacency_list bundles.
A single quotient-space.
QuotientSpace(const ompl::base::SpaceInformationPtr &si, QuotientSpace *parent_=nullptr)
Quotient Space contains three OMPL spaces, which we call Q1, Q0 and X1.
Main namespace. Contains everything in this library.