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