All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
TRRT.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: Dave Coleman */
36 
37 #include "ompl/geometric/planners/rrt/TRRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include "ompl/tools/config/MagicConstants.h"
41 #include <limits>
42 
43 ompl::geometric::TRRT::TRRT(const base::SpaceInformationPtr &si) : base::Planner(si, "TRRT")
44 {
45  // Standard RRT Variables
47  specs_.directed = true;
48 
49  goalBias_ = 0.05;
50  maxDistance_ = 0.0; // set in setup()
51  lastGoalMotion_ = NULL;
52 
53  Planner::declareParam<double>("range", this, &TRRT::setRange, &TRRT::getRange, "0.:1.:10000.");
54  Planner::declareParam<double>("goal_bias", this, &TRRT::setGoalBias, &TRRT::getGoalBias, "0.:.05:1.");
55 
56  // TRRT Specific Variables
57  frontierThreshold_ = 0.0; // set in setup()
58  kConstant_ = 0.0; // set in setup()
59  maxStatesFailed_ = 10; // threshold for when to start increasing the temperatuer
60  tempChangeFactor_ = 2.0; // how much to decrease or increase the temp each time
61  minTemperature_ = 10e-10; // lower limit of the temperature change
62  initTemperature_ = 10e-6; // where the temperature starts out
63  frontierNodeRatio_ = 0.1; // 1/10, or 1 nonfrontier for every 10 frontier
64 
65  Planner::declareParam<unsigned int>("max_states_failed", this, &TRRT::setMaxStatesFailed, &TRRT::getMaxStatesFailed, "1:1000");
66  Planner::declareParam<double>("temp_change_factor", this, &TRRT::setTempChangeFactor, &TRRT::getTempChangeFactor,"0.:.1:10.");
67  Planner::declareParam<double>("min_temperature", this, &TRRT::setMinTemperature, &TRRT::getMinTemperature);
68  Planner::declareParam<double>("init_temperature", this, &TRRT::setInitTemperature, &TRRT::getInitTemperature);
69  Planner::declareParam<double>("frontier_threshold", this, &TRRT::setFrontierThreshold, &TRRT::getFrontierThreshold);
70  Planner::declareParam<double>("frontierNodeRatio", this, &TRRT::setFrontierNodeRatio, &TRRT::getFrontierNodeRatio);
71  Planner::declareParam<double>("k_constant", this, &TRRT::setKConstant, &TRRT::getKConstant);
72 }
73 
74 ompl::geometric::TRRT::~TRRT(void)
75 {
76  freeMemory();
77 }
78 
80 {
81  Planner::clear();
82  sampler_.reset();
83  freeMemory();
84  if (nearestNeighbors_)
85  nearestNeighbors_->clear();
86  lastGoalMotion_ = NULL;
87 
88  // Clear TRRT specific variables ---------------------------------------------------------
89  numStatesFailed_ = 0;
90  temp_ = initTemperature_;
91  nonfrontierCount_ = 1;
92  frontierCount_ = 1; // init to 1 to prevent division by zero error
93 }
94 
96 {
97  Planner::setup();
98  tools::SelfConfig selfConfig(si_, getName());
99 
100  // Find the average cost of states by sampling x=100 random states
101  double averageCost = si_->averageStateCost(100);
102 
103  // Set maximum distance a new node can be from its nearest neighbor
104  if (maxDistance_ < std::numeric_limits<double>::epsilon())
105  {
106  selfConfig.configurePlannerRange(maxDistance_);
108  }
109 
110  // Set the threshold that decides if a new node is a frontier node or non-frontier node
111  if (frontierThreshold_ < std::numeric_limits<double>::epsilon())
112  {
113  frontierThreshold_ = si_->getMaximumExtent() * 0.01; // 5.0
114  OMPL_DEBUG("Frontier threshold detected to be %lf", frontierThreshold_);
115  }
116 
117  // Autoconfigure the K constant
118  if (kConstant_ < std::numeric_limits<double>::epsilon())
119  {
120  kConstant_ = averageCost;
121  OMPL_DEBUG("K constant detected to be %lf", kConstant_);
122  }
123 
124  // Create the nearest neighbor function the first time setup is run
125  if (!nearestNeighbors_)
126  nearestNeighbors_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
127 
128  // Set the distance function
129  nearestNeighbors_->setDistanceFunction(boost::bind(&TRRT::distanceFunction, this, _1, _2));
130 
131  // Setup TRRT specific variables ---------------------------------------------------------
132  numStatesFailed_ = 0;
133  temp_ = initTemperature_;
134  nonfrontierCount_ = 1;
135  frontierCount_ = 1; // init to 1 to prevent division by zero error
136 }
137 
139 {
140  // Delete all motions, states and the nearest neighbors data structure
141  if (nearestNeighbors_)
142  {
143  std::vector<Motion*> motions;
144  nearestNeighbors_->list(motions);
145  for (unsigned int i = 0 ; i < motions.size() ; ++i)
146  {
147  if (motions[i]->state)
148  si_->freeState(motions[i]->state);
149  delete motions[i];
150  }
151  }
152 }
153 
156 {
157  // Basic error checking
158  checkValidity();
159 
160  // Goal information
161  base::Goal *goal = pdef_->getGoal().get();
162  base::GoalSampleableRegion *goalRegion = dynamic_cast<base::GoalSampleableRegion*>(goal);
163 
164  // Object for getting the cost of a state
165  const base::StateValidityCheckerPtr &stateValidityChecker = si_->getStateValidityChecker();
166 
167  // Input States ---------------------------------------------------------------------------------
168 
169  // Loop through valid input states and add to tree
170  while (const base::State *state = pis_.nextStart())
171  {
172  // Allocate memory for a new start state motion based on the "space-information"-size
173  Motion *motion = new Motion(si_);
174 
175  // Copy destination <= source (backwards???)
176  si_->copyState(motion->state, state);
177 
178  // Set cost for this start state
179  motion->cost = stateValidityChecker->cost(motion->state);
180 
181  // Add start motion to the tree
182  nearestNeighbors_->add(motion);
183  }
184 
185  // Check that input states exist
186  if (nearestNeighbors_->size() == 0)
187  {
188  OMPL_ERROR("There are no valid initial states!");
190  }
191 
192  // Create state sampler if this is TRRT's first run
193  if (!sampler_)
194  sampler_ = si_->allocStateSampler();
195 
196  // Debug
197  OMPL_INFORM("Starting with %u states", nearestNeighbors_->size());
198 
199 
200  // Solver variables ------------------------------------------------------------------------------------
201 
202  // the final solution
203  Motion *solution = NULL;
204  // the approximate solution, returned if no final solution found
205  Motion *approxSolution = NULL;
206  // track the distance from goal to closest solution yet found
207  double approxDifference = std::numeric_limits<double>::infinity();
208 
209  // distance between states - the intial state and the interpolated state (may be the same)
210  double randMotionDistance;
211  double motionDistance;
212 
213  // Create random motion and a pointer (for optimization) to its state
214  Motion *randMotion = new Motion(si_);
215  Motion *nearMotion;
216 
217  // STATES
218  // The random state
219  base::State *randState = randMotion->state;
220  // The new state that is generated between states *to* and *from*
221  base::State *interpolatedState = si_->allocState(); // Allocates "space information"-sized memory for a state
222  // The chosen state btw rand_state and interpolated_state
223  base::State *newState;
224 
225  // Begin sampling --------------------------------------------------------------------------------------
226  while (plannerTerminationCondition() == false)
227  {
228  // I.
229 
230  // Sample random state (with goal biasing probability)
231  if (goalRegion && rng_.uniform01() < goalBias_ && goalRegion->canSample())
232  {
233  // Bias sample towards goal
234  goalRegion->sampleGoal(randState);
235  }
236  else
237  {
238  // Uniformly Sample
239  sampler_->sampleUniform(randState);
240  }
241 
242  // II.
243 
244  // Find closest state in the tree
245  nearMotion = nearestNeighbors_->nearest(randMotion);
246 
247  // III.
248 
249  // Distance from near state q_n to a random state
250  randMotionDistance = si_->distance(nearMotion->state, randState);
251 
252  // Check if the rand_state is too far away
253  if(randMotionDistance > maxDistance_)
254  {
255  // Computes the state that lies at time t in [0, 1] on the segment that connects *from* state to *to* state.
256  // The memory location of *state* is not required to be different from the memory of either *from* or *to*.
257  si_->getStateSpace()->interpolate(nearMotion->state, randState,
258  maxDistance_ / randMotionDistance, interpolatedState);
259 
260  // Update the distance between near and new with the interpolated_state
261  motionDistance = si_->distance(nearMotion->state, interpolatedState);
262 
263  // Use the interpolated state as the new state
264  newState = interpolatedState;
265  }
266  else
267  {
268  // Random state is close enough
269  newState = randState;
270 
271  // Copy the distance
272  motionDistance = randMotionDistance;
273  }
274 
275  // IV.
276  // this stage integrates collision detections in the presence of obstacles and checks for collisions
277 
285  if(!si_->checkMotion(nearMotion->state, newState))
286  continue; // try a new sample
287 
288 
289  // Minimum Expansion Control
290  // A possible side effect may appear when the tree expansion toward unexplored regions remains slow, and the
291  // new nodes contribute only to refine already explored regions.
292  if(!minExpansionControl(randMotionDistance))
293  {
294  continue; // give up on this one and try a new sample
295  }
296 
297  double childCost = stateValidityChecker->cost(newState);
298 
299  // Only add this motion to the tree if the tranistion test accepts it
300  if(!transitionTest(childCost, nearMotion->cost, motionDistance))
301  {
302  continue; // give up on this one and try a new sample
303  }
304 
305  // V.
306 
307  // Create a motion
308  Motion *motion = new Motion(si_);
309  si_->copyState(motion->state, newState);
310  motion->parent = nearMotion; // link q_new to q_near as an edge
311  motion->distance = motionDistance; // cache the distance btw parent and state
312  motion->cost = childCost;
313 
314  // Add motion to data structure
315  nearestNeighbors_->add(motion);
316 
317  // VI.
318 
319  // Check if this motion is the goal
320  double distToGoal = 0.0;
321  bool isSatisfied = goal->isSatisfied(motion->state, &distToGoal);
322  if (isSatisfied)
323  {
324  approxDifference = distToGoal; // the tolerated error distance btw state and goal
325  solution = motion; // set the final solution
326  break;
327  }
328 
329  // Is this the closest solution we've found so far
330  if (distToGoal < approxDifference)
331  {
332  approxDifference = distToGoal;
333  approxSolution = motion;
334  }
335 
336  } // end of solver sampling loop
337 
338 
339  // Finish solution processing --------------------------------------------------------------------
340 
341  bool solved = false;
342  bool approximate = false;
343 
344  // Substitute an empty solution with the best approximation
345  if (solution == NULL)
346  {
347  solution = approxSolution;
348  approximate = true;
349  }
350 
351  // Generate solution path for real/approx solution
352  if (solution != NULL)
353  {
354  lastGoalMotion_ = solution;
355 
356  // construct the solution path
357  std::vector<Motion*> mpath;
358  while (solution != NULL)
359  {
360  mpath.push_back(solution);
361  solution = solution->parent;
362  }
363 
364  // set the solution path
365  PathGeometric *path = new PathGeometric(si_);
366  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
367  path->append(mpath[i]->state);
368 
369  pdef_->addSolutionPath(base::PathPtr(path), approximate, approxDifference);
370  solved = true;
371  }
372 
373  // Clean up ---------------------------------------------------------------------------------------
374 
375  si_->freeState(interpolatedState);
376  if (randMotion->state)
377  si_->freeState(randMotion->state);
378  delete randMotion;
379 
380  OMPL_INFORM("Created %u states", nearestNeighbors_->size());
381 
382  return base::PlannerStatus(solved, approximate);
383 }
384 
386 {
387  Planner::getPlannerData(data);
388 
389  std::vector<Motion*> motions;
390  if (nearestNeighbors_)
391  nearestNeighbors_->list(motions);
392 
393  if (lastGoalMotion_)
394  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
395 
396  for (unsigned int i = 0 ; i < motions.size() ; ++i)
397  {
398  if (motions[i]->parent == NULL)
399  data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
400  else
401  data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
402  base::PlannerDataVertex(motions[i]->state));
403  }
404 }
405 
406 bool ompl::geometric::TRRT::transitionTest(double childCost, double parentCost, double distance)
407 {
408  // Always accept if new state has same or lower cost than old state
409  if(childCost <= parentCost)
410  return true;
411 
412  // Difference in cost
413  double costSlope = (childCost - parentCost) / distance;
414 
415  // The probability of acceptance of a new configuration is defined by comparing its cost c_j
416  // relatively to the cost c_i of its parent in the tree. Based on the Metropolis criterion.
417  double transitionProbability = 1.; // if cost_slope is <= 0, probabilty is 1
418 
419  // Only return at end
420  bool result = false;
421 
422  // Calculate tranision probabilty
423  if(costSlope > 0)
424  {
425  transitionProbability = exp(-costSlope / (kConstant_ * temp_));
426  }
427 
428  // Check if we can accept it
429  if(rng_.uniform01() <= transitionProbability)
430  {
431  if (temp_ > minTemperature_)
432  {
433  temp_ /= tempChangeFactor_;
434 
435  // Prevent temp_ from getting too small
436  if(temp_ < minTemperature_)
437  {
438  temp_ = minTemperature_;
439  }
440  }
441 
442  numStatesFailed_ = 0;
443 
444  result = true;
445  }
446  else
447  {
448  // State has failed
449  if(numStatesFailed_ >= maxStatesFailed_)
450  {
451  temp_ *= tempChangeFactor_;
452  numStatesFailed_ = 0;
453  }
454  else
455  {
456  ++numStatesFailed_;
457  }
458 
459  }
460 
461  return result;
462 }
463 
464 bool ompl::geometric::TRRT::minExpansionControl(double randMotionDistance)
465 {
466  // Decide to accept or not
467  if(randMotionDistance > frontierThreshold_)
468  {
469  // participates in the tree expansion
470  ++frontierCount_;
471 
472  return true;
473  }
474  else
475  {
476  // participates in the tree refinement
477 
478  // check our ratio first before accepting it
479  if(nonfrontierCount_ / frontierCount_ > frontierNodeRatio_)
480  {
481  // Increment so that the temperature rises faster
482  ++numStatesFailed_;
483 
484  // reject this node as being too much refinement
485  return false;
486  }
487  else
488  {
489  ++nonfrontierCount_;
490  return true;
491  }
492  }
493 }