LBKPIECE1.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/kpiece/LBKPIECE1.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <cassert>
41 
42 ompl::geometric::LBKPIECE1::LBKPIECE1(const base::SpaceInformationPtr &si)
43  : base::Planner(si, "LBKPIECE1")
44  , dStart_([this](Motion *m) { freeMotion(m); })
45  , dGoal_([this](Motion *m) { freeMotion(m); })
46 {
47  specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
48 
49  Planner::declareParam<double>("range", this, &LBKPIECE1::setRange, &LBKPIECE1::getRange, "0.:1.:10000");
50  Planner::declareParam<double>("border_fraction", this, &LBKPIECE1::setBorderFraction, &LBKPIECE1::getBorderFraction,
51  "0.:.05:1.");
52  Planner::declareParam<double>("min_valid_path_fraction", this, &LBKPIECE1::setMinValidPathFraction,
54 }
55 
56 ompl::geometric::LBKPIECE1::~LBKPIECE1() = default;
57 
59 {
60  Planner::setup();
61  tools::SelfConfig sc(si_, getName());
62  sc.configureProjectionEvaluator(projectionEvaluator_);
63  sc.configurePlannerRange(maxDistance_);
64 
65  if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
66  throw Exception("The minimum valid path fraction must be in the range (0,1]");
67 
68  dStart_.setDimension(projectionEvaluator_->getDimension());
69  dGoal_.setDimension(projectionEvaluator_->getDimension());
70 }
71 
73 {
74  checkValidity();
75  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
76 
77  if (goal == nullptr)
78  {
79  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
81  }
82 
83  Discretization<Motion>::Coord xcoord(projectionEvaluator_->getDimension());
84 
85  while (const base::State *st = pis_.nextStart())
86  {
87  auto *motion = new Motion(si_);
88  si_->copyState(motion->state, st);
89  motion->root = st;
90  motion->valid = true;
91  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
92  dStart_.addMotion(motion, xcoord);
93  }
94 
95  if (dStart_.getMotionCount() == 0)
96  {
97  OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
99  }
100 
101  if (!goal->couldSample())
102  {
103  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
105  }
106 
107  if (!sampler_)
108  sampler_ = si_->allocStateSampler();
109 
110  OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
111  (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
112 
113  base::State *xstate = si_->allocState();
114  bool startTree = true;
115  bool solved = false;
116 
117  while (!ptc)
118  {
119  Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
120  startTree = !startTree;
121  Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
122  disc.countIteration();
123 
124  // if we have not sampled too many goals already
125  if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
126  {
127  const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
128  if (st != nullptr)
129  {
130  auto *motion = new Motion(si_);
131  si_->copyState(motion->state, st);
132  motion->root = motion->state;
133  motion->valid = true;
134  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
135  dGoal_.addMotion(motion, xcoord);
136  }
137  if (dGoal_.getMotionCount() == 0)
138  {
139  OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
140  break;
141  }
142  }
143 
144  Discretization<Motion>::Cell *ecell = nullptr;
145  Motion *existing = nullptr;
146  disc.selectMotion(existing, ecell);
147  assert(existing);
148  sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
149 
150  /* create a motion */
151  auto *motion = new Motion(si_);
152  si_->copyState(motion->state, xstate);
153  motion->parent = existing;
154  motion->root = existing->root;
155  existing->children.push_back(motion);
156  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
157  disc.addMotion(motion, xcoord);
158 
159  /* attempt to connect trees */
160  Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord);
161  if ((ocell != nullptr) && !ocell->data->motions.empty())
162  {
163  Motion *connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];
164 
165  if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root,
166  startTree ? motion->root : connectOther->root))
167  {
168  auto *connect = new Motion(si_);
169  si_->copyState(connect->state, connectOther->state);
170  connect->parent = motion;
171  connect->root = motion->root;
172  motion->children.push_back(connect);
173  projectionEvaluator_->computeCoordinates(connect->state, xcoord);
174  disc.addMotion(connect, xcoord);
175 
176  if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
177  {
178  if (startTree)
179  connectionPoint_ = std::make_pair(connectOther->state, motion->state);
180  else
181  connectionPoint_ = std::make_pair(motion->state, connectOther->state);
182 
183  /* extract the motions and put them in solution vector */
184 
185  std::vector<Motion *> mpath1;
186  while (motion != nullptr)
187  {
188  mpath1.push_back(motion);
189  motion = motion->parent;
190  }
191 
192  std::vector<Motion *> mpath2;
193  while (connectOther != nullptr)
194  {
195  mpath2.push_back(connectOther);
196  connectOther = connectOther->parent;
197  }
198 
199  if (startTree)
200  mpath1.swap(mpath2);
201 
202  auto path(std::make_shared<PathGeometric>(si_));
203  path->getStates().reserve(mpath1.size() + mpath2.size());
204  for (int i = mpath1.size() - 1; i >= 0; --i)
205  path->append(mpath1[i]->state);
206  for (auto &i : mpath2)
207  path->append(i->state);
208 
209  pdef_->addSolutionPath(path, false, 0.0, getName());
210  solved = true;
211  break;
212  }
213  }
214  }
215  }
216 
217  si_->freeState(xstate);
218 
219  OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on "
220  "boundary))",
221  getName().c_str(), dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(),
222  dGoal_.getMotionCount(), dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(),
223  dStart_.getGrid().countExternal(), dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
224 
226 }
227 
229 {
230  std::vector<Motion *> mpath;
231 
232  /* construct the solution path */
233  while (motion != nullptr)
234  {
235  mpath.push_back(motion);
236  motion = motion->parent;
237  }
238 
239  std::pair<base::State *, double> lastValid;
240  lastValid.first = temp;
241 
242  /* check the path */
243  for (int i = mpath.size() - 1; i >= 0; --i)
244  if (!mpath[i]->valid)
245  {
246  if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid))
247  mpath[i]->valid = true;
248  else
249  {
250  Motion *parent = mpath[i]->parent;
251  removeMotion(disc, mpath[i]);
252 
253  // add the valid part of the path, if sufficiently long
254  if (lastValid.second > minValidPathFraction_)
255  {
256  auto *reAdd = new Motion(si_);
257  si_->copyState(reAdd->state, lastValid.first);
258  reAdd->parent = parent;
259  reAdd->root = parent->root;
260  parent->children.push_back(reAdd);
261  reAdd->valid = true;
262  Discretization<Motion>::Coord coord(projectionEvaluator_->getDimension());
263  projectionEvaluator_->computeCoordinates(reAdd->state, coord);
264  disc.addMotion(reAdd, coord);
265  }
266 
267  return false;
268  }
269  }
270  return true;
271 }
272 
274 {
275  /* remove from grid */
276 
277  Discretization<Motion>::Coord coord(projectionEvaluator_->getDimension());
278  projectionEvaluator_->computeCoordinates(motion->state, coord);
279  disc.removeMotion(motion, coord);
280 
281  /* remove self from parent list */
282 
283  if (motion->parent != nullptr)
284  {
285  for (unsigned int i = 0; i < motion->parent->children.size(); ++i)
286  if (motion->parent->children[i] == motion)
287  {
288  motion->parent->children.erase(motion->parent->children.begin() + i);
289  break;
290  }
291  }
292 
293  /* remove children */
294  for (auto &i : motion->children)
295  {
296  i->parent = nullptr;
297  removeMotion(disc, i);
298  }
299 
300  freeMotion(motion);
301 }
302 
304 {
305  if (motion->state != nullptr)
306  si_->freeState(motion->state);
307  delete motion;
308 }
309 
311 {
312  Planner::clear();
313 
314  sampler_.reset();
315  dStart_.clear();
316  dGoal_.clear();
317  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
318 }
319 
321 {
322  Planner::getPlannerData(data);
323  dStart_.getPlannerData(data, 1, true, nullptr);
324  dGoal_.getPlannerData(data, 2, false, nullptr);
325 
326  // Insert the edge connecting the two trees
327  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
328 }
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
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: LBKPIECE1.cpp:320
bool isPathValid(Discretization< Motion > &disc, Motion *motion, base::State *temp)
Since solutions are computed in a lazy fashion, once trees are connected, the solution found needs to...
Definition: LBKPIECE1.cpp:228
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 getMinValidPathFraction() const
Get the value of the fraction set by setMinValidPathFraction()
Definition: LBKPIECE1.h:248
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: LBKPIECE1.h:288
const base::State * root
The root state (start state) that leads to this motion.
Definition: LBKPIECE1.h:276
typename Grid::Cell Cell
The datatype for the maintained grid cells.
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: LBKPIECE1.h:207
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
unsigned int addMotion(Motion *motion, const Coord &coord, double dist=0.0)
Add a motion to the grid containing motions. As a hint, dist specifies the distance to the goal from ...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LBKPIECE1.cpp:310
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: LBKPIECE1.cpp:72
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
@ INVALID_GOAL
Invalid goal state.
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
A class to store the exit status of Planner::solve()
double getRange() const
Get the range the planner is using.
Definition: LBKPIECE1.h:213
void setMinValidPathFraction(double fraction)
When extending a motion, the planner can decide to keep the first valid part of it,...
Definition: LBKPIECE1.h:242
void selectMotion(Motion *&smotion, Cell *&scell)
Select a motion and the cell it is part of from the grid of motions. This is where preference is give...
void setBorderFraction(double bp)
Set the fraction of time for focusing on the border (between 0 and 1). This is the minimum fraction u...
Definition: LBKPIECE1.h:223
typename Grid::Coord Coord
The datatype for the maintained grid coordinates.
LBKPIECE1(const base::SpaceInformationPtr &si)
Constructor.
Definition: LBKPIECE1.cpp:42
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:152
base::State * state
The state contained by this motion.
Definition: LBKPIECE1.h:279
@ EXACT_SOLUTION
The planner found an exact solution.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LBKPIECE1.cpp:58
Motion * parent
The parent motion in the exploration tree.
Definition: LBKPIECE1.h:282
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
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
Definition: LBKPIECE1.h:231
void removeMotion(Discretization< Motion > &disc, Motion *motion)
Remove a motion from a tree of motions.
Definition: LBKPIECE1.cpp:273
One-level discretization used for KPIECE.
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...
Abstract definition of a goal region that can be sampled.
The exception type for ompl.
Definition: Exception.h:78
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: LBKPIECE1.cpp:303
@ INVALID_START
Invalid start state or no start state specified.
Representation of a motion for this algorithm.
Definition: LBKPIECE1.h:262