All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
LBTRRT.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Oren Salzman
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: Oren Salzman, Sertac Karaman, Ioan Sucan */
36 
37 #include "ompl/geometric/planners/rrt/LBTRRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/datastructures/NearestNeighborsGNAT.h"
40 #include "ompl/tools/config/SelfConfig.h"
41 #include <limits>
42 #include <math.h>
43 
44 const double ompl::geometric::LBTRRT::kRRG = 5.5;
45 
47  : base::Planner(si, "LBTRRT")
48 {
49 
51  specs_.directed = true;
52 
53  goalBias_ = 0.05;
54  maxDistance_ = 0.0;
55  lastGoalMotion_ = NULL;
56  epsilon_ = 0.4;
57 
58 
59  Planner::declareParam<double>("range", this, &LBTRRT::setRange, &LBTRRT::getRange);
60  Planner::declareParam<double>("goal_bias", this, &LBTRRT::setGoalBias, &LBTRRT::getGoalBias);
61  Planner::declareParam<double>("epsilon", this, &LBTRRT::setApproximationFactor, &LBTRRT::getApproximationFactor);
62 }
63 
64 ompl::geometric::LBTRRT::~LBTRRT(void)
65 {
66  freeMemory();
67 }
68 
70 {
71  Planner::clear();
72  sampler_.reset();
73  freeMemory();
74  if (nn_)
75  nn_->clear();
76  lastGoalMotion_ = NULL;
77 }
78 
80 {
81  Planner::setup();
82  tools::SelfConfig sc(si_, getName());
83  sc.configurePlannerRange(maxDistance_);
84 
85  if (!nn_)
86  nn_.reset(new NearestNeighborsGNAT<Motion*>());
87  nn_->setDistanceFunction(boost::bind(&LBTRRT::distanceFunction, this, _1, _2));
88 }
89 
91 {
92  if (nn_)
93  {
94  std::vector<Motion*> motions;
95  nn_->list(motions);
96  for (unsigned int i = 0 ; i < motions.size() ; ++i)
97  {
98  if (motions[i]->state)
99  si_->freeState(motions[i]->state);
100  delete motions[i];
101  }
102  }
103 }
104 
106 {
107  checkValidity();
108  base::Goal *goal = pdef_->getGoal().get();
109  base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
110 
111  while (const base::State *st = pis_.nextStart())
112  {
113  Motion *motion = new Motion(si_);
114  si_->copyState(motion->state, st);
115  nn_->add(motion);
116  }
117 
118  if (nn_->size() == 0)
119  {
120  OMPL_ERROR("There are no valid initial states!");
122  }
123 
124  if (!sampler_)
125  sampler_ = si_->allocStateSampler();
126 
127  OMPL_INFORM("Starting with %u states", nn_->size());
128 
129  Motion *solution = NULL;
130  Motion *approxSol = NULL;
131  double approxdif = std::numeric_limits<double>::infinity();
132 
133  Motion *rmotion = new Motion(si_);
134  base::State *rstate = rmotion->state;
135  base::State *xstate = si_->allocState();
136 
137  while ( ptc() == false )
138  {
139  /* sample random state (with goal biasing) */
140  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
141  goal_s->sampleGoal(rstate);
142  else
143  sampler_->sampleUniform(rstate);
144 
145  /* find closest state in the tree */
146  Motion *nmotion = nn_->nearest(rmotion);
147  base::State *dstate = rstate;
148 
149  /* find state to add */
150  double d = si_->distance(nmotion->state, rstate);
151  if (d > maxDistance_)
152  {
153  si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
154  dstate = xstate;
155  }
156 
157  if (si_->checkMotion(nmotion->state, dstate))
158  {
159  /* create a motion */
160  Motion *motion = new Motion(si_);
161  si_->copyState(motion->state, dstate);
162 
163  /* update fields */
164  motion->parentLb_ = nmotion;
165  motion->parentApx_ = nmotion;
166  double distN = distanceFunction(nmotion, motion);
167  motion->costLb_ = nmotion->costLb_ + distN;
168  motion->costApx_ = nmotion->costApx_ + distN;
169 
170  nmotion->childrenLb_.push_back(motion);
171  nmotion->childrenApx_.push_back(motion);
172 
173  nn_->add(motion);
174 
175  /* do lazy rewiring */
176  double k = std::log(double(nn_->size())) * kRRG;
177  std::vector<Motion *> nnVec;
178  nn_->nearestK(rmotion, static_cast<int>(k), nnVec);
179 
180  IsLessThan isLessThan(this,motion);
181  std::sort (nnVec.begin(), nnVec.end(), isLessThan);
182 
183  for (std::size_t i = 0; i < nnVec.size(); ++i)
184  attemptNodeUpdate(motion, nnVec[i]);
185 
186  for (std::size_t i = 0; i < nnVec.size(); ++i)
187  attemptNodeUpdate(nnVec[i], motion);
188 
189  double dist = 0.0;
190  bool sat = goal->isSatisfied(motion->state, &dist);
191  if (sat || dist < approxdif)
192  {
193  approxdif = dist;
194  solution = motion;
195  }
196  }
197  }
198 
199  bool solved = false;
200  bool approximate = false;
201 
202  if (solution == NULL)
203  {
204  solution = approxSol;
205  approximate = true;
206  }
207 
208  if (solution != NULL)
209  {
210  lastGoalMotion_ = solution;
211 
212  /* construct the solution path */
213  std::vector<Motion*> mpath;
214  while (solution != NULL)
215  {
216  mpath.push_back(solution);
217  solution = solution->parentApx_;
218  }
219 
220  /* set the solution path */
221  PathGeometric *path = new PathGeometric(si_);
222  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
223  path->append(mpath[i]->state);
224  pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif);
225  solved = true;
226  }
227 
228  si_->freeState(xstate);
229  if (rmotion->state)
230  si_->freeState(rmotion->state);
231  delete rmotion;
232 
233  OMPL_INFORM("Created %u states", nn_->size());
234 
235  return base::PlannerStatus(solved, approximate);
236 }
237 
239 {
240  double dist = distanceFunction(potentialParent, child);
241  double potentialLb = potentialParent->costLb_ + dist;
242  double potentialApx = potentialParent->costApx_ + dist;
243 
244  if (child->costLb_ <= potentialLb)
245  return;
246 
247  if (child->costApx_ > 1.0 + epsilon_ * potentialLb)
248  {
249  if (si_->checkMotion(potentialParent->state, child->state) == false)
250  return;
251 
252  removeFromParentLb(child);
253  double deltaLb = potentialLb - child->costLb_;
254  child->parentLb_ = potentialParent;
255  potentialParent->childrenLb_.push_back(child);
256  child->costLb_ = potentialLb;
257  updateChildCostsLb(child, deltaLb);
258 
259 
260  if (child->costApx_ <= potentialApx)
261  return;
262 
263  removeFromParentApx(child);
264  double deltaApx = potentialApx - child->costApx_;
265  child->parentApx_ = potentialParent;
266  potentialParent->childrenApx_.push_back(child);
267  child->costApx_ = potentialApx;
268  updateChildCostsApx(child, deltaApx);
269  }
270  else //(child->costApx_ <= 1 + epsilon_ * potentialLb)
271  {
272  removeFromParentLb(child);
273  double deltaLb = potentialLb - child->costLb_;
274  child->parentLb_ = potentialParent;
275  potentialParent->childrenLb_.push_back(child);
276  child->costLb_ = potentialLb;
277  updateChildCostsLb(child, deltaLb);
278  }
279 
280  return;
281 }
282 
284 {
285  Planner::getPlannerData(data);
286 
287  std::vector<Motion*> motions;
288  if (nn_)
289  nn_->list(motions);
290 
291  if (lastGoalMotion_)
292  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
293 
294  for (unsigned int i = 0 ; i < motions.size() ; ++i)
295  {
296  if (motions[i]->parentApx_ == NULL)
297  data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
298  else
299  data.addEdge(base::PlannerDataVertex(motions[i]->parentApx_->state),
300  base::PlannerDataVertex(motions[i]->state));
301  }
302 }
303 
304 
306 {
307  for (size_t i = 0; i < m->childrenLb_.size(); ++i)
308  {
309  m->childrenLb_[i]->costLb_ += delta;
310  updateChildCostsLb(m->childrenLb_[i], delta);
311  }
312 }
314 {
315  for (size_t i = 0; i < m->childrenApx_.size(); ++i)
316  {
317  m->childrenApx_[i]->costApx_ += delta;
318  updateChildCostsApx(m->childrenApx_[i], delta);
319  }
320 }
321 
323 {
324  return removeFromParent(m, m->parentLb_->childrenLb_);
325 }
327 {
328  return removeFromParent(m, m->parentApx_->childrenApx_);
329 }
330 void ompl::geometric::LBTRRT::removeFromParent(const Motion *m, std::vector<Motion*>& vec)
331 {
332  std::vector<Motion*>::iterator it = vec.begin ();
333  while (it != vec.end ())
334  {
335  if (*it == m)
336  {
337  it = vec.erase(it);
338  it = vec.end ();
339  }
340  else
341  ++it;
342  }
343 }
void updateChildCostsApx(Motion *m, double delta)
update the child cost of the approximation tree
Definition: LBTRRT.cpp:313
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
void attemptNodeUpdate(Motion *potentialParent, Motion *child)
attempt to rewire the trees
Definition: LBTRRT.cpp:238
bool canSample(void) const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
void removeFromParentLb(Motion *m)
remove motion from its parent in the lower bound tree
Definition: LBTRRT.cpp:322
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
double getRange(void) const
Get the range the planner is using.
Definition: LBTRRT.h:113
void setApproximationFactor(double epsilon)
Set the apprimation factor.
Definition: LBTRRT.h:128
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:104
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: LBTRRT.cpp:105
LBTRRT(const base::SpaceInformationPtr &si)
Constructor.
Definition: LBTRRT.cpp:46
Motion * parentLb_
The parent motion in the exploration tree.
Definition: LBTRRT.h:168
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
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...
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: LBTRRT.cpp:283
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: LBTRRT.h:236
base::State * state
The state contained by the motion.
Definition: LBTRRT.h:165
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
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: LBTRRT.h:221
void removeFromParentApx(Motion *m)
remove motion from its parent in the approximation tree
Definition: LBTRRT.cpp:326
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
Definition: LBTRRT.h:233
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.
double getApproximationFactor(void) const
Get the apprimation factor.
Definition: LBTRRT.h:134
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: LBTRRT.h:245
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void freeMemory(void)
Free the memory allocated by this planner.
Definition: LBTRRT.cpp:90
void updateChildCostsLb(Motion *m, double delta)
update the child cost of the lower bound tree
Definition: LBTRRT.cpp:305
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.
double epsilon_
approximation factor
Definition: LBTRRT.h:239
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: LBTRRT.h:107
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: LBTRRT.cpp:69
Motion * parentApx_
The parent motion in the exploration tree.
Definition: LBTRRT.h:171
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:404
static const double kRRG
kRRG = 2e~5.5 is a valid choice for all problem instances
Definition: LBTRRT.h:141
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
void setGoalBias(double goalBias)
Set the goal bias.
Definition: LBTRRT.h:91
Representation of a motion.
Definition: LBTRRT.h:147
void removeFromParent(const Motion *m, std::vector< Motion * > &vec)
remove motion from a vector
Definition: LBTRRT.cpp:330
double getGoalBias(void) const
Get the goal bias the planner is using.
Definition: LBTRRT.h:97
A boost shared pointer wrapper for ompl::base::Path.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LBTRRT.cpp:79
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68