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...
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 sampleUniformNear(State *state, const State *near, const double distance) override
Call sampleUniformNear for each of the subspace states with distance scaled by the corresponding subs...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
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.
void sampleGaussian(State *state, const State *mean, const double stdDev) override
Call sampleGaussian for each of the subspace states with stdDev scaled by the corresponding subspace ...
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, const double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev)...
void sampleUniformNear(State *state, const State *near, const double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
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:502
std::vector< double > weightImportance_
The weight of each sampler (used when sampling near a state)
Definition: StateSampler.h:145