STRRTstar.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2021, Technische Universität Berlin (TU Berlin)
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 TU Berlin 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: Francesco Grothe */
36 
37 #include "ompl/geometric/planners/rrt/STRRTstar.h"
38 #include <ompl/util/Exception.h>
39 
41  : Planner(si, "SpaceTimeRRT"), sampler_(&(*si), startMotion_, goalMotions_, newBatchGoalMotions_, sampleOldBatch_)
42 {
43  if (std::dynamic_pointer_cast<ompl::base::SpaceTimeStateSpace>(si->getStateSpace()) == nullptr) {
44  OMPL_ERROR("%s: State Space needs to be of type SpaceTimeStateSpace.", getName().c_str());
45  throw ompl::Exception("Non-SpaceTimeStateSpace");
46  }
47  specs_.optimizingPaths = true;
49  Planner::declareParam<double>("range", this, &STRRTstar::setRange, &STRRTstar::getRange, "0.:1.:10000.");
50  distanceBetweenTrees_ = std::numeric_limits<double>::infinity();
51 }
52 
53 ompl::geometric::STRRTstar::~STRRTstar()
54 {
55  freeMemory();
56 }
57 
59 {
60  Planner::setup();
61  ompl::tools::SelfConfig sc(si_, getName());
62  sc.configurePlannerRange(maxDistance_);
63 
64  if (!tStart_)
66  if (!tGoal_)
68  tStart_->setDistanceFunction([this](const base::Motion *a, const base::Motion *b) { return distanceFunction(a, b); });
69  tGoal_->setDistanceFunction([this](const base::Motion *a, const base::Motion *b) { return distanceFunction(a, b); });
70 
71  if (si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->getTimeComponent()->isBounded())
72  {
73  upperTimeBound_ =
74  si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->getTimeComponent()->getMaxTimeBound();
75  isTimeBounded_ = true;
76  }
77  else
78  {
79  upperTimeBound_ = std::numeric_limits<double>::infinity();
80  isTimeBounded_ = false;
81  }
82  initialTimeBound_ = upperTimeBound_;
83 
84  si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->updateEpsilon();
85  // Calculate some constants:
86  calculateRewiringLowerBounds();
87 }
88 
90 {
91  std::vector<base::Motion *> motions;
92 
93  if (tStart_)
94  {
95  tStart_->list(motions);
96  for (auto &motion : motions)
97  {
98  if (motion->state != nullptr)
99  si_->freeState(motion->state);
100  delete motion;
101  }
102  }
103 
104  if (tGoal_)
105  {
106  tGoal_->list(motions);
107  for (auto &motion : motions)
108  {
109  if (motion->state != nullptr)
110  si_->freeState(motion->state);
111  delete motion;
112  }
113  }
114 
115  if (tempState_)
116  si_->freeState(tempState_);
117 }
118 
120 {
121  checkValidity();
122  auto *goal = dynamic_cast<ompl::base::GoalSampleableRegion *>(pdef_->getGoal().get());
123 
124  if (goal == nullptr)
125  {
126  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
128  }
129 
130  while (const ompl::base::State *st = pis_.nextStart())
131  {
132  auto *motion = new base::Motion(si_);
133  si_->copyState(motion->state, st);
134  motion->root = motion->state;
135  tStart_->add(motion);
136  startMotion_ = motion;
137  }
138 
139  if (tStart_->size() == 0)
140  {
141  OMPL_ERROR("%s: base::Motion planning start tree could not be initialized!", getName().c_str());
143  }
144 
145  if (!goal->couldSample())
146  {
147  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
149  }
150 
151  OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
152  (int)(tStart_->size() + tGoal_->size()));
153 
154  TreeGrowingInfo tgi;
155  tgi.xstate = si_->allocState();
156 
157  std::vector<base::Motion *> nbh;
158  const ompl::base::ReportIntermediateSolutionFn intermediateSolutionCallback =
159  pdef_->getIntermediateSolutionCallback();
160 
161  base::Motion *approxsol = nullptr;
162  double approxdif = std::numeric_limits<double>::infinity();
163  auto *rmotion = new base::Motion(si_);
164  ompl::base::State *rstate = rmotion->state;
165  bool startTree = true;
166  bool solved = false;
167 
168  // samples to fill the current batch
169  unsigned int batchSize = initialBatchSize_;
170 
171  // number of samples in the current batch
172  int numBatchSamples = static_cast<int>(tStart_->size() + tGoal_->size()); // number of samples in the current batch
173 
174  // number of goal samples in the new batch region
175  int newBatchGoalSamples = 0;
176 
177  bool firstBatch = true;
178 
179  // probability to sample the old batch region
180  double oldBatchSampleProb = 1.0;
181 
182  // Time Bound factor for the old batch.
183  double oldBatchTimeBoundFactor = initialTimeBoundFactor_;
184 
185  // Time Bound factor for the new batch.
186  double newBatchTimeBoundFactor = initialTimeBoundFactor_;
187 
188  bool forceGoalSample = true;
189 
190  OMPL_INFORM("%s: Starting planning with time bound factor %.2f", getName().c_str(), newBatchTimeBoundFactor);
191 
192  while (!ptc)
193  {
194  numIterations_++;
195  TreeData &tree = startTree ? tStart_ : tGoal_;
196  tgi.start = startTree;
197  startTree = !startTree;
198  TreeData &otherTree = startTree ? tStart_ : tGoal_;
199 
200  // batch is full
201  if (!isTimeBounded_ && (unsigned int)numBatchSamples >= batchSize)
202  {
203  if (firstBatch)
204  {
205  firstBatch = false;
206  oldBatchSampleProb = 0.5 * (1 / timeBoundFactorIncrease_);
207  }
208  increaseTimeBound(false, oldBatchTimeBoundFactor, newBatchTimeBoundFactor, startTree,
209  batchSize, numBatchSamples);
210  // transfer new batch goals to old batch
211  if (!newBatchGoalMotions_.empty())
212  {
213  goalMotions_.insert(goalMotions_.end(), newBatchGoalMotions_.begin(), newBatchGoalMotions_.end());
214  newBatchGoalMotions_.clear();
215  }
216  continue;
217  }
218 
219  // determine whether the old or new batch is sampled
220  sampleOldBatch_ =
221  (firstBatch || isTimeBounded_ || !sampleUniformForUnboundedTime_ || rng_.uniform01() <= oldBatchSampleProb);
222 
223  // *** Begin Goal Sampling ***
224 
225  ompl::base::State *goalState{nullptr};
226  if (sampleOldBatch_)
227  {
228  // sample until successful or time is up
229  if (goalMotions_.empty() && isTimeBounded_)
230  goalState = nextGoal(ptc, oldBatchTimeBoundFactor, newBatchTimeBoundFactor);
231  // sample for n tries, with n = batch size
232  else if (goalMotions_.empty() && !isTimeBounded_)
233  {
234  goalState = nextGoal(static_cast<int>(batchSize), oldBatchTimeBoundFactor, newBatchTimeBoundFactor);
235  // the goal region is most likely blocked for this time period -> increase upper time bound
236  if (goalState == nullptr)
237  {
238  increaseTimeBound(true, oldBatchTimeBoundFactor, newBatchTimeBoundFactor, startTree,
239  batchSize, numBatchSamples);
240  continue;
241  }
242  }
243  // sample for a single try
244  else if (forceGoalSample ||
245  goalMotions_.size() < (tGoal_->size() - newBatchGoalSamples) / goalStateSampleRatio_)
246  {
247  goalState = nextGoal(1, oldBatchTimeBoundFactor, newBatchTimeBoundFactor);
248  forceGoalSample = false;
249  }
250  }
251  else
252  {
253  if (newBatchGoalMotions_.empty())
254  {
255  goalState = nextGoal(static_cast<int>(batchSize), oldBatchTimeBoundFactor, newBatchTimeBoundFactor);
256  // the goal region is most likely blocked for this time period -> increase upper time bound
257  if (goalState == nullptr)
258  {
259  increaseTimeBound(false, oldBatchTimeBoundFactor, newBatchTimeBoundFactor, startTree,
260  batchSize, numBatchSamples);
261  continue;
262  }
263  }
264  else if (forceGoalSample || newBatchGoalMotions_.size() < (unsigned long)(newBatchGoalSamples / goalStateSampleRatio_))
265  {
266  goalState = nextGoal(1, oldBatchTimeBoundFactor, newBatchTimeBoundFactor);
267  forceGoalSample = false;
268  }
269  }
270 
271  if (goalState != nullptr)
272  {
273  auto *motion = new base::Motion(si_);
274  si_->copyState(motion->state, goalState);
275  motion->root = motion->state;
276  tGoal_->add(motion);
277  if (sampleOldBatch_)
278  goalMotions_.push_back(motion);
279  else
280  {
281  newBatchGoalMotions_.push_back(motion);
282  newBatchGoalSamples++;
283  }
284 
285  minimumTime_ =
286  std::min(minimumTime_, si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->timeToCoverDistance(
287  startMotion_->state, goalState));
288  numBatchSamples++;
289  }
290 
291  // *** End Goal Sampling ***
292 
293  /* sample random state */
294  bool success = sampler_.sample(rstate);
295  if (!success)
296  {
297  forceGoalSample = true;
298  continue;
299  }
300 
301  // EXTEND
302  GrowState gs = growTree(tree, tgi, rmotion, nbh, false);
303  if (gs != TRAPPED)
304  {
305  numBatchSamples++;
306  /* remember which motion was just added */
307  base::Motion *addedMotion = tgi.xmotion;
308  base::Motion *startMotion;
309  base::Motion *goalMotion;
310 
311  /* rewire the goal tree */
312  bool newSolution = false;
313  if (!tgi.start && rewireState_ != OFF)
314  {
315  newSolution = rewireGoalTree(addedMotion);
316  if (newSolution)
317  {
318  // find connection point
319  std::queue<base::Motion *> queue;
320  queue.push(addedMotion);
321  while (!queue.empty())
322  {
323  if (queue.front()->connectionPoint != nullptr)
324  {
325  goalMotion = queue.front();
326  startMotion = queue.front()->connectionPoint;
327  break;
328  }
329  else
330  {
331  for (base::Motion *c : queue.front()->children)
332  queue.push(c);
333  }
334  queue.pop();
335  }
336  }
337  }
338 
339  /* if reached, it means we used rstate directly, no need to copy again */
340  if (gs != REACHED)
341  si_->copyState(rstate, tgi.xstate);
342 
343  tgi.start = startTree;
344 
345  /* attempt to connect trees, if rewiring didn't find a new solution */
346  // CONNECT
347  if (!newSolution)
348  {
349  int totalSamples = static_cast<int>(tStart_->size() + tGoal_->size());
350  GrowState gsc = growTree(otherTree, tgi, rmotion, nbh, true);
351  if (gsc == REACHED)
352  {
353  newSolution = true;
354  startMotion = startTree ? tgi.xmotion : addedMotion;
355  goalMotion = startTree ? addedMotion : tgi.xmotion;
356  // it must be the case that either the start tree or the goal tree has made some progress
357  // so one of the parents is not nullptr. We go one step 'back' to avoid having a duplicate state
358  // on the solution path
359  if (startMotion->parent != nullptr)
360  startMotion = startMotion->parent;
361  else
362  goalMotion = goalMotion->parent;
363  }
364  numBatchSamples += static_cast<int>(tStart_->size() + tGoal_->size()) - totalSamples;
365  }
366 
367  /* update distance between trees */
368  const double newDist = tree->getDistanceFunction()(addedMotion, otherTree->nearest(addedMotion));
369  if (newDist < distanceBetweenTrees_)
370  {
371  distanceBetweenTrees_ = newDist;
372  // OMPL_INFORM("Estimated distance to go: %f", distanceBetweenTrees_);
373  }
374 
375  /* if we connected the trees in a valid way (start and goal pair is valid)*/
376  if (newSolution && goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
377  {
378  constructSolution(startMotion, goalMotion, intermediateSolutionCallback, ptc);
379  solved = true;
380  if (ptc || upperTimeBound_ == minimumTime_)
381  break; // first solution is enough or optimal solution is found
382  // continue to look for solutions with the narrower time bound until the termination condition is met
383  }
384  else
385  {
386  // We didn't reach the goal, but if we were extending the start
387  // tree, then we can mark/improve the approximate path so far.
388  if (!startTree)
389  {
390  // We were working from the startTree.
391  double dist = 0.0;
392  goal->isSatisfied(tgi.xmotion->state, &dist);
393  if (dist < approxdif)
394  {
395  approxdif = dist;
396  approxsol = tgi.xmotion;
397  }
398  }
399  }
400  }
401  }
402 
403  si_->freeState(tgi.xstate);
404  si_->freeState(rstate);
405  delete rmotion;
406 
407  OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(),
408  tStart_->size(), tGoal_->size());
409 
410  if (approxsol && !solved)
411  {
412  /* construct the solution path */
413  std::vector<base::Motion *> mpath;
414  while (approxsol != nullptr)
415  {
416  mpath.push_back(approxsol);
417  approxsol = approxsol->parent;
418  }
419 
420  auto path(std::make_shared<ompl::geometric::PathGeometric>(si_));
421  for (int i = mpath.size() - 1; i >= 0; --i)
422  path->append(mpath[i]->state);
423  pdef_->addSolutionPath(path, true, approxdif, getName());
425  }
426  if (solved)
427  {
428  // Add the solution path.
429  ompl::base::PlannerSolution psol(bestSolution_);
430  psol.setPlannerName(getName());
431 
432  ompl::base::OptimizationObjectivePtr optimizationObjective = std::make_shared<ompl::base::MinimizeArrivalTime>(si_);
433  psol.setOptimized(optimizationObjective, ompl::base::Cost(bestTime_), false);
434  pdef_->addSolutionPath(psol);
435  }
436 
438 }
439 
442  base::Motion *rmotion, std::vector<base::Motion *> &nbh, bool connect)
443 {
444  // If connect, advance from single nearest neighbor until the random state is reached or trapped
445  if (connect)
446  {
447  GrowState gsc = ADVANCED;
448  while (gsc == ADVANCED)
449  {
450  // get nearest motion
451  base::Motion *nmotion = tree->nearest(rmotion);
452  gsc = growTreeSingle(tree, tgi, rmotion, nmotion);
453  }
454  return gsc;
455  }
456  if (rewireState_ == OFF)
457  {
458  base::Motion *nmotion = tree->nearest(rmotion);
459  return growTreeSingle(tree, tgi, rmotion, nmotion);
460  }
461  // get Neighborhood of random state
462  getNeighbors(tree, rmotion, nbh);
463  // in start tree sort by distance
464  if (tgi.start)
465  {
466  std::sort(nbh.begin(), nbh.end(),
467  [this, &rmotion](base::Motion *a, base::Motion *b)
468  { return si_->distance(a->state, rmotion->state) < si_->distance(b->state, rmotion->state); });
469  }
470  // in goal tree sort by time of root node
471  else
472  {
473  std::sort(
474  nbh.begin(), nbh.end(),
475  [](base::Motion *a, base::Motion *b)
476  {
477  auto t1 =
478  a->root->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
479  auto t2 =
480  b->root->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
481  return t1 < t2;
482  });
483  }
484 
485  // attempt to grow the tree for all neighbors in sorted order
486  GrowState gs = TRAPPED;
487  auto rt = rmotion->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
488  for (base::Motion *nmotion : nbh)
489  {
490  auto nt =
491  nmotion->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
492  // trees grow only in one direction in time
493  if ((tgi.start && nt > rt) || (!tgi.start && nt < rt))
494  continue;
495  gs = growTreeSingle(tree, tgi, rmotion, nmotion);
496  if (gs != TRAPPED)
497  return gs;
498  }
499  // when radius_ is used for neighborhood calculation, the neighborhood might be empty
500  if (nbh.empty())
501  {
502  base::Motion *nmotion = tree->nearest(rmotion);
503  return growTreeSingle(tree, tgi, rmotion, nmotion);
504  }
505  // can't grow Tree
506  return gs;
507 }
508 
511  base::Motion *rmotion, base::Motion *nmotion)
512 {
513  /* assume we can reach the state we go towards */
514  bool reach = true;
515 
516  /* find state to add */
517  ompl::base::State *dstate = rmotion->state;
518  double d = si_->distance(nmotion->state, rmotion->state);
519 
520  if (d > maxDistance_)
521  {
522  si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
523  /* Check if we have moved at all. Due to some stranger state spaces (e.g., the constrained state spaces),
524  * interpolate can fail and no progress is made. Without this check, the algorithm gets stuck in a loop as it
525  * thinks it is making progress, when none is actually occurring. */
526  if (si_->equalStates(nmotion->state, tgi.xstate))
527  return TRAPPED;
528  dstate = tgi.xstate;
529  reach = false;
530  }
531 
532  bool validMotion = tgi.start ? si_->checkMotion(nmotion->state, dstate) :
533  si_->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
534 
535  if (!validMotion)
536  return TRAPPED;
537 
538  auto *motion = new base::Motion(si_);
539  si_->copyState(motion->state, dstate);
540  motion->parent = nmotion;
541  motion->root = nmotion->root;
542  motion->parent->children.push_back(motion);
543  tree->add(motion);
544  tgi.xmotion = motion;
545 
546  return reach ? REACHED : ADVANCED;
547 }
548 
549 void ompl::geometric::STRRTstar::increaseTimeBound(bool hasSameBounds, double &oldBatchTimeBoundFactor,
550  double &newBatchTimeBoundFactor, bool &startTree,
551  unsigned int &batchSize, int &numBatchSamples)
552 {
553  oldBatchTimeBoundFactor = hasSameBounds ? newBatchTimeBoundFactor * timeBoundFactorIncrease_ : newBatchTimeBoundFactor;
554  newBatchTimeBoundFactor *= timeBoundFactorIncrease_;
555  startTree = true;
556  if (sampleUniformForUnboundedTime_)
557  batchSize = std::ceil(2.0 * (timeBoundFactorIncrease_ - 1.0) *
558  static_cast<double>(tStart_->size() + tGoal_->size()));
559  numBatchSamples = 0;
560  OMPL_INFORM("%s: Increased time bound factor to %.2f", getName().c_str(), newBatchTimeBoundFactor);
561 }
562 
563 void ompl::geometric::STRRTstar::constructSolution(
564  base::Motion *startMotion, base::Motion *goalMotion,
565  const ompl::base::ReportIntermediateSolutionFn &intermediateSolutionCallback, const ompl::base::PlannerTerminationCondition& ptc)
566 {
567  if (goalMotion->connectionPoint == nullptr)
568  {
569  goalMotion->connectionPoint = startMotion;
570  base::Motion *tMotion = goalMotion;
571  while (tMotion != nullptr)
572  {
573  tMotion->numConnections++;
574  tMotion = tMotion->parent;
575  }
576  }
577  // check whether the found solution is an improvement
578  auto newTime =
579  goalMotion->root->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
580  if (newTime >= upperTimeBound_)
581  return;
582 
583  numSolutions_++;
584  isTimeBounded_ = true;
585  if (!newBatchGoalMotions_.empty())
586  {
587  goalMotions_.insert(goalMotions_.end(), newBatchGoalMotions_.begin(), newBatchGoalMotions_.end());
588  newBatchGoalMotions_.clear();
589  }
590 
591  /* construct the solution path */
592  base::Motion *solution = startMotion;
593  std::vector<base::Motion *> mpath1;
594  while (solution != nullptr)
595  {
596  mpath1.push_back(solution);
597  solution = solution->parent;
598  }
599 
600  solution = goalMotion;
601  std::vector<base::Motion *> mpath2;
602  while (solution != nullptr)
603  {
604  mpath2.push_back(solution);
605  solution = solution->parent;
606  }
607 
608  std::vector<const ompl::base::State *> constPath;
609 
610  auto path(std::make_shared<ompl::geometric::PathGeometric>(si_));
611  path->getStates().reserve(mpath1.size() + mpath2.size());
612  for (int i = mpath1.size() - 1; i >= 0; --i)
613  {
614  constPath.push_back(mpath1[i]->state);
615  path->append(mpath1[i]->state);
616  }
617  for (auto &i : mpath2)
618  {
619  constPath.push_back(i->state);
620  path->append(i->state);
621  }
622 
623  bestSolution_ = path;
624  auto reachedGaol = path->getState(path->getStateCount() - 1);
625  bestTime_ = reachedGaol->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
626 
627  if (intermediateSolutionCallback)
628  {
629  intermediateSolutionCallback(this, constPath, ompl::base::Cost(bestTime_));
630  }
631 
632  // Update Time Limit
633  upperTimeBound_ = (bestTime_ - minimumTime_) * optimumApproxFactor_ + minimumTime_;
634 
635  if (ptc)
636  return;
637  // Prune Start and Goal Trees
638  pruneStartTree();
639  base::Motion *newSolution = pruneGoalTree();
640 
641  // loop as long as a new solution is found by rewiring the goal tree
642  if (newSolution != nullptr)
643  constructSolution(newSolution->connectionPoint, goalMotion, intermediateSolutionCallback, ptc);
644 }
645 
647 {
648  std::queue<base::Motion *> queue;
649 
650  tStart_->clear();
651  tStart_->add(startMotion_);
652  for (auto &c : startMotion_->children)
653  queue.push(c);
654  while (!queue.empty())
655  {
656  double t = queue.front()
657  ->state->as<ompl::base::CompoundState>()
658  ->as<ompl::base::TimeStateSpace::StateType>(1)
659  ->position;
660  double timeToNearestGoal = std::numeric_limits<double>::infinity();
661  for (const auto &g : goalMotions_)
662  {
663  double deltaT = si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->timeToCoverDistance(
664  queue.front()->state, g->state);
665  if (deltaT < timeToNearestGoal)
666  timeToNearestGoal = deltaT;
667  }
668  // base::Motion is still valid, re-add to tree
669  if (t + timeToNearestGoal <= upperTimeBound_)
670  {
671  tStart_->add(queue.front());
672  for (auto &c : queue.front()->children)
673  queue.push(c);
674  }
675  // base::Motion is invalid due to the new time limit, delete motion
676  else
677  {
678  // Remove the motion from its parent
679  removeFromParent(queue.front());
680 
681  // for deletion first construct list of all descendants
682  std::queue<base::Motion *> deletionQueue;
683  std::vector<base::Motion *> deletionList;
684 
685  deletionQueue.push(queue.front());
686  while (!deletionQueue.empty())
687  {
688  for (auto &c : deletionQueue.front()->children)
689  deletionQueue.push(c);
690  deletionList.push_back(deletionQueue.front());
691  deletionQueue.pop();
692  }
693 
694  // then free all descendants
695  for (auto &m : deletionList)
696  {
697  // Erase the actual motion
698  // First free the state
699  if (m->state)
700  si_->freeState(m->state);
701  // then delete the pointer
702  delete m;
703  }
704  }
705  // finally remove motion from the queue
706  queue.pop();
707  }
708 }
709 
711 {
712  // it's possible to get multiple new solutions during the rewiring process. Store the best.
713  double bestSolutionTime = upperTimeBound_;
714  base::Motion *solutionMotion{nullptr};
715 
716  tGoal_->clear();
717  std::vector<base::Motion *> validGoals;
718  std::vector<base::Motion *> invalidGoals;
719 
720  // re-add goals with smallest time first
721  std::sort(
722  goalMotions_.begin(), goalMotions_.end(),
723  [](base::Motion *a, base::Motion *b)
724  {
725  return a->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position <
726  b->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
727  });
728  for (auto &m : goalMotions_)
729  {
730  double t = m->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
731  // add goal with all descendants to the tree
732  if (t <= upperTimeBound_)
733  {
734  tGoal_->add(m);
735  addDescendants(m, tGoal_);
736  validGoals.push_back(m);
737  }
738  // try to rewire descendants to a valid goal
739  else
740  {
741  invalidGoals.push_back(m);
742  std::queue<base::Motion *> queue;
743  for (auto &c : m->children)
744  queue.push(c);
745  while (!queue.empty())
746  {
747  bool addedToTree = false;
748  if (tGoal_->size() != 0)
749  {
750  double costToGo = std::numeric_limits<double>::infinity();
751  double costSoFar = queue.front()
752  ->state->as<ompl::base::CompoundState>()
753  ->as<ompl::base::TimeStateSpace::StateType>(1)
754  ->position;
755  for (auto &g : validGoals)
756  {
757  auto deltaT = si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->timeToCoverDistance(
758  queue.front()->state, g->state);
759  if (deltaT < costToGo)
760  costToGo = deltaT;
761  }
762  // try to rewire to the nearest neighbor
763 
764  if (costSoFar + costToGo <= upperTimeBound_)
765  {
766  TreeGrowingInfo tgi{};
767  tgi.xstate = si_->allocState();
768  tgi.start = false;
769  std::vector<base::Motion *> nbh;
770  GrowState gsc = growTree(tGoal_, tgi, queue.front(), nbh, true);
771  // connection successful, add all descendants and check if a new solution was found.
772  if (gsc == REACHED)
773  {
774  // the motion was copied and added to the tree with a new parent
775  // adjust children and parent pointers
776  tgi.xmotion->children = queue.front()->children;
777  for (auto &c : tgi.xmotion->children)
778  {
779  c->parent = tgi.xmotion;
780  }
781  tgi.xmotion->connectionPoint = queue.front()->connectionPoint;
782  tgi.xmotion->numConnections = queue.front()->numConnections;
783  base::Motion *p = tgi.xmotion->parent;
784  while (p != nullptr)
785  {
786  p->numConnections += tgi.xmotion->numConnections;
787  p = p->parent;
788  }
789  addDescendants(tgi.xmotion, tGoal_);
790  // new solution found
791  if (tgi.xmotion->numConnections > 0 &&
792  tgi.xmotion->root->as<ompl::base::CompoundState>()
794  ->position < bestSolutionTime)
795  {
796  bestSolutionTime = tgi.xmotion->root->as<ompl::base::CompoundState>()
797  ->as<ompl::base::TimeStateSpace::StateType>(1)
798  ->position;
799  solutionMotion = computeSolutionMotion(tgi.xmotion);
800  }
801  addedToTree = true;
802  }
803  }
804  }
805  // Free motion and state
806  if (!addedToTree)
807  {
808  // add children to queue, so they might be rewired
809  for (auto &c : queue.front()->children)
810  queue.push(c);
811  }
812  // Erase the actual motion
813  // First free the state
814  if (queue.front()->state)
815  si_->freeState(queue.front()->state);
816  // then delete the pointer
817  delete queue.front();
818 
819  queue.pop();
820  }
821  }
822  }
823 
824  removeInvalidGoals(invalidGoals);
825  return solutionMotion;
826 }
827 
830 {
831  std::queue<base::Motion *> connectionQueue;
832  connectionQueue.push(motion);
833  while (!connectionQueue.empty())
834  {
835  if (connectionQueue.front()->connectionPoint != nullptr)
836  {
837  return connectionQueue.front();
838  }
839  else
840  {
841  for (base::Motion *c : connectionQueue.front()->children)
842  connectionQueue.push(c);
843  }
844  connectionQueue.pop();
845  }
846  // suppress compiler warning
847  return nullptr;
848 }
849 
850 void ompl::geometric::STRRTstar::removeInvalidGoals(const std::vector<base::Motion *> &invalidGoals)
851 {
852  for (auto &g : invalidGoals)
853  {
854  for (auto it = goalMotions_.begin(); it != goalMotions_.end(); ++it)
855  {
856  if (*it == g)
857  {
858  goalMotions_.erase(it);
859  break;
860  }
861  }
862  if (g->state)
863  si_->freeState(g->state);
864  delete g;
865  }
866 }
867 
868 
870 {
871  setup_ = false;
872  Planner::clear();
873  freeMemory();
874  if (tStart_)
875  tStart_->clear();
876  if (tGoal_)
877  tGoal_->clear();
878  distanceBetweenTrees_ = std::numeric_limits<double>::infinity();
879  bestSolution_ = nullptr;
880  bestTime_ = std::numeric_limits<double>::infinity();
881  minimumTime_ = std::numeric_limits<double>::infinity();
882  numIterations_ = 0;
883  numSolutions_ = 0;
884  startMotion_ = nullptr;
885  goalMotions_.clear();
886  newBatchGoalMotions_.clear();
887  tempState_ = nullptr;
888  sampleOldBatch_ = true;
889  upperTimeBound_ = initialTimeBound_;
890  isTimeBounded_ = initialTimeBound_ != std::numeric_limits<double>::infinity();
891 }
892 
894 {
895  Planner::getPlannerData(data);
896 
897  std::vector<base::Motion *> motions;
898  if (tStart_)
899  tStart_->list(motions);
900 
901  for (auto &motion : motions)
902  {
903  if (motion->parent == nullptr)
904  data.addStartVertex(ompl::base::PlannerDataVertex(motion->state, 1));
905  else
906  {
907  data.addEdge(ompl::base::PlannerDataVertex(motion->parent->state, 1),
908  ompl::base::PlannerDataVertex(motion->state, 1));
909  }
910  }
911 
912  motions.clear();
913  if (tGoal_)
914  tGoal_->list(motions);
915 
916  for (auto &motion : motions)
917  {
918  if (motion->parent == nullptr)
919  data.addGoalVertex(ompl::base::PlannerDataVertex(motion->state, 2));
920  else
921  {
922  // The edges in the goal tree are reversed to be consistent with start tree
923  data.addEdge(ompl::base::PlannerDataVertex(motion->state, 2),
924  ompl::base::PlannerDataVertex(motion->parent->state, 2));
925  }
926  // add edges connecting the two trees
927  if (motion->connectionPoint != nullptr)
928  data.addEdge(data.vertexIndex(motion->connectionPoint->state), data.vertexIndex(motion->state));
929  }
930 
931  // Add some info.
932  data.properties["approx goal distance REAL"] = ompl::toString(distanceBetweenTrees_);
933 }
934 
936 {
937  for (auto it = m->parent->children.begin(); it != m->parent->children.end(); ++it)
938  {
939  if (*it == m)
940  {
941  m->parent->children.erase(it);
942  break;
943  }
944  }
945 }
946 
956 {
957  std::queue<base::Motion *> queue;
958  for (auto &c : m->children)
959  queue.push(c);
960  while (!queue.empty())
961  {
962  for (auto &c : queue.front()->children)
963  queue.push(c);
964  queue.front()->root = m->root;
965  tree->add(queue.front());
966  queue.pop();
967  }
968 }
969 
971  base::Motion *motion,
972  std::vector<base::Motion *> &nbh) const
973 {
974  auto card = static_cast<double>(tree->size() + 1u);
975  if (rewireState_ == RADIUS)
976  {
977  // r = min( r_rrt * (log(card(V))/card(V))^(1 / d + 1), distance)
978  // for the formula change of the RRTStar paper, see 'Revisiting the asymptotic optimality of RRT*'
979  double r = std::min(maxDistance_, r_rrt_ * std::pow(log(card) / card,
980  1.0 / 1.0 + static_cast<double>(si_->getStateDimension())));
981  tree->nearestR(motion, r, nbh);
982  }
983  else if (rewireState_ == KNEAREST)
984  {
985  // k = k_rrt * log(card(V))
986  unsigned int k = std::ceil(k_rrt_ * log(card));
987  tree->nearestK(motion, k, nbh);
988  }
989 }
990 
991 bool ompl::geometric::STRRTstar::rewireGoalTree(base::Motion *addedMotion)
992 {
993  bool solved = false;
994  std::vector<base::Motion *> nbh;
995  getNeighbors(tGoal_, addedMotion, nbh);
996  double nodeT =
997  addedMotion->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
998  double goalT =
999  addedMotion->root->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
1000 
1001  for (base::Motion *otherMotion : nbh)
1002  {
1003  double otherNodeT =
1004  otherMotion->state->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
1005  double otherGoalT =
1006  otherMotion->root->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position;
1007  // rewire, if goal time is improved and the otherMotion node can be connected to the added node
1008  if (otherNodeT < nodeT && goalT < otherGoalT && si_->checkMotion(otherMotion->state, addedMotion->state))
1009  {
1010  if (otherMotion->numConnections > 0)
1011  {
1012  base::Motion *p = otherMotion->parent;
1013  while (p != nullptr)
1014  {
1015  p->numConnections--;
1016  p = p->parent;
1017  }
1018  }
1019  removeFromParent(otherMotion);
1020  otherMotion->parent = addedMotion;
1021  otherMotion->root = addedMotion->root;
1022  addedMotion->children.push_back(otherMotion);
1023  // increase connection count of new ancestors
1024  if (otherMotion->numConnections > 0)
1025  {
1026  base::Motion *p = otherMotion->parent;
1027  while (p != nullptr)
1028  {
1029  p->numConnections++;
1030  p = p->parent;
1031  }
1032  if (otherMotion->root->as<ompl::base::CompoundState>()
1034  ->position < upperTimeBound_)
1035  {
1036  solved = true;
1037  }
1038  }
1039  }
1040  }
1041 
1042  return solved;
1043 }
1044 
1046 {
1047  const auto dim = static_cast<double>(si_->getStateDimension());
1048 
1049  // r_rrt > (2*(1+1/d))^(1/d)*(measure/ballvolume)^(1/d)
1050  // prunedMeasure_ is set to si_->getSpaceMeasure();
1051  r_rrt_ = rewireFactor_ * std::pow(2 * (1.0 + 1.0 / dim) *
1052  (si_->getSpaceMeasure() / ompl::unitNBallMeasure(si_->getStateDimension())),
1053  1.0 / dim);
1054 
1055  // k_rrg > e * (1 + 1 / d). K-nearest RRT*
1056  k_rrt_ = rewireFactor_ * boost::math::constants::e<double>() * (1.0 + 1.0 / dim);
1057 }
1058 
1059 bool ompl::geometric::STRRTstar::sampleGoalTime(ompl::base::State *goal, double oldBatchTimeBoundFactor,
1060  double newBatchTimeBoundFactor)
1061 {
1062  double ltb, utb;
1063  double minTime =
1064  si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->timeToCoverDistance(startMotion_->state, goal);
1065  if (isTimeBounded_)
1066  {
1067  ltb = minTime;
1068  utb = upperTimeBound_;
1069  }
1070  else if (sampleOldBatch_)
1071  {
1072  ltb = minTime;
1073  utb = minTime * oldBatchTimeBoundFactor;
1074  }
1075  else
1076  {
1077  ltb = minTime * oldBatchTimeBoundFactor;
1078  utb = minTime * newBatchTimeBoundFactor;
1079  }
1080 
1081  if (ltb > utb)
1082  return false; // goal can't be reached in time
1083 
1084  double time = ltb == utb ? ltb : rng_.uniformReal(ltb, utb);
1085  goal->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position = time;
1086  return true;
1087 }
1088 
1089 ompl::base::State *ompl::geometric::STRRTstar::nextGoal(int n, double oldBatchTimeBoundFactor,
1090  double newBatchTimeBoundFactor)
1091 {
1093  return nextGoal(ptc, n, oldBatchTimeBoundFactor, newBatchTimeBoundFactor);
1094 }
1095 
1097  double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
1098 {
1099  return nextGoal(ptc, -1, oldBatchTimeBoundFactor, newBatchTimeBoundFactor);
1100 }
1101 
1103  double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
1104 {
1105  if (pdef_->getGoal() != nullptr)
1106  {
1108  pdef_->getGoal()->as<ompl::base::GoalSampleableRegion>() :
1109  nullptr;
1110 
1111  if (goal != nullptr)
1112  {
1113  if (tempState_ == nullptr)
1114  tempState_ = si_->allocState();
1115  int tryCount = 0;
1116  do
1117  {
1118  goal->sampleGoal(tempState_); // sample space component
1119  // sample time component dependant on sampled space
1120  bool inTime = sampleGoalTime(tempState_, oldBatchTimeBoundFactor, newBatchTimeBoundFactor);
1121  bool bounds = inTime && si_->satisfiesBounds(tempState_);
1122  bool valid = bounds && si_->isValid(tempState_);
1123  if (valid)
1124  {
1125  return tempState_;
1126  }
1127  } while (!ptc.eval() && ++tryCount != n);
1128  }
1129  }
1130 
1131  return nullptr;
1132 }
1133 
1135 {
1136  maxDistance_ = distance;
1137 }
1138 
1140 {
1141  return maxDistance_;
1142 }
1143 
1145 {
1146  return optimumApproxFactor_;
1147 }
1148 
1150 {
1151  if (optimumApproxFactor <= 0 || optimumApproxFactor > 1)
1152  {
1153  OMPL_ERROR("%s: The optimum approximation factor needs to be between 0 and 1.", getName().c_str());
1154  }
1155  optimumApproxFactor_ = optimumApproxFactor;
1156 }
1157 
1158 std::string ompl::geometric::STRRTstar::getRewiringState() const
1159 {
1160  std::vector<std::string> s{"Radius", "KNearest", "Off"};
1161  return s[rewireState_];
1162 }
1163 
1165 {
1166  rewireState_ = OFF;
1167 }
1168 
1170 {
1171  rewireState_ = RADIUS;
1172 }
1173 
1175 {
1176  rewireState_ = KNEAREST;
1177 }
1178 
1179 double ompl::geometric::STRRTstar::getRewireFactor() const
1180 {
1181  return rewireFactor_;
1182 }
1183 
1184 void ompl::geometric::STRRTstar::setRewireFactor(double v)
1185 {
1186  if (v <= 1)
1187  {
1188  OMPL_ERROR("%s: Rewire Factor needs to be greater than 1.", getName().c_str());
1189  }
1190  rewireFactor_ = v;
1191 }
1192 
1194 {
1195  return initialBatchSize_;
1196 }
1197 
1198 void ompl::geometric::STRRTstar::setBatchSize(int v)
1199 {
1200  if (v < 1)
1201  {
1202  OMPL_ERROR("%s: Batch Size needs to be at least 1.", getName().c_str());
1203  }
1204  initialBatchSize_ = v;
1205 }
1206 
1208 {
1209  if (f <= 1.0)
1210  {
1211  OMPL_ERROR("%s: Time Bound Factor Increase needs to be higher than 1.", getName().c_str());
1212  }
1213  timeBoundFactorIncrease_ = f;
1214 }
1215 
1217 {
1218  if (f <= 1.0)
1219  {
1220  OMPL_ERROR("%s: Initial Time Bound Factor Increase needs to be higher than 1.", getName().c_str());
1221  }
1222  initialTimeBoundFactor_ = f;
1223 }
1224 
1226 {
1227  sampleUniformForUnboundedTime_ = uniform;
1228 }
void setSampleUniformForUnboundedTime(bool uniform)
Whether the state space is sampled uniformly or centered at lower time values.
Definition: STRRTstar.cpp:1225
Representation of a motion.
Definition of a compound state.
Definition: State.h:150
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
A shared pointer wrapper for ompl::base::SpaceInformation.
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.
base::Motion * pruneGoalTree()
Prune the goal tree after a solution was found. Return the goal motion, that is connected to the star...
Definition: STRRTstar.cpp:710
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: STRRTstar.cpp:893
static base::Motion * computeSolutionMotion(base::Motion *motion)
Find the solution (connecting) motion for a motion that is indirectly connected.
Definition: STRRTstar.cpp:829
double position
The position in time.
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:159
static void removeFromParent(base::Motion *m)
Removes the given motion from the parent's child list.
Definition: STRRTstar.cpp:935
Definition of an abstract state.
Definition: State.h:113
void setRewiringToKNearest()
Rewire by k-nearest.
Definition: STRRTstar.cpp:1174
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:123
unsigned int getBatchSize() const
The number of samples before the time bound is increased.
Definition: STRRTstar.cpp:1193
A state space consisting of a space and a time component.
bool sampleGoalTime(base::State *goal, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
Samples the time component of a goal state dependant on its space component. Returns false,...
Definition: STRRTstar.cpp:1059
GrowState growTreeSingle(TreeData &tree, TreeGrowingInfo &tgi, base::Motion *rmotion, base::Motion *nmotion)
Grow a tree towards a random state for a single nearest state.
Definition: STRRTstar.cpp:509
void setTimeBoundFactorIncrease(double f)
The value by which the time bound factor is multiplied in each increase step.
Definition: STRRTstar.cpp:1207
Representation of a solution to a planning problem.
Information attached to growing a tree of motions (used internally)
Definition: STRRTstar.h:229
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
void increaseTimeBound(bool hasEqualBounds, double &oldBatchTimeBoundFactor, double &newBatchTimeBoundFactor, bool &startTree, unsigned int &batchSize, int &numBatchSamples)
Definition: STRRTstar.cpp:549
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
double getRange() const
Get the range the planner is using.
Definition: STRRTstar.cpp:1139
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
A shared pointer wrapper for ompl::base::OptimizationObjective.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
GrowState
The state of the tree after an attempt to extend it.
Definition: STRRTstar.h:237
double getOptimumApproxFactor() const
The Optimum Approximation factor (0 - 1).
Definition: STRRTstar.cpp:1144
void removeInvalidGoals(const std::vector< base::Motion * > &invalidGoals)
Remove invalid goal states from the goal set.
Definition: STRRTstar.cpp:850
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
double timeToCoverDistance(const ompl::base::State *state1, const ompl::base::State *state2) const
The time to get from state1 to state2 with respect to vMax.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:486
@ INVALID_GOAL
Invalid goal state.
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...
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:263
A class to store the exit status of Planner::solve()
base::State * nextGoal(int n, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
N tries to sample a goal.
Definition: STRRTstar.cpp:1089
double distanceBetweenTrees_
Distance between the nearest pair of start tree and goal tree nodes.
Definition: STRRTstar.h:301
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths.
Definition: Planner.h:275
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: STRRTstar.cpp:119
bool hasType(GoalType type) const
Check if this goal can be cast to a particular goal type.
Definition: Goal.h:166
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: STRRTstar.cpp:1045
The definition of a time state.
A nearest neighbors datastructure that uses linear search.
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: STRRTstar.cpp:1134
bool eval() const
The implementation of some termination condition. By default, this just calls fn_()
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:152
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:474
std::shared_ptr< ompl::NearestNeighbors< base::Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
Definition: STRRTstar.h:226
void freeMemory()
Free the memory allocated by this planner.
Definition: STRRTstar.cpp:89
@ EXACT_SOLUTION
The planner found an exact solution.
void setRewiringToRadius()
Rewire by radius.
Definition: STRRTstar.cpp:1169
void setRewiringToOff()
Do not rewire at all.
Definition: STRRTstar.cpp:1164
static void addDescendants(base::Motion *m, const TreeData &tree)
Adds given all descendants of the given motion to given tree and checks whether one of the added moti...
Definition: STRRTstar.cpp:954
PlannerTerminationCondition plannerNonTerminatingCondition()
Simple termination condition that always returns false. The termination condition will never be met.
void setOptimized(const OptimizationObjectivePtr &opt, Cost cost, bool meetsObjective)
Set the optimization objective used to optimize this solution, the cost of the solution and whether i...
std::function< void(const Planner *, const std::vector< const base::State * > &, const Cost)> ReportIntermediateSolutionFn
When a planner has an intermediate solution (e.g., optimizing planners), a function with this signatu...
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...
GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, base::Motion *rmotion, std::vector< base::Motion * > &nbh, bool connect)
Attempt to grow a tree towards a random state for the neighborhood of the random state....
Definition: STRRTstar.cpp:440
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: STRRTstar.cpp:869
void getNeighbors(TreeData &tree, base::Motion *motion, std::vector< base::Motion * > &nbh) const
Gets the neighbours of a given motion, using either k-nearest or radius_ as appropriate.
Definition: STRRTstar.cpp:970
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
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...
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 a goal region that can be sampled.
void pruneStartTree()
Prune the start tree after a solution was found.
Definition: STRRTstar.cpp:646
The exception type for ompl.
Definition: Exception.h:78
std::vector< Motion * > children
The set of motions descending from the current motion.
void setOptimumApproxFactor(double optimumApproxFactor)
Set the Optimum Approximation factor. This allows the planner to converge more quickly,...
Definition: STRRTstar.cpp:1149
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: STRRTstar.cpp:58
STRRTstar(const ompl::base::SpaceInformationPtr &si)
Constructor.
Definition: STRRTstar.cpp:40
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
@ INVALID_START
Invalid start state or no start state specified.
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82
void setInitialTimeBoundFactor(double f)
The initial time bound factor.
Definition: STRRTstar.cpp:1216
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122