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 
43  : base::Planner(si, "BKPIECE1")
44  , dStart_([this](Motion *m)
45  {
46  freeMotion(m);
47  })
48  , dGoal_([this](Motion *m)
49  {
50  freeMotion(m);
51  })
52 {
54 
57  maxDistance_ = 0.0;
58  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
59 
60  Planner::declareParam<double>("range", this, &BKPIECE1::setRange, &BKPIECE1::getRange, "0.:1.:10000.");
61  Planner::declareParam<double>("border_fraction", this, &BKPIECE1::setBorderFraction, &BKPIECE1::getBorderFraction,
62  "0.:.05:1.");
63  Planner::declareParam<double>("failed_expansion_score_factor", this, &BKPIECE1::setFailedExpansionCellScoreFactor,
65  Planner::declareParam<double>("min_valid_path_fraction", this, &BKPIECE1::setMinValidPathFraction,
67 }
68 
69 ompl::geometric::BKPIECE1::~BKPIECE1() = default;
70 
72 {
73  Planner::setup();
77 
78  if (failedExpansionScoreFactor_ < std::numeric_limits<double>::epsilon() || failedExpansionScoreFactor_ > 1.0)
79  throw Exception("Failed expansion cell score factor must be in the range (0,1]");
80  if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
81  throw Exception("The minimum valid path fraction must be in the range (0,1]");
82 
83  dStart_.setDimension(projectionEvaluator_->getDimension());
84  dGoal_.setDimension(projectionEvaluator_->getDimension());
85 }
86 
88 {
89  checkValidity();
90  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
91 
92  if (!goal)
93  {
94  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
96  }
97 
99 
100  while (const base::State *st = pis_.nextStart())
101  {
102  auto *motion = new Motion(si_);
103  si_->copyState(motion->state, st);
104  motion->root = motion->state;
105  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
106  dStart_.addMotion(motion, xcoord);
107  }
108 
109  if (dStart_.getMotionCount() == 0)
110  {
111  OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
113  }
114 
115  if (!goal->couldSample())
116  {
117  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
119  }
120 
121  if (!sampler_)
122  sampler_ = si_->allocValidStateSampler();
123 
124  OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
125  (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
126 
127  std::vector<Motion *> solution;
128  base::State *xstate = si_->allocState();
129  bool startTree = true;
130  bool solved = false;
131 
132  while (ptc == false)
133  {
134  Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
135  startTree = !startTree;
136  Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
137  disc.countIteration();
138 
139  // if we have not sampled too many goals already
140  if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
141  {
142  const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
143  if (st)
144  {
145  auto *motion = new Motion(si_);
146  si_->copyState(motion->state, st);
147  motion->root = motion->state;
148  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
149  dGoal_.addMotion(motion, xcoord);
150  }
151  if (dGoal_.getMotionCount() == 0)
152  {
153  OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
154  break;
155  }
156  }
157 
158  Discretization<Motion>::Cell *ecell = nullptr;
159  Motion *existing = nullptr;
160  disc.selectMotion(existing, ecell);
161  assert(existing);
162  if (sampler_->sampleNear(xstate, existing->state, maxDistance_))
163  {
164  std::pair<base::State *, double> fail(xstate, 0.0);
165  bool keep = si_->checkMotion(existing->state, xstate, fail);
166  if (!keep && fail.second > minValidPathFraction_)
167  keep = true;
168 
169  if (keep)
170  {
171  /* create a motion */
172  auto *motion = new Motion(si_);
173  si_->copyState(motion->state, xstate);
174  motion->root = existing->root;
175  motion->parent = existing;
176 
177  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
178  disc.addMotion(motion, xcoord);
179 
180  Discretization<Motion>::Cell *cellC = otherDisc.getGrid().getCell(xcoord);
181 
182  if (cellC && !cellC->data->motions.empty())
183  {
184  Motion *connectOther = cellC->data->motions[rng_.uniformInt(0, cellC->data->motions.size() - 1)];
185 
186  if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root,
187  startTree ? motion->root : connectOther->root) &&
188  si_->checkMotion(motion->state, connectOther->state))
189  {
190  if (startTree)
191  connectionPoint_ = std::make_pair(connectOther->state, motion->state);
192  else
193  connectionPoint_ = std::make_pair(motion->state, connectOther->state);
194 
195  /* extract the motions and put them in solution vector */
196 
197  std::vector<Motion *> mpath1;
198  while (motion != nullptr)
199  {
200  mpath1.push_back(motion);
201  motion = motion->parent;
202  }
203 
204  std::vector<Motion *> mpath2;
205  while (connectOther != nullptr)
206  {
207  mpath2.push_back(connectOther);
208  connectOther = connectOther->parent;
209  }
210 
211  if (startTree)
212  mpath1.swap(mpath2);
213 
214  auto path(std::make_shared<PathGeometric>(si_));
215  path->getStates().reserve(mpath1.size() + mpath2.size());
216  for (int i = mpath1.size() - 1; i >= 0; --i)
217  path->append(mpath1[i]->state);
218  for (auto &i : mpath2)
219  path->append(i->state);
220 
221  pdef_->addSolutionPath(path, false, 0.0, getName());
222  solved = true;
223  break;
224  }
225  }
226  }
227  else
228  ecell->data->score *= failedExpansionScoreFactor_;
229  }
230  else
231  ecell->data->score *= failedExpansionScoreFactor_;
232  disc.updateCell(ecell);
233  }
234 
235  si_->freeState(xstate);
236 
237  OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on "
238  "boundary))",
239  getName().c_str(), dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(),
240  dGoal_.getMotionCount(), dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(),
241  dStart_.getGrid().countExternal(), dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
242 
244 }
245 
247 {
248  if (motion->state)
249  si_->freeState(motion->state);
250  delete motion;
251 }
252 
254 {
255  Planner::clear();
256 
257  sampler_.reset();
258  dStart_.clear();
259  dGoal_.clear();
260  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
261 }
262 
264 {
265  Planner::getPlannerData(data);
266  dStart_.getPlannerData(data, 1, true, nullptr);
267  dGoal_.getPlannerData(data, 2, false, nullptr);
268 
269  // Insert the edge connecting the two trees
270  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
271 }
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:106
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
Discretization< Motion > dGoal_
The goal tree.
Definition: BKPIECE1.h:215
const base::State * root
The root state (start state) that leads to this motion.
Definition: BKPIECE1.h:193
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: BKPIECE1.h:236
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:206
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: BKPIECE1.cpp:246
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::ProjectionEvaluatorPtr projectionEvaluator_
The employed projection evaluator.
Definition: BKPIECE1.h:209
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
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BKPIECE1.cpp:71
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
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: BKPIECE1.cpp:263
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:220
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
Discretization< Motion > dStart_
The start tree.
Definition: BKPIECE1.h:212
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:175
RNG rng_
The random number generator.
Definition: BKPIECE1.h:233
double getFailedExpansionCellScoreFactor() const
Get the factor that is multiplied to a cell&#39;s score if extending a motion from that cell failed...
Definition: BKPIECE1.h:147
double getRange() const
Get the range the planner is using.
Definition: BKPIECE1.h:112
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:230
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
Representation of a motion for this algorithm.
Definition: BKPIECE1.h:178
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.
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:158
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BKPIECE1.cpp:253
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
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: BKPIECE1.cpp:87
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:140
base::State * state
The state contained by this motion.
Definition: BKPIECE1.h:196
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
Motion * parent
The parent motion in the exploration tree.
Definition: BKPIECE1.h:199
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
Definition: BKPIECE1.h:131
double getMinValidPathFraction() const
Get the value of the fraction set by setMinValidPathFraction()
Definition: BKPIECE1.h:164
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 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
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:227
BKPIECE1(const base::SpaceInformationPtr &si)
Constructor.
Definition: BKPIECE1.cpp:42
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
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:123
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:81
base::ValidStateSamplerPtr sampler_
The employed state sampler.
Definition: BKPIECE1.h:206
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