RRT.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/RRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <limits>
41 
42 ompl::geometric::RRT::RRT(const base::SpaceInformationPtr &si) : base::Planner(si, "RRT")
43 {
45  specs_.directed = true;
46 
47  goalBias_ = 0.05;
48  maxDistance_ = 0.0;
49  lastGoalMotion_ = NULL;
50 
51  Planner::declareParam<double>("range", this, &RRT::setRange, &RRT::getRange, "0.:1.:10000.");
52  Planner::declareParam<double>("goal_bias", this, &RRT::setGoalBias, &RRT::getGoalBias, "0.:.05:1.");
53 }
54 
55 ompl::geometric::RRT::~RRT()
56 {
57  freeMemory();
58 }
59 
61 {
62  Planner::clear();
63  sampler_.reset();
64  freeMemory();
65  if (nn_)
66  nn_->clear();
67  lastGoalMotion_ = NULL;
68 }
69 
71 {
72  Planner::setup();
73  tools::SelfConfig sc(si_, getName());
74  sc.configurePlannerRange(maxDistance_);
75 
76  if (!nn_)
77  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
78  nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
79 }
80 
82 {
83  if (nn_)
84  {
85  std::vector<Motion*> motions;
86  nn_->list(motions);
87  for (unsigned int i = 0 ; i < motions.size() ; ++i)
88  {
89  if (motions[i]->state)
90  si_->freeState(motions[i]->state);
91  delete motions[i];
92  }
93  }
94 }
95 
97 {
98  checkValidity();
99  base::Goal *goal = pdef_->getGoal().get();
100  base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
101 
102  while (const base::State *st = pis_.nextStart())
103  {
104  Motion *motion = new Motion(si_);
105  si_->copyState(motion->state, st);
106  nn_->add(motion);
107  }
108 
109  if (nn_->size() == 0)
110  {
111  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
113  }
114 
115  if (!sampler_)
116  sampler_ = si_->allocStateSampler();
117 
118  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
119 
120  Motion *solution = NULL;
121  Motion *approxsol = NULL;
122  double approxdif = std::numeric_limits<double>::infinity();
123  Motion *rmotion = new Motion(si_);
124  base::State *rstate = rmotion->state;
125  base::State *xstate = si_->allocState();
126 
127  while (ptc == false)
128  {
129 
130  /* sample random state (with goal biasing) */
131  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
132  goal_s->sampleGoal(rstate);
133  else
134  sampler_->sampleUniform(rstate);
135 
136  /* find closest state in the tree */
137  Motion *nmotion = nn_->nearest(rmotion);
138  base::State *dstate = rstate;
139 
140  /* find state to add */
141  double d = si_->distance(nmotion->state, rstate);
142  if (d > maxDistance_)
143  {
144  si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
145  dstate = xstate;
146  }
147 
148  if (si_->checkMotion(nmotion->state, dstate))
149  {
150  /* create a motion */
151  Motion *motion = new Motion(si_);
152  si_->copyState(motion->state, dstate);
153  motion->parent = nmotion;
154 
155  nn_->add(motion);
156  double dist = 0.0;
157  bool sat = goal->isSatisfied(motion->state, &dist);
158  if (sat)
159  {
160  approxdif = dist;
161  solution = motion;
162  break;
163  }
164  if (dist < approxdif)
165  {
166  approxdif = dist;
167  approxsol = motion;
168  }
169  }
170  }
171 
172  bool solved = false;
173  bool approximate = false;
174  if (solution == NULL)
175  {
176  solution = approxsol;
177  approximate = true;
178  }
179 
180  if (solution != NULL)
181  {
182  lastGoalMotion_ = solution;
183 
184  /* construct the solution path */
185  std::vector<Motion*> mpath;
186  while (solution != NULL)
187  {
188  mpath.push_back(solution);
189  solution = solution->parent;
190  }
191 
192  /* set the solution path */
193  PathGeometric *path = new PathGeometric(si_);
194  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
195  path->append(mpath[i]->state);
196  pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, getName());
197  solved = true;
198  }
199 
200  si_->freeState(xstate);
201  if (rmotion->state)
202  si_->freeState(rmotion->state);
203  delete rmotion;
204 
205  OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
206 
207  return base::PlannerStatus(solved, approximate);
208 }
209 
211 {
212  Planner::getPlannerData(data);
213 
214  std::vector<Motion*> motions;
215  if (nn_)
216  nn_->list(motions);
217 
218  if (lastGoalMotion_)
219  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
220 
221  for (unsigned int i = 0 ; i < motions.size() ; ++i)
222  {
223  if (motions[i]->parent == NULL)
224  data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
225  else
226  data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
227  base::PlannerDataVertex(motions[i]->state));
228  }
229 }
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: RRT.h:176
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRT.cpp:70
Motion * parent
The parent motion in the exploration tree.
Definition: RRT.h:153
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: RRT.h:105
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...
Abstract definition of goals.
Definition: Goal.h:63
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
double getGoalBias() const
Get the goal bias the planner is using.
Definition: RRT.h:95
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRT.cpp:60
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: RRT.cpp:210
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
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: RRT.h:161
double getRange() const
Get the range the planner is using.
Definition: RRT.h:111
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...
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
void freeMemory()
Free the memory allocated by this planner.
Definition: RRT.cpp:81
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
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: RRT.h:182
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
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
Representation of a motion.
Definition: RRT.h:132
RRT(const base::SpaceInformationPtr &si)
Constructor.
Definition: RRT.cpp:42
Definition of a geometric path.
Definition: PathGeometric.h:60
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
Definition: RRT.h:173
void setGoalBias(double goalBias)
Set the goal bias.
Definition: RRT.h:89
A boost shared pointer wrapper for ompl::base::Path.
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: RRT.cpp:96
base::State * state
The state contained by the motion.
Definition: RRT.h:150
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68