Loading...
Searching...
No Matches
VFRRT.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2015, Caleb Voss and Wilson Beebe
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 Willow Garage 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/* Authors: Caleb Voss, Wilson Beebe */
36
37#include <utility>
38
39#include "ompl/geometric/planners/rrt/VFRRT.h"
40#include "ompl/base/goals/GoalSampleableRegion.h"
41
42namespace ompl
43{
44 namespace magic
45 {
47 static const unsigned int VFRRT_MEAN_NORM_SAMPLES = 1000;
48 }
49}
50
51ompl::geometric::VFRRT::VFRRT(const base::SpaceInformationPtr &si, VectorField vf, double exploration,
52 double initial_lambda, unsigned int update_freq)
53 : RRT(si)
54 , vf_(std::move(vf))
55 , explorationSetting_(exploration)
56 , lambda_(initial_lambda)
57 , nth_step_(update_freq)
58{
59 setName("VFRRT");
60 maxDistance_ = si->getStateValidityCheckingResolution();
61}
62
64
66{
67 RRT::clear();
68 efficientCount_ = 0;
69 inefficientCount_ = 0;
70 explorationInefficiency_ = 0.;
71 step_ = 0;
72}
73
75{
76 RRT::setup();
77 vfdim_ = si_->getStateSpace()->getValueLocations().size();
78}
79
81{
82 ompl::base::State *rstate = si_->allocState();
83 double sum = 0.;
84 for (unsigned int i = 0; i < magic::VFRRT_MEAN_NORM_SAMPLES; i++)
85 {
86 sampler_->sampleUniform(rstate);
87 sum += vf_(rstate).norm();
88 }
89 si_->freeState(rstate);
91}
92
93Eigen::VectorXd ompl::geometric::VFRRT::getNewDirection(const base::State *qnear, const base::State *qrand)
94{
95 // Set vrand to be the normalized vector from qnear to qrand
96 Eigen::VectorXd vrand(vfdim_);
97 for (unsigned int i = 0; i < vfdim_; i++)
98 vrand[i] = *si_->getStateSpace()->getValueAddressAtIndex(qrand, i) -
99 *si_->getStateSpace()->getValueAddressAtIndex(qnear, i);
100 vrand /= si_->distance(qnear, qrand);
101
102 // Get the vector at qnear, and normalize
103 Eigen::VectorXd vfield = vf_(qnear);
104 const double lambdaScale = vfield.norm();
105 // In the case where there is no vector field present, vfield.norm() == 0,
106 // return the direction of the random state.
107 if (lambdaScale < std::numeric_limits<float>::epsilon())
108 return vrand;
109 vfield /= lambdaScale;
110 // Sample a weight from the distribution
111 const double omega = biasedSampling(vrand, vfield, lambdaScale);
112 // Determine updated direction
113 return computeAlphaBeta(omega, vrand, vfield);
114}
115
116double ompl::geometric::VFRRT::biasedSampling(const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield,
117 double lambdaScale)
118{
119 double sigma = .25 * (vrand - vfield).squaredNorm();
120 updateGain();
121 double scaledLambda = lambda_ * lambdaScale / meanNorm_;
122 double phi = scaledLambda / (1. - std::exp(-2. * scaledLambda));
123 double z = -std::log(1. - sigma * scaledLambda / phi) / scaledLambda;
124 return std::sqrt(2. * z);
125}
126
128{
129 if (step_ == nth_step_)
130 {
131 lambda_ = lambda_ * (1 - explorationInefficiency_ + explorationSetting_);
132 efficientCount_ = inefficientCount_ = 0;
133 explorationInefficiency_ = 0;
134 step_ = 0;
135 }
136 else
137 step_++;
138}
139
140Eigen::VectorXd ompl::geometric::VFRRT::computeAlphaBeta(double omega, const Eigen::VectorXd &vrand,
141 const Eigen::VectorXd &vfield)
142{
143 double w2 = omega * omega;
144 double c = vfield.dot(vrand);
145 double cc_1 = c * c - 1.;
146 double root = std::sqrt(cc_1 * w2 * (w2 - 4.));
147 double beta = -root / (2. * cc_1);
148 double sign = (beta < 0.) ? -1. : 1.;
149 beta *= sign;
150 double alpha = (sign * c * root + cc_1 * (2. - w2)) / (2. * cc_1);
151 return alpha * vfield + beta * vrand;
152}
153
155 const Eigen::VectorXd &v)
156{
157 base::State *newState = si_->allocState();
158 si_->copyState(newState, m->state);
159
160 double d = si_->distance(m->state, rstate);
161 if (d > maxDistance_)
162 d = maxDistance_;
163
164 const base::StateSpacePtr &space = si_->getStateSpace();
165 for (unsigned int i = 0; i < vfdim_; i++)
166 *space->getValueAddressAtIndex(newState, i) += d * v[i];
167 if (!v.hasNaN() && si_->checkMotion(m->state, newState))
168 {
169 auto *motion = new Motion(si_);
170 motion->state = newState;
171 motion->parent = m;
172 updateExplorationEfficiency(motion);
173 nn_->add(motion);
174 return motion;
175 }
176 else
177 {
178 si_->freeState(newState);
179 inefficientCount_++;
180 return nullptr;
181 }
182}
183
185{
186 Motion *near = nn_->nearest(m);
187 if (distanceFunction(m, near) < si_->getStateValidityCheckingResolution())
188 inefficientCount_++;
189 else
190 efficientCount_++;
191 explorationInefficiency_ = inefficientCount_ / (double)(efficientCount_ + inefficientCount_);
192}
193
195{
196 checkValidity();
197 base::Goal *goal = pdef_->getGoal().get();
198 auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
199
200 if (!sampler_)
201 sampler_ = si_->allocStateSampler();
202
203 meanNorm_ = determineMeanNorm();
204
205 while (const base::State *st = pis_.nextStart())
206 {
207 auto *motion = new Motion(si_);
208 si_->copyState(motion->state, st);
209 nn_->add(motion);
210 }
211
212 if (nn_->size() == 0)
213 {
214 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
216 }
217
218 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
219
220 Motion *solution = nullptr;
221 Motion *approxsol = nullptr;
222 double approxdif = std::numeric_limits<double>::infinity();
223 auto *rmotion = new Motion(si_);
224 base::State *rstate = rmotion->state;
225 base::State *xstate = si_->allocState();
226
227 while (ptc == false)
228 {
229 // Sample random state (with goal biasing)
230 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
231 goal_s->sampleGoal(rstate);
232 else
233 sampler_->sampleUniform(rstate);
234
235 // Find closest state in the tree
236 Motion *nmotion = nn_->nearest(rmotion);
237
238 // Modify direction based on vector field before extending
239 Motion *motion = extendTree(nmotion, rstate, getNewDirection(nmotion->state, rstate));
240 if (!motion)
241 continue;
242
243 // Check if we can connect to the goal
244 double dist = 0;
245 bool sat = goal->isSatisfied(motion->state, &dist);
246 if (sat)
247 {
248 approxdif = dist;
249 solution = motion;
250 break;
251 }
252 if (dist < approxdif)
253 {
254 approxdif = dist;
255 approxsol = motion;
256 }
257 }
258
259 bool solved = false;
260 bool approximate = false;
261 if (solution == nullptr)
262 {
263 solution = approxsol;
264 approximate = true;
265 }
266
267 if (solution != nullptr)
268 {
269 lastGoalMotion_ = solution;
270
271 // Construct the solution path
272 std::vector<Motion *> mpath;
273 while (solution != nullptr)
274 {
275 mpath.push_back(solution);
276 solution = solution->parent;
277 }
278
279 // Set the solution path
280 auto path(std::make_shared<PathGeometric>(si_));
281 for (int i = mpath.size() - 1; i >= 0; --i)
282 path->append(mpath[i]->state);
283 pdef_->addSolutionPath(path, approximate, approxdif, name_);
284 solved = true;
285 }
286
287 si_->freeState(xstate);
288 if (rmotion->state)
289 si_->freeState(rmotion->state);
290 delete rmotion;
291
292 OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
293
294 return {solved, approximate};
295}
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
Definition: Goal.h:63
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void setName(const std::string &name)
Set the name of the planner.
Definition: Planner.cpp:61
Definition of an abstract state.
Definition: State.h:50
Representation of a motion.
Definition: RRT.h:148
Motion * parent
The parent motion in the exploration tree.
Definition: RRT.h:163
base::State * state
The state contained by the motion.
Definition: RRT.h:160
Rapidly-exploring Random Trees.
Definition: RRT.h:66
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRT.cpp:71
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRT.cpp:61
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: RRT.h:186
double biasedSampling(const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield, double lambdaScale)
Definition: VFRRT.cpp:116
VFRRT(const base::SpaceInformationPtr &si, VectorField vf, double exploration, double initial_lambda, unsigned int update_freq)
Definition: VFRRT.cpp:51
double determineMeanNorm()
Definition: VFRRT.cpp:80
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Definition: VFRRT.cpp:194
void clear() override
Definition: VFRRT.cpp:65
Eigen::VectorXd getNewDirection(const base::State *qnear, const base::State *qrand)
Definition: VFRRT.cpp:93
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: VFRRT.cpp:74
Motion * extendTree(Motion *m, base::State *rstate, const Eigen::VectorXd &v)
Definition: VFRRT.cpp:154
void updateExplorationEfficiency(Motion *m)
Definition: VFRRT.cpp:184
Eigen::VectorXd computeAlphaBeta(double omega, const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield)
Definition: VFRRT.cpp:140
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
static const unsigned int VFRRT_MEAN_NORM_SAMPLES
Number of sampler to determine mean vector field norm in gVFRRT.
Definition: VFRRT.cpp:47
Main namespace. Contains everything in this library.
STL namespace.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56