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)
43  : base::Planner(si, "BKPIECE1")
44  , dStart_([this](Motion *m) { freeMotion(m); })
45  , dGoal_([this](Motion *m) { freeMotion(m); })
46 {
47  specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
48 
49  Planner::declareParam<double>("range", this, &BKPIECE1::setRange, &BKPIECE1::getRange, "0.:1.:10000.");
50  Planner::declareParam<double>("border_fraction", this, &BKPIECE1::setBorderFraction, &BKPIECE1::getBorderFraction,
51  "0.:.05:1.");
52  Planner::declareParam<double>("failed_expansion_score_factor", this, &BKPIECE1::setFailedExpansionCellScoreFactor,
54  Planner::declareParam<double>("min_valid_path_fraction", this, &BKPIECE1::setMinValidPathFraction,
56 }
57 
58 ompl::geometric::BKPIECE1::~BKPIECE1() = default;
59 
61 {
62  Planner::setup();
63  tools::SelfConfig sc(si_, getName());
64  sc.configureProjectionEvaluator(projectionEvaluator_);
65  sc.configurePlannerRange(maxDistance_);
66 
67  if (failedExpansionScoreFactor_ < std::numeric_limits<double>::epsilon() || failedExpansionScoreFactor_ > 1.0)
68  throw Exception("Failed expansion cell score factor must be in the range (0,1]");
69  if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
70  throw Exception("The minimum valid path fraction must be in the range (0,1]");
71 
72  dStart_.setDimension(projectionEvaluator_->getDimension());
73  dGoal_.setDimension(projectionEvaluator_->getDimension());
74 }
75 
77 {
78  checkValidity();
79  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
80 
81  if (goal == nullptr)
82  {
83  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
85  }
86 
87  Discretization<Motion>::Coord xcoord(projectionEvaluator_->getDimension());
88 
89  while (const base::State *st = pis_.nextStart())
90  {
91  auto *motion = new Motion(si_);
92  si_->copyState(motion->state, st);
93  motion->root = motion->state;
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_->allocValidStateSampler();
112 
113  OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
114  (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
115 
116  std::vector<Motion *> solution;
117  base::State *xstate = si_->allocState();
118  bool startTree = true;
119  bool solved = false;
120 
121  while (!ptc)
122  {
123  Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
124  startTree = !startTree;
125  Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
126  disc.countIteration();
127 
128  // if we have not sampled too many goals already
129  if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
130  {
131  const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
132  if (st != nullptr)
133  {
134  auto *motion = new Motion(si_);
135  si_->copyState(motion->state, st);
136  motion->root = motion->state;
137  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
138  dGoal_.addMotion(motion, xcoord);
139  }
140  if (dGoal_.getMotionCount() == 0)
141  {
142  OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
143  break;
144  }
145  }
146 
147  Discretization<Motion>::Cell *ecell = nullptr;
148  Motion *existing = nullptr;
149  disc.selectMotion(existing, ecell);
150  assert(existing);
151  if (sampler_->sampleNear(xstate, existing->state, maxDistance_))
152  {
153  std::pair<base::State *, double> fail(xstate, 0.0);
154  bool keep = si_->checkMotion(existing->state, xstate, fail);
155  if (!keep && fail.second > minValidPathFraction_)
156  keep = true;
157 
158  if (keep)
159  {
160  /* create a motion */
161  auto *motion = new Motion(si_);
162  si_->copyState(motion->state, xstate);
163  motion->root = existing->root;
164  motion->parent = existing;
165 
166  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
167  disc.addMotion(motion, xcoord);
168 
169  Discretization<Motion>::Cell *cellC = otherDisc.getGrid().getCell(xcoord);
170 
171  if ((cellC != nullptr) && !cellC->data->motions.empty())
172  {
173  Motion *connectOther = cellC->data->motions[rng_.uniformInt(0, cellC->data->motions.size() - 1)];
174 
175  if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root,
176  startTree ? motion->root : connectOther->root) &&
177  si_->checkMotion(motion->state, connectOther->state))
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 != nullptr)
188  {
189  mpath1.push_back(motion);
190  motion = motion->parent;
191  }
192 
193  std::vector<Motion *> mpath2;
194  while (connectOther != nullptr)
195  {
196  mpath2.push_back(connectOther);
197  connectOther = connectOther->parent;
198  }
199 
200  if (startTree)
201  mpath1.swap(mpath2);
202 
203  auto path(std::make_shared<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 (auto &i : mpath2)
208  path->append(i->state);
209 
210  pdef_->addSolutionPath(path, false, 0.0, getName());
211  solved = true;
212  break;
213  }
214  }
215  }
216  else
217  ecell->data->score *= failedExpansionScoreFactor_;
218  }
219  else
220  ecell->data->score *= failedExpansionScoreFactor_;
221  disc.updateCell(ecell);
222  }
223 
224  si_->freeState(xstate);
225 
226  OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on "
227  "boundary))",
228  getName().c_str(), dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(),
229  dGoal_.getMotionCount(), dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(),
230  dStart_.getGrid().countExternal(), dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
231 
233 }
234 
236 {
237  if (motion->state != nullptr)
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 *>(nullptr, nullptr);
250 }
251 
253 {
254  Planner::getPlannerData(data);
255  dStart_.getPlannerData(data, 1, true, nullptr);
256  dGoal_.getPlannerData(data, 2, false, nullptr);
257 
258  // Insert the edge connecting the two trees
259  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
260 }
The exception type for ompl.
Definition: Exception.h:47
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition: GridN.h:123
Abstract definition of a goal region that can be sampled.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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...
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...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition of an abstract state.
Definition: State.h:50
Representation of a motion for this algorithm.
Definition: BKPIECE1.h:179
Motion * parent
The parent motion in the exploration tree.
Definition: BKPIECE1.h:197
base::State * state
The state contained by this motion.
Definition: BKPIECE1.h:194
const base::State * root
The root state (start state) that leads to this motion.
Definition: BKPIECE1.h:191
void setFailedExpansionCellScoreFactor(double factor)
When extending a motion from a cell, the extension can be successful or it can fail....
Definition: BKPIECE1.h:140
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:252
void setMinValidPathFraction(double fraction)
When extending a motion, the planner can decide to keep the first valid part of it,...
Definition: BKPIECE1.h:158
double getMinValidPathFraction() const
Get the value of the fraction set by setMinValidPathFraction()
Definition: BKPIECE1.h:164
BKPIECE1(const base::SpaceInformationPtr &si)
Constructor.
Definition: BKPIECE1.cpp:42
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:76
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
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
Definition: BKPIECE1.h:131
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BKPIECE1.cpp:60
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: BKPIECE1.h:106
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BKPIECE1.cpp:242
double getRange() const
Get the range the planner is using.
Definition: BKPIECE1.h:112
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: BKPIECE1.cpp:235
double getFailedExpansionCellScoreFactor() const
Get the factor that is multiplied to a cell's score if extending a motion from that cell failed.
Definition: BKPIECE1.h:147
One-level discretization used for KPIECE.
typename Grid::Coord Coord
The datatype for the maintained grid coordinates.
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 ...
typename Grid::Cell Cell
The datatype for the maintained grid cells.
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...
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
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
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:56
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66
@ INVALID_GOAL
Invalid goal state.
Definition: PlannerStatus.h:58
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62