Loading...
Searching...
No Matches
STRRTstar.h
152 GrowState growTreeSingle(TreeData &tree, TreeGrowingInfo &tgi, base::Motion *rmotion, base::Motion *nmotion);
159 GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, base::Motion *rmotion, std::vector<base::Motion *> &nbh,
163 void increaseTimeBound(bool hasEqualBounds, double &oldBatchTimeBoundFactor, double &newBatchTimeBoundFactor,
167 void getNeighbors(TreeData &tree, base::Motion *motion, std::vector<base::Motion *> &nbh) const;
250 base::State *nextGoal(const base::PlannerTerminationCondition &ptc, double oldBatchTimeBoundFactor,
254 base::State *nextGoal(const base::PlannerTerminationCondition &ptc, int n, double oldBatchTimeBoundFactor,
259 bool sampleGoalTime(base::State *goal, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor);
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition RandomNumbers.h:58
The Conditional Sampler samples feasible Space-Time States. First, a space configuration is sampled....
Definition ConditionalStateSampler.h:72
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition PlannerTerminationCondition.h:64
A shared pointer wrapper for ompl::base::SpaceInformation.
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
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,...
Definition STRRTstar.cpp:1149
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
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.
Definition STRRTstar.cpp:935
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
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
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition STRRTstar.cpp:1045
void setSampleUniformForUnboundedTime(bool uniform)
Whether the state space is sampled uniformly or centered at lower time values.
Definition STRRTstar.cpp:1225
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).
Definition STRRTstar.cpp:1144
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
void setTimeBoundFactorIncrease(double f)
The value by which the time bound factor is multiplied in each increase step.
Definition STRRTstar.cpp:1207
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
base::PathPtr bestSolution_
The current best solution path with respect to shortest time.
Definition STRRTstar.h:208
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition STRRTstar.cpp:869
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
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...
Definition STRRTstar.cpp:893
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition STRRTstar.cpp:58
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
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
base::State * nextGoal(int n, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)
N tries to sample a goal.
Definition STRRTstar.cpp:1089
void removeInvalidGoals(const std::vector< base::Motion * > &invalidGoals)
Remove invalid goal states from the goal set.
Definition STRRTstar.cpp:850
void increaseTimeBound(bool hasEqualBounds, double &oldBatchTimeBoundFactor, double &newBatchTimeBoundFactor, bool &startTree, unsigned int &batchSize, int &numBatchSamples)
Definition STRRTstar.cpp:549
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
static base::Motion * computeSolutionMotion(base::Motion *motion)
Find the solution (connecting) motion for a motion that is indirectly connected.
Definition STRRTstar.cpp:829
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.
Definition STRRTstar.cpp:1134
unsigned int getBatchSize() const
The number of samples before the time bound is increased.
Definition STRRTstar.cpp:1193
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...
Definition ProblemDefinition.h:144
Main namespace. Contains everything in this library.
Definition MultiLevelPlanarManipulatorDemo.cpp:66
A class to store the exit status of Planner::solve()
Definition PlannerStatus.h:49
Information attached to growing a tree of motions (used internally)
Definition STRRTstar.h:134