Loading...
Searching...
No Matches
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
40ompl::geometric::STRRTstar::STRRTstar(const ompl::base::SpaceInformationPtr &si)
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;
48 specs_.canReportIntermediateSolutions = true;
49 Planner::declareParam<double>("range", this, &STRRTstar::setRange, &STRRTstar::getRange, "0.:1.:10000.");
50 distanceBetweenTrees_ = std::numeric_limits<double>::infinity();
51}
52
53ompl::geometric::STRRTstar::~STRRTstar()
54{
55 freeMemory();
56}
57
59{
60 Planner::setup();
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 {
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 }
83
84 si_->getStateSpace()->as<ompl::base::SpaceTimeStateSpace>()->updateEpsilon();
85 // Calculate some constants:
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{
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 {
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 {
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
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.
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 =
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
549void 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
563void 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>()
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);
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>()
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>()
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
850void 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;
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
991bool 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
1059bool 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);
1086 return true;
1087}
1088
1089ompl::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
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
1158std::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
1179double ompl::geometric::STRRTstar::getRewireFactor() const
1180{
1181 return rewireFactor_;
1182}
1183
1184void 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
1198void 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 }
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 }
1223}
1224
A nearest neighbors datastructure that uses linear search.
Definition of a compound state.
Definition State.h:87
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition State.h:95
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Abstract definition of a goal region that can be sampled.
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
Representation of a motion.
std::vector< Motion * > children
The set of motions descending from the current motion.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
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...
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...
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...
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...
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
bool eval() const
The implementation of some termination condition. By default, this just calls fn_()
T * as()
Cast this instance to a desired type.
Definition Planner.h:230
PlannerInputStates pis_
Utility class to extract valid input states.
Definition Planner.h:416
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition Planner.h:422
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
const std::string & getName() const
Get the name of the planner.
Definition Planner.cpp:56
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
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:106
bool setup_
Flag indicating whether setup() has been called.
Definition Planner.h:433
A state space consisting of a space and a time component.
TimeStateSpace * getTimeComponent()
The time component as a TimeStateSpace pointer.
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.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition StateSpace.h:78
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
The definition of a time state.
double position
The position in time.
bool isBounded() const
Check if the time is bounded or not.
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...
TreeData tGoal_
The goal tree.
Definition STRRTstar.h:199
double k_rrt_
A constant for k-nearest rewiring calculations.
Definition STRRTstar.h:289
base::Motion * startMotion_
The start Motion, used for conditional sampling and start tree pruning.
Definition STRRTstar.h:229
void setOptimumApproxFactor(double optimumApproxFactor)
Set the Optimum Approximation factor. This allows the planner to converge more quickly,...
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....
unsigned int initialBatchSize_
Number of samples of the first batch.
Definition STRRTstar.h:306
std::vector< base::Motion * > newBatchGoalMotions_
The goal Motions, that were added in the current expansion step, used for uniform sampling over a gro...
Definition STRRTstar.h:236
static void removeFromParent(base::Motion *m)
Removes the given motion from the parent's child list.
void freeMemory()
Free the memory allocated by this planner.
Definition STRRTstar.cpp:89
double bestTime_
The current best time i.e. cost of all found solutions.
Definition STRRTstar.h:211
GrowState growTreeSingle(TreeData &tree, TreeGrowingInfo &tgi, base::Motion *rmotion, base::Motion *nmotion)
Grow a tree towards a random state for a single nearest state.
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...
TreeData tStart_
The start tree.
Definition STRRTstar.h:196
GrowState
The state of the tree after an attempt to extend it.
Definition STRRTstar.h:142
@ ADVANCED
progress has been made towards the randomly sampled state
Definition STRRTstar.h:146
@ TRAPPED
no progress has been made
Definition STRRTstar.h:144
@ REACHED
the randomly sampled state was reached
Definition STRRTstar.h:148
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
void setSampleUniformForUnboundedTime(bool uniform)
Whether the state space is sampled uniformly or centered at lower time values.
double r_rrt_
A constant for r-disc rewiring calculations.
Definition STRRTstar.h:292
int goalStateSampleRatio_
The ratio, a goal state is sampled compared to the size of the goal tree.
Definition STRRTstar.h:322
double getOptimumApproxFactor() const
The Optimum Approximation factor (0 - 1).
unsigned int numIterations_
The number of while loop iterations.
Definition STRRTstar.h:214
double upperTimeBound_
Upper bound for the time up to which solutions are searched for.
Definition STRRTstar.h:223
double optimumApproxFactor_
The factor to which found solution times need to be reduced compared to minimum time,...
Definition STRRTstar.h:226
double rewireFactor_
The rewiring factor, s, so that r_rrt = s \times r_rrt* > r_rrt* (or k_rrt = s \times k_rrt* > k_rrt*...
Definition STRRTstar.h:286
double timeBoundFactorIncrease_
The factor, the time bound is increased with after the batch is full.
Definition STRRTstar.h:313
bool sampleUniformForUnboundedTime_
Whether the samples are uniformly distributed over the whole space or are centered at lower times.
Definition STRRTstar.h:319
double getRange() const
Get the range the planner is using.
void setTimeBoundFactorIncrease(double f)
The value by which the time bound factor is multiplied in each increase step.
std::vector< base::Motion * > goalMotions_
The goal Motions, used for conditional sampling and pruning.
Definition STRRTstar.h:232
double distanceBetweenTrees_
Distance between the nearest pair of start tree and goal tree nodes.
Definition STRRTstar.h:205
void setInitialTimeBoundFactor(double f)
The initial time bound factor.
base::PathPtr bestSolution_
The current best solution path with respect to shortest time.
Definition STRRTstar.h:208
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition STRRTstar.h:202
void pruneStartTree()
Prune the start tree after a solution was found.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
int numSolutions_
The number of found solutions.
Definition STRRTstar.h:217
base::Motion * pruneGoalTree()
Prune the goal tree after a solution was found. Return the goal motion, that is connected to the star...
double distanceFunction(const base::Motion *a, const base::Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition STRRTstar.h:173
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
STRRTstar(const ompl::base::SpaceInformationPtr &si)
Constructor.
Definition STRRTstar.cpp:40
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition STRRTstar.cpp:58
void setRewiringToOff()
Do not rewire at all.
bool sampleGoalTime(base::State *goal, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
Samples the time component of a goal state dependant on its space component. Returns false,...
ompl::RNG rng_
The random number generator.
Definition STRRTstar.h:325
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.
base::State * nextGoal(int n, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
N tries to sample a goal.
void removeInvalidGoals(const std::vector< base::Motion * > &invalidGoals)
Remove invalid goal states from the goal set.
base::ConditionalStateSampler sampler_
State sampler.
Definition STRRTstar.h:193
double initialTimeBoundFactor_
Initial factor, the minimum time of each goal is multiplied with to calculate the upper time bound.
Definition STRRTstar.h:310
double minimumTime_
Minimum Time at which any goal can be reached, if moving on a straight line.
Definition STRRTstar.h:220
void setRewiringToRadius()
Rewire by radius.
void setRewiringToKNearest()
Rewire by k-nearest.
static base::Motion * computeSolutionMotion(base::Motion *motion)
Find the solution (connecting) motion for a motion that is indirectly connected.
bool isTimeBounded_
Whether the time is bounded or not. The first solution automatically bounds the time.
Definition STRRTstar.h:300
std::shared_ptr< ompl::NearestNeighbors< base::Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
Definition STRRTstar.h:130
void setRange(double distance)
Set the range the planner is supposed to use.
unsigned int getBatchSize() const
The number of samples before the time bound is increased.
double initialTimeBound_
The time bound the planner is initialized with. Used to reset for repeated planning.
Definition STRRTstar.h:303
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition GoalTypes.h:56
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...
PlannerTerminationCondition plannerNonTerminatingCondition()
Simple termination condition that always returns false. The termination condition will never be met.
Namespace containing time datatypes and time operations.
Definition Time.h:50
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition String.cpp:82
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
Representation of a solution to a planning problem.
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
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...
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ EXACT_SOLUTION
The planner found an exact solution.
@ INVALID_GOAL
Invalid goal state.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.
@ TIMEOUT
The planner failed to find a solution.
Information attached to growing a tree of motions (used internally)
Definition STRRTstar.h:134