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