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, Ryan Luna */
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 #include "ompl/base/OptimizationObjective.h"
43 
44 /*
45  NOTES:
46  **Variable Names that have been converted to longer versions from standards:
47  nearest_neighbors_ -> nn_
48  planner_termination_condition -> ptc
49 
50  **Inherited Member Variables Key:
51  si_ -> SpaceInformation
52  pdef_ -> ProblemDefinition
53  pis_ -> PlannerInputStates - Utility class to extract valid input states
54 */
55 
56 namespace ompl
57 {
58  namespace geometric
59  {
83  class TRRT : public base::Planner
84  {
85  public:
87  TRRT(const base::SpaceInformationPtr &si);
88 
89  ~TRRT() override;
90 
91  void getPlannerData(base::PlannerData &data) const override;
92 
93  base::PlannerStatus solve(const base::PlannerTerminationCondition &plannerTerminationCondition) override;
94 
95  void clear() override;
96 
106  void setGoalBias(double goalBias)
107  {
108  goalBias_ = goalBias;
109  }
110 
112  double getGoalBias() const
113  {
114  return goalBias_;
115  }
116 
122  void setRange(double distance)
123  {
124  maxDistance_ = distance;
125  }
126 
128  double getRange() const
129  {
130  return maxDistance_;
131  }
132 
138  void setTempChangeFactor(double factor)
139  {
140  tempChangeFactor_ = exp(factor);
141  }
142 
144  double getTempChangeFactor() const
145  {
146  return log(tempChangeFactor_);
147  }
148 
152  void setCostThreshold(double maxCost)
153  {
154  costThreshold_ = base::Cost(maxCost);
155  }
156 
160  double getCostThreshold() const
161  {
162  return costThreshold_.value();
163  }
164 
167  void setInitTemperature(double initTemperature)
168  {
169  initTemperature_ = initTemperature;
171  }
172 
174  double getInitTemperature() const
175  {
176  return initTemperature_;
177  }
178 
181  void setFrontierThreshold(double frontier_threshold)
182  {
183  frontierThreshold_ = frontier_threshold;
184  }
185 
188  double getFrontierThreshold() const
189  {
190  return frontierThreshold_;
191  }
192 
195  void setFrontierNodeRatio(double frontierNodeRatio)
196  {
197  frontierNodeRatio_ = frontierNodeRatio;
198  }
199 
202  double getFrontierNodeRatio() const
203  {
204  return frontierNodeRatio_;
205  }
206 
208  template <template <typename T> class NN>
209  void setNearestNeighbors()
210  {
211  if (nearestNeighbors_->size() == 0)
212  OMPL_WARN("Calling setNearestNeighbors will clear all states.");
213  clear();
214  nearestNeighbors_ = std::make_shared<NN<Motion *>>();
215  setup();
216  }
217 
218  void setup() override;
219 
220  protected:
225  class Motion
226  {
227  public:
228  Motion() = default;
229 
231  Motion(const base::SpaceInformationPtr &si) : state(si->allocState())
232  {
233  }
234 
235  ~Motion() = default;
236 
238  base::State *state{nullptr};
239 
241  Motion *parent{nullptr};
242 
245  };
246 
248  void freeMemory();
249 
251  double distanceFunction(const Motion *a, const Motion *b) const
252  {
253  return si_->distance(a->state, b->state);
254  }
255 
259  bool transitionTest(const base::Cost &motionCost);
260 
262  bool minExpansionControl(double randMotionDistance);
263 
265  base::StateSamplerPtr sampler_;
266 
268  std::shared_ptr<NearestNeighbors<Motion *>> nearestNeighbors_;
269 
272  double goalBias_{.05};
273 
275  double maxDistance_{0.};
276 
278  RNG rng_;
279 
281  Motion *lastGoalMotion_{nullptr};
282 
283  // *********************************************************************************************************
284  // TRRT-Specific Variables
285  // *********************************************************************************************************
286 
287  // Transtion Test -----------------------------------------------------------------------
288 
292  double temp_;
293 
296 
299 
302 
306 
308  double initTemperature_;
309 
310  // Minimum Expansion Control --------------------------------------------------------------
311 
313  double nonfrontierCount_;
315  double frontierCount_;
316 
319  double frontierThreshold_;
320 
322  double frontierNodeRatio_;
323 
326  };
327  }
328 }
329 
330 #endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:89
double getGoalBias() const
Get the goal bias the planner is using.
Definition: TRRT.h:208
base::Cost costThreshold_
All motion costs must be better than this cost (default is infinity)
Definition: TRRT.h:397
double getRange() const
Get the range the planner is using.
Definition: TRRT.h:224
double initTemperature_
The initial value of temp_.
Definition: TRRT.h:404
double temp_
Temperature parameter used to control the difficulty level of transition tests. Low temperatures limi...
Definition: TRRT.h:388
void freeMemory()
Free the memory allocated by this planner.
Definition: TRRT.cpp:135
void setCostThreshold(double maxCost)
Set the cost threshold (default is infinity). Any motion cost that is not better than this cost (acco...
Definition: TRRT.h:248
Definition of an abstract state.
Definition: State.h:113
base::Cost worstCost_
The least desirable (e.g., maximum) cost value in the search tree.
Definition: TRRT.h:394
Representation of a motion.
Definition: TRRT.h:321
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: TRRT.h:347
double getInitTemperature() const
Get the temperature at the start of planning.
Definition: TRRT.h:270
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: TRRT.cpp:90
double frontierCount_
The number of frontier nodes in the search tree.
Definition: TRRT.h:411
double value() const
The value of the cost.
Definition: Cost.h:152
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
RNG rng_
The random number generator.
Definition: TRRT.h:374
double getFrontierThreshold() const
Get the distance between a new state and the nearest neighbor that qualifies that state as being a fr...
Definition: TRRT.h:284
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
A shared pointer wrapper for ompl::base::OptimizationObjective.
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: TRRT.h:305
double getTempChangeFactor() const
Get the factor by which the temperature rises based on current acceptance/rejection rate.
Definition: TRRT.h:240
void setGoalBias(double goalBias)
Set the goal bias.
Definition: TRRT.h:202
double tempChangeFactor_
The value of the expression exp^T_rate. The temperature is increased by this factor whenever the tran...
Definition: TRRT.h:401
double getFrontierNodeRatio() const
Get the ratio between adding nonfrontier nodes to frontier nodes, for example .1 is 1/10 or one nonfr...
Definition: TRRT.h:298
double frontierThreshold_
The distance between an old state and a new state that qualifies it as a frontier state.
Definition: TRRT.h:415
void setFrontierNodeRatio(double frontierNodeRatio)
Set the ratio between adding nonfrontier nodes to frontier nodes, for example .1 is 1/10 or one nonfr...
Definition: TRRT.h:291
void setFrontierThreshold(double frontier_threshold)
Set the distance between a new state and the nearest neighbor that qualifies that state as being a fr...
Definition: TRRT.h:277
void setInitTemperature(double initTemperature)
Set the initial temperature at the beginning of the algorithm. Should be high to allow for initial ex...
Definition: TRRT.h:263
void setTempChangeFactor(double factor)
Set the factor by which the temperature is increased after a failed transition test....
Definition: TRRT.h:234
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: TRRT.h:377
base::Cost bestCost_
The most desirable (e.g., minimum) cost value in the search tree.
Definition: TRRT.h:391
double nonfrontierCount_
The number of non-frontier nodes in the search tree.
Definition: TRRT.h:409
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: TRRT.cpp:73
double frontierNodeRatio_
Target ratio of non-frontier nodes to frontier nodes. rho.
Definition: TRRT.h:418
TRRT(const base::SpaceInformationPtr &si)
Constructor.
Definition: TRRT.cpp:44
base::PlannerStatus solve(const base::PlannerTerminationCondition &plannerTerminationCondition) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: TRRT.cpp:152
base::StateSamplerPtr sampler_
State sampler.
Definition: TRRT.h:361
ompl::base::OptimizationObjectivePtr opt_
The optimization objective being optimized by TRRT.
Definition: TRRT.h:421
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
Definition: TRRT.h:368
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
std::shared_ptr< NearestNeighbors< Motion * > > nearestNeighbors_
A nearest-neighbors datastructure containing the tree of motions.
Definition: TRRT.h:364
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: TRRT.h:371
base::State * state
The state contained by the motion.
Definition: TRRT.h:334
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: TRRT.cpp:365
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
double getCostThreshold() const
Get the cost threshold (default is infinity). Any motion cost that is not better than this cost (acco...
Definition: TRRT.h:256
base::Cost cost
Cost of the state.
Definition: TRRT.h:340
bool minExpansionControl(double randMotionDistance)
Use ratio to prefer frontier nodes to nonfrontier ones.
Definition: TRRT.cpp:412
bool transitionTest(const base::Cost &motionCost)
Filter irrelevant configuration regarding the search of low-cost paths before inserting into tree.
Definition: TRRT.cpp:385
Motion * parent
The parent motion in the exploration tree.
Definition: TRRT.h:337
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: TRRT.h:218
Main namespace. Contains everything in this library.