All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
TRRT.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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: Dave Coleman */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_TRRT_
38 #define OMPL_GEOMETRIC_PLANNERS_RRT_TRRT_
39 
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
42 
43 /*
44  NOTES:
45  **Variable Names that have been converted to longer versions from standards:
46  nearest_neighbors_ -> nn_
47  planner_termination_condition -> ptc
48 
49  **Inherited Member Variables Key:
50  si_ -> SpaceInformation
51  pdef_ -> ProblemDefinition
52  pis_ -> PlannerInputStates - Utility class to extract valid input states
53 */
54 
55 
56 namespace ompl
57 {
58 
59  namespace geometric
60  {
61 
77  class TRRT : public base::Planner
78  {
79  public:
80 
83 
84  virtual ~TRRT(void);
85 
86  virtual void getPlannerData(base::PlannerData &data) const;
87 
88  virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &plannerTerminationCondition);
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 
129  void setMaxStatesFailed( double maxStatesFailed )
130  {
131  maxStatesFailed_ = maxStatesFailed;
132  }
133 
135  double getMaxStatesFailed( void ) const
136  {
137  return maxStatesFailed_;
138  }
139 
141  void setTempChangeFactor( double tempChangeFactor )
142  {
143  tempChangeFactor_ = tempChangeFactor;
144  }
145 
147  double getTempChangeFactor( void ) const
148  {
149  return tempChangeFactor_;
150  }
151 
153  void setMinTemperature( double minTemperature )
154  {
155  minTemperature_ = minTemperature;
156  }
157 
159  double getMinTemperature( void ) const
160  {
161  return minTemperature_;
162  }
163 
165  void setInitTemperature( double initTemperature )
166  {
167  initTemperature_ = initTemperature;
168  }
169 
171  double getInitTemperature( void ) const
172  {
173  return initTemperature_;
174  }
175 
178  void setFrontierThreshold( double frontier_threshold )
179  {
180  frontierThreshold_ = frontier_threshold;
181  }
182 
185  double getFrontierThreshold( void ) const
186  {
187  return frontierThreshold_;
188  }
189 
192  void setFrontierNodeRatio( double frontierNodeRatio )
193  {
194  frontierNodeRatio_ = frontierNodeRatio;
195  }
196 
199  double getFrontierNodeRatio( void ) const
200  {
201  return frontierNodeRatio_;
202  }
203 
205  void setKConstant( double kConstant )
206  {
207  kConstant_ = kConstant;
208  }
209 
211  double getKConstant( void ) const
212  {
213  return kConstant_;
214  }
215 
217  template<template<typename T> class NN>
219  {
220  nearestNeighbors_.reset(new NN<Motion*>());
221  }
222 
223  virtual void setup(void);
224 
225  protected:
226 
227 
232  class Motion
233  {
234  public:
235 
236  Motion(void) : state(NULL), parent(NULL)
237  {
238  }
239 
241  Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
242  {
243  }
244 
245  ~Motion(void)
246  {
247  }
248 
251 
254 
256  double distance;
257 
259  double cost;
260 
261  };
262 
264  void freeMemory(void);
265 
267  double distanceFunction(const Motion* a, const Motion* b) const
268  {
269  return si_->distance(a->state, b->state);
270  }
271 
277  bool transitionTest( double childCost, double parentCost, double distance );
278 
280  bool minExpansionControl( double randMotionDistance );
281 
284 
286  boost::shared_ptr< NearestNeighbors<Motion*> > nearestNeighbors_;
287 
289  double goalBias_;
290 
292  double maxDistance_;
293 
296 
299 
301  bool verbose_;
302 
303  // *********************************************************************************************************
304  // TRRT-Specific Variables
305  // *********************************************************************************************************
306 
307  // Transtion Test -----------------------------------------------------------------------
308 
312  double temp_;
313 
317  double kConstant_;
318 
320  unsigned int maxStatesFailed_;
321 
324 
327 
330 
332  unsigned int numStatesFailed_;
333 
334 
335  // Minimum Expansion Control --------------------------------------------------------------
336 
339  double frontierCount_;
340 
343 
346 
347  };
348  }
349 }
350 
351 #endif