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:
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;
170  }
171 
173  double getInitTemperature() const
174  {
175  return initTemperature_;
176  }
177 
180  void setFrontierThreshold(double frontier_threshold)
181  {
182  frontierThreshold_ = frontier_threshold;
183  }
184 
187  double getFrontierThreshold() const
188  {
189  return frontierThreshold_;
190  }
191 
194  void setFrontierNodeRatio(double frontierNodeRatio)
195  {
196  frontierNodeRatio_ = frontierNodeRatio;
197  }
198 
201  double getFrontierNodeRatio() const
202  {
203  return frontierNodeRatio_;
204  }
205 
207  template <template <typename T> class NN>
209  {
210  if (nearestNeighbors_->size() == 0)
211  OMPL_WARN("Calling setNearestNeighbors will clear all states.");
212  clear();
213  nearestNeighbors_ = std::make_shared<NN<Motion *>>();
214  setup();
215  }
216 
217  void setup() override;
218 
219  protected:
224  class Motion
225  {
226  public:
227  Motion() : state(nullptr), parent(nullptr)
228  {
229  }
230 
232  Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr)
233  {
234  }
235 
236  ~Motion() = default;
237 
240 
243 
246  };
247 
249  void freeMemory();
250 
252  double distanceFunction(const Motion *a, const Motion *b) const
253  {
254  return si_->distance(a->state, b->state);
255  }
256 
260  bool transitionTest(const base::Cost &motionCost);
261 
263  bool minExpansionControl(double randMotionDistance);
264 
267 
269  std::shared_ptr<NearestNeighbors<Motion *>> nearestNeighbors_;
270 
273  double goalBias_;
274 
276  double maxDistance_;
277 
280 
283 
284  // *********************************************************************************************************
285  // TRRT-Specific Variables
286  // *********************************************************************************************************
287 
288  // Transtion Test -----------------------------------------------------------------------
289 
293  double temp_;
294 
297 
300 
303 
307 
310 
311  // Minimum Expansion Control --------------------------------------------------------------
312 
317 
321 
324 
327  };
328  }
329 }
330 
331 #endif
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
double initTemperature_
The initial value of temp_.
Definition: TRRT.h:309
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
std::shared_ptr< NearestNeighbors< Motion * > > nearestNeighbors_
A nearest-neighbors datastructure containing the tree of motions.
Definition: TRRT.h:269
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: TRRT.h:252
A shared pointer wrapper for ompl::base::StateSampler.
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: TRRT.h:208
double temp_
Temperature parameter used to control the difficulty level of transition tests. Low temperatures limi...
Definition: TRRT.h:293
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:156
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:369
base::Cost worstCost_
The least desirable (e.g., maximum) cost value in the search tree.
Definition: TRRT.h:299
Motion * parent
The parent motion in the exploration tree.
Definition: TRRT.h:242
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
double getGoalBias() const
Get the goal bias the planner is using.
Definition: TRRT.h:112
RNG rng_
The random number generator.
Definition: TRRT.h:279
void setTempChangeFactor(double factor)
Set the factor by which the temperature is increased after a failed transition test. This value should be in the range (0, 1], typically close to zero (default is 0.1). This value is an exponential (e^factor) that is multiplied with the current temperature.
Definition: TRRT.h:138
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: TRRT.h:276
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:201
double frontierThreshold_
The distance between an old state and a new state that qualifies it as a frontier state...
Definition: TRRT.h:320
double getTempChangeFactor() const
Get the factor by which the temperature rises based on current acceptance/rejection rate...
Definition: TRRT.h:144
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
bool transitionTest(const base::Cost &motionCost)
Filter irrelevant configuration regarding the search of low-cost paths before inserting into tree...
Definition: TRRT.cpp:389
TRRT(const base::SpaceInformationPtr &si)
Constructor.
Definition: TRRT.cpp:44
void setGoalBias(double goalBias)
Set the goal bias.
Definition: TRRT.h:106
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
base::Cost costThreshold_
All motion costs must be better than this cost (default is infinity)
Definition: TRRT.h:302
Representation of a motion.
Definition: TRRT.h:224
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: TRRT.h:282
Base class for a planner.
Definition: Planner.h:232
base::Cost cost
Cost of the state.
Definition: TRRT.h:245
ompl::base::OptimizationObjectivePtr opt_
The optimization objective being optimized by TRRT.
Definition: TRRT.h:326
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: TRRT.cpp:77
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: TRRT.h:122
base::State * state
The state contained by the motion.
Definition: TRRT.h:239
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
base::StateSamplerPtr sampler_
State sampler.
Definition: TRRT.h:266
A shared pointer wrapper for ompl::base::SpaceInformation.
double getCostThreshold() const
Get the cost threshold (default is infinity). Any motion cost that is not better than this cost (acco...
Definition: TRRT.h:160
Definition of an abstract state.
Definition: State.h:49
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
double frontierCount_
The number of frontier nodes in the search tree.
Definition: TRRT.h:316
double getRange() const
Get the range the planner is using.
Definition: TRRT.h:128
double getInitTemperature() const
Get the temperature at the start of planning.
Definition: TRRT.h:173
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:273
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: TRRT.cpp:94
A shared pointer wrapper for ompl::base::OptimizationObjective.
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:194
base::Cost bestCost_
The most desirable (e.g., minimum) cost value in the search tree.
Definition: TRRT.h:296
bool minExpansionControl(double randMotionDistance)
Use ratio to prefer frontier nodes to nonfrontier ones.
Definition: TRRT.cpp:416
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:167
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition: TRRT.h:232
Transition-based Rapidly-exploring Random Trees.
Definition: TRRT.h:83
double value() const
The value of the cost.
Definition: Cost.h:56
double nonfrontierCount_
The number of non-frontier nodes in the search tree.
Definition: TRRT.h:314
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:187
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
double tempChangeFactor_
The value of the expression exp^T_rate. The temperature is increased by this factor whenever the tran...
Definition: TRRT.h:306
void freeMemory()
Free the memory allocated by this planner.
Definition: TRRT.cpp:139
double frontierNodeRatio_
Target ratio of non-frontier nodes to frontier nodes. rho.
Definition: TRRT.h:323
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:152
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
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:180