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