All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
BKPIECE1.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University,
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 Rice University 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/BKPIECE1.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <cassert>
41 
42 ompl::geometric::BKPIECE1::BKPIECE1(const base::SpaceInformationPtr &si) : base::Planner(si, "BKPIECE1"),
43  dStart_(boost::bind(&BKPIECE1::freeMotion, this, _1)),
44  dGoal_(boost::bind(&BKPIECE1::freeMotion, this, _1))
45 {
47 
50  maxDistance_ = 0.0;
51  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
52 
53  Planner::declareParam<double>("range", this, &BKPIECE1::setRange, &BKPIECE1::getRange, "0.:1.:10000.");
54  Planner::declareParam<double>("border_fraction", this, &BKPIECE1::setBorderFraction, &BKPIECE1::getBorderFraction, "0.:.05:1.");
55  Planner::declareParam<double>("failed_expansion_score_factor", this, &BKPIECE1::setFailedExpansionCellScoreFactor, &BKPIECE1::getFailedExpansionCellScoreFactor);
56  Planner::declareParam<double>("min_valid_path_fraction", this, &BKPIECE1::setMinValidPathFraction, &BKPIECE1::getMinValidPathFraction);
57 }
58 
59 ompl::geometric::BKPIECE1::~BKPIECE1(void)
60 {
61 }
62 
64 {
65  Planner::setup();
66  tools::SelfConfig sc(si_, getName());
67  sc.configureProjectionEvaluator(projectionEvaluator_);
68  sc.configurePlannerRange(maxDistance_);
69 
70  if (failedExpansionScoreFactor_ < std::numeric_limits<double>::epsilon() || failedExpansionScoreFactor_ > 1.0)
71  throw Exception("Failed expansion cell score factor must be in the range (0,1]");
72  if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
73  throw Exception("The minimum valid path fraction must be in the range (0,1]");
74 
75  dStart_.setDimension(projectionEvaluator_->getDimension());
76  dGoal_.setDimension(projectionEvaluator_->getDimension());
77 }
78 
80 {
81  checkValidity();
82  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
83 
84  if (!goal)
85  {
86  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
88  }
89 
91 
92  while (const base::State *st = pis_.nextStart())
93  {
94  Motion* motion = new Motion(si_);
95  si_->copyState(motion->state, st);
96  motion->root = motion->state;
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_->allocValidStateSampler();
115 
116  OMPL_INFORM("%s: Starting with %d states", getName().c_str(), (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
117 
118  std::vector<Motion*> solution;
119  base::State *xstate = si_->allocState();
120  bool startTree = true;
121  bool solved = false;
122 
123  while (ptc == false)
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)
135  {
136  Motion* motion = new Motion(si_);
137  si_->copyState(motion->state, st);
138  motion->root = motion->state;
139  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
140  dGoal_.addMotion(motion, xcoord);
141  }
142  if (dGoal_.getMotionCount() == 0)
143  {
144  OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
145  break;
146  }
147  }
148 
149  Discretization<Motion>::Cell *ecell = NULL;
150  Motion *existing = NULL;
151  disc.selectMotion(existing, ecell);
152  assert(existing);
153  if (sampler_->sampleNear(xstate, existing->state, maxDistance_))
154  {
155  std::pair<base::State*, double> fail(xstate, 0.0);
156  bool keep = si_->checkMotion(existing->state, xstate, fail);
157  if (!keep && fail.second > minValidPathFraction_)
158  keep = true;
159 
160  if (keep)
161  {
162  /* create a motion */
163  Motion *motion = new Motion(si_);
164  si_->copyState(motion->state, xstate);
165  motion->root = existing->root;
166  motion->parent = existing;
167 
168  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
169  disc.addMotion(motion, xcoord);
170 
171  Discretization<Motion>::Cell* cellC = otherDisc.getGrid().getCell(xcoord);
172 
173  if (cellC && !cellC->data->motions.empty())
174  {
175  Motion* connectOther = cellC->data->motions[rng_.uniformInt(0, cellC->data->motions.size() - 1)];
176 
177  if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root) &&
178  si_->checkMotion(motion->state, connectOther->state))
179  {
180  if (startTree)
181  connectionPoint_ = std::make_pair(connectOther->state, motion->state);
182  else
183  connectionPoint_ = std::make_pair(motion->state, connectOther->state);
184 
185  /* extract the motions and put them in solution vector */
186 
187  std::vector<Motion*> mpath1;
188  while (motion != NULL)
189  {
190  mpath1.push_back(motion);
191  motion = motion->parent;
192  }
193 
194  std::vector<Motion*> mpath2;
195  while (connectOther != NULL)
196  {
197  mpath2.push_back(connectOther);
198  connectOther = connectOther->parent;
199  }
200 
201  if (startTree)
202  mpath1.swap(mpath2);
203 
204  PathGeometric *path = new PathGeometric(si_);
205  path->getStates().reserve(mpath1.size() + mpath2.size());
206  for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
207  path->append(mpath1[i]->state);
208  for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
209  path->append(mpath2[i]->state);
210 
211  pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
212  solved = true;
213  break;
214  }
215  }
216  }
217  else
218  ecell->data->score *= failedExpansionScoreFactor_;
219  }
220  else
221  ecell->data->score *= failedExpansionScoreFactor_;
222  disc.updateCell(ecell);
223  }
224 
225  si_->freeState(xstate);
226 
227  OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
228  getName().c_str(),
229  dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
230  dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
231  dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
232 
234 }
235 
237 {
238  if (motion->state)
239  si_->freeState(motion->state);
240  delete motion;
241 }
242 
244 {
245  Planner::clear();
246 
247  sampler_.reset();
248  dStart_.clear();
249  dGoal_.clear();
250  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
251 }
252 
254 {
255  Planner::getPlannerData(data);
256  dStart_.getPlannerData(data, 1, true, NULL);
257  dGoal_.getPlannerData(data, 2, false, NULL);
258 
259  // Insert the edge connecting the two trees
260  data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
261 }
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: BKPIECE1.cpp:253
Grid::Coord Coord
The datatype for the maintained grid coordinates.
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: BKPIECE1.h:111
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
const base::State * root
The root state (start state) that leads to this motion.
Definition: BKPIECE1.h:202
The planner failed to find a solution.
Definition: PlannerStatus.h:62
Bi-directional KPIECE with one level of discretization.
Definition: BKPIECE1.h:77
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:206
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: BKPIECE1.cpp:236
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BKPIECE1.cpp:63
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: BKPIECE1.h:245
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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: BKPIECE1.cpp:79
_T data
The data we store in the cell.
Definition: Grid.h:62
std::vector< base::State * > & getStates(void)
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
double failedExpansionScoreFactor_
When extending a motion from a cell, the extension can fail. If it is, the score of the cell is multi...
Definition: BKPIECE1.h:229
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
double getRange(void) const
Get the range the planner is using.
Definition: BKPIECE1.h:117
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BKPIECE1.cpp:243
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: BKPIECE1.h:239
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
One-level discretization used for KPIECE.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
The planner found an exact solution.
Definition: PlannerStatus.h:66
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...
Representation of a motion for this algorithm.
Definition: BKPIECE1.h:184
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 ...
double getMinValidPathFraction(void) const
Get the value of the fraction set by setMinValidPathFraction()
Definition: BKPIECE1.h:169
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...
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
A boost shared pointer wrapper for ompl::base::SpaceInformation.
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: BKPIECE1.h:163
virtual bool couldSample(void) 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.
Definition of an abstract state.
Definition: State.h:50
void setFailedExpansionCellScoreFactor(double factor)
When extending a motion from a cell, the extension can be successful or it can fail. If the extension fails, the score of the cell is multiplied by factor. These number should be in the range (0, 1].
Definition: BKPIECE1.h:145
base::State * state
The state contained by this motion.
Definition: BKPIECE1.h:205
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:404
Definition of a cell in this grid.
Definition: Grid.h:59
Motion * parent
The parent motion in the exploration tree.
Definition: BKPIECE1.h:208
The exception type for ompl.
Definition: Exception.h:47
double getFailedExpansionCellScoreFactor(void) const
Get the factor that is multiplied to a cell's score if extending a motion from that cell failed...
Definition: BKPIECE1.h:152
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:232
double getBorderFraction(void) const
Get the fraction of time to focus exploration on boundary.
Definition: BKPIECE1.h:136
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 configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:226
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:56
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: BKPIECE1.h:236
BKPIECE1(const base::SpaceInformationPtr &si)
Constructor.
Definition: BKPIECE1.cpp:42
Definition of a geometric path.
Definition: PathGeometric.h:55
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: BKPIECE1.h:128
A boost shared pointer wrapper for ompl::base::Path.
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