STRRTstar.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2021, Technische Universität Berlin (TU Berlin)
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 TU Berlin 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: Francesco Grothe */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_STRRT_STAR_
38 #define OMPL_GEOMETRIC_PLANNERS_RRT_STRRT_STAR_
39 
40 #include <ompl/datastructures/NearestNeighbors.h>
41 #include <ompl/base/goals/GoalSampleableRegion.h>
42 #include <ompl/geometric/planners/PlannerIncludes.h>
43 #include <ompl/base/OptimizationObjective.h>
44 #include <ompl/base/spaces/SpaceTimeStateSpace.h>
45 #include <ompl/base/objectives/MinimizeArrivalTime.h>
46 #include <ompl/base/samplers/ConditionalStateSampler.h>
47 #include <ompl/tools/config/SelfConfig.h>
48 #include <ompl/util/GeometricEquations.h>
49 #include <boost/math/constants/constants.hpp>
50 
51 namespace ompl
52 {
53  namespace geometric
54  {
65  class STRRTstar : public base::Planner
66  {
67  public:
69  explicit STRRTstar(const ompl::base::SpaceInformationPtr &si);
70 
71  ~STRRTstar() override;
72 
73  void clear() override;
74 
75  void getPlannerData(base::PlannerData &data) const override;
76 
77  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
78 
84  void setRange(double distance);
85 
87  double getRange() const;
88 
90  double getOptimumApproxFactor() const;
91 
94  void setOptimumApproxFactor(double optimumApproxFactor);
95 
96  std::string getRewiringState() const;
97 
99  void setRewiringToOff();
100 
102  void setRewiringToRadius();
103 
105  void setRewiringToKNearest();
106 
107  double getRewireFactor() const;
108 
109  void setRewireFactor(double v);
110 
112  unsigned int getBatchSize() const;
113 
114  void setBatchSize(int v);
115 
117  void setTimeBoundFactorIncrease(double f);
118 
120  void setInitialTimeBoundFactor(double f);
121 
123  void setSampleUniformForUnboundedTime(bool uniform);
124 
125  void setup() override;
126 
127  protected:
128 
130  using TreeData = std::shared_ptr<ompl::NearestNeighbors<base::Motion *>>;
131 
133  struct TreeGrowingInfo
134  {
135  base::State *xstate;
136  base::Motion *xmotion;
137  bool start;
138  };
139 
141  enum GrowState
142  {
144  TRAPPED,
146  ADVANCED,
148  REACHED
149  };
150 
152  GrowState growTreeSingle(TreeData &tree, TreeGrowingInfo &tgi, base::Motion *rmotion, base::Motion *nmotion);
153 
159  GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, base::Motion *rmotion, std::vector<base::Motion *> &nbh,
160  bool connect);
161 
163  void increaseTimeBound(bool hasEqualBounds, double &oldBatchTimeBoundFactor, double &newBatchTimeBoundFactor,
164  bool &startTree, unsigned int &batchSize, int &numBatchSamples);
165 
167  void getNeighbors(TreeData &tree, base::Motion *motion, std::vector<base::Motion *> &nbh) const;
168 
170  void freeMemory();
171 
173  double distanceFunction(const base::Motion *a, const base::Motion *b) const
174  {
175  return si_->distance(a->state, b->state);
176  }
177 
179  void pruneStartTree();
180 
184  base::Motion *pruneGoalTree();
185 
187  static base::Motion *computeSolutionMotion(base::Motion *motion);
188 
190  void removeInvalidGoals(const std::vector<base::Motion *> &invalidGoals);
191 
193  base::ConditionalStateSampler sampler_;
194 
197 
200 
202  double maxDistance_{0.};
203 
205  double distanceBetweenTrees_;
206 
208  base::PathPtr bestSolution_{nullptr};
209 
211  double bestTime_ = std::numeric_limits<double>::infinity();
212 
214  unsigned int numIterations_ = 0;
215 
217  int numSolutions_ = 0;
218 
220  double minimumTime_ = std::numeric_limits<double>::infinity();
221 
223  double upperTimeBound_;
224 
226  double optimumApproxFactor_ = 1.0;
227 
230 
232  std::vector<base::Motion *> goalMotions_{};
233 
236  std::vector<base::Motion *> newBatchGoalMotions_{};
237 
244  base::State *tempState_{nullptr}; // temporary sampled goal states are stored here.
245 
247  base::State *nextGoal(int n, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor);
248 
250  base::State *nextGoal(const base::PlannerTerminationCondition &ptc, double oldBatchTimeBoundFactor,
251  double newBatchTimeBoundFactor);
252 
254  base::State *nextGoal(const base::PlannerTerminationCondition &ptc, int n, double oldBatchTimeBoundFactor,
255  double newBatchTimeBoundFactor);
256 
259  bool sampleGoalTime(base::State *goal, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor);
260 
262  static void removeFromParent(base::Motion *m);
263 
266  static void addDescendants(base::Motion *m, const TreeData &tree);
267 
268  void constructSolution(base::Motion *startMotion, base::Motion *goalMotion,
269  const base::ReportIntermediateSolutionFn &intermediateSolutionCallback,
271 
272  enum RewireState
273  {
274  // use r-disc search for rewiring
275  RADIUS,
276  // use k-nearest for rewiring
277  KNEAREST,
278  // don't use any rewiring
279  OFF
280  };
281 
282  RewireState rewireState_ = KNEAREST;
283 
286  double rewireFactor_{1.1};
287 
289  double k_rrt_{0u};
290 
292  double r_rrt_{0.};
293 
296 
297  bool rewireGoalTree(base::Motion *addedMotion);
298 
300  bool isTimeBounded_;
301 
303  double initialTimeBound_;
304 
306  unsigned int initialBatchSize_ = 512;
307 
311 
314 
315  bool sampleOldBatch_ = true;
316 
320 
323 
326  };
327  } // namespace geometric
328 } // namespace ompl
329 
330 #endif
void setSampleUniformForUnboundedTime(bool uniform)
Whether the state space is sampled uniformly or centered at lower time values.
Definition: STRRTstar.cpp:1225
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:89
Representation of a motion.
double k_rrt_
A constant for k-nearest rewiring calculations.
Definition: STRRTstar.h:385
bool isTimeBounded_
Whether the time is bounded or not. The first solution automatically bounds the time.
Definition: STRRTstar.h:396
unsigned int numIterations_
The number of while loop iterations.
Definition: STRRTstar.h:310
A shared pointer wrapper for ompl::base::SpaceInformation.
std::vector< base::Motion * > goalMotions_
The goal Motions, used for conditional sampling and pruning.
Definition: STRRTstar.h:328
base::Motion * pruneGoalTree()
Prune the goal tree after a solution was found. Return the goal motion, that is connected to the star...
Definition: STRRTstar.cpp:710
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: STRRTstar.cpp:893
static base::Motion * computeSolutionMotion(base::Motion *motion)
Find the solution (connecting) motion for a motion that is indirectly connected.
Definition: STRRTstar.cpp:829
double timeBoundFactorIncrease_
The factor, the time bound is increased with after the batch is full.
Definition: STRRTstar.h:409
static void removeFromParent(base::Motion *m)
Removes the given motion from the parent's child list.
Definition: STRRTstar.cpp:935
unsigned int initialBatchSize_
Number of samples of the first batch.
Definition: STRRTstar.h:402
Definition of an abstract state.
Definition: State.h:113
bool sampleUniformForUnboundedTime_
Whether the samples are uniformly distributed over the whole space or are centered at lower times.
Definition: STRRTstar.h:415
std::vector< base::Motion * > newBatchGoalMotions_
The goal Motions, that were added in the current expansion step, used for uniform sampling over a gro...
Definition: STRRTstar.h:332
void setRewiringToKNearest()
Rewire by k-nearest.
Definition: STRRTstar.cpp:1174
unsigned int getBatchSize() const
The number of samples before the time bound is increased.
Definition: STRRTstar.cpp:1193
TreeData tStart_
The start tree.
Definition: STRRTstar.h:292
double distanceFunction(const base::Motion *a, const base::Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: STRRTstar.h:269
bool sampleGoalTime(base::State *goal, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
Samples the time component of a goal state dependant on its space component. Returns false,...
Definition: STRRTstar.cpp:1059
GrowState growTreeSingle(TreeData &tree, TreeGrowingInfo &tgi, base::Motion *rmotion, base::Motion *nmotion)
Grow a tree towards a random state for a single nearest state.
Definition: STRRTstar.cpp:509
void setTimeBoundFactorIncrease(double f)
The value by which the time bound factor is multiplied in each increase step.
Definition: STRRTstar.cpp:1207
@ REACHED
the randomly sampled state was reached
Definition: STRRTstar.h:244
void increaseTimeBound(bool hasEqualBounds, double &oldBatchTimeBoundFactor, double &newBatchTimeBoundFactor, bool &startTree, unsigned int &batchSize, int &numBatchSamples)
Definition: STRRTstar.cpp:549
double getRange() const
Get the range the planner is using.
Definition: STRRTstar.cpp:1139
double initialTimeBound_
The time bound the planner is initialized with. Used to reset for repeated planning.
Definition: STRRTstar.h:399
base::State * tempState_
Definition: STRRTstar.h:340
base::ConditionalStateSampler sampler_
State sampler.
Definition: STRRTstar.h:289
GrowState
The state of the tree after an attempt to extend it.
Definition: STRRTstar.h:237
@ TRAPPED
no progress has been made
Definition: STRRTstar.h:240
double getOptimumApproxFactor() const
The Optimum Approximation factor (0 - 1).
Definition: STRRTstar.cpp:1144
TreeData tGoal_
The goal tree.
Definition: STRRTstar.h:295
void removeInvalidGoals(const std::vector< base::Motion * > &invalidGoals)
Remove invalid goal states from the goal set.
Definition: STRRTstar.cpp:850
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
base::PathPtr bestSolution_
The current best solution path with respect to shortest time.
Definition: STRRTstar.h:304
double initialTimeBoundFactor_
Initial factor, the minimum time of each goal is multiplied with to calculate the upper time bound.
Definition: STRRTstar.h:406
base::State * nextGoal(int n, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
N tries to sample a goal.
Definition: STRRTstar.cpp:1089
double distanceBetweenTrees_
Distance between the nearest pair of start tree and goal tree nodes.
Definition: STRRTstar.h:301
double upperTimeBound_
Upper bound for the time up to which solutions are searched for.
Definition: STRRTstar.h:319
int goalStateSampleRatio_
The ratio, a goal state is sampled compared to the size of the goal tree.
Definition: STRRTstar.h:418
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: STRRTstar.cpp:119
@ ADVANCED
progress has been made towards the randomly sampled state
Definition: STRRTstar.h:242
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: STRRTstar.h:298
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: STRRTstar.cpp:1045
base::Motion * startMotion_
The start Motion, used for conditional sampling and start tree pruning.
Definition: STRRTstar.h:325
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: STRRTstar.cpp:1134
std::shared_ptr< ompl::NearestNeighbors< base::Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
Definition: STRRTstar.h:226
void freeMemory()
Free the memory allocated by this planner.
Definition: STRRTstar.cpp:89
void setRewiringToRadius()
Rewire by radius.
Definition: STRRTstar.cpp:1169
double bestTime_
The current best time i.e. cost of all found solutions.
Definition: STRRTstar.h:307
double r_rrt_
A constant for r-disc rewiring calculations.
Definition: STRRTstar.h:388
double minimumTime_
Minimum Time at which any goal can be reached, if moving on a straight line.
Definition: STRRTstar.h:316
void setRewiringToOff()
Do not rewire at all.
Definition: STRRTstar.cpp:1164
static void addDescendants(base::Motion *m, const TreeData &tree)
Adds given all descendants of the given motion to given tree and checks whether one of the added moti...
Definition: STRRTstar.cpp:954
double rewireFactor_
The rewiring factor, s, so that r_rrt = s \times r_rrt* > r_rrt* (or k_rrt = s \times k_rrt* > k_rrt*...
Definition: STRRTstar.h:382
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
std::function< void(const Planner *, const std::vector< const base::State * > &, const Cost)> ReportIntermediateSolutionFn
When a planner has an intermediate solution (e.g., optimizing planners), a function with this signatu...
GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, base::Motion *rmotion, std::vector< base::Motion * > &nbh, bool connect)
Attempt to grow a tree towards a random state for the neighborhood of the random state....
Definition: STRRTstar.cpp:440
ompl::RNG rng_
The random number generator.
Definition: STRRTstar.h:421
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: STRRTstar.cpp:869
int numSolutions_
The number of found solutions.
Definition: STRRTstar.h:313
void getNeighbors(TreeData &tree, base::Motion *motion, std::vector< base::Motion * > &nbh) const
Gets the neighbours of a given motion, using either k-nearest or radius_ as appropriate.
Definition: STRRTstar.cpp:970
void pruneStartTree()
Prune the start tree after a solution was found.
Definition: STRRTstar.cpp:646
void setOptimumApproxFactor(double optimumApproxFactor)
Set the Optimum Approximation factor. This allows the planner to converge more quickly,...
Definition: STRRTstar.cpp:1149
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: STRRTstar.cpp:58
STRRTstar(const ompl::base::SpaceInformationPtr &si)
Constructor.
Definition: STRRTstar.cpp:40
void setInitialTimeBoundFactor(double f)
The initial time bound factor.
Definition: STRRTstar.cpp:1216
Main namespace. Contains everything in this library.
double optimumApproxFactor_
The factor to which found solution times need to be reduced compared to minimum time,...
Definition: STRRTstar.h:322