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