Loading...
Searching...
No Matches
LBTRRT.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, Sertac Karaman, Ioan Sucan, Mark Moll */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_RRT_LBT_RRT_
38#define OMPL_GEOMETRIC_PLANNERS_RRT_LBT_RRT_
39
40#include "ompl/geometric/planners/PlannerIncludes.h"
41#include "ompl/base/OptimizationObjective.h"
42#include "ompl/datastructures/NearestNeighbors.h"
43#include "ompl/datastructures/DynamicSSSP.h"
44
45#include <fstream>
46
47namespace ompl
48{
49 namespace geometric
50 {
68 class LBTRRT : public base::Planner
69 {
70 public:
72 LBTRRT(const base::SpaceInformationPtr &si);
73
74 ~LBTRRT() override;
75
76 void getPlannerData(base::PlannerData &data) const override;
77
79
80 void clear() override;
81
91 void setGoalBias(double goalBias)
92 {
93 goalBias_ = goalBias;
94 }
95
97 double getGoalBias() const
98 {
99 return goalBias_;
100 }
101
107 void setRange(double distance)
108 {
109 maxDistance_ = distance;
110 }
111
113 double getRange() const
114 {
115 return maxDistance_;
116 }
117
119 template <template <typename T> class NN>
121 {
122 if (nn_ && nn_->size() != 0)
123 OMPL_WARN("Calling setNearestNeighbors will clear all states.");
124 clear();
125 nn_ = std::make_shared<NN<Motion *>>();
126 setup();
127 }
128
129 void setup() override;
130
132 void setApproximationFactor(double epsilon)
133 {
134 epsilon_ = epsilon;
135 }
136
139 {
140 return epsilon_;
141 }
142
144 // Planner progress property functions
145 std::string getIterationCount() const
146 {
147 return std::to_string(iterations_);
148 }
149 std::string getBestCost() const
150 {
151 return std::to_string(bestCost_);
152 }
153
154 protected:
159 class Motion
160 {
161 public:
162 Motion() = default;
163
165 Motion(const base::SpaceInformationPtr &si)
166 : state_(si->allocState())
167 {
168 }
169
170 ~Motion() = default;
171
175 std::size_t id_;
181 double costLb_;
185 double costApx_{0.0};
187 std::vector<Motion *> childrenApx_;
188 };
189
192 {
193 IsLessThan(LBTRRT *plannerPtr, Motion *motion) : plannerPtr_(plannerPtr), motion_(motion)
194 {
195 }
196
197 bool operator()(const Motion *motionA, const Motion *motionB)
198 {
199 double costA = motionA->costLb_;
200 double costB = motionB->costLb_;
201
202 double distA = plannerPtr_->distanceFunction(motionA, motion_);
203 double distB = plannerPtr_->distanceFunction(motionB, motion_);
204
205 return costA + distA < costB + distB;
206 }
207 LBTRRT *plannerPtr_;
208 Motion *motion_;
209 }; // IsLessThan
210
213 {
214 IsLessThanLB(LBTRRT *plannerPtr) : plannerPtr_(plannerPtr)
215 {
216 }
217
218 bool operator()(const Motion *motionA, const Motion *motionB) const
219 {
220 return motionA->costLb_ < motionB->costLb_;
221 }
222 LBTRRT *plannerPtr_;
223 }; // IsLessThanLB
224
226 void considerEdge(Motion *parent, Motion *child, double c);
227
229 double lazilyUpdateApxParent(Motion *child, Motion *parent);
230
232 void updateChildCostsApx(Motion *m, double delta);
233
236
238 void freeMemory();
239
241 double distanceFunction(const Motion *a, const Motion *b) const
242 {
243 return si_->distance(a->state_, b->state_);
244 }
245
247 bool checkMotion(const Motion *a, const Motion *b)
248 {
249 return checkMotion(a->state_, b->state_);
250 }
252 bool checkMotion(const base::State *a, const base::State *b)
253 {
254 return si_->checkMotion(a, b);
255 }
256
258 Motion *getMotion(std::size_t i)
259 {
260 return idToMotionMap_[i];
261 }
262
264 base::StateSamplerPtr sampler_;
265
267 std::shared_ptr<NearestNeighbors<Motion *>> nn_;
268
271
273 std::vector<Motion *> idToMotionMap_;
274
277 double goalBias_{.05};
278
280 double maxDistance_{0.};
281
283 double epsilon_{.4};
284
287
290
292 // Planner progress properties
294 unsigned int iterations_{0u};
296 double bestCost_;
297 };
298 }
299}
300
301#endif // OMPL_GEOMETRIC_PLANNERS_RRT_LBT_RRT_
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
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: LBTRRT.h:160
std::size_t id_
unique id of the motion
Definition: LBTRRT.h:175
double costLb_
The lower bound cost of the motion while it is stored in the lowerBoundGraph_ and this may seem redun...
Definition: LBTRRT.h:181
base::State * state_
The state contained by the motion.
Definition: LBTRRT.h:173
Motion * parentApx_
The parent motion in the approximation tree.
Definition: LBTRRT.h:183
double costApx_
The approximation cost.
Definition: LBTRRT.h:185
std::vector< Motion * > childrenApx_
The children in the approximation tree.
Definition: LBTRRT.h:187
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition: LBTRRT.h:165
Lower Bound Tree Rapidly-exploring Random Trees.
Definition: LBTRRT.h:69
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LBTRRT.cpp:84
double getGoalBias() const
Get the goal bias the planner is using.
Definition: LBTRRT.h:97
void removeFromParentApx(Motion *m)
remove motion from its parent in the approximation tree
Definition: LBTRRT.cpp:426
DynamicSSSP lowerBoundGraph_
A graph of motions Glb.
Definition: LBTRRT.h:270
void setApproximationFactor(double epsilon)
Set the apprimation factor.
Definition: LBTRRT.h:132
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: LBTRRT.h:241
double getRange() const
Get the range the planner is using.
Definition: LBTRRT.h:113
unsigned int iterations_
Number of iterations the algorithm performed.
Definition: LBTRRT.h:294
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: LBTRRT.h:267
double lazilyUpdateApxParent(Motion *child, Motion *parent)
lazily update the parent in the approximation tree without updating costs to cildren
Definition: LBTRRT.cpp:414
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: LBTRRT.h:107
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: LBTRRT.cpp:385
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: LBTRRT.h:289
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
Definition: LBTRRT.h:277
double epsilon_
approximation factor
Definition: LBTRRT.h:283
bool checkMotion(const base::State *a, const base::State *b)
local planner
Definition: LBTRRT.h:252
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: LBTRRT.cpp:112
RNG rng_
The random number generator.
Definition: LBTRRT.h:286
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LBTRRT.cpp:70
void freeMemory()
Free the memory allocated by this planner.
Definition: LBTRRT.cpp:98
double bestCost_
Best cost found so far by algorithm.
Definition: LBTRRT.h:296
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: LBTRRT.h:280
double getApproximationFactor() const
Get the apprimation factor.
Definition: LBTRRT.h:138
void considerEdge(Motion *parent, Motion *child, double c)
consider an edge for addition to the roadmap
Definition: LBTRRT.cpp:307
void setGoalBias(double goalBias)
Set the goal bias.
Definition: LBTRRT.h:91
void updateChildCostsApx(Motion *m, double delta)
update the child cost of the approximation tree
Definition: LBTRRT.cpp:405
bool checkMotion(const Motion *a, const Motion *b)
local planner
Definition: LBTRRT.h:247
Motion * getMotion(std::size_t i)
get motion from id
Definition: LBTRRT.h:258
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: LBTRRT.h:120
base::StateSamplerPtr sampler_
State sampler.
Definition: LBTRRT.h:264
std::vector< Motion * > idToMotionMap_
mapping between a motion id and the motion
Definition: LBTRRT.h:273
#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
comparator - metric is the lower bound cost
Definition: LBTRRT.h:213
comparator - metric is the cost to reach state via a specific state
Definition: LBTRRT.h:192