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;
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() = default;
228 
230  Motion(const base::SpaceInformationPtr &si) : state(si->allocState())
231  {
232  }
233 
234  ~Motion() = default;
235 
237  base::State *state{nullptr};
238 
240  Motion *parent{nullptr};
241 
244  };
245 
247  void freeMemory();
248 
250  double distanceFunction(const Motion *a, const Motion *b) const
251  {
252  return si_->distance(a->state, b->state);
253  }
254 
258  bool transitionTest(const base::Cost &motionCost);
259 
261  bool minExpansionControl(double randMotionDistance);
262 
264  base::StateSamplerPtr sampler_;
265 
267  std::shared_ptr<NearestNeighbors<Motion *>> nearestNeighbors_;
268 
271  double goalBias_{.05};
272 
274  double maxDistance_{0.};
275 
277  RNG rng_;
278 
280  Motion *lastGoalMotion_{nullptr};
281 
282  // *********************************************************************************************************
283  // TRRT-Specific Variables
284  // *********************************************************************************************************
285 
286  // Transtion Test -----------------------------------------------------------------------
287 
291  double temp_;
292 
295 
298 
301 
305 
307  double initTemperature_;
308 
309  // Minimum Expansion Control --------------------------------------------------------------
310 
312  double nonfrontierCount_;
314  double frontierCount_;
315 
318  double frontierThreshold_;
319 
321  double frontierNodeRatio_;
322 
325  };
326  }
327 }
328 
329 #endif
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:396
double getRange() const
Get the range the planner is using.
Definition: TRRT.h:224
double initTemperature_
The initial value of temp_.
Definition: TRRT.h:403
double temp_
Temperature parameter used to control the difficulty level of transition tests. Low temperatures limi...
Definition: TRRT.h:387
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:393
Representation of a motion.
Definition: TRRT.h:320
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: TRRT.h:346
double getInitTemperature() const
Get the temperature at the start of planning.
Definition: TRRT.h:269
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:410
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:373
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:283
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:304
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:400
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:297
double frontierThreshold_
The distance between an old state and a new state that qualifies it as a frontier state.
Definition: TRRT.h:414
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:290
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:276
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:376
base::Cost bestCost_
The most desirable (e.g., minimum) cost value in the search tree.
Definition: TRRT.h:390
double nonfrontierCount_
The number of non-frontier nodes in the search tree.
Definition: TRRT.h:408
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:417
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:360
ompl::base::OptimizationObjectivePtr opt_
The optimization objective being optimized by TRRT.
Definition: TRRT.h:420
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:367
#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:363
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: TRRT.h:370
base::State * state
The state contained by the motion.
Definition: TRRT.h:333
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:470
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:339
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:336
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: TRRT.h:218
Main namespace. Contains everything in this library.