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 {
getK(unsigned int n,double k_rrg)45 int getK(unsigned int n, double k_rrg)
46 {
47 return std::ceil(k_rrg * log((double)(n + 1)));
48 }
49 }
50
LazyLBTRRT(const base::SpaceInformationPtr & si)51 ompl::geometric::LazyLBTRRT::LazyLBTRRT(const base::SpaceInformationPtr &si)
52 : base::Planner(si, "LazyLBTRRT")
53 {
54 specs_.approximateSolutions = true;
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,
60 &LazyLBTRRT::getApproximationFactor, "0.:.1:10.");
61
62 addPlannerProgressProperty("iterations INTEGER", [this]
63 {
64 return getIterationCount();
65 });
66 addPlannerProgressProperty("best cost REAL", [this]
67 {
68 return getBestCost();
69 });
70 }
71
~LazyLBTRRT()72 ompl::geometric::LazyLBTRRT::~LazyLBTRRT()
73 {
74 freeMemory();
75 }
76
clear()77 void ompl::geometric::LazyLBTRRT::clear()
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
setup()92 void ompl::geometric::LazyLBTRRT::setup()
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
freeMemory()106 void ompl::geometric::LazyLBTRRT::freeMemory()
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
solve(const base::PlannerTerminationCondition & ptc)122 ompl::base::PlannerStatus ompl::geometric::LazyLBTRRT::solve(const base::PlannerTerminationCondition &ptc)
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());
132 return base::PlannerStatus::INVALID_GOAL;
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());
144 return base::PlannerStatus::INVALID_START;
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
168 ////////////////////////////////////////////
169 // step (1) - RRT
170 ////////////////////////////////////////////
171 bestCost_ = std::numeric_limits<double>::infinity();
172 rrt(ptc, goal_s, xstate, rmotion, approxdif);
173 if (!ptc())
174 {
175 solved = true;
176
177 ////////////////////////////////////////////
178 // step (2) - Lazy construction of G_lb
179 ////////////////////////////////////////////
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
195 ////////////////////////////////////////////
196 // step (3) - anytime planning
197 ////////////////////////////////////////////
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
rrtExtend(const base::GoalSampleableRegion * goal_s,base::State * xstate,Motion * rmotion,double & approxdif)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
rrt(const base::PlannerTerminationCondition & ptc,base::GoalSampleableRegion * goal_s,base::State * xstate,Motion * rmotion,double & approxdif)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
getPlannerData(base::PlannerData & data) const318 void ompl::geometric::LazyLBTRRT::getPlannerData(base::PlannerData &data) const
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)
329 data.addGoalVertex(base::PlannerDataVertex(parent));
330 if (boost::out_degree(i, graphApx_) == 0)
331 data.addStartVertex(base::PlannerDataVertex(parent));
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
sampleBiased(const base::GoalSampleableRegion * goal_s,base::State * rstate)344 void ompl::geometric::LazyLBTRRT::sampleBiased(const base::GoalSampleableRegion *goal_s, base::State *rstate)
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
createMotion(const base::GoalSampleableRegion * goal_s,const base::State * st)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
369 ompl::geometric::LazyLBTRRT::Motion *
createGoalMotion(const base::GoalSampleableRegion * goal_s)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
closeBounds(const base::PlannerTerminationCondition & ptc)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 }
427