Class PathGeometric

Inheritance Relationships

Base Type

Class Documentation

class PathGeometric : public ompl::base::Path

Definition of a geometric path.

This is the type of path computed by geometric planners.

Path operations

void interpolate(unsigned int count)

Insert a number of states in a path so that the path is made up of exactly count states. States are inserted uniformly (more states on longer segments). Changes are performed only if a path has less than count states.

void interpolate()

Insert a number of states in a path so that the path is made up of (approximately) the states checked for validity when a discrete motion validator is used.

void subdivide()

Add a state at the middle of each segment.

void reverse()

Reverse the path.

std::pair<bool, bool> checkAndRepair(unsigned int attempts)

Check if the path is valid. If it is not, attempts are made to fix the path by sampling around invalid states. Not more than attempts samples are drawn.

Note

If repairing a path fails, the path may still be altered

Returns:

A pair of boolean values is returned. The first value represents the validity of the path before any change was made. The second value represents the validity of the path after changes were attempted.

void overlay(const PathGeometric &over, unsigned int startIndex = 0)

Overlay the path over on top of the current path. States are added to the current path if needed (by copying the last state).

If over consists of states form a different state space than the existing path, the data from those states is copied over, for the corresponding components. If over is from the same state space as this path, and startIndex is 0, this function’s result will be the same as with operator=()

void append(const base::State *state)

Append state to the end of this path. The memory for state is copied.

void append(const PathGeometric &path)

Append path at the end of this path. States from path are copied.

Let the existing path consist of states [ s1, s2, …, sk ]. Let path consist of states [y1, …, yp].

If the existing path and path consist of states from the same state space, [y1, …, yp] are added after sk. If they are not from the same state space, states [z1, …, zp] are added, where each zi is a copy of sk that has components overwritten with ones in yi (if there are any common subspaces).

void prepend(const base::State *state)

Prepend state to the start of this path. The memory for state is copied.

void keepAfter(const base::State *state)

Keep the part of the path that is after state (getClosestIndex() is used to find out which way-point is closest to state)

void keepBefore(const base::State *state)

Keep the part of the path that is before state (getClosestIndex() is used to find out which way-point is closest to state)

void random()

Set this path to a random segment.

bool randomValid(unsigned int attempts)

Set this path to a random valid segment. Sample attempts times for valid segments. Returns true on success.

Functionality for accessing states

int getClosestIndex(const base::State *state) const

Get the index of the way-point along the path that is closest to state. Returns -1 for an empty path.

inline std::vector<base::State*> &getStates()

Get the states that make up the path (as a reference, so it can be modified, hence the function is not const)

inline base::State *getState(unsigned int index)

Get the state located at index along the path.

inline const base::State *getState(unsigned int index) const

Get the state located at index along the path.

inline std::size_t getStateCount() const

Get the number of states (way-points) that make up this path.

void clear()

Remove all states and clear memory.

Public Functions

inline PathGeometric(const base::SpaceInformationPtr &si)

Construct a path instance for a given space information.

PathGeometric(const PathGeometric &path)

Copy constructor.

PathGeometric(const base::SpaceInformationPtr &si, const base::State *state)

Construct a path instance from a single state.

PathGeometric(const base::SpaceInformationPtr &si, const base::State *state1, const base::State *state2)

Construct a path instance from two states (thus making a segment)

PathGeometric(const base::SpaceInformationPtr &si, std::vector<const base::State*> &states)

Construct a path from a sequence of states.

inline ~PathGeometric() override
PathGeometric &operator=(const PathGeometric &other)

Assignment operator.

base::Cost cost(const base::OptimizationObjectivePtr &obj) const override

The sum of the costs for the sequence of segments that make up the path, computed using OptimizationObjective::motionCost(). OptimizationObjective::initialCost() and OptimizationObjective::terminalCost() are also used in the computation for the first and last states, respectively. Empty paths have identity cost.

virtual double length() const override

Compute the length of a geometric path (sum of lengths of segments that make up the path)

virtual bool check() const override

Check if the path is valid.

double smoothness() const

Compute a notion of smoothness for this path. The closer the value is to 0, the smoother the path. Detailed formula follows.

The idea is to look at the triangles formed by consecutive path segments and compute the angle between those segments using Pythagora’s theorem. Then, the outside angle for the computed angle is normalized by the path segments and contributes to the path smoothness. For a straight line path, the smoothness will be 0.

\[ \mbox{smoothness} = \sum\limits_{i=2}^{n-1}\left(\frac{2\left(\pi - \arccos\left(\frac{a_i^2+b_i^2-c_i^2}{2 a_i b_i}\right)\right)}{a_i + b_i}\right)^2 \]
where \(a_i = \mbox{dist}(s_{i-2}, s_{i-1}), b_i = \mbox{dist}(s_{i-1}, s_{i}), c_i = \mbox{dist}(s_{i-2}, s_i)\), \(s_i\) is the ith state along the path (see getState()) and \(\mbox{dist}(s_i, s_j)\) gives the distance between two states (see ompl::base::StateSpace::distance()).

double clearance() const

Compute the clearance of the way-points along the path (no interpolation is performed). Detailed formula follows.

The formula used for computing clearance is:

\[ \mbox{clearance} = \frac{1}{n}\sum\limits_{i=0}^{n-1}cl(s_i) \]
\(n\) is the number of states along the path (see getStateCount()) \(s_i\) is the ith state along the path (see getState()) \(cl()\) gives the distance to the nearest invalid state for a particular state (see ompl::base::StateValidityChecker::clearance())

virtual void print(std::ostream &out) const override

Print the path to a stream.

virtual void printAsMatrix(std::ostream &out) const

Print the path as a real-valued matrix where the i-th row represents the i-th state along the path. Each row contains the state components as returned by ompl::base::StateSpace::copyToReals.

Protected Functions

void freeMemory()

Free the memory corresponding to the states on this path.

void copyFrom(const PathGeometric &other)

Copy data to this path from another path instance.

Protected Attributes

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

The list of states that make up the path.