Loading...
Searching...
No Matches
RRT.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_RRT_RRT_
38#define OMPL_GEOMETRIC_PLANNERS_RRT_RRT_
39
40#include "ompl/datastructures/NearestNeighbors.h"
41#include "ompl/geometric/planners/PlannerIncludes.h"
42
43namespace ompl
44{
45 namespace geometric
46 {
65 class RRT : public base::Planner
66 {
67 public:
69 RRT(const base::SpaceInformationPtr &si, bool addIntermediateStates = false);
70
71 ~RRT() override;
72
73 void getPlannerData(base::PlannerData &data) const override;
74
76
77 void clear() override;
78
88 void setGoalBias(double goalBias)
89 {
90 goalBias_ = goalBias;
91 }
92
94 double getGoalBias() const
95 {
96 return goalBias_;
97 }
98
102 {
104 }
105
108 void setIntermediateStates(bool addIntermediateStates)
109 {
110 addIntermediateStates_ = addIntermediateStates;
111 }
112
118 void setRange(double distance)
119 {
120 maxDistance_ = distance;
121 }
122
124 double getRange() const
125 {
126 return maxDistance_;
127 }
128
130 template <template <typename T> class NN>
132 {
133 if (nn_ && nn_->size() != 0)
134 OMPL_WARN("Calling setNearestNeighbors will clear all states.");
135 clear();
136 nn_ = std::make_shared<NN<Motion *>>();
137 setup();
138 }
139
140 void setup() override;
141
142 protected:
147 class Motion
148 {
149 public:
150 Motion() = default;
151
153 Motion(const base::SpaceInformationPtr &si) : state(si->allocState())
154 {
155 }
156
157 ~Motion() = default;
158
161
163 Motion *parent{nullptr};
164 };
165
167 void freeMemory();
168
170 double distanceFunction(const Motion *a, const Motion *b) const
171 {
172 return si_->distance(a->state, b->state);
173 }
174
176 base::StateSamplerPtr sampler_;
177
179 std::shared_ptr<NearestNeighbors<Motion *>> nn_;
180
183 double goalBias_{.05};
184
186 double maxDistance_{0.};
187
190
193
196 };
197 }
198}
199
200#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 RRT.h:148
Motion * parent
The parent motion in the exploration tree.
Definition RRT.h:163
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition RRT.h:153
base::State * state
The state contained by the motion.
Definition RRT.h:160
Rapidly-exploring Random Trees.
Definition RRT.h:66
RNG rng_
The random number generator.
Definition RRT.h:192
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 RRT.cpp:97
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition RRT.cpp:71
bool addIntermediateStates_
Flag indicating whether intermediate states are added to the built tree of motions.
Definition RRT.h:189
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition RRT.h:131
void setGoalBias(double goalBias)
Set the goal bias.
Definition RRT.h:88
base::StateSamplerPtr sampler_
State sampler.
Definition RRT.h:176
void setIntermediateStates(bool addIntermediateStates)
Specify whether the intermediate states generated along motions are to be added to the tree itself.
Definition RRT.h:108
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition RRT.h:170
double getGoalBias() const
Get the goal bias the planner is using.
Definition RRT.h:94
bool getIntermediateStates() const
Return true if the intermediate states generated along motions are to be added to the tree itself.
Definition RRT.h:101
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition RRT.cpp:61
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 RRT.cpp:232
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition RRT.h:186
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition RRT.h:195
void setRange(double distance)
Set the range the planner is supposed to use.
Definition RRT.h:118
void freeMemory()
Free the memory allocated by this planner.
Definition RRT.cpp:82
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
Definition RRT.h:183
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition RRT.h:179
double getRange() const
Get the range the planner is using.
Definition RRT.h:124
#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()