Class SST

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Class Documentation

class SST : public ompl::base::Planner

Short description

SST (Stable Sparse RRT) is a asymptotically near-optimal incremental sampling-based motion planning algorithm for systems with dynamics. It makes use of random control inputs to perform a search for the best control inputs to explore the state space.

External documentation

Yanbo Li, Zakary Littlefield, Kostas E. Bekris, Sampling-based Asymptotically Optimal Sampling-based Kinodynamic Planning. [PDF]

Public Functions

SST(const SpaceInformationPtr &si)

Constructor.

~SST() override
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.

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

Continue solving for some amount of time. Return true if solution was found.

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 void clear() override

Clear datastructures. Call this function if the input data to the planner has changed and you do not want to continue planning.

inline void setGoalBias(double goalBias)

In the process of randomly selecting states in the state space to attempt to go towards, the algorithm may in fact choose the actual goal state, if it knows it, with some probability. This probability is a real number between 0.0 and 1.0; its value should usually be around 0.05 and should not be too large. It is probably a good idea to use the default value.

inline double getGoalBias() const

Get the goal bias the planner is using.

inline void setSelectionRadius(double selectionRadius)

Set the radius for selecting nodes relative to random sample.

This radius is used to mimic behavior of RRT* in that it promotes extending from nodes with good path cost from the root of the tree. Making this radius larger will provide higher quality paths, but has two major drawbacks; exploration will occur much more slowly and exploration around the boundary of the state space may become impossible.

inline double getSelectionRadius() const

Get the selection radius the planner is using.

inline void setPruningRadius(double pruningRadius)

Set the radius for pruning nodes.

This is the radius used to surround nodes in the witness set. Within this radius around a state in the witness set, only one active tree node can exist. This limits the size of the tree and forces computation to focus on low path costs nodes. If this value is too large, narrow passages will be impossible to traverse. In addition, children nodes may be removed if they are not at least this distance away from their parent nodes.

inline double getPruningRadius() const

Get the pruning radius the planner is using.

template<template<typename T> class NN>
inline void setNearestNeighbors()

Set a different nearest neighbors datastructure.

Protected Functions

Motion *selectNode(Motion *sample)

Finds the best node in the tree withing the selection radius around a random sample.

Witness *findClosestWitness(Motion *node)

Find the closest witness node to a newly generated potential node.

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)

Protected Attributes

base::StateSamplerPtr sampler_

State sampler.

ControlSamplerPtr controlSampler_

Control sampler.

const SpaceInformation *siC_

The base::SpaceInformation cast as control::SpaceInformation, for convenience.

std::shared_ptr<NearestNeighbors<Motion*>> nn_

A nearest-neighbors datastructure containing the tree of motions.

std::shared_ptr<NearestNeighbors<Motion*>> witnesses_

A nearest-neighbors datastructure containing the tree of witness motions.

double goalBias_ = {0.05}

The fraction of time the goal is picked as the state to expand towards (if such a state is available)

double selectionRadius_ = {0.2}

The radius for determining the node selected for extension.

double pruningRadius_ = {0.1}

The radius for determining the size of the pruning region.

RNG rng_

The random number generator.

std::vector<base::State*> prevSolution_

The best solution we found so far.

std::vector<Control*> prevSolutionControls_
std::vector<unsigned> prevSolutionSteps_
base::Cost prevSolutionCost_

The best solution cost we found so far.

base::OptimizationObjectivePtr opt_

The optimization objective.

class Motion

Representation of a motion.

This only contains pointers to parent motions as we only need to go backwards in the tree.

Subclassed by ompl::control::SST::Witness

Public Functions

Motion() = default
inline Motion(const SpaceInformation *si)

Constructor that allocates memory for the state and the control.

virtual ~Motion() = default
inline virtual base::State *getState() const
inline virtual Motion *getParent() const

Public Members

base::Cost accCost_ = {0}
base::State *state_ = {nullptr}

The state contained by the motion.

Control *control_ = {nullptr}

The control contained by the motion.

unsigned int steps_ = {0}

The number of steps_ the control is applied for.

Motion *parent_ = {nullptr}

The parent motion in the exploration tree.

unsigned numChildren_ = {0}

Number of children.

bool inactive_ = {false}

If inactive, this node is not considered for selection.

class Witness : public ompl::control::SST::Motion

Public Functions

Witness() = default
inline Witness(const SpaceInformation *si)
inline virtual base::State *getState() const override
inline virtual Motion *getParent() const override
inline void linkRep(Motion *lRep)

Public Members

Motion *rep_ = {nullptr}

The node in the tree that is within the pruning radius.