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);
52  Planner::declareParam<double>("goal_bias", this, &EST::setGoalBias, &EST::getGoalBias);
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  logError("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  logInform("Starting with %u states", tree_.size);
125 
126  Motion *solution = NULL;
127  double slndist = std::numeric_limits<double>::infinity();
128  Motion *rmotion = new Motion(siC_);
129  bool solved = false;
130 
131  while (!ptc())
132  {
133  // Select a state to expand the tree from
134  Motion *existing = selectMotion();
135  assert (existing);
136 
137  // sample a random state (with goal biasing) near the state selected for expansion
138  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
139  goal_s->sampleGoal(rmotion->state);
140  else
141  {
142  if (!sampler_->sampleNear(rmotion->state, existing->state, maxDistance_))
143  continue;
144  }
145 
146  // Extend a motion toward the state we just sampled
147  unsigned int duration = controlSampler_->sampleTo(rmotion->control, existing->control,
148  existing->state, rmotion->state);
149 
150  // Propagate the system from the state selected for expansion using the control we
151  // just sampled for the given duration. Save the resulting state into rmotion->state.
152  duration = siC_->propagateWhileValid(existing->state, rmotion->control, duration, rmotion->state);
153 
154  // If the system was propagated for a meaningful amount of time, save into the tree
155  if (duration >= siC_->getMinControlDuration())
156  {
157  // create a motion to the resulting state
158  Motion *motion = new Motion(siC_);
159  si_->copyState(motion->state, rmotion->state);
160  siC_->copyControl(motion->control, rmotion->control);
161  motion->steps = duration;
162  motion->parent = existing;
163 
164  // save the state
165  addMotion(motion);
166 
167  // Check if this state is the goal state, or improves the best solution so far
168  double dist = 0.0;
169  solved = goal->isSatisfied(motion->state, &dist);
170  if (solved || dist < slndist)
171  {
172  slndist = dist;
173  solution = motion;
174 
175  if (solved)
176  break;
177  }
178  }
179  }
180 
181  bool addedSolution = false;
182 
183  // Constructing the solution path
184  if (solution != NULL)
185  {
186  lastGoalMotion_ = solution;
187 
188  std::vector<Motion*> mpath;
189  while (solution != NULL)
190  {
191  mpath.push_back(solution);
192  solution = solution->parent;
193  }
194 
195  PathControl *path = new PathControl(si_);
196  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
197  if (mpath[i]->parent)
198  path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
199  else
200  path->append(mpath[i]->state);
201  addedSolution = true;
202  pdef_->addSolutionPath(base::PathPtr(path), !solved, slndist);
203  }
204 
205  // Cleaning up memory
206  if (rmotion->state)
207  si_->freeState(rmotion->state);
208  if (rmotion->control)
209  siC_->freeControl(rmotion->control);
210  delete rmotion;
211 
212  logInform("Created %u states in %u cells", tree_.size, tree_.grid.size());
213 
215 }
216 
218 {
219  GridCell* cell = pdf_.sample(rng_.uniform01());
220  return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
221 }
222 
224 {
226  projectionEvaluator_->computeCoordinates(motion->state, coord);
227  GridCell* cell = tree_.grid.getCell(coord);
228  if (cell)
229  {
230  cell->data.push_back(motion);
231  pdf_.update(cell->data.elem_, 1.0/cell->data.size());
232  }
233  else
234  {
235  cell = tree_.grid.createCell(coord);
236  cell->data.push_back(motion);
237  tree_.grid.add(cell);
238  cell->data.elem_ = pdf_.add(cell, 1.0);
239  }
240  tree_.size++;
241 }
242 
244 {
245  Planner::getPlannerData(data);
246 
247  std::vector<MotionInfo> motions;
248  tree_.grid.getContent(motions);
249 
250  double stepSize = siC_->getPropagationStepSize();
251 
252  if (lastGoalMotion_)
253  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
254 
255  for (unsigned int i = 0 ; i < motions.size() ; ++i)
256  for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
257  {
258  if (motions[i][j]->parent)
259  {
260  if (data.hasControls())
261  data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state),
262  base::PlannerDataVertex (motions[i][j]->state),
263  PlannerDataEdgeControl(motions[i][j]->control, motions[i][j]->steps * stepSize));
264  else
265  data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state),
266  base::PlannerDataVertex (motions[i][j]->state));
267  }
268  else
269  data.addStartVertex (base::PlannerDataVertex (motions[i][j]->state));
270  }
271 }