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  maxDistance_ = 0.0; // set in setup()
51  connectionPoint_ = std::make_pair<Motion *, Motion *>(nullptr, nullptr);
52 
53  Planner::declareParam<double>("range", this, &BiTRRT::setRange, &BiTRRT::getRange, "0.:1.:10000.");
54 
55  // BiTRRT Specific Variables
56  frontierThreshold_ = 0.0; // set in setup()
57  setTempChangeFactor(0.1); // how much to increase the temp each time
58  costThreshold_ = base::Cost(std::numeric_limits<double>::infinity());
59  initTemperature_ = 100; // where the temperature starts out
60  frontierNodeRatio_ = 0.1; // 1/10, or 1 non-frontier for every 10 frontier
61 
62  Planner::declareParam<double>("temp_change_factor", this, &BiTRRT::setTempChangeFactor,
63  &BiTRRT::getTempChangeFactor, "0.:.1:1.");
64  Planner::declareParam<double>("init_temperature", this, &BiTRRT::setInitTemperature, &BiTRRT::getInitTemperature);
65  Planner::declareParam<double>("frontier_threshold", this, &BiTRRT::setFrontierThreshold,
67  Planner::declareParam<double>("frontier_node_ratio", this, &BiTRRT::setFrontierNodeRatio,
69  Planner::declareParam<double>("cost_threshold", this, &BiTRRT::setCostThreshold, &BiTRRT::getCostThreshold);
70 }
71 
72 ompl::geometric::BiTRRT::~BiTRRT()
73 {
74  freeMemory();
75 }
76 
78 {
79  std::vector<Motion *> motions;
80 
81  if (tStart_)
82  {
83  tStart_->list(motions);
84  for (auto &motion : motions)
85  {
86  if (motion->state)
87  si_->freeState(motion->state);
88  delete motion;
89  }
90  }
91 
92  if (tGoal_)
93  {
94  tGoal_->list(motions);
95  for (auto &motion : motions)
96  {
97  if (motion->state)
98  si_->freeState(motion->state);
99  delete motion;
100  }
101  }
102 }
103 
105 {
106  Planner::clear();
107  freeMemory();
108  if (tStart_)
109  tStart_->clear();
110  if (tGoal_)
111  tGoal_->clear();
112  connectionPoint_ = std::make_pair<Motion *, Motion *>(nullptr, nullptr);
113 
114  // TRRT specific variables
116  nonfrontierCount_ = 1;
117  frontierCount_ = 1; // init to 1 to prevent division by zero error
118  if (opt_)
119  bestCost_ = worstCost_ = opt_->identityCost();
120 }
121 
123 {
124  Planner::setup();
126 
127  // Configuring the range of the planner
128  if (maxDistance_ < std::numeric_limits<double>::epsilon())
129  {
132  }
133 
134  // Configuring nearest neighbors structures for the planning trees
135  if (!tStart_)
136  tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
137  if (!tGoal_)
138  tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
139  tStart_->setDistanceFunction([this](const Motion *a, const Motion *b)
140  {
141  return distanceFunction(a, b);
142  });
143  tGoal_->setDistanceFunction([this](const Motion *a, const Motion *b)
144  {
145  return distanceFunction(a, b);
146  });
147 
148  // Setup the optimization objective, if it isn't specified
149  if (!pdef_ || !pdef_->hasOptimizationObjective())
150  {
151  OMPL_INFORM("%s: No optimization objective specified. Defaulting to mechanical work minimization.",
152  getName().c_str());
153  opt_ = std::make_shared<base::MechanicalWorkOptimizationObjective>(si_);
154  }
155  else
156  opt_ = pdef_->getOptimizationObjective();
157 
158  // Set the threshold that decides if a new node is a frontier node or non-frontier node
159  if (frontierThreshold_ < std::numeric_limits<double>::epsilon())
160  {
161  frontierThreshold_ = si_->getMaximumExtent() * 0.01;
162  OMPL_DEBUG("%s: Frontier threshold detected to be %lf", getName().c_str(), frontierThreshold_);
163  }
164 
165  // initialize TRRT specific variables
167  nonfrontierCount_ = 1;
168  frontierCount_ = 1; // init to 1 to prevent division by zero error
169  bestCost_ = worstCost_ = opt_->identityCost();
170  connectionRange_ = 10.0 * si_->getStateSpace()->getLongestValidSegmentLength();
171 }
172 
174  Motion *parent)
175 {
176  auto *motion = new Motion(si_);
177  si_->copyState(motion->state, state);
178  motion->cost = opt_->stateCost(motion->state);
179  motion->parent = parent;
180  motion->root = parent ? parent->root : nullptr;
181 
182  if (opt_->isCostBetterThan(motion->cost, bestCost_)) // motion->cost is better than the existing best
183  bestCost_ = motion->cost;
184  if (opt_->isCostBetterThan(worstCost_, motion->cost)) // motion->cost is worse than the existing worst
185  worstCost_ = motion->cost;
186 
187  // Add start motion to the tree
188  tree->add(motion);
189  return motion;
190 }
191 
193 {
194  // Disallow any cost that is not better than the cost threshold
195  if (!opt_->isCostBetterThan(motionCost, costThreshold_))
196  return false;
197 
198  // Always accept if the cost is near or below zero
199  if (motionCost.value() < 1e-4)
200  return true;
201 
202  double dCost = motionCost.value();
203  double transitionProbability = exp(-dCost / temp_);
204  if (transitionProbability > 0.5)
205  {
206  double costRange = worstCost_.value() - bestCost_.value();
207  if (fabs(costRange) > 1e-4) // Do not divide by zero
208  // Successful transition test. Decrease the temperature slightly
209  temp_ /= exp(dCost / (0.1 * costRange));
210 
211  return true;
212  }
213 
214  // The transition failed. Increase the temperature (slightly)
216  return false;
217 }
218 
220 {
221  if (dist > frontierThreshold_) // Exploration
222  {
223  ++frontierCount_;
224  return true;
225  }
226  else // Refinement
227  {
228  // Check the current ratio first before accepting it
229  if ((double)nonfrontierCount_ / (double)frontierCount_ > frontierNodeRatio_)
230  return false;
231 
233  return true;
234  }
235 }
236 
238  Motion *toMotion, Motion *&result)
239 {
240  bool reach = true;
241 
242  // Compute the state to extend toward
243  double d = si_->distance(nearest->state, toMotion->state);
244  // Truncate the random state to be no more than maxDistance_ from nearest neighbor
245  if (d > maxDistance_)
246  {
247  si_->getStateSpace()->interpolate(nearest->state, toMotion->state, maxDistance_ / d, toMotion->state);
248  d = maxDistance_;
249  reach = false;
250  }
251 
252  // Validating the motion
253  // If we are in the goal tree, we validate the motion in reverse
254  // si_->checkMotion assumes that the first argument is valid, so we must check this explicitly
255  // If the motion is valid, check the probabilistic transition test and the
256  // expansion control to ensure high quality nodes are added.
257  bool validMotion =
258  (tree == tStart_ ? si_->checkMotion(nearest->state, toMotion->state) :
259  si_->isValid(toMotion->state) && si_->checkMotion(toMotion->state, nearest->state)) &&
260  transitionTest(opt_->motionCost(nearest->state, toMotion->state)) && minExpansionControl(d);
261 
262  if (validMotion)
263  {
264  result = addMotion(toMotion->state, tree, nearest);
265  return reach ? SUCCESS : ADVANCED;
266  }
267 
268  return FAILED;
269 }
270 
272  Motion *&result)
273 {
274  // Nearest neighbor
275  Motion *nearest = tree->nearest(toMotion);
276  return extendTree(nearest, tree, toMotion, result);
277 }
278 
280 {
281  // Get the nearest state to nmotion in tree (nmotion is NOT in tree)
282  Motion *nearest = tree->nearest(nmotion);
283  double dist = si_->distance(nearest->state, nmotion->state);
284 
285  // Do not attempt a connection if the trees are far apart
286  if (dist > connectionRange_)
287  return false;
288 
289  // Copy the resulting state into our scratch space
290  si_->copyState(xmotion->state, nmotion->state);
291 
292  // Do not try to connect states directly. Must chop up the
293  // extension into segments, just in case one piece fails
294  // the transition test
295  GrowResult result;
296  Motion *next = nullptr;
297  do
298  {
299  // Extend tree from nearest toward xmotion
300  // Store the result into next
301  // This function MAY trash xmotion
302  result = extendTree(nearest, tree, xmotion, next);
303 
304  if (result == ADVANCED)
305  {
306  nearest = next;
307 
308  // xmotion may get trashed during extension, so we reload it here
309  si_->copyState(xmotion->state,
310  nmotion->state); // xmotion may get trashed during extension, so we reload it here
311  }
312  } while (result == ADVANCED);
313 
314  // Successful connection
315  if (result == SUCCESS)
316  {
317  bool treeIsStart = tree == tStart_;
318  Motion *startMotion = treeIsStart ? next : nmotion;
319  Motion *goalMotion = treeIsStart ? nmotion : next;
320 
321  // Make sure start-goal pair is valid
322  if (pdef_->getGoal()->isStartGoalPairValid(startMotion->root, goalMotion->root))
323  {
324  // Since we have connected, nmotion->state and next->state have the same value
325  // We need to check one of their parents to avoid a duplicate state in the solution path
326  // One of these must be true, since we do not ever attempt to connect start and goal directly.
327  if (startMotion->parent)
328  startMotion = startMotion->parent;
329  else
330  goalMotion = goalMotion->parent;
331 
332  connectionPoint_ = std::make_pair(startMotion, goalMotion);
333  return true;
334  }
335  }
336 
337  return false;
338 }
339 
341 {
342  // Basic error checking
343  checkValidity();
344 
345  // Goal information
346  base::Goal *goal = pdef_->getGoal().get();
347  base::GoalSampleableRegion *gsr = dynamic_cast<base::GoalSampleableRegion *>(goal);
348 
349  if (!gsr)
350  {
351  OMPL_ERROR("%s: Goal object does not derive from GoalSampleableRegion", getName().c_str());
353  }
354 
355  // Loop through the (valid) input states and add them to the start tree
356  while (const base::State *state = pis_.nextStart())
357  {
358  auto *motion = new Motion(si_);
359  si_->copyState(motion->state, state);
360  motion->cost = opt_->stateCost(motion->state);
361  motion->root = motion->state; // this state is the root of a tree
362 
363  if (tStart_->size() == 0) // do not overwrite best/worst from a prior call to solve
364  worstCost_ = bestCost_ = motion->cost;
365 
366  // Add start motion to the tree
367  tStart_->add(motion);
368  }
369 
370  if (tStart_->size() == 0)
371  {
372  OMPL_ERROR("%s: Start tree has no valid states!", getName().c_str());
374  }
375 
376  // Do the same for the goal tree, if it is empty, but only once
377  if (tGoal_->size() == 0)
378  {
379  const base::State *state = pis_.nextGoal(ptc);
380  if (state)
381  {
382  Motion *motion = addMotion(state, tGoal_);
383  motion->root = motion->state; // this state is the root of a tree
384  }
385  }
386 
387  if (tGoal_->size() == 0)
388  {
389  OMPL_ERROR("%s: Goal tree has no valid states!", getName().c_str());
391  }
392 
393  OMPL_INFORM("%s: Planning started with %d states already in datastructure", getName().c_str(),
394  (int)(tStart_->size() + tGoal_->size()));
395 
396  base::StateSamplerPtr sampler = si_->allocStateSampler();
397 
398  auto *rmotion = new Motion(si_);
399  base::State *rstate = rmotion->state;
400 
401  auto *xmotion = new Motion(si_);
402  base::State *xstate = xmotion->state;
403 
404  TreeData tree = tStart_;
405  TreeData otherTree = tGoal_;
406 
407  bool solved = false;
408  // Planning loop
409  while (ptc == false)
410  {
411  // Check if there are more goal states
412  if (pis_.getSampledGoalsCount() < tGoal_->size() / 2)
413  {
414  if (const base::State *state = pis_.nextGoal())
415  {
416  Motion *motion = addMotion(state, tGoal_);
417  motion->root = motion->state; // this state is the root of a tree
418  }
419  }
420 
421  // Sample a state uniformly at random
422  sampler->sampleUniform(rstate);
423 
424  Motion *result; // the motion that gets added in extendTree
425  if (extendTree(rmotion, tree, result) != FAILED) // we added something new to the tree
426  {
427  // Try to connect the other tree to the node we just added
428  if (connectTrees(result, otherTree, xmotion))
429  {
430  // The trees have been connected. Construct the solution path
431  Motion *solution = connectionPoint_.first;
432  std::vector<Motion *> mpath1;
433  while (solution != nullptr)
434  {
435  mpath1.push_back(solution);
436  solution = solution->parent;
437  }
438 
439  solution = connectionPoint_.second;
440  std::vector<Motion *> mpath2;
441  while (solution != nullptr)
442  {
443  mpath2.push_back(solution);
444  solution = solution->parent;
445  }
446 
447  auto path(std::make_shared<PathGeometric>(si_));
448  path->getStates().reserve(mpath1.size() + mpath2.size());
449  for (int i = mpath1.size() - 1; i >= 0; --i)
450  path->append(mpath1[i]->state);
451  for (auto &i : mpath2)
452  path->append(i->state);
453 
454  pdef_->addSolutionPath(path, false, 0.0, getName());
455  solved = true;
456  break;
457  }
458  }
459 
460  std::swap(tree, otherTree);
461  }
462 
463  si_->freeState(rstate);
464  si_->freeState(xstate);
465  delete rmotion;
466  delete xmotion;
467 
468  OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(),
469  tStart_->size(), tGoal_->size());
471 }
472 
474 {
475  Planner::getPlannerData(data);
476 
477  std::vector<Motion *> motions;
478  if (tStart_)
479  tStart_->list(motions);
480  for (auto &motion : motions)
481  {
482  if (motion->parent == nullptr)
483  data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
484  else
485  {
486  data.addEdge(base::PlannerDataVertex(motion->parent->state, 1), base::PlannerDataVertex(motion->state, 1));
487  }
488  }
489 
490  motions.clear();
491  if (tGoal_)
492  tGoal_->list(motions);
493  for (auto &motion : motions)
494  {
495  if (motion->parent == nullptr)
496  data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
497  else
498  {
499  // The edges in the goal tree are reversed to be consistent with start tree
500  data.addEdge(base::PlannerDataVertex(motion->state, 2), base::PlannerDataVertex(motion->parent->state, 2));
501  }
502  }
503 
504  // Add the edge connecting the two trees
505  if (connectionPoint_.first && connectionPoint_.second)
506  data.addEdge(data.vertexIndex(connectionPoint_.first->state), data.vertexIndex(connectionPoint_.second->state));
507 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
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:340
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:173
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BiTRRT.cpp:122
double frontierThreshold_
The distance between an existing state and a new state that qualifies it as a frontier state...
Definition: BiTRRT.h:283
base::State * state
The state contained by the motion.
Definition: BiTRRT.h:199
bool minExpansionControl(double dist)
Use frontier node ratio to filter nodes that do not add new information to the search tree...
Definition: BiTRRT.cpp:219
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:192
base::Cost bestCost_
The most desirable (e.g., minimum) cost value in the search tree.
Definition: BiTRRT.h:270
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:269
GrowResult extendTree(Motion *rmotion, TreeData &tree, Motion *&xmotion)
Extend tree toward the state in rmotion. Store the result of the extension, if any, in result.
Definition: BiTRRT.cpp:271
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:202
double nonfrontierCount_
A count of the number of non-frontier nodes in the trees.
Definition: BiTRRT.h:292
TreeData tStart_
The start tree.
Definition: BiTRRT.h:306
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:217
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
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
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:473
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:279
double connectionRange_
The range at which the algorithm will attempt to connect the two trees.
Definition: BiTRRT.h:299
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:273
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:267
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: BiTRRT.h:263
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:303
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:77
#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:234
const base::State * root
Pointer to the root of the tree this motion is contained in.
Definition: BiTRRT.h:209
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:309
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:239
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:295
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:257
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:312
*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:421
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:427
No extension was possible.
Definition: BiTRRT.h:237
const State * nextStart()
Return the next valid start state or nullptr 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
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:286
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:276
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
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:279
The desired state was reached during extension.
Definition: BiTRRT.h:241
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:104
double temp_
The current temperature.
Definition: BiTRRT.h:289
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68