StateSampler.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
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 Rice University 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 /* Author: Ioan Sucan */
36 
37 #ifndef OMPL_BASE_STATE_SAMPLER_
38 #define OMPL_BASE_STATE_SAMPLER_
39 
40 #include "ompl/base/State.h"
41 #include "ompl/util/RandomNumbers.h"
42 #include "ompl/util/ClassForward.h"
43 #include <vector>
44 #include <string>
45 #include <functional>
46 
47 namespace ompl
48 {
49  namespace base
50  {
52  OMPL_CLASS_FORWARD(StateSpace);
54 
56 
57  OMPL_CLASS_FORWARD(StateSampler);
59 
64  class StateSampler
65  {
66  public:
67  // non-copyable
68  StateSampler(const StateSampler &) = delete;
69  StateSampler &operator=(const StateSampler &) = delete;
70 
72  StateSampler(const StateSpace *space) : space_(space)
73  {
74  }
75 
76  virtual ~StateSampler() = default;
77 
79  virtual void sampleUniform(State *state) = 0;
80 
91  virtual void sampleUniformNear(State *state, const State *near, double distance) = 0;
92 
100  virtual void sampleGaussian(State *state, const State *mean, double stdDev) = 0;
101 
102  protected:
104  const StateSpace *space_;
105 
107  RNG rng_;
108  };
109 
111  class CompoundStateSampler : public StateSampler
112  {
113  public:
115  CompoundStateSampler(const StateSpace *space) : StateSampler(space), samplerCount_(0)
116  {
117  }
118 
120  ~CompoundStateSampler() override = default;
121 
128  virtual void addSampler(const StateSamplerPtr &sampler, double weightImportance);
129 
130  void sampleUniform(State *state) override;
131 
134  void sampleUniformNear(State *state, const State *near, double distance) override;
135 
138  void sampleGaussian(State *state, const State *mean, double stdDev) override;
139 
140  protected:
142  std::vector<StateSamplerPtr> samplers_;
143 
145  std::vector<double> weightImportance_;
146 
147  private:
149  unsigned int samplerCount_;
150  };
151 
153  class SubspaceStateSampler : public StateSampler
154  {
155  public:
159  SubspaceStateSampler(const StateSpace *space, const StateSpace *subspace, double weight);
160  ~SubspaceStateSampler() override;
161 
162  void sampleUniform(State *state) override;
163 
164  void sampleUniformNear(State *state, const State *near, double distance) override;
165 
166  void sampleGaussian(State *state, const State *mean, double stdDev) override;
167 
168  protected:
170  const StateSpace *subspace_;
171 
174 
176  double weight_;
177 
180  std::vector<std::string> subspaces_;
181 
182  private:
184  State *work_;
185 
187  State *work2_;
188  };
189 
191  using StateSamplerAllocator = std::function<StateSamplerPtr(const StateSpace *)>;
192  }
193 }
194 
195 #endif
virtual void sampleUniform(State *state)=0
Sample a state.
virtual void addSampler(const StateSamplerPtr &sampler, double weightImportance)
Add a sampler as part of the new compound sampler. This sampler is used to sample part of the compoun...
Construct a sampler that samples only within a subspace of the space.
Definition: StateSampler.h:217
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
Definition of an abstract state.
Definition: State.h:113
void sampleUniform(State *state) override
Sample a state.
std::vector< StateSamplerPtr > samplers_
The samplers that are composed.
Definition: StateSampler.h:206
double weight_
The weigth factor to multiply distance and stdDev when sampling in the vicinity of a state.
Definition: StateSampler.h:240
~CompoundStateSampler() override=default
Destructor. This frees the added samplers as well.
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:168
std::vector< std::string > subspaces_
The names of common subspaces between space_ and subspace_; these are the ones copied after sampling ...
Definition: StateSampler.h:244
std::vector< double > weightImportance_
The weight of each sampler (used when sampling near a state)
Definition: StateSampler.h:209
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
SubspaceStateSampler(const StateSpace *space, const StateSpace *subspace, double weight)
Construct a sampler for space but only sample components common to subspace. Use weight as a multipli...
StateSamplerPtr subspaceSampler_
The sampler for the subspace.
Definition: StateSampler.h:237
std::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
Definition: StateSampler.h:255
const StateSpace * subspace_
The subspace to sample.
Definition: StateSampler.h:234
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:171
void sampleGaussian(State *state, const State *mean, double stdDev) override
Call sampleGaussian for each of the subspace states with stdDev scaled by the corresponding subspace ...
Abstract definition of a state space sampler.
Definition: StateSampler.h:128
A shared pointer wrapper for ompl::base::StateSampler.
virtual void sampleGaussian(State *state, const State *mean, double stdDev)=0
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev).
void sampleUniform(State *state) override
Sample a state.
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev).
virtual void sampleUniformNear(State *state, const State *near, double distance)=0
Sample a state near another, within a neighborhood controlled by a distance parameter.
Main namespace. Contains everything in this library.
void sampleUniformNear(State *state, const State *near, double distance) override
Call sampleUniformNear for each of the subspace states with distance scaled by the corresponding subs...
CompoundStateSampler(const StateSpace *space)
Constructor.
Definition: StateSampler.h:179