All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
TRRT.cpp
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 #include "ompl/geometric/planners/rrt/TRRT.h"
38 #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h"
39 #include "ompl/base/goals/GoalSampleableRegion.h"
40 #include "ompl/tools/config/SelfConfig.h"
41 #include "ompl/tools/config/MagicConstants.h"
42 #include <limits>
43 
44 ompl::geometric::TRRT::TRRT(const base::SpaceInformationPtr &si) : base::Planner(si, "TRRT")
45 {
46  // Standard RRT Variables
48  specs_.directed = true;
49 
50  goalBias_ = 0.05;
51  maxDistance_ = 0.0; // set in setup()
52  lastGoalMotion_ = NULL;
53 
54  Planner::declareParam<double>("range", this, &TRRT::setRange, &TRRT::getRange, "0.:1.:10000.");
55  Planner::declareParam<double>("goal_bias", this, &TRRT::setGoalBias, &TRRT::getGoalBias, "0.:.05:1.");
56 
57  // TRRT Specific Variables
58  frontierThreshold_ = 0.0; // set in setup()
59  kConstant_ = 0.0; // set in setup()
60  maxStatesFailed_ = 10; // threshold for when to start increasing the temperatuer
61  tempChangeFactor_ = 2.0; // how much to decrease or increase the temp each time
62  minTemperature_ = 10e-10; // lower limit of the temperature change
63  initTemperature_ = 10e-6; // where the temperature starts out
64  frontierNodeRatio_ = 0.1; // 1/10, or 1 nonfrontier for every 10 frontier
65 
66  Planner::declareParam<unsigned int>("max_states_failed", this, &TRRT::setMaxStatesFailed, &TRRT::getMaxStatesFailed, "1:1000");
67  Planner::declareParam<double>("temp_change_factor", this, &TRRT::setTempChangeFactor, &TRRT::getTempChangeFactor,"0.:.1:10.");
68  Planner::declareParam<double>("min_temperature", this, &TRRT::setMinTemperature, &TRRT::getMinTemperature);
69  Planner::declareParam<double>("init_temperature", this, &TRRT::setInitTemperature, &TRRT::getInitTemperature);
70  Planner::declareParam<double>("frontier_threshold", this, &TRRT::setFrontierThreshold, &TRRT::getFrontierThreshold);
71  Planner::declareParam<double>("frontierNodeRatio", this, &TRRT::setFrontierNodeRatio, &TRRT::getFrontierNodeRatio);
72  Planner::declareParam<double>("k_constant", this, &TRRT::setKConstant, &TRRT::getKConstant);
73 }
74 
75 ompl::geometric::TRRT::~TRRT(void)
76 {
77  freeMemory();
78 }
79 
81 {
82  Planner::clear();
83  sampler_.reset();
84  freeMemory();
85  if (nearestNeighbors_)
86  nearestNeighbors_->clear();
87  lastGoalMotion_ = NULL;
88 
89  // Clear TRRT specific variables ---------------------------------------------------------
90  numStatesFailed_ = 0;
91  temp_ = initTemperature_;
92  nonfrontierCount_ = 1;
93  frontierCount_ = 1; // init to 1 to prevent division by zero error
94 }
95 
97 {
98  Planner::setup();
99  tools::SelfConfig selfConfig(si_, getName());
100 
101  bool usingDefaultObjective = false;
102  if (!pdef_->hasOptimizationObjective())
103  {
104  OMPL_INFORM("%s: No optimization objective specified.", getName().c_str());
105  usingDefaultObjective = true;
106  }
107  else if (!boost::dynamic_pointer_cast<
108  base::MechanicalWorkOptimizationObjective>(pdef_->getOptimizationObjective()))
109  {
110  OMPL_INFORM("%s: TRRT was supplied an inappropriate optimization objective; it can only handle types of ompl::base::MechanicalWorkOptimizationObjective.", getName().c_str());
111  usingDefaultObjective = true;
112  }
113 
114  if (usingDefaultObjective)
115  {
116  opt_.reset(new base::MechanicalWorkOptimizationObjective(si_));
117  OMPL_INFORM("%s: Defaulting to optimizing path length.", getName().c_str());
118  }
119  else
120  opt_ = pdef_->getOptimizationObjective();
121 
122  // Set maximum distance a new node can be from its nearest neighbor
123  if (maxDistance_ < std::numeric_limits<double>::epsilon())
124  {
125  selfConfig.configurePlannerRange(maxDistance_);
127  }
128 
129  // Set the threshold that decides if a new node is a frontier node or non-frontier node
130  if (frontierThreshold_ < std::numeric_limits<double>::epsilon())
131  {
132  frontierThreshold_ = si_->getMaximumExtent() * 0.01;
133  OMPL_DEBUG("%s: Frontier threshold detected to be %lf", getName().c_str(), frontierThreshold_);
134  }
135 
136  // Autoconfigure the K constant
137  if (kConstant_ < std::numeric_limits<double>::epsilon())
138  {
139  // Find the average cost of states by sampling
140  double averageCost = opt_->averageStateCost(magic::TEST_STATE_COUNT).v;
141  kConstant_ = averageCost;
142  OMPL_DEBUG("%s: K constant detected to be %lf", getName().c_str(), kConstant_);
143  }
144 
145  // Create the nearest neighbor function the first time setup is run
146  if (!nearestNeighbors_)
147  nearestNeighbors_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
148 
149  // Set the distance function
150  nearestNeighbors_->setDistanceFunction(boost::bind(&TRRT::distanceFunction, this, _1, _2));
151 
152  // Setup TRRT specific variables ---------------------------------------------------------
153  numStatesFailed_ = 0;
154  temp_ = initTemperature_;
155  nonfrontierCount_ = 1;
156  frontierCount_ = 1; // init to 1 to prevent division by zero error
157 }
158 
160 {
161  // Delete all motions, states and the nearest neighbors data structure
162  if (nearestNeighbors_)
163  {
164  std::vector<Motion*> motions;
165  nearestNeighbors_->list(motions);
166  for (unsigned int i = 0 ; i < motions.size() ; ++i)
167  {
168  if (motions[i]->state)
169  si_->freeState(motions[i]->state);
170  delete motions[i];
171  }
172  }
173 }
174 
177 {
178  // Basic error checking
179  checkValidity();
180 
181  // Goal information
182  base::Goal *goal = pdef_->getGoal().get();
183  base::GoalSampleableRegion *goalRegion = dynamic_cast<base::GoalSampleableRegion*>(goal);
184 
185  // Input States ---------------------------------------------------------------------------------
186 
187  // Loop through valid input states and add to tree
188  while (const base::State *state = pis_.nextStart())
189  {
190  // Allocate memory for a new start state motion based on the "space-information"-size
191  Motion *motion = new Motion(si_);
192 
193  // Copy destination <= source
194  si_->copyState(motion->state, state);
195 
196  // Set cost for this start state
197  motion->cost = opt_->stateCost(motion->state);
198 
199  // Add start motion to the tree
200  nearestNeighbors_->add(motion);
201  }
202 
203  // Check that input states exist
204  if (nearestNeighbors_->size() == 0)
205  {
206  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
208  }
209 
210  // Create state sampler if this is TRRT's first run
211  if (!sampler_)
212  sampler_ = si_->allocStateSampler();
213 
214  // Debug
215  OMPL_INFORM("%s: Starting with %u states", getName().c_str(), nearestNeighbors_->size());
216 
217 
218  // Solver variables ------------------------------------------------------------------------------------
219 
220  // the final solution
221  Motion *solution = NULL;
222  // the approximate solution, returned if no final solution found
223  Motion *approxSolution = NULL;
224  // track the distance from goal to closest solution yet found
225  double approxDifference = std::numeric_limits<double>::infinity();
226 
227  // distance between states - the intial state and the interpolated state (may be the same)
228  double randMotionDistance;
229  double motionDistance;
230 
231  // Create random motion and a pointer (for optimization) to its state
232  Motion *randMotion = new Motion(si_);
233  Motion *nearMotion;
234 
235  // STATES
236  // The random state
237  base::State *randState = randMotion->state;
238  // The new state that is generated between states *to* and *from*
239  base::State *interpolatedState = si_->allocState(); // Allocates "space information"-sized memory for a state
240  // The chosen state btw rand_state and interpolated_state
241  base::State *newState;
242 
243  // Begin sampling --------------------------------------------------------------------------------------
244  while (plannerTerminationCondition() == false)
245  {
246  // I.
247 
248  // Sample random state (with goal biasing probability)
249  if (goalRegion && rng_.uniform01() < goalBias_ && goalRegion->canSample())
250  {
251  // Bias sample towards goal
252  goalRegion->sampleGoal(randState);
253  }
254  else
255  {
256  // Uniformly Sample
257  sampler_->sampleUniform(randState);
258  }
259 
260  // II.
261 
262  // Find closest state in the tree
263  nearMotion = nearestNeighbors_->nearest(randMotion);
264 
265  // III.
266 
267  // Distance from near state q_n to a random state
268  randMotionDistance = si_->distance(nearMotion->state, randState);
269 
270  // Check if the rand_state is too far away
271  if (randMotionDistance > maxDistance_)
272  {
273  // Computes the state that lies at time t in [0, 1] on the segment that connects *from* state to *to* state.
274  // The memory location of *state* is not required to be different from the memory of either *from* or *to*.
275  si_->getStateSpace()->interpolate(nearMotion->state, randState,
276  maxDistance_ / randMotionDistance, interpolatedState);
277 
278  // Update the distance between near and new with the interpolated_state
279  motionDistance = si_->distance(nearMotion->state, interpolatedState);
280 
281  // Use the interpolated state as the new state
282  newState = interpolatedState;
283  }
284  else
285  {
286  // Random state is close enough
287  newState = randState;
288 
289  // Copy the distance
290  motionDistance = randMotionDistance;
291  }
292 
293  // IV.
294  // this stage integrates collision detections in the presence of obstacles and checks for collisions
295 
303  if (!si_->checkMotion(nearMotion->state, newState))
304  continue; // try a new sample
305 
306 
307  // Minimum Expansion Control
308  // A possible side effect may appear when the tree expansion toward unexplored regions remains slow, and the
309  // new nodes contribute only to refine already explored regions.
310  if (!minExpansionControl(randMotionDistance))
311  {
312  continue; // give up on this one and try a new sample
313  }
314 
315  base::Cost childCost = opt_->stateCost(newState);
316 
317  // Only add this motion to the tree if the tranistion test accepts it
318  if(!transitionTest(childCost.v, nearMotion->cost.v, motionDistance))
319  {
320  continue; // give up on this one and try a new sample
321  }
322 
323  // V.
324 
325  // Create a motion
326  Motion *motion = new Motion(si_);
327  si_->copyState(motion->state, newState);
328  motion->parent = nearMotion; // link q_new to q_near as an edge
329  motion->cost = childCost;
330 
331  // Add motion to data structure
332  nearestNeighbors_->add(motion);
333 
334  // VI.
335 
336  // Check if this motion is the goal
337  double distToGoal = 0.0;
338  bool isSatisfied = goal->isSatisfied(motion->state, &distToGoal);
339  if (isSatisfied)
340  {
341  approxDifference = distToGoal; // the tolerated error distance btw state and goal
342  solution = motion; // set the final solution
343  break;
344  }
345 
346  // Is this the closest solution we've found so far
347  if (distToGoal < approxDifference)
348  {
349  approxDifference = distToGoal;
350  approxSolution = motion;
351  }
352 
353  } // end of solver sampling loop
354 
355 
356  // Finish solution processing --------------------------------------------------------------------
357 
358  bool solved = false;
359  bool approximate = false;
360 
361  // Substitute an empty solution with the best approximation
362  if (solution == NULL)
363  {
364  solution = approxSolution;
365  approximate = true;
366  }
367 
368  // Generate solution path for real/approx solution
369  if (solution != NULL)
370  {
371  lastGoalMotion_ = solution;
372 
373  // construct the solution path
374  std::vector<Motion*> mpath;
375  while (solution != NULL)
376  {
377  mpath.push_back(solution);
378  solution = solution->parent;
379  }
380 
381  // set the solution path
382  PathGeometric *path = new PathGeometric(si_);
383  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
384  path->append(mpath[i]->state);
385 
386  pdef_->addSolutionPath(base::PathPtr(path), approximate, approxDifference);
387  solved = true;
388  }
389 
390  // Clean up ---------------------------------------------------------------------------------------
391 
392  si_->freeState(interpolatedState);
393  if (randMotion->state)
394  si_->freeState(randMotion->state);
395  delete randMotion;
396 
397  OMPL_INFORM("%s: Created %u states", getName().c_str(), nearestNeighbors_->size());
398 
399  return base::PlannerStatus(solved, approximate);
400 }
401 
403 {
404  Planner::getPlannerData(data);
405 
406  std::vector<Motion*> motions;
407  if (nearestNeighbors_)
408  nearestNeighbors_->list(motions);
409 
410  if (lastGoalMotion_)
411  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
412 
413  for (unsigned int i = 0 ; i < motions.size() ; ++i)
414  {
415  if (motions[i]->parent == NULL)
416  data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
417  else
418  data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
419  base::PlannerDataVertex(motions[i]->state));
420  }
421 }
422 
423 bool ompl::geometric::TRRT::transitionTest(double childCost, double parentCost, double distance)
424 {
425  // Always accept if new state has same or lower cost than old state
426  if (childCost <= parentCost)
427  return true;
428 
429  // Difference in cost
430  double costSlope = (childCost - parentCost) / distance;
431 
432  // The probability of acceptance of a new configuration is defined by comparing its cost c_j
433  // relatively to the cost c_i of its parent in the tree. Based on the Metropolis criterion.
434  double transitionProbability = 1.; // if cost_slope is <= 0, probabilty is 1
435 
436  // Only return at end
437  bool result = false;
438 
439  // Calculate tranision probabilty
440  if (costSlope > 0)
441  {
442  transitionProbability = exp(-costSlope / (kConstant_ * temp_));
443  }
444 
445  // Check if we can accept it
446  if (rng_.uniform01() <= transitionProbability)
447  {
448  if (temp_ > minTemperature_)
449  {
450  temp_ /= tempChangeFactor_;
451 
452  // Prevent temp_ from getting too small
453  if (temp_ < minTemperature_)
454  {
455  temp_ = minTemperature_;
456  }
457  }
458 
459  numStatesFailed_ = 0;
460 
461  result = true;
462  }
463  else
464  {
465  // State has failed
466  if (numStatesFailed_ >= maxStatesFailed_)
467  {
468  temp_ *= tempChangeFactor_;
469  numStatesFailed_ = 0;
470  }
471  else
472  {
473  ++numStatesFailed_;
474  }
475 
476  }
477 
478  return result;
479 }
480 
481 bool ompl::geometric::TRRT::minExpansionControl(double randMotionDistance)
482 {
483  // Decide to accept or not
484  if (randMotionDistance > frontierThreshold_)
485  {
486  // participates in the tree expansion
487  ++frontierCount_;
488 
489  return true;
490  }
491  else
492  {
493  // participates in the tree refinement
494 
495  // check our ratio first before accepting it
496  if ((double)nonfrontierCount_ / (double)frontierCount_ > frontierNodeRatio_)
497  {
498  // Increment so that the temperature rises faster
499  ++numStatesFailed_;
500 
501  // reject this node as being too much refinement
502  return false;
503  }
504  else
505  {
506  ++nonfrontierCount_;
507  return true;
508  }
509  }
510 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
double getRange(void) const
Get the range the planner is using.
Definition: TRRT.h:123
bool canSample(void) const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
double getFrontierThreshold(void) const
Get the distance between a new state and the nearest neighbor that qualifies that state as being a fr...
Definition: TRRT.h:185
void setKConstant(double kConstant)
Set the constant value used to normalize the expression.
Definition: TRRT.h:205
double initTemperature_
A very low value at initialization to authorize very easy positive slopes.
Definition: TRRT.h:326
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: TRRT.cpp:96
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: TRRT.cpp:80
unsigned int maxStatesFailed_
Max number of rejections allowed.
Definition: TRRT.h:317
An optimization objective which defines path cost using the idea of mechanical work. To be used in conjunction with TRRT.
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
void freeMemory(void)
Free the memory allocated by this planner.
Definition: TRRT.cpp:159
Abstract definition of goals.
Definition: Goal.h:63
double getFrontierNodeRatio(void) const
Get the ratio between adding nonfrontier nodes to frontier nodes, for example .1 is 1/10 or one nonfr...
Definition: TRRT.h:199
void setMinTemperature(double minTemperature)
Set the minimum the temperature can drop to before being floored at that value.
Definition: TRRT.h:153
Motion * parent
The parent motion in the exploration tree.
Definition: TRRT.h:253
void setTempChangeFactor(double tempChangeFactor)
Set the factor by which the temperature rises or falls based on current acceptance/rejection rate...
Definition: TRRT.h:141
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
double v
The value of the cost.
Definition: Cost.h:52
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: TRRT.h:289
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
bool transitionTest(double childCost, double parentCost, double distance)
Filter irrelevant configuration regarding the search of low-cost paths before inserting into tree...
Definition: TRRT.cpp:423
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:220
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
double getKConstant(void) const
Get the constant value used to normalize the expression.
Definition: TRRT.h:211
double frontierThreshold_
The distance between an old state and a new state that qualifies it as a frontier state...
Definition: TRRT.h:339
double kConstant_
Constant value used to normalize expression. Based on order of magnitude of the considered costs...
Definition: TRRT.h:314
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
double getInitTemperature(void) const
Get the initial temperature at the beginning of the algorithm. Should be low.
Definition: TRRT.h:171
TRRT(const base::SpaceInformationPtr &si)
Constructor.
Definition: TRRT.cpp:44
void setGoalBias(double goalBias)
Set the goal bias.
Definition: TRRT.h:101
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
Representation of a motion.
Definition: TRRT.h:232
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: TRRT.h:295
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
base::Cost cost
Cost of the state.
Definition: TRRT.h:256
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: TRRT.h:117
base::State * state
The state contained by the motion.
Definition: TRRT.h:250
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
A boost shared pointer wrapper for ompl::base::SpaceInformation.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: TRRT.cpp:402
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: TRRT.h:264
double getTempChangeFactor(void) const
Get the factor by which the temperature rises or falls based on current acceptance/rejection rate...
Definition: TRRT.h:147
void setMaxStatesFailed(double maxStatesFailed)
Set the maximum number of states that can be rejected before the temperature starts to rise...
Definition: TRRT.h:129
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
Definition: State.h:50
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &plannerTerminationCondition)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: TRRT.cpp:176
double getMinTemperature(void) const
Get the minimum the temperature can drop to before being floored at that value.
Definition: TRRT.h:159
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:286
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:404
double getMaxStatesFailed(void) const
Get the maximum number of states that can be rejected before the temperature starts to rise...
Definition: TRRT.h:135
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
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:192
double minTemperature_
Prevent temperature from dropping too far.
Definition: TRRT.h:323
static const double COST_MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION
For cost-based planners it has been observed that smaller ranges are typically suitable. The same range computation strategy is used for all planners, but for cost planners an additional factor (smaller than 1) is multiplied in.
bool minExpansionControl(double randMotionDistance)
Use ratio to prefer frontier nodes to nonfrontier ones.
Definition: TRRT.cpp:481
void setInitTemperature(double initTemperature)
Set the initial temperature at the beginning of the algorithm. Should be low.
Definition: TRRT.h:165
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:226
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:56
double getGoalBias(void) const
Get the goal bias the planner is using.
Definition: TRRT.h:107
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
Definition of a geometric path.
Definition: PathGeometric.h:55
double tempChangeFactor_
Failure temperature factor used when max_num_failed_ failures occur.
Definition: TRRT.h:320
double frontierNodeRatio_
Target ratio of nonfrontier nodes to frontier nodes. rho.
Definition: TRRT.h:342
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:178
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68