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("Unknown type of goal (or goal undefined)");
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("Motion planning start tree could not be initialized!");
105  }
106 
107  if (!goal->couldSample())
108  {
109  OMPL_ERROR("Insufficient states in sampleable goal region");
111  }
112 
113  if (!sampler_)
114  sampler_ = si_->allocValidStateSampler();
115 
116  OMPL_INFORM("Starting with %d states", (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("Unable to sample any valid states for goal tree");
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("Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
228  dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
229  dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
230  dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
231 
233 }
234 
236 {
237  if (motion->state)
238  si_->freeState(motion->state);
239  delete motion;
240 }
241 
243 {
244  Planner::clear();
245 
246  sampler_.reset();
247  dStart_.clear();
248  dGoal_.clear();
249  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
250 }
251 
253 {
254  Planner::getPlannerData(data);
255  dStart_.getPlannerData(data, 1, true, NULL);
256  dGoal_.getPlannerData(data, 2, false, NULL);
257 
258  // Insert the edge connecting the two trees
259  data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
260 }