Class STRRTstar

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Class Documentation

class STRRTstar : public ompl::base::Planner

Space-Time RRT* (STRRTstar)

Short description

ST-RRT* is a bidirectional, time-optimal planner for planning in space-time. It operates similar to a bidirectional version of RRT*, but allows planning in unbounded time spaces by gradual time-bound extensions and is highly optimized for planning in space-time by employing Conditional Sampling and Simplified Rewiring.

Public Types

using Motion = base::ConditionalStateSampler::Motion
using TreeData = std::shared_ptr<ompl::NearestNeighbors<Motion*>>

A nearest-neighbor datastructure representing a tree of motions.

Public Functions

explicit STRRTstar(const ompl::base::SpaceInformationPtr &si)

Constructor.

~STRRTstar() override
virtual void clear() override

Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() will ignore all previous work.

virtual void getPlannerData(base::PlannerData &data) const override

Get information about the current run of the motion planner. Repeated calls to this function will update data (only additions are made). This is useful to see what changed in the exploration datastructure, between calls to solve(), for example (without calling clear() in between).

virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override

Function that can solve the motion planning problem. This function can be called multiple times on the same problem, without calling clear() in between. This allows the planner to continue work for more time on an unsolved problem, for example. If this option is used, it is assumed the problem definition is not changed (unpredictable results otherwise). The only change in the problem definition that is accounted for is the addition of starting or goal states (but not changing previously added start/goal states). If clearQuery() is called, the planner may retain prior datastructures generated from a previous query on a new problem definition. The function terminates if the call to ptc returns true.

void setRange(double distance)

Set the range the planner is supposed to use. 

    This parameter greatly influences the runtime of the
    algorithm. It represents the maximum length of a
    motion to be added in the tree of motions.
double getRange() const

Get the range the planner is using.

double getOptimumApproxFactor() const

The Optimum Approximation factor (0 - 1).

void setOptimumApproxFactor(double optimumApproxFactor)

Set the Optimum Approximation factor. This allows the planner to converge more quickly, but only yields approximately optimal solutions.

std::string getRewiringState() const
void setRewiringToOff()

Do not rewire at all.

void setRewiringToRadius()

Rewire by radius.

void setRewiringToKNearest()

Rewire by k-nearest.

double getRewireFactor() const
void setRewireFactor(double v)
unsigned int getBatchSize() const

The number of samples before the time bound is increased.

void setBatchSize(int v)
void setTimeBoundFactorIncrease(double f)

The value by which the time bound factor is multiplied in each increase step.

void setInitialTimeBoundFactor(double f)

The initial time bound factor.

void setSampleUniformForUnboundedTime(bool uniform)

Whether the state space is sampled uniformly or centered at lower time values.

virtual void setup() override

Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceInformation::setup() if needed. This must be called before solving.

Protected Types

enum GrowState

The state of the tree after an attempt to extend it.

Values:

enumerator TRAPPED

no progress has been made

enumerator ADVANCED

progress has been made towards the randomly sampled state

enumerator REACHED

the randomly sampled state was reached

enum RewireState

Values:

enumerator RADIUS
enumerator KNEAREST
enumerator OFF

Protected Functions

GrowState growTreeSingle(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion, Motion *nmotion)

Grow a tree towards a random state for a single nearest state.

GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion, std::vector<Motion*> &nbh, bool connect)

Attempt to grow a tree towards a random state for the neighborhood of the random state. The neighborhood is determined by the used rewire state. For the start tree closest state with respect to distance are tried first. For the goal tree states with the minimum time root node are tried first. If connect is true, multiple vertices can be added to the tree until the random state is reached or an basestacle is met. If connect is false, the tree is only extended by a single new state.

void increaseTimeBound(bool hasEqualBounds, double &oldBatchTimeBoundFactor, double &newBatchTimeBoundFactor, bool &startTree, unsigned int &batchSize, int &numBatchSamples)

void getNeighbors(TreeData &tree, Motion *motion, std::vector<Motion*> &nbh) const

Gets the neighbours of a given motion, using either k-nearest or radius_ as appropriate.

void freeMemory()

Free the memory allocated by this planner.

inline double distanceFunction(const Motion *a, const Motion *b) const

Compute distance between motions (actually distance between contained states)

void pruneStartTree()

Prune the start tree after a solution was found.

Motion *pruneGoalTree()

Prune the goal tree after a solution was found. Return the goal motion, that is connected to the start tree, if a new solution was found. If no new solution was found, return nullpointer.

void removeInvalidGoals(const std::vector<Motion*> &invalidGoals)

Remove invalid goal states from the goal set.

base::State *nextGoal(int n, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)

N tries to sample a goal.

base::State *nextGoal(const base::PlannerTerminationCondition &ptc, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)

Samples a goal until successful or the termination condition is fulfilled.

base::State *nextGoal(const base::PlannerTerminationCondition &ptc, int n, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)

Samples a goal until successful or the termination condition is fulfilled.

bool sampleGoalTime(base::State *goal, double oldBatchTimeBoundFactor, double newBatchTimeBoundFactor)

Samples the time component of a goal state dependant on its space component. Returns false, if goal can’t be reached in time.

void constructSolution(Motion *startMotion, Motion *goalMotion, const base::ReportIntermediateSolutionFn &intermediateSolutionCallback, const ompl::base::PlannerTerminationCondition &ptc)
void calculateRewiringLowerBounds()

Calculate the k_RRG* and r_RRG* terms.

bool rewireGoalTree(Motion *addedMotion)

Protected Attributes

base::ConditionalStateSampler sampler_

State sampler.

TreeData tStart_

The start tree.

TreeData tGoal_

The goal tree.

double maxDistance_ = {0.}

The maximum length of a motion to be added to a tree.

double distanceBetweenTrees_

Distance between the nearest pair of start tree and goal tree nodes.

base::PathPtr bestSolution_ = {nullptr}

The current best solution path with respect to shortest time.

double bestTime_ = std::numeric_limits<double>::infinity()

The current best time i.e. cost of all found solutions.

unsigned int numIterations_ = 0

The number of while loop iterations.

int numSolutions_ = 0

The number of found solutions.

double minimumTime_ = std::numeric_limits<double>::infinity()

Minimum Time at which any goal can be reached, if moving on a straight line.

double upperTimeBound_

Upper bound for the time up to which solutions are searched for.

double optimumApproxFactor_ = 1.0

The factor to which found solution times need to be reduced compared to minimum time, (0, 1].

Motion *startMotion_ = {nullptr}

The start Motion, used for conditional sampling and start tree pruning.

std::vector<Motion*> goalMotions_ = {}

The goal Motions, used for conditional sampling and pruning.

std::vector<Motion*> newBatchGoalMotions_ = {}

The goal Motions, that were added in the current expansion step, used for uniform sampling over a growing region.

base::State *tempState_ = {nullptr}

Goal Sampling is not handled by PlannerInputStates, but directly by the SpaceTimeRRT, because the time component of every goal sample is sampled dependent on the sampled space component.

RewireState rewireState_ = KNEAREST
double rewireFactor_ = {1.1}

The rewiring factor, s, so that r_rrt = s \times r_rrt* > r_rrt* (or k_rrt = s \times k_rrt* > k_rrt*)

double k_rrt_ = {0u}

A constant for k-nearest rewiring calculations.

double r_rrt_ = {0.}

A constant for r-disc rewiring calculations.

bool isTimeBounded_

Whether the time is bounded or not. The first solution automatically bounds the time.

double initialTimeBound_

The time bound the planner is initialized with. Used to reset for repeated planning.

unsigned int initialBatchSize_ = 512

Number of samples of the first batch.

double initialTimeBoundFactor_ = 2.0

Initial factor, the minimum time of each goal is multiplied with to calculate the upper time bound.

double timeBoundFactorIncrease_ = 2.0

The factor, the time bound is increased with after the batch is full.

bool sampleOldBatch_ = true
bool sampleUniformForUnboundedTime_ = true

Whether the samples are uniformly distributed over the whole space or are centered at lower times.

int goalStateSampleRatio_ = 4

The ratio, a goal state is sampled compared to the size of the goal tree.

ompl::RNG rng_

The random number generator.

Protected Static Functions

static Motion *computeSolutionMotion(Motion *motion)

Find the solution (connecting) motion for a motion that is indirectly connected.

static void removeFromParent(Motion *m)

Removes the given motion from the parent’s child list.

static void addDescendants(Motion *m, const TreeData &tree)

Adds given all descendants of the given motion to given tree and checks whether one of the added motions is the goal motion.

struct TreeGrowingInfo

Information attached to growing a tree of motions (used internally)

Public Members

base::State *xstate
Motion *xmotion
bool start