Loading...
Searching...
No Matches
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_)
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.
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,...
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:403
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition Planner.h:422
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
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
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...
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.
LazyLBTRRT(const base::SpaceInformationPtr &si)
Constructor.
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...
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
#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:205
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition Planner.h:195
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ INVALID_GOAL
Invalid goal state.