Loading...
Searching...
No Matches
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)
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.
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.
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
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...
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
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.
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.
std::vector< std::string > subspaces_
The names of common subspaces between space_ and subspace_; these are the ones copied after sampling ...
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