KPIECE1.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/KPIECE1.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <limits>
41 #include <cassert>
42 
43 ompl::geometric::KPIECE1::KPIECE1(const base::SpaceInformationPtr &si)
44  : base::Planner(si, "KPIECE1"), disc_([this](Motion *m) { freeMotion(m); })
45 {
46  specs_.approximateSolutions = true;
47  specs_.directed = true;
48 
49  Planner::declareParam<double>("range", this, &KPIECE1::setRange, &KPIECE1::getRange, "0.:1.:10000.");
50  Planner::declareParam<double>("goal_bias", this, &KPIECE1::setGoalBias, &KPIECE1::getGoalBias, "0.:.05:1.");
51  Planner::declareParam<double>("border_fraction", this, &KPIECE1::setBorderFraction, &KPIECE1::getBorderFraction,
52  "0.:0.05:1.");
53  Planner::declareParam<double>("failed_expansion_score_factor", this, &KPIECE1::setFailedExpansionCellScoreFactor,
55  Planner::declareParam<double>("min_valid_path_fraction", this, &KPIECE1::setMinValidPathFraction,
57 }
58 
59 ompl::geometric::KPIECE1::~KPIECE1() = default;
60 
62 {
63  Planner::setup();
64  tools::SelfConfig sc(si_, getName());
65  sc.configureProjectionEvaluator(projectionEvaluator_);
66  sc.configurePlannerRange(maxDistance_);
67 
68  if (failedExpansionScoreFactor_ < std::numeric_limits<double>::epsilon() || failedExpansionScoreFactor_ > 1.0)
69  throw Exception("Failed expansion cell score factor must be in the range (0,1]");
70  if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
71  throw Exception("The minimum valid path fraction must be in the range (0,1]");
72 
73  disc_.setDimension(projectionEvaluator_->getDimension());
74 }
75 
77 {
78  Planner::clear();
79  sampler_.reset();
80  disc_.clear();
81  lastGoalMotion_ = nullptr;
82 }
83 
85 {
86  if (motion->state != nullptr)
87  si_->freeState(motion->state);
88  delete motion;
89 }
90 
92 {
93  checkValidity();
94  base::Goal *goal = pdef_->getGoal().get();
95  auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
96 
97  Discretization<Motion>::Coord xcoord(projectionEvaluator_->getDimension());
98 
99  while (const base::State *st = pis_.nextStart())
100  {
101  auto *motion = new Motion(si_);
102  si_->copyState(motion->state, st);
103  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
104  disc_.addMotion(motion, xcoord, 1.0);
105  }
106 
107  if (disc_.getMotionCount() == 0)
108  {
109  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
111  }
112 
113  if (!sampler_)
114  sampler_ = si_->allocStateSampler();
115 
116  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
117  disc_.getMotionCount());
118 
119  Motion *solution = nullptr;
120  Motion *approxsol = nullptr;
121  double approxdif = std::numeric_limits<double>::infinity();
122  base::State *xstate = si_->allocState();
123 
124  while (!ptc)
125  {
126  disc_.countIteration();
127 
128  /* Decide on a state to expand from */
129  Motion *existing = nullptr;
130  Discretization<Motion>::Cell *ecell = nullptr;
131  disc_.selectMotion(existing, ecell);
132  assert(existing);
133 
134  /* sample random state (with goal biasing) */
135  if ((goal_s != nullptr) && rng_.uniform01() < goalBias_ && goal_s->canSample())
136  goal_s->sampleGoal(xstate);
137  else
138  sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
139 
140  std::pair<base::State *, double> fail(xstate, 0.0);
141  bool keep = si_->checkMotion(existing->state, xstate, fail);
142  if (!keep && fail.second > minValidPathFraction_)
143  keep = true;
144 
145  if (keep)
146  {
147  /* create a motion */
148  auto *motion = new Motion(si_);
149  si_->copyState(motion->state, xstate);
150  motion->parent = existing;
151 
152  double dist = 0.0;
153  bool solv = goal->isSatisfied(motion->state, &dist);
154  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
155  disc_.addMotion(motion, xcoord, dist); // this will also update the discretization heaps as needed, so no
156  // call to updateCell() is needed
157 
158  if (solv)
159  {
160  approxdif = dist;
161  solution = motion;
162  break;
163  }
164  if (dist < approxdif)
165  {
166  approxdif = dist;
167  approxsol = motion;
168  }
169  }
170  else
171  ecell->data->score *= failedExpansionScoreFactor_;
172  disc_.updateCell(ecell);
173  }
174 
175  bool solved = false;
176  bool approximate = false;
177  if (solution == nullptr)
178  {
179  solution = approxsol;
180  approximate = true;
181  }
182 
183  if (solution != nullptr)
184  {
185  lastGoalMotion_ = solution;
186 
187  /* construct the solution path */
188  std::vector<Motion *> mpath;
189  while (solution != nullptr)
190  {
191  mpath.push_back(solution);
192  solution = solution->parent;
193  }
194 
195  /* set the solution path */
196  auto path(std::make_shared<PathGeometric>(si_));
197  for (int i = mpath.size() - 1; i >= 0; --i)
198  path->append(mpath[i]->state);
199  pdef_->addSolutionPath(path, approximate, approxdif, getName());
200  solved = true;
201  }
202 
203  si_->freeState(xstate);
204 
205  OMPL_INFORM("%s: Created %u states in %u cells (%u internal + %u external)", getName().c_str(),
206  disc_.getMotionCount(), disc_.getCellCount(), disc_.getGrid().countInternal(),
207  disc_.getGrid().countExternal());
208 
209  return {solved, approximate};
210 }
211 
213 {
214  Planner::getPlannerData(data);
215  disc_.getPlannerData(data, 0, true, lastGoalMotion_);
216 }
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
Definition: KPIECE1.h:228
double getFailedExpansionCellScoreFactor() const
Get the factor that is multiplied to a cell's score if extending a motion from that cell failed.
Definition: KPIECE1.h:261
base::State * state
The state contained by this motion.
Definition: KPIECE1.h:305
void setFailedExpansionCellScoreFactor(double factor)
When extending a motion from a cell, the extension can be successful or it can fail....
Definition: KPIECE1.h:254
Motion * parent
The parent motion in the exploration tree.
Definition: KPIECE1.h:308
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: KPIECE1.h:204
Definition of an abstract state.
Definition: State.h:113
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:123
void setGoalBias(double goalBias)
Set the goal bias.
Definition: KPIECE1.h:188
typename Grid::Cell Cell
The datatype for the maintained grid cells.
double getRange() const
Get the range the planner is using.
Definition: KPIECE1.h:210
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
KPIECE1(const base::SpaceInformationPtr &si)
Constructor.
Definition: KPIECE1.cpp:43
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
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: KPIECE1.cpp:91
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
double getMinValidPathFraction() const
Get the value of the fraction set by setMinValidPathFraction()
Definition: KPIECE1.h:245
Representation of a motion for this algorithm.
Definition: KPIECE1.h:292
A class to store the exit status of Planner::solve()
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
typename Grid::Coord Coord
The datatype for the maintained grid coordinates.
Abstract definition of goals.
Definition: Goal.h:126
double getGoalBias() const
Get the goal bias the planner is using.
Definition: KPIECE1.h:194
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: KPIECE1.h:221
void setMinValidPathFraction(double fraction)
When extending a motion, the planner can decide to keep the first valid part of it,...
Definition: KPIECE1.h:239
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: KPIECE1.cpp:61
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_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: KPIECE1.cpp:76
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: KPIECE1.cpp:84
Abstract definition of a goal region that can be sampled.
The exception type for ompl.
Definition: Exception.h:78
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: KPIECE1.cpp:212
@ INVALID_START
Invalid start state or no start state specified.