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 
56  maxDistance_ = 0.0;
57  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
58 
59  Planner::declareParam<double>("range", this, &LBKPIECE1::setRange, &LBKPIECE1::getRange, "0.:1.:10000");
60  Planner::declareParam<double>("border_fraction", this, &LBKPIECE1::setBorderFraction, &LBKPIECE1::getBorderFraction,
61  "0.:.05:1.");
62  Planner::declareParam<double>("min_valid_path_fraction", this, &LBKPIECE1::setMinValidPathFraction,
64 }
65 
66 ompl::geometric::LBKPIECE1::~LBKPIECE1() = default;
67 
69 {
70  Planner::setup();
74 
75  if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
76  throw Exception("The minimum valid path fraction must be in the range (0,1]");
77 
78  dStart_.setDimension(projectionEvaluator_->getDimension());
79  dGoal_.setDimension(projectionEvaluator_->getDimension());
80 }
81 
83 {
84  checkValidity();
85  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
86 
87  if (!goal)
88  {
89  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
91  }
92 
94 
95  while (const base::State *st = pis_.nextStart())
96  {
97  auto *motion = new Motion(si_);
98  si_->copyState(motion->state, st);
99  motion->root = st;
100  motion->valid = true;
101  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
102  dStart_.addMotion(motion, xcoord);
103  }
104 
105  if (dStart_.getMotionCount() == 0)
106  {
107  OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
109  }
110 
111  if (!goal->couldSample())
112  {
113  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
115  }
116 
117  if (!sampler_)
118  sampler_ = si_->allocStateSampler();
119 
120  OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
121  (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
122 
123  base::State *xstate = si_->allocState();
124  bool startTree = true;
125  bool solved = false;
126 
127  while (ptc == false)
128  {
129  Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
130  startTree = !startTree;
131  Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
132  disc.countIteration();
133 
134  // if we have not sampled too many goals already
135  if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
136  {
137  const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
138  if (st)
139  {
140  auto *motion = new Motion(si_);
141  si_->copyState(motion->state, st);
142  motion->root = motion->state;
143  motion->valid = true;
144  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
145  dGoal_.addMotion(motion, xcoord);
146  }
147  if (dGoal_.getMotionCount() == 0)
148  {
149  OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
150  break;
151  }
152  }
153 
154  Discretization<Motion>::Cell *ecell = nullptr;
155  Motion *existing = nullptr;
156  disc.selectMotion(existing, ecell);
157  assert(existing);
158  sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
159 
160  /* create a motion */
161  auto *motion = new Motion(si_);
162  si_->copyState(motion->state, xstate);
163  motion->parent = existing;
164  motion->root = existing->root;
165  existing->children.push_back(motion);
166  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
167  disc.addMotion(motion, xcoord);
168 
169  /* attempt to connect trees */
170  Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord);
171  if (ocell && !ocell->data->motions.empty())
172  {
173  Motion *connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];
174 
175  if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root,
176  startTree ? motion->root : connectOther->root))
177  {
178  auto *connect = new Motion(si_);
179  si_->copyState(connect->state, connectOther->state);
180  connect->parent = motion;
181  connect->root = motion->root;
182  motion->children.push_back(connect);
183  projectionEvaluator_->computeCoordinates(connect->state, xcoord);
184  disc.addMotion(connect, xcoord);
185 
186  if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
187  {
188  if (startTree)
189  connectionPoint_ = std::make_pair(connectOther->state, motion->state);
190  else
191  connectionPoint_ = std::make_pair(motion->state, connectOther->state);
192 
193  /* extract the motions and put them in solution vector */
194 
195  std::vector<Motion *> mpath1;
196  while (motion != nullptr)
197  {
198  mpath1.push_back(motion);
199  motion = motion->parent;
200  }
201 
202  std::vector<Motion *> mpath2;
203  while (connectOther != nullptr)
204  {
205  mpath2.push_back(connectOther);
206  connectOther = connectOther->parent;
207  }
208 
209  if (startTree)
210  mpath1.swap(mpath2);
211 
212  auto path(std::make_shared<PathGeometric>(si_));
213  path->getStates().reserve(mpath1.size() + mpath2.size());
214  for (int i = mpath1.size() - 1; i >= 0; --i)
215  path->append(mpath1[i]->state);
216  for (auto &i : mpath2)
217  path->append(i->state);
218 
219  pdef_->addSolutionPath(path, false, 0.0, getName());
220  solved = true;
221  break;
222  }
223  }
224  }
225  }
226 
227  si_->freeState(xstate);
228 
229  OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on "
230  "boundary))",
231  getName().c_str(), dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(),
232  dGoal_.getMotionCount(), dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(),
233  dStart_.getGrid().countExternal(), dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
234 
236 }
237 
239 {
240  std::vector<Motion *> mpath;
241 
242  /* construct the solution path */
243  while (motion != nullptr)
244  {
245  mpath.push_back(motion);
246  motion = motion->parent;
247  }
248 
249  std::pair<base::State *, double> lastValid;
250  lastValid.first = temp;
251 
252  /* check the path */
253  for (int i = mpath.size() - 1; i >= 0; --i)
254  if (!mpath[i]->valid)
255  {
256  if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid))
257  mpath[i]->valid = true;
258  else
259  {
260  Motion *parent = mpath[i]->parent;
261  removeMotion(disc, mpath[i]);
262 
263  // add the valid part of the path, if sufficiently long
264  if (lastValid.second > minValidPathFraction_)
265  {
266  auto *reAdd = new Motion(si_);
267  si_->copyState(reAdd->state, lastValid.first);
268  reAdd->parent = parent;
269  reAdd->root = parent->root;
270  parent->children.push_back(reAdd);
271  reAdd->valid = true;
273  projectionEvaluator_->computeCoordinates(reAdd->state, coord);
274  disc.addMotion(reAdd, coord);
275  }
276 
277  return false;
278  }
279  }
280  return true;
281 }
282 
284 {
285  /* remove from grid */
286 
288  projectionEvaluator_->computeCoordinates(motion->state, coord);
289  disc.removeMotion(motion, coord);
290 
291  /* remove self from parent list */
292 
293  if (motion->parent)
294  {
295  for (unsigned int i = 0; i < motion->parent->children.size(); ++i)
296  if (motion->parent->children[i] == motion)
297  {
298  motion->parent->children.erase(motion->parent->children.begin() + i);
299  break;
300  }
301  }
302 
303  /* remove children */
304  for (auto &i : motion->children)
305  {
306  i->parent = nullptr;
307  removeMotion(disc, i);
308  }
309 
310  freeMotion(motion);
311 }
312 
314 {
315  if (motion->state)
316  si_->freeState(motion->state);
317  delete motion;
318 }
319 
321 {
322  Planner::clear();
323 
324  sampler_.reset();
325  dStart_.clear();
326  dGoal_.clear();
327  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
328 }
329 
331 {
332  Planner::getPlannerData(data);
333  dStart_.getPlannerData(data, 1, true, nullptr);
334  dGoal_.getPlannerData(data, 2, false, nullptr);
335 
336  // Insert the edge connecting the two trees
337  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
338 }
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:214
Motion * parent
The parent motion in the exploration tree.
Definition: LBKPIECE1.h:188
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:82
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:206
virtual bool couldSample() const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
base::State * state
The state contained by this motion.
Definition: LBKPIECE1.h:185
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:269
virtual bool isStartGoalPairValid(const State *, const State *) const
Since there can be multiple starting states (and multiple goal states) it is possible certain pairs a...
Definition: Goal.h:136
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: LBKPIECE1.h:236
void removeMotion(Discretization< Motion > &disc, Motion *motion)
Remove a motion from a tree of motions.
Definition: LBKPIECE1.cpp:283
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:230
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
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:227
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:194
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:217
Discretization< Motion > dGoal_
The goal tree.
Definition: LBKPIECE1.h:220
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:238
base::StateSamplerPtr sampler_
The employed state sampler.
Definition: LBKPIECE1.h:211
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:421
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: LBKPIECE1.cpp:313
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:427
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:230
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:68
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LBKPIECE1.cpp:320
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
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:330
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:233
const base::State * root
The root state (start state) that leads to this motion.
Definition: LBKPIECE1.h:182
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