LazyLBTRRT.h
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#ifndef OMPL_CONTRIB_LAZY_LBTRRT_
38#define OMPL_CONTRIB_LAZY_LBTRRT_
39
40#include "ompl/geometric/planners/PlannerIncludes.h"
41#include "ompl/datastructures/NearestNeighbors.h"
42#include "ompl/base/goals/GoalSampleableRegion.h"
43#include "ompl/datastructures/LPAstarOnGraph.h"
44
45#include <fstream>
46#include <vector>
47#include <tuple>
48#include <cassert>
49
50#include <boost/graph/graph_traits.hpp>
51#include <boost/graph/adjacency_list.hpp>
52
53namespace ompl
54{
55 namespace geometric
56 {
59 {
60 public:
62 LazyLBTRRT(const base::SpaceInformationPtr &si);
63
64 ~LazyLBTRRT() override;
65
66 void getPlannerData(base::PlannerData &data) const override;
67
69
70 void clear() override;
71
81 void setGoalBias(double goalBias)
82 {
83 goalBias_ = goalBias;
84 }
85
87 double getGoalBias() const
88 {
89 return goalBias_;
90 }
91
97 void setRange(double distance)
98 {
99 maxDistance_ = distance;
100 }
101
103 double getRange() const
104 {
105 return maxDistance_;
106 }
107
109 template <template <typename T> class NN>
111 {
112 if (nn_ && nn_->size() != 0)
113 OMPL_WARN("Calling setNearestNeighbors will clear all states.");
114 clear();
115 nn_ = std::make_shared<NN<Motion *>>();
116 setup();
117 }
118
119 void setup() override;
120
122 void setApproximationFactor(double epsilon)
123 {
124 epsilon_ = epsilon;
125 }
126
128 // Planner progress property functions
129 std::string getIterationCount() const
130 {
131 return std::to_string(iterations_);
132 }
133 std::string getBestCost() const
134 {
135 return std::to_string(bestCost_);
136 }
137
138 protected:
140 class Motion
141 {
142 public:
143 Motion() = default;
144
146 Motion(const base::SpaceInformationPtr &si) : state_(si->allocState())
147 {
148 }
149
150 ~Motion() = default;
151
153 std::size_t id_;
154
157 };
158
159 using WeightProperty = boost::property<boost::edge_weight_t, double>;
160 using BoostGraph = boost::adjacency_list<boost::vecS, // container type for the out edge list
161 boost::vecS, // container type for the vertex list
162 boost::undirectedS, // directedS / undirectedS / bidirectionalS.
163 std::size_t, // vertex properties
164 WeightProperty // edge properties
165 >;
166
167 friend class CostEstimatorApx; // allow CostEstimatorApx access to private members
169 {
170 public:
171 CostEstimatorApx(LazyLBTRRT *alg) : alg_(alg)
172 {
173 }
174 double operator()(std::size_t i)
175 {
176 double lb_estimate = (*(alg_->LPAstarLb_))(i);
177 if (lb_estimate != std::numeric_limits<double>::infinity())
178 return lb_estimate;
179
180 return alg_->distanceFunction(alg_->idToMotionMap_[i], alg_->startMotion_);
181 }
182
183 private:
184 LazyLBTRRT *alg_;
185 }; // CostEstimatorApx
186
188 {
189 public:
190 CostEstimatorLb(base::Goal *goal, std::vector<Motion *> &idToMotionMap)
191 : goal_(goal), idToMotionMap_(idToMotionMap)
192 {
193 }
194 double operator()(std::size_t i)
195 {
196 double dist = 0.0;
197 goal_->isSatisfied(idToMotionMap_[i]->state_, &dist);
198
199 return dist;
200 }
201
202 private:
203 base::Goal *goal_;
204 std::vector<Motion *> &idToMotionMap_;
205 }; // CostEstimatorLb
206
209
211 void sampleBiased(const base::GoalSampleableRegion *goal_s, base::State *rstate);
212
214 void freeMemory();
215
217 double distanceFunction(const base::State *a, const base::State *b) const
218 {
219 return si_->distance(a, b);
220 }
221 double distanceFunction(const Motion *a, const Motion *b) const
222 {
223 return si_->distance(a->state_, b->state_);
224 }
225 bool checkMotion(const base::State *a, const base::State *b) const
226 {
227 return si_->checkMotion(a, b);
228 }
229 bool checkMotion(const Motion *a, const Motion *b) const
230 {
231 return si_->checkMotion(a->state_, b->state_);
232 }
233
234 Motion *getMotion(std::size_t id) const
235 {
236 assert(idToMotionMap_.size() > id);
237 return idToMotionMap_[id];
238 }
239 void addVertex(const Motion *a)
240 {
241 boost::add_vertex(a->id_, graphApx_);
242 boost::add_vertex(a->id_, graphLb_);
243 }
244
245 void addEdgeApx(Motion *a, Motion *b, double c)
246 {
247 WeightProperty w(c);
248 boost::add_edge(a->id_, b->id_, w, graphApx_);
249 LPAstarApx_->insertEdge(a->id_, b->id_, c);
250 LPAstarApx_->insertEdge(b->id_, a->id_, c);
251 }
252 void addEdgeLb(const Motion *a, const Motion *b, double c)
253 {
254 WeightProperty w(c);
255 boost::add_edge(a->id_, b->id_, w, graphLb_);
256 LPAstarLb_->insertEdge(a->id_, b->id_, c);
257 LPAstarLb_->insertEdge(b->id_, a->id_, c);
258 }
259 bool edgeExistsApx(std::size_t a, std::size_t b)
260 {
261 return boost::edge(a, b, graphApx_).second;
262 }
263 bool edgeExistsApx(const Motion *a, const Motion *b)
264 {
265 return edgeExistsApx(a->id_, b->id_);
266 }
267 bool edgeExistsLb(const Motion *a, const Motion *b)
268 {
269 return boost::edge(a->id_, b->id_, graphLb_).second;
270 }
271 void removeEdgeLb(const Motion *a, const Motion *b)
272 {
273 boost::remove_edge(a->id_, b->id_, graphLb_);
274 LPAstarLb_->removeEdge(a->id_, b->id_);
275 LPAstarLb_->removeEdge(b->id_, a->id_);
276 }
277 std::tuple<Motion *, base::State *, double> rrtExtend(const base::GoalSampleableRegion *goal_s,
278 base::State *xstate, Motion *rmotion,
279 double &approxdif);
280 void rrt(const base::PlannerTerminationCondition &ptc, base::GoalSampleableRegion *goal_s,
281 base::State *xstate, Motion *rmotion, double &approxdif);
282 Motion *createMotion(const base::GoalSampleableRegion *goal_s, const base::State *st);
283 Motion *createGoalMotion(const base::GoalSampleableRegion *goal_s);
284
285 void closeBounds(const base::PlannerTerminationCondition &ptc);
286
289 {
290 return epsilon_;
291 }
292
294 base::StateSamplerPtr sampler_;
295
297 std::shared_ptr<NearestNeighbors<Motion *>> nn_;
298
301 double goalBias_{0.05};
302
304 double maxDistance_{0.};
305
308
310 double epsilon_{.4};
311
314
315 BoostGraph graphLb_;
316 BoostGraph graphApx_;
317 Motion *startMotion_;
318 Motion *goalMotion_{nullptr}; // root of LPAstarApx_
319 LPAstarApx *LPAstarApx_{nullptr}; // rooted at target
320 LPAstarLb *LPAstarLb_{nullptr}; // rooted at source
321 std::vector<Motion *> idToMotionMap_;
322
324 // Planner progress properties
326 unsigned int iterations_{0};
328 double bestCost_;
329 };
330 }
331}
332
333#endif // OMPL_CONTRIB_LAZY_LBTRRT_
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
Definition: Goal.h:63
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
Definition of an abstract state.
Definition: State.h:50
Representation of a motion.
Definition: LazyLBTRRT.h:141
base::State * state_
The state contained by the motion.
Definition: LazyLBTRRT.h:156
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition: LazyLBTRRT.h:146
std::size_t id_
The id of the motion.
Definition: LazyLBTRRT.h:153
Rapidly-exploring Random Trees.
Definition: LazyLBTRRT.h:59
RNG rng_
The random number generator.
Definition: LazyLBTRRT.h:307
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
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:301
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LazyLBTRRT.cpp:92
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: LazyLBTRRT.h:313
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LazyLBTRRT.cpp:77
base::StateSamplerPtr sampler_
State sampler.
Definition: LazyLBTRRT.h:294
double distanceFunction(const base::State *a, const base::State *b) const
Compute distance between motions (actually distance between contained states)
Definition: LazyLBTRRT.h:217
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 maxDistance_
The maximum length of a motion to be added to a tree.
Definition: LazyLBTRRT.h:304
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: LazyLBTRRT.h:110
double getGoalBias() const
Get the goal bias the planner is using.
Definition: LazyLBTRRT.h:87
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: LazyLBTRRT.h:297
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
unsigned int iterations_
Number of iterations the algorithm performed.
Definition: LazyLBTRRT.h:326
double epsilon_
approximation factor
Definition: LazyLBTRRT.h:310
double bestCost_
Best cost found so far by algorithm.
Definition: LazyLBTRRT.h:328
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
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49