PathLengthDirectInfSampler.h
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 #ifndef OMPL_BASE_SAMPLERS_INFORMED_PATH_LENGTH_DIRECT_INFORMED_SAMPLER_
38 #define OMPL_BASE_SAMPLERS_INFORMED_PATH_LENGTH_DIRECT_INFORMED_SAMPLER_
39 
40 // We inherit from InformedStateSampler
41 #include "ompl/base/samplers/InformedStateSampler.h"
42 
43 // For std::list
44 #include <list>
45 
46 namespace ompl
47 {
48  namespace base
49  {
82  {
83  public:
86  PathLengthDirectInfSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls);
87  ~PathLengthDirectInfSampler() override;
88 
92  bool sampleUniform(State *statePtr, const Cost &maxCost) override;
93 
97  bool sampleUniform(State *statePtr, const Cost &minCost, const Cost &maxCost) override;
98 
100  bool hasInformedMeasure() const override;
101 
106  double getInformedMeasure(const Cost &currentCost) const override;
107 
110  Cost heuristicSolnCost(const State *statePtr) const override;
111 
112  private:
114  typedef std::shared_ptr<const ompl::ProlateHyperspheroid> ProlateHyperspheroidCPtr;
115 
116  // Helper functions:
117  // High level
120  bool sampleUniform(State *statePtr, const Cost &maxCost, unsigned int *iters);
121 
124  bool sampleBoundsRejectPhs(State *statePtr, unsigned int *iters);
125 
128  bool samplePhsRejectBounds(State *statePtr, unsigned int *iters);
129 
130  // Low level
132  std::vector<double> getInformedSubstate(const State *statePtr) const;
133 
136  void createFullState(State *statePtr, const std::vector<double> &informedVector);
137 
141  void updatePhsDefinitions(const Cost &maxCost);
142 
145  ompl::ProlateHyperspheroidPtr randomPhsPtr();
146 
149  bool keepSample(const std::vector<double> &informedVector);
150 
152  bool isInAnyPhs(const std::vector<double> &informedVector) const;
153 
155  bool isInPhs(const ProlateHyperspheroidCPtr &phsCPtr, const std::vector<double> &informedVector) const;
156 
158  unsigned int numberOfPhsInclusions(const std::vector<double> &informedVector) const;
159 
160  // Variables
162  std::list<ompl::ProlateHyperspheroidPtr> listPhsPtrs_;
163 
165  double summedMeasure_;
166 
169  unsigned int informedIdx_;
170 
172  StateSpacePtr informedSubSpace_;
173 
176  unsigned int uninformedIdx_;
177 
180  StateSpacePtr uninformedSubSpace_;
181 
184  StateSamplerPtr baseSampler_;
185 
187  StateSamplerPtr uninformedSubSampler_;
188 
190  RNG rng_;
191  }; // PathLengthDirectInfSampler
192  }
193 }
194 
195 #endif // OMPL_BASE_SAMPLERS_INFORMED_DIRECT_PATH_LENGTH_INFORMED_SAMPLER_
Cost heuristicSolnCost(const State *statePtr) const override
A helper function to calculate the heuristic estimate of the solution cost for the informed subset of...
A shared pointer wrapper for ompl::base::ProblemDefinition.
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
An abstract class for the concept of using information about the state space and the current solution...
double getInformedMeasure(const Cost &currentCost) const override
The measure of the subset of the state space defined by the current solution cost that is being searc...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:56
bool hasInformedMeasure() const override
Whether the sampler can provide a measure of the informed subset.
An informed sampler for problems seeking to minimize path length.
Definition of an abstract state.
Definition: State.h:49
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...
PathLengthDirectInfSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls)
Construct a sampler that only generates states with a heuristic solution estimate that is less than t...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47