An SE(2) state space where distance is measured by the length of Reeds-Shepp curves. More...
#include <ompl/base/spaces/ReedsSheppStateSpace.h>

Public Types | |
enum | ReedsSheppPathSegmentType { RS_NOP = 0, RS_LEFT = 1, RS_STRAIGHT = 2, RS_RIGHT = 3 } |
The Reeds-Shepp path segment types. | |
Public Member Functions | |
ReedsSheppStateSpace (double turningRadius=1.0) | |
virtual double | distance (const State *state1, const State *state2) const |
Computes distance between two states. This function satisfies the properties of a metric if isMetricSpace() is true, and its return value will always be between 0 and getMaximumExtent() | |
virtual void | interpolate (const State *from, const State *to, const double t, State *state) const |
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state. The memory location of state is not required to be different from the memory of either from or to. | |
virtual void | interpolate (const State *from, const State *to, const double t, bool &firstTime, ReedsSheppPath &path, State *state) const |
virtual void | sanityChecks () const |
Convenience function that allows derived state spaces to choose which checks should pass (see SanityChecks flags) and how strict the checks are. This just calls sanityChecks() with some default arguments. | |
ReedsSheppPath | reedsShepp (const State *state1, const State *state2) const |
Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2. | |
Static Public Attributes | |
static const ReedsSheppPathSegmentType | reedsSheppPathType [18][5] |
Reeds-Shepp path types. | |
Protected Member Functions | |
virtual void | interpolate (const State *from, const ReedsSheppPath &path, const double t, State *state) const |
Protected Attributes | |
double | rho_ |
Turning radius. |
Detailed Description
An SE(2) state space where distance is measured by the length of Reeds-Shepp curves.
The notation and solutions are taken from: J.A. Reeds and L.A. Shepp, “Optimal paths for a car that goes both forwards and backwards,” Pacific Journal of Mathematics, 145(2):367–393, 1990.
This implementation explicitly computes all 48 Reeds-Shepp curves and returns the shortest valid solution. This can be improved by using the configuration space partition described in: P. Souères and J.-P. Laumond, “Shortest paths synthesis for a car-like robot,” IEEE Trans. on Automatic Control, 41(5):672–688, May 1996.
Definition at line 64 of file ReedsSheppStateSpace.h.
Member Data Documentation
const ompl::base::ReedsSheppStateSpace::ReedsSheppPathSegmentType base::ReedsSheppStateSpace::reedsSheppPathType [static] |
{ { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP }, { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP }, { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP }, { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP }, { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP }, { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT }, { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT } }
Reeds-Shepp path types.
Definition at line 71 of file ReedsSheppStateSpace.h.
The documentation for this class was generated from the following files:
- ompl/base/spaces/ReedsSheppStateSpace.h
- ompl/base/spaces/src/ReedsSheppStateSpace.cpp