Loading...
Searching...
No Matches
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
46namespace 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
79 {
80 OMPL_WARN("Deterministic sampler does not support near sampling.");
81 }
82
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
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
DeterministicStateSampler(const StateSpace *space, DeterministicSamplerType type=DeterministicSamplerType::HALTON)
Constructor, which creates the sequence internally based on the specified sequence type....
The lower and upper bounds for an Rn space.
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.
double * values
The value of the actual vector in Rn
A state space representing Rn. The distance function is the L2 norm.
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.
void sampleUniform(State *state) override
Sample a state.
A state in SE(2): (x, y, yaw)
void setX(double x)
Set the X component of the state.
A state space representing SE(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).
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.
The definition of a state in SO(2)
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 unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
Main namespace. Contains everything in this library.