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 
42 namespace ompl
43 {
44  namespace magic
45  {
47  static const unsigned int VFRRT_MEAN_NORM_SAMPLES = 1000;
48  }
49 }
50 
51 ompl::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);
90  return sum / magic::VFRRT_MEAN_NORM_SAMPLES;
91 }
92 
93 Eigen::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 
116 double 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 
140 Eigen::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();
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 }
void setName(const std::string &name)
Set the name of the planner.
Definition: Planner.cpp:61
Eigen::VectorXd getNewDirection(const base::State *qnear, const base::State *qrand)
Definition: VFRRT.cpp:93
Definition of an abstract state.
Definition: State.h:113
Motion * parent
The parent motion in the exploration tree.
Definition: RRT.h:259
base::State * state
The state contained by the motion.
Definition: RRT.h:256
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Definition: VFRRT.cpp:194
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
static const unsigned int VFRRT_MEAN_NORM_SAMPLES
Number of sampler to determine mean vector field norm in gVFRRT.
Definition: VFRRT.cpp:111
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: VFRRT.cpp:74
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRT.cpp:71
double biasedSampling(const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield, double lambdaScale)
Definition: VFRRT.cpp:116
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRT.cpp:61
Eigen::VectorXd computeAlphaBeta(double omega, const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield)
Definition: VFRRT.cpp:140
void clear() override
Definition: VFRRT.cpp:65
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
VFRRT(const base::SpaceInformationPtr &si, VectorField vf, double exploration, double initial_lambda, unsigned int update_freq)
Definition: VFRRT.cpp:51
Motion * extendTree(Motion *m, base::State *rstate, const Eigen::VectorXd &v)
Definition: VFRRT.cpp:154
Rapidly-exploring Random Trees.
Definition: RRT.h:129
A class to store the exit status of Planner::solve()
Representation of a motion.
Definition: RRT.h:243
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
void updateExplorationEfficiency(Motion *m)
Definition: VFRRT.cpp:184
Abstract definition of goals.
Definition: Goal.h:126
double determineMeanNorm()
Definition: VFRRT.cpp:80
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: RRT.h:282
Abstract definition of a goal region that can be sampled.
@ INVALID_START
Invalid start state or no start state specified.
Main namespace. Contains everything in this library.