All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
BallTreeRRTstar.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 /* Authors: Alejandro Perez, Sertac Karaman, Ioan Sucan */
36 
37 #ifndef OMPL_CONTRIB_RRT_STAR_BTRRTSTAR_
38 #define OMPL_CONTRIB_RRT_STAR_BTRRTSTAR_
39 
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
42 #include "ompl/base/spaces/RealVectorStateSpace.h"
43 #include <limits>
44 #include <vector>
45 
46 
47 namespace ompl
48 {
49 
50  namespace geometric
51  {
52 
79  {
80  public:
81 
83 
84  virtual ~BallTreeRRTstar(void);
85 
86  virtual void getPlannerData(base::PlannerData &data) const;
87 
89 
90  virtual void clear(void);
91 
101  void setGoalBias(double goalBias)
102  {
103  goalBias_ = goalBias;
104  }
105 
107  double getGoalBias(void) const
108  {
109  return goalBias_;
110  }
111 
117  void setRange(double distance)
118  {
119  maxDistance_ = distance;
120  }
121 
123  double getRange(void) const
124  {
125  return maxDistance_;
126  }
127 
137  void setBallRadiusConstant(double ballRadiusConstant)
138  {
139  ballRadiusConst_ = ballRadiusConstant;
140  }
141 
145  double getBallRadiusConstant(void) const
146  {
147  return ballRadiusConst_;
148  }
149 
158  void setMaxBallRadius(double maxBallRadius)
159  {
160  ballRadiusMax_ = maxBallRadius;
161  }
162 
165  double getMaxBallRadius(void) const
166  {
167  return ballRadiusMax_;
168  }
169 
174  void setInitialVolumeRadius(double rO)
175  {
176  rO_ = rO;
177  }
178 
180  double getInitialVolumeRadius(void) const
181  {
182  return rO_;
183  }
184 
186  bool inVolume(base::State *state)
187  {
188  for (unsigned int i = 0 ; i < motions_.size() ; ++i)
189  {
190  if ((si_->distance(motions_[i]->state, state) <= motions_[i]->volRadius))
191  return true;
192  }
193  return false;
194  }
195 
196 
198  template<template<typename T> class NN>
200  {
201  nn_.reset(new NN<Motion*>());
202  }
203 
211  void setDelayCC(bool delayCC)
212  {
213  delayCC_ = delayCC;
214  }
215 
217  bool getDelayCC(void) const
218  {
219  return delayCC_;
220  }
221 
222  virtual void setup(void);
223 
224  protected:
225 
227  class Motion
228  {
229  public:
230 
231  Motion(double rO) : state(NULL), parent(NULL), cost(0.0), volRadius(rO)
232  {
233  }
234 
236  Motion(const base::SpaceInformationPtr &si, double rO) : state(si->allocState()), parent(NULL), cost(0.0), volRadius(rO)
237 
238  {
239  }
240 
241  ~Motion(void)
242  {
243  }
244 
247 
250 
252  double cost;
253 
255  double volRadius;
256 
258  std::vector<Motion*> children;
259  };
260 
262  void freeMemory(void);
263 
265  void addMotion(Motion* m)
266  {
267  nn_->add(m);
268  motions_.push_back(m);
269  }
270 
272  static bool compareMotion(const Motion* a, const Motion* b)
273  {
274  return (a->cost < b->cost);
275  }
277  double distanceFunction(const Motion* a, const Motion* b) const
278  {
279  return (si_->distance(a->state, b->state)) - a->volRadius;
280  }
281 
283  void removeFromParent(Motion *m);
284 
286  void updateChildCosts(Motion *m, double delta);
287 
290 
292  boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
293 
295  std::vector<Motion*> motions_;
296 
298  double goalBias_;
299 
301  double maxDistance_;
302 
305 
308 
311 
313  bool delayCC_;
314 
316  double rO_;
317  };
318 
319  }
320 }
321 
322 #endif