LazyLBTRRT.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, Mark Moll */
36 
37 #include "ompl/geometric/planners/rrt/LazyLBTRRT.h"
38 #include "ompl/tools/config/SelfConfig.h"
39 #include <limits>
40 #include <boost/foreach.hpp>
41 #include <boost/math/constants/constants.hpp>
42 
43 namespace
44 {
45  int getK(unsigned int n, double k_rrg)
46  {
47  return std::ceil(k_rrg * log((double)(n + 1)));
48  }
49 }
50 
52  : base::Planner(si, "LazyLBTRRT")
53  , goalBias_(0.05)
54  , maxDistance_(0.0)
55  , epsilon_(0.4)
56  , lastGoalMotion_(nullptr)
57  , goalMotion_(nullptr)
58  , LPAstarApx_(nullptr)
59  , LPAstarLb_(nullptr)
60  , iterations_(0)
61 {
63  specs_.directed = true;
64 
65  Planner::declareParam<double>("range", this, &LazyLBTRRT::setRange, &LazyLBTRRT::getRange, "0.:1.:10000.");
66  Planner::declareParam<double>("goal_bias", this, &LazyLBTRRT::setGoalBias, &LazyLBTRRT::getGoalBias, "0.:.05:1.");
67  Planner::declareParam<double>("epsilon", this, &LazyLBTRRT::setApproximationFactor,
69 
70  addPlannerProgressProperty("iterations INTEGER", [this]
71  {
72  return getIterationCount();
73  });
74  addPlannerProgressProperty("best cost REAL", [this]
75  {
76  return getBestCost();
77  });
78 }
79 
80 ompl::geometric::LazyLBTRRT::~LazyLBTRRT()
81 {
82  freeMemory();
83 }
84 
86 {
87  Planner::clear();
88  sampler_.reset();
89  freeMemory();
90  if (nn_)
91  nn_->clear();
92  graphLb_.clear();
93  graphApx_.clear();
94  lastGoalMotion_ = nullptr;
95 
96  iterations_ = 0;
97  bestCost_ = std::numeric_limits<double>::infinity();
98 }
99 
101 {
102  Planner::setup();
105 
106  if (!nn_)
107  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
108  nn_->setDistanceFunction([this](const Motion *a, const Motion *b)
109  {
110  return distanceFunction(a, b);
111  });
112 }
113 
115 {
116  if (idToMotionMap_.size() > 0)
117  {
118  for (auto &i : idToMotionMap_)
119  {
120  if (i->state_)
121  si_->freeState(i->state_);
122  delete i;
123  }
124  idToMotionMap_.clear();
125  }
126  delete LPAstarApx_;
127  delete LPAstarLb_;
128 }
129 
131 {
132  checkValidity();
133  // update goal and check validity
134  base::Goal *goal = pdef_->getGoal().get();
135  base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
136 
137  if (!goal)
138  {
139  OMPL_ERROR("%s: Goal undefined", getName().c_str());
141  }
142 
143  while (const base::State *st = pis_.nextStart())
144  {
145  startMotion_ = createMotion(goal_s, st);
146  break;
147  }
148 
149  if (nn_->size() == 0)
150  {
151  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
153  }
154 
155  if (!sampler_)
156  sampler_ = si_->allocStateSampler();
157 
158  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
159 
160  bool solved = false;
161 
162  auto *rmotion = new Motion(si_);
163  base::State *xstate = si_->allocState();
164 
165  goalMotion_ = createGoalMotion(goal_s);
166 
167  CostEstimatorLb costEstimatorLb(goal, idToMotionMap_);
168  LPAstarLb_ = new LPAstarLb(startMotion_->id_, goalMotion_->id_, graphLb_, costEstimatorLb); // rooted at source
169  CostEstimatorApx costEstimatorApx(this);
170  LPAstarApx_ = new LPAstarApx(goalMotion_->id_, startMotion_->id_, graphApx_, costEstimatorApx); // rooted at target
171  double approxdif = std::numeric_limits<double>::infinity();
172  // e+e/d. K-nearest RRT*
173  double k_rrg = boost::math::constants::e<double>() +
174  boost::math::constants::e<double>() / (double)si_->getStateSpace()->getDimension();
175 
177  // step (1) - RRT
179  bestCost_ = std::numeric_limits<double>::infinity();
180  rrt(ptc, goal_s, xstate, rmotion, approxdif);
181  if (ptc() == false)
182  {
183  solved = true;
184 
186  // step (2) - Lazy construction of G_lb
188  idToMotionMap_.push_back(goalMotion_);
189  int k = getK(idToMotionMap_.size(), k_rrg);
190  std::vector<Motion *> nnVec;
191  nnVec.reserve(k);
192  BOOST_FOREACH (Motion *motion, idToMotionMap_)
193  {
194  nn_->nearestK(motion, k, nnVec);
195  BOOST_FOREACH (Motion *neighbor, nnVec)
196  if (neighbor->id_ != motion->id_ && !edgeExistsLb(motion, neighbor))
197  addEdgeLb(motion, neighbor, distanceFunction(motion, neighbor));
198  }
199  idToMotionMap_.pop_back();
200  closeBounds(ptc);
201  }
202 
204  // step (3) - anytime planning
206  while (ptc() == false)
207  {
208  std::tuple<Motion *, base::State *, double> res = rrtExtend(goal_s, xstate, rmotion, approxdif);
209  Motion *nmotion = std::get<0>(res);
210  base::State *dstate = std::get<1>(res);
211  double d = std::get<2>(res);
212 
213  iterations_++;
214  if (dstate != nullptr)
215  {
216  /* create a motion */
217  Motion *motion = createMotion(goal_s, dstate);
218  addEdgeApx(nmotion, motion, d);
219  addEdgeLb(nmotion, motion, d);
220 
221  int k = getK(nn_->size(), k_rrg);
222  std::vector<Motion *> nnVec;
223  nnVec.reserve(k);
224  nn_->nearestK(motion, k, nnVec);
225 
226  BOOST_FOREACH (Motion *neighbor, nnVec)
227  if (neighbor->id_ != motion->id_ && !edgeExistsLb(motion, neighbor))
228  addEdgeLb(motion, neighbor, distanceFunction(motion, neighbor));
229 
230  closeBounds(ptc);
231  }
232 
233  std::list<std::size_t> pathApx;
234  double costApx = LPAstarApx_->computeShortestPath(pathApx);
235  if (bestCost_ > costApx)
236  {
237  OMPL_INFORM("%s: approximation cost = %g", getName().c_str(), costApx);
238  bestCost_ = costApx;
239  }
240  }
241 
242  if (solved)
243  {
244  std::list<std::size_t> pathApx;
245  LPAstarApx_->computeShortestPath(pathApx);
246 
247  /* set the solution path */
248  auto path(std::make_shared<PathGeometric>(si_));
249 
250  // the path is in reverse order
251  for (auto rit = pathApx.rbegin(); rit != pathApx.rend(); ++rit)
252  path->append(idToMotionMap_[*rit]->state_);
253 
254  pdef_->addSolutionPath(path, !solved, 0);
255  }
256 
257  si_->freeState(xstate);
258  if (rmotion->state_)
259  si_->freeState(rmotion->state_);
260  delete rmotion;
261 
262  OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
263 
264  return base::PlannerStatus(solved, !solved);
265 }
266 
267 std::tuple<ompl::geometric::LazyLBTRRT::Motion *, ompl::base::State *, double> ompl::geometric::LazyLBTRRT::rrtExtend(
268  const base::GoalSampleableRegion *goal_s, base::State *xstate, Motion *rmotion, double &approxdif)
269 {
270  base::State *rstate = rmotion->state_;
271  sampleBiased(goal_s, rstate);
272  /* find closest state in the tree */
273  Motion *nmotion = nn_->nearest(rmotion);
274  base::State *dstate = rstate;
275 
276  /* find state to add */
277  double d = distanceFunction(nmotion->state_, rstate);
278  if (d > maxDistance_)
279  {
280  si_->getStateSpace()->interpolate(nmotion->state_, rstate, maxDistance_ / d, xstate);
281  dstate = xstate;
282  d = maxDistance_;
283  }
284 
285  if (checkMotion(nmotion->state_, dstate) == false)
286  return std::make_tuple((Motion *)nullptr, (base::State *)nullptr, 0.0);
287 
288  // motion is valid
289  double dist = 0.0;
290  bool sat = goal_s->isSatisfied(dstate, &dist);
291  if (sat)
292  {
293  approxdif = dist;
294  }
295  if (dist < approxdif)
296  {
297  approxdif = dist;
298  }
299 
300  return std::make_tuple(nmotion, dstate, d);
301 }
302 
303 void ompl::geometric::LazyLBTRRT::rrt(const base::PlannerTerminationCondition &ptc, base::GoalSampleableRegion *goal_s,
304  base::State *xstate, Motion *rmotion, double &approxdif)
305 {
306  while (ptc() == false)
307  {
308  std::tuple<Motion *, base::State *, double> res = rrtExtend(goal_s, xstate, rmotion, approxdif);
309  Motion *nmotion = std::get<0>(res);
310  base::State *dstate = std::get<1>(res);
311  double d = std::get<2>(res);
312 
313  iterations_++;
314  if (dstate != nullptr)
315  {
316  /* create a motion */
317  Motion *motion = createMotion(goal_s, dstate);
318  addEdgeApx(nmotion, motion, d);
319 
320  if (motion == goalMotion_)
321  return;
322  }
323  }
324 }
325 
327 {
328  Planner::getPlannerData(data);
329 
330  if (lastGoalMotion_)
332 
333  for (unsigned int i = 0; i < idToMotionMap_.size(); ++i)
334  {
335  const base::State *parent = idToMotionMap_[i]->state_;
336  if (boost::in_degree(i, graphApx_) == 0)
338  if (boost::out_degree(i, graphApx_) == 0)
340  else
341  {
342  boost::graph_traits<BoostGraph>::out_edge_iterator ei, ei_end;
343  for (boost::tie(ei, ei_end) = boost::out_edges(i, graphApx_); ei != ei_end; ++ei)
344  {
345  std::size_t v = boost::target(*ei, graphApx_);
346  data.addEdge(base::PlannerDataVertex(idToMotionMap_[v]->state_), base::PlannerDataVertex(parent));
347  }
348  }
349  }
350 }
351 
353 {
354  /* sample random state (with goal biasing) */
355  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
356  goal_s->sampleGoal(rstate);
357  else
358  sampler_->sampleUniform(rstate);
359  return;
360 };
361 
362 ompl::geometric::LazyLBTRRT::Motion *ompl::geometric::LazyLBTRRT::createMotion(const base::GoalSampleableRegion *goal_s,
363  const base::State *st)
364 {
365  if (goal_s->isSatisfied(st))
366  return goalMotion_;
367 
368  auto *motion = new Motion(si_);
369  si_->copyState(motion->state_, st);
370  motion->id_ = idToMotionMap_.size();
371  nn_->add(motion);
372  idToMotionMap_.push_back(motion);
373  addVertex(motion);
374 
375  return motion;
376 }
377 
379 ompl::geometric::LazyLBTRRT::createGoalMotion(const base::GoalSampleableRegion *goal_s)
380 {
381  ompl::base::State *gstate = si_->allocState();
382  goal_s->sampleGoal(gstate);
383 
384  auto *motion = new Motion(si_);
385  motion->state_ = gstate;
386  motion->id_ = idToMotionMap_.size();
387  idToMotionMap_.push_back(motion);
388  addVertex(motion);
389 
390  return motion;
391 }
392 
393 void ompl::geometric::LazyLBTRRT::closeBounds(const base::PlannerTerminationCondition &ptc)
394 {
395  std::list<std::size_t> pathApx;
396  double costApx = LPAstarApx_->computeShortestPath(pathApx);
397  std::list<std::size_t> pathLb;
398  double costLb = LPAstarLb_->computeShortestPath(pathLb);
399 
400  while (costApx > (1. + epsilon_) * costLb)
401  {
402  if (ptc())
403  return;
404 
405  auto pathLbIter = pathLb.end();
406  pathLbIter--;
407  std::size_t v = *pathLbIter;
408  pathLbIter--;
409  std::size_t u = *pathLbIter;
410 
411  while (edgeExistsApx(u, v))
412  {
413  v = u;
414  --pathLbIter;
415  u = *pathLbIter;
416  }
417 
418  Motion *motionU = idToMotionMap_[u];
419  Motion *motionV = idToMotionMap_[v];
420  if (checkMotion(motionU, motionV))
421  {
422  // note that we change the direction between u and v due to the diff in definition between Apx and LB
423  addEdgeApx(motionV, motionU,
424  distanceFunction(motionU, motionV)); // the distance here can be obtained from the LB graph
425  pathApx.clear();
426  costApx = LPAstarApx_->computeShortestPath(pathApx);
427  }
428  else // the edge (u,v) was not collision free
429  {
430  removeEdgeLb(motionU, motionV);
431  pathLb.clear();
432  costLb = LPAstarLb_->computeShortestPath(pathLb);
433  }
434  }
435 }
base::State * state_
The state contained by the motion.
Definition: LazyLBTRRT.h:158
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
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:408
base::StateSamplerPtr sampler_
State sampler.
Definition: LazyLBTRRT.h:299
unsigned int iterations_
Number of iterations the algorithm performed.
Definition: LazyLBTRRT.h:331
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
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: LazyLBTRRT.cpp:114
bool isSatisfied(const State *st) const override
Equivalent to calling isSatisfied(const State *, double *) with a nullptr second argument.
Definition: GoalRegion.cpp:47
void setGoalBias(double goalBias)
Set the goal bias.
Definition: LazyLBTRRT.h:81
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:62
double getGoalBias() const
Get the goal bias the planner is using.
Definition: LazyLBTRRT.h:87
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: LazyLBTRRT.h:318
double getRange() const
Get the range the planner is using.
Definition: LazyLBTRRT.h:103
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
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
Representation of a motion.
Definition: LazyLBTRRT.h:140
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:68
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: LazyLBTRRT.h:97
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
#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.
std::size_t id_
The id of the motion.
Definition: LazyLBTRRT.h:155
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...
A shared pointer wrapper for ompl::base::SpaceInformation.
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:49
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
double epsilon_
approximation factor
Definition: LazyLBTRRT.h:315
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:421
LazyLBTRRT(const base::SpaceInformationPtr &si)
Constructor.
Definition: LazyLBTRRT.cpp:51
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
Definition: LazyLBTRRT.h:306
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:427
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:230
void setApproximationFactor(double epsilon)
Set the apprimation factor.
Definition: LazyLBTRRT.h:122
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
void sampleBiased(const base::GoalSampleableRegion *goal_s, base::State *rstate)
sample with goal biasing
Definition: LazyLBTRRT.cpp:352
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:59
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: LazyLBTRRT.cpp:326
double bestCost_
Best cost found so far by algorithm.
Definition: LazyLBTRRT.h:333
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: LazyLBTRRT.h:302
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LazyLBTRRT.cpp:100
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: LazyLBTRRT.h:309
RNG rng_
The random number generator.
Definition: LazyLBTRRT.h:312
double getApproximationFactor() const
Get the apprimation factor.
Definition: LazyLBTRRT.h:293
double distanceFunction(const base::State *a, const base::State *b) const
Compute distance between motions (actually distance between contained states)
Definition: LazyLBTRRT.h:221
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LazyLBTRRT.cpp:85
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: LazyLBTRRT.cpp:130
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68