BiTRRT.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Rice University
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 Rice University 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: Ryan Luna */
36 
37 #include <limits>
38 
39 #include "ompl/geometric/planners/rrt/BiTRRT.h"
40 #include "ompl/base/goals/GoalSampleableRegion.h"
41 #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h"
42 #include "ompl/tools/config/MagicConstants.h"
43 #include "ompl/tools/config/SelfConfig.h"
44 
45 ompl::geometric::BiTRRT::BiTRRT(const base::SpaceInformationPtr &si) : base::Planner(si, "BiTRRT")
46 {
48  specs_.directed = true;
49 
50  Planner::declareParam<double>("range", this, &BiTRRT::setRange, &BiTRRT::getRange, "0.:1.:10000.");
51 
52  // BiTRRT Specific Variables
53  setTempChangeFactor(0.1); // how much to increase the temp each time
54 
55  Planner::declareParam<double>("temp_change_factor", this, &BiTRRT::setTempChangeFactor,
56  &BiTRRT::getTempChangeFactor, "0.:.1:1.");
57  Planner::declareParam<double>("init_temperature", this, &BiTRRT::setInitTemperature, &BiTRRT::getInitTemperature);
58  Planner::declareParam<double>("frontier_threshold", this, &BiTRRT::setFrontierThreshold,
60  Planner::declareParam<double>("frontier_node_ratio", this, &BiTRRT::setFrontierNodeRatio,
62  Planner::declareParam<double>("cost_threshold", this, &BiTRRT::setCostThreshold, &BiTRRT::getCostThreshold);
63 }
64 
65 ompl::geometric::BiTRRT::~BiTRRT()
66 {
67  freeMemory();
68 }
69 
71 {
72  std::vector<Motion *> motions;
73 
74  if (tStart_)
75  {
76  tStart_->list(motions);
77  for (auto &motion : motions)
78  {
79  if (motion->state != nullptr)
80  si_->freeState(motion->state);
81  delete motion;
82  }
83  }
84 
85  if (tGoal_)
86  {
87  tGoal_->list(motions);
88  for (auto &motion : motions)
89  {
90  if (motion->state != nullptr)
91  si_->freeState(motion->state);
92  delete motion;
93  }
94  }
95 }
96 
98 {
99  Planner::clear();
100  freeMemory();
101  if (tStart_)
102  tStart_->clear();
103  if (tGoal_)
104  tGoal_->clear();
105  connectionPoint_ = std::make_pair<Motion *, Motion *>(nullptr, nullptr);
106 
107  // TRRT specific variables
109  nonfrontierCount_ = 1;
110  frontierCount_ = 1; // init to 1 to prevent division by zero error
111  if (opt_)
112  bestCost_ = worstCost_ = opt_->identityCost();
113 }
114 
116 {
117  Planner::setup();
119 
120  // Configuring the range of the planner
121  if (maxDistance_ < std::numeric_limits<double>::epsilon())
122  {
125  }
126 
127  // Configuring nearest neighbors structures for the planning trees
128  if (!tStart_)
129  tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
130  if (!tGoal_)
131  tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
132  tStart_->setDistanceFunction([this](const Motion *a, const Motion *b)
133  {
134  return distanceFunction(a, b);
135  });
136  tGoal_->setDistanceFunction([this](const Motion *a, const Motion *b)
137  {
138  return distanceFunction(a, b);
139  });
140 
141  // Setup the optimization objective, if it isn't specified
142  if (!pdef_ || !pdef_->hasOptimizationObjective())
143  {
144  OMPL_INFORM("%s: No optimization objective specified. Defaulting to mechanical work minimization.",
145  getName().c_str());
146  opt_ = std::make_shared<base::MechanicalWorkOptimizationObjective>(si_);
147  }
148  else
149  opt_ = pdef_->getOptimizationObjective();
150 
151  // Set the threshold that decides if a new node is a frontier node or non-frontier node
152  if (frontierThreshold_ < std::numeric_limits<double>::epsilon())
153  {
154  frontierThreshold_ = si_->getMaximumExtent() * 0.01;
155  OMPL_DEBUG("%s: Frontier threshold detected to be %lf", getName().c_str(), frontierThreshold_);
156  }
157 
158  // initialize TRRT specific variables
160  nonfrontierCount_ = 1;
161  frontierCount_ = 1; // init to 1 to prevent division by zero error
162  bestCost_ = worstCost_ = opt_->identityCost();
163  connectionRange_ = 10.0 * si_->getStateSpace()->getLongestValidSegmentLength();
164 }
165 
167  Motion *parent)
168 {
169  auto *motion = new Motion(si_);
170  si_->copyState(motion->state, state);
171  motion->cost = opt_->stateCost(motion->state);
172  motion->parent = parent;
173  motion->root = parent != nullptr ? parent->root : nullptr;
174 
175  if (opt_->isCostBetterThan(motion->cost, bestCost_)) // motion->cost is better than the existing best
176  bestCost_ = motion->cost;
177  if (opt_->isCostBetterThan(worstCost_, motion->cost)) // motion->cost is worse than the existing worst
178  worstCost_ = motion->cost;
179 
180  // Add start motion to the tree
181  tree->add(motion);
182  return motion;
183 }
184 
186 {
187  // Disallow any cost that is not better than the cost threshold
188  if (!opt_->isCostBetterThan(motionCost, costThreshold_))
189  return false;
190 
191  // Always accept if the cost is near or below zero
192  if (motionCost.value() < 1e-4)
193  return true;
194 
195  double dCost = motionCost.value();
196  double transitionProbability = exp(-dCost / temp_);
197  if (transitionProbability > 0.5)
198  {
199  double costRange = worstCost_.value() - bestCost_.value();
200  if (fabs(costRange) > 1e-4) // Do not divide by zero
201  // Successful transition test. Decrease the temperature slightly
202  temp_ /= exp(dCost / (0.1 * costRange));
203 
204  return true;
205  }
206 
207  // The transition failed. Increase the temperature (slightly)
209  return false;
210 }
211 
213 {
214  if (dist > frontierThreshold_) // Exploration
215  {
216  ++frontierCount_;
217  return true;
218  }
219  // Refinement
220  // Check the current ratio first before accepting it
221  if ((double)nonfrontierCount_ / (double)frontierCount_ > frontierNodeRatio_)
222  return false;
223 
225  return true;
226 }
227 
229  Motion *toMotion, Motion *&result)
230 {
231  bool reach = true;
232 
233  // Compute the state to extend toward
234  double d = si_->distance(nearest->state, toMotion->state);
235  // Truncate the random state to be no more than maxDistance_ from nearest neighbor
236  if (d > maxDistance_)
237  {
238  si_->getStateSpace()->interpolate(nearest->state, toMotion->state, maxDistance_ / d, toMotion->state);
239  d = maxDistance_;
240  reach = false;
241  }
242 
243  // Validating the motion
244  // If we are in the goal tree, we validate the motion in reverse
245  // si_->checkMotion assumes that the first argument is valid, so we must check this explicitly
246  // If the motion is valid, check the probabilistic transition test and the
247  // expansion control to ensure high quality nodes are added.
248  bool validMotion =
249  (tree == tStart_ ? si_->checkMotion(nearest->state, toMotion->state) :
250  si_->isValid(toMotion->state) && si_->checkMotion(toMotion->state, nearest->state)) &&
251  transitionTest(opt_->motionCost(nearest->state, toMotion->state)) && minExpansionControl(d);
252 
253  if (validMotion)
254  {
255  result = addMotion(toMotion->state, tree, nearest);
256  return reach ? SUCCESS : ADVANCED;
257  }
258 
259  return FAILED;
260 }
261 
263  Motion *&result)
264 {
265  // Nearest neighbor
266  Motion *nearest = tree->nearest(toMotion);
267  return extendTree(nearest, tree, toMotion, result);
268 }
269 
271 {
272  // Get the nearest state to nmotion in tree (nmotion is NOT in tree)
273  Motion *nearest = tree->nearest(nmotion);
274  double dist = si_->distance(nearest->state, nmotion->state);
275 
276  // Do not attempt a connection if the trees are far apart
277  if (dist > connectionRange_)
278  return false;
279 
280  // Copy the resulting state into our scratch space
281  si_->copyState(xmotion->state, nmotion->state);
282 
283  // Do not try to connect states directly. Must chop up the
284  // extension into segments, just in case one piece fails
285  // the transition test
286  GrowResult result;
287  Motion *next = nullptr;
288  do
289  {
290  // Extend tree from nearest toward xmotion
291  // Store the result into next
292  // This function MAY trash xmotion
293  result = extendTree(nearest, tree, xmotion, next);
294 
295  if (result == ADVANCED)
296  {
297  nearest = next;
298 
299  // xmotion may get trashed during extension, so we reload it here
300  si_->copyState(xmotion->state,
301  nmotion->state); // xmotion may get trashed during extension, so we reload it here
302  }
303  } while (result == ADVANCED);
304 
305  // Successful connection
306  if (result == SUCCESS)
307  {
308  bool treeIsStart = tree == tStart_;
309  Motion *startMotion = treeIsStart ? next : nmotion;
310  Motion *goalMotion = treeIsStart ? nmotion : next;
311 
312  // Make sure start-goal pair is valid
313  if (pdef_->getGoal()->isStartGoalPairValid(startMotion->root, goalMotion->root))
314  {
315  // Since we have connected, nmotion->state and next->state have the same value
316  // We need to check one of their parents to avoid a duplicate state in the solution path
317  // One of these must be true, since we do not ever attempt to connect start and goal directly.
318  if (startMotion->parent != nullptr)
319  startMotion = startMotion->parent;
320  else
321  goalMotion = goalMotion->parent;
322 
323  connectionPoint_ = std::make_pair(startMotion, goalMotion);
324  return true;
325  }
326  }
327 
328  return false;
329 }
330 
332 {
333  // Basic error checking
334  checkValidity();
335 
336  // Goal information
337  base::Goal *goal = pdef_->getGoal().get();
338  auto *gsr = dynamic_cast<base::GoalSampleableRegion *>(goal);
339 
340  if (gsr == nullptr)
341  {
342  OMPL_ERROR("%s: Goal object does not derive from GoalSampleableRegion", getName().c_str());
344  }
345 
346  // Loop through the (valid) input states and add them to the start tree
347  while (const base::State *state = pis_.nextStart())
348  {
349  auto *motion = new Motion(si_);
350  si_->copyState(motion->state, state);
351  motion->cost = opt_->stateCost(motion->state);
352  motion->root = motion->state; // this state is the root of a tree
353 
354  if (tStart_->size() == 0) // do not overwrite best/worst from a prior call to solve
355  worstCost_ = bestCost_ = motion->cost;
356 
357  // Add start motion to the tree
358  tStart_->add(motion);
359  }
360 
361  if (tStart_->size() == 0)
362  {
363  OMPL_ERROR("%s: Start tree has no valid states!", getName().c_str());
365  }
366 
367  // Do the same for the goal tree, if it is empty, but only once
368  if (tGoal_->size() == 0)
369  {
370  const base::State *state = pis_.nextGoal(ptc);
371  if (state != nullptr)
372  {
373  Motion *motion = addMotion(state, tGoal_);
374  motion->root = motion->state; // this state is the root of a tree
375  }
376  }
377 
378  if (tGoal_->size() == 0)
379  {
380  OMPL_ERROR("%s: Goal tree has no valid states!", getName().c_str());
382  }
383 
384  OMPL_INFORM("%s: Planning started with %d states already in datastructure", getName().c_str(),
385  (int)(tStart_->size() + tGoal_->size()));
386 
387  base::StateSamplerPtr sampler = si_->allocStateSampler();
388 
389  auto *rmotion = new Motion(si_);
390  base::State *rstate = rmotion->state;
391 
392  auto *xmotion = new Motion(si_);
393  base::State *xstate = xmotion->state;
394 
395  TreeData tree = tStart_;
396  TreeData otherTree = tGoal_;
397 
398  bool solved = false;
399  // Planning loop
400  while (!ptc)
401  {
402  // Check if there are more goal states
403  if (pis_.getSampledGoalsCount() < tGoal_->size() / 2)
404  {
405  if (const base::State *state = pis_.nextGoal())
406  {
407  Motion *motion = addMotion(state, tGoal_);
408  motion->root = motion->state; // this state is the root of a tree
409  }
410  }
411 
412  // Sample a state uniformly at random
413  sampler->sampleUniform(rstate);
414 
415  Motion *result; // the motion that gets added in extendTree
416  if (extendTree(rmotion, tree, result) != FAILED) // we added something new to the tree
417  {
418  // Try to connect the other tree to the node we just added
419  if (connectTrees(result, otherTree, xmotion))
420  {
421  // The trees have been connected. Construct the solution path
422  Motion *solution = connectionPoint_.first;
423  std::vector<Motion *> mpath1;
424  while (solution != nullptr)
425  {
426  mpath1.push_back(solution);
427  solution = solution->parent;
428  }
429 
430  solution = connectionPoint_.second;
431  std::vector<Motion *> mpath2;
432  while (solution != nullptr)
433  {
434  mpath2.push_back(solution);
435  solution = solution->parent;
436  }
437 
438  auto path(std::make_shared<PathGeometric>(si_));
439  path->getStates().reserve(mpath1.size() + mpath2.size());
440  for (int i = mpath1.size() - 1; i >= 0; --i)
441  path->append(mpath1[i]->state);
442  for (auto &i : mpath2)
443  path->append(i->state);
444 
445  pdef_->addSolutionPath(path, false, 0.0, getName());
446  solved = true;
447  break;
448  }
449  }
450 
451  std::swap(tree, otherTree);
452  }
453 
454  si_->freeState(rstate);
455  si_->freeState(xstate);
456  delete rmotion;
457  delete xmotion;
458 
459  OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(),
460  tStart_->size(), tGoal_->size());
462 }
463 
465 {
466  Planner::getPlannerData(data);
467 
468  std::vector<Motion *> motions;
469  if (tStart_)
470  tStart_->list(motions);
471  for (auto &motion : motions)
472  {
473  if (motion->parent == nullptr)
474  data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
475  else
476  {
477  data.addEdge(base::PlannerDataVertex(motion->parent->state, 1), base::PlannerDataVertex(motion->state, 1));
478  }
479  }
480 
481  motions.clear();
482  if (tGoal_)
483  tGoal_->list(motions);
484  for (auto &motion : motions)
485  {
486  if (motion->parent == nullptr)
487  data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
488  else
489  {
490  // The edges in the goal tree are reversed to be consistent with start tree
491  data.addEdge(base::PlannerDataVertex(motion->state, 2), base::PlannerDataVertex(motion->parent->state, 2));
492  }
493  }
494 
495  // Add the edge connecting the two trees
496  if ((connectionPoint_.first != nullptr) && (connectionPoint_.second != nullptr))
497  data.addEdge(data.vertexIndex(connectionPoint_.first->state), data.vertexIndex(connectionPoint_.second->state));
498 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:203
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: BiTRRT.cpp:331
Motion * addMotion(const base::State *state, TreeData &tree, Motion *parent=nullptr)
Add a state to the given tree. The motion created is returned.
Definition: BiTRRT.cpp:166
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BiTRRT.cpp:115
double frontierThreshold_
The distance between an existing state and a new state that qualifies it as a frontier state...
Definition: BiTRRT.h:281
base::State * state
The state contained by the motion.
Definition: BiTRRT.h:197
bool minExpansionControl(double dist)
Use frontier node ratio to filter nodes that do not add new information to the search tree...
Definition: BiTRRT.cpp:212
The planner failed to find a solution.
Definition: PlannerStatus.h:62
bool transitionTest(const base::Cost &motionCost)
Transition test that filters transitions based on the motion cost. If the motion cost is near or belo...
Definition: BiTRRT.cpp:185
GrowResult extendTree(Motion *toMotion, TreeData &tree, Motion *&result)
Extend tree toward the state in rmotion. Store the result of the extension, if any, in result.
Definition: BiTRRT.cpp:262
base::Cost bestCost_
The most desirable (e.g., minimum) cost value in the search tree.
Definition: BiTRRT.h:268
A shared pointer wrapper for ompl::base::StateSampler.
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:264
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...
double getFrontierThreshold() const
Get the distance between a new state and the nearest neighbor that qualifies a state as being a front...
Definition: BiTRRT.h:149
Abstract definition of goals.
Definition: Goal.h:62
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
Motion * parent
The parent motion in the exploration tree.
Definition: BiTRRT.h:200
double nonfrontierCount_
A count of the number of non-frontier nodes in the trees.
Definition: BiTRRT.h:290
TreeData tStart_
The start tree.
Definition: BiTRRT.h:304
BiTRRT(const base::SpaceInformationPtr &si)
Constructor.
Definition: BiTRRT.cpp:45
std::shared_ptr< NearestNeighbors< Motion * > > TreeData
The nearest-neighbors data structure that contains the entire the tree of motions generated during pl...
Definition: BiTRRT.h:215
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:213
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: BiTRRT.cpp:464
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:175
double initTemperature_
The temperature that planning begins at.
Definition: BiTRRT.h:277
double connectionRange_
The range at which the algorithm will attempt to connect the two trees.
Definition: BiTRRT.h:297
void setFrontierNodeRatio(double frontierNodeRatio)
Set the ratio between adding non-frontier nodes to frontier nodes. For example: .1 is one non-frontie...
Definition: BiTRRT.h:157
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
base::Cost worstCost_
The least desirable (e.g., maximum) cost value in the search tree.
Definition: BiTRRT.h:271
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
double tempChangeFactor_
The factor by which the temperature is increased after a failed transition test.
Definition: BiTRRT.h:265
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: BiTRRT.h:261
Abstract definition of a goal region that can be sampled.
std::pair< Motion *, Motion * > connectionPoint_
The most recent connection point for the two trees. Used for PlannerData computation.
Definition: BiTRRT.h:301
double getInitTemperature() const
Get the initial temperature at the start of planning.
Definition: BiTRRT.h:135
void freeMemory()
Free all memory allocated during planning.
Definition: BiTRRT.cpp:70
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
double getRange() const
Get the range the planner is using.
Definition: BiTRRT.h:89
double getFrontierNodeRatio() const
Get the ratio between adding non-frontier nodes to frontier nodes.
Definition: BiTRRT.h:164
The planner found an exact solution.
Definition: PlannerStatus.h:66
GrowResult
The result of a call to extendTree.
Definition: BiTRRT.h:232
const base::State * root
Pointer to the root of the tree this motion is contained in.
Definition: BiTRRT.h:207
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: BiTRRT.h:99
TreeData tGoal_
The goal tree.
Definition: BiTRRT.h:307
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...
void setInitTemperature(double initTemperature)
Set the initial temperature at the start of planning. Should be high to allow for initial exploration...
Definition: BiTRRT.h:129
Progress was made toward extension.
Definition: BiTRRT.h:237
A shared pointer wrapper for ompl::base::SpaceInformation.
double getTempChangeFactor() const
Get the factor by which the temperature is increased after a failed transition.
Definition: BiTRRT.h:106
double frontierCount_
A count of the number of frontier nodes in the trees.
Definition: BiTRRT.h:293
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:49
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: BiTRRT.h:255
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:101
ompl::base::OptimizationObjectivePtr opt_
The objective (cost function) being optimized.
Definition: BiTRRT.h:310
*double getCostThreshold() const
Get the cost threshold (default is infinity). Any motion cost that is not better than this cost (acco...
Definition: BiTRRT.h:122
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
void setFrontierThreshold(double frontierThreshold)
Set the distance between a new state and the nearest neighbor that qualifies a state as being a front...
Definition: BiTRRT.h:142
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
No extension was possible.
Definition: BiTRRT.h:235
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Representation of a motion in the search tree.
Definition: BiTRRT.h:183
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.
double frontierNodeRatio_
The target ratio of non-frontier nodes to frontier nodes.
Definition: BiTRRT.h:284
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
void setRange(double distance)
Set the maximum possible length of any one motion in the search tree. Very short/long motions may inh...
Definition: BiTRRT.h:83
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:59
double value() const
The value of the cost.
Definition: Cost.h:56
base::Cost costThreshold_
All motion costs must be better than this cost (default is infinity)
Definition: BiTRRT.h:274
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
void setCostThreshold(double maxCost)
Set the cost threshold (default is infinity). Any motion cost that is not better than this cost (acco...
Definition: BiTRRT.h:114
bool connectTrees(Motion *nmotion, TreeData &tree, Motion *xmotion)
Attempt to connect tree to nmotion, which is in the other tree. xmotion is scratch space and will be ...
Definition: BiTRRT.cpp:270
The desired state was reached during extension.
Definition: BiTRRT.h:239
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BiTRRT.cpp:97
double temp_
The current temperature.
Definition: BiTRRT.h:287
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68