Loading...
Searching...
No Matches
BundleSpaceGraph.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, Sohaib Akbar */
37
38#ifndef OMPL_MULTILEVEL_PLANNERS_BUNDLESPACE_BUNDLEGRAPH_
39#define OMPL_MULTILEVEL_PLANNERS_BUNDLESPACE_BUNDLEGRAPH_
40
41#include <ompl/multilevel/datastructures/BundleSpace.h>
42#include <ompl/multilevel/datastructures/pathrestriction/FindSectionTypes.h>
43#include <limits>
44#include <ompl/geometric/planners/PlannerIncludes.h>
45#include <ompl/datastructures/NearestNeighbors.h>
46#include <ompl/base/Cost.h>
47#include <ompl/control/Control.h>
48#include <ompl/datastructures/PDF.h>
49#include <ompl/base/spaces/RealVectorStateSpace.h>
50
51#include <boost/pending/disjoint_sets.hpp>
52#include <boost/graph/graph_traits.hpp>
53#include <boost/graph/adjacency_list.hpp>
54#include <boost/graph/random.hpp>
55// #include <boost/graph/subgraph.hpp> //Note: Everything would be nicer with
56// subgraphs, but there are still some bugs in the boost impl which prevent
57// us to use them here
58#include <boost/graph/properties.hpp>
59#include <boost/random/linear_congruential.hpp>
60#include <boost/random/variate_generator.hpp>
61
62namespace ompl
63{
64 namespace base
65 {
66 const double dInf = std::numeric_limits<double>::infinity();
67 }
68 namespace multilevel
69 {
71
72 OMPL_CLASS_FORWARD(BundleSpaceImportance);
74 OMPL_CLASS_FORWARD(BundleSpaceGraphSampler);
76 OMPL_CLASS_FORWARD(PathRestriction);
78 }
79 namespace geometric
80 {
81 OMPL_CLASS_FORWARD(PathSimplifier);
82 }
83
84 namespace multilevel
85 {
88 {
89 using BaseT = BundleSpace;
90
91 public:
92 using normalized_index_type = int;
93
96 {
97 public:
98 Configuration() = delete;
101
102 ompl::base::State *state{nullptr};
103 ompl::control::Control *control{nullptr};
104
105 unsigned int total_connection_attempts{0};
106 unsigned int successful_connection_attempts{0};
107 bool on_shortest_path{false};
108
112 void setPDFElement(void *element_)
113 {
114 pdf_element = element_;
115 }
116 void *getPDFElement()
117 {
118 return pdf_element;
119 }
120
121 bool isStart{false};
122 bool isGoal{false};
123
126
129
132
134 std::vector<Configuration *> children;
135
140 normalized_index_type index{-1};
141
145 normalized_index_type representativeIndex{-1};
146
148 // boost::property<vertex_list_t, std::set<VertexIndexType>,
149 std::set<normalized_index_type> nonInterfaceIndexList;
150
152 // boost::property<vertex_interface_list_t, std::unordered_map<VertexIndexType,
153 // std::set<VertexIndexType>>>
154 std::unordered_map<normalized_index_type, std::set<normalized_index_type>> interfaceIndexList;
155
156 std::vector<Configuration *> reachableSet;
157 };
158
161 {
162 public:
163 EdgeInternalState() = default;
164 EdgeInternalState(ompl::base::Cost cost_) : cost(cost_){};
166 {
167 cost = eis.cost;
168 }
169 EdgeInternalState& operator=(const EdgeInternalState &eis)
170 {
171 cost = eis.cost;
172 return *this;
173 }
174 void setWeight(double d)
175 {
176 cost = ompl::base::Cost(d);
177 }
178 ompl::base::Cost getCost()
179 {
180 return cost;
181 }
182
183 private:
184 ompl::base::Cost cost{+ompl::base::dInf};
185 };
186
188 {
189 std::string name{"BundleSpaceGraph"};
190 };
191
196 using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, Configuration *,
198 using BGT = boost::graph_traits<Graph>;
199 using Vertex = BGT::vertex_descriptor;
200 using Edge = BGT::edge_descriptor;
201 using VertexIndex = BGT::vertices_size_type;
202 using IEIterator = BGT::in_edge_iterator;
203 using OEIterator = BGT::out_edge_iterator;
204 using VertexParent = Vertex;
205 using VertexRank = VertexIndex;
206 using RoadmapNeighborsPtr = std::shared_ptr<NearestNeighbors<Configuration *>>;
209
210 public:
212 virtual ~BundleSpaceGraph();
213
214 /* \brief Number of vertices on boost graph */
215 virtual unsigned int getNumberOfVertices() const;
216
217 /* \brief Number of edges on boost graph */
218 virtual unsigned int getNumberOfEdges() const;
219
220 /* \brief A null vertex representing a non-existing vertex */
221 Vertex nullVertex() const;
222
223 /* \brief One iteration of the growing the graph */
224 void grow() override = 0;
225
226 /* \brief Sample from the graph (used in restriction sampling for
227 * parent bundle space) */
228 void sampleFromDatastructure(ompl::base::State *) override;
229
230 /* \brief as sampleBundle() but with goal bias */
231 virtual void sampleBundleGoalBias(ompl::base::State *xRandom);
232
233 /* \brief Check that there exist a path on graph */
234 bool getSolution(ompl::base::PathPtr &solution) override;
235
236 /* \brief Return best cost path on graph (as reference) */
237 virtual ompl::base::PathPtr &getSolutionPathByReference();
238
241 void getPlannerData(ompl::base::PlannerData &data) const override;
242
243 /* \brief Given graph, fill in the ompl::base::PlannerData structure */
244 void getPlannerDataGraph(ompl::base::PlannerData &data, const Graph &graph, const Vertex vStart) const;
245
248 double getImportance() const override;
249
252 virtual void init();
253
254 void setup() override;
255 void clear() override;
256
257 /* \brief Delete all vertices and their attached configurations */
258 virtual void clearVertices();
259
260 /* \brief Delete a configuration and free its state */
261 virtual void deleteConfiguration(Configuration *q);
262
263 /* \brief Create nearest neighbors structure */
264 template <template <typename T> class NN>
265 void setNearestNeighbors();
266
267 /* \brief Unite two components */
268 void uniteComponents(Vertex m1, Vertex m2);
269 /* \brief Check if both vertices are in the same component */
270 bool sameComponent(Vertex m1, Vertex m2);
271 std::map<Vertex, VertexRank> vrank;
272 std::map<Vertex, Vertex> vparent;
273 boost::disjoint_sets<boost::associative_property_map<std::map<Vertex, VertexRank>>,
274 boost::associative_property_map<std::map<Vertex, Vertex>>>
275 disjointSets_{boost::make_assoc_property_map(vrank), boost::make_assoc_property_map(vparent)};
276
277 /* \brief Get nearest configuration to configuration s */
278 virtual const Configuration *nearest(const Configuration *s) const;
279
280 /* \brief Set metric to be used for growing graph */
281 void setMetric(const std::string &sMetric) override;
282 void setPropagator(const std::string &sPropagator) override;
283 virtual void setImportance(const std::string &sImportance);
284 virtual void setGraphSampler(const std::string &sGraphSampler);
285
286 /* \brief Set strategy to solve the find section problem */
287 virtual void setFindSectionStrategy(FindSectionType type);
288
289 /* \brief Get current graph sampler */
290 BundleSpaceGraphSamplerPtr getGraphSampler();
291
293 base::Cost bestCost_{+base::dInf};
294
295 /* \brief Best cost path on graph as vertex representation */
296 std::vector<Vertex> shortestVertexPath_;
297
299 virtual Graph &getGraphNonConst();
301 virtual const Graph &getGraph() const;
302
303 const RoadmapNeighborsPtr &getRoadmapNeighborsPtr() const;
304
306 void print(std::ostream &out) const override;
307
309 void writeToGraphviz(std::string filename) const;
310
312 virtual void printConfiguration(const Configuration *) const;
313
314 void setGoalBias(double goalBias);
315
316 double getGoalBias() const;
317
318 void setRange(double distance);
319
320 double getRange() const;
321
323 ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal);
324 ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal, Graph &graph);
325
328 virtual double distance(const Configuration *a, const Configuration *b) const;
331 virtual bool checkMotion(const Configuration *a, const Configuration *b) const;
332
335 bool connect(const Configuration *from, const Configuration *to);
336
339 Configuration *steerTowards(const Configuration *from, const Configuration *to);
340
344
348
351 virtual void interpolate(const Configuration *a, const Configuration *b, Configuration *dest) const;
352
355
358 virtual Vertex addConfiguration(Configuration *q);
361
363 virtual void addBundleEdge(const Configuration *a, const Configuration *b);
364
366 virtual const std::pair<Edge, bool> addEdge(const Vertex a, const Vertex b);
367
369 virtual Vertex getStartIndex() const;
371 virtual Vertex getGoalIndex() const;
373 virtual void setStartIndex(Vertex);
374
376 bool findSection() override;
377
378 protected:
379 ompl::base::PathPtr solutionPath_;
380
381 Vertex vStart_;
382
383 ompl::base::Cost costHeuristic(Vertex u, Vertex v) const;
384
386 RoadmapNeighborsPtr nearestDatastructure_;
387 Graph graph_;
388 unsigned int numVerticesWhenComputingSolutionPath_{0};
389 RNG rng_;
390 using RNGType = boost::minstd_rand;
391 RNGType rng_boost;
392
395 double graphLength_{0.0};
396
398 double maxDistance_{-1.0};
399
401 double goalBias_{.1};
402
405
409 BundleSpaceImportancePtr importanceCalculator_{nullptr};
410
412 BundleSpaceGraphSamplerPtr graphSampler_{nullptr};
413
418 PathRestrictionPtr pathRestriction_{nullptr};
419
422
425
428
430 std::vector<Configuration *> startConfigurations_;
431
433 std::vector<Configuration *> goalConfigurations_;
434 };
435 } // namespace multilevel
436} // namespace ompl
437
438#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
Definition of an abstract control.
Definition Control.h:48
A shared pointer wrapper for ompl::geometric::PathSimplifier.
void * pdf_element
Element of Probability Density Function (needed to update probability)
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
Configuration * parent
parent index for {qrrt*}
base::Cost cost
cost to reach until current vertex in {qrrt*}
normalized_index_type representativeIndex
Access to the representatives (Sparse Vertex) of the Dense vertices For Sparse Graph: Store index of ...
base::Cost lineCost
same as rrt*, connection cost with parent {qrrt*}
std::unordered_map< normalized_index_type, std::set< normalized_index_type > > interfaceIndexList
Access to the interface-supporting vertice hashes of the sparse nodes.
std::set< normalized_index_type > nonInterfaceIndexList
Access to all non-interface supporting vertices of the sparse nodes.
std::vector< Configuration * > children
The set of motions descending from the current motion {qrrt*}.
A graph on a Bundle-space.
bool connect(const Configuration *from, const Configuration *to)
Try to connect configuration a to configuration b using the current metric.
BundleSpaceGraphSamplerPtr graphSampler_
Pointer to strategy to sample from graph.
bool getSolution(ompl::base::PathPtr &solution) override
Return best solution.
BundleSpaceImportancePtr importanceCalculator_
Pointer to strategy to compute importance of this bundle space (which is used to decide which bundle ...
PathRestrictionPtr pathRestriction_
Pointer to current path restriction (the set of points which project onto the best cost path on the b...
Configuration * extendGraphTowards_Range(const Configuration *from, Configuration *to)
Steer system at Configuration *from to Configuration *to while system is valid, stopping if maxDistan...
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
double maxDistance_
Maximum expansion step.
virtual Vertex addConfiguration(Configuration *q)
Add configuration to graph. Return its vertex in boost graph.
RoadmapNeighborsPtr nearestDatastructure_
Nearest neighbor structure for Bundle space configurations.
Configuration * qGoal_
The (best) goal configuration.
void writeToGraphviz(std::string filename) const
Write class to graphviz.
Configuration * xRandom_
Temporary random configuration.
double getImportance() const override
Importance of Bundle-space depending on number of vertices in Bundle-graph.
Configuration * steerTowards_Range(const Configuration *from, Configuration *to)
Steer system at Configuration *from to Configuration *to, stopping if maxdistance is reached.
virtual const std::pair< Edge, bool > addEdge(const Vertex a, const Vertex b)
Add edge between Vertex a and Vertex b to graph.
ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal)
Shortest path on Bundle-graph.
virtual void setStartIndex(Vertex)
Set vertex representing the start.
Configuration * steerTowards(const Configuration *from, const Configuration *to)
Steer system at Configuration *from to Configuration *to.
std::vector< Configuration * > goalConfigurations_
List of configurations that satisfy the goal condition.
base::Cost bestCost_
Best cost found so far by algorithm.
virtual void init()
Initialization methods for the first iteration (adding start configuration and doing sanity checks)
std::vector< Configuration * > startConfigurations_
List of configurations that satisfy the start condition.
virtual Vertex getStartIndex() const
Get vertex representing the start.
virtual void interpolate(const Configuration *a, const Configuration *b, Configuration *dest) const
Interpolate from configuration a to configuration b and store results in dest.
void grow() override=0
Perform an iteration of the planner.
virtual Configuration * addBundleConfiguration(base::State *)
Add ompl::base::State to graph. Return its configuration.
virtual void printConfiguration(const Configuration *) const
Print configuration to std::cout.
virtual Graph & getGraphNonConst()
Get underlying boost graph representation (non const)
virtual void addBundleEdge(const Configuration *a, const Configuration *b)
Add edge between configuration a and configuration b to 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.
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, Configuration *, EdgeInternalState, GraphMetaData > Graph
A Bundle-graph structure using boost::adjacency_list bundles.
virtual const Graph & getGraph() const
Get underlying boost graph representation.
void addGoalConfiguration(Configuration *x)
Add configuration to graph as goal vertex.
void print(std::ostream &out) const override
Print class to ostream.
ompl::geometric::PathSimplifierPtr optimizer_
A path optimizer.
bool findSection() override
Call algorithm to solve the find section problem.
Configuration * qStart_
Start configuration.
void getPlannerData(ompl::base::PlannerData &data) const override
Return plannerdata structure, whereby each vertex is marked depending to which component it belongs (...
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
double graphLength_
Length of graph (useful for determing importance of Bundle-space.
virtual Vertex getGoalIndex() const
Get vertex representing the goal.
A single Bundle-space.
Definition BundleSpace.h:64
BundleSpace(const ompl::base::SpaceInformationPtr &si, BundleSpace *baseSpace_=nullptr)
Bundle Space contains three primary characters, the bundle space, the base space and the projection.
Main namespace. Contains everything in this library.