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 {
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();
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
base::ProjectionEvaluatorPtr projectionEvaluator_
The employed projection evaluator.
Definition: LBKPIECE1.h:212
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
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:197
base::State * state
The state contained by this motion.
Definition: LBKPIECE1.h:183
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:264
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: LBKPIECE1.h:234
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 maxDistance_
The maximum length of a motion to be added to a tree.
Definition: LBKPIECE1.h:228
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:175
double getMinValidPathFraction() const
Get the value of the fraction set by setMinValidPathFraction()
Definition: LBKPIECE1.h:152
double minValidPathFraction_
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.
Definition: LBKPIECE1.h:225
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
Discretization< Motion > dStart_
The start tree.
Definition: LBKPIECE1.h:215
Discretization< Motion > dGoal_
The goal tree.
Definition: LBKPIECE1.h:218
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
base::StateSamplerPtr sampler_
The employed state sampler.
Definition: LBKPIECE1.h:209
Definition of an abstract state.
Definition: State.h:49
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:101
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: LBKPIECE1.cpp:309
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
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
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
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
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
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
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:81
RNG rng_
The random number generator.
Definition: LBKPIECE1.h:231
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