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
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 }
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();
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
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>()
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
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;
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
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);
1085 goal->as<ompl::base::CompoundState>()->as<ompl::base::TimeStateSpace::StateType>(1)->position = time;
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
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
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 }
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}
A nearest neighbors datastructure that uses linear search.
virtual void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const =0
Get the nearest neighbors of a point, within a specified radius.
const DistanceFunction & getDistanceFunction() const
Get the distance function used.
virtual void add(const _T &data)=0
Add an element to the datastructure.
virtual std::size_t size() const =0
Get the number of elements in the datastructure.
virtual void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const =0
Get the k-nearest neighbors of a point.
virtual _T nearest(const _T &data) const =0
Get the nearest neighbor of a point.
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.
bool hasType(GoalType type) const
Check if this goal can be cast to a particular goal type.
Definition Goal.h:102
Representation of a motion.
std::vector< Motion * > children
The set of motions descending from the current motion.
A shared pointer wrapper for ompl::base::OptimizationObjective.
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_()
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition Planner.h:422
A shared pointer wrapper for ompl::base::SpaceInformation.
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.
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...
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....
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
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...
GrowState
The state of the tree after an attempt to extend it.
Definition STRRTstar.h:142
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 getOptimumApproxFactor() const
The Optimum Approximation factor (0 - 1).
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.
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.
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...
base::Motion * pruneGoalTree()
Prune the goal tree after a solution was found. Return the goal motion, that is connected to the star...
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,...
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.
void increaseTimeBound(bool hasEqualBounds, double &oldBatchTimeBoundFactor, double &newBatchTimeBoundFactor, bool &startTree, unsigned int &batchSize, int &numBatchSamples)
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.
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.
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.
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...
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition Planner.h:199
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths.
Definition Planner.h:211
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