StateSampler.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/StateSampler.h"
38#include "ompl/base/StateSpace.h"
39
40void ompl::base::CompoundStateSampler::addSampler(const StateSamplerPtr &sampler, double weightImportance)
41{
42 samplers_.push_back(sampler);
43 weightImportance_.push_back(weightImportance);
44 samplerCount_ = samplers_.size();
45}
46
48{
49 State **comps = state->as<CompoundState>()->components;
50 for (unsigned int i = 0; i < samplerCount_; ++i)
51 samplers_[i]->sampleUniform(comps[i]);
52}
53
54void ompl::base::CompoundStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
55{
56 State **comps = state->as<CompoundState>()->components;
57 State **nearComps = near->as<CompoundState>()->components;
58 for (unsigned int i = 0; i < samplerCount_; ++i)
59 if (weightImportance_[i] > std::numeric_limits<double>::epsilon())
60 samplers_[i]->sampleUniformNear(comps[i], nearComps[i], distance * weightImportance_[i]);
61 else
62 samplers_[i]->sampleUniform(comps[i]);
63}
64
65void ompl::base::CompoundStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
66{
67 State **comps = state->as<CompoundState>()->components;
68 State **meanComps = mean->as<CompoundState>()->components;
69 for (unsigned int i = 0; i < samplerCount_; ++i)
70 samplers_[i]->sampleGaussian(comps[i], meanComps[i], stdDev * weightImportance_[i]);
71}
72
74 double weight)
75 : StateSampler(space), subspace_(subspace), weight_(weight)
76{
77 work_ = subspace_->allocState();
78 work2_ = subspace_->allocState();
81 if (subspaces_.empty())
82 OMPL_WARN("Subspace state sampler did not find any common subspaces. Sampling will have no effect.");
83}
84
85ompl::base::SubspaceStateSampler::~SubspaceStateSampler()
86{
87 subspace_->freeState(work_);
88 subspace_->freeState(work2_);
89}
90
92{
93 subspaceSampler_->sampleUniform(work_);
94 copyStateData(space_, state, subspace_, work_, subspaces_);
95}
96
97void ompl::base::SubspaceStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
98{
99 copyStateData(subspace_, work2_, space_, near);
100 subspaceSampler_->sampleUniformNear(work_, work2_, distance * weight_);
101 copyStateData(space_, state, subspace_, work_, subspaces_);
102}
103
104void ompl::base::SubspaceStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
105{
106 copyStateData(subspace_, work2_, space_, mean);
107 subspaceSampler_->sampleGaussian(work_, work2_, stdDev * weight_);
108 copyStateData(space_, state, subspace_, work_, subspaces_);
109}
std::vector< double > weightImportance_
The weight of each sampler (used when sampling near a state)
Definition: StateSampler.h:145
virtual void addSampler(const StateSamplerPtr &sampler, double weightImportance)
Add a sampler as part of the new compound sampler. This sampler is used to sample part of the compoun...
void sampleGaussian(State *state, const State *mean, double stdDev) override
Call sampleGaussian for each of the subspace states with stdDev scaled by the corresponding subspace ...
std::vector< StateSamplerPtr > samplers_
The samplers that are composed.
Definition: StateSampler.h:142
void sampleUniform(State *state) override
Sample a state.
void sampleUniformNear(State *state, const State *near, double distance) override
Call sampleUniformNear for each of the subspace states with distance scaled by the corresponding subs...
Definition of a compound state.
Definition: State.h:87
A shared pointer wrapper for ompl::base::StateSampler.
Abstract definition of a state space sampler.
Definition: StateSampler.h:65
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:104
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:71
void getCommonSubspaces(const StateSpacePtr &other, std::vector< std::string > &subspaces) const
Get the set of subspaces that this space and other have in common. The computed list of subspaces doe...
Definition: StateSpace.cpp:493
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
Definition: StateSpace.cpp:800
virtual State * allocState() const =0
Allocate a state that can store a point in the described 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
const StateSpace * subspace_
The subspace to sample.
Definition: StateSampler.h:170
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).
StateSamplerPtr subspaceSampler_
The sampler for the subspace.
Definition: StateSampler.h:173
std::vector< std::string > subspaces_
The names of common subspaces between space_ and subspace_; these are the ones copied after sampling ...
Definition: StateSampler.h:180
SubspaceStateSampler(const StateSpace *space, const StateSpace *subspace, double weight)
Construct a sampler for space but only sample components common to subspace. Use weight as a multipli...
void sampleUniform(State *state) override
Sample a state.
AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
Copy data from source (state from space sourceS) to dest (state from space destS) on a component by c...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66