ProjEST.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/geometric/planners/est/ProjEST.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <limits>
41 #include <cassert>
42 
43 ompl::geometric::ProjEST::ProjEST(const base::SpaceInformationPtr &si) : base::Planner(si, "ProjEST")
44 {
46  specs_.directed = true;
47 
48  Planner::declareParam<double>("range", this, &ProjEST::setRange, &ProjEST::getRange, "0.:1.:10000.");
49  Planner::declareParam<double>("goal_bias", this, &ProjEST::setGoalBias, &ProjEST::getGoalBias, "0.:.05:1.");
50 }
51 
52 ompl::geometric::ProjEST::~ProjEST()
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  freeMemory();
72  tree_.grid.clear();
73  tree_.size = 0;
74  pdf_.clear();
75  lastGoalMotion_ = nullptr;
76 }
77 
79 {
80  for (const auto &it : tree_.grid)
81  {
82  for (auto &motion : it.second->data.motions_)
83  {
84  if (motion->state != nullptr)
85  si_->freeState(motion->state);
86  delete motion;
87  }
88  }
89 }
90 
92 {
93  checkValidity();
94  base::Goal *goal = pdef_->getGoal().get();
95  auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
96 
97  while (const base::State *st = pis_.nextStart())
98  {
99  auto *motion = new Motion(si_);
100  si_->copyState(motion->state, st);
101  addMotion(motion);
102  }
103 
104  if (tree_.grid.empty())
105  {
106  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
108  }
109 
110  if (!sampler_)
111  sampler_ = si_->allocValidStateSampler();
112 
113  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), tree_.size);
114 
115  Motion *solution = nullptr;
116  Motion *approxsol = nullptr;
117  double approxdif = std::numeric_limits<double>::infinity();
118  base::State *xstate = si_->allocState();
119 
120  while (!ptc)
121  {
122  /* Decide on a state to expand from */
123  Motion *existing = selectMotion();
124  assert(existing);
125 
126  /* sample random state (with goal biasing) */
127  if ((goal_s != nullptr) && rng_.uniform01() < goalBias_ && goal_s->canSample())
128  goal_s->sampleGoal(xstate);
129  else if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
130  continue;
131 
132  if (si_->checkMotion(existing->state, xstate))
133  {
134  /* create a motion */
135  auto *motion = new Motion(si_);
136  si_->copyState(motion->state, xstate);
137  motion->parent = existing;
138 
139  addMotion(motion);
140  double dist = 0.0;
141  bool solved = goal->isSatisfied(motion->state, &dist);
142  if (solved)
143  {
144  approxdif = dist;
145  solution = motion;
146  break;
147  }
148  if (dist < approxdif)
149  {
150  approxdif = dist;
151  approxsol = motion;
152  }
153  }
154  }
155 
156  bool solved = false;
157  bool approximate = false;
158  if (solution == nullptr)
159  {
160  solution = approxsol;
161  approximate = true;
162  }
163 
164  if (solution != nullptr)
165  {
166  lastGoalMotion_ = solution;
167 
168  /* construct the solution path */
169  std::vector<Motion *> mpath;
170  while (solution != nullptr)
171  {
172  mpath.push_back(solution);
173  solution = solution->parent;
174  }
175 
176  /* set the solution path */
177  auto path(std::make_shared<PathGeometric>(si_));
178  for (int i = mpath.size() - 1; i >= 0; --i)
179  path->append(mpath[i]->state);
180  pdef_->addSolutionPath(path, approximate, approxdif, getName());
181  solved = true;
182  }
183 
184  si_->freeState(xstate);
185 
186  OMPL_INFORM("%s: Created %u states in %u cells", getName().c_str(), tree_.size, tree_.grid.size());
187 
188  return {solved, approximate};
189 }
190 
192 {
193  GridCell *cell = pdf_.sample(rng_.uniform01());
194  return (cell != nullptr) && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : nullptr;
195 }
196 
198 {
199  Grid<MotionInfo>::Coord coord(projectionEvaluator_->getDimension());
200  projectionEvaluator_->computeCoordinates(motion->state, coord);
201  GridCell *cell = tree_.grid.getCell(coord);
202  if (cell != nullptr)
203  {
204  cell->data.push_back(motion);
205  pdf_.update(cell->data.elem_, 1.0 / cell->data.size());
206  }
207  else
208  {
209  cell = tree_.grid.createCell(coord);
210  cell->data.push_back(motion);
211  tree_.grid.add(cell);
212  cell->data.elem_ = pdf_.add(cell, 1.0);
213  }
214  tree_.size++;
215 }
216 
218 {
219  Planner::getPlannerData(data);
220 
221  std::vector<MotionInfo> motionInfo;
222  tree_.grid.getContent(motionInfo);
223 
224  if (lastGoalMotion_ != nullptr)
225  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
226 
227  for (auto &m : motionInfo)
228  for (auto &motion : m.motions_)
229  {
230  if (motion->parent == nullptr)
231  data.addStartVertex(base::PlannerDataVertex(motion->state));
232  else
233  data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
234  }
235 }
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
double getRange() const
Get the range the planner is using.
Definition: ProjEST.h:209
Definition of an abstract state.
Definition: State.h:113
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:123
double getGoalBias() const
Get the goal bias the planner is using.
Definition: ProjEST.h:193
Motion * selectMotion()
Select a motion to continue the expansion of the tree from.
Definition: ProjEST.cpp:191
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
void addMotion(Motion *motion)
Add a motion to the exploration tree.
Definition: ProjEST.cpp:197
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
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:269
Eigen::VectorXi Coord
Definition of a coordinate within this grid.
Definition: Grid.h:119
A class to store the exit status of Planner::solve()
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: ProjEST.cpp:91
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: ProjEST.cpp:57
Definition of a cell in this grid.
Definition: Grid.h:122
The definition of a motion.
Definition: ProjEST.h:240
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
Abstract definition of goals.
Definition: Goal.h:126
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards,...
Definition: ProjEST.h:187
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: ProjEST.h:203
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: ProjEST.cpp:67
Motion * parent
The parent motion in the exploration tree.
Definition: ProjEST.h:256
void freeMemory()
Free the memory allocated by this planner.
Definition: ProjEST.cpp:78
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: ProjEST.cpp:217
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...
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_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
base::State * state
The state contained by the motion.
Definition: ProjEST.h:253
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...
Abstract definition of a goal region that can be sampled.
_T data
The data we store in the cell.
Definition: Grid.h:125
ProjEST(const base::SpaceInformationPtr &si)
Constructor.
Definition: ProjEST.cpp:43
@ INVALID_START
Invalid start state or no start state specified.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122