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 solved = false;
231
232 while (!ptc)
233 {
234 TreeData &tree = startTree_ ? tStart_ : tGoal_;
235 tgi.start = startTree_;
236 startTree_ = !startTree_;
237 TreeData &otherTree = startTree_ ? tStart_ : tGoal_;
238
239 if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
240 {
241 const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
242 if (st != nullptr)
243 {
244 auto *motion = new Motion(si_);
245 si_->copyState(motion->state, st);
246 motion->root = motion->state;
247 tGoal_->add(motion);
248 }
249
250 if (tGoal_->size() == 0)
251 {
252 OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
253 break;
254 }
255 }
256
257 /* sample random state */
258 sampler_->sampleUniform(rstate);
259
260 GrowState gs = growTree(tree, tgi, rmotion);
261
262 if (gs != TRAPPED)
263 {
264 /* remember which motion was just added */
265 Motion *addedMotion = tgi.xmotion;
266
267 /* attempt to connect trees */
268
269 /* if reached, it means we used rstate directly, no need to copy again */
270 if (gs != REACHED)
271 si_->copyState(rstate, tgi.xstate);
272
273 tgi.start = startTree_;
274
275 /* if initial progress cannot be done from the otherTree, restore tgi.start */
276 GrowState gsc = growTree(otherTree, tgi, rmotion);
277 if (gsc == TRAPPED)
278 tgi.start = !tgi.start;
279
280 while (gsc == ADVANCED)
281 gsc = growTree(otherTree, tgi, rmotion);
282
283 /* update distance between trees */
284 const double newDist = tree->getDistanceFunction()(addedMotion, otherTree->nearest(addedMotion));
285 if (newDist < distanceBetweenTrees_)
286 {
287 distanceBetweenTrees_ = newDist;
288 // OMPL_INFORM("Estimated distance to go: %f", distanceBetweenTrees_);
289 }
290
291 Motion *startMotion = tgi.start ? tgi.xmotion : addedMotion;
292 Motion *goalMotion = tgi.start ? addedMotion : tgi.xmotion;
293
294 /* if we connected the trees in a valid way (start and goal pair is valid)*/
295 if (gsc == REACHED && goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
296 {
297 // it must be the case that either the start tree or the goal tree has made some progress
298 // so one of the parents is not nullptr. We go one step 'back' to avoid having a duplicate state
299 // on the solution path
300 if (startMotion->parent != nullptr)
301 startMotion = startMotion->parent;
302 else
303 goalMotion = goalMotion->parent;
304
305 connectionPoint_ = std::make_pair(startMotion->state, goalMotion->state);
306
307 /* construct the solution path */
308 Motion *solution = startMotion;
309 std::vector<Motion *> mpath1;
310 while (solution != nullptr)
311 {
312 mpath1.push_back(solution);
313 solution = solution->parent;
314 }
315
316 solution = goalMotion;
317 std::vector<Motion *> mpath2;
318 while (solution != nullptr)
319 {
320 mpath2.push_back(solution);
321 solution = solution->parent;
322 }
323
324 auto path(std::make_shared<PathGeometric>(si_));
325 path->getStates().reserve(mpath1.size() + mpath2.size());
326 for (int i = mpath1.size() - 1; i >= 0; --i)
327 path->append(mpath1[i]->state);
328 for (auto &i : mpath2)
329 path->append(i->state);
330
331 pdef_->addSolutionPath(path, false, 0.0, getName());
332 solved = true;
333 break;
334 }
335 else
336 {
337 // We didn't reach the goal, but if we were extending the start
338 // tree, then we can mark/improve the approximate path so far.
339 if (tgi.start)
340 {
341 // We were working from the startTree.
342 double dist = 0.0;
343 goal->isSatisfied(tgi.xmotion->state, &dist);
344 if (dist < approxdif)
345 {
346 approxdif = dist;
347 approxsol = tgi.xmotion;
348 }
349 }
350 }
351 }
352 }
353
354 si_->freeState(tgi.xstate);
355 si_->freeState(rstate);
356 delete rmotion;
357
358 OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(),
359 tStart_->size(), tGoal_->size());
360
361 if (approxsol && !solved)
362 {
363 /* construct the solution path */
364 std::vector<Motion *> mpath;
365 while (approxsol != nullptr)
366 {
367 mpath.push_back(approxsol);
368 approxsol = approxsol->parent;
369 }
370
371 auto path(std::make_shared<PathGeometric>(si_));
372 for (int i = mpath.size() - 1; i >= 0; --i)
373 path->append(mpath[i]->state);
374 pdef_->addSolutionPath(path, true, approxdif, getName());
376 }
377
379}
380
382{
383 Planner::getPlannerData(data);
384
385 std::vector<Motion *> motions;
386 if (tStart_)
387 tStart_->list(motions);
388
389 for (auto &motion : motions)
390 {
391 if (motion->parent == nullptr)
392 data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
393 else
394 {
395 data.addEdge(base::PlannerDataVertex(motion->parent->state, 1), base::PlannerDataVertex(motion->state, 1));
396 }
397 }
398
399 motions.clear();
400 if (tGoal_)
401 tGoal_->list(motions);
402
403 for (auto &motion : motions)
404 {
405 if (motion->parent == nullptr)
406 data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
407 else
408 {
409 // The edges in the goal tree are reversed to be consistent with start tree
410 data.addEdge(base::PlannerDataVertex(motion->state, 2), base::PlannerDataVertex(motion->parent->state, 2));
411 }
412 }
413
414 // Add the edge connecting the two trees
415 data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
416
417 // Add some info.
418 data.properties["approx goal distance REAL"] = ompl::toString(distanceBetweenTrees_);
419}
const DistanceFunction & getDistanceFunction() const
Get the distance function used.
virtual void add(const _T &data)=0
Add an element to the datastructure.
virtual _T nearest(const _T &data) const =0
Get the nearest neighbor of a point.
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:422
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:193
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:187
void freeMemory()
Free the memory allocated by this planner.
std::shared_ptr< NearestNeighbors< Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
Definition RRTConnect.h:138
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:196
RRTConnect(const base::SpaceInformationPtr &si, bool addIntermediateStates=false)
Constructor.
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:205
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition Planner.h:189
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