Loading...
Searching...
No Matches
GaussianValidStateSampler.cpp
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#include "ompl/base/samplers/GaussianValidStateSampler.h"
38#include "ompl/base/SpaceInformation.h"
39#include "ompl/tools/config/MagicConstants.h"
40
43 , sampler_(si->allocStateSampler())
44 , stddev_(si->getMaximumExtent() * magic::STD_DEV_AS_SPACE_EXTENT_FRACTION)
45{
46 name_ = "gaussian";
47 params_.declareParam<double>("standard_deviation",
48 [this](double stddev)
49 {
50 setStdDev(stddev);
51 },
52 [this]
53 {
54 return getStdDev();
55 });
56}
57
59{
60 bool result = false;
61 unsigned int attempts = 0;
62 State *temp = si_->allocState();
63 do
64 {
65 sampler_->sampleUniform(state);
66 bool v1 = si_->isValid(state);
67 sampler_->sampleGaussian(temp, state, stddev_);
68 bool v2 = si_->isValid(temp);
69 if (v1 != v2)
70 {
71 if (v2)
72 si_->copyState(state, temp);
73 result = true;
74 }
75 ++attempts;
76 } while (!result && attempts < attempts_);
77 si_->freeState(temp);
78 return result;
79}
80
81bool ompl::base::GaussianValidStateSampler::sampleNear(State *state, const State *near, const double distance)
82{
83 bool result = false;
84 unsigned int attempts = 0;
85 State *temp = si_->allocState();
86 do
87 {
88 sampler_->sampleUniformNear(state, near, distance);
89 bool v1 = si_->isValid(state);
90 sampler_->sampleGaussian(temp, state, distance);
91 bool v2 = si_->isValid(temp);
92 if (v1 != v2)
93 {
94 if (v2)
95 si_->copyState(state, temp);
96 result = true;
97 }
98 ++attempts;
99 } while (!result && attempts < attempts_);
100 si_->freeState(temp);
101 return result;
102}
bool sampleNear(State *state, const State *near, double distance) override
Sample a state near another, within specified distance. Return false, in case of failure.
bool sample(State *state) override
Sample a state. Return false in case of failure.
void setStdDev(double stddev)
Set the standard deviation to use when sampling.
GaussianValidStateSampler(const SpaceInformation *si)
Constructor.
double getStdDev() const
Get the standard deviation used when sampling.
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=[] { return T();})
This function declares a parameter name, and specifies the setter and getter functions.
Definition: GenericParam.h:231
The base class for space information. This contains all the information about the space planning is d...
Definition of an abstract state.
Definition: State.h:50
Abstract definition of a state sampler.
ParamSet params_
The parameters for this instance of the valid state sampler.
std::string name_
The name of the sampler.