Loading...
Searching...
No Matches
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
51namespace ompl
52{
53 namespace geometric
54 {
65 class STRRTstar : public base::Planner
66 {
67 public:
70
71 ~STRRTstar() override;
72
73 void clear() override;
74
75 void getPlannerData(base::PlannerData &data) const override;
76
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
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
134 {
135 base::State *xstate;
136 base::Motion *xmotion;
137 bool start;
138 };
139
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
185
188
190 void removeInvalidGoals(const std::vector<base::Motion *> &invalidGoals);
191
194
197
200
202 double maxDistance_{0.};
203
206
208 base::PathPtr bestSolution_{nullptr};
209
211 double bestTime_ = std::numeric_limits<double>::infinity();
212
214 unsigned int numIterations_ = 0;
215
218
220 double minimumTime_ = std::numeric_limits<double>::infinity();
221
224
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
301
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
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
The Conditional Sampler samples feasible Space-Time States. First, a space configuration is sampled....
Representation of a motion.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition State.h:50
Space-Time RRT* (STRRTstar)
Definition STRRTstar.h:66
std::shared_ptr< ompl::NearestNeighbors< base::Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
Definition STRRTstar.h:130
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...
TreeData tGoal_
The goal tree.
Definition STRRTstar.h:199
double k_rrt_
A constant for k-nearest rewiring calculations.
Definition STRRTstar.h:289
base::Motion * startMotion_
The start Motion, used for conditional sampling and start tree pruning.
Definition STRRTstar.h:229
void setOptimumApproxFactor(double optimumApproxFactor)
Set the Optimum Approximation factor. This allows the planner to converge more quickly,...
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....
unsigned int initialBatchSize_
Number of samples of the first batch.
Definition STRRTstar.h:306
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:236
static void removeFromParent(base::Motion *m)
Removes the given motion from the parent's child list.
void freeMemory()
Free the memory allocated by this planner.
Definition STRRTstar.cpp:89
double bestTime_
The current best time i.e. cost of all found solutions.
Definition STRRTstar.h:211
GrowState growTreeSingle(TreeData &tree, TreeGrowingInfo &tgi, base::Motion *rmotion, base::Motion *nmotion)
Grow a tree towards a random state for a single nearest state.
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...
TreeData tStart_
The start tree.
Definition STRRTstar.h:196
GrowState
The state of the tree after an attempt to extend it.
Definition STRRTstar.h:142
@ ADVANCED
progress has been made towards the randomly sampled state
Definition STRRTstar.h:146
@ TRAPPED
no progress has been made
Definition STRRTstar.h:144
@ REACHED
the randomly sampled state was reached
Definition STRRTstar.h:148
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
void setSampleUniformForUnboundedTime(bool uniform)
Whether the state space is sampled uniformly or centered at lower time values.
double r_rrt_
A constant for r-disc rewiring calculations.
Definition STRRTstar.h:292
int goalStateSampleRatio_
The ratio, a goal state is sampled compared to the size of the goal tree.
Definition STRRTstar.h:322
double getOptimumApproxFactor() const
The Optimum Approximation factor (0 - 1).
unsigned int numIterations_
The number of while loop iterations.
Definition STRRTstar.h:214
double upperTimeBound_
Upper bound for the time up to which solutions are searched for.
Definition STRRTstar.h:223
double optimumApproxFactor_
The factor to which found solution times need to be reduced compared to minimum time,...
Definition STRRTstar.h:226
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:286
double timeBoundFactorIncrease_
The factor, the time bound is increased with after the batch is full.
Definition STRRTstar.h:313
bool sampleUniformForUnboundedTime_
Whether the samples are uniformly distributed over the whole space or are centered at lower times.
Definition STRRTstar.h:319
double getRange() const
Get the range the planner is using.
void setTimeBoundFactorIncrease(double f)
The value by which the time bound factor is multiplied in each increase step.
std::vector< base::Motion * > goalMotions_
The goal Motions, used for conditional sampling and pruning.
Definition STRRTstar.h:232
double distanceBetweenTrees_
Distance between the nearest pair of start tree and goal tree nodes.
Definition STRRTstar.h:205
void setInitialTimeBoundFactor(double f)
The initial time bound factor.
base::PathPtr bestSolution_
The current best solution path with respect to shortest time.
Definition STRRTstar.h:208
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition STRRTstar.h:202
void pruneStartTree()
Prune the start tree after a solution was found.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
int numSolutions_
The number of found solutions.
Definition STRRTstar.h:217
base::Motion * pruneGoalTree()
Prune the goal tree after a solution was found. Return the goal motion, that is connected to the star...
double distanceFunction(const base::Motion *a, const base::Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition STRRTstar.h:173
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition STRRTstar.cpp:58
void setRewiringToOff()
Do not rewire at all.
bool sampleGoalTime(base::State *goal, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
Samples the time component of a goal state dependant on its space component. Returns false,...
ompl::RNG rng_
The random number generator.
Definition STRRTstar.h:325
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.
base::State * nextGoal(int n, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
N tries to sample a goal.
void removeInvalidGoals(const std::vector< base::Motion * > &invalidGoals)
Remove invalid goal states from the goal set.
void increaseTimeBound(bool hasEqualBounds, double &oldBatchTimeBoundFactor, double &newBatchTimeBoundFactor, bool &startTree, unsigned int &batchSize, int &numBatchSamples)
base::ConditionalStateSampler sampler_
State sampler.
Definition STRRTstar.h:193
double initialTimeBoundFactor_
Initial factor, the minimum time of each goal is multiplied with to calculate the upper time bound.
Definition STRRTstar.h:310
double minimumTime_
Minimum Time at which any goal can be reached, if moving on a straight line.
Definition STRRTstar.h:220
void setRewiringToRadius()
Rewire by radius.
void setRewiringToKNearest()
Rewire by k-nearest.
static base::Motion * computeSolutionMotion(base::Motion *motion)
Find the solution (connecting) motion for a motion that is indirectly connected.
bool isTimeBounded_
Whether the time is bounded or not. The first solution automatically bounds the time.
Definition STRRTstar.h:300
void setRange(double distance)
Set the range the planner is supposed to use.
unsigned int getBatchSize() const
The number of samples before the time bound is increased.
double initialTimeBound_
The time bound the planner is initialized with. Used to reset for repeated planning.
Definition STRRTstar.h:303
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...
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Information attached to growing a tree of motions (used internally)
Definition STRRTstar.h:134