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 
47  Planner::declareParam<double>("goal_bias", this, &RRT::setGoalBias, &RRT::getGoalBias, "0.:.05:1.");
48  Planner::declareParam<bool>("intermediate_states", this, &RRT::setIntermediateStates, &RRT::getIntermediateStates);
49 }
50 
51 ompl::control::RRT::~RRT()
52 {
53  freeMemory();
54 }
55 
57 {
59  if (!nn_)
60  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
61  nn_->setDistanceFunction([this](const Motion *a, const Motion *b)
62  {
63  return distanceFunction(a, b);
64  });
65 }
66 
68 {
69  Planner::clear();
70  sampler_.reset();
71  controlSampler_.reset();
72  freeMemory();
73  if (nn_)
74  nn_->clear();
75  lastGoalMotion_ = nullptr;
76 }
77 
79 {
80  if (nn_)
81  {
82  std::vector<Motion *> motions;
83  nn_->list(motions);
84  for (auto &motion : motions)
85  {
86  if (motion->state)
87  si_->freeState(motion->state);
88  if (motion->control)
89  siC_->freeControl(motion->control);
90  delete motion;
91  }
92  }
93 }
94 
96 {
97  checkValidity();
98  base::Goal *goal = pdef_->getGoal().get();
99  auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
100 
101  while (const base::State *st = pis_.nextStart())
102  {
103  auto *motion = new Motion(siC_);
104  si_->copyState(motion->state, st);
105  siC_->nullControl(motion->control);
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  if (!controlSampler_)
118  controlSampler_ = siC_->allocDirectedControlSampler();
119 
120  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
121 
122  Motion *solution = nullptr;
123  Motion *approxsol = nullptr;
124  double approxdif = std::numeric_limits<double>::infinity();
125 
126  auto *rmotion = new Motion(siC_);
127  base::State *rstate = rmotion->state;
128  Control *rctrl = rmotion->control;
129  base::State *xstate = si_->allocState();
130 
131  while (ptc == false)
132  {
133  /* sample random state (with goal biasing) */
134  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
135  goal_s->sampleGoal(rstate);
136  else
137  sampler_->sampleUniform(rstate);
138 
139  /* find closest state in the tree */
140  Motion *nmotion = nn_->nearest(rmotion);
141 
142  /* sample a random control that attempts to go towards the random state, and also sample a control duration */
143  unsigned int cd = controlSampler_->sampleTo(rctrl, nmotion->control, nmotion->state, rmotion->state);
144 
145  if (addIntermediateStates_)
146  {
147  // this code is contributed by Jennifer Barry
148  std::vector<base::State *> pstates;
149  cd = siC_->propagateWhileValid(nmotion->state, rctrl, cd, pstates, true);
150 
151  if (cd >= siC_->getMinControlDuration())
152  {
153  Motion *lastmotion = nmotion;
154  bool solved = false;
155  size_t p = 0;
156  for (; p < pstates.size(); ++p)
157  {
158  /* create a motion */
159  auto *motion = new Motion();
160  motion->state = pstates[p];
161  // we need multiple copies of rctrl
162  motion->control = siC_->allocControl();
163  siC_->copyControl(motion->control, rctrl);
164  motion->steps = 1;
165  motion->parent = lastmotion;
166  lastmotion = motion;
167  nn_->add(motion);
168  double dist = 0.0;
169  solved = goal->isSatisfied(motion->state, &dist);
170  if (solved)
171  {
172  approxdif = dist;
173  solution = motion;
174  break;
175  }
176  if (dist < approxdif)
177  {
178  approxdif = dist;
179  approxsol = motion;
180  }
181  }
182 
183  // free any states after we hit the goal
184  while (++p < pstates.size())
185  si_->freeState(pstates[p]);
186  if (solved)
187  break;
188  }
189  else
190  for (auto &pstate : pstates)
191  si_->freeState(pstate);
192  }
193  else
194  {
195  if (cd >= siC_->getMinControlDuration())
196  {
197  /* create a motion */
198  auto *motion = new Motion(siC_);
199  si_->copyState(motion->state, rmotion->state);
200  siC_->copyControl(motion->control, rctrl);
201  motion->steps = cd;
202  motion->parent = nmotion;
203 
204  nn_->add(motion);
205  double dist = 0.0;
206  bool solv = goal->isSatisfied(motion->state, &dist);
207  if (solv)
208  {
209  approxdif = dist;
210  solution = motion;
211  break;
212  }
213  if (dist < approxdif)
214  {
215  approxdif = dist;
216  approxsol = motion;
217  }
218  }
219  }
220  }
221 
222  bool solved = false;
223  bool approximate = false;
224  if (solution == nullptr)
225  {
226  solution = approxsol;
227  approximate = true;
228  }
229 
230  if (solution != nullptr)
231  {
232  lastGoalMotion_ = solution;
233 
234  /* construct the solution path */
235  std::vector<Motion *> mpath;
236  while (solution != nullptr)
237  {
238  mpath.push_back(solution);
239  solution = solution->parent;
240  }
241 
242  /* set the solution path */
243  auto path(std::make_shared<PathControl>(si_));
244  for (int i = mpath.size() - 1; i >= 0; --i)
245  if (mpath[i]->parent)
246  path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
247  else
248  path->append(mpath[i]->state);
249  solved = true;
250  pdef_->addSolutionPath(path, approximate, approxdif, getName());
251  }
252 
253  if (rmotion->state)
254  si_->freeState(rmotion->state);
255  if (rmotion->control)
256  siC_->freeControl(rmotion->control);
257  delete rmotion;
258  si_->freeState(xstate);
259 
260  OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
261 
262  return base::PlannerStatus(solved, approximate);
263 }
264 
266 {
267  Planner::getPlannerData(data);
268 
269  std::vector<Motion *> motions;
270  if (nn_)
271  nn_->list(motions);
272 
273  double delta = siC_->getPropagationStepSize();
274 
275  if (lastGoalMotion_)
276  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
277 
278  for (auto m : motions)
279  {
280  if (m->parent)
281  {
282  if (data.hasControls())
283  data.addEdge(base::PlannerDataVertex(m->parent->state), base::PlannerDataVertex(m->state),
284  control::PlannerDataEdgeControl(m->control, m->steps * delta));
285  else
286  data.addEdge(base::PlannerDataVertex(m->parent->state), base::PlannerDataVertex(m->state));
287  }
288  else
289  data.addStartVertex(base::PlannerDataVertex(m->state));
290  }
291 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:203
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
Definition of an abstract control.
Definition: Control.h:47
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:62
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRT.cpp:56
base::State * state
The state contained by the motion.
Definition: RRT.h:147
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:78
bool getIntermediateStates() const
Return true if the intermediate states generated along motions are to be added to the tree itself...
Definition: RRT.h:101
void setIntermediateStates(bool addIntermediateStates)
Specify whether the intermediate states generated along motions are to be added to the tree itself...
Definition: RRT.h:108
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.
Representation of a motion.
Definition: RRT.h:133
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Control * control
The control contained by the motion.
Definition: RRT.h:150
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp: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...
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
A shared pointer wrapper for ompl::control::SpaceInformation.
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continue solving for some amount of time. Return true if solution was found.
Definition: RRT.cpp:95
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
void setGoalBias(double goalBias)
Definition: RRT.h:88
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: RRT.cpp:265
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition: RRT.h:175
double getGoalBias() const
Get the goal bias the planner is using.
Definition: RRT.h:94
void clear() override
Clear datastructures. Call this function if the input data to the planner has changed and you do not ...
Definition: RRT.cpp:67
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68