Loading...
Searching...
No Matches
RRTConnect.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/geometric/planners/rrt/RRTConnect.h"
38#include "ompl/base/goals/GoalSampleableRegion.h"
39#include "ompl/tools/config/SelfConfig.h"
40#include "ompl/util/String.h"
41
42ompl::geometric::RRTConnect::RRTConnect(const base::SpaceInformationPtr &si, bool addIntermediateStates)
43 : base::Planner(si, addIntermediateStates ? "RRTConnectIntermediate" : "RRTConnect")
44{
46 specs_.directed = true;
47
48 Planner::declareParam<double>("range", this, &RRTConnect::setRange, &RRTConnect::getRange, "0.:1.:10000.");
49 Planner::declareParam<bool>("intermediate_states", this, &RRTConnect::setIntermediateStates,
51
52 connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
53 distanceBetweenTrees_ = std::numeric_limits<double>::infinity();
54 addIntermediateStates_ = addIntermediateStates;
55}
56
57ompl::geometric::RRTConnect::~RRTConnect()
58{
59 freeMemory();
60}
61
63{
64 Planner::setup();
65 tools::SelfConfig sc(si_, getName());
66 sc.configurePlannerRange(maxDistance_);
67
68 if (!tStart_)
69 tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
70 if (!tGoal_)
71 tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
72 tStart_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });
73 tGoal_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });
74}
75
77{
78 std::vector<Motion *> motions;
79
80 if (tStart_)
81 {
82 tStart_->list(motions);
83 for (auto &motion : motions)
84 {
85 if (motion->state != nullptr)
86 si_->freeState(motion->state);
87 delete motion;
88 }
89 }
90
91 if (tGoal_)
92 {
93 tGoal_->list(motions);
94 for (auto &motion : motions)
95 {
96 if (motion->state != nullptr)
97 si_->freeState(motion->state);
98 delete motion;
99 }
100 }
101}
102
104{
105 Planner::clear();
106 sampler_.reset();
107 freeMemory();
108 if (tStart_)
109 tStart_->clear();
110 if (tGoal_)
111 tGoal_->clear();
112 connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
113 distanceBetweenTrees_ = std::numeric_limits<double>::infinity();
114}
115
117 Motion *rmotion)
118{
119 /* find closest state in the tree */
120 Motion *nmotion = tree->nearest(rmotion);
121
122 /* assume we can reach the state we go towards */
123 bool reach = true;
124
125 /* find state to add */
126 base::State *dstate = rmotion->state;
127 double d = si_->distance(nmotion->state, rmotion->state);
128 if (d > maxDistance_)
129 {
130 si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
131
132 /* Check if we have moved at all. Due to some stranger state spaces (e.g., the constrained state spaces),
133 * interpolate can fail and no progress is made. Without this check, the algorithm gets stuck in a loop as it
134 * thinks it is making progress, when none is actually occurring. */
135 if (si_->equalStates(nmotion->state, tgi.xstate))
136 return TRAPPED;
137
138 dstate = tgi.xstate;
139 reach = false;
140 }
141
142 bool validMotion = tgi.start ? si_->checkMotion(nmotion->state, dstate) :
143 si_->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
144
145 if (!validMotion)
146 return TRAPPED;
147
148 if (addIntermediateStates_)
149 {
150 const base::State *astate = tgi.start ? nmotion->state : dstate;
151 const base::State *bstate = tgi.start ? dstate : nmotion->state;
152
153 std::vector<base::State *> states;
154 const unsigned int count = si_->getStateSpace()->validSegmentCount(astate, bstate);
155
156 if (si_->getMotionStates(astate, bstate, states, count, true, true))
157 si_->freeState(states[0]);
158
159 for (std::size_t i = 1; i < states.size(); ++i)
160 {
161 auto *motion = new Motion;
162 motion->state = states[i];
163 motion->parent = nmotion;
164 motion->root = nmotion->root;
165 tree->add(motion);
166
167 nmotion = motion;
168 }
169
170 tgi.xmotion = nmotion;
171 }
172 else
173 {
174 auto *motion = new Motion(si_);
175 si_->copyState(motion->state, dstate);
176 motion->parent = nmotion;
177 motion->root = nmotion->root;
178 tree->add(motion);
179
180 tgi.xmotion = motion;
181 }
182
183 return reach ? REACHED : ADVANCED;
184}
185
187{
188 checkValidity();
189 auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
190
191 if (goal == nullptr)
192 {
193 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
195 }
196
197 while (const base::State *st = pis_.nextStart())
198 {
199 auto *motion = new Motion(si_);
200 si_->copyState(motion->state, st);
201 motion->root = motion->state;
202 tStart_->add(motion);
203 }
204
205 if (tStart_->size() == 0)
206 {
207 OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
209 }
210
211 if (!goal->couldSample())
212 {
213 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
215 }
216
217 if (!sampler_)
218 sampler_ = si_->allocStateSampler();
219
220 OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
221 (int)(tStart_->size() + tGoal_->size()));
222
223 TreeGrowingInfo tgi;
224 tgi.xstate = si_->allocState();
225
226 Motion *approxsol = nullptr;
227 double approxdif = std::numeric_limits<double>::infinity();
228 auto *rmotion = new Motion(si_);
229 base::State *rstate = rmotion->state;
230 bool startTree = true;
231 bool solved = false;
232
233 while (!ptc)
234 {
235 TreeData &tree = startTree ? tStart_ : tGoal_;
236 tgi.start = startTree;
237 startTree = !startTree;
238 TreeData &otherTree = startTree ? tStart_ : tGoal_;
239
240 if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
241 {
242 const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
243 if (st != nullptr)
244 {
245 auto *motion = new Motion(si_);
246 si_->copyState(motion->state, st);
247 motion->root = motion->state;
248 tGoal_->add(motion);
249 }
250
251 if (tGoal_->size() == 0)
252 {
253 OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
254 break;
255 }
256 }
257
258 /* sample random state */
259 sampler_->sampleUniform(rstate);
260
261 GrowState gs = growTree(tree, tgi, rmotion);
262
263 if (gs != TRAPPED)
264 {
265 /* remember which motion was just added */
266 Motion *addedMotion = tgi.xmotion;
267
268 /* attempt to connect trees */
269
270 /* if reached, it means we used rstate directly, no need to copy again */
271 if (gs != REACHED)
272 si_->copyState(rstate, tgi.xstate);
273
274 GrowState gsc = ADVANCED;
275 tgi.start = startTree;
276 while (gsc == ADVANCED)
277 gsc = growTree(otherTree, tgi, rmotion);
278
279 /* update distance between trees */
280 const double newDist = tree->getDistanceFunction()(addedMotion, otherTree->nearest(addedMotion));
281 if (newDist < distanceBetweenTrees_)
282 {
283 distanceBetweenTrees_ = newDist;
284 // OMPL_INFORM("Estimated distance to go: %f", distanceBetweenTrees_);
285 }
286
287 Motion *startMotion = startTree ? tgi.xmotion : addedMotion;
288 Motion *goalMotion = startTree ? addedMotion : tgi.xmotion;
289
290 /* if we connected the trees in a valid way (start and goal pair is valid)*/
291 if (gsc == REACHED && goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
292 {
293 // it must be the case that either the start tree or the goal tree has made some progress
294 // so one of the parents is not nullptr. We go one step 'back' to avoid having a duplicate state
295 // on the solution path
296 if (startMotion->parent != nullptr)
297 startMotion = startMotion->parent;
298 else
299 goalMotion = goalMotion->parent;
300
301 connectionPoint_ = std::make_pair(startMotion->state, goalMotion->state);
302
303 /* construct the solution path */
304 Motion *solution = startMotion;
305 std::vector<Motion *> mpath1;
306 while (solution != nullptr)
307 {
308 mpath1.push_back(solution);
309 solution = solution->parent;
310 }
311
312 solution = goalMotion;
313 std::vector<Motion *> mpath2;
314 while (solution != nullptr)
315 {
316 mpath2.push_back(solution);
317 solution = solution->parent;
318 }
319
320 auto path(std::make_shared<PathGeometric>(si_));
321 path->getStates().reserve(mpath1.size() + mpath2.size());
322 for (int i = mpath1.size() - 1; i >= 0; --i)
323 path->append(mpath1[i]->state);
324 for (auto &i : mpath2)
325 path->append(i->state);
326
327 pdef_->addSolutionPath(path, false, 0.0, getName());
328 solved = true;
329 break;
330 }
331 else
332 {
333 // We didn't reach the goal, but if we were extending the start
334 // tree, then we can mark/improve the approximate path so far.
335 if (!startTree)
336 {
337 // We were working from the startTree.
338 double dist = 0.0;
339 goal->isSatisfied(tgi.xmotion->state, &dist);
340 if (dist < approxdif)
341 {
342 approxdif = dist;
343 approxsol = tgi.xmotion;
344 }
345 }
346 }
347 }
348 }
349
350 si_->freeState(tgi.xstate);
351 si_->freeState(rstate);
352 delete rmotion;
353
354 OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(),
355 tStart_->size(), tGoal_->size());
356
357 if (approxsol && !solved)
358 {
359 /* construct the solution path */
360 std::vector<Motion *> mpath;
361 while (approxsol != nullptr)
362 {
363 mpath.push_back(approxsol);
364 approxsol = approxsol->parent;
365 }
366
367 auto path(std::make_shared<PathGeometric>(si_));
368 for (int i = mpath.size() - 1; i >= 0; --i)
369 path->append(mpath[i]->state);
370 pdef_->addSolutionPath(path, true, approxdif, getName());
372 }
373
375}
376
378{
379 Planner::getPlannerData(data);
380
381 std::vector<Motion *> motions;
382 if (tStart_)
383 tStart_->list(motions);
384
385 for (auto &motion : motions)
386 {
387 if (motion->parent == nullptr)
388 data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
389 else
390 {
391 data.addEdge(base::PlannerDataVertex(motion->parent->state, 1), base::PlannerDataVertex(motion->state, 1));
392 }
393 }
394
395 motions.clear();
396 if (tGoal_)
397 tGoal_->list(motions);
398
399 for (auto &motion : motions)
400 {
401 if (motion->parent == nullptr)
402 data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
403 else
404 {
405 // The edges in the goal tree are reversed to be consistent with start tree
406 data.addEdge(base::PlannerDataVertex(motion->state, 2), base::PlannerDataVertex(motion->parent->state, 2));
407 }
408 }
409
410 // Add the edge connecting the two trees
411 data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
412
413 // Add some info.
414 data.properties["approx goal distance REAL"] = ompl::toString(distanceBetweenTrees_);
415}
Abstract definition of a goal region that can be sampled.
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 vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
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...
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
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
Definition of an abstract state.
Definition State.h:50
Representation of a motion.
Definition RRTConnect.h:122
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition RRTConnect.h:190
GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
Grow a tree towards a random state.
double getRange() const
Get the range the planner is using.
Definition RRTConnect.h:100
void setRange(double distance)
Set the range the planner is supposed to use.
Definition RRTConnect.h:94
GrowState
The state of the tree after an attempt to extend it.
Definition RRTConnect.h:150
bool addIntermediateStates_
Flag indicating whether intermediate states are added to the built tree of motions.
Definition RRTConnect.h:184
void freeMemory()
Free the memory allocated by this planner.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
bool getIntermediateStates() const
Return true if the intermediate states generated along motions are to be added to the tree itself.
Definition RRTConnect.h:77
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
void setIntermediateStates(bool addIntermediateStates)
Specify whether the intermediate states generated along motions are to be added to the tree itself.
Definition RRTConnect.h:84
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...
double distanceBetweenTrees_
Distance between the nearest pair of start tree and goal tree nodes.
Definition RRTConnect.h:193
RRTConnect(const base::SpaceInformationPtr &si, bool addIntermediateStates=false)
Constructor.
std::shared_ptr< NearestNeighbors< Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
Definition RRTConnect.h:138
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
#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
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition GoalTypes.h:56
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition String.cpp:82
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition Planner.h:212
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition Planner.h:196
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ EXACT_SOLUTION
The planner found an exact solution.
@ INVALID_GOAL
Invalid goal state.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.
@ TIMEOUT
The planner failed to find a solution.
Information attached to growing a tree of motions (used internally)
Definition RRTConnect.h:142