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()
76 {
77  freeMemory();
78 }
79 
81 {
82  Planner::clear();
83  sampler_.reset();
84  freeMemory();
86  nearestNeighbors_->clear();
87  lastGoalMotion_ = NULL;
88 
89  // Clear TRRT specific variables ---------------------------------------------------------
90  numStatesFailed_ = 0;
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
108  {
109  usingDefaultObjective = false;
110  }
111 
112  if (usingDefaultObjective)
113  {
115  OMPL_INFORM("%s: Defaulting to optimizing path length.", getName().c_str());
116  }
117  else
118  opt_ = pdef_->getOptimizationObjective();
119 
120  // Set maximum distance a new node can be from its nearest neighbor
121  if (maxDistance_ < std::numeric_limits<double>::epsilon())
122  {
125  }
126 
127  // Set the threshold that decides if a new node is a frontier node or non-frontier node
128  if (frontierThreshold_ < std::numeric_limits<double>::epsilon())
129  {
130  frontierThreshold_ = si_->getMaximumExtent() * 0.01;
131  OMPL_DEBUG("%s: Frontier threshold detected to be %lf", getName().c_str(), frontierThreshold_);
132  }
133 
134  // Autoconfigure the K constant
135  if (kConstant_ < std::numeric_limits<double>::epsilon())
136  {
137  // Find the average cost of states by sampling
138  double averageCost = opt_->averageStateCost(magic::TEST_STATE_COUNT).value();
139  kConstant_ = averageCost;
140  OMPL_DEBUG("%s: K constant detected to be %lf", getName().c_str(), kConstant_);
141  }
142 
143  // Create the nearest neighbor function the first time setup is run
144  if (!nearestNeighbors_)
145  nearestNeighbors_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
146 
147  // Set the distance function
148  nearestNeighbors_->setDistanceFunction(boost::bind(&TRRT::distanceFunction, this, _1, _2));
149 
150  // Setup TRRT specific variables ---------------------------------------------------------
151  numStatesFailed_ = 0;
153  nonfrontierCount_ = 1;
154  frontierCount_ = 1; // init to 1 to prevent division by zero error
155 }
156 
158 {
159  // Delete all motions, states and the nearest neighbors data structure
160  if (nearestNeighbors_)
161  {
162  std::vector<Motion*> motions;
163  nearestNeighbors_->list(motions);
164  for (unsigned int i = 0 ; i < motions.size() ; ++i)
165  {
166  if (motions[i]->state)
167  si_->freeState(motions[i]->state);
168  delete motions[i];
169  }
170  }
171 }
172 
175 {
176  // Basic error checking
177  checkValidity();
178 
179  // Goal information
180  base::Goal *goal = pdef_->getGoal().get();
181  base::GoalSampleableRegion *goalRegion = dynamic_cast<base::GoalSampleableRegion*>(goal);
182 
183  // Input States ---------------------------------------------------------------------------------
184 
185  // Loop through valid input states and add to tree
186  while (const base::State *state = pis_.nextStart())
187  {
188  // Allocate memory for a new start state motion based on the "space-information"-size
189  Motion *motion = new Motion(si_);
190 
191  // Copy destination <= source
192  si_->copyState(motion->state, state);
193 
194  // Set cost for this start state
195  motion->cost = opt_->stateCost(motion->state);
196 
197  // Add start motion to the tree
198  nearestNeighbors_->add(motion);
199  }
200 
201  // Check that input states exist
202  if (nearestNeighbors_->size() == 0)
203  {
204  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
206  }
207 
208  // Create state sampler if this is TRRT's first run
209  if (!sampler_)
210  sampler_ = si_->allocStateSampler();
211 
212  // Debug
213  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nearestNeighbors_->size());
214 
215 
216  // Solver variables ------------------------------------------------------------------------------------
217 
218  // the final solution
219  Motion *solution = NULL;
220  // the approximate solution, returned if no final solution found
221  Motion *approxSolution = NULL;
222  // track the distance from goal to closest solution yet found
223  double approxDifference = std::numeric_limits<double>::infinity();
224 
225  // distance between states - the intial state and the interpolated state (may be the same)
226  double randMotionDistance;
227  double motionDistance;
228 
229  // Create random motion and a pointer (for optimization) to its state
230  Motion *randMotion = new Motion(si_);
231  Motion *nearMotion;
232 
233  // STATES
234  // The random state
235  base::State *randState = randMotion->state;
236  // The new state that is generated between states *to* and *from*
237  base::State *interpolatedState = si_->allocState(); // Allocates "space information"-sized memory for a state
238  // The chosen state btw rand_state and interpolated_state
239  base::State *newState;
240 
241  // Begin sampling --------------------------------------------------------------------------------------
242  while (plannerTerminationCondition() == false)
243  {
244  // I.
245 
246  // Sample random state (with goal biasing probability)
247  if (goalRegion && rng_.uniform01() < goalBias_ && goalRegion->canSample())
248  {
249  // Bias sample towards goal
250  goalRegion->sampleGoal(randState);
251  }
252  else
253  {
254  // Uniformly Sample
255  sampler_->sampleUniform(randState);
256  }
257 
258  // II.
259 
260  // Find closest state in the tree
261  nearMotion = nearestNeighbors_->nearest(randMotion);
262 
263  // III.
264 
265  // Distance from near state q_n to a random state
266  randMotionDistance = si_->distance(nearMotion->state, randState);
267 
268  // Check if the rand_state is too far away
269  if (randMotionDistance > maxDistance_)
270  {
271  // Computes the state that lies at time t in [0, 1] on the segment that connects *from* state to *to* state.
272  // The memory location of *state* is not required to be different from the memory of either *from* or *to*.
273  si_->getStateSpace()->interpolate(nearMotion->state, randState,
274  maxDistance_ / randMotionDistance, interpolatedState);
275 
276  // Update the distance between near and new with the interpolated_state
277  motionDistance = si_->distance(nearMotion->state, interpolatedState);
278 
279  // Use the interpolated state as the new state
280  newState = interpolatedState;
281  }
282  else
283  {
284  // Random state is close enough
285  newState = randState;
286 
287  // Copy the distance
288  motionDistance = randMotionDistance;
289  }
290 
291  // IV.
292  // this stage integrates collision detections in the presence of obstacles and checks for collisions
293 
301  if (!si_->checkMotion(nearMotion->state, newState))
302  continue; // try a new sample
303 
304 
305  // Minimum Expansion Control
306  // A possible side effect may appear when the tree expansion toward unexplored regions remains slow, and the
307  // new nodes contribute only to refine already explored regions.
308  if (!minExpansionControl(randMotionDistance))
309  {
310  continue; // give up on this one and try a new sample
311  }
312 
313  base::Cost childCost = opt_->stateCost(newState);
314 
315  // Only add this motion to the tree if the tranistion test accepts it
316  if (!transitionTest(childCost.value(), nearMotion->cost.value(), motionDistance))
317  {
318  continue; // give up on this one and try a new sample
319  }
320 
321  // V.
322 
323  // Create a motion
324  Motion *motion = new Motion(si_);
325  si_->copyState(motion->state, newState);
326  motion->parent = nearMotion; // link q_new to q_near as an edge
327  motion->cost = childCost;
328 
329  // Add motion to data structure
330  nearestNeighbors_->add(motion);
331 
332  // VI.
333 
334  // Check if this motion is the goal
335  double distToGoal = 0.0;
336  bool isSatisfied = goal->isSatisfied(motion->state, &distToGoal);
337  if (isSatisfied)
338  {
339  approxDifference = distToGoal; // the tolerated error distance btw state and goal
340  solution = motion; // set the final solution
341  break;
342  }
343 
344  // Is this the closest solution we've found so far
345  if (distToGoal < approxDifference)
346  {
347  approxDifference = distToGoal;
348  approxSolution = motion;
349  }
350 
351  } // end of solver sampling loop
352 
353 
354  // Finish solution processing --------------------------------------------------------------------
355 
356  bool solved = false;
357  bool approximate = false;
358 
359  // Substitute an empty solution with the best approximation
360  if (solution == NULL)
361  {
362  solution = approxSolution;
363  approximate = true;
364  }
365 
366  // Generate solution path for real/approx solution
367  if (solution != NULL)
368  {
369  lastGoalMotion_ = solution;
370 
371  // construct the solution path
372  std::vector<Motion*> mpath;
373  while (solution != NULL)
374  {
375  mpath.push_back(solution);
376  solution = solution->parent;
377  }
378 
379  // set the solution path
380  PathGeometric *path = new PathGeometric(si_);
381  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
382  path->append(mpath[i]->state);
383 
384  pdef_->addSolutionPath(base::PathPtr(path), approximate, approxDifference, getName());
385  solved = true;
386  }
387 
388  // Clean up ---------------------------------------------------------------------------------------
389 
390  si_->freeState(interpolatedState);
391  if (randMotion->state)
392  si_->freeState(randMotion->state);
393  delete randMotion;
394 
395  OMPL_INFORM("%s: Created %u states", getName().c_str(), nearestNeighbors_->size());
396 
397  return base::PlannerStatus(solved, approximate);
398 }
399 
401 {
402  Planner::getPlannerData(data);
403 
404  std::vector<Motion*> motions;
405  if (nearestNeighbors_)
406  nearestNeighbors_->list(motions);
407 
408  if (lastGoalMotion_)
410 
411  for (unsigned int i = 0 ; i < motions.size() ; ++i)
412  {
413  if (motions[i]->parent == NULL)
414  data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
415  else
416  data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
417  base::PlannerDataVertex(motions[i]->state));
418  }
419 }
420 
421 bool ompl::geometric::TRRT::transitionTest(double childCost, double parentCost, double distance)
422 {
423  // Always accept if new state has same or lower cost than old state
424  if (childCost <= parentCost)
425  return true;
426 
427  // Difference in cost
428  double costSlope = (childCost - parentCost) / distance;
429 
430  // The probability of acceptance of a new configuration is defined by comparing its cost c_j
431  // relatively to the cost c_i of its parent in the tree. Based on the Metropolis criterion.
432  double transitionProbability = 1.; // if cost_slope is <= 0, probabilty is 1
433 
434  // Only return at end
435  bool result = false;
436 
437  // Calculate tranision probabilty
438  if (costSlope > 0)
439  {
440  transitionProbability = exp(-costSlope / (kConstant_ * temp_));
441  }
442 
443  // Check if we can accept it
444  if (rng_.uniform01() <= transitionProbability)
445  {
446  if (temp_ > minTemperature_)
447  {
449 
450  // Prevent temp_ from getting too small
451  if (temp_ < minTemperature_)
452  {
454  }
455  }
456 
457  numStatesFailed_ = 0;
458 
459  result = true;
460  }
461  else
462  {
463  // State has failed
465  {
467  numStatesFailed_ = 0;
468  }
469  else
470  {
472  }
473 
474  }
475 
476  return result;
477 }
478 
479 bool ompl::geometric::TRRT::minExpansionControl(double randMotionDistance)
480 {
481  // Decide to accept or not
482  if (randMotionDistance > frontierThreshold_)
483  {
484  // participates in the tree expansion
485  ++frontierCount_;
486 
487  return true;
488  }
489  else
490  {
491  // participates in the tree refinement
492 
493  // check our ratio first before accepting it
494  if ((double)nonfrontierCount_ / (double)frontierCount_ > frontierNodeRatio_)
495  {
496  // Increment so that the temperature rises faster
498 
499  // reject this node as being too much refinement
500  return false;
501  }
502  else
503  {
505  return true;
506  }
507  }
508 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
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
boost::shared_ptr< NearestNeighbors< Motion * > > nearestNeighbors_
A nearest-neighbors datastructure containing the tree of motions.
Definition: TRRT.h:283
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.
double temp_
Temperature parameter used to control the difficulty level of transition tests. Low temperatures limi...
Definition: TRRT.h:309
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...
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
double getGoalBias() const
Get the goal bias the planner is using.
Definition: TRRT.h:107
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...
RNG rng_
The random number generator.
Definition: TRRT.h:292
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:421
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:400
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:222
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
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:62
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
double getRange() const
Get the range the planner is using.
Definition: TRRT.h:123
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
ompl::base::OptimizationObjectivePtr opt_
The optimization objective being optimized by TRRT.
Definition: TRRT.h:345
double value() const
The value of the cost.
Definition: Cost.h:54
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...
base::StateSamplerPtr sampler_
State sampler.
Definition: TRRT.h:280
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
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:400
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 void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:100
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:174
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:403
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:409
double getMaxStatesFailed(void) const
Get the maximum number of states that can be rejected before the temperature starts to rise...
Definition: TRRT.h:135
const State * nextStart()
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:230
#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:479
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:232
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:58
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
double nonfrontierCount_
Ratio counters for nodes that expand the search space versus those that do not.
Definition: TRRT.h:335
Definition of a geometric path.
Definition: PathGeometric.h:60
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
double tempChangeFactor_
Failure temperature factor used when max_num_failed_ failures occur.
Definition: TRRT.h:320
void freeMemory()
Free the memory allocated by this planner.
Definition: TRRT.cpp:157
double frontierNodeRatio_
Target ratio of nonfrontier nodes to frontier nodes. rho.
Definition: TRRT.h:342
unsigned int numStatesFailed_
Failure counter for states that are rejected.
Definition: TRRT.h:329
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: TRRT.cpp:96
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: TRRT.cpp:80
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:178
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:55
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68