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_CONTROL_PLANNERS_RRT_RRT_
38#define OMPL_CONTROL_PLANNERS_RRT_RRT_
39
40#include "ompl/control/planners/PlannerIncludes.h"
41#include "ompl/datastructures/NearestNeighbors.h"
42
43namespace ompl
44{
45 namespace control
46 {
65 class RRT : public base::Planner
66 {
67 public:
69 RRT(const SpaceInformationPtr &si);
70
71 ~RRT() override;
72
75
79 void clear() override;
80
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
113 void getPlannerData(base::PlannerData &data) const override;
114
116 template <template <typename T> class NN>
118 {
119 if (nn_ && nn_->size() != 0)
120 OMPL_WARN("Calling setNearestNeighbors will clear all states.");
121 clear();
122 nn_ = std::make_shared<NN<Motion *>>();
123 setup();
124 }
125
126 void setup() override;
127
128 protected:
133 class Motion
134 {
135 public:
136 Motion() = default;
137
140 : state(si->allocState()), control(si->allocControl())
141 {
142 }
143
144 ~Motion() = default;
145
148
150 Control *control{nullptr};
151
153 unsigned int steps{0};
154
156 Motion *parent{nullptr};
157 };
158
160 void freeMemory();
161
163 double distanceFunction(const Motion *a, const Motion *b) const
164 {
165 return si_->distance(a->state, b->state);
166 }
167
169 base::StateSamplerPtr sampler_;
170
173
176
178 std::shared_ptr<NearestNeighbors<Motion *>> nn_;
179
182 double goalBias_{0.05};
183
186
189
192 };
193 }
194}
195
196#endif
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
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition State.h:50
Definition of an abstract control.
Definition Control.h:48
A shared pointer wrapper for ompl::control::DirectedControlSampler.
Representation of a motion.
Definition RRT.h:134
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
Definition RRT.h:139
unsigned int steps
The number of steps the control is applied for.
Definition RRT.h:153
base::State * state
The state contained by the motion.
Definition RRT.h:147
Control * control
The control contained by the motion.
Definition RRT.h:150
Motion * parent
The parent motion in the exploration tree.
Definition RRT.h:156
Rapidly-exploring Random Tree.
Definition RRT.h:66
RNG rng_
The random number generator.
Definition RRT.h:188
void setGoalBias(double goalBias)
Definition RRT.h:88
bool getIntermediateStates() const
Return true if the intermediate states generated along motions are to be added to the tree itself.
Definition RRT.h:101
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:182
void freeMemory()
Free the memory allocated by this planner.
Definition RRT.cpp:76
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition RRT.h:175
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition RRT.h:117
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:163
bool addIntermediateStates_
Flag indicating whether intermediate states are added to the built tree of motions.
Definition RRT.h:185
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continue solving for some amount of time. Return true if solution was found.
Definition RRT.cpp:93
double getGoalBias() const
Get the goal bias the planner is using.
Definition RRT.h:94
void clear() override
Clear datastructures. Call this function if the input data to the planner has changed and you do not ...
Definition RRT.cpp:65
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:263
base::StateSamplerPtr sampler_
State sampler.
Definition RRT.h:169
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition RRT.h:191
DirectedControlSamplerPtr controlSampler_
Control sampler.
Definition RRT.h:172
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition RRT.cpp:57
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition RRT.h:178
Space information containing necessary information for planning with controls. setup() needs to be ca...
#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()