RRTXstatic.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2016, Georgia Institute of Technology
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Florian Hauer */
36 
37 #include "ompl/geometric/planners/rrt/RRTXstatic.h"
38 #include <algorithm>
39 #include <boost/math/constants/constants.hpp>
40 #include <limits>
41 #include "ompl/base/Goal.h"
42 #include "ompl/base/goals/GoalSampleableRegion.h"
43 #include "ompl/base/goals/GoalState.h"
44 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
45 #include "ompl/base/samplers/InformedStateSampler.h"
46 #include "ompl/base/samplers/informed/RejectionInfSampler.h"
47 #include "ompl/tools/config/SelfConfig.h"
48 #include "ompl/util/GeometricEquations.h"
49 
50 ompl::geometric::RRTXstatic::RRTXstatic(const base::SpaceInformationPtr &si)
51  : base::Planner(si, "RRTXstatic")
52  , goalBias_(0.05)
53  , maxDistance_(0.0)
54  , useKNearest_(true)
55  , rewireFactor_(1.1)
56  , k_rrt_(0u)
57  , r_rrt_(0.0)
58  , lastGoalMotion_(nullptr)
59  , bestCost_(std::numeric_limits<double>::quiet_NaN())
60  , iterations_(0u)
61  , mc_(opt_, pdef_)
62  , q_(mc_)
63  , epsilonCost_(0.0)
64  , updateChildren_(true)
65  , variant_(0)
66  , alpha_(1.0)
67  , useInformedSampling_(false)
68  , useRejectionSampling_(false)
69  , numSampleAttempts_(100u)
70 {
72  specs_.optimizingPaths = true;
74 
75  Planner::declareParam<double>("range", this, &RRTXstatic::setRange, &RRTXstatic::getRange, "0.:1.:10000.");
76  Planner::declareParam<double>("goal_bias", this, &RRTXstatic::setGoalBias, &RRTXstatic::getGoalBias, "0.:.05:1.");
77  Planner::declareParam<double>("epsilon", this, &RRTXstatic::setEpsilon, &RRTXstatic::getEpsilon, "0.:.01:10.");
78  Planner::declareParam<double>("rewire_factor", this, &RRTXstatic::setRewireFactor, &RRTXstatic::getRewireFactor,
79  "1.0:0.01:2."
80  "0");
81  Planner::declareParam<bool>("use_k_nearest", this, &RRTXstatic::setKNearest, &RRTXstatic::getKNearest, "0,1");
82  Planner::declareParam<bool>("update_children", this, &RRTXstatic::setUpdateChildren, &RRTXstatic::getUpdateChildren,
83  "0,1");
84  Planner::declareParam<int>("rejection_variant", this, &RRTXstatic::setVariant, &RRTXstatic::getVariant, "0:3");
85  Planner::declareParam<double>("rejection_variant_alpha", this, &RRTXstatic::setAlpha, &RRTXstatic::getAlpha, "0.:"
86  "1.");
87  Planner::declareParam<bool>("informed_sampling", this, &RRTXstatic::setInformedSampling,
89  "1");
90  Planner::declareParam<bool>("sample_rejection", this, &RRTXstatic::setSampleRejection,
92  Planner::declareParam<bool>("number_sampling_attempts", this, &RRTXstatic::setNumSamplingAttempts,
93  &RRTXstatic::getNumSamplingAttempts, "10:10:100000");
94 
95  addPlannerProgressProperty("iterations INTEGER", [this] { return numIterationsProperty(); });
96  addPlannerProgressProperty("motions INTEGER", [this] { return numMotionsProperty(); });
97  addPlannerProgressProperty("best cost REAL", [this] { return bestCostProperty(); });
98 }
99 
100 ompl::geometric::RRTXstatic::~RRTXstatic()
101 {
102  freeMemory();
103 }
104 
106 {
107  Planner::setup();
110  if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
111  {
112  OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
113  }
114 
115  if (!nn_)
116  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
117  nn_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });
118 
119  // Setup optimization objective
120  //
121  // If no optimization objective was specified, then default to
122  // optimizing path length as computed by the distance() function
123  // in the state space.
124  if (pdef_)
125  {
126  if (pdef_->hasOptimizationObjective())
127  opt_ = pdef_->getOptimizationObjective();
128  else
129  {
130  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
131  "planning time.",
132  getName().c_str());
133  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
134 
135  // Store the new objective in the problem def'n
136  pdef_->setOptimizationObjective(opt_);
137  }
138  mc_ = MotionCompare(opt_, pdef_);
140  }
141  else
142  {
143  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
144  setup_ = false;
145  }
146 
147  // Calculate some constants:
149 
150  // Set the bestCost_ and prunedCost_ as infinite
151  bestCost_ = opt_->infiniteCost();
152 }
153 
155 {
156  setup_ = false;
157  Planner::clear();
158  sampler_.reset();
159  infSampler_.reset();
160  freeMemory();
161  if (nn_)
162  nn_->clear();
163 
164  lastGoalMotion_ = nullptr;
165  goalMotions_.clear();
166 
167  iterations_ = 0;
168  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
169 }
170 
172 {
173  checkValidity();
174  base::Goal *goal = pdef_->getGoal().get();
175  base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
176 
177  // Check if there are more starts
178  if (pis_.haveMoreStartStates() == true)
179  {
180  // There are, add them
181  while (const base::State *st = pis_.nextStart())
182  {
183  auto *motion = new Motion(si_);
184  si_->copyState(motion->state, st);
185  motion->cost = opt_->identityCost();
186  nn_->add(motion);
187  }
188 
189  // And assure that, if we're using an informed sampler, it's reset
190  infSampler_.reset();
191  }
192  // No else
193 
194  if (nn_->size() == 0)
195  {
196  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
198  }
199 
200  // Allocate a sampler if necessary
201  if (!sampler_ && !infSampler_)
202  {
203  allocSampler();
204  }
205 
206  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
207 
208  if (!si_->getStateSpace()->isMetricSpace())
209  OMPL_WARN("%s: The state space (%s) is not metric and as a result the optimization objective may not satisfy "
210  "the triangle inequality. "
211  "You may need to disable rejection.",
212  getName().c_str(), si_->getStateSpace()->getName().c_str());
213 
214  const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
215 
216  Motion *solution = lastGoalMotion_;
217 
218  Motion *approximation = nullptr;
219  double approximatedist = std::numeric_limits<double>::infinity();
220  bool sufficientlyShort = false;
221 
222  auto *rmotion = new Motion(si_);
223  base::State *rstate = rmotion->state;
224  base::State *xstate = si_->allocState();
225  base::State *dstate;
226 
227  Motion *motion;
228  Motion *nmotion;
229  Motion *nb;
230  Motion *min;
231  Motion *c;
232  bool feas;
233 
234  unsigned int rewireTest = 0;
235  unsigned int statesGenerated = 0;
236 
237  base::Cost incCost, cost;
238 
239  if (solution)
240  OMPL_INFORM("%s: Starting planning with existing solution of cost %.5f", getName().c_str(),
241  solution->cost.value());
242 
243  if (useKNearest_)
244  OMPL_INFORM("%s: Initial k-nearest value of %u", getName().c_str(),
245  (unsigned int)std::ceil(k_rrt_ * log((double)(nn_->size() + 1u))));
246  else
247  OMPL_INFORM(
248  "%s: Initial rewiring radius of %.2f", getName().c_str(),
249  std::min(maxDistance_, r_rrt_ * std::pow(log((double)(nn_->size() + 1u)) / ((double)(nn_->size() + 1u)),
250  1 / (double)(si_->getStateDimension()))));
251 
252  while (ptc == false)
253  {
254  iterations_++;
255 
256  // Computes the RRG values for this iteration (number or radius of neighbors)
257  calculateRRG();
258 
259  // sample random state (with goal biasing)
260  // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal
261  // states.
262  if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ &&
263  goal_s->canSample())
264  goal_s->sampleGoal(rstate);
265  else
266  {
267  // Attempt to generate a sample, if we fail (e.g., too many rejection attempts), skip the remainder of this
268  // loop and return to try again
269  if (!sampleUniform(rstate))
270  continue;
271  }
272 
273  // find closest state in the tree
274  nmotion = nn_->nearest(rmotion);
275 
276  if (intermediateSolutionCallback && si_->equalStates(nmotion->state, rstate))
277  continue;
278 
279  dstate = rstate;
280 
281  // find state to add to the tree
282  double d = si_->distance(nmotion->state, rstate);
283  if (d > maxDistance_)
284  {
285  si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
286  dstate = xstate;
287  }
288 
289  // Check if the motion between the nearest state and the state to add is valid
290  if (si_->checkMotion(nmotion->state, dstate))
291  {
292  // create a motion
293  motion = new Motion(si_);
294  si_->copyState(motion->state, dstate);
295  motion->parent = nmotion;
296  incCost = opt_->motionCost(nmotion->state, motion->state);
297  motion->cost = opt_->combineCosts(nmotion->cost, incCost);
298 
299  // Find nearby neighbors of the new motion
300  getNeighbors(motion);
301 
302  // find which one we connect the new state to
303  for (auto it = motion->nbh.begin(); it != motion->nbh.end();)
304  {
305  nb = it->first;
306  feas = it->second;
307 
308  // Compute cost using nb as a parent
309  incCost = opt_->motionCost(nb->state, motion->state);
310  cost = opt_->combineCosts(nb->cost, incCost);
311  if (opt_->isCostBetterThan(cost, motion->cost))
312  {
313  // Check range and feasibility
314  if ((!useKNearest_ || distanceFunction(motion, nb) < maxDistance_) &&
315  si_->checkMotion(nb->state, motion->state))
316  {
317  // mark than the motino has been checked as valid
318  it->second = true;
319 
320  motion->cost = cost;
321  motion->parent = nb;
322  ++it;
323  }
324  else
325  {
326  // Remove unfeasible neighbor from the list of neighbors
327  it = motion->nbh.erase(it);
328  }
329  }
330  else
331  {
332  ++it;
333  }
334  }
335 
336  // Check if the vertex should included
337  if (!includeVertex(motion))
338  {
339  si_->freeState(motion->state);
340  delete motion;
341  continue;
342  }
343 
344  // Update neighbor motions neighbor datastructure
345  for (auto it = motion->nbh.begin(); it != motion->nbh.end(); ++it)
346  {
347  it->first->nbh.push_back(std::make_pair(motion, it->second));
348  }
349 
350  // add motion to the tree
351  ++statesGenerated;
352  nn_->add(motion);
353  if (updateChildren_)
354  motion->parent->children.push_back(motion);
355 
356  // add the new motion to the queue to propagate the changes
357  updateQueue(motion);
358 
359  bool checkForSolution = false;
360 
361  // Add the new motion to the goalMotion_ list, if it satisfies the goal
362  double distanceFromGoal;
363  if (goal->isSatisfied(motion->state, &distanceFromGoal))
364  {
365  goalMotions_.push_back(motion);
366  checkForSolution = true;
367  }
368 
369  // Process the elements in the queue and rewire the tree until epsilon-optimality
370  while (!q_.empty())
371  {
372  // Get element to update
373  min = q_.top()->data;
374  // Remove element from the queue and NULL the handle so that we know it's not in the queue anymore
375  q_.pop();
376  min->handle = nullptr;
377 
378  // Stop cost propagation if it is not in the relevant region
379  if (opt_->isCostBetterThan(bestCost_, mc_.costPlusHeuristic(min)))
380  break;
381 
382  // Try min as a parent to optimize each neighbor
383  for (auto it = min->nbh.begin(); it != min->nbh.end();)
384  {
385  nb = it->first;
386  feas = it->second;
387 
388  // Neighbor culling: removes neighbors farther than the neighbor radius
389  if ((!useKNearest_ || min->nbh.size() > rrg_k_) && distanceFunction(min, nb) > rrg_r_)
390  {
391  it = min->nbh.erase(it);
392  continue;
393  }
394 
395  // Calculate cost of nb using min as a parent
396  incCost = opt_->motionCost(min->state, nb->state);
397  cost = opt_->combineCosts(min->cost, incCost);
398 
399  // If cost improvement is better than epsilon
400  if (opt_->isCostBetterThan(opt_->combineCosts(cost, epsilonCost_), nb->cost))
401  {
402  if (nb->parent != min)
403  {
404  // changing parent, check feasibility
405  if (!feas)
406  {
407  feas = si_->checkMotion(nb->state, min->state);
408  if (!feas)
409  {
410  // Remove unfeasible neighbor from the list of neighbors
411  it = min->nbh.erase(it);
412  continue;
413  }
414  else
415  {
416  // mark than the motino has been checked as valid
417  it->second = true;
418  }
419  }
420  if (updateChildren_)
421  {
422  // Remove this node from its parent list
423  removeFromParent(nb);
424  // add it as a children of min
425  min->children.push_back(nb);
426  }
427  // Add this node to the new parent
428  nb->parent = min;
429  ++rewireTest;
430  }
431  nb->cost = cost;
432 
433  // Add to the queue for more improvements
434  updateQueue(nb);
435 
436  checkForSolution = true;
437  }
438  ++it;
439  }
440  if (updateChildren_)
441  {
442  // Propagatino of the cost to the children
443  for (auto it = min->children.begin(), end = min->children.end(); it != end; ++it)
444  {
445  c = *it;
446  incCost = opt_->motionCost(min->state, c->state);
447  cost = opt_->combineCosts(min->cost, incCost);
448  c->cost = cost;
449  // Add to the queue for more improvements
450  updateQueue(c);
451 
452  checkForSolution = true;
453  }
454  }
455  }
456 
457  // empty q and reset handles
458  while (!q_.empty())
459  {
460  q_.top()->data->handle = nullptr;
461  q_.pop();
462  }
463  q_.clear();
464 
465  // Checking for solution or iterative improvement
466  if (checkForSolution)
467  {
468  bool updatedSolution = false;
469  for (auto &goalMotion : goalMotions_)
470  {
471  if (opt_->isCostBetterThan(goalMotion->cost, bestCost_))
472  {
473  if (opt_->isFinite(bestCost_) == false)
474  {
475  OMPL_INFORM("%s: Found an initial solution with a cost of %.2f in %u iterations (%u "
476  "vertices in the graph)",
477  getName().c_str(), goalMotion->cost.value(), iterations_, nn_->size());
478  }
479  bestCost_ = goalMotion->cost;
480  updatedSolution = true;
481  }
482 
483  sufficientlyShort = opt_->isSatisfied(goalMotion->cost);
484  if (sufficientlyShort)
485  {
486  solution = goalMotion;
487  break;
488  }
489  else if (!solution || opt_->isCostBetterThan(goalMotion->cost, solution->cost))
490  {
491  solution = goalMotion;
492  updatedSolution = true;
493  }
494  }
495 
496  if (updatedSolution)
497  {
498  if (intermediateSolutionCallback)
499  {
500  std::vector<const base::State *> spath;
501  Motion *intermediate_solution =
502  solution->parent; // Do not include goal state to simplify code.
503 
504  // Push back until we find the start, but not the start itself
505  while (intermediate_solution->parent != nullptr)
506  {
507  spath.push_back(intermediate_solution->state);
508  intermediate_solution = intermediate_solution->parent;
509  }
510 
511  intermediateSolutionCallback(this, spath, bestCost_);
512  }
513  }
514  }
515 
516  // Checking for approximate solution (closest state found to the goal)
517  if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
518  {
519  approximation = motion;
520  approximatedist = distanceFromGoal;
521  }
522  }
523 
524  // terminate if a sufficient solution is found
525  if (solution && sufficientlyShort)
526  break;
527  }
528 
529  bool approximate = (solution == nullptr);
530  bool addedSolution = false;
531  if (approximate)
532  solution = approximation;
533  else
534  lastGoalMotion_ = solution;
535 
536  if (solution != nullptr)
537  {
538  ptc.terminate();
539  // construct the solution path
540  std::vector<Motion *> mpath;
541  while (solution != nullptr)
542  {
543  mpath.push_back(solution);
544  solution = solution->parent;
545  }
546 
547  // set the solution path
548  auto path = std::make_shared<PathGeometric>(si_);
549  for (int i = mpath.size() - 1; i >= 0; --i)
550  path->append(mpath[i]->state);
551 
552  // Add the solution path.
553  base::PlannerSolution psol(path);
554  psol.setPlannerName(getName());
555  if (approximate)
556  psol.setApproximate(approximatedist);
557  // Does the solution satisfy the optimization objective?
558  psol.setOptimized(opt_, bestCost_, sufficientlyShort);
559  pdef_->addSolutionPath(psol);
560 
561  addedSolution = true;
562  }
563 
564  si_->freeState(xstate);
565  if (rmotion->state)
566  si_->freeState(rmotion->state);
567  delete rmotion;
568 
569  OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree. Final solution cost "
570  "%.3f",
571  getName().c_str(), statesGenerated, rewireTest, goalMotions_.size(), bestCost_.value());
572 
573  return base::PlannerStatus(addedSolution, approximate);
574 }
575 
577 {
578  // If x->handle is not NULL, x is already in the queue and needs to be update, otherwise it is inserted
579  if (x->handle != nullptr)
580  {
581  q_.update(x->handle);
582  }
583  else
584  {
585  x->handle = q_.insert(x);
586  }
587 }
588 
590 {
591  for (auto it = m->parent->children.begin(); it != m->parent->children.end(); ++it)
592  {
593  if (*it == m)
594  {
595  m->parent->children.erase(it);
596  break;
597  }
598  }
599 }
600 
602 {
603  double cardDbl = static_cast<double>(nn_->size() + 1u);
604  rrg_k_ = std::ceil(k_rrt_ * log(cardDbl));
605  rrg_r_ = std::min(maxDistance_,
606  r_rrt_ * std::pow(log(cardDbl) / cardDbl, 1 / static_cast<double>(si_->getStateDimension())));
607 }
608 
610 {
611  if (motion->nbh.size() > 0)
612  {
613  return;
614  }
615 
616  std::vector<Motion *> nbh;
617  if (useKNearest_)
618  {
619  //- k-nearest RRT*
620  nn_->nearestK(motion, rrg_k_, nbh);
621  }
622  else
623  {
624  nn_->nearestR(motion, rrg_r_, nbh);
625  }
626 
627  motion->nbh.resize(nbh.size());
628  std::transform(nbh.begin(), nbh.end(), motion->nbh.begin(),
629  [](Motion *m) { return std::pair<Motion *, bool>(m, false); });
630 }
631 
633 {
634  switch (variant_)
635  {
636  case 1:
637  return opt_->isCostBetterThan(mc_.alphaCostPlusHeuristic(x, alpha_), opt_->infiniteCost()); // Always true?
638  case 2:
639  return opt_->isCostBetterThan(mc_.alphaCostPlusHeuristic(x->parent, alpha_), bestCost_);
640  case 3:
641  return opt_->isCostBetterThan(mc_.alphaCostPlusHeuristic(x, alpha_), bestCost_);
642  default: // no rejection
643  return true;
644  }
645 }
646 
648 {
649  if (nn_)
650  {
651  std::vector<Motion *> motions;
652  nn_->list(motions);
653  for (auto &motion : motions)
654  {
655  if (motion->state)
656  si_->freeState(motion->state);
657  delete motion;
658  }
659  }
660 }
661 
663 {
664  Planner::getPlannerData(data);
665 
666  std::vector<Motion *> motions;
667  if (nn_)
668  nn_->list(motions);
669 
670  if (lastGoalMotion_)
672 
673  for (auto &motion : motions)
674  {
675  if (motion->parent == nullptr)
676  data.addStartVertex(base::PlannerDataVertex(motion->state));
677  else
678  data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
679  }
680 }
681 
683 {
684  if (static_cast<bool>(opt_) == true)
685  {
686  if (opt_->hasCostToGoHeuristic() == false)
687  {
688  OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
689  }
690  }
691 
692  // This option is mutually exclusive with setSampleRejection, assert that:
693  if (informedSampling == true && useRejectionSampling_ == true)
694  {
695  OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
696  }
697 
698  // Check if we're changing the setting of informed sampling. If we are, we will need to create a new sampler, which
699  // we only want to do if one is already allocated.
700  if (informedSampling != useInformedSampling_)
701  {
702  // Store the value
703  useInformedSampling_ = informedSampling;
704 
705  // If we currently have a sampler, we need to make a new one
706  if (sampler_ || infSampler_)
707  {
708  // Reset the samplers
709  sampler_.reset();
710  infSampler_.reset();
711 
712  // Create the sampler
713  allocSampler();
714  }
715  }
716 }
717 
719 {
720  if (static_cast<bool>(opt_) == true)
721  {
722  if (opt_->hasCostToGoHeuristic() == false)
723  {
724  OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
725  }
726  }
727 
728  // This option is mutually exclusive with setSampleRejection, assert that:
729  if (reject == true && useInformedSampling_ == true)
730  {
731  OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
732  }
733 
734  // Check if we're changing the setting of rejection sampling. If we are, we will need to create a new sampler, which
735  // we only want to do if one is already allocated.
736  if (reject != useRejectionSampling_)
737  {
738  // Store the setting
739  useRejectionSampling_ = reject;
740 
741  // If we currently have a sampler, we need to make a new one
742  if (sampler_ || infSampler_)
743  {
744  // Reset the samplers
745  sampler_.reset();
746  infSampler_.reset();
747 
748  // Create the sampler
749  allocSampler();
750  }
751  }
752 }
753 
755 {
756  // Allocate the appropriate type of sampler.
758  {
759  // We are using informed sampling, this can end-up reverting to rejection sampling in some cases
760  OMPL_INFORM("%s: Using informed sampling.", getName().c_str());
761  infSampler_ = opt_->allocInformedStateSampler(pdef_, numSampleAttempts_);
762  }
763  else if (useRejectionSampling_)
764  {
765  // We are explicitly using rejection sampling.
766  OMPL_INFORM("%s: Using rejection sampling.", getName().c_str());
767  infSampler_ = std::make_shared<base::RejectionInfSampler>(pdef_, numSampleAttempts_);
768  }
769  else
770  {
771  // We are using a regular sampler
772  sampler_ = si_->allocStateSampler();
773  }
774 }
775 
777 {
778  // Use the appropriate sampler
780  {
781  // Attempt the focused sampler and return the result.
782  // If bestCost is changing a lot by small amounts, this could
783  // be prunedCost_ to reduce the number of times the informed sampling
784  // transforms are recalculated.
785  return infSampler_->sampleUniform(statePtr, bestCost_);
786  }
787  else
788  {
789  // Simply return a state from the regular sampler
790  sampler_->sampleUniform(statePtr);
791 
792  // Always true
793  return true;
794  }
795 }
796 
798 {
799  double dimDbl = static_cast<double>(si_->getStateDimension());
800 
801  // k_rrt > 2^(d + 1) * e * (1 + 1 / d). K-nearest RRT*
802  k_rrt_ = rewireFactor_ * (std::pow(2, dimDbl + 1) * boost::math::constants::e<double>() * (1.0 + 1.0 / dimDbl));
803 
804  // r_rrt > (2*(1+1/d))^(1/d)*(measure/ballvolume)^(1/d)
805  r_rrt_ = rewireFactor_ * std::pow(2 * (1.0 + 1.0 / dimDbl) * (si_->getSpaceMeasure() / unitNBallMeasure(si_->getStateDimension())), 1.0 / dimDbl);
806 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition: Planner.h:408
void setUpdateChildren(bool val)
Set whether or not to always propagate cost updates to children.
Definition: RRTXstatic.h:239
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
void allocSampler()
Create the samplers.
Definition: RRTXstatic.cpp:754
bool getUpdateChildren() const
True if the cost is always propagate to children.
Definition: RRTXstatic.h:245
base::State * state
The state contained by the motion.
Definition: RRTXstatic.h:340
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
bool useRejectionSampling_
The status of the sample rejection parameter.
Definition: RRTstar.h:484
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRTXstatic.cpp:105
Representation of a solution to a planning problem.
std::vector< std::pair< Motion *, bool > > nbh
The set of neighbors of this motion with a boolean indicating if the feasibility of edge as been test...
Definition: RRTXstatic.h:353
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: RRTXstatic.cpp:797
void setAlpha(const double a)
Set the value alpha used for rejection sampling.
Definition: RRTXstatic.h:265
bool sampleUniform(base::State *statePtr)
Generate a sample.
Definition: RRTXstatic.cpp:776
base::OptimizationObjectivePtr opt_
Objective we&#39;re optimizing.
Definition: RRTstar.h:463
Representation of a motion (node of the tree)
Definition: RRTXstatic.h:326
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: RRTstar.h:391
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: RRTXstatic.h:349
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
Definition: RRTstar.h:438
Abstract definition of goals.
Definition: Goal.h:62
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
BinaryHeap< Motion *, MotionCompare >::Element * handle
Handle to identify the motion in the queue.
Definition: RRTXstatic.h:356
bool getKNearest() const
Get the state of using a k-nearest search for rewiring.
Definition: RRTXstatic.h:233
void setGoalBias(double goalBias)
Set the goal bias.
Definition: RRTXstatic.h:127
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: RRTstar.h:466
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: RRTXstatic.cpp:171
std::vector< Motion * > goalMotions_
A list of states in the tree that satisfy the goal condition.
Definition: RRTstar.h:469
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths...
Definition: Planner.h:228
base::Cost bestCost_
Best cost found so far by algorithm.
Definition: RRTstar.h:505
int getVariant() const
Get variant used for rejection sampling.
Definition: RRTXstatic.h:259
void setKNearest(bool useKNearest)
Use a k-nearest search for rewiring instead of a r-disc search.
Definition: RRTXstatic.h:227
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s r_rrg* (or k_rrg = s k_rrg*) ...
Definition: RRTXstatic.h:202
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: RRTstar.h:441
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
base::State * state
The state contained by the motion.
Definition: RRTstar.h:347
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:342
void setSampleRejection(const bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition: RRTXstatic.cpp:718
void removeFromParent(Motion *m)
Removes the given motion from the parent&#39;s child list.
Definition: RRTXstatic.cpp:589
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTstar.cpp:647
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
void calculateRRG()
Calculate the rrg_r_ and rrg_k_ terms.
Definition: RRTXstatic.cpp:601
void setNumSamplingAttempts(unsigned int numAttempts)
Set the number of attempts to make while performing rejection or informed sampling.
Definition: RRTXstatic.h:158
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:68
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:438
Abstract definition of a goal region that can be sampled.
void getNeighbors(Motion *motion, std::vector< Motion *> &nbh) const
Gets the neighbours of a given motion, using either k-nearest of radius as appropriate.
Definition: RRTstar.cpp:609
virtual unsigned int maxSampleCount() const =0
Return the maximum number of samples that can be asked for before repeating.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTXstatic.cpp:647
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual void setEpsilon(double epsilon)
Set the threshold epsilon.
Definition: RRTXstatic.h:173
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...
bool getSampleRejection() const
Get the state of the sample rejection option.
Definition: RRTXstatic.h:152
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRTXstatic.cpp:154
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
Definition: State.h:49
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:101
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:421
base::InformedSamplerPtr infSampler_
An informed sampler.
Definition: RRTstar.h:431
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:427
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:230
void getNeighbors(Motion *motion) const
Gets the neighbours of a given motion, using either k-nearest of radius as appropriate.
Definition: RRTXstatic.cpp:609
base::StateSamplerPtr sampler_
State sampler.
Definition: RRTstar.h:428
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...
void setInformedSampling(bool informedSampling)
Use direct sampling of the heuristic for the generation of random samples (e.g., x_rand). If a direct sampling method is not defined for the objective, rejection sampling will be used by default.
Definition: RRTXstatic.cpp:682
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...
double getGoalBias() const
Get the goal bias the planner is using.
Definition: RRTXstatic.h:133
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: RRTstar.h:363
RNG rng_
The random number generator.
Definition: RRTstar.h:444
double r_rrt_
A constant for r-disc rewiring calculations.
Definition: RRTstar.h:457
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
unsigned int getNumSamplingAttempts() const
Get the number of attempts to make while performing rejection or informed sampling.
Definition: RRTXstatic.h:164
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:59
double rewireFactor_
The rewiring factor, s, so that r_rrt = s r_rrt* > r_rrt* (or k_rrt = s k_rrt* > k_rrt*) ...
Definition: RRTstar.h:451
bool useInformedSampling_
Option to use informed sampling.
Definition: RRTstar.h:481
bool sampleUniform(base::State *statePtr)
Generate a sample.
Definition: RRTstar.cpp:1124
double k_rrt_
A constant for k-nearest rewiring calculations.
Definition: RRTstar.h:454
double getAlpha() const
Get the value alpha used for rejection sampling.
Definition: RRTXstatic.h:271
void allocSampler()
Create the samplers.
Definition: RRTstar.cpp:1095
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:216
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: RRTstar.cpp:1145
double getEpsilon() const
Get the threshold epsilon the planner is using.
Definition: RRTXstatic.h:179
double value() const
The value of the cost.
Definition: Cost.h:56
bool getInformedSampling() const
Get the state direct heuristic sampling.
Definition: RRTXstatic.h:143
void removeFromParent(Motion *m)
Removes the given motion from the parent&#39;s child list.
Definition: RRTstar.cpp:626
void updateQueue(Motion *x)
Update (or add) a motion in the queue.
Definition: RRTXstatic.cpp:576
void setVariant(const int variant)
Set variant used for rejection sampling.
Definition: RRTXstatic.h:251
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: RRTXstatic.h:189
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: RRTXstatic.cpp:662
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: RRTstar.h:434
bool includeVertex(const Motion *x) const
Test if the vertex should be included according to the variant in use.
Definition: RRTXstatic.cpp:632
double getRange() const
Get the range the planner is using.
Definition: RRTXstatic.h:195
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
base::Cost cost
The cost up to this motion.
Definition: RRTXstatic.h:346
double getRewireFactor() const
Set the rewiring scale factor, s, such that r_rrg = s r_rrg* > r_rrg* (or k_rrg = s k_rrg* > k_rrg*...
Definition: RRTXstatic.h:210
Defines the operator to compare motions.
Definition: RRTXstatic.h:290
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns...
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:139
bool useKNearest_
Option to use k-nearest search for rewiring.
Definition: RRTstar.h:447
unsigned int iterations_
Number of iterations the algorithm performed.
Definition: RRTstar.h:515
Motion * parent
The parent motion in the exploration tree.
Definition: RRTXstatic.h:343
unsigned int numSampleAttempts_
The number of attempts to make at informed sampling.
Definition: RRTstar.h:493
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68