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
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...
STRRTstar(const ompl::base::SpaceInformationPtr &si)
Constructor.
Definition STRRTstar.cpp:40
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
std::shared_ptr< ompl::NearestNeighbors< base::Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
Definition STRRTstar.h:130
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