LBTRRT.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Oren Salzman
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 /* Author: Oren Salzman, Sertac Karaman, Ioan Sucan, Mark Moll */
36 
37 #include "ompl/geometric/planners/rrt/LBTRRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
41 #include <limits>
42 #include <math.h>
43 
44 const double ompl::geometric::LBTRRT::kRRG = 5.5;
45 
47  base::Planner(si, "LBTRRT"),
48  goalBias_(0.05),
49  maxDistance_(0.0),
50  epsilon_(0.4),
51  lastGoalMotion_(NULL),
52  iterations_(0),
53  bestCost_(std::numeric_limits<double>::quiet_NaN())
54  {
55 
57  specs_.directed = true;
58 
59  Planner::declareParam<double>("range", this, &LBTRRT::setRange, &LBTRRT::getRange, "0.:1.:10000.");
60  Planner::declareParam<double>("goal_bias", this, &LBTRRT::setGoalBias, &LBTRRT::getGoalBias, "0.:.05:1.");
61  Planner::declareParam<double>("epsilon", this, &LBTRRT::setApproximationFactor, &LBTRRT::getApproximationFactor, "0.:.1:10.");
62 
63  addPlannerProgressProperty("iterations INTEGER",
64  boost::bind(&LBTRRT::getIterationCount, this));
65  addPlannerProgressProperty("best cost REAL",
66  boost::bind(&LBTRRT::getBestCost, this));
67 
68 }
69 
70 ompl::geometric::LBTRRT::~LBTRRT()
71 {
72  freeMemory();
73 }
74 
76 {
77  Planner::clear();
78  sampler_.reset();
79  freeMemory();
80  if (nn_)
81  nn_->clear();
82  lastGoalMotion_ = NULL;
83  goalMotions_.clear();
84 
85  iterations_ = 0;
86  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
87 }
88 
90 {
91  Planner::setup();
94 
95  if (!nn_)
96  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
97  nn_->setDistanceFunction(boost::bind(&LBTRRT::distanceFunction, this, _1, _2));
98 
99  // Setup optimization objective
100  //
101  // If no optimization objective was specified, then default to
102  // optimizing path length as computed by the distance() function
103  // in the state space.
104  if (pdef_)
105  {
106  if (pdef_->hasOptimizationObjective())
107  {
108  opt_ = pdef_->getOptimizationObjective();
109  if (!dynamic_cast<base::PathLengthOptimizationObjective*>(opt_.get()))
110  OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence for other optimizaton objectives is not guaranteed.", getName().c_str());
111  }
112  else
114  }
115  else
116  {
117  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
118  setup_ = false;
119  }
120 }
121 
123 {
124  if (nn_)
125  {
126  std::vector<Motion*> motions;
127  nn_->list(motions);
128  for (unsigned int i = 0 ; i < motions.size() ; ++i)
129  {
130  if (motions[i]->state)
131  si_->freeState(motions[i]->state);
132  delete motions[i];
133  }
134  }
135 }
136 
138 {
139  checkValidity();
140  base::Goal *goal = pdef_->getGoal().get();
141  base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
142 
143  while (const base::State *st = pis_.nextStart())
144  {
145  Motion *motion = new Motion(si_);
146  si_->copyState(motion->state, st);
147  motion->costLb_ = motion->costApx_ = opt_->identityCost();
148  nn_->add(motion);
149  }
150 
151  if (nn_->size() == 0)
152  {
153  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
155  }
156 
157  if (!sampler_)
158  sampler_ = si_->allocStateSampler();
159 
160  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
161 
162  Motion *solution = lastGoalMotion_;
163 
164  // \TODO Make this variable unnecessary, or at least have it
165  // persist across solve runs
166  base::Cost bestCost = opt_->infiniteCost();
167  Motion *approximation = NULL;
168 
169  double approximatedist = std::numeric_limits<double>::infinity();
170  bool sufficientlyShort = false;
171 
172  Motion *rmotion = new Motion(si_);
173  base::State *rstate = rmotion->state;
174  base::State *xstate = si_->allocState();
175  unsigned int statesGenerated = 0;
176 
177  while (ptc() == false)
178  {
179  iterations_++;
180  /* sample random state (with goal biasing) */
181  // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal states.
182  if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ && goal_s->canSample())
183  goal_s->sampleGoal(rstate);
184  else
185  sampler_->sampleUniform(rstate);
186 
187  /* find closest state in the tree */
188  Motion *nmotion = nn_->nearest(rmotion);
189  base::State *dstate = rstate;
190 
191  /* find state to add */
192  double d = si_->distance(nmotion->state, rstate);
193  if (d > maxDistance_)
194  {
195  si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
196  dstate = xstate;
197  }
198 
199  if (si_->checkMotion(nmotion->state, dstate))
200  {
201  statesGenerated++;
202  /* create a motion */
203  Motion *motion = new Motion(si_);
204  si_->copyState(motion->state, dstate);
205 
206  /* update fields */
207  motion->parentLb_ = nmotion;
208  motion->parentApx_ = nmotion;
209  motion->incCost_ = costFunction(nmotion, motion);
210  motion->costLb_ = opt_->combineCosts(nmotion->costLb_, motion->incCost_);
211  motion->costApx_ = opt_->combineCosts(nmotion->costApx_, motion->incCost_);
212 
213  nmotion->childrenLb_.push_back(motion);
214  nmotion->childrenApx_.push_back(motion);
215 
216  nn_->add(motion);
217 
218  bool checkForSolution = false;
219  /* do lazy rewiring */
220  unsigned int k = std::ceil(std::log(double(nn_->size())) * kRRG);
221  std::vector<Motion *> nnVec;
222  nn_->nearestK(rmotion, k, nnVec);
223 
224  CostCompare costCompare(*opt_, motion);
225  std::sort(nnVec.begin(), nnVec.end(), costCompare);
226 
227  for (std::size_t i = 0; i < nnVec.size(); ++i)
228  checkForSolution |= attemptNodeUpdate(motion, nnVec[i]);
229 
230  for (std::size_t i = 0; i < nnVec.size(); ++i)
231  checkForSolution |= attemptNodeUpdate(nnVec[i], motion);
232 
233  double distanceFromGoal;
234  if (goal->isSatisfied(motion->state, &distanceFromGoal))
235  {
236  goalMotions_.push_back(motion);
237  checkForSolution = true;
238  }
239 
240  // Checking for solution or iterative improvement
241  if (checkForSolution)
242  {
243  for (size_t i = 0; i < goalMotions_.size(); ++i)
244  {
245  if (opt_->isCostBetterThan(goalMotions_[i]->costApx_, bestCost))
246  {
247  bestCost = goalMotions_[i]->costApx_;
248  bestCost_ = bestCost;
249  }
250 
251  sufficientlyShort = opt_->isSatisfied(goalMotions_[i]->costApx_);
252  if (sufficientlyShort)
253  {
254  solution = goalMotions_[i];
255  break;
256  }
257  else if (!solution ||
258  opt_->isCostBetterThan(goalMotions_[i]->costApx_, solution->costApx_))
259  solution = goalMotions_[i];
260  }
261  }
262 
263  // Checking for approximate solution (closest state found to the goal)
264  if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
265  {
266  approximation = motion;
267  approximatedist = distanceFromGoal;
268  }
269  }
270 
271  // terminate if a sufficient solution is found
272  if (solution && sufficientlyShort)
273  break;
274  }
275 
276  bool approximate = (solution == 0);
277  bool addedSolution = false;
278  if (approximate)
279  solution = approximation;
280  else
281  lastGoalMotion_ = solution;
282 
283  if (solution != NULL)
284  {
285  /* construct the solution path */
286  std::vector<Motion*> mpath;
287  while (solution != NULL)
288  {
289  mpath.push_back(solution);
290  solution = solution->parentApx_;
291  }
292 
293  /* set the solution path */
294  PathGeometric *geoPath = new PathGeometric(si_);
295  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
296  geoPath->append(mpath[i]->state);
297 
298  base::PathPtr path(geoPath);
299  // Add the solution path.
300  base::PlannerSolution psol(path);
301  psol.setPlannerName(getName());
302  if (approximate)
303  psol.setApproximate(approximatedist);
304  // Does the solution satisfy the optimization objective?
305  psol.setOptimized(opt_, bestCost, sufficientlyShort);
306  pdef_->addSolutionPath(psol);
307 
308  addedSolution = true;
309  }
310 
311  si_->freeState(xstate);
312  if (rmotion->state)
313  si_->freeState(rmotion->state);
314  delete rmotion;
315 
316  OMPL_INFORM("%s: Created %u states. %u goal states in tree.", getName().c_str(), statesGenerated, goalMotions_.size());
317 
318  return base::PlannerStatus(addedSolution, approximate);
319 }
320 
322 {
323  base::Cost incCost = costFunction(potentialParent, child);
324  base::Cost potentialLb = opt_->combineCosts(potentialParent->costLb_, incCost);
325  base::Cost potentialApx = opt_->combineCosts(potentialParent->costApx_, incCost);
326 
327  if (!opt_->isCostBetterThan(potentialLb, child->costLb_))
328  return false;
329 
330  if (opt_->isCostBetterThan(base::Cost((1.0 + epsilon_) * potentialLb.value()), child->costApx_))
331  {
332  if (si_->checkMotion(potentialParent->state, child->state) == false)
333  return false;
334 
335  removeFromParentLb(child);
336  child->parentLb_ = potentialParent;
337  potentialParent->childrenLb_.push_back(child);
338  child->costLb_ = potentialLb;
339  child->incCost_ = incCost;
340  updateChildCostsLb(child);
341 
342 
343  if (!opt_->isCostBetterThan(potentialApx, child->costApx_))
344  return false;
345 
346  removeFromParentApx(child);
347  child->parentApx_ = potentialParent;
348  potentialParent->childrenApx_.push_back(child);
349  child->costApx_ = potentialApx;
350  updateChildCostsApx(child);
351 
352  if (opt_->isCostBetterThan(potentialApx, bestCost_))
353  return true;
354  }
355  else //(child->costApx_ <= (1 + epsilon_) * potentialLb)
356  {
357  removeFromParentLb(child);
358  child->parentLb_ = potentialParent;
359  potentialParent->childrenLb_.push_back(child);
360  child->costLb_ = potentialLb;
361  child->incCost_ = incCost;
362  updateChildCostsLb(child);
363  }
364  return false;
365 }
366 
368 {
369  Planner::getPlannerData(data);
370 
371  std::vector<Motion*> motions;
372  if (nn_)
373  nn_->list(motions);
374 
375  if (lastGoalMotion_)
377 
378  for (unsigned int i = 0 ; i < motions.size() ; ++i)
379  {
380  if (motions[i]->parentApx_ == NULL)
381  data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
382  else
383  data.addEdge(base::PlannerDataVertex(motions[i]->parentApx_->state),
384  base::PlannerDataVertex(motions[i]->state));
385  }
386 }
387 
389 {
390  for (std::size_t i = 0; i < m->childrenLb_.size(); ++i)
391  {
392  m->childrenLb_[i]->costLb_ = opt_->combineCosts(m->costLb_, m->childrenLb_[i]->incCost_);
393  updateChildCostsLb(m->childrenLb_[i]);
394  }
395 }
397 {
398  for (std::size_t i = 0; i < m->childrenApx_.size(); ++i)
399  {
400  m->childrenApx_[i]->costApx_ = opt_->combineCosts(m->costApx_, m->childrenApx_[i]->incCost_);
401  updateChildCostsApx(m->childrenApx_[i]);
402  }
403 }
404 
406 {
407  return removeFromParent(m, m->parentLb_->childrenLb_);
408 }
410 {
411  return removeFromParent(m, m->parentApx_->childrenApx_);
412 }
413 void ompl::geometric::LBTRRT::removeFromParent(const Motion *m, std::vector<Motion*>& vec)
414 {
415  for (std::vector<Motion*>::iterator it = vec.begin (); it != vec.end(); ++it)
416  if (*it == m)
417  {
418  vec.erase(it);
419  break;
420  }
421 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition: Planner.h:391
void removeFromParentLb(Motion *m)
remove motion from its parent in the lower bound tree
Definition: LBTRRT.cpp:405
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
void setApproximationFactor(double epsilon)
Set the apprimation factor.
Definition: LBTRRT.h:129
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
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:104
void freeMemory()
Free the memory allocated by this planner.
Definition: LBTRRT.cpp:122
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: LBTRRT.cpp:137
LBTRRT(const base::SpaceInformationPtr &si)
Constructor.
Definition: LBTRRT.cpp:46
Representation of a solution to a planning problem.
bool attemptNodeUpdate(Motion *potentialParent, Motion *child)
attempt to rewire the trees
Definition: LBTRRT.cpp:321
Motion * parentLb_
The parent motion in the exploration tree.
Definition: LBTRRT.h:181
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: LBTRRT.h:235
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Abstract definition of goals.
Definition: Goal.h:63
double getGoalBias() const
Get the goal bias the planner is using.
Definition: LBTRRT.h:98
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: LBTRRT.h:254
base::State * state
The state contained by the motion.
Definition: LBTRRT.h:178
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:400
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:222
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
void removeFromParentApx(Motion *m)
remove motion from its parent in the approximation tree
Definition: LBTRRT.cpp:409
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
Definition: LBTRRT.h:251
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:62
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:418
base::Cost costLb_
Cost lower bound on path from start to state.
Definition: LBTRRT.h:187
Abstract definition of a goal region that can be sampled.
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LBTRRT.cpp:89
RNG rng_
The random number generator.
Definition: LBTRRT.h:260
virtual unsigned int maxSampleCount() const =0
Return the maximum number of samples that can be asked for before repeating.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: LBTRRT.h:266
#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.
double getRange() const
Get the range the planner is using.
Definition: LBTRRT.h:114
void removeFromParent(const Motion *m, std::vector< Motion *> &vec)
remove motion from a vector
Definition: LBTRRT.cpp:413
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: LBTRRT.cpp:367
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
void updateChildCostsLb(Motion *m)
update the child cost of the lower bound tree
Definition: LBTRRT.cpp:388
A boost shared pointer wrapper for ompl::base::SpaceInformation.
double epsilon_
approximation factor
Definition: LBTRRT.h:257
An optimization objective which corresponds to optimizing path length.
void updateChildCostsApx(Motion *m)
update the child cost of the approximation tree
Definition: LBTRRT.cpp:396
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
Definition: State.h:50
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: LBTRRT.h:108
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:100
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:403
base::OptimizationObjectivePtr opt_
Objective we&#39;re optimizing.
Definition: LBTRRT.h:263
Motion * parentApx_
The parent motion in the exploration tree.
Definition: LBTRRT.h:184
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
const State * nextStart()
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:230
void setOptimized(const OptimizationObjectivePtr &opt, Cost cost, bool meetsObjective)
Set the optimization objective used to optimize this solution, the cost of the solution and whether i...
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:55
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LBTRRT.cpp:75
static const double kRRG
kRRG = 2e~5.5 is a valid choice for all problem instances
Definition: LBTRRT.h:154
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:232
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:58
double value() const
The value of the cost.
Definition: Cost.h:54
boost::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: LBTRRT.h:248
std::vector< Motion * > goalMotions_
A list of states in the tree that satisfy the goal condition.
Definition: LBTRRT.h:269
double getApproximationFactor() const
Get the apprimation factor.
Definition: LBTRRT.h:135
Definition of a geometric path.
Definition: PathGeometric.h:60
void setGoalBias(double goalBias)
Set the goal bias.
Definition: LBTRRT.h:92
Representation of a motion.
Definition: LBTRRT.h:160
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
base::Cost incCost_
The incremental lower bound cost of this motion&#39;s parent to this motion (this is stored to save dista...
Definition: LBTRRT.h:191
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
unsigned int iterations_
Number of iterations the algorithm performed.
Definition: LBTRRT.h:274
A boost shared pointer wrapper for ompl::base::Path.
base::StateSamplerPtr sampler_
State sampler.
Definition: LBTRRT.h:245
base::Cost costApx_
Approximate cost on path from start to state.
Definition: LBTRRT.h:189
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
base::Cost bestCost_
Best cost found so far by algorithm.
Definition: LBTRRT.h:276