BiTRRT.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Rice University
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 Rice University 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: Ryan Luna */
36 
37 #include <limits>
38 
39 #include "ompl/geometric/planners/rrt/BiTRRT.h"
40 #include "ompl/base/goals/GoalSampleableRegion.h"
41 #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h"
42 #include "ompl/tools/config/MagicConstants.h"
43 #include "ompl/tools/config/SelfConfig.h"
44 
45 ompl::geometric::BiTRRT::BiTRRT(const base::SpaceInformationPtr &si) : base::Planner(si, "BiTRRT")
46 {
48  specs_.directed = true;
49 
50  Planner::declareParam<double>("range", this, &BiTRRT::setRange, &BiTRRT::getRange, "0.:1.:10000.");
51 
52  // BiTRRT Specific Variables
53  setTempChangeFactor(0.1); // how much to increase the temp each time
54 
55  Planner::declareParam<double>("temp_change_factor", this, &BiTRRT::setTempChangeFactor,
56  &BiTRRT::getTempChangeFactor, "0.:.1:1.");
57  Planner::declareParam<double>("init_temperature", this, &BiTRRT::setInitTemperature, &BiTRRT::getInitTemperature);
58  Planner::declareParam<double>("frontier_threshold", this, &BiTRRT::setFrontierThreshold,
60  Planner::declareParam<double>("frontier_node_ratio", this, &BiTRRT::setFrontierNodeRatio,
62  Planner::declareParam<double>("cost_threshold", this, &BiTRRT::setCostThreshold, &BiTRRT::getCostThreshold);
63 }
64 
65 ompl::geometric::BiTRRT::~BiTRRT()
66 {
67  freeMemory();
68 }
69 
71 {
72  std::vector<Motion *> motions;
73 
74  if (tStart_)
75  {
76  tStart_->list(motions);
77  for (auto &motion : motions)
78  {
79  if (motion->state != nullptr)
80  si_->freeState(motion->state);
81  delete motion;
82  }
83  }
84 
85  if (tGoal_)
86  {
87  tGoal_->list(motions);
88  for (auto &motion : motions)
89  {
90  if (motion->state != nullptr)
91  si_->freeState(motion->state);
92  delete motion;
93  }
94  }
95 }
96 
98 {
99  Planner::clear();
100  freeMemory();
101  if (tStart_)
102  tStart_->clear();
103  if (tGoal_)
104  tGoal_->clear();
105  connectionPoint_ = std::make_pair<Motion *, Motion *>(nullptr, nullptr);
106 
107  // TRRT specific variables
108  temp_ = initTemperature_;
109  nonfrontierCount_ = 1;
110  frontierCount_ = 1; // init to 1 to prevent division by zero error
111  if (opt_)
112  bestCost_ = worstCost_ = opt_->identityCost();
113 }
114 
116 {
117  Planner::setup();
118  tools::SelfConfig sc(si_, getName());
119 
120  // Configuring the range of the planner
121  if (maxDistance_ < std::numeric_limits<double>::epsilon())
122  {
123  sc.configurePlannerRange(maxDistance_);
125  }
126 
127  // Configuring nearest neighbors structures for the planning trees
128  if (!tStart_)
129  tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
130  if (!tGoal_)
131  tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
132  tStart_->setDistanceFunction([this](const Motion *a, const Motion *b)
133  {
134  return distanceFunction(a, b);
135  });
136  tGoal_->setDistanceFunction([this](const Motion *a, const Motion *b)
137  {
138  return distanceFunction(a, b);
139  });
140 
141  // Setup the optimization objective, if it isn't specified
142  if (!pdef_ || !pdef_->hasOptimizationObjective())
143  {
144  OMPL_INFORM("%s: No optimization objective specified. Defaulting to mechanical work minimization.",
145  getName().c_str());
146  opt_ = std::make_shared<base::MechanicalWorkOptimizationObjective>(si_);
147  }
148  else
149  opt_ = pdef_->getOptimizationObjective();
150 
151  // Set the threshold that decides if a new node is a frontier node or non-frontier node
152  if (frontierThreshold_ < std::numeric_limits<double>::epsilon())
153  {
154  frontierThreshold_ = si_->getMaximumExtent() * 0.01;
155  OMPL_DEBUG("%s: Frontier threshold detected to be %lf", getName().c_str(), frontierThreshold_);
156  }
157 
158  // initialize TRRT specific variables
159  temp_ = initTemperature_;
160  nonfrontierCount_ = 1;
161  frontierCount_ = 1; // init to 1 to prevent division by zero error
162  bestCost_ = worstCost_ = opt_->identityCost();
163  connectionRange_ = 10.0 * si_->getStateSpace()->getLongestValidSegmentLength();
164 }
165 
167  Motion *parent)
168 {
169  auto *motion = new Motion(si_);
170  si_->copyState(motion->state, state);
171  motion->cost = opt_->stateCost(motion->state);
172  motion->parent = parent;
173  motion->root = parent != nullptr ? parent->root : nullptr;
174 
175  if (opt_->isCostBetterThan(motion->cost, bestCost_)) // motion->cost is better than the existing best
176  bestCost_ = motion->cost;
177  if (opt_->isCostBetterThan(worstCost_, motion->cost)) // motion->cost is worse than the existing worst
178  worstCost_ = motion->cost;
179 
180  // Add start motion to the tree
181  tree->add(motion);
182  return motion;
183 }
184 
186 {
187  // Disallow any cost that is not better than the cost threshold
188  if (!opt_->isCostBetterThan(motionCost, costThreshold_))
189  return false;
190 
191  // Always accept if the cost is near or below zero
192  if (motionCost.value() < 1e-4)
193  return true;
194 
195  double dCost = motionCost.value();
196  double transitionProbability = exp(-dCost / temp_);
197  if (transitionProbability > 0.5)
198  {
199  double costRange = worstCost_.value() - bestCost_.value();
200  if (fabs(costRange) > 1e-4) // Do not divide by zero
201  // Successful transition test. Decrease the temperature slightly
202  temp_ /= exp(dCost / (0.1 * costRange));
203 
204  return true;
205  }
206 
207  // The transition failed. Increase the temperature (slightly)
208  temp_ *= tempChangeFactor_;
209  return false;
210 }
211 
213 {
214  if (dist > frontierThreshold_) // Exploration
215  {
216  ++frontierCount_;
217  return true;
218  }
219  // Refinement
220  // Check the current ratio first before accepting it
221  if ((double)nonfrontierCount_ / (double)frontierCount_ > frontierNodeRatio_)
222  return false;
223 
224  ++nonfrontierCount_;
225  return true;
226 }
227 
229  Motion *toMotion, Motion *&result)
230 {
231  bool reach = true;
232 
233  // Compute the state to extend toward
234  bool treeIsStart = (tree == tStart_);
235  double d = (treeIsStart ? si_->distance(nearest->state, toMotion->state)
236  : si_->distance(toMotion->state, nearest->state));
237  // Truncate the random state to be no more than maxDistance_ from nearest neighbor
238  if (d > maxDistance_)
239  {
240  if (tree == tStart_)
241  si_->getStateSpace()->interpolate(nearest->state, toMotion->state, maxDistance_ / d, toMotion->state);
242  else
243  si_->getStateSpace()->interpolate(toMotion->state, nearest->state, 1.0 - maxDistance_ / d, toMotion->state);
244  d = maxDistance_;
245  reach = false;
246  }
247 
248  // Validating the motion
249  // If we are in the goal tree, we validate the motion in reverse
250  // si_->checkMotion assumes that the first argument is valid, so we must check this explicitly
251  // If the motion is valid, check the probabilistic transition test and the
252  // expansion control to ensure high quality nodes are added.
253  bool validMotion =
254  (tree == tStart_ ? si_->checkMotion(nearest->state, toMotion->state) :
255  si_->isValid(toMotion->state) && si_->checkMotion(toMotion->state, nearest->state)) &&
256  transitionTest(tree == tStart_ ? opt_->motionCost(nearest->state, toMotion->state)
257  : opt_->motionCost(toMotion->state, nearest->state)) &&
258  minExpansionControl(d);
259 
260  if (validMotion)
261  {
262  result = addMotion(toMotion->state, tree, nearest);
263  return reach ? SUCCESS : ADVANCED;
264  }
265 
266  return FAILED;
267 }
268 
270  Motion *&result)
271 {
272  // Nearest neighbor
273  Motion *nearest = tree->nearest(toMotion);
274  return extendTree(nearest, tree, toMotion, result);
275 }
276 
278 {
279  // Get the nearest state to nmotion in tree (nmotion is NOT in tree)
280  Motion *nearest = tree->nearest(nmotion);
281  bool treeIsStart = tree == tStart_;
282  double dist = (treeIsStart ? si_->distance(nearest->state, nmotion->state)
283  : si_->distance(nmotion->state, nearest->state));
284 
285  // Do not attempt a connection if the trees are far apart
286  if (dist > connectionRange_)
287  return false;
288 
289  // Copy the resulting state into our scratch space
290  si_->copyState(xmotion->state, nmotion->state);
291 
292  // Do not try to connect states directly. Must chop up the
293  // extension into segments, just in case one piece fails
294  // the transition test
295  GrowResult result;
296  Motion *next = nullptr;
297  do
298  {
299  // Extend tree from nearest toward xmotion
300  // Store the result into next
301  // This function MAY trash xmotion
302  result = extendTree(nearest, tree, xmotion, next);
303 
304  if (result == ADVANCED)
305  {
306  nearest = next;
307 
308  // xmotion may get trashed during extension, so we reload it here
309  si_->copyState(xmotion->state,
310  nmotion->state); // xmotion may get trashed during extension, so we reload it here
311  }
312  } while (result == ADVANCED);
313 
314  // Successful connection
315  if (result == SUCCESS)
316  {
317  Motion *startMotion = treeIsStart ? next : nmotion;
318  Motion *goalMotion = treeIsStart ? nmotion : next;
319 
320  // Make sure start-goal pair is valid
321  if (pdef_->getGoal()->isStartGoalPairValid(startMotion->root, goalMotion->root))
322  {
323  // Since we have connected, nmotion->state and next->state have the same value
324  // We need to check one of their parents to avoid a duplicate state in the solution path
325  // One of these must be true, since we do not ever attempt to connect start and goal directly.
326  if (startMotion->parent != nullptr)
327  startMotion = startMotion->parent;
328  else
329  goalMotion = goalMotion->parent;
330 
331  connectionPoint_ = std::make_pair(startMotion, goalMotion);
332  return true;
333  }
334  }
335 
336  return false;
337 }
338 
340 {
341  // Basic error checking
342  checkValidity();
343 
344  // Goal information
345  base::Goal *goal = pdef_->getGoal().get();
346  auto *gsr = dynamic_cast<base::GoalSampleableRegion *>(goal);
347 
348  if (gsr == nullptr)
349  {
350  OMPL_ERROR("%s: Goal object does not derive from GoalSampleableRegion", getName().c_str());
352  }
353 
354  // Loop through the (valid) input states and add them to the start tree
355  while (const base::State *state = pis_.nextStart())
356  {
357  auto *motion = new Motion(si_);
358  si_->copyState(motion->state, state);
359  motion->cost = opt_->stateCost(motion->state);
360  motion->root = motion->state; // this state is the root of a tree
361 
362  if (tStart_->size() == 0) // do not overwrite best/worst from a prior call to solve
363  worstCost_ = bestCost_ = motion->cost;
364 
365  // Add start motion to the tree
366  tStart_->add(motion);
367  }
368 
369  if (tStart_->size() == 0)
370  {
371  OMPL_ERROR("%s: Start tree has no valid states!", getName().c_str());
373  }
374 
375  // Do the same for the goal tree, if it is empty, but only once
376  if (tGoal_->size() == 0)
377  {
378  const base::State *state = pis_.nextGoal(ptc);
379  if (state != nullptr)
380  {
381  Motion *motion = addMotion(state, tGoal_);
382  motion->root = motion->state; // this state is the root of a tree
383  }
384  }
385 
386  if (tGoal_->size() == 0)
387  {
388  OMPL_ERROR("%s: Goal tree has no valid states!", getName().c_str());
390  }
391 
392  OMPL_INFORM("%s: Planning started with %d states already in datastructure", getName().c_str(),
393  (int)(tStart_->size() + tGoal_->size()));
394 
395  base::StateSamplerPtr sampler = si_->allocStateSampler();
396 
397  auto *rmotion = new Motion(si_);
398  base::State *rstate = rmotion->state;
399 
400  auto *xmotion = new Motion(si_);
401  base::State *xstate = xmotion->state;
402 
403  TreeData tree = tStart_;
404  TreeData otherTree = tGoal_;
405 
406  bool solved = false;
407  // Planning loop
408  while (!ptc)
409  {
410  // Check if there are more goal states
411  if (pis_.getSampledGoalsCount() < tGoal_->size() / 2)
412  {
413  if (const base::State *state = pis_.nextGoal())
414  {
415  Motion *motion = addMotion(state, tGoal_);
416  motion->root = motion->state; // this state is the root of a tree
417  }
418  }
419 
420  // Sample a state uniformly at random
421  sampler->sampleUniform(rstate);
422 
423  Motion *result; // the motion that gets added in extendTree
424  if (extendTree(rmotion, tree, result) != FAILED) // we added something new to the tree
425  {
426  // Try to connect the other tree to the node we just added
427  if (connectTrees(result, otherTree, xmotion))
428  {
429  // The trees have been connected. Construct the solution path
430  Motion *solution = connectionPoint_.first;
431  std::vector<Motion *> mpath1;
432  while (solution != nullptr)
433  {
434  mpath1.push_back(solution);
435  solution = solution->parent;
436  }
437 
438  solution = connectionPoint_.second;
439  std::vector<Motion *> mpath2;
440  while (solution != nullptr)
441  {
442  mpath2.push_back(solution);
443  solution = solution->parent;
444  }
445 
446  auto path(std::make_shared<PathGeometric>(si_));
447  path->getStates().reserve(mpath1.size() + mpath2.size());
448  for (int i = mpath1.size() - 1; i >= 0; --i)
449  path->append(mpath1[i]->state);
450  for (auto &i : mpath2)
451  path->append(i->state);
452 
453  pdef_->addSolutionPath(path, false, 0.0, getName());
454  solved = true;
455  break;
456  }
457  }
458 
459  std::swap(tree, otherTree);
460  }
461 
462  si_->freeState(rstate);
463  si_->freeState(xstate);
464  delete rmotion;
465  delete xmotion;
466 
467  OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(),
468  tStart_->size(), tGoal_->size());
470 }
471 
473 {
474  Planner::getPlannerData(data);
475 
476  std::vector<Motion *> motions;
477  if (tStart_)
478  tStart_->list(motions);
479  for (auto &motion : motions)
480  {
481  if (motion->parent == nullptr)
482  data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
483  else
484  {
485  data.addEdge(base::PlannerDataVertex(motion->parent->state, 1), base::PlannerDataVertex(motion->state, 1));
486  }
487  }
488 
489  motions.clear();
490  if (tGoal_)
491  tGoal_->list(motions);
492  for (auto &motion : motions)
493  {
494  if (motion->parent == nullptr)
495  data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
496  else
497  {
498  // The edges in the goal tree are reversed to be consistent with start tree
499  data.addEdge(base::PlannerDataVertex(motion->state, 2), base::PlannerDataVertex(motion->parent->state, 2));
500  }
501  }
502 
503  // Add the edge connecting the two trees
504  if ((connectionPoint_.first != nullptr) && (connectionPoint_.second != nullptr))
505  data.addEdge(data.vertexIndex(connectionPoint_.first->state), data.vertexIndex(connectionPoint_.second->state));
506 }
BiTRRT(const base::SpaceInformationPtr &si)
Constructor.
Definition: BiTRRT.cpp:45
double getFrontierNodeRatio() const
Get the ratio between adding non-frontier nodes to frontier nodes.
Definition: BiTRRT.h:260
void setRange(double distance)
Set the maximum possible length of any one motion in the search tree. Very short/long motions may inh...
Definition: BiTRRT.h:179
bool transitionTest(const base::Cost &motionCost)
Transition test that filters transitions based on the motion cost. If the motion cost is near or belo...
Definition: BiTRRT.cpp:185
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
void setInitTemperature(double initTemperature)
Set the initial temperature at the start of planning. Should be high to allow for initial exploration...
Definition: BiTRRT.h:225
void setFrontierNodeRatio(double frontierNodeRatio)
Set the ratio between adding non-frontier nodes to frontier nodes. For example: .1 is one non-frontie...
Definition: BiTRRT.h:253
const base::State * root
Pointer to the root of the tree this motion is contained in.
Definition: BiTRRT.h:303
Motion * parent
The parent motion in the exploration tree.
Definition: BiTRRT.h:296
double getFrontierThreshold() const
Get the distance between a new state and the nearest neighbor that qualifies a state as being a front...
Definition: BiTRRT.h:245
Definition of an abstract state.
Definition: State.h:113
GrowResult extendTree(Motion *toMotion, TreeData &tree, Motion *&result)
Extend tree toward the state in rmotion. Store the result of the extension, if any,...
Definition: BiTRRT.cpp:269
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:123
bool minExpansionControl(double dist)
Use frontier node ratio to filter nodes that do not add new information to the search tree.
Definition: BiTRRT.cpp:212
base::State * state
The state contained by the motion.
Definition: BiTRRT.h:293
double value() const
The value of the cost.
Definition: Cost.h:152
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
Motion * addMotion(const base::State *state, TreeData &tree, Motion *parent=nullptr)
Add a state to the given tree. The motion created is returned.
Definition: BiTRRT.cpp:166
double getInitTemperature() const
Get the initial temperature at the start of planning.
Definition: BiTRRT.h:231
bool connectTrees(Motion *nmotion, TreeData &tree, Motion *xmotion)
Attempt to connect tree to nmotion, which is in the other tree. xmotion is scratch space and will be ...
Definition: BiTRRT.cpp:277
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: BiTRRT.cpp:339
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: BiTRRT.cpp:472
void freeMemory()
Free all memory allocated during planning.
Definition: BiTRRT.cpp:70
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:486
@ INVALID_GOAL
Invalid goal state.
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:269
void setTempChangeFactor(double factor)
Set the factor by which the temperature is increased after a failed transition test....
Definition: BiTRRT.h:195
void setCostThreshold(double maxCost)
Set the cost threshold (default is infinity). Any motion cost that is not better than this cost (acco...
Definition: BiTRRT.h:210
A class to store the exit status of Planner::solve()
GrowResult
The result of a call to extendTree.
Definition: BiTRRT.h:328
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BiTRRT.cpp:97
Abstract definition of goals.
Definition: Goal.h:126
@ EXACT_SOLUTION
The planner found an exact solution.
double getTempChangeFactor() const
Get the factor by which the temperature is increased after a failed transition.
Definition: BiTRRT.h:202
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...
double getRange() const
Get the range the planner is using.
Definition: BiTRRT.h:185
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
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...
std::shared_ptr< NearestNeighbors< Motion * > > TreeData
The nearest-neighbors data structure that contains the entire the tree of motions generated during pl...
Definition: BiTRRT.h:311
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 a goal region that can be sampled.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BiTRRT.cpp:115
double getCostThreshold() const
Get the cost threshold (default is infinity). Any motion cost that is not better than this cost (acco...
Definition: BiTRRT.h:218
@ INVALID_START
Invalid start state or no start state specified.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
Representation of a motion in the search tree.
Definition: BiTRRT.h:279
void setFrontierThreshold(double frontierThreshold)
Set the distance between a new state and the nearest neighbor that qualifies a state as being a front...
Definition: BiTRRT.h:238
static const double COST_MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION
For cost-based planners it has been observed that smaller ranges are typically suitable....