Loading...
Searching...
No Matches
BridgeTestValidStateSampler.cpp
1#include "ompl/base/samplers/BridgeTestValidStateSampler.h"
2#include "ompl/base/SpaceInformation.h"
3#include "ompl/tools/config/MagicConstants.h"
4
6 : ValidStateSampler(si)
7 , sampler_(si->allocStateSampler())
8 , stddev_(si->getMaximumExtent() * magic::STD_DEV_AS_SPACE_EXTENT_FRACTION)
9{
10 name_ = "bridge_test";
11 params_.declareParam<double>("standard_deviation", [this](double stddev) { setStdDev(stddev); },
12 [this] { return getStdDev(); });
13}
14
16{
17 unsigned int attempts = 0;
18 bool valid = false;
19 State *endpoint = si_->allocState();
20 do
21 {
22 sampler_->sampleUniform(state);
23 bool v1 = si_->isValid(state);
24 if (!v1)
25 {
26 sampler_->sampleGaussian(endpoint, state, stddev_);
27 bool v2 = si_->isValid(endpoint);
28 if (!v2)
29 {
30 si_->getStateSpace()->interpolate(endpoint, state, 0.5, state);
31 valid = si_->isValid(state);
32 }
33 }
34 ++attempts;
35 } while (!valid && attempts < attempts_);
36
37 si_->freeState(endpoint);
38 return valid;
39}
40
41bool ompl::base::BridgeTestValidStateSampler::sampleNear(State *state, const State *near, const double distance)
42{
43 unsigned int attempts = 0;
44 bool valid = false;
45 State *endpoint = si_->allocState();
46 do
47 {
48 sampler_->sampleUniformNear(state, near, distance);
49 bool v1 = si_->isValid(state);
50 if (!v1)
51 {
52 sampler_->sampleGaussian(endpoint, state, distance);
53 bool v2 = si_->isValid(endpoint);
54 if (!v2)
55 {
56 si_->getStateSpace()->interpolate(endpoint, state, 0.5, state);
57 valid = si_->isValid(state);
58 }
59 }
60 ++attempts;
61 } while (!valid && attempts < attempts_);
62
63 si_->freeState(endpoint);
64 return valid;
65}
bool sampleNear(State *state, const State *near, double distance) override
Sample a state near another, within specified distance. Return false, in case of failure.
double getStdDev() const
Get the standard deviation used when sampling.
double stddev_
The standard deviation to use in the sampling process.
BridgeTestValidStateSampler(const SpaceInformation *si)
Constructor.
StateSamplerPtr sampler_
The sampler to build upon.
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.
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
ParamSet params_
The parameters for this instance of the valid state sampler.
const SpaceInformation * si_
The state space this sampler samples.
std::string name_
The name of the sampler.
unsigned int attempts_
Number of attempts to find a valid sample.
This namespace includes magic constants used in various places in OMPL.
Definition Constraint.h:52