All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
STRIDE.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: Bryant Gipson, Mark Moll, Ioan Sucan */
36 
37 #include "ompl/geometric/planners/stride/STRIDE.h"
38 // enable sampling from the GNAT data structure
39 #define GNAT_SAMPLER
40 #include "ompl/datastructures/NearestNeighborsGNAT.h"
41 #include "ompl/base/goals/GoalSampleableRegion.h"
42 #include "ompl/tools/config/SelfConfig.h"
43 #include <limits>
44 #include <cassert>
45 
47  bool useProjectedDistance,
48  unsigned int degree, unsigned int minDegree,
49  unsigned int maxDegree, unsigned int maxNumPtsPerLeaf, double estimatedDimension)
50  : base::Planner(si, "STRIDE"), goalBias_(0.05), maxDistance_(0.),
51  useProjectedDistance_(useProjectedDistance),
52  degree_(degree), minDegree_(minDegree), maxDegree_(maxDegree),
53  maxNumPtsPerLeaf_(maxNumPtsPerLeaf), estimatedDimension_(estimatedDimension),
54  minValidPathFraction_(0.2)
55 {
57 
58  if (estimatedDimension_ < 1.)
59  estimatedDimension_ = si->getStateDimension();
60 
61  Planner::declareParam<double>("range", this, &STRIDE::setRange, &STRIDE::getRange, "0.:1.:10000.");
62  Planner::declareParam<double>("goal_bias", this, &STRIDE::setGoalBias, &STRIDE::getGoalBias, "0.:.05:1.");
63  Planner::declareParam<bool>("use_projected_distance", this, &STRIDE::setUseProjectedDistance, &STRIDE::getUseProjectedDistance, "0,1");
64  Planner::declareParam<unsigned int>("degree", this, &STRIDE::setDegree, &STRIDE::getDegree, "2:20");
65  Planner::declareParam<unsigned int>("max_degree", this, &STRIDE::setMaxDegree, &STRIDE::getMaxDegree, "2:20");
66  Planner::declareParam<unsigned int>("min_degree", this, &STRIDE::setMinDegree, &STRIDE::getMinDegree, "2:20");
67  Planner::declareParam<unsigned int>("max_pts_per_leaf", this, &STRIDE::setMaxNumPtsPerLeaf, &STRIDE::getMaxNumPtsPerLeaf, "1:200");
68  Planner::declareParam<double>("estimated_dimension", this, &STRIDE::setEstimatedDimension, &STRIDE::getEstimatedDimension, "1.:30.");
69  Planner::declareParam<double>("min_valid_path_fraction", this, &STRIDE::setMinValidPathFraction, &STRIDE::getMinValidPathFraction, "0.:.05:1.");
70 }
71 
72 ompl::geometric::STRIDE::~STRIDE(void)
73 {
74  freeMemory();
75 }
76 
78 {
79  Planner::setup();
80  tools::SelfConfig sc(si_, getName());
81  sc.configureProjectionEvaluator(projectionEvaluator_);
82  sc.configurePlannerRange(maxDistance_);
83  setupTree();
84 }
85 
87 {
88  tree_.reset(new NearestNeighborsGNAT<Motion*>(degree_, minDegree_, maxDegree_, maxNumPtsPerLeaf_, estimatedDimension_));
89  if (useProjectedDistance_)
90  tree_->setDistanceFunction(boost::bind(&STRIDE::projectedDistanceFunction, this, _1, _2));
91  else
92  tree_->setDistanceFunction(boost::bind(&STRIDE::distanceFunction, this, _1, _2));
93 }
94 
96 {
97  Planner::clear();
98  sampler_.reset();
99  freeMemory();
100  setupTree();
101 }
102 
104 {
105  if (tree_)
106  {
107  std::vector<Motion*> motions;
108  tree_->list(motions);
109  for (std::size_t i = 0 ; i < motions.size() ; ++i)
110  {
111  if (motions[i]->state)
112  si_->freeState(motions[i]->state);
113  delete motions[i];
114  }
115  tree_.reset();
116  }
117 }
118 
120 {
121  checkValidity();
122  base::Goal *goal = pdef_->getGoal().get();
123  base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
124 
125  while (const base::State *st = pis_.nextStart())
126  {
127  Motion *motion = new Motion(si_);
128  si_->copyState(motion->state, st);
129  addMotion(motion);
130  }
131 
132  if (tree_->size() == 0)
133  {
134  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
136  }
137 
138  if (!sampler_)
139  sampler_ = si_->allocValidStateSampler();
140 
141  OMPL_INFORM("%s: Starting with %u states", getName().c_str(), tree_->size());
142 
143  Motion *solution = NULL;
144  Motion *approxsol = NULL;
145  double approxdif = std::numeric_limits<double>::infinity();
146  base::State *xstate = si_->allocState();
147 
148  while (ptc == false)
149  {
150  /* Decide on a state to expand from */
151  Motion *existing = selectMotion();
152  assert(existing);
153 
154  /* sample random state (with goal biasing) */
155  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
156  goal_s->sampleGoal(xstate);
157  else
158  if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
159  continue;
160 
161  std::pair<base::State*, double> fail(xstate, 0.0);
162  bool keep = si_->checkMotion(existing->state, xstate, fail) || fail.second > minValidPathFraction_;
163 
164  if (keep)
165  {
166  /* create a motion */
167  Motion *motion = new Motion(si_);
168  si_->copyState(motion->state, xstate);
169  motion->parent = existing;
170 
171  addMotion(motion);
172  double dist = 0.0;
173  bool solved = goal->isSatisfied(motion->state, &dist);
174  if (solved)
175  {
176  approxdif = dist;
177  solution = motion;
178  break;
179  }
180  if (dist < approxdif)
181  {
182  approxdif = dist;
183  approxsol = motion;
184  }
185  }
186  }
187 
188  bool solved = false;
189  bool approximate = false;
190  if (solution == NULL)
191  {
192  solution = approxsol;
193  approximate = true;
194  }
195 
196  if (solution != NULL)
197  {
198  /* construct the solution path */
199  std::vector<Motion*> mpath;
200  while (solution != NULL)
201  {
202  mpath.push_back(solution);
203  solution = solution->parent;
204  }
205 
206  /* set the solution path */
207  PathGeometric *path = new PathGeometric(si_);
208  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
209  path->append(mpath[i]->state);
210  pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif);
211  solved = true;
212  }
213 
214  si_->freeState(xstate);
215 
216  OMPL_INFORM("%s: Created %u states", getName().c_str(), tree_->size());
217 
218  return base::PlannerStatus(solved, approximate);
219 }
220 
222 {
223  tree_->add(motion);
224 }
225 
227 {
228  return tree_->sample(rng_);
229 }
230 
232 {
233  Planner::getPlannerData(data);
234 
235  std::vector<Motion*> motions;
236  tree_->list(motions);
237  for (std::vector<Motion*>::iterator it=motions.begin(); it!=motions.end(); it++)
238  {
239  if((*it)->parent == NULL)
240  data.addStartVertex(base::PlannerDataVertex((*it)->state,1));
241  else
242  data.addEdge(base::PlannerDataVertex((*it)->parent->state,1),base::PlannerDataVertex((*it)->state,1));
243  }
244 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
bool canSample(void) const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
bool getUseProjectedDistance(void) const
Return whether nearest neighbors are computed based on distances in a projection of the state rather ...
Definition: STRIDE.h:124
Motion * parent
The parent motion in the exploration tree.
Definition: STRIDE.h:262
The definition of a motion.
Definition: STRIDE.h:242
double estimatedDimension_
Estimate of the local dimensionality of the free space around a state.
Definition: STRIDE.h:321
void setMinDegree(unsigned int minDegree)
Set minimum degree of a node in the GNAT.
Definition: STRIDE.h:140
void setEstimatedDimension(double estimatedDimension)
Set estimated dimension of the free space, which is needed to compute the sampling weight for a node ...
Definition: STRIDE.h:174
Abstract definition of goals.
Definition: Goal.h:63
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Geometric Near-neighbor Access Tree (GNAT), a data structure for nearest neighbor search...
unsigned int getDegree(void) const
Get desired degree of a node in the GNAT.
Definition: STRIDE.h:135
unsigned int getMaxNumPtsPerLeaf(void) const
Get maximum number of elements stored in a leaf node of the GNAT.
Definition: STRIDE.h:167
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: STRIDE.cpp:77
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
unsigned int getMinDegree(void) const
Get minimum degree of a node in the GNAT.
Definition: STRIDE.h:145
void setDegree(unsigned int degree)
Set desired degree of a node in the GNAT.
Definition: STRIDE.h:130
void setMaxNumPtsPerLeaf(unsigned int maxNumPtsPerLeaf)
Set maximum number of elements stored in a leaf node of the GNAT.
Definition: STRIDE.h:161
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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 boost shared pointer wrapper for ompl::base::SpaceInformation.
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
Definition: State.h:50
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: STRIDE.h:191
double getMinValidPathFraction(void) const
Get the value of the fraction set by setMinValidPathFraction()
Definition: STRIDE.h:213
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:404
STRIDE(const base::SpaceInformationPtr &si, bool useProjectedDistance=false, unsigned int degree=16, unsigned int minDegree=12, unsigned int maxDegree=18, unsigned int maxNumPtsPerLeaf=6, double estimatedDimension=0.0)
Constructor.
Definition: STRIDE.cpp:46
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: STRIDE.cpp:119
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
double projectedDistanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between projections of contained states) ...
Definition: STRIDE.h:279
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
Motion * selectMotion(void)
Select a motion to continue the expansion of the tree from.
Definition: STRIDE.cpp:226
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: STRIDE.cpp:95
unsigned int getMaxDegree(void) const
Set maximum degree of a node in the GNAT.
Definition: STRIDE.h:155
void setupTree(void)
Initialize GNAT data structure.
Definition: STRIDE.cpp:86
Definition of a geometric path.
Definition: PathGeometric.h:55
void freeMemory(void)
Free the memory allocated by this planner.
Definition: STRIDE.cpp:103
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: STRIDE.h:207
double getGoalBias(void) const
Get the goal bias the planner is using.
Definition: STRIDE.h:109
double getRange(void) const
Get the range the planner is using.
Definition: STRIDE.h:197
base::State * state
The state contained by the motion.
Definition: STRIDE.h:259
double getEstimatedDimension(void) const
Get estimated dimension of the free space, which is needed to compute the sampling weight for a node ...
Definition: STRIDE.h:181
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: STRIDE.cpp:231
void setUseProjectedDistance(bool useProjectedDistance)
Set whether nearest neighbors are computed based on distances in a projection of the state rather dis...
Definition: STRIDE.h:117
void setMaxDegree(unsigned int maxDegree)
Set maximum degree of a node in the GNAT.
Definition: STRIDE.h:150
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards, the algorithm may in fact choose the actual goal state, if it knows it, with some probability. This probability is a real number between 0.0 and 1.0; its value should usually be around 0.05 and should not be too large. It is probably a good idea to use the default value.
Definition: STRIDE.h:103
void addMotion(Motion *motion)
Add a motion to the exploration tree.
Definition: STRIDE.cpp:221
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: STRIDE.h:273
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68