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  Planner::declareParam<double>("range", this, &RRTConnect::setRange, &RRTConnect::getRange, "0.:1.:10000.");
47  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
48 }
49 
50 ompl::geometric::RRTConnect::~RRTConnect()
51 {
52  freeMemory();
53 }
54 
56 {
57  Planner::setup();
60 
61  if (!tStart_)
62  tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
63  if (!tGoal_)
64  tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
65  tStart_->setDistanceFunction([this](const Motion *a, const Motion *b)
66  {
67  return distanceFunction(a, b);
68  });
69  tGoal_->setDistanceFunction([this](const Motion *a, const Motion *b)
70  {
71  return distanceFunction(a, b);
72  });
73 }
74 
76 {
77  std::vector<Motion *> motions;
78 
79  if (tStart_)
80  {
81  tStart_->list(motions);
82  for (auto &motion : motions)
83  {
84  if (motion->state != nullptr)
85  si_->freeState(motion->state);
86  delete motion;
87  }
88  }
89 
90  if (tGoal_)
91  {
92  tGoal_->list(motions);
93  for (auto &motion : motions)
94  {
95  if (motion->state != nullptr)
96  si_->freeState(motion->state);
97  delete motion;
98  }
99  }
100 }
101 
103 {
104  Planner::clear();
105  sampler_.reset();
106  freeMemory();
107  if (tStart_)
108  tStart_->clear();
109  if (tGoal_)
110  tGoal_->clear();
111  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
112 }
113 
115  Motion *rmotion)
116 {
117  /* find closest state in the tree */
118  Motion *nmotion = tree->nearest(rmotion);
119 
120  /* assume we can reach the state we go towards */
121  bool reach = true;
122 
123  /* find state to add */
124  base::State *dstate = rmotion->state;
125  double d = si_->distance(nmotion->state, rmotion->state);
126  if (d > maxDistance_)
127  {
128  si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
129  dstate = tgi.xstate;
130  reach = false;
131  }
132  // if we are in the start tree, we just check the motion like we normally do;
133  // if we are in the goal tree, we need to check the motion in reverse, but checkMotion() assumes the first state it
134  // receives as argument is valid,
135  // so we check that one first
136  bool validMotion = tgi.start ?
137  si_->checkMotion(nmotion->state, dstate) :
138  si_->getStateValidityChecker()->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
139 
140  if (validMotion)
141  {
142  /* create a motion */
143  auto *motion = new Motion(si_);
144  si_->copyState(motion->state, dstate);
145  motion->parent = nmotion;
146  motion->root = nmotion->root;
147  tgi.xmotion = motion;
148 
149  tree->add(motion);
150  return reach ? REACHED : ADVANCED;
151  }
152  else
153  return TRAPPED;
154 }
155 
157 {
158  checkValidity();
159  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
160 
161  if (goal == nullptr)
162  {
163  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
165  }
166 
167  while (const base::State *st = pis_.nextStart())
168  {
169  auto *motion = new Motion(si_);
170  si_->copyState(motion->state, st);
171  motion->root = motion->state;
172  tStart_->add(motion);
173  }
174 
175  if (tStart_->size() == 0)
176  {
177  OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
179  }
180 
181  if (!goal->couldSample())
182  {
183  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
185  }
186 
187  if (!sampler_)
188  sampler_ = si_->allocStateSampler();
189 
190  OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
191  (int)(tStart_->size() + tGoal_->size()));
192 
193  TreeGrowingInfo tgi;
194  tgi.xstate = si_->allocState();
195 
196  auto *rmotion = new Motion(si_);
197  base::State *rstate = rmotion->state;
198  bool startTree = true;
199  bool solved = false;
200 
201  while (!ptc)
202  {
203  TreeData &tree = startTree ? tStart_ : tGoal_;
204  tgi.start = startTree;
205  startTree = !startTree;
206  TreeData &otherTree = startTree ? tStart_ : tGoal_;
207 
208  if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
209  {
210  const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
211  if (st != nullptr)
212  {
213  auto *motion = new Motion(si_);
214  si_->copyState(motion->state, st);
215  motion->root = motion->state;
216  tGoal_->add(motion);
217  }
218 
219  if (tGoal_->size() == 0)
220  {
221  OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
222  break;
223  }
224  }
225 
226  /* sample random state */
227  sampler_->sampleUniform(rstate);
228 
229  GrowState gs = growTree(tree, tgi, rmotion);
230 
231  if (gs != TRAPPED)
232  {
233  /* remember which motion was just added */
234  Motion *addedMotion = tgi.xmotion;
235 
236  /* attempt to connect trees */
237 
238  /* if reached, it means we used rstate directly, no need top copy again */
239  if (gs != REACHED)
240  si_->copyState(rstate, tgi.xstate);
241 
242  GrowState gsc = ADVANCED;
243  tgi.start = startTree;
244  while (gsc == ADVANCED)
245  gsc = growTree(otherTree, tgi, rmotion);
246 
247  Motion *startMotion = startTree ? tgi.xmotion : addedMotion;
248  Motion *goalMotion = startTree ? addedMotion : tgi.xmotion;
249 
250  /* if we connected the trees in a valid way (start and goal pair is valid)*/
251  if (gsc == REACHED && goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
252  {
253  // it must be the case that either the start tree or the goal tree has made some progress
254  // so one of the parents is not nullptr. We go one step 'back' to avoid having a duplicate state
255  // on the solution path
256  if (startMotion->parent != nullptr)
257  startMotion = startMotion->parent;
258  else
259  goalMotion = goalMotion->parent;
260 
261  connectionPoint_ = std::make_pair(startMotion->state, goalMotion->state);
262 
263  /* construct the solution path */
264  Motion *solution = startMotion;
265  std::vector<Motion *> mpath1;
266  while (solution != nullptr)
267  {
268  mpath1.push_back(solution);
269  solution = solution->parent;
270  }
271 
272  solution = goalMotion;
273  std::vector<Motion *> mpath2;
274  while (solution != nullptr)
275  {
276  mpath2.push_back(solution);
277  solution = solution->parent;
278  }
279 
280  auto path(std::make_shared<PathGeometric>(si_));
281  path->getStates().reserve(mpath1.size() + mpath2.size());
282  for (int i = mpath1.size() - 1; i >= 0; --i)
283  path->append(mpath1[i]->state);
284  for (auto &i : mpath2)
285  path->append(i->state);
286 
287  pdef_->addSolutionPath(path, false, 0.0, getName());
288  solved = true;
289  break;
290  }
291  }
292  }
293 
294  si_->freeState(tgi.xstate);
295  si_->freeState(rstate);
296  delete rmotion;
297 
298  OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(),
299  tStart_->size(), tGoal_->size());
300 
302 }
303 
305 {
306  Planner::getPlannerData(data);
307 
308  std::vector<Motion *> motions;
309  if (tStart_)
310  tStart_->list(motions);
311 
312  for (auto &motion : motions)
313  {
314  if (motion->parent == nullptr)
315  data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
316  else
317  {
318  data.addEdge(base::PlannerDataVertex(motion->parent->state, 1), base::PlannerDataVertex(motion->state, 1));
319  }
320  }
321 
322  motions.clear();
323  if (tGoal_)
324  tGoal_->list(motions);
325 
326  for (auto &motion : motions)
327  {
328  if (motion->parent == nullptr)
329  data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
330  else
331  {
332  // The edges in the goal tree are reversed to be consistent with start tree
333  data.addEdge(base::PlannerDataVertex(motion->state, 2), base::PlannerDataVertex(motion->parent->state, 2));
334  }
335  }
336 
337  // Add the edge connecting the two trees
338  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
339 }
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:158
TreeData tGoal_
The goal tree.
Definition: RRTConnect.h:164
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:197
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:264
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:102
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:138
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTConnect.cpp:75
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: RRTConnect.h:149
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: RRTConnect.h:173
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:213
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:167
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:135
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:412
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
the randomly sampled state was reached
Definition: RRTConnect.h:142
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRTConnect.cpp:55
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:127
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:304
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
progress has been made towards the randomly sampled state
Definition: RRTConnect.h:140
std::shared_ptr< NearestNeighbors< Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
Definition: RRTConnect.h:124
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:156
GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
Grow a tree towards a random state.
Definition: RRTConnect.cpp:114
TreeData tStart_
The start tree.
Definition: RRTConnect.h:161
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