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...
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:216
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
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()
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