All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
EST.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
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 Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ryan Luna */
36 
37 #ifndef OMPL_CONTROL_PLANNERS_EST_EST_
38 #define OMPL_CONTROL_PLANNERS_EST_EST_
39 
40 #include "ompl/datastructures/Grid.h"
41 #include "ompl/control/planners/PlannerIncludes.h"
42 #include "ompl/base/ProjectionEvaluator.h"
43 #include "ompl/datastructures/PDF.h"
44 #include <boost/unordered_map.hpp>
45 #include <vector>
46 
47 namespace ompl
48 {
49 
50  namespace control
51  {
52 
74  class EST : public base::Planner
75  {
76  public:
77 
79  EST(const SpaceInformationPtr &si);
80 
81  virtual ~EST(void);
82 
84 
85  virtual void clear(void);
86 
94  void setGoalBias(double goalBias)
95  {
96  goalBias_ = goalBias;
97  }
98 
100  double getGoalBias(void) const
101  {
102  return goalBias_;
103  }
104 
110  void setRange(double distance)
111  {
112  maxDistance_ = distance;
113  }
114 
116  double getRange(void) const
117  {
118  return maxDistance_;
119  }
120 
123  void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
124  {
125  projectionEvaluator_ = projectionEvaluator;
126  }
127 
130  void setProjectionEvaluator(const std::string &name)
131  {
132  projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
133  }
134 
137  {
138  return projectionEvaluator_;
139  }
140 
141  virtual void setup(void);
142 
143  virtual void getPlannerData(base::PlannerData &data) const;
144 
145  protected:
146 
151  class Motion
152  {
153  public:
154 
155  Motion(void) : state(NULL), control(NULL), steps(0), parent(NULL)
156  {
157  }
158 
160  Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(NULL)
161  {
162  }
163 
164  ~Motion(void)
165  {
166  }
167 
170 
173 
175  unsigned int steps;
176 
179  };
180 
181  struct MotionInfo;
182 
185 
188 
190  struct MotionInfo
191  {
192  Motion* operator[](unsigned int i)
193  {
194  return motions_[i];
195  }
196  const Motion* operator[](unsigned int i) const
197  {
198  return motions_[i];
199  }
200  void push_back(Motion* m)
201  {
202  motions_.push_back(m);
203  }
204  unsigned int size(void) const
205  {
206  return motions_.size();
207  }
208  bool empty(void) const
209  {
210  return motions_.empty();
211  }
212  std::vector<Motion*> motions_;
213  CellPDF::Element* elem_;
214  };
215 
217  struct TreeData
218  {
219  TreeData(void) : grid(0), size(0)
220  {
221  }
222 
225 
227  unsigned int size;
228  };
229 
231  void freeMemory(void);
232 
234  void addMotion(Motion *motion);
235 
237  Motion* selectMotion(void);
238 
241 
244 
247 
250 
253 
255  double goalBias_;
256 
258  double maxDistance_;
259 
262 
265 
268  };
269 
270  }
271 }
272 
273 #endif