All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
EST.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
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 Rice University 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: Ryan Luna */
36 
37 #include "ompl/control/planners/est/EST.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <limits>
41 #include <cassert>
42 
43 ompl::control::EST::EST(const SpaceInformationPtr &si) : base::Planner(si, "EST")
44 {
46  goalBias_ = 0.05;
47  maxDistance_ = 0.0;
48  siC_ = si.get();
49  lastGoalMotion_ = NULL;
50 
51  Planner::declareParam<double>("range", this, &EST::setRange, &EST::getRange, "0.:1.:10000.");
52  Planner::declareParam<double>("goal_bias", this, &EST::setGoalBias, &EST::getGoalBias, "0.:.05:1.");
53 }
54 
55 ompl::control::EST::~EST(void)
56 {
57  freeMemory();
58 }
59 
61 {
62  Planner::setup();
63  tools::SelfConfig sc(si_, getName());
64  sc.configureProjectionEvaluator(projectionEvaluator_);
65  sc.configurePlannerRange(maxDistance_);
66 
67  tree_.grid.setDimension(projectionEvaluator_->getDimension());
68 }
69 
71 {
72  Planner::clear();
73  sampler_.reset();
74  controlSampler_.reset();
75  freeMemory();
76  tree_.grid.clear();
77  tree_.size = 0;
78  pdf_.clear ();
79  lastGoalMotion_ = NULL;
80 }
81 
83 {
84  for (Grid<MotionInfo>::iterator it = tree_.grid.begin(); it != tree_.grid.end() ; ++it)
85  {
86  for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
87  {
88  if (it->second->data[i]->state)
89  si_->freeState(it->second->data[i]->state);
90  if (it->second->data[i]->control)
91  siC_->freeControl(it->second->data[i]->control);
92  delete it->second->data[i];
93  }
94  }
95 }
96 
98 {
99  checkValidity();
100  base::Goal *goal = pdef_->getGoal().get();
101  base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
102 
103  // Initializing tree with start state(s)
104  while (const base::State *st = pis_.nextStart())
105  {
106  Motion *motion = new Motion(siC_);
107  si_->copyState(motion->state, st);
108  siC_->nullControl(motion->control);
109  addMotion(motion);
110  }
111 
112  if (tree_.grid.size() == 0)
113  {
114  OMPL_ERROR("There are no valid initial states!");
116  }
117 
118  // Ensure that we have a state sampler AND a control sampler
119  if (!sampler_)
120  sampler_ = si_->allocValidStateSampler();
121  if (!controlSampler_)
122  controlSampler_ = siC_->allocDirectedControlSampler();
123 
124  OMPL_INFORM("Starting with %u states", tree_.size);
125 
126  Motion *solution = NULL;
127  Motion *approxsol = NULL;
128  double approxdif = std::numeric_limits<double>::infinity();
129  Motion *rmotion = new Motion(siC_);
130  bool solved = false;
131 
132  while (!ptc)
133  {
134  // Select a state to expand the tree from
135  Motion *existing = selectMotion();
136  assert (existing);
137 
138  // sample a random state (with goal biasing) near the state selected for expansion
139  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
140  goal_s->sampleGoal(rmotion->state);
141  else
142  {
143  if (!sampler_->sampleNear(rmotion->state, existing->state, maxDistance_))
144  continue;
145  }
146 
147  // Extend a motion toward the state we just sampled
148  unsigned int duration = controlSampler_->sampleTo(rmotion->control, existing->control,
149  existing->state, rmotion->state);
150 
151  // Propagate the system from the state selected for expansion using the control we
152  // just sampled for the given duration. Save the resulting state into rmotion->state.
153  duration = siC_->propagateWhileValid(existing->state, rmotion->control, duration, rmotion->state);
154 
155  // If the system was propagated for a meaningful amount of time, save into the tree
156  if (duration >= siC_->getMinControlDuration())
157  {
158  // create a motion to the resulting state
159  Motion *motion = new Motion(siC_);
160  si_->copyState(motion->state, rmotion->state);
161  siC_->copyControl(motion->control, rmotion->control);
162  motion->steps = duration;
163  motion->parent = existing;
164 
165  // save the state
166  addMotion(motion);
167 
168  // Check if this state is the goal state, or improves the best solution so far
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 
185  bool approximate = false;
186  if (solution == NULL)
187  {
188  solution = approxsol;
189  approximate = true;
190  }
191 
192  // Constructing the solution path
193  if (solution != NULL)
194  {
195  lastGoalMotion_ = solution;
196 
197  std::vector<Motion*> mpath;
198  while (solution != NULL)
199  {
200  mpath.push_back(solution);
201  solution = solution->parent;
202  }
203 
204  PathControl *path = new PathControl(si_);
205  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
206  if (mpath[i]->parent)
207  path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
208  else
209  path->append(mpath[i]->state);
210  solved = true;
211  pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif);
212  }
213 
214  // Cleaning up memory
215  if (rmotion->state)
216  si_->freeState(rmotion->state);
217  if (rmotion->control)
218  siC_->freeControl(rmotion->control);
219  delete rmotion;
220 
221  OMPL_INFORM("Created %u states in %u cells", tree_.size, tree_.grid.size());
222 
223  return base::PlannerStatus(solved, approximate);
224 }
225 
227 {
228  GridCell* cell = pdf_.sample(rng_.uniform01());
229  return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
230 }
231 
233 {
235  projectionEvaluator_->computeCoordinates(motion->state, coord);
236  GridCell* cell = tree_.grid.getCell(coord);
237  if (cell)
238  {
239  cell->data.push_back(motion);
240  pdf_.update(cell->data.elem_, 1.0/cell->data.size());
241  }
242  else
243  {
244  cell = tree_.grid.createCell(coord);
245  cell->data.push_back(motion);
246  tree_.grid.add(cell);
247  cell->data.elem_ = pdf_.add(cell, 1.0);
248  }
249  tree_.size++;
250 }
251 
253 {
254  Planner::getPlannerData(data);
255 
256  std::vector<MotionInfo> motions;
257  tree_.grid.getContent(motions);
258 
259  double stepSize = siC_->getPropagationStepSize();
260 
261  if (lastGoalMotion_)
262  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
263 
264  for (unsigned int i = 0 ; i < motions.size() ; ++i)
265  for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
266  {
267  if (motions[i][j]->parent)
268  {
269  if (data.hasControls())
270  data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state),
271  base::PlannerDataVertex (motions[i][j]->state),
272  PlannerDataEdgeControl(motions[i][j]->control, motions[i][j]->steps * stepSize));
273  else
274  data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state),
275  base::PlannerDataVertex (motions[i][j]->state));
276  }
277  else
278  data.addStartVertex (base::PlannerDataVertex (motions[i][j]->state));
279  }
280 }