Loading...
Searching...
No Matches
GraphSampler.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2020,
5 * Max Planck Institute for Intelligent Systems (MPI-IS).
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the MPI-IS nor the names
19 * of its contributors may be used to endorse or promote products
20 * derived from this software without specific prior written
21 * permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *********************************************************************/
36
37/* Author: Andreas Orthey */
38
39#include <ompl/multilevel/datastructures/graphsampler/GraphSampler.h>
40
41ompl::multilevel::BundleSpaceGraphSampler::BundleSpaceGraphSampler(BundleSpaceGraph *bundleSpaceGraph)
42 : bundleSpaceGraph_(bundleSpaceGraph)
43{
44 double mu = bundleSpaceGraph_->getBundle()->getMaximumExtent();
45 epsilonGraphThickening_ = mu * epsilonGraphThickeningFraction_;
46 OMPL_DEBUG("Epsilon Graph Thickening constant set to %f", epsilonGraphThickening_);
47
48 pathBiasDecay_.setLambda(exponentialDecayLambda_);
49 pathBiasDecay_.setValueInit(pathBiasFixed_);
50
51 pathThickeningGrowth_.setLambda(exponentialDecayLambda_);
52 pathThickeningGrowth_.setValueInit(epsilonGraphThickening_);
53 pathThickeningGrowth_.setValueTarget(0.0);
54
55 graphThickeningGrowth_.setLambda(exponentialDecayLambda_);
56 graphThickeningGrowth_.setValueInit(epsilonGraphThickening_);
57 graphThickeningGrowth_.setValueTarget(0.0);
58}
59
60void ompl::multilevel::BundleSpaceGraphSampler::clear()
61{
62 double mu = bundleSpaceGraph_->getBundle()->getMaximumExtent();
63 epsilonGraphThickening_ = mu * epsilonGraphThickeningFraction_;
64
65 pathBiasDecay_.reset();
66 pathThickeningGrowth_.reset();
67 graphThickeningGrowth_.reset();
68}
69
70void ompl::multilevel::BundleSpaceGraphSampler::disableSegmentBias()
71{
72 this->segmentBias_ = false;
73}
74
75void ompl::multilevel::BundleSpaceGraphSampler::disablePathBias()
76{
77 pathBiasDecay_.setLambda(0);
78 pathBiasDecay_.setValueTarget(0);
79 pathBiasDecay_.setValueInit(0);
80}
81
82void ompl::multilevel::BundleSpaceGraphSampler::setPathBiasStartSegment(double s)
83{
84 if (!segmentBias_)
85 {
86 this->pathBiasStartSegment_ = 0;
87 }
88 else
89 {
90 if (s > this->pathBiasStartSegment_)
91 {
92 geometric::PathGeometric &spath =
93 static_cast<geometric::PathGeometric &>(*bundleSpaceGraph_->getSolutionPathByReference());
94 OMPL_DEBUG("Set path bias: %f/%f", s, spath.length());
95 if (s > spath.length())
96 {
97 s = spath.length();
98 }
99 this->pathBiasStartSegment_ = s;
100 }
101 }
102}
103
104double ompl::multilevel::BundleSpaceGraphSampler::getPathBiasStartSegment()
105{
106 return this->pathBiasStartSegment_;
107}
108
109void ompl::multilevel::BundleSpaceGraphSampler::sample(base::State *xRandom)
110{
111 base::SpaceInformationPtr bundle = bundleSpaceGraph_->getBundle();
112
113 double p = rng_.uniform01();
114 double pd = pathBiasDecay_();
115
116 if (p < pd && !bundleSpaceGraph_->isDynamic())
117 {
118 geometric::PathGeometric &spath =
119 static_cast<geometric::PathGeometric &>(*bundleSpaceGraph_->getSolutionPathByReference());
120
121 std::vector<base::State *> states = spath.getStates();
122
123 if (states.size() < 2)
124 {
125 // empty solution returned, cannot use path bias sampling
126 sampleImplementation(xRandom);
127 }
128 else
129 {
130 // First one works well for SE3->R3, second one works best for
131 // SE3^k->SE3^{k-1}
132
133 double endLength = spath.length();
134 double distStopping = pathBiasStartSegment_ + rng_.uniform01() * (endLength - pathBiasStartSegment_);
135
136 base::State *s1 = nullptr;
137 base::State *s2 = nullptr;
138
139 int ctr = 0;
140 double distLastSegment = 0;
141 double distCountedSegments = 0;
142 while (distCountedSegments < distStopping && (ctr < (int)states.size() - 1))
143 {
144 s1 = states.at(ctr);
145 s2 = states.at(ctr + 1);
146 distLastSegment = bundle->distance(s1, s2);
147 distCountedSegments += distLastSegment;
148 ctr++;
149 }
150
151 // |---- d -----|
152 //---O------O------------O
153 //|--------- t ----------|
154 //|--------- s ------|
155 // |d-(t-s) |
156 double step = (distLastSegment - (distCountedSegments - distStopping)) / (distLastSegment);
157 bundle->getStateSpace()->interpolate(s1, s2, step, xRandom);
158
159 if (epsilonGraphThickening_ > 0)
160 {
161 double eps = pathThickeningGrowth_();
162 bundleSpaceGraph_->getBundleSamplerPtr()->sampleUniformNear(xRandom, xRandom, eps);
163 }
164 }
165 }
166 else
167 {
168 sampleImplementation(xRandom);
169 }
170
171 // Perturbate sample in epsilon neighborhood
172 // Note: Alternatively, we can use sampleGaussian (but seems to give similar
173 // results)
174 if (epsilonGraphThickening_ > 0)
175 {
176 // Decay on graph thickening (reflects or believe in the usefullness of
177 // the graph for biasing our sampling)
178 double eps = graphThickeningGrowth_();
179 bundleSpaceGraph_->getBundleSamplerPtr()->sampleUniformNear(xRandom, xRandom, eps);
180 }
181}
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition Console.h:70