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>
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 
279 
282 
283  // *********************************************************************************************************
284  // TRRT-Specific Variables
285  // *********************************************************************************************************
286 
287  // Transtion Test -----------------------------------------------------------------------
288 
292  double temp_;
293 
296 
299 
302 
306 
309 
310  // Minimum Expansion Control --------------------------------------------------------------
311 
316 
320 
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:58
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
double value() const
The value of the cost.
Definition: Cost.h:56
A shared pointer wrapper for ompl::base::OptimizationObjective.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
Definition of an abstract state.
Definition: State.h:50
Representation of a motion.
Definition: TRRT.h:226
Motion * parent
The parent motion in the exploration tree.
Definition: TRRT.h:241
base::Cost cost
Cost of the state.
Definition: TRRT.h:244
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition: TRRT.h:231
base::State * state
The state contained by the motion.
Definition: TRRT.h:238
Transition-based Rapidly-exploring Random Trees.
Definition: TRRT.h:84
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:202
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
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: TRRT.h:251
void freeMemory()
Free the memory allocated by this planner.
Definition: TRRT.cpp:135
std::shared_ptr< NearestNeighbors< Motion * > > nearestNeighbors_
A nearest-neighbors datastructure containing the tree of motions.
Definition: TRRT.h:268
void setTempChangeFactor(double factor)
Set the factor by which the temperature is increased after a failed transition test....
Definition: TRRT.h:138
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
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:272
double frontierThreshold_
The distance between an old state and a new state that qualifies it as a frontier state.
Definition: TRRT.h:319
double frontierCount_
The number of frontier nodes in the search tree.
Definition: TRRT.h:315
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:195
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
base::Cost worstCost_
The least desirable (e.g., maximum) cost value in the search tree.
Definition: TRRT.h:298
double getRange() const
Get the range the planner is using.
Definition: TRRT.h:128
base::Cost costThreshold_
All motion costs must be better than this cost (default is infinity)
Definition: TRRT.h:301
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:181
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: TRRT.h:281
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
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:188
double getTempChangeFactor() const
Get the factor by which the temperature rises based on current acceptance/rejection rate.
Definition: TRRT.h:144
double tempChangeFactor_
The value of the expression exp^T_rate. The temperature is increased by this factor whenever the tran...
Definition: TRRT.h:305
RNG rng_
The random number generator.
Definition: TRRT.h:278
double nonfrontierCount_
The number of non-frontier nodes in the search tree.
Definition: TRRT.h:313
ompl::base::OptimizationObjectivePtr opt_
The optimization objective being optimized by TRRT.
Definition: TRRT.h:325
double temp_
Temperature parameter used to control the difficulty level of transition tests. Low temperatures limi...
Definition: TRRT.h:292
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: TRRT.h:122
double initTemperature_
The initial value of temp_.
Definition: TRRT.h:308
double getGoalBias() const
Get the goal bias the planner is using.
Definition: TRRT.h:112
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
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: TRRT.cpp:73
base::Cost bestCost_
The most desirable (e.g., minimum) cost value in the search tree.
Definition: TRRT.h:295
bool minExpansionControl(double randMotionDistance)
Use ratio to prefer frontier nodes to nonfrontier ones.
Definition: TRRT.cpp:412
void setGoalBias(double goalBias)
Set the goal bias.
Definition: TRRT.h:106
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: TRRT.cpp:90
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: TRRT.h:209
TRRT(const base::SpaceInformationPtr &si)
Constructor.
Definition: TRRT.cpp:44
bool transitionTest(const base::Cost &motionCost)
Filter irrelevant configuration regarding the search of low-cost paths before inserting into tree.
Definition: TRRT.cpp:385
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: TRRT.h:275
base::StateSamplerPtr sampler_
State sampler.
Definition: TRRT.h:265
double frontierNodeRatio_
Target ratio of non-frontier nodes to frontier nodes. rho.
Definition: TRRT.h:322
double getInitTemperature() const
Get the temperature at the start of planning.
Definition: TRRT.h:174
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49