Loading...
Searching...
No Matches
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...
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,...
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
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...
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...
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...
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.
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
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...
#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()