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 
46 namespace ompl
47 {
48  namespace base
49  {
64  class DeterministicStateSampler : public StateSampler
65  {
66  public:
67  enum DeterministicSamplerType
68  {
69  HALTON
70  };
71 
74  DeterministicStateSampler(const StateSpace *space,
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 
103  class SO2DeterministicStateSampler : public DeterministicStateSampler
104  {
105  public:
108  SO2DeterministicStateSampler(const StateSpace *space,
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 
126  class RealVectorDeterministicStateSampler : public DeterministicStateSampler
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 
155  class SE2DeterministicStateSampler : public DeterministicStateSampler
156  {
157  public:
160  SE2DeterministicStateSampler(const StateSpace *space,
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
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).
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 sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev).
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
virtual void sampleUniformNear(State *, const State *, double)
Sample a state near another, within a neighborhood controlled by a distance parameter.
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:168
virtual void copyFromReals(State *destination, const std::vector< double > &reals) const
Copy the values from reals to the state destination using getValueAddressAtLocation()
Definition: StateSpace.cpp:337
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.
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.
RealVectorDeterministicStateSampler(const StateSpace *space, DeterministicSamplerType type=DeterministicSamplerType::HALTON)
Constructor, which creates the sequence internally based on the specified sequence type....
SO2DeterministicStateSampler(const StateSpace *space, DeterministicSamplerType type=DeterministicSamplerType::HALTON)
Constructor, which creates the sequence internally based on the specified sequence type....
SE2DeterministicStateSampler(const StateSpace *space, DeterministicSamplerType type=DeterministicSamplerType::HALTON)
Constructor, which creates the sequence internally based on the specified sequence type....
DeterministicStateSampler(const StateSpace *space, DeterministicSamplerType type=DeterministicSamplerType::HALTON)
Constructor, which creates the sequence internally based on the specified sequence type....
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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).
void sampleUniform(State *state) override
Sample a state.
virtual void sampleUniform(State *state)
Sample a state.
Main namespace. Contains everything in this library.