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  {
80  class PathLengthDirectInfSampler : public InformedSampler
81  {
82  public:
85  PathLengthDirectInfSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls);
86  ~PathLengthDirectInfSampler() override;
87 
91  bool sampleUniform(State *statePtr, const Cost &maxCost) override;
92 
96  bool sampleUniform(State *statePtr, const Cost &minCost, const Cost &maxCost) override;
97 
99  bool hasInformedMeasure() const override;
100 
105  double getInformedMeasure(const Cost &currentCost) const override;
106 
109  Cost heuristicSolnCost(const State *statePtr) const override;
110 
111  private:
113  using ProlateHyperspheroidCPtr = std::shared_ptr<const ompl::ProlateHyperspheroid>;
114 
115  // Helper functions:
116  // High level
119  bool sampleUniform(State *statePtr, const Cost &maxCost, unsigned int *iters);
120 
123  bool sampleBoundsRejectPhs(State *statePtr, unsigned int *iters);
124 
127  bool samplePhsRejectBounds(State *statePtr, unsigned int *iters);
128 
129  // Low level
131  std::vector<double> getInformedSubstate(const State *statePtr) const;
132 
135  void createFullState(State *statePtr, const std::vector<double> &informedVector);
136 
140  void updatePhsDefinitions(const Cost &maxCost);
141 
144  ompl::ProlateHyperspheroidPtr randomPhsPtr();
145 
148  bool keepSample(const std::vector<double> &informedVector);
149 
151  bool isInAnyPhs(const std::vector<double> &informedVector) const;
152 
154  bool isInPhs(const ProlateHyperspheroidCPtr &phsCPtr, const std::vector<double> &informedVector) const;
155 
157  unsigned int numberOfPhsInclusions(const std::vector<double> &informedVector) const;
158 
159  // Variables
161  std::list<ompl::ProlateHyperspheroidPtr> listPhsPtrs_;
162 
164  double summedMeasure_;
165 
168  unsigned int informedIdx_;
169 
171  StateSpacePtr informedSubSpace_;
172 
175  unsigned int uninformedIdx_;
176 
179  StateSpacePtr uninformedSubSpace_;
180 
183  StateSamplerPtr baseSampler_;
184 
186  StateSamplerPtr uninformedSubSampler_;
187 
189  RNG rng_;
190  }; // PathLengthDirectInfSampler
191  }
192 }
193 
194 #endif // OMPL_BASE_SAMPLERS_INFORMED_DIRECT_PATH_LENGTH_INFORMED_SAMPLER_
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:89
PathLengthDirectInfSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls)
Construct a sampler that only generates states with a heuristic solution estimate that is less than t...
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...
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.
bool hasInformedMeasure() const override
Whether the sampler can provide a measure of the informed subset.
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
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...
Main namespace. Contains everything in this library.