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/control/planners/rrt/RRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <limits>
41 
42 ompl::control::RRT::RRT(const SpaceInformationPtr &si) : base::Planner(si, "RRT")
43 {
45  siC_ = si.get();
46  addIntermediateStates_ = false;
47  lastGoalMotion_ = NULL;
48 
49  goalBias_ = 0.05;
50 
51  Planner::declareParam<double>("goal_bias", this, &RRT::setGoalBias, &RRT::getGoalBias, "0.:.05:1.");
52  Planner::declareParam<bool>("intermediate_states", this, &RRT::setIntermediateStates, &RRT::getIntermediateStates);
53 }
54 
55 ompl::control::RRT::~RRT()
56 {
57  freeMemory();
58 }
59 
61 {
63  if (!nn_)
64  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
65  nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
66 }
67 
69 {
70  Planner::clear();
71  sampler_.reset();
72  controlSampler_.reset();
73  freeMemory();
74  if (nn_)
75  nn_->clear();
76  lastGoalMotion_ = NULL;
77 }
78 
80 {
81  if (nn_)
82  {
83  std::vector<Motion*> motions;
84  nn_->list(motions);
85  for (unsigned int i = 0 ; i < motions.size() ; ++i)
86  {
87  if (motions[i]->state)
88  si_->freeState(motions[i]->state);
89  if (motions[i]->control)
90  siC_->freeControl(motions[i]->control);
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(siC_);
105  si_->copyState(motion->state, st);
106  siC_->nullControl(motion->control);
107  nn_->add(motion);
108  }
109 
110  if (nn_->size() == 0)
111  {
112  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
114  }
115 
116  if (!sampler_)
117  sampler_ = si_->allocStateSampler();
118  if (!controlSampler_)
120 
121  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
122 
123  Motion *solution = NULL;
124  Motion *approxsol = NULL;
125  double approxdif = std::numeric_limits<double>::infinity();
126 
127  Motion *rmotion = new Motion(siC_);
128  base::State *rstate = rmotion->state;
129  Control *rctrl = rmotion->control;
130  base::State *xstate = si_->allocState();
131 
132  while (ptc == false)
133  {
134  /* sample random state (with goal biasing) */
135  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
136  goal_s->sampleGoal(rstate);
137  else
138  sampler_->sampleUniform(rstate);
139 
140  /* find closest state in the tree */
141  Motion *nmotion = nn_->nearest(rmotion);
142 
143  /* sample a random control that attempts to go towards the random state, and also sample a control duration */
144  unsigned int cd = controlSampler_->sampleTo(rctrl, nmotion->control, nmotion->state, rmotion->state);
145 
147  {
148  // this code is contributed by Jennifer Barry
149  std::vector<base::State *> pstates;
150  cd = siC_->propagateWhileValid(nmotion->state, rctrl, cd, pstates, true);
151 
152  if (cd >= siC_->getMinControlDuration())
153  {
154  Motion *lastmotion = nmotion;
155  bool solved = false;
156  size_t p = 0;
157  for ( ; p < pstates.size(); ++p)
158  {
159  /* create a motion */
160  Motion *motion = new Motion();
161  motion->state = pstates[p];
162  //we need multiple copies of rctrl
163  motion->control = siC_->allocControl();
164  siC_->copyControl(motion->control, rctrl);
165  motion->steps = 1;
166  motion->parent = lastmotion;
167  lastmotion = motion;
168  nn_->add(motion);
169  double dist = 0.0;
170  solved = goal->isSatisfied(motion->state, &dist);
171  if (solved)
172  {
173  approxdif = dist;
174  solution = motion;
175  break;
176  }
177  if (dist < approxdif)
178  {
179  approxdif = dist;
180  approxsol = motion;
181  }
182  }
183 
184  //free any states after we hit the goal
185  while (++p < pstates.size())
186  si_->freeState(pstates[p]);
187  if (solved)
188  break;
189  }
190  else
191  for (size_t p = 0 ; p < pstates.size(); ++p)
192  si_->freeState(pstates[p]);
193  }
194  else
195  {
196  if (cd >= siC_->getMinControlDuration())
197  {
198  /* create a motion */
199  Motion *motion = new Motion(siC_);
200  si_->copyState(motion->state, rmotion->state);
201  siC_->copyControl(motion->control, rctrl);
202  motion->steps = cd;
203  motion->parent = nmotion;
204 
205  nn_->add(motion);
206  double dist = 0.0;
207  bool solv = goal->isSatisfied(motion->state, &dist);
208  if (solv)
209  {
210  approxdif = dist;
211  solution = motion;
212  break;
213  }
214  if (dist < approxdif)
215  {
216  approxdif = dist;
217  approxsol = motion;
218  }
219  }
220  }
221  }
222 
223  bool solved = false;
224  bool approximate = false;
225  if (solution == NULL)
226  {
227  solution = approxsol;
228  approximate = true;
229  }
230 
231  if (solution != NULL)
232  {
233  lastGoalMotion_ = solution;
234 
235  /* construct the solution path */
236  std::vector<Motion*> mpath;
237  while (solution != NULL)
238  {
239  mpath.push_back(solution);
240  solution = solution->parent;
241  }
242 
243  /* set the solution path */
244  PathControl *path = new PathControl(si_);
245  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
246  if (mpath[i]->parent)
247  path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
248  else
249  path->append(mpath[i]->state);
250  solved = true;
251  pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, getName());
252  }
253 
254  if (rmotion->state)
255  si_->freeState(rmotion->state);
256  if (rmotion->control)
257  siC_->freeControl(rmotion->control);
258  delete rmotion;
259  si_->freeState(xstate);
260 
261  OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
262 
263  return base::PlannerStatus(solved, approximate);
264 }
265 
267 {
268  Planner::getPlannerData(data);
269 
270  std::vector<Motion*> motions;
271  if (nn_)
272  nn_->list(motions);
273 
274  double delta = siC_->getPropagationStepSize();
275 
276  if (lastGoalMotion_)
278 
279  for (unsigned int i = 0 ; i < motions.size() ; ++i)
280  {
281  const Motion *m = motions[i];
282  if (m->parent)
283  {
284  if (data.hasControls())
288  else
291  }
292  else
294  }
295 }
DirectedControlSamplerPtr controlSampler_
Control sampler.
Definition: RRT.h:174
DirectedControlSamplerPtr allocDirectedControlSampler() const
Allocate an instance of the DirectedControlSampler to use. This will be the default (SimpleDirectedCo...
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
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
unsigned int getMinControlDuration() const
Get the minimum number of steps a control is propagated for.
unsigned int propagateWhileValid(const base::State *state, const Control *control, int steps, base::State *result) const
Propagate the model of the system forward, starting at a given state, with a given control...
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
void nullControl(Control *control) const
Make the control have no effect if it were to be applied to a state for any amount of time...
Definition of an abstract control.
Definition: Control.h:48
virtual void clear()
Clear datastructures. Call this function if the input data to the planner has changed and you do not ...
Definition: RRT.cpp:68
boost::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: RRT.h:180
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
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Continue solving for some amount of time. Return true if solution was found.
Definition: RRT.cpp:96
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
base::State * state
The state contained by the motion.
Definition: RRT.h:149
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:60
RRT(const SpaceInformationPtr &si)
Constructor.
Definition: RRT.cpp:42
void freeMemory()
Free the memory allocated by this planner.
Definition: RRT.cpp:79
Definition of a control path.
Definition: PathControl.h:60
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: RRT.h:165
Control * allocControl() const
Allocate memory for a control.
bool getIntermediateStates() const
Return true if the intermediate states generated along motions are to be added to the tree itself...
Definition: RRT.h:102
bool addIntermediateStates_
Flag indicating whether intermediate states are added to the built tree of motions.
Definition: RRT.h:186
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:400
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
void setIntermediateStates(bool addIntermediateStates)
Specify whether the intermediate states generated along motions are to be added to the tree itself...
Definition: RRT.h:108
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRT.cpp:60
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:62
base::StateSamplerPtr sampler_
State sampler.
Definition: RRT.h:171
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
Motion * parent
The parent motion in the exploration tree.
Definition: RRT.h:158
Abstract definition of a goal region that can be sampled.
Representation of a motion.
Definition: RRT.h:131
void copyControl(Control *destination, const Control *source) const
Copy a control to another.
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:266
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Control * control
The control contained by the motion.
Definition: RRT.h:152
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: RRT.h:192
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:86
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...
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 checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:100
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:403
A boost shared pointer wrapper for ompl::control::SpaceInformation.
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 hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:55
void setGoalBias(double goalBias)
Definition: RRT.h:90
void freeControl(Control *control) const
Free the memory of a control.
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition: RRT.h:177
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
double getGoalBias() const
Get the goal bias the planner is using.
Definition: RRT.h:96
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:183
unsigned int steps
The number of steps the control is applied for.
Definition: RRT.h:155
A boost shared pointer wrapper for ompl::base::Path.
RNG rng_
The random number generator.
Definition: RRT.h:189
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68