Loading...
Searching...
No Matches
RRT.cpp
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#include "ompl/control/planners/rrt/RRT.h"
38#include "ompl/base/goals/GoalSampleableRegion.h"
39#include "ompl/tools/config/SelfConfig.h"
40#include <limits>
41
42ompl::control::RRT::RRT(const SpaceInformationPtr &si) : base::Planner(si, "RRT")
43{
45 siC_ = si.get();
46
47 Planner::declareParam<double>("goal_bias", this, &RRT::setGoalBias, &RRT::getGoalBias, "0.:.05:1.");
48 Planner::declareParam<bool>("intermediate_states", this, &RRT::setIntermediateStates, &RRT::getIntermediateStates,
49 "0,1");
50}
51
52ompl::control::RRT::~RRT()
53{
54 freeMemory();
55}
56
58{
60 if (!nn_)
61 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
62 nn_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });
63}
64
66{
67 Planner::clear();
68 sampler_.reset();
69 controlSampler_.reset();
70 freeMemory();
71 if (nn_)
72 nn_->clear();
73 lastGoalMotion_ = nullptr;
74}
75
77{
78 if (nn_)
79 {
80 std::vector<Motion *> motions;
81 nn_->list(motions);
82 for (auto &motion : motions)
83 {
84 if (motion->state)
85 si_->freeState(motion->state);
86 if (motion->control)
87 siC_->freeControl(motion->control);
88 delete motion;
89 }
90 }
91}
92
94{
95 checkValidity();
96 base::Goal *goal = pdef_->getGoal().get();
97 auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
98
99 while (const base::State *st = pis_.nextStart())
100 {
101 auto *motion = new Motion(siC_);
102 si_->copyState(motion->state, st);
103 siC_->nullControl(motion->control);
104 nn_->add(motion);
105 }
106
107 if (nn_->size() == 0)
108 {
109 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
111 }
112
113 if (!sampler_)
114 sampler_ = si_->allocStateSampler();
115 if (!controlSampler_)
116 controlSampler_ = siC_->allocDirectedControlSampler();
117
118 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
119
120 Motion *solution = nullptr;
121 Motion *approxsol = nullptr;
122 double approxdif = std::numeric_limits<double>::infinity();
123
124 auto *rmotion = new Motion(siC_);
125 base::State *rstate = rmotion->state;
126 Control *rctrl = rmotion->control;
127 base::State *xstate = si_->allocState();
128
129 while (ptc == false)
130 {
131 /* sample random state (with goal biasing) */
132 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
133 goal_s->sampleGoal(rstate);
134 else
135 sampler_->sampleUniform(rstate);
136
137 /* find closest state in the tree */
138 Motion *nmotion = nn_->nearest(rmotion);
139
140 /* sample a random control that attempts to go towards the random state, and also sample a control duration */
141 unsigned int cd = controlSampler_->sampleTo(rctrl, nmotion->control, nmotion->state, rmotion->state);
142
143 if (addIntermediateStates_)
144 {
145 // this code is contributed by Jennifer Barry
146 std::vector<base::State *> pstates;
147 cd = siC_->propagateWhileValid(nmotion->state, rctrl, cd, pstates, true);
148
149 if (cd >= siC_->getMinControlDuration())
150 {
151 Motion *lastmotion = nmotion;
152 bool solved = false;
153 size_t p = 0;
154 for (; p < pstates.size(); ++p)
155 {
156 /* create a motion */
157 auto *motion = new Motion();
158 motion->state = pstates[p];
159 // we need multiple copies of rctrl
160 motion->control = siC_->allocControl();
161 siC_->copyControl(motion->control, rctrl);
162 motion->steps = 1;
163 motion->parent = lastmotion;
164 lastmotion = motion;
165 nn_->add(motion);
166 double dist = 0.0;
167 solved = goal->isSatisfied(motion->state, &dist);
168 if (solved)
169 {
170 approxdif = dist;
171 solution = motion;
172 break;
173 }
174 if (dist < approxdif)
175 {
176 approxdif = dist;
177 approxsol = motion;
178 }
179 }
180
181 // free any states after we hit the goal
182 while (++p < pstates.size())
183 si_->freeState(pstates[p]);
184 if (solved)
185 break;
186 }
187 else
188 for (auto &pstate : pstates)
189 si_->freeState(pstate);
190 }
191 else
192 {
193 if (cd >= siC_->getMinControlDuration())
194 {
195 /* create a motion */
196 auto *motion = new Motion(siC_);
197 si_->copyState(motion->state, rmotion->state);
198 siC_->copyControl(motion->control, rctrl);
199 motion->steps = cd;
200 motion->parent = nmotion;
201
202 nn_->add(motion);
203 double dist = 0.0;
204 bool solv = goal->isSatisfied(motion->state, &dist);
205 if (solv)
206 {
207 approxdif = dist;
208 solution = motion;
209 break;
210 }
211 if (dist < approxdif)
212 {
213 approxdif = dist;
214 approxsol = motion;
215 }
216 }
217 }
218 }
219
220 bool solved = false;
221 bool approximate = false;
222 if (solution == nullptr)
223 {
224 solution = approxsol;
225 approximate = true;
226 }
227
228 if (solution != nullptr)
229 {
230 lastGoalMotion_ = solution;
231
232 /* construct the solution path */
233 std::vector<Motion *> mpath;
234 while (solution != nullptr)
235 {
236 mpath.push_back(solution);
237 solution = solution->parent;
238 }
239
240 /* set the solution path */
241 auto path(std::make_shared<PathControl>(si_));
242 for (int i = mpath.size() - 1; i >= 0; --i)
243 if (mpath[i]->parent)
244 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
245 else
246 path->append(mpath[i]->state);
247 solved = true;
248 pdef_->addSolutionPath(path, approximate, approxdif, getName());
249 }
250
251 if (rmotion->state)
252 si_->freeState(rmotion->state);
253 if (rmotion->control)
254 siC_->freeControl(rmotion->control);
255 delete rmotion;
256 si_->freeState(xstate);
257
258 OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
259
260 return {solved, approximate};
261}
262
264{
265 Planner::getPlannerData(data);
266
267 std::vector<Motion *> motions;
268 if (nn_)
269 nn_->list(motions);
270
271 double delta = siC_->getPropagationStepSize();
272
273 if (lastGoalMotion_)
274 data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
275
276 for (auto m : motions)
277 {
278 if (m->parent)
279 {
280 if (data.hasControls())
281 data.addEdge(base::PlannerDataVertex(m->parent->state), base::PlannerDataVertex(m->state),
282 control::PlannerDataEdgeControl(m->control, m->steps * delta));
283 else
284 data.addEdge(base::PlannerDataVertex(m->parent->state), base::PlannerDataVertex(m->state));
285 }
286 else
288 }
289}
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
Definition Goal.h:63
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition Planner.h:429
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition Planner.cpp:92
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
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition PlannerData.h:61
Representation of a motion.
Definition RRT.h:134
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
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
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 setIntermediateStates(bool addIntermediateStates)
Specify whether the intermediate states generated along motions are to be added to the tree itself.
Definition RRT.h:108
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
RRT(const SpaceInformationPtr &si)
Constructor.
Definition RRT.cpp:42
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition RRT.cpp:57
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition Planner.h:202
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.