Robotics Library  0.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
Rrt.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2009, Markus Rickert
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 // * Neither the name of the Technische Universitaet Muenchen nor the names of
14 // its contributors may be used to endorse or promote products derived from
15 // this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #ifndef _RL_PLAN_RRT_H_
31 #define _RL_PLAN_RRT_H_
32 
33 #include <boost/graph/adjacency_list.hpp>
34 #include <CGAL/Search_traits.h>
35 
36 #include "MatrixPtr.h"
38 #include "Planner.h"
39 #include "TransformPtr.h"
40 #include "VectorPtr.h"
41 
42 namespace rl
43 {
44  namespace plan
45  {
46  class Model;
47  class Sampler;
48  class Verifier;
49 
53  class Rrt : public Planner
54  {
55  public:
56  Rrt(const ::std::size_t& trees = 1);
57 
58  virtual ~Rrt();
59 
60  virtual ::std::string getName() const;
61 
62  virtual ::std::size_t getNumEdges() const;
63 
64  virtual ::std::size_t getNumVertices() const;
65 
66  virtual void getPath(VectorList& path);
67 
68  virtual void reset();
69 
70  virtual bool solve();
71 
74 
77 
79  bool kd;
80 
82 
83  protected:
84  struct VertexBundle
85  {
86  ::std::size_t index;
87 
89 
91 
93  };
94 
95  struct TreeBundle;
96 
97  typedef ::boost::adjacency_list<
98  ::boost::listS,
99  ::boost::listS,
100  ::boost::bidirectionalS,
101  VertexBundle,
102  ::boost::no_property,
103  TreeBundle
104  > Tree;
105 
106  typedef ::boost::adjacency_list_traits<
107  ::boost::listS,
108  ::boost::listS,
109  ::boost::bidirectionalS,
110  ::boost::listS
111  >::vertex_descriptor Vertex;
112 
113  typedef ::std::pair< const ::rl::math::Vector*, Vertex > QueryItem;
114 
116  {
118 
120 
121  const ::rl::math::Real* operator()(const QueryItem& p, const int&) const;
122  };
123 
124  struct Distance
125  {
127 
128  Distance();
129 
130  Distance(Model* model);
131 
132  template< typename SearchTraits > ::rl::math::Real max_distance_to_rectangle(const Query_item& q, const ::CGAL::Kd_tree_rectangle< SearchTraits >& r) const;
133 
134  template< typename SearchTraits > ::rl::math::Real min_distance_to_rectangle(const Query_item& q, const ::CGAL::Kd_tree_rectangle< SearchTraits >& r) const;
135 
136  ::rl::math::Real min_distance_to_rectangle(const ::rl::math::Real& q, const ::rl::math::Real& min, const ::rl::math::Real& max, const ::std::size_t& cutting_dimension) const;
137 
138  ::rl::math::Real new_distance(const ::rl::math::Real& dist, const ::rl::math::Real& old_off, const ::rl::math::Real& new_off, const int& cutting_dimension) const;
139 
141 
142  ::rl::math::Real transformed_distance(const Query_item& q1, const Query_item& q2) const;
143 
145  };
146 
147  typedef ::CGAL::Search_traits< ::rl::math::Real, QueryItem, const ::rl::math::Real*, CartesianIterator > SearchTraits;
148 
150 
151  typedef NeighborSearch::Tree NeighborSearchTree;
152 
153  typedef ::boost::shared_ptr< NeighborSearchTree > NeighborSearchTreePtr;
154 
155  typedef ::std::vector< NeighborSearchTreePtr > NearestNeighbors;
156 
157  struct TreeBundle
158  {
160  };
161 
162  typedef ::boost::graph_traits< Tree >::edge_descriptor Edge;
163 
164  typedef ::boost::graph_traits< Tree >::edge_iterator EdgeIterator;
165 
166  typedef ::std::pair< EdgeIterator, EdgeIterator > EdgeIteratorPair;
167 
168  typedef ::boost::graph_traits< Tree >::vertex_iterator VertexIterator;
169 
170  typedef ::std::pair< VertexIterator, VertexIterator > VertexIteratorPair;
171 
172  typedef ::std::pair< Vertex, ::rl::math::Real > Neighbor;
173 
174  virtual Edge addEdge(const Vertex& u, const Vertex& v, Tree& tree);
175 
176  void addPoint(NearestNeighbors& nn, const QueryItem& p);
177 
178  Vertex addVertex(Tree& tree, const VectorPtr& q);
179 
181 
182  virtual void choose(::rl::math::Vector& chosen);
183 
184  virtual Vertex connect(Tree& tree, const Neighbor& nearest, const ::rl::math::Vector& chosen);
185 
186  virtual Vertex extend(Tree& tree, const Neighbor& nearest, const ::rl::math::Vector& chosen);
187 
188  virtual Neighbor nearest(const Tree& tree, const ::rl::math::Vector& chosen);
189 
190  ::std::vector< Vertex > begin;
191 
192  ::std::vector< Vertex > end;
193 
194  ::std::vector< Tree > tree;
195 
196  private:
197 
198  };
199  }
200 }
201 
202 #endif // _RL_PLAN_RRT_H_