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