RejectionInfSampler.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, University of Toronto
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 University of Toronto 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 /* Authors: Jonathan Gammell */
36 
37 #include "ompl/base/samplers/informed/RejectionInfSampler.h"
38 #include "ompl/base/OptimizationObjective.h"
39 
40 namespace ompl
41 {
42  namespace base
43  {
44  // The default rejection-sampling class:
45  RejectionInfSampler::RejectionInfSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls)
46  : InformedSampler(probDefn, maxNumberCalls)
47  {
48  // Create the basic sampler
49  baseSampler_ = InformedSampler::space_->allocDefaultStateSampler();
50 
51  // Warn if a cost-to-go heuristic is not defined
52  if (!InformedSampler::opt_->hasCostToGoHeuristic())
53  {
54  OMPL_WARN("RejectionInfSampler: The optimization objective does not have a cost-to-go heuristic "
55  "defined. Informed sampling will likely have little to no effect.");
56  }
57  // No else
58  }
59 
60  bool RejectionInfSampler::sampleUniform(State *statePtr, const Cost &maxCost)
61  {
62  // Variable
63  // The persistent iteration counter:
64  unsigned int iter = 0u;
65 
66  // Call the sampleUniform helper function with my iteration counter:
67  return sampleUniform(statePtr, maxCost, &iter);
68  }
69 
70  bool RejectionInfSampler::sampleUniform(State *statePtr, const Cost &minCost, const Cost &maxCost)
71  {
72  // Variable
73  // Whether we were successful in creating an informed sample. Initially not:
74  bool foundSample = false;
75 
76  // Spend numIters_ iterations trying to find an informed sample:
77  for (unsigned int i = 0u; i < InformedSampler::numIters_ && !foundSample; ++i)
78  {
79  // Call the helper function for the larger cost. It will move our iteration counter:
80  foundSample = sampleUniform(statePtr, maxCost, &i);
81 
82  // Did we find a sample?
83  if (foundSample)
84  {
85  // We did, but it only satisfied the upper bound. Check that it meets the lower bound.
86 
87  // Variables
88  // The cost of the sample we found:
89  Cost sampledCost = InformedSampler::heuristicSolnCost(statePtr);
90 
91  // Check if the sample's cost is greater than or equal to the lower bound
92  foundSample = InformedSampler::opt_->isCostEquivalentTo(minCost, sampledCost) ||
93  InformedSampler::opt_->isCostBetterThan(minCost, sampledCost);
94  }
95  // No else, no sample was found.
96  }
97 
98  // One way or the other, we're done:
99  return foundSample;
100  }
101 
103  {
104  return false;
105  }
106 
107  double RejectionInfSampler::getInformedMeasure(const Cost & /*currentCost*/) const
108  {
109  return InformedSampler::space_->getMeasure();
110  }
111 
112  double RejectionInfSampler::getInformedMeasure(const Cost & /*minCost*/, const Cost & /*maxCost*/) const
113  {
114  return InformedSampler::space_->getMeasure();
115  }
116 
117  bool RejectionInfSampler::sampleUniform(State *statePtr, const Cost &maxCost, unsigned int *iterPtr)
118  {
119  // Variable
120  // Whether we were successful in creating an informed sample. Initially not:
121  bool foundSample = false;
122 
123  // Make numIters_ attempts at finding a sample whose heuristic estimate of solution cost through the sample
124  // is better than maxCost by sampling the entire planning domain
125  for (/* Provided iteration counter */; *iterPtr < InformedSampler::numIters_ && !foundSample;
126  ++(*iterPtr))
127  {
128  // Get a sample:
129  baseSampler_->sampleUniform(statePtr);
130 
131  // Check if it's found, i.e., if f(state) <= maxCost
132  foundSample =
133  InformedSampler::opt_->isCostBetterThan(InformedSampler::heuristicSolnCost(statePtr), maxCost);
134  }
135 
136  // All done, one way or the other:
137  return foundSample;
138  }
139  }; // base
140 }; // ompl
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
bool sampleUniform(State *statePtr, const Cost &maxCost) override
Sample uniformly in the subset of the state space whose heuristic solution estimates are less than th...
A shared pointer wrapper for ompl::base::ProblemDefinition.
OptimizationObjectivePtr opt_
A copy of the optimization objective.
RejectionInfSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls)
Construct a rejection sampler that only generates states with a heuristic solution estimate that is l...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
unsigned int numIters_
The number of iterations I'm allowed to attempt.
virtual Cost heuristicSolnCost(const State *statePtr) const
A helper function to calculate the heuristic estimate of the solution cost for a given state using th...
double getInformedMeasure(const Cost &) const override
The measure of the subset of the state space defined by the current solution cost that is being searc...
StateSpacePtr space_
A copy of the state space.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
bool hasInformedMeasure() const override
Whether the sampler can provide a measure of the informed subset.