DeterministicStateSampler.cpp
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 #include "ompl/base/samplers/DeterministicStateSampler.h"
38 #include "ompl/base/samplers/deterministic/HaltonSequence.h"
39 #include "ompl/base/spaces/SE2StateSpace.h"
40 #include "ompl/base/spaces/SO2StateSpace.h"
41 #include "ompl/base/spaces/RealVectorStateSpace.h"
42 
43 #include <iostream>
44 #include <boost/math/constants/constants.hpp>
45 
46 namespace ompl
47 {
48  namespace base
49  {
50  DeterministicStateSampler::DeterministicStateSampler(const StateSpace *space, DeterministicSamplerType type)
51  : StateSampler(space)
52  {
53  switch (type)
54  {
55  case HALTON:
56  sequence_ptr_ = std::make_shared<HaltonSequence>(space->getDimension());
57  break;
58  default:
59  OMPL_WARN("Unknown deterministic sampler type specified, using Halton instead.");
60 
61  break;
62  }
63  }
64 
66  std::shared_ptr<DeterministicSequence> sequence_ptr)
67  : StateSampler(space), sequence_ptr_(sequence_ptr)
68  {
69  }
70 
72  {
73  auto sample = sequence_ptr_->sample();
74  state->as<SO2StateSpace::StateType>()->value =
75  -boost::math::constants::pi<double>() + sample[0] * 2 * boost::math::constants::pi<double>();
76  }
77 
78  void SO2DeterministicStateSampler::sampleUniformNear(State *, const State *, double)
79  {
80  OMPL_WARN("Deterministic sampler does not support near sampling.");
81  }
82 
83  void SO2DeterministicStateSampler::sampleGaussian(State *, const State *, double)
84  {
85  OMPL_WARN("Deterministic sampler does not support Gaussian sampling.");
86  }
87 
89  {
90  auto sample = sequence_ptr_->sample();
91 
92  const unsigned int dim = space_->getDimension();
93 
94  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace *>(space_)->getBounds();
95 
96  if (stretch_)
97  {
98  auto *rstate = static_cast<RealVectorStateSpace::StateType *>(state);
99  for (unsigned int i = 0; i < dim; ++i)
100  rstate->values[i] = bounds.low[i] + sample[i] * (bounds.high[i] - bounds.low[i]);
101  }
102  else
103  {
104  auto *rstate = static_cast<RealVectorStateSpace::StateType *>(state);
105  for (unsigned int i = 0; i < dim; ++i)
106  rstate->values[i] = sample[i];
107  }
108  }
109 
110  void RealVectorDeterministicStateSampler::sampleUniformNear(State *, const State *, double)
111  {
112  OMPL_WARN("Deterministic sampler does not support near sampling.");
113  }
114 
116  {
117  OMPL_WARN("Deterministic sampler does not support Gaussian sampling.");
118  }
119 
121  {
122  auto sample = sequence_ptr_->sample();
123 
124  const RealVectorBounds &bounds = static_cast<const SE2StateSpace *>(space_)->getBounds();
125 
126  auto se2_state_ptr = static_cast<SE2StateSpace::StateType *>(state);
127  if (stretch_rv_)
128  {
129  se2_state_ptr->setX(bounds.low[0] + sample[0] * (bounds.high[0] - bounds.low[0]));
130  se2_state_ptr->setY(bounds.low[1] + sample[1] * (bounds.high[1] - bounds.low[1]));
131  }
132  else
133  se2_state_ptr->setXY(sample[0], sample[1]);
134 
135  if (stretch_so2_)
136  se2_state_ptr->setYaw(-boost::math::constants::pi<double>() +
137  sample[2] * 2 * boost::math::constants::pi<double>());
138  else
139  se2_state_ptr->setYaw(sample[2]);
140  }
141 
143  {
144  OMPL_WARN("Deterministic sampler does not support near sampling.");
145  }
146 
148  {
149  OMPL_WARN("Deterministic sampler does not support Gaussian sampling.");
150  }
151  } // namespace base
152 } // namespace ompl
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).
Definition of an abstract state.
Definition: State.h:114
void setX(double x)
Set the X component of the 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 unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
void sampleUniformNear(State *state, const State *near, double distance) override
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
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
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.
A state space representing SE(2)
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.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
A state in SE(2): (x, y, yaw)
DeterministicStateSampler(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.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
The lower and upper bounds for an Rn space.