Loading...
Searching...
No Matches
InformedStateSampler.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/InformedStateSampler.h"
38#include "ompl/util/Exception.h"
39#include "ompl/base/OptimizationObjective.h"
40// The goal definitions
41#include "ompl/base/Goal.h"
42
43namespace ompl
44{
45 namespace base
46 {
48 // InformedSampler
49 InformedSampler::InformedSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls)
50 : probDefn_(probDefn), space_(probDefn->getSpaceInformation()->getStateSpace()), numIters_(maxNumberCalls)
51 {
52 // Sanity check the problem.
53 // Check that there is an optimization objective
54 if (!probDefn_->hasOptimizationObjective())
55 {
56 throw Exception("InformedSampler: An optimization objective must be specified at construction.");
57 }
58 // No else
59
60 // Make sure we have at least one start and warn if we have more than one
61 if (probDefn_->getStartStateCount() == 0u)
62 {
63 throw Exception("InformedSampler: At least one start state must be specified at construction.");
64 }
65 // No else
66
67 // Store the optimization objective for later ease.
68 opt_ = probDefn_->getOptimizationObjective();
69 }
70
71 double InformedSampler::getInformedMeasure(const Cost &minCost, const Cost &maxCost) const
72 {
73 // Subtract the measures defined by the max and min costs. These will be defined in the deriving class.
74 return getInformedMeasure(maxCost) - getInformedMeasure(minCost);
75 }
76
78 {
79 // Return the best heuristic estimate of the cost-to-come and cost-to-go from the state considering all
80 // starts.
81
82 // If there's only one start, be simple:
83 if (probDefn_->getStartStateCount() == 1u)
84 {
85 // Calculate and from the one and only start
86 return opt_->combineCosts(opt_->motionCostHeuristic(probDefn_->getStartState(0u), statePtr),
87 opt_->costToGo(statePtr, probDefn_->getGoal().get()));
88 }
89
90
91 // Calculate and return the best
92
93 // Variable
94 // The best cost so far
95 Cost bestCost = opt_->infiniteCost();
96
97 // Iterate over each start and store the best
98 for (unsigned int i = 0u; i < probDefn_->getStartStateCount(); ++i)
99 {
100 // Store the best
101 bestCost = opt_->betterCost(
102 bestCost, opt_->combineCosts(opt_->motionCostHeuristic(probDefn_->getStartState(i), statePtr),
103 opt_->costToGo(statePtr, probDefn_->getGoal().get())));
104 }
105
106 // Return the best
107 return bestCost;
108 }
109
114
116 {
117 return numIters_;
118 }
119
120
122 // InformedStateSampler
123 InformedStateSampler::InformedStateSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls,
124 const GetCurrentCostFunc &costFunc)
125 : StateSampler(probDefn->getSpaceInformation()->getStateSpace().get())
126 {
127 // Call the common constructor with the default informed sampler
128 commonConstructor(
129 costFunc, probDefn->getOptimizationObjective()->allocInformedStateSampler(probDefn, maxNumberCalls));
130 }
131
133 const GetCurrentCostFunc &costFunc,
134 const InformedSamplerPtr &infSampler)
135 : StateSampler(probDefn->getSpaceInformation()->getStateSpace().get())
136 {
137 // Call the common constructor with the given informed sampler
138 commonConstructor(costFunc, infSampler);
139 }
140
141 void InformedStateSampler::commonConstructor(const GetCurrentCostFunc &costFunc,
142 const InformedSamplerPtr &infSampler)
143 {
144 // Store the cost function
145 bestCostFunc_ = costFunc;
146
147 // Store the informed sampler
148 infSampler_ = infSampler;
149
150 // Allocate a base sampler
151 baseSampler_ = StateSampler::space_->allocDefaultStateSampler();
152 }
153
155 {
156 // Variable
157 // Whether informed sampling was successful
158 bool informedSuccess;
159
160 // Call sample uniform with the current best cost, check returning function:
161 informedSuccess = infSampler_->sampleUniform(statePtr, bestCostFunc_());
162
163 // If we were unsuccessful, return a regular sample
164 if (!informedSuccess)
165 {
166 baseSampler_->sampleUniform(statePtr);
167 }
168 // No else.
169 }
170
171 void InformedStateSampler::sampleUniformNear(State *statePtr, const State *near, const double distance)
172 {
173 // Warn:
174 OMPL_WARN("sampleUniformNear is not informed.");
175 return baseSampler_->sampleUniformNear(statePtr, near, distance);
176 }
177
178 void InformedStateSampler::sampleGaussian(State *statePtr, const State *mean, const double stdDev)
179 {
180 // Warn:
181 OMPL_WARN("sampleGaussian is not informed.");
182 return baseSampler_->sampleGaussian(statePtr, mean, stdDev);
183 }
184
185 }; // base
186}; // ompl
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
virtual double getInformedMeasure(const Cost &currentCost) const =0
The measure of the subset of the state space defined by the current solution cost that is being searc...
OptimizationObjectivePtr opt_
A copy of the optimization objective.
unsigned int getMaxNumberOfIters() const
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...
ProblemDefinitionPtr probDefn_
A copy of the problem definition.
unsigned int numIters_
The number of iterations I'm allowed to attempt.
ProblemDefinitionPtr getProblemDefn() const
StateSpacePtr space_
A copy of the state space.
void sampleUniform(State *statePtr) override
Sample uniformly in the subset of the state space whose heuristic solution estimates are less than th...
std::function< Cost()> GetCurrentCostFunc
The definition of a function pointer for querying the current solution cost.
void sampleUniformNear(State *statePtr, const State *near, double distance) override
By default sampleUniformNear throws. This can be overloaded by a specific informed sampler if desired...
InformedStateSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls, const GetCurrentCostFunc &costFunc)
Construct a sampler that only generates states with a heuristic solution estimate that is less than t...
void sampleGaussian(State *statePtr, const State *mean, double stdDev) override
By default sampleGaussian throws. This can be overloaded by a specific informed sampler if desired.
A shared pointer wrapper for ompl::base::ProblemDefinition.
const StateSpace * space_
The state space this sampler samples.
Definition of an abstract state.
Definition State.h:50
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Main namespace. Contains everything in this library.