Loading...
Searching...
No Matches
DeterministicStateSampler.h
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, Robert Bosch GmbH
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 Robert Bosch GmbH 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: Leonard Bruns */
36
37#ifndef OMPL_BASE_DETERMINISTIC_STATE_SAMPLER_
38#define OMPL_BASE_DETERMINISTIC_STATE_SAMPLER_
39
40#include "ompl/base/StateSpace.h"
41#include "ompl/base/StateSampler.h"
42#include "ompl/base/samplers/deterministic/DeterministicSequence.h"
43
44#include <memory>
45
46namespace ompl
47{
48 namespace base
49 {
65 {
66 public:
67 enum DeterministicSamplerType
68 {
69 HALTON
70 };
71
75 DeterministicSamplerType type = DeterministicSamplerType::HALTON);
78 DeterministicStateSampler(const StateSpace *space, std::shared_ptr<DeterministicSequence> sequence_ptr);
79
80 virtual ~DeterministicStateSampler() = default;
81
82 virtual void sampleUniform(State *state)
83 {
84 std::vector<double> sample = sequence_ptr_->sample();
85 space_->copyFromReals(state, sample);
86 return;
87 }
88
89 virtual void sampleUniformNear(State *, const State *, double)
90 {
91 OMPL_ERROR("sampleUniformNear is not supported for DeterministicStateSampler");
92 }
93 virtual void sampleGaussian(State *, const State *, double)
94 {
95 OMPL_ERROR("sampleGaussian is not supported for DeterministicStateSampler");
96 }
97
98 protected:
99 std::shared_ptr<DeterministicSequence> sequence_ptr_;
100 };
101
104 {
105 public:
109 DeterministicSamplerType type = DeterministicSamplerType::HALTON)
110 : DeterministicStateSampler(space, type)
111 {
112 }
115 SO2DeterministicStateSampler(const StateSpace *space, std::shared_ptr<DeterministicSequence> sequence_ptr)
116 : DeterministicStateSampler(space, sequence_ptr)
117 {
118 }
119
120 void sampleUniform(State *state) override;
121 void sampleUniformNear(State *state, const State *near, double distance) override;
122 void sampleGaussian(State *state, const State *mean, double stdDev) override;
123 };
124
127 {
128 public:
132 DeterministicSamplerType type = DeterministicSamplerType::HALTON)
133 : DeterministicStateSampler(space, type), stretch_(true)
134 {
135 }
139 std::shared_ptr<DeterministicSequence> sequence_ptr,
140 bool stretch = true)
141 : DeterministicStateSampler(space, sequence_ptr), stretch_(stretch)
142 {
143 }
144
145 void sampleUniform(State *state) override;
146 void sampleUniformNear(State *state, const State *near, double distance) override;
147 void sampleGaussian(State *state, const State *mean, double stdDev) override;
148
149 private:
150 bool stretch_{false}; // indicates whether the state is samples in [0,1] and should be stretched to the
151 // state space boundaries
152 };
153
156 {
157 public:
161 DeterministicSamplerType type = DeterministicSamplerType::HALTON)
162 : DeterministicStateSampler(space, type), stretch_rv_(true), stretch_so2_(true)
163 {
164 }
167 SE2DeterministicStateSampler(const StateSpace *space, std::shared_ptr<DeterministicSequence> sequence_ptr,
168 bool stretch_rv = true, bool stretch_so2 = true)
169 : DeterministicStateSampler(space, sequence_ptr), stretch_rv_(stretch_rv), stretch_so2_(stretch_so2)
170 {
171 }
172
173 void sampleUniform(State *state) override;
174 void sampleUniformNear(State *state, const State *near, double distance) override;
175 void sampleGaussian(State *state, const State *mean, double stdDev) override;
176
177 private:
178 bool stretch_rv_; // indicates whether the xy state is sampled in [0,1] and should be stretched to the
179 // state space boundaries
180 bool stretch_so2_; // indicates whether the so2 state is sampled in [0,1] and should be stretched to
181 // [-pi;pi]
182 };
183 } // namespace base
184} // namespace ompl
185
186#endif
An abstract class for the concept of using deterministic sampling sequences to decrease the dispersio...
virtual void sampleGaussian(State *, const State *, double)
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev).
virtual void sampleUniform(State *state)
Sample a state.
virtual void sampleUniformNear(State *, const State *, double)
Sample a state near another, within a neighborhood controlled by a distance parameter.
DeterministicStateSampler(const StateSpace *space, DeterministicSamplerType type=DeterministicSamplerType::HALTON)
Constructor, which creates the sequence internally based on the specified sequence type....
Deterministic state sampler for the Rn state space.
RealVectorDeterministicStateSampler(const StateSpace *space, DeterministicSamplerType type=DeterministicSamplerType::HALTON)
Constructor, which creates the sequence internally based on the specified sequence type....
RealVectorDeterministicStateSampler(const StateSpace *space, std::shared_ptr< DeterministicSequence > sequence_ptr, bool stretch=true)
Constructor that takes a pointer to a DeterministicSequence and uses that object instead of its own....
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).
void sampleUniform(State *state) override
Sample a state.
Deterministic state sampler for the Rn state space.
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev).
SE2DeterministicStateSampler(const StateSpace *space, std::shared_ptr< DeterministicSequence > sequence_ptr, bool stretch_rv=true, bool stretch_so2=true)
Constructor that takes a pointer to a DeterministicSequence and uses that object instead of its own....
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
SE2DeterministicStateSampler(const StateSpace *space, DeterministicSamplerType type=DeterministicSamplerType::HALTON)
Constructor, which creates the sequence internally based on the specified sequence type....
void sampleUniform(State *state) override
Sample a state.
Deterministic state space sampler for SO(2)
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev).
SO2DeterministicStateSampler(const StateSpace *space, std::shared_ptr< DeterministicSequence > sequence_ptr)
Constructor that takes a pointer to a DeterministicSequence and uses that object instead of its own....
SO2DeterministicStateSampler(const StateSpace *space, DeterministicSamplerType type=DeterministicSamplerType::HALTON)
Constructor, which creates the sequence internally based on the specified sequence type....
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
void sampleUniform(State *state) override
Sample a state.
Abstract definition of a state space sampler.
const StateSpace * space_
The state space this sampler samples.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
virtual void copyFromReals(State *destination, const std::vector< double > &reals) const
Copy the values from reals to the state destination using getValueAddressAtLocation()
Definition of an abstract state.
Definition State.h:50
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
Main namespace. Contains everything in this library.