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 
57 namespace 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  {
67  class QuotientSpaceGraph : public QuotientSpace
68  {
69  using BaseT = QuotientSpace;
70 
71  public:
72  using normalized_index_type = int;
73 
75  class Configuration
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 
88  void *pdf_element;
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 
110  class EdgeInternalState
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 
132  struct GraphBundle
133  {
134  std::string name{"quotient_graph"};
135  };
137  using Graph = boost::adjacency_list<boost::vecS,
138  boost::vecS,
139  boost::undirectedS,
140  Configuration *,
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 *>>;
157  using PDF_Element = PDF::Element;
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
void * pdf_element
Element of Probability Density Function (needed to update probability)
RoadmapNeighborsPtr nearestDatastructure_
Nearest neighbor structure for quotient space configurations.
A shared pointer wrapper for ompl::base::Path.
A shared pointer wrapper for ompl::base::SpaceInformation.
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition of an abstract state.
Definition: State.h:114
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...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:112
void clearQuery() override
Clears internal datastructures of any query-specific information from the previous query....
void init()
Initialization methods for the first iteration (adding start configuration and doing sanity checks)
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:81
void printConfiguration(const Configuration *) const
Print configuration to std::cout.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:239
ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal)
Shortest path on quotient-graph.
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
A graph on a quotient-space.
double graphLength_
Length of graph (useful for determing importance of quotient-space.
virtual void print(std::ostream &out) const override
Internal function implementing actual printing to stream.
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.
A class that will hold data contained in the PDF.
Definition: PDF.h:117
virtual void getPlannerData(ompl::base::PlannerData &data) const override
Return plannerdata structure, whereby each vertex is marked depending to which component it belongs (...
Main namespace. Contains everything in this library.
Definition: AppBase.h:22