LBTRRT.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Tel Aviv University
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 Tel Aviv University 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, Aditya Mandalika, 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 <limits>
41 #include <cmath>
42 #include <boost/math/constants/constants.hpp>
43 
44 ompl::geometric::LBTRRT::LBTRRT(const base::SpaceInformationPtr &si)
45  : base::Planner(si, "LBTRRT")
46 {
48  specs_.directed = true;
49 
50  Planner::declareParam<double>("range", this, &LBTRRT::setRange, &LBTRRT::getRange, "0.:1.:10000.");
51  Planner::declareParam<double>("goal_bias", this, &LBTRRT::setGoalBias, &LBTRRT::getGoalBias, "0.:.05:1.");
52  Planner::declareParam<double>("epsilon", this, &LBTRRT::setApproximationFactor, &LBTRRT::getApproximationFactor,
53  "0.:.1:10.");
54 
55  addPlannerProgressProperty("iterations INTEGER", [this]
56  {
57  return getIterationCount();
58  });
59  addPlannerProgressProperty("best cost REAL", [this]
60  {
61  return getBestCost();
62  });
63 }
64 
65 ompl::geometric::LBTRRT::~LBTRRT()
66 {
67  freeMemory();
68 }
69 
71 {
72  Planner::clear();
73  sampler_.reset();
74  freeMemory();
75  if (nn_)
76  nn_->clear();
77  lowerBoundGraph_.clear();
78  lastGoalMotion_ = nullptr;
79 
80  iterations_ = 0;
81  bestCost_ = std::numeric_limits<double>::infinity();
82 }
83 
85 {
86  Planner::setup();
87  tools::SelfConfig sc(si_, getName());
88  sc.configurePlannerRange(maxDistance_);
89 
90  if (!nn_)
91  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
92  nn_->setDistanceFunction([this](const Motion *a, const Motion *b)
93  {
94  return distanceFunction(a, b);
95  });
96 }
97 
99 {
100  if (!idToMotionMap_.empty())
101  {
102  for (auto &i : idToMotionMap_)
103  {
104  if (i->state_ != nullptr)
105  si_->freeState(i->state_);
106  delete i;
107  }
108  }
109  idToMotionMap_.clear();
110 }
111 
113 {
114  checkValidity();
115  // update goal and check validity
116  base::Goal *goal = pdef_->getGoal().get();
117  auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
118 
119  if (goal == nullptr)
120  {
121  OMPL_ERROR("%s: Goal undefined", getName().c_str());
123  }
124 
125  // update start and check validity
126  while (const base::State *st = pis_.nextStart())
127  {
128  auto *motion = new Motion(si_);
129  si_->copyState(motion->state_, st);
130  motion->id_ = nn_->size();
131  idToMotionMap_.push_back(motion);
132  nn_->add(motion);
133  lowerBoundGraph_.addVertex(motion->id_);
134  }
135 
136  if (nn_->size() == 0)
137  {
138  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
140  }
141 
142  if (nn_->size() > 1)
143  {
144  OMPL_ERROR("%s: There are multiple start states - currently not supported!", getName().c_str());
146  }
147 
148  if (!sampler_)
149  sampler_ = si_->allocStateSampler();
150 
151  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
152 
153  Motion *solution = lastGoalMotion_;
154  Motion *approxSol = nullptr;
155  double approxdif = std::numeric_limits<double>::infinity();
156  // e*(1+1/d) K-nearest constant, as used in RRT*
157  double k_rrg =
158  boost::math::constants::e<double>() + boost::math::constants::e<double>() / (double)si_->getStateDimension();
159 
160  auto *rmotion = new Motion(si_);
161  base::State *rstate = rmotion->state_;
162  base::State *xstate = si_->allocState();
163  unsigned int statesGenerated = 0;
164 
165  bestCost_ = lastGoalMotion_ != nullptr ? lastGoalMotion_->costApx_ : std::numeric_limits<double>::infinity();
166  while (!ptc())
167  {
168  iterations_++;
169  /* sample random state (with goal biasing) */
170  if ((goal_s != nullptr) && rng_.uniform01() < goalBias_ && goal_s->canSample())
171  goal_s->sampleGoal(rstate);
172  else
173  sampler_->sampleUniform(rstate);
174 
175  /* find closest state in the tree */
176  Motion *nmotion = nn_->nearest(rmotion);
177  base::State *dstate = rstate;
178 
179  /* find state to add */
180  double d = si_->distance(nmotion->state_, rstate);
181  if (d == 0) // this takes care of the case that the goal is a single point and we re-sample it multiple times
182  continue;
183  if (d > maxDistance_)
184  {
185  si_->getStateSpace()->interpolate(nmotion->state_, rstate, maxDistance_ / d, xstate);
186  dstate = xstate;
187  }
188 
189  if (checkMotion(nmotion->state_, dstate))
190  {
191  statesGenerated++;
192  /* create a motion */
193  auto *motion = new Motion(si_);
194  si_->copyState(motion->state_, dstate);
195 
196  /* update fields */
197  double distN = distanceFunction(nmotion, motion);
198 
199  motion->id_ = nn_->size();
200  idToMotionMap_.push_back(motion);
201  lowerBoundGraph_.addVertex(motion->id_);
202  motion->parentApx_ = nmotion;
203 
204  std::list<std::size_t> dummy;
205  lowerBoundGraph_.addEdge(nmotion->id_, motion->id_, distN, false, dummy);
206 
207  motion->costLb_ = nmotion->costLb_ + distN;
208  motion->costApx_ = nmotion->costApx_ + distN;
209  nmotion->childrenApx_.push_back(motion);
210 
211  std::vector<Motion *> nnVec;
212  unsigned int k = std::ceil(k_rrg * log((double)(nn_->size() + 1)));
213  nn_->nearestK(motion, k, nnVec);
214  nn_->add(motion); // if we add the motion before the nearestK call, we will get ourselves...
215 
216  IsLessThan isLessThan(this, motion);
217  std::sort(nnVec.begin(), nnVec.end(), isLessThan);
218 
219  //-------------------------------------------------//
220  // Rewiring Part (i) - find best parent of motion //
221  //-------------------------------------------------//
222  if (motion->parentApx_ != nnVec.front())
223  {
224  for (auto potentialParent : nnVec)
225  {
226  double dist = distanceFunction(potentialParent, motion);
227  considerEdge(potentialParent, motion, dist);
228  }
229  }
230 
231  //------------------------------------------------------------------//
232  // Rewiring Part (ii) //
233  // check if motion may be a better parent to one of its neighbors //
234  //------------------------------------------------------------------//
235  for (auto child : nnVec)
236  {
237  double dist = distanceFunction(motion, child);
238  considerEdge(motion, child, dist);
239  }
240 
241  double dist = 0.0;
242  bool sat = goal->isSatisfied(motion->state_, &dist);
243 
244  if (sat)
245  {
246  approxdif = dist;
247  solution = motion;
248  }
249  if (dist < approxdif)
250  {
251  approxdif = dist;
252  approxSol = motion;
253  }
254 
255  if (solution != nullptr && bestCost_ != solution->costApx_)
256  {
257  OMPL_INFORM("%s: approximation cost = %g", getName().c_str(), solution->costApx_);
258  bestCost_ = solution->costApx_;
259  }
260  }
261  }
262 
263  bool solved = false;
264  bool approximate = false;
265 
266  if (solution == nullptr)
267  {
268  solution = approxSol;
269  approximate = true;
270  }
271 
272  if (solution != nullptr)
273  {
274  lastGoalMotion_ = solution;
275 
276  /* construct the solution path */
277  std::vector<Motion *> mpath;
278  while (solution != nullptr)
279  {
280  mpath.push_back(solution);
281  solution = solution->parentApx_;
282  }
283 
284  /* set the solution path */
285  auto path(std::make_shared<PathGeometric>(si_));
286  for (int i = mpath.size() - 1; i >= 0; --i)
287  path->append(mpath[i]->state_);
288  // Add the solution path.
289  base::PlannerSolution psol(path);
290  psol.setPlannerName(getName());
291  if (approximate)
292  psol.setApproximate(approxdif);
293  pdef_->addSolutionPath(psol);
294  solved = true;
295  }
296 
297  si_->freeState(xstate);
298  if (rmotion->state_ != nullptr)
299  si_->freeState(rmotion->state_);
300  delete rmotion;
301 
302  OMPL_INFORM("%s: Created %u states", getName().c_str(), statesGenerated);
303 
304  return {solved, approximate};
305 }
306 
307 void ompl::geometric::LBTRRT::considerEdge(Motion *parent, Motion *child, double c)
308 {
309  // optimization - check if the bounded approximation invariant
310  // will be violated after the edge insertion (at least for the child node)
311  // if this is the case - perform the local planning
312  // this prevents the update of the graph due to the edge insertion and then the re-update as it is removed
313  double potential_cost = parent->costLb_ + c;
314  if (child->costApx_ > (1 + epsilon_) * potential_cost)
315  if (!checkMotion(parent, child))
316  return;
317 
318  // update lowerBoundGraph_
319  std::list<std::size_t> affected;
320 
321  lowerBoundGraph_.addEdge(parent->id_, child->id_, c, true, affected);
322 
323  // now, check if the bounded apprimation invariant has been violated for each affected vertex
324  // insert them into a priority queue ordered according to the lb cost
325  std::list<std::size_t>::iterator iter;
326  IsLessThanLB isLessThanLB(this);
327  std::set<Motion *, IsLessThanLB> queue(isLessThanLB);
328 
329  for (iter = affected.begin(); iter != affected.end(); ++iter)
330  {
331  Motion *m = getMotion(*iter);
332  m->costLb_ = lowerBoundGraph_.getShortestPathCost(*iter);
333  if (m->costApx_ > (1 + epsilon_) * m->costLb_)
334  queue.insert(m);
335  }
336 
337  while (!queue.empty())
338  {
339  Motion *motion = *(queue.begin());
340  queue.erase(queue.begin());
341 
342  if (motion->costApx_ > (1 + epsilon_) * motion->costLb_)
343  {
344  Motion *potential_parent = getMotion(lowerBoundGraph_.getShortestPathParent(motion->id_));
345  if (checkMotion(potential_parent, motion))
346  {
347  double delta = lazilyUpdateApxParent(motion, potential_parent);
348  updateChildCostsApx(motion, delta);
349  }
350  else
351  {
352  affected.clear();
353 
354  lowerBoundGraph_.removeEdge(potential_parent->id_, motion->id_, true, affected);
355 
356  for (iter = affected.begin(); iter != affected.end(); ++iter)
357  {
358  Motion *affected = getMotion(*iter);
359  auto lb_queue_iter = queue.find(affected);
360  if (lb_queue_iter != queue.end())
361  {
362  queue.erase(lb_queue_iter);
363  affected->costLb_ = lowerBoundGraph_.getShortestPathCost(affected->id_);
364  if (affected->costApx_ > (1 + epsilon_) * affected->costLb_)
365  queue.insert(affected);
366  }
367  else
368  {
369  affected->costLb_ = lowerBoundGraph_.getShortestPathCost(affected->id_);
370  }
371  }
372 
373  motion->costLb_ = lowerBoundGraph_.getShortestPathCost(motion->id_);
374  if (motion->costApx_ > (1 + epsilon_) * motion->costLb_)
375  queue.insert(motion);
376 
377  // optimization - we can remove the opposite edge
378  lowerBoundGraph_.removeEdge(motion->id_, potential_parent->id_, false, affected);
379  }
380  }
381  }
382 
383  }
384 
386 {
387  Planner::getPlannerData(data);
388 
389  std::vector<Motion *> motions;
390  if (nn_)
391  nn_->list(motions);
392 
393  if (lastGoalMotion_ != nullptr)
394  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state_));
395 
396  for (auto &motion : motions)
397  {
398  if (motion->parentApx_ == nullptr)
399  data.addStartVertex(base::PlannerDataVertex(motion->state_));
400  else
401  data.addEdge(base::PlannerDataVertex(motion->parentApx_->state_), base::PlannerDataVertex(motion->state_));
402  }
403 }
404 
406 {
407  for (auto child : m->childrenApx_)
408  {
409  child->costApx_ += delta;
410  updateChildCostsApx(child, delta);
411  }
412 }
413 
415 {
416  double dist = distanceFunction(parent, child);
417  removeFromParentApx(child);
418  double deltaApx = parent->costApx_ + dist - child->costApx_;
419  child->parentApx_ = parent;
420  parent->childrenApx_.push_back(child);
421  child->costApx_ = parent->costApx_ + dist;
422 
423  return deltaApx;
424 }
425 
427 {
428  std::vector<Motion *> &vec = m->parentApx_->childrenApx_;
429  for (auto it = vec.begin(); it != vec.end(); ++it)
430  if (*it == m)
431  {
432  vec.erase(it);
433  break;
434  }
435 }
double lazilyUpdateApxParent(Motion *child, Motion *parent)
lazily update the parent in the approximation tree without updating costs to cildren
Definition: LBTRRT.cpp:414
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
double getGoalBias() const
Get the goal bias the planner is using.
Definition: LBTRRT.h:193
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LBTRRT.cpp:84
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: LBTRRT.h:203
Motion * parentApx_
The parent motion in the approximation tree.
Definition: LBTRRT.h:279
Representation of a motion.
Definition: LBTRRT.h:255
Definition of an abstract state.
Definition: State.h:113
comparator - metric is the lower bound cost
Definition: LBTRRT.h:308
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:467
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:123
double getRange() const
Get the range the planner is using.
Definition: LBTRRT.h:209
std::size_t id_
unique id of the motion
Definition: LBTRRT.h:271
Representation of a solution to a planning problem.
double getApproximationFactor() const
Get the apprimation factor.
Definition: LBTRRT.h:234
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
void freeMemory()
Free the memory allocated by this planner.
Definition: LBTRRT.cpp:98
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
void setGoalBias(double goalBias)
Set the goal bias.
Definition: LBTRRT.h:187
comparator - metric is the cost to reach state via a specific state
Definition: LBTRRT.h:287
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:486
@ INVALID_GOAL
Invalid goal state.
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:269
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LBTRRT.cpp:70
void updateChildCostsApx(Motion *m, double delta)
update the child cost of the approximation tree
Definition: LBTRRT.cpp:405
A class to store the exit status of Planner::solve()
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: LBTRRT.cpp:385
void removeFromParentApx(Motion *m)
remove motion from its parent in the approximation tree
Definition: LBTRRT.cpp:426
Abstract definition of goals.
Definition: Goal.h:126
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...
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: LBTRRT.cpp:112
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
double costLb_
The lower bound cost of the motion while it is stored in the lowerBoundGraph_ and this may seem redun...
Definition: LBTRRT.h:277
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
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...
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 a goal region that can be sampled.
base::State * state_
The state contained by the motion.
Definition: LBTRRT.h:269
std::vector< Motion * > childrenApx_
The children in the approximation tree.
Definition: LBTRRT.h:283
@ INVALID_START
Invalid start state or no start state specified.
double costApx_
The approximation cost.
Definition: LBTRRT.h:281
void setApproximationFactor(double epsilon)
Set the apprimation factor.
Definition: LBTRRT.h:228
LBTRRT(const base::SpaceInformationPtr &si)
Constructor.
Definition: LBTRRT.cpp:44
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
void considerEdge(Motion *parent, Motion *child, double c)
consider an edge for addition to the roadmap
Definition: LBTRRT.cpp:307