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 #include <ompl/config.h>
41 #if !OMPL_HAVE_EIGEN3
42 #error The PathLengthDirectInfSampler class uses Eigen3, which was not detected at build time.
43 #endif
44 
45 // We inherit from InformedStateSampler
46 #include "ompl/base/samplers/InformedStateSampler.h"
47 
48 // For std::list
49 #include <list>
50 
51 namespace ompl
52 {
53  namespace base
54  {
87  {
88  public:
91  PathLengthDirectInfSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls);
92  ~PathLengthDirectInfSampler() override;
93 
97  bool sampleUniform(State *statePtr, const Cost &maxCost) override;
98 
102  bool sampleUniform(State *statePtr, const Cost &minCost, const Cost &maxCost) override;
103 
105  bool hasInformedMeasure() const override;
106 
111  double getInformedMeasure(const Cost &currentCost) const override;
112 
115  Cost heuristicSolnCost(const State *statePtr) const override;
116 
117  private:
119  typedef std::shared_ptr<const ompl::ProlateHyperspheroid> ProlateHyperspheroidCPtr;
120 
121  // Helper functions:
122  // High level
125  bool sampleUniform(State *statePtr, const Cost &maxCost, unsigned int *iters);
126 
129  bool sampleBoundsRejectPhs(State *statePtr, unsigned int *iters);
130 
133  bool samplePhsRejectBounds(State *statePtr, unsigned int *iters);
134 
135  // Low level
137  std::vector<double> getInformedSubstate(const State *statePtr) const;
138 
141  void createFullState(State *statePtr, const std::vector<double> &informedVector);
142 
146  void updatePhsDefinitions(const Cost &maxCost);
147 
150  ompl::ProlateHyperspheroidPtr randomPhsPtr();
151 
154  bool keepSample(const std::vector<double> &informedVector);
155 
157  bool isInAnyPhs(const std::vector<double> &informedVector) const;
158 
160  bool isInPhs(const ProlateHyperspheroidCPtr &phsCPtr, const std::vector<double> &informedVector) const;
161 
163  unsigned int numberOfPhsInclusions(const std::vector<double> &informedVector) const;
164 
165  // Variables
167  std::list<ompl::ProlateHyperspheroidPtr> listPhsPtrs_;
168 
170  double summedMeasure_;
171 
174  unsigned int informedIdx_;
175 
177  StateSpacePtr informedSubSpace_;
178 
181  unsigned int uninformedIdx_;
182 
185  StateSpacePtr uninformedSubSpace_;
186 
189  StateSamplerPtr baseSampler_;
190 
192  StateSamplerPtr uninformedSubSampler_;
193 
195  RNG rng_;
196  }; // PathLengthDirectInfSampler
197  }
198 }
199 
200 #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:58
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