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
108  temp_ = initTemperature_;
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();
118  tools::SelfConfig sc(si_, getName());
119 
120  // Configuring the range of the planner
121  if (maxDistance_ < std::numeric_limits<double>::epsilon())
122  {
123  sc.configurePlannerRange(maxDistance_);
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
159  temp_ = initTemperature_;
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)
208  temp_ *= tempChangeFactor_;
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 
224  ++nonfrontierCount_;
225  return true;
226 }
227 
229  Motion *toMotion, Motion *&result)
230 {
231  bool reach = true;
232 
233  // Compute the state to extend toward
234  bool treeIsStart = (tree == tStart_);
235  double d = (treeIsStart ? si_->distance(nearest->state, toMotion->state)
236  : si_->distance(toMotion->state, nearest->state));
237  // Truncate the random state to be no more than maxDistance_ from nearest neighbor
238  if (d > maxDistance_)
239  {
240  if (tree == tStart_)
241  si_->getStateSpace()->interpolate(nearest->state, toMotion->state, maxDistance_ / d, toMotion->state);
242  else
243  si_->getStateSpace()->interpolate(toMotion->state, nearest->state, 1.0 - maxDistance_ / d, toMotion->state);
244  d = maxDistance_;
245  reach = false;
246  }
247 
248  // Validating the motion
249  // If we are in the goal tree, we validate the motion in reverse
250  // si_->checkMotion assumes that the first argument is valid, so we must check this explicitly
251  // If the motion is valid, check the probabilistic transition test and the
252  // expansion control to ensure high quality nodes are added.
253  bool validMotion =
254  (tree == tStart_ ? si_->checkMotion(nearest->state, toMotion->state) :
255  si_->isValid(toMotion->state) && si_->checkMotion(toMotion->state, nearest->state)) &&
256  transitionTest(tree == tStart_ ? opt_->motionCost(nearest->state, toMotion->state)
257  : opt_->motionCost(toMotion->state, nearest->state)) &&
258  minExpansionControl(d);
259 
260  if (validMotion)
261  {
262  result = addMotion(toMotion->state, tree, nearest);
263  return reach ? SUCCESS : ADVANCED;
264  }
265 
266  return FAILED;
267 }
268 
270  Motion *&result)
271 {
272  // Nearest neighbor
273  Motion *nearest = tree->nearest(toMotion);
274  return extendTree(nearest, tree, toMotion, result);
275 }
276 
278 {
279  // Get the nearest state to nmotion in tree (nmotion is NOT in tree)
280  Motion *nearest = tree->nearest(nmotion);
281  bool treeIsStart = tree == tStart_;
282  double dist = (treeIsStart ? si_->distance(nearest->state, nmotion->state)
283  : si_->distance(nmotion->state, nearest->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  Motion *startMotion = treeIsStart ? next : nmotion;
318  Motion *goalMotion = treeIsStart ? nmotion : next;
319 
320  // Make sure start-goal pair is valid
321  if (pdef_->getGoal()->isStartGoalPairValid(startMotion->root, goalMotion->root))
322  {
323  // Since we have connected, nmotion->state and next->state have the same value
324  // We need to check one of their parents to avoid a duplicate state in the solution path
325  // One of these must be true, since we do not ever attempt to connect start and goal directly.
326  if (startMotion->parent != nullptr)
327  startMotion = startMotion->parent;
328  else
329  goalMotion = goalMotion->parent;
330 
331  connectionPoint_ = std::make_pair(startMotion, goalMotion);
332  return true;
333  }
334  }
335 
336  return false;
337 }
338 
340 {
341  // Basic error checking
342  checkValidity();
343 
344  // Goal information
345  base::Goal *goal = pdef_->getGoal().get();
346  auto *gsr = dynamic_cast<base::GoalSampleableRegion *>(goal);
347 
348  if (gsr == nullptr)
349  {
350  OMPL_ERROR("%s: Goal object does not derive from GoalSampleableRegion", getName().c_str());
352  }
353 
354  // Loop through the (valid) input states and add them to the start tree
355  while (const base::State *state = pis_.nextStart())
356  {
357  auto *motion = new Motion(si_);
358  si_->copyState(motion->state, state);
359  motion->cost = opt_->stateCost(motion->state);
360  motion->root = motion->state; // this state is the root of a tree
361 
362  if (tStart_->size() == 0) // do not overwrite best/worst from a prior call to solve
363  worstCost_ = bestCost_ = motion->cost;
364 
365  // Add start motion to the tree
366  tStart_->add(motion);
367  }
368 
369  if (tStart_->size() == 0)
370  {
371  OMPL_ERROR("%s: Start tree has no valid states!", getName().c_str());
373  }
374 
375  // Do the same for the goal tree, if it is empty, but only once
376  if (tGoal_->size() == 0)
377  {
378  const base::State *state = pis_.nextGoal(ptc);
379  if (state != nullptr)
380  {
381  Motion *motion = addMotion(state, tGoal_);
382  motion->root = motion->state; // this state is the root of a tree
383  }
384  }
385 
386  if (tGoal_->size() == 0)
387  {
388  OMPL_ERROR("%s: Goal tree has no valid states!", getName().c_str());
390  }
391 
392  OMPL_INFORM("%s: Planning started with %d states already in datastructure", getName().c_str(),
393  (int)(tStart_->size() + tGoal_->size()));
394 
395  base::StateSamplerPtr sampler = si_->allocStateSampler();
396 
397  auto *rmotion = new Motion(si_);
398  base::State *rstate = rmotion->state;
399 
400  auto *xmotion = new Motion(si_);
401  base::State *xstate = xmotion->state;
402 
403  TreeData tree = tStart_;
404  TreeData otherTree = tGoal_;
405 
406  bool solved = false;
407  // Planning loop
408  while (!ptc)
409  {
410  // Check if there are more goal states
411  if (pis_.getSampledGoalsCount() < tGoal_->size() / 2)
412  {
413  if (const base::State *state = pis_.nextGoal())
414  {
415  Motion *motion = addMotion(state, tGoal_);
416  motion->root = motion->state; // this state is the root of a tree
417  }
418  }
419 
420  // Sample a state uniformly at random
421  sampler->sampleUniform(rstate);
422 
423  Motion *result; // the motion that gets added in extendTree
424  if (extendTree(rmotion, tree, result) != FAILED) // we added something new to the tree
425  {
426  // Try to connect the other tree to the node we just added
427  if (connectTrees(result, otherTree, xmotion))
428  {
429  // The trees have been connected. Construct the solution path
430  Motion *solution = connectionPoint_.first;
431  std::vector<Motion *> mpath1;
432  while (solution != nullptr)
433  {
434  mpath1.push_back(solution);
435  solution = solution->parent;
436  }
437 
438  solution = connectionPoint_.second;
439  std::vector<Motion *> mpath2;
440  while (solution != nullptr)
441  {
442  mpath2.push_back(solution);
443  solution = solution->parent;
444  }
445 
446  auto path(std::make_shared<PathGeometric>(si_));
447  path->getStates().reserve(mpath1.size() + mpath2.size());
448  for (int i = mpath1.size() - 1; i >= 0; --i)
449  path->append(mpath1[i]->state);
450  for (auto &i : mpath2)
451  path->append(i->state);
452 
453  pdef_->addSolutionPath(path, false, 0.0, getName());
454  solved = true;
455  break;
456  }
457  }
458 
459  std::swap(tree, otherTree);
460  }
461 
462  si_->freeState(rstate);
463  si_->freeState(xstate);
464  delete rmotion;
465  delete xmotion;
466 
467  OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(),
468  tStart_->size(), tGoal_->size());
470 }
471 
473 {
474  Planner::getPlannerData(data);
475 
476  std::vector<Motion *> motions;
477  if (tStart_)
478  tStart_->list(motions);
479  for (auto &motion : motions)
480  {
481  if (motion->parent == nullptr)
482  data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
483  else
484  {
485  data.addEdge(base::PlannerDataVertex(motion->parent->state, 1), base::PlannerDataVertex(motion->state, 1));
486  }
487  }
488 
489  motions.clear();
490  if (tGoal_)
491  tGoal_->list(motions);
492  for (auto &motion : motions)
493  {
494  if (motion->parent == nullptr)
495  data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
496  else
497  {
498  // The edges in the goal tree are reversed to be consistent with start tree
499  data.addEdge(base::PlannerDataVertex(motion->state, 2), base::PlannerDataVertex(motion->parent->state, 2));
500  }
501  }
502 
503  // Add the edge connecting the two trees
504  if ((connectionPoint_.first != nullptr) && (connectionPoint_.second != nullptr))
505  data.addEdge(data.vertexIndex(connectionPoint_.first->state), data.vertexIndex(connectionPoint_.second->state));
506 }
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:339
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
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:269
A shared pointer wrapper for ompl::base::StateSampler.
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
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
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:472
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
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
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
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
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
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 getCostThreshold() const
Get the cost threshold (default is infinity). Any motion cost that is not better than this cost (acco...
Definition: BiTRRT.h:122
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
#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.
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
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:277
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
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68