Loading...
Searching...
No Matches
LazyRRT.h
1
2/*********************************************************************
3* Software License Agreement (BSD License)
4*
5* Copyright (c) 2008, Willow Garage, Inc.
6* All rights reserved.
7*
8* Redistribution and use in source and binary forms, with or without
9* modification, are permitted provided that the following conditions
10* are met:
11*
12* * Redistributions of source code must retain the above copyright
13* notice, this list of conditions and the following disclaimer.
14* * Redistributions in binary form must reproduce the above
15* copyright notice, this list of conditions and the following
16* disclaimer in the documentation and/or other materials provided
17* with the distribution.
18* * Neither the name of the Willow Garage nor the names of its
19* contributors may be used to endorse or promote products derived
20* from this software without specific prior written permission.
21*
22* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33* POSSIBILITY OF SUCH DAMAGE.
34*********************************************************************/
35
36/* Author: Ioan Sucan */
37
38#ifndef OMPL_GEOMETRIC_PLANNERS_RRT_LAZY_RRT_
39#define OMPL_GEOMETRIC_PLANNERS_RRT_LAZY_RRT_
40
41#include "ompl/geometric/planners/PlannerIncludes.h"
42#include "ompl/datastructures/NearestNeighbors.h"
43#include <vector>
44
45namespace ompl
46{
47 namespace geometric
48 {
81 class LazyRRT : public base::Planner
82 {
83 public:
85 LazyRRT(const base::SpaceInformationPtr &si);
86
87 ~LazyRRT() override;
88
89 void getPlannerData(base::PlannerData &data) const override;
90
92
93 void clear() override;
94
104 void setGoalBias(double goalBias)
105 {
106 goalBias_ = goalBias;
107 }
108
110 double getGoalBias() const
111 {
112 return goalBias_;
113 }
114
120 void setRange(double distance)
121 {
122 maxDistance_ = distance;
123 }
124
126 double getRange() const
127 {
128 return maxDistance_;
129 }
130
132 template <template <typename T> class NN>
134 {
135 if (nn_ && nn_->size() != 0)
136 OMPL_WARN("Calling setNearestNeighbors will clear all states.");
137 clear();
138 nn_ = std::make_shared<NN<Motion *>>();
139 setup();
140 }
141
142 void setup() override;
143
144 protected:
146 class Motion
147 {
148 public:
149 Motion() = default;
150
152 Motion(const base::SpaceInformationPtr &si) : state(si->allocState())
153 {
154 }
155
156 ~Motion() = default;
157
160
162 Motion *parent{nullptr};
163
165 bool valid{false};
166
168 std::vector<Motion *> children;
169 };
170
172 void freeMemory();
173
175 void removeMotion(Motion *motion);
176
178 double distanceFunction(const Motion *a, const Motion *b) const
179 {
180 return si_->distance(a->state, b->state);
181 }
182
184 base::StateSamplerPtr sampler_;
185
187 std::shared_ptr<NearestNeighbors<Motion *>> nn_;
188
191 double goalBias_{.05};
192
194 double maxDistance_{0.};
195
198
201 };
202 }
203}
204
205#endif
virtual std::size_t size() const =0
Get the number of elements in the datastructure.
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: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 LazyRRT.h:147
std::vector< Motion * > children
The set of motions that descend from this one.
Definition LazyRRT.h:168
Motion * parent
The parent motion in the exploration tree.
Definition LazyRRT.h:162
base::State * state
The state contained by the motion.
Definition LazyRRT.h:159
bool valid
Flag indicating whether this motion has been validated.
Definition LazyRRT.h:165
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition LazyRRT.h:152
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
Definition LazyRRT.h:191
void setGoalBias(double goalBias)
Set the goal biasing.
Definition LazyRRT.h:104
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition LazyRRT.h:133
LazyRRT(const base::SpaceInformationPtr &si)
Constructor.
Definition LazyRRT.cpp:42
void setRange(double distance)
Set the range the planner is supposed to use.
Definition LazyRRT.h:120
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition LazyRRT.h:200
void removeMotion(Motion *motion)
Remove a motion from the tree datastructure.
Definition LazyRRT.cpp:207
double getGoalBias() const
Get the goal bias the planner is using.
Definition LazyRRT.h:110
base::StateSamplerPtr sampler_
State sampler.
Definition LazyRRT.h:184
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition LazyRRT.h:194
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 LazyRRT.cpp:235
double getRange() const
Get the range the planner is using.
Definition LazyRRT.h:126
RNG rng_
The random number generator.
Definition LazyRRT.h:197
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition LazyRRT.h:178
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition LazyRRT.h:187
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 LazyRRT.cpp:94
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition LazyRRT.cpp:55
void freeMemory()
Free the memory allocated by this planner.
Definition LazyRRT.cpp:79
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition LazyRRT.cpp:69
#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()