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) : base::Planner(si, "LBKPIECE1"),
43  dStart_(boost::bind(&LBKPIECE1::freeMotion, this, _1)),
44  dGoal_(boost::bind(&LBKPIECE1::freeMotion, this, _1))
45 {
47 
49  maxDistance_ = 0.0;
50  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
51 
52  Planner::declareParam<double>("range", this, &LBKPIECE1::setRange, &LBKPIECE1::getRange, "0.:1.:10000");
53  Planner::declareParam<double>("border_fraction", this, &LBKPIECE1::setBorderFraction, &LBKPIECE1::getBorderFraction, "0.:.05:1.");
54  Planner::declareParam<double>("min_valid_path_fraction", this, &LBKPIECE1::setMinValidPathFraction, &LBKPIECE1::getMinValidPathFraction);
55 }
56 
57  ompl::geometric::LBKPIECE1::~LBKPIECE1()
58 {
59 }
60 
62 {
63  Planner::setup();
67 
68  if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
69  throw Exception("The minimum valid path fraction must be in the range (0,1]");
70 
71  dStart_.setDimension(projectionEvaluator_->getDimension());
72  dGoal_.setDimension(projectionEvaluator_->getDimension());
73 }
74 
76 {
77  checkValidity();
78  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
79 
80  if (!goal)
81  {
82  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
84  }
85 
87 
88  while (const base::State *st = pis_.nextStart())
89  {
90  Motion *motion = new Motion(si_);
91  si_->copyState(motion->state, st);
92  motion->root = st;
93  motion->valid = true;
94  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
95  dStart_.addMotion(motion, xcoord);
96  }
97 
98  if (dStart_.getMotionCount() == 0)
99  {
100  OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
102  }
103 
104  if (!goal->couldSample())
105  {
106  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
108  }
109 
110  if (!sampler_)
111  sampler_ = si_->allocStateSampler();
112 
113  OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(), (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
114 
115  base::State *xstate = si_->allocState();
116  bool startTree = true;
117  bool solved = false;
118 
119  while (ptc == false)
120  {
121  Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
122  startTree = !startTree;
123  Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
124  disc.countIteration();
125 
126  // if we have not sampled too many goals already
127  if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
128  {
129  const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
130  if (st)
131  {
132  Motion *motion = new Motion(si_);
133  si_->copyState(motion->state, st);
134  motion->root = motion->state;
135  motion->valid = true;
136  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
137  dGoal_.addMotion(motion, xcoord);
138  }
139  if (dGoal_.getMotionCount() == 0)
140  {
141  OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
142  break;
143  }
144  }
145 
146  Discretization<Motion>::Cell *ecell = NULL;
147  Motion *existing = NULL;
148  disc.selectMotion(existing, ecell);
149  assert(existing);
150  sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
151 
152  /* create a motion */
153  Motion *motion = new Motion(si_);
154  si_->copyState(motion->state, xstate);
155  motion->parent = existing;
156  motion->root = existing->root;
157  existing->children.push_back(motion);
158  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
159  disc.addMotion(motion, xcoord);
160 
161  /* attempt to connect trees */
162  Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord);
163  if (ocell && !ocell->data->motions.empty())
164  {
165  Motion *connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];
166 
167  if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root))
168  {
169  Motion *connect = new Motion(si_);
170  si_->copyState(connect->state, connectOther->state);
171  connect->parent = motion;
172  connect->root = motion->root;
173  motion->children.push_back(connect);
174  projectionEvaluator_->computeCoordinates(connect->state, xcoord);
175  disc.addMotion(connect, xcoord);
176 
177  if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
178  {
179  if (startTree)
180  connectionPoint_ = std::make_pair(connectOther->state, motion->state);
181  else
182  connectionPoint_ = std::make_pair(motion->state, connectOther->state);
183 
184  /* extract the motions and put them in solution vector */
185 
186  std::vector<Motion*> mpath1;
187  while (motion != NULL)
188  {
189  mpath1.push_back(motion);
190  motion = motion->parent;
191  }
192 
193  std::vector<Motion*> mpath2;
194  while (connectOther != NULL)
195  {
196  mpath2.push_back(connectOther);
197  connectOther = connectOther->parent;
198  }
199 
200  if (startTree)
201  mpath1.swap(mpath2);
202 
203  PathGeometric *path = new PathGeometric(si_);
204  path->getStates().reserve(mpath1.size() + mpath2.size());
205  for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
206  path->append(mpath1[i]->state);
207  for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
208  path->append(mpath2[i]->state);
209 
210  pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName());
211  solved = true;
212  break;
213  }
214  }
215  }
216  }
217 
218  si_->freeState(xstate);
219 
220  OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
221  getName().c_str(),
222  dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
223  dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
224  dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
225 
227 }
228 
230 {
231  std::vector<Motion*> mpath;
232 
233  /* construct the solution path */
234  while (motion != NULL)
235  {
236  mpath.push_back(motion);
237  motion = motion->parent;
238  }
239 
240  std::pair<base::State*, double> lastValid;
241  lastValid.first = temp;
242 
243  /* check the path */
244  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
245  if (!mpath[i]->valid)
246  {
247  if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid))
248  mpath[i]->valid = true;
249  else
250  {
251  Motion *parent = mpath[i]->parent;
252  removeMotion(disc, mpath[i]);
253 
254  // add the valid part of the path, if sufficiently long
255  if (lastValid.second > minValidPathFraction_)
256  {
257  Motion *reAdd = new Motion(si_);
258  si_->copyState(reAdd->state, lastValid.first);
259  reAdd->parent = parent;
260  reAdd->root = parent->root;
261  parent->children.push_back(reAdd);
262  reAdd->valid = true;
264  projectionEvaluator_->computeCoordinates(reAdd->state, coord);
265  disc.addMotion(reAdd, coord);
266  }
267 
268  return false;
269  }
270  }
271  return true;
272 }
273 
275 {
276  /* remove from grid */
277 
279  projectionEvaluator_->computeCoordinates(motion->state, coord);
280  disc.removeMotion(motion, coord);
281 
282  /* remove self from parent list */
283 
284  if (motion->parent)
285  {
286  for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
287  if (motion->parent->children[i] == motion)
288  {
289  motion->parent->children.erase(motion->parent->children.begin() + i);
290  break;
291  }
292  }
293 
294  /* remove children */
295  for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
296  {
297  motion->children[i]->parent = NULL;
298  removeMotion(disc, motion->children[i]);
299  }
300 
301  freeMotion(motion);
302 }
303 
304 
306 {
307  if (motion->state)
308  si_->freeState(motion->state);
309  delete motion;
310 }
311 
313 {
314  Planner::clear();
315 
316  sampler_.reset();
317  dStart_.clear();
318  dGoal_.clear();
319  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
320 }
321 
323 {
324  Planner::getPlannerData(data);
325  dStart_.getPlannerData(data, 1, true, NULL);
326  dGoal_.getPlannerData(data, 2, false, NULL);
327 
328  // Insert the edge connecting the two trees
329  data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
330 }
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:164
base::ProjectionEvaluatorPtr projectionEvaluator_
The employed projection evaluator.
Definition: LBKPIECE1.h:218
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: LBKPIECE1.cpp:322
Motion * parent
The parent motion in the exploration tree.
Definition: LBKPIECE1.h:192
Representation of a motion for this algorithm.
Definition: LBKPIECE1.h:168
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LBKPIECE1.cpp:312
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: LBKPIECE1.h:198
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
Definition: LBKPIECE1.h:136
The planner failed to find a solution.
Definition: PlannerStatus.h:62
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:208
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:189
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or NULL 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:271
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
void removeMotion(Discretization< Motion > &disc, Motion *motion)
Remove a motion from a tree of motions.
Definition: LBKPIECE1.cpp:274
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:62
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: LBKPIECE1.h:234
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:400
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:181
double getMinValidPathFraction() const
Get the value of the fraction set by setMinValidPathFraction()
Definition: LBKPIECE1.h:153
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:231
double getRange() const
Get the range the planner is using.
Definition: LBKPIECE1.h:118
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
One-level discretization used for KPIECE.
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
#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:112
The planner found an exact solution.
Definition: PlannerStatus.h:66
Discretization< Motion > dStart_
The start tree.
Definition: LBKPIECE1.h:221
Discretization< Motion > dGoal_
The goal tree.
Definition: LBKPIECE1.h:224
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 boost 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:229
base::StateSamplerPtr sampler_
The employed state sampler.
Definition: LBKPIECE1.h:215
Definition of an abstract state.
Definition: State.h:50
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:100
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:403
bool valid
Flag indicating whether this motion has been checked for validity.
Definition: LBKPIECE1.h:195
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: LBKPIECE1.cpp:305
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
const State * nextStart()
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:230
Definition of a cell in this grid.
Definition: Grid.h:59
Lazy Bi-directional KPIECE with one level of discretization.
Definition: LBKPIECE1.h:78
The exception type for ompl.
Definition: Exception.h:47
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:238
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: LBKPIECE1.cpp:75
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:55
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:147
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:232
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:58
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: LBKPIECE1.h:240
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LBKPIECE1.cpp:61
Definition of a geometric path.
Definition: PathGeometric.h:60
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:75
RNG rng_
The random number generator.
Definition: LBKPIECE1.h:237
const base::State * root
The root state (start state) that leads to this motion.
Definition: LBKPIECE1.h:186
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:128
A boost shared pointer wrapper for ompl::base::Path.
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:55
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68