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 
62 namespace 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  {
87  class BundleSpaceGraph : public BundleSpace
88  {
89  using BaseT = BundleSpace;
90 
91  public:
92  using normalized_index_type = int;
93 
95  class Configuration
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 
111  void *pdf_element;
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 
125  Configuration *parent{nullptr};
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 
160  class EdgeInternalState
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  }
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 *>>;
208  using PDF_Element = PDF::Element;
209 
210  public:
211  BundleSpaceGraph(const ompl::base::SpaceInformationPtr &si, BundleSpace *parent = nullptr);
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 
404  Configuration *xRandom_{nullptr};
405 
409  BundleSpaceImportancePtr importanceCalculator_{nullptr};
410 
412  BundleSpaceGraphSamplerPtr graphSampler_{nullptr};
413 
418  PathRestrictionPtr pathRestriction_{nullptr};
419 
422 
424  Configuration *qStart_{nullptr};
425 
427  Configuration *qGoal_{nullptr};
428 
430  std::vector<Configuration *> startConfigurations_;
431 
433  std::vector<Configuration *> goalConfigurations_;
434  };
435  } // namespace multilevel
436 } // namespace ompl
437 
438 #endif
double getImportance() const override
Importance of Bundle-space depending on number of vertices in Bundle-graph.
double maxDistance_
Maximum expansion step.
Configuration * qStart_
Start configuration.
A shared pointer wrapper for ompl::base::Path.
Definition of an abstract control.
Definition: Control.h:111
A shared pointer wrapper for ompl::base::SpaceInformation.
BundleSpace(const ompl::base::SpaceInformationPtr &si, BundleSpace *baseSpace_=nullptr)
Bundle Space contains three primary characters, the bundle space, the base space and the projection.
Definition: BundleSpace.cpp:60
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
base::Cost cost
cost to reach until current vertex in {qrrt*}
ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal)
Shortest path on Bundle-graph.
normalized_index_type representativeIndex
Access to the representatives (Sparse Vertex) of the Dense vertices For Sparse Graph: Store index of ...
void getPlannerData(ompl::base::PlannerData &data) const override
Return plannerdata structure, whereby each vertex is marked depending to which component it belongs (...
bool connect(const Configuration *from, const Configuration *to)
Try to connect configuration a to configuration b using the current metric.
A shared pointer wrapper for ompl::geometric::PathSimplifier.
std::vector< Configuration * > goalConfigurations_
List of configurations that satisfy the goal condition.
std::unordered_map< normalized_index_type, std::set< normalized_index_type > > interfaceIndexList
Access to the interface-supporting vertice hashes of the sparse nodes.
Definition of an abstract state.
Definition: State.h:113
Configuration * steerTowards_Range(const Configuration *from, Configuration *to)
Steer system at Configuration *from to Configuration *to, stopping if maxdistance is reached.
virtual void setStartIndex(Vertex)
Set vertex representing the start.
virtual Graph & getGraphNonConst()
Get underlying boost graph representation (non const)
Configuration * extendGraphTowards_Range(const Configuration *from, Configuration *to)
Steer system at Configuration *from to Configuration *to while system is valid, stopping if maxDistan...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
bool findSection() override
Call algorithm to solve the find section problem.
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.
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:80
std::vector< Configuration * > startConfigurations_
List of configurations that satisfy the start condition.
std::vector< Configuration * > children
The set of motions descending from the current motion {qrrt*}.
virtual Vertex getGoalIndex() const
Get vertex representing the goal.
A graph on a Bundle-space.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
std::set< normalized_index_type > nonInterfaceIndexList
Access to all non-interface supporting vertices of the sparse nodes.
base::Cost lineCost
same as rrt*, connection cost with parent {qrrt*}
virtual void interpolate(const Configuration *a, const Configuration *b, Configuration *dest) const
Interpolate from configuration a to configuration b and store results in dest.
PathRestrictionPtr pathRestriction_
Pointer to current path restriction (the set of points which project onto the best cost path on the b...
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, Configuration *, EdgeInternalState, GraphMetaData > Graph
A Bundle-graph structure using boost::adjacency_list bundles.
Configuration * qGoal_
The (best) goal configuration.
ompl::geometric::PathSimplifierPtr optimizer_
A path optimizer.
void writeToGraphviz(std::string filename) const
Write class to graphviz.
void * pdf_element
Element of Probability Density Function (needed to update probability)
virtual void init()
Initialization methods for the first iteration (adding start configuration and doing sanity checks)
bool getSolution(ompl::base::PathPtr &solution) override
Return best solution.
virtual Vertex addConfiguration(Configuration *q)
Add configuration to graph. Return its vertex in boost graph.
double graphLength_
Length of graph (useful for determing importance of Bundle-space.
void addGoalConfiguration(Configuration *x)
Add configuration to graph as goal vertex.
Configuration * xRandom_
Temporary random configuration.
BundleSpaceImportancePtr importanceCalculator_
Pointer to strategy to compute importance of this bundle space (which is used to decide which bundle ...
Configuration * parent
parent index for {qrrt*}
void grow() override=0
Perform an iteration of the planner.
RoadmapNeighborsPtr nearestDatastructure_
Nearest neighbor structure for Bundle space configurations.
A single Bundle-space.
Definition: BundleSpace.h:131
virtual void addBundleEdge(const Configuration *a, const Configuration *b)
Add edge between configuration a and configuration b to graph.
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.
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
Configuration * steerTowards(const Configuration *from, const Configuration *to)
Steer system at Configuration *from to Configuration *to.
BundleSpaceGraphSamplerPtr graphSampler_
Pointer to strategy to sample from graph.
base::Cost bestCost_
Best cost found so far by algorithm.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
void print(std::ostream &out) const override
Print class to ostream.
virtual Vertex getStartIndex() const
Get vertex representing the start.
virtual double distance(const Configuration *a, const Configuration *b) const
Distance between two configurations using the current metric.
A class that will hold data contained in the PDF.
Definition: PDF.h:116
virtual const Graph & getGraph() const
Get underlying boost graph representation.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
virtual const std::pair< Edge, bool > addEdge(const Vertex a, const Vertex b)
Add edge between Vertex a and Vertex b to graph.