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 getGoalBias() const
Get the goal bias the planner is using.
Definition: LBTRRT.h:98
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
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
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
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.
virtual unsigned int maxSampleCount() const =0
Return the maximum number of samples that can be asked for before repeating.
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
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: LBTRRT.h:235
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 getApproximationFactor() const
Get the apprimation factor.
Definition: LBTRRT.h:135
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
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
double value() const
The value of the cost.
Definition: Cost.h:54
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
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
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
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
double getRange() const
Get the range the planner is using.
Definition: LBTRRT.h:114
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...
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
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
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 removeFromParent(const Motion *m, std::vector< Motion * > &vec)
remove motion from a vector
Definition: LBTRRT.cpp:413
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
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
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:55
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_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
#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