Loading...
Searching...
No Matches
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
47namespace ompl
48{
49 namespace base
50 {
52 OMPL_CLASS_FORWARD(StateSpace);
54
56
57 OMPL_CLASS_FORWARD(StateSampler);
59
62
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:
105
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:
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
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
std::vector< double > weightImportance_
The weight of each sampler (used when sampling near 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...
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 ...
std::vector< StateSamplerPtr > samplers_
The samplers that are composed.
~CompoundStateSampler() override=default
Destructor. This frees the added samplers as well.
void sampleUniform(State *state) override
Sample a state.
CompoundStateSampler(const StateSpace *space)
Constructor.
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...
A shared pointer wrapper for ompl::base::StateSampler.
Abstract definition of a state space sampler.
StateSampler(const StateSpace *space)
Constructor.
virtual void sampleUniformNear(State *state, const State *near, double distance)=0
Sample a state near another, within a neighborhood controlled by a distance parameter.
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).
RNG rng_
An instance of a random number generator.
const StateSpace * space_
The state space this sampler samples.
virtual void sampleUniform(State *state)=0
Sample a state.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
Definition of an abstract state.
Definition State.h:50
const StateSpace * subspace_
The subspace to sample.
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev).
StateSamplerPtr subspaceSampler_
The sampler for the subspace.
std::vector< std::string > subspaces_
The names of common subspaces between space_ and subspace_; these are the ones copied after sampling ...
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...
double weight_
The weigth factor to multiply distance and stdDev when sampling in the vicinity of a state.
void sampleUniform(State *state) override
Sample a state.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
std::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
Main namespace. Contains everything in this library.