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 
41 ompl::geometric::RRTConnect::RRTConnect(const base::SpaceInformationPtr &si) : base::Planner(si, "RRTConnect")
42 {
44  specs_.directed = true;
45 
46  maxDistance_ = 0.0;
47 
48  Planner::declareParam<double>("range", this, &RRTConnect::setRange, &RRTConnect::getRange, "0.:1.:10000.");
49  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
50 }
51 
52 ompl::geometric::RRTConnect::~RRTConnect()
53 {
54  freeMemory();
55 }
56 
58 {
59  Planner::setup();
62 
63  if (!tStart_)
64  tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
65  if (!tGoal_)
66  tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
67  tStart_->setDistanceFunction([this](const Motion *a, const Motion *b)
68  {
69  return distanceFunction(a, b);
70  });
71  tGoal_->setDistanceFunction([this](const Motion *a, const Motion *b)
72  {
73  return distanceFunction(a, b);
74  });
75 }
76 
78 {
79  std::vector<Motion *> motions;
80 
81  if (tStart_)
82  {
83  tStart_->list(motions);
84  for (auto &motion : motions)
85  {
86  if (motion->state)
87  si_->freeState(motion->state);
88  delete motion;
89  }
90  }
91 
92  if (tGoal_)
93  {
94  tGoal_->list(motions);
95  for (auto &motion : motions)
96  {
97  if (motion->state)
98  si_->freeState(motion->state);
99  delete motion;
100  }
101  }
102 }
103 
105 {
106  Planner::clear();
107  sampler_.reset();
108  freeMemory();
109  if (tStart_)
110  tStart_->clear();
111  if (tGoal_)
112  tGoal_->clear();
113  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
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  dstate = tgi.xstate;
132  reach = false;
133  }
134  // if we are in the start tree, we just check the motion like we normally do;
135  // if we are in the goal tree, we need to check the motion in reverse, but checkMotion() assumes the first state it
136  // receives as argument is valid,
137  // so we check that one first
138  bool validMotion = tgi.start ?
139  si_->checkMotion(nmotion->state, dstate) :
140  si_->getStateValidityChecker()->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
141 
142  if (validMotion)
143  {
144  /* create a motion */
145  auto *motion = new Motion(si_);
146  si_->copyState(motion->state, dstate);
147  motion->parent = nmotion;
148  motion->root = nmotion->root;
149  tgi.xmotion = motion;
150 
151  tree->add(motion);
152  if (reach)
153  return REACHED;
154  else
155  return ADVANCED;
156  }
157  else
158  return TRAPPED;
159 }
160 
162 {
163  checkValidity();
164  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
165 
166  if (!goal)
167  {
168  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
170  }
171 
172  while (const base::State *st = pis_.nextStart())
173  {
174  auto *motion = new Motion(si_);
175  si_->copyState(motion->state, st);
176  motion->root = motion->state;
177  tStart_->add(motion);
178  }
179 
180  if (tStart_->size() == 0)
181  {
182  OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
184  }
185 
186  if (!goal->couldSample())
187  {
188  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
190  }
191 
192  if (!sampler_)
193  sampler_ = si_->allocStateSampler();
194 
195  OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
196  (int)(tStart_->size() + tGoal_->size()));
197 
198  TreeGrowingInfo tgi;
199  tgi.xstate = si_->allocState();
200 
201  auto *rmotion = new Motion(si_);
202  base::State *rstate = rmotion->state;
203  bool startTree = true;
204  bool solved = false;
205 
206  while (ptc == false)
207  {
208  TreeData &tree = startTree ? tStart_ : tGoal_;
209  tgi.start = startTree;
210  startTree = !startTree;
211  TreeData &otherTree = startTree ? tStart_ : tGoal_;
212 
213  if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
214  {
215  const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
216  if (st)
217  {
218  auto *motion = new Motion(si_);
219  si_->copyState(motion->state, st);
220  motion->root = motion->state;
221  tGoal_->add(motion);
222  }
223 
224  if (tGoal_->size() == 0)
225  {
226  OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
227  break;
228  }
229  }
230 
231  /* sample random state */
232  sampler_->sampleUniform(rstate);
233 
234  GrowState gs = growTree(tree, tgi, rmotion);
235 
236  if (gs != TRAPPED)
237  {
238  /* remember which motion was just added */
239  Motion *addedMotion = tgi.xmotion;
240 
241  /* attempt to connect trees */
242 
243  /* if reached, it means we used rstate directly, no need top copy again */
244  if (gs != REACHED)
245  si_->copyState(rstate, tgi.xstate);
246 
247  GrowState gsc = ADVANCED;
248  tgi.start = startTree;
249  while (gsc == ADVANCED)
250  gsc = growTree(otherTree, tgi, rmotion);
251 
252  Motion *startMotion = startTree ? tgi.xmotion : addedMotion;
253  Motion *goalMotion = startTree ? addedMotion : tgi.xmotion;
254 
255  /* if we connected the trees in a valid way (start and goal pair is valid)*/
256  if (gsc == REACHED && goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
257  {
258  // it must be the case that either the start tree or the goal tree has made some progress
259  // so one of the parents is not nullptr. We go one step 'back' to avoid having a duplicate state
260  // on the solution path
261  if (startMotion->parent)
262  startMotion = startMotion->parent;
263  else
264  goalMotion = goalMotion->parent;
265 
266  connectionPoint_ = std::make_pair(startMotion->state, goalMotion->state);
267 
268  /* construct the solution path */
269  Motion *solution = startMotion;
270  std::vector<Motion *> mpath1;
271  while (solution != nullptr)
272  {
273  mpath1.push_back(solution);
274  solution = solution->parent;
275  }
276 
277  solution = goalMotion;
278  std::vector<Motion *> mpath2;
279  while (solution != nullptr)
280  {
281  mpath2.push_back(solution);
282  solution = solution->parent;
283  }
284 
285  auto path(std::make_shared<PathGeometric>(si_));
286  path->getStates().reserve(mpath1.size() + mpath2.size());
287  for (int i = mpath1.size() - 1; i >= 0; --i)
288  path->append(mpath1[i]->state);
289  for (auto &i : mpath2)
290  path->append(i->state);
291 
292  pdef_->addSolutionPath(path, false, 0.0, getName());
293  solved = true;
294  break;
295  }
296  }
297  }
298 
299  si_->freeState(tgi.xstate);
300  si_->freeState(rstate);
301  delete rmotion;
302 
303  OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(),
304  tStart_->size(), tGoal_->size());
305 
307 }
308 
310 {
311  Planner::getPlannerData(data);
312 
313  std::vector<Motion *> motions;
314  if (tStart_)
315  tStart_->list(motions);
316 
317  for (auto &motion : motions)
318  {
319  if (motion->parent == nullptr)
320  data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
321  else
322  {
323  data.addEdge(base::PlannerDataVertex(motion->parent->state, 1), base::PlannerDataVertex(motion->state, 1));
324  }
325  }
326 
327  motions.clear();
328  if (tGoal_)
329  tGoal_->list(motions);
330 
331  for (auto &motion : motions)
332  {
333  if (motion->parent == nullptr)
334  data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
335  else
336  {
337  // The edges in the goal tree are reversed to be consistent with start tree
338  data.addEdge(base::PlannerDataVertex(motion->state, 2), base::PlannerDataVertex(motion->parent->state, 2));
339  }
340  }
341 
342  // Add the edge connecting the two trees
343  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
344 }
RRTConnect(const base::SpaceInformationPtr &si)
Constructor.
Definition: RRTConnect.cpp:41
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
base::StateSamplerPtr sampler_
State sampler.
Definition: RRTConnect.h:162
TreeData tGoal_
The goal tree.
Definition: RRTConnect.h:168
The planner failed to find a solution.
Definition: PlannerStatus.h:62
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: RRTConnect.h:80
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:206
virtual bool couldSample() const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:269
virtual bool isStartGoalPairValid(const State *, const State *) const
Since there can be multiple starting states (and multiple goal states) it is possible certain pairs a...
Definition: Goal.h:136
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...
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRTConnect.cpp:104
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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...
no progress has been made
Definition: RRTConnect.h:142
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTConnect.cpp:77
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: RRTConnect.h:153
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: RRTConnect.h:177
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:222
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:175
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: RRTConnect.h:171
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
double getRange() const
Get the range the planner is using.
Definition: RRTConnect.h:86
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
Representation of a motion.
Definition: RRTConnect.h:107
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
The planner found an exact solution.
Definition: PlannerStatus.h:66
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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...
A shared pointer wrapper for ompl::base::SpaceInformation.
GrowState
The state of the tree after an attempt to extend it.
Definition: RRTConnect.h:139
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...
Definition of an abstract state.
Definition: State.h:49
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:101
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:421
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:427
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:230
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
the randomly sampled state was reached
Definition: RRTConnect.h:146
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRTConnect.cpp:57
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:59
Information attached to growing a tree of motions (used internally)
Definition: RRTConnect.h:131
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: RRTConnect.cpp:309
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
progress has been made towards the randomly sampled state
Definition: RRTConnect.h:144
std::shared_ptr< NearestNeighbors< Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
Definition: RRTConnect.h:128
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: RRTConnect.cpp:161
GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
Grow a tree towards a random state.
Definition: RRTConnect.cpp:116
TreeData tStart_
The start tree.
Definition: RRTConnect.h:165
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:56
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68