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