Definition of a geometric path. More...
#include <ompl/geometric/PathGeometric.h>

Public Member Functions | |
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. | |
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. | |
double | length () const override |
Compute the length of a geometric path (sum of lengths of segments that make up the path) | |
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. | |
double | clearance () const |
Compute the clearance of the way-points along the path (no interpolation is performed). Detailed formula follows. | |
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. | |
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. | |
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). | |
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. | |
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. | |
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) | |
base::State * | getState (unsigned int index) |
Get the state located at index along the path. | |
const base::State * | getState (unsigned int index) const |
Get the state located at index along the path. | |
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. | |
![]() | |
Path (const Path &)=delete | |
Path & | operator= (const Path &)=delete |
Path (SpaceInformationPtr si) | |
Constructor. A path must always know the space information it is part of. | |
virtual | ~Path ()=default |
Destructor. | |
const SpaceInformationPtr & | getSpaceInformation () const |
Get the space information associated to this class. | |
template<class T > | |
const T * | as () const |
Cast this instance to a desired type. | |
template<class T > | |
T * | as () |
Cast this instance to a desired type. | |
virtual Cost | cost (const OptimizationObjectivePtr &obj) const =0 |
Return the cost of the path with respect to a specified optimization objective. | |
Protected Member 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. | |
![]() | |
SpaceInformationPtr | si_ |
The space information this path is part of. | |
Detailed Description
Definition of a geometric path.
This is the type of path computed by geometric planners.
Definition at line 65 of file PathGeometric.h.
Constructor & Destructor Documentation
◆ PathGeometric() [1/5]
|
inline |
Construct a path instance for a given space information.
Definition at line 69 of file PathGeometric.h.
◆ PathGeometric() [2/5]
ompl::geometric::PathGeometric::PathGeometric | ( | const PathGeometric & | path | ) |
Copy constructor.
Definition at line 46 of file PathGeometric.cpp.
◆ PathGeometric() [3/5]
ompl::geometric::PathGeometric::PathGeometric | ( | const base::SpaceInformationPtr & | si, |
const base::State * | state ) |
Construct a path instance from a single state.
Definition at line 51 of file PathGeometric.cpp.
◆ PathGeometric() [4/5]
ompl::geometric::PathGeometric::PathGeometric | ( | const base::SpaceInformationPtr & | si, |
const base::State * | state1, | ||
const base::State * | state2 ) |
Construct a path instance from two states (thus making a segment)
Definition at line 58 of file PathGeometric.cpp.
◆ PathGeometric() [5/5]
ompl::geometric::PathGeometric::PathGeometric | ( | const base::SpaceInformationPtr & | si, |
std::vector< const base::State * > & | states ) |
Construct a path from a sequence of states.
Definition at line 67 of file PathGeometric.cpp.
◆ ~PathGeometric()
|
inlineoverride |
Definition at line 87 of file PathGeometric.h.
Member Function Documentation
◆ append() [1/2]
void ompl::geometric::PathGeometric::append | ( | const base::State * | state | ) |
Append state to the end of this path. The memory for state is copied.
Definition at line 462 of file PathGeometric.cpp.
◆ append() [2/2]
void ompl::geometric::PathGeometric::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).
Definition at line 467 of file PathGeometric.cpp.
◆ check()
|
overridevirtual |
Check if the path is valid.
Implements ompl::base::Path.
Definition at line 168 of file PathGeometric.cpp.
◆ checkAndRepair()
std::pair< bool, bool > ompl::geometric::PathGeometric::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.
- 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. If no changes are attempted, the both values are true.
- Note
- If repairing a path fails, the path may still be altered
Definition at line 211 of file PathGeometric.cpp.
◆ clear()
void ompl::geometric::PathGeometric::clear | ( | ) |
Remove all states and clear memory.
Definition at line 542 of file PathGeometric.cpp.
◆ clearance()
double ompl::geometric::PathGeometric::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())
Definition at line 120 of file PathGeometric.cpp.
◆ copyFrom()
|
protected |
Copy data to this path from another path instance.
Definition at line 87 of file PathGeometric.cpp.
◆ cost()
|
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.
Definition at line 100 of file PathGeometric.cpp.
◆ freeMemory()
|
protected |
Free the memory corresponding to the states on this path.
Definition at line 94 of file PathGeometric.cpp.
◆ getClosestIndex()
int ompl::geometric::PathGeometric::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.
Definition at line 523 of file PathGeometric.cpp.
◆ getState() [1/2]
|
inline |
Get the state located at index along the path.
Definition at line 248 of file PathGeometric.h.
◆ getState() [2/2]
|
inline |
Get the state located at index along the path.
Definition at line 254 of file PathGeometric.h.
◆ getStateCount()
|
inline |
Get the number of states (way-points) that make up this path.
Definition at line 260 of file PathGeometric.h.
◆ getStates()
|
inline |
Get the states that make up the path (as a reference, so it can be modified, hence the function is not const)
Definition at line 242 of file PathGeometric.h.
◆ interpolate() [1/2]
void ompl::geometric::PathGeometric::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.
Definition at line 316 of file PathGeometric.cpp.
◆ interpolate() [2/2]
void ompl::geometric::PathGeometric::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.
Definition at line 337 of file PathGeometric.cpp.
◆ keepAfter()
void ompl::geometric::PathGeometric::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)
Definition at line 484 of file PathGeometric.cpp.
◆ keepBefore()
void ompl::geometric::PathGeometric::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)
Definition at line 502 of file PathGeometric.cpp.
◆ length()
|
overridevirtual |
Compute the length of a geometric path (sum of lengths of segments that make up the path)
Implements ompl::base::Path.
Definition at line 112 of file PathGeometric.cpp.
◆ operator=()
ompl::geometric::PathGeometric & ompl::geometric::PathGeometric::operator= | ( | const PathGeometric & | other | ) |
Assignment operator.
Definition at line 76 of file PathGeometric.cpp.
◆ overlay()
void ompl::geometric::PathGeometric::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=()
Definition at line 441 of file PathGeometric.cpp.
◆ prepend()
void ompl::geometric::PathGeometric::prepend | ( | const base::State * | state | ) |
Prepend state to the start of this path. The memory for state is copied.
Definition at line 479 of file PathGeometric.cpp.
◆ print()
|
overridevirtual |
Print the path to a stream.
Implements ompl::base::Path.
Definition at line 191 of file PathGeometric.cpp.
◆ printAsMatrix()
|
virtual |
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.
Definition at line 198 of file PathGeometric.cpp.
◆ random()
void ompl::geometric::PathGeometric::random | ( | ) |
Set this path to a random segment.
Definition at line 404 of file PathGeometric.cpp.
◆ randomValid()
bool ompl::geometric::PathGeometric::randomValid | ( | unsigned int | attempts | ) |
Set this path to a random valid segment. Sample attempts times for valid segments. Returns true on success.
Definition at line 415 of file PathGeometric.cpp.
◆ reverse()
void ompl::geometric::PathGeometric::reverse | ( | ) |
Reverse the path.
Definition at line 399 of file PathGeometric.cpp.
◆ smoothness()
double ompl::geometric::PathGeometric::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()).
Definition at line 132 of file PathGeometric.cpp.
◆ subdivide()
void ompl::geometric::PathGeometric::subdivide | ( | ) |
Add a state at the middle of each segment.
Definition at line 301 of file PathGeometric.cpp.
Member Data Documentation
◆ states_
|
protected |
The list of states that make up the path.
Definition at line 278 of file PathGeometric.h.
The documentation for this class was generated from the following files:
- ompl/geometric/PathGeometric.h
- ompl/geometric/src/PathGeometric.cpp