All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 
66 void ompl::base::CompoundStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
67 {
68  State **comps = state->as<CompoundState>()->components;
69  State **meanComps = mean->as<CompoundState>()->components;
70  for (unsigned int i = 0 ; i < samplerCount_ ; ++i)
71  samplers_[i]->sampleGaussian(comps[i], meanComps[i], stdDev * weightImportance_[i]);
72 }
73 
74 ompl::base::SubspaceStateSampler::SubspaceStateSampler(const StateSpace *space, const StateSpace *subspace, double weight) : StateSampler(space), subspace_(subspace), weight_(weight)
75 {
76  work_ = subspace_->allocState();
77  work2_ = subspace_->allocState();
80  if (subspaces_.empty())
81  logWarn("Subspace state sampler did not find any common subspaces. Sampling will have no effect.");
82 }
83 
84 ompl::base::SubspaceStateSampler::~SubspaceStateSampler(void)
85 {
86  subspace_->freeState(work_);
87  subspace_->freeState(work2_);
88 }
89 
91 {
92  subspaceSampler_->sampleUniform(work_);
93  copyStateData(space_, state, subspace_, work_, subspaces_);
94 }
95 
96 void ompl::base::SubspaceStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
97 {
98  copyStateData(subspace_, work2_, space_, near);
99  subspaceSampler_->sampleUniformNear(work_, work2_, distance * weight_);
100  copyStateData(space_, state, subspace_, work_, subspaces_);
101 }
102 
103 void ompl::base::SubspaceStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
104 {
105  copyStateData(subspace_, work2_, space_, mean);
106  subspaceSampler_->sampleGaussian(work_, work2_, stdDev * weight_);
107  copyStateData(space_, state, subspace_, work_, subspaces_);
108 }