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 
40 void 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 
54 void 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 
65 void 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 
85 ompl::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 
97 void 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 
104 void 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 }
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:104
Definition of a compound state.
Definition: State.h:86
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
A shared pointer wrapper for ompl::base::StateSampler.
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...
std::vector< StateSamplerPtr > samplers_
The samplers that are composed.
Definition: StateSampler.h:142
StateSamplerPtr subspaceSampler_
The sampler for the subspace.
Definition: StateSampler.h:173
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 sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
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...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev)...
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
const StateSpace * subspace_
The subspace to sample.
Definition: StateSampler.h:170
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
void sampleUniform(State *state) override
Sample a state.
Definition of an abstract state.
Definition: State.h:49
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
double weight_
The weigth factor to multiply distance and stdDev when sampling in the vicinity of a state...
Definition: StateSampler.h:176
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 ...
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...
void sampleUniform(State *state) override
Sample a state.
Abstract definition of a state space sampler.
Definition: StateSampler.h:64
virtual void freeState(State *state) const =0
Free the memory of the allocated state.
virtual State * allocState() const =0
Allocate a state that can store a point in the described space.
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:500
std::vector< double > weightImportance_
The weight of each sampler (used when sampling near a state)
Definition: StateSampler.h:145