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  siC_ = si.get();
47 
48  Planner::declareParam<double>("range", this, &EST::setRange, &EST::getRange, "0.:1.:10000.");
49  Planner::declareParam<double>("goal_bias", this, &EST::setGoalBias, &EST::getGoalBias, "0.:.05:1.");
50 }
51 
52 ompl::control::EST::~EST()
53 {
54  freeMemory();
55 }
56 
58 {
59  Planner::setup();
60  tools::SelfConfig sc(si_, getName());
61  sc.configureProjectionEvaluator(projectionEvaluator_);
62  sc.configurePlannerRange(maxDistance_);
63 
64  tree_.grid.setDimension(projectionEvaluator_->getDimension());
65 }
66 
68 {
69  Planner::clear();
70  sampler_.reset();
71  controlSampler_.reset();
72  freeMemory();
73  tree_.grid.clear();
74  tree_.size = 0;
75  pdf_.clear();
76  lastGoalMotion_ = nullptr;
77 }
78 
80 {
81  for (const auto &it : tree_.grid)
82  {
83  for (const auto &motion : it.second->data.motions_)
84  {
85  if (motion->state)
86  si_->freeState(motion->state);
87  if (motion->control)
88  siC_->freeControl(motion->control);
89  delete motion;
90  }
91  }
92 }
93 
95 {
96  checkValidity();
97  base::Goal *goal = pdef_->getGoal().get();
98  auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
99 
100  // Initializing tree with start state(s)
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  addMotion(motion);
107  }
108 
109  if (tree_.grid.size() == 0)
110  {
111  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
113  }
114 
115  // Ensure that we have a state sampler AND a control sampler
116  if (!sampler_)
117  sampler_ = si_->allocValidStateSampler();
118  if (!controlSampler_)
119  controlSampler_ = siC_->allocDirectedControlSampler();
120 
121  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), tree_.size);
122 
123  Motion *solution = nullptr;
124  Motion *approxsol = nullptr;
125  double approxdif = std::numeric_limits<double>::infinity();
126  auto *rmotion = new Motion(siC_);
127  bool solved = false;
128 
129  while (!ptc)
130  {
131  // Select a state to expand the tree from
132  Motion *existing = selectMotion();
133  assert(existing);
134 
135  // sample a random state (with goal biasing) near the state selected for expansion
136  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
137  goal_s->sampleGoal(rmotion->state);
138  else
139  {
140  if (!sampler_->sampleNear(rmotion->state, existing->state, maxDistance_))
141  continue;
142  }
143 
144  // Extend a motion toward the state we just sampled
145  unsigned int duration =
146  controlSampler_->sampleTo(rmotion->control, existing->control, existing->state, rmotion->state);
147 
148  // If the system was propagated for a meaningful amount of time, save into the tree
149  if (duration >= siC_->getMinControlDuration())
150  {
151  // create a motion to the resulting state
152  auto *motion = new Motion(siC_);
153  si_->copyState(motion->state, rmotion->state);
154  siC_->copyControl(motion->control, rmotion->control);
155  motion->steps = duration;
156  motion->parent = existing;
157 
158  // save the state
159  addMotion(motion);
160 
161  // Check if this state is the goal state, or improves the best solution so far
162  double dist = 0.0;
163  solved = goal->isSatisfied(motion->state, &dist);
164  if (solved)
165  {
166  approxdif = dist;
167  solution = motion;
168  break;
169  }
170  if (dist < approxdif)
171  {
172  approxdif = dist;
173  approxsol = motion;
174  }
175  }
176  }
177 
178  bool approximate = false;
179  if (solution == nullptr)
180  {
181  solution = approxsol;
182  approximate = true;
183  }
184 
185  // Constructing the solution path
186  if (solution != nullptr)
187  {
188  lastGoalMotion_ = solution;
189 
190  std::vector<Motion *> mpath;
191  while (solution != nullptr)
192  {
193  mpath.push_back(solution);
194  solution = solution->parent;
195  }
196 
197  auto path(std::make_shared<PathControl>(si_));
198  for (int i = mpath.size() - 1; i >= 0; --i)
199  if (mpath[i]->parent)
200  path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
201  else
202  path->append(mpath[i]->state);
203  solved = true;
204  pdef_->addSolutionPath(path, approximate, approxdif, getName());
205  }
206 
207  // Cleaning up memory
208  if (rmotion->state)
209  si_->freeState(rmotion->state);
210  if (rmotion->control)
211  siC_->freeControl(rmotion->control);
212  delete rmotion;
213 
214  OMPL_INFORM("%s: Created %u states in %u cells", getName().c_str(), tree_.size, tree_.grid.size());
215 
216  return {solved, approximate};
217 }
218 
220 {
221  GridCell *cell = pdf_.sample(rng_.uniform01());
222  return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : nullptr;
223 }
224 
226 {
227  Grid<MotionInfo>::Coord coord(projectionEvaluator_->getDimension());
228  projectionEvaluator_->computeCoordinates(motion->state, coord);
229  GridCell *cell = tree_.grid.getCell(coord);
230  if (cell)
231  {
232  cell->data.push_back(motion);
233  pdf_.update(cell->data.elem_, 1.0 / cell->data.size());
234  }
235  else
236  {
237  cell = tree_.grid.createCell(coord);
238  cell->data.push_back(motion);
239  tree_.grid.add(cell);
240  cell->data.elem_ = pdf_.add(cell, 1.0);
241  }
242  tree_.size++;
243 }
244 
246 {
247  Planner::getPlannerData(data);
248 
249  std::vector<MotionInfo> motionInfo;
250  tree_.grid.getContent(motionInfo);
251 
252  double stepSize = siC_->getPropagationStepSize();
253 
254  if (lastGoalMotion_)
255  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
256 
257  for (auto &mi : motionInfo)
258  for (auto &motion : mi.motions_)
259  {
260  if (motion->parent)
261  {
262  if (data.hasControls())
263  data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state),
264  PlannerDataEdgeControl(motion->control, motion->steps * stepSize));
265  else
266  data.addEdge(base::PlannerDataVertex(motion->parent->state),
267  base::PlannerDataVertex(motion->state));
268  }
269  else
270  data.addStartVertex(base::PlannerDataVertex(motion->state));
271  }
272 }
Eigen::VectorXi Coord
Definition of a coordinate within this grid.
Definition: Grid.h:55
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
Definition: Goal.h:63
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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...
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...
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...
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance.
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:429
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
Representation of a motion.
Definition: EST.h:149
base::State * state
The state contained by the motion.
Definition: EST.h:162
Control * control
The control contained by the motion.
Definition: EST.h:165
Motion * parent
The parent motion in the exploration tree.
Definition: EST.h:171
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition: EST.h:237
void freeMemory()
Free the memory allocated by this planner.
Definition: EST.cpp:79
double getGoalBias() const
Get the goal bias the planner is using.
Definition: EST.h:98
void addMotion(Motion *motion)
Add a motion to the exploration tree.
Definition: EST.cpp:225
double getRange() const
Get the range the planner is using.
Definition: EST.h:114
EST(const SpaceInformationPtr &si)
Constructor.
Definition: EST.cpp:43
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: EST.cpp:94
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: EST.h:108
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: EST.cpp:57
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: EST.cpp:67
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: EST.cpp:245
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards,...
Definition: EST.h:92
Motion * selectMotion()
Select a motion to continue the expansion of the tree from.
Definition: EST.cpp:219
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:61
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:231
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Definition of a cell in this grid.
Definition: Grid.h:59
_T data
The data we store in the cell.
Definition: Grid.h:61
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:202
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56