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
43namespace
44{
45 int getK(unsigned int n, double k_rrg)
46 {
47 return std::ceil(k_rrg * log((double)(n + 1)));
48 }
49}
50
51ompl::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
72ompl::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
259std::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
295void 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
353ompl::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
370ompl::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
384void 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}
bool isSatisfied(const State *st) const override
Equivalent to calling isSatisfied(const State *, double *) with a nullptr second argument.
Definition: GoalRegion.cpp:47
Abstract definition of a goal region that can be sampled.
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
Abstract definition of goals.
Definition: Goal.h:63
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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...
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...
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...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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:410
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:429
Definition of an abstract state.
Definition: State.h:50
Representation of a motion.
Definition: LazyLBTRRT.h:141
std::size_t id_
The id of the motion.
Definition: LazyLBTRRT.h:153
double getApproximationFactor() const
Get the apprimation factor.
Definition: LazyLBTRRT.h:288
void setApproximationFactor(double epsilon)
Set the apprimation factor.
Definition: LazyLBTRRT.h:122
void sampleBiased(const base::GoalSampleableRegion *goal_s, base::State *rstate)
sample with goal biasing
Definition: LazyLBTRRT.cpp:344
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
double getRange() const
Get the range the planner is using.
Definition: LazyLBTRRT.h:103
void setGoalBias(double goalBias)
Set the goal bias.
Definition: LazyLBTRRT.h:81
double getGoalBias() const
Get the goal bias the planner is using.
Definition: LazyLBTRRT.h:87
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 setRange(double distance)
Set the range the planner is supposed to use.
Definition: LazyLBTRRT.h:97
void freeMemory()
Free the memory allocated by this planner.
Definition: LazyLBTRRT.cpp:106
LazyLBTRRT(const base::SpaceInformationPtr &si)
Constructor.
Definition: LazyLBTRRT.cpp:51
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
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:212
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:202
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
@ INVALID_GOAL
Invalid goal state.
Definition: PlannerStatus.h:58