00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_BASE_SPACES_DUBINS_STATE_SPACE_
00038 #define OMPL_BASE_SPACES_DUBINS_STATE_SPACE_
00039
00040 #include "ompl/base/spaces/SE2StateSpace.h"
00041 #include "ompl/base/MotionValidator.h"
00042 #include <boost/math/constants/constants.hpp>
00043
00044 namespace ompl
00045 {
00046 namespace base
00047 {
00048
00072 class DubinsStateSpace : public SE2StateSpace
00073 {
00074 public:
00075
00077 enum DubinsPathSegmentType { DUBINS_LEFT=0, DUBINS_STRAIGHT=1, DUBINS_RIGHT=2 };
00079 static const DubinsPathSegmentType dubinsPathType[6][3];
00081 class DubinsPath
00082 {
00083 public:
00084 DubinsPath(const DubinsPathSegmentType* type = dubinsPathType[0],
00085 double t=0., double p=std::numeric_limits<double>::max(), double q=0.)
00086 : reverse_(false)
00087 {
00088 memcpy(type_, type, 3*sizeof(DubinsPathSegmentType));
00089 length_[0] = t;
00090 length_[1] = p;
00091 length_[2] = q;
00092 assert(t >= 0.);
00093 assert(p >= 0.);
00094 assert(q >= 0.);
00095 }
00096 double length() const
00097 {
00098 return length_[0] + length_[1] + length_[2];
00099 }
00100
00102 DubinsPathSegmentType type_[3];
00104 double length_[3];
00106 bool reverse_;
00107 };
00108
00109 DubinsStateSpace(double turningRadius = 1.0, bool isSymmetric = false)
00110 : SE2StateSpace(), rho_(turningRadius), isSymmetric_(isSymmetric)
00111 {
00112 }
00113
00114 virtual double distance(const State *state1, const State *state2) const;
00115
00116 virtual void interpolate(const State *from, const State *to, const double t,
00117 State *state) const;
00118 virtual void interpolate(const State *from, const State *to, const double t,
00119 bool& firstTime, DubinsPath& path, State *state) const;
00120
00121 virtual void sanityChecks(void) const
00122 {
00123 double zero = std::numeric_limits<double>::epsilon();
00124 double eps = std::numeric_limits<float>::epsilon();
00125 int flags = ~(STATESPACE_INTERPOLATION | STATESPACE_TRIANGLE_INEQUALITY | STATESPACE_DISTANCE_BOUND);
00126 if (!isSymmetric_)
00127 flags &= ~STATESPACE_DISTANCE_SYMMETRIC;
00128 StateSpace::sanityChecks(zero, eps, flags);
00129 }
00130
00132 DubinsPath dubins(const State *state1, const State *state2) const;
00133
00134 protected:
00135 virtual void interpolate(const State *from, const DubinsPath& path, const double t,
00136 State *state) const;
00137
00139 double rho_;
00140
00148 bool isSymmetric_;
00149 };
00150
00157 class DubinsMotionValidator : public MotionValidator
00158 {
00159 public:
00160 DubinsMotionValidator(SpaceInformation* si) : MotionValidator(si)
00161 {
00162 defaultSettings();
00163 }
00164 DubinsMotionValidator(const SpaceInformationPtr &si) : MotionValidator(si)
00165 {
00166 defaultSettings();
00167 }
00168 virtual ~DubinsMotionValidator(void)
00169 {
00170 }
00171 virtual bool checkMotion(const State *s1, const State *s2) const;
00172 virtual bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const;
00173 private:
00174 DubinsStateSpace *stateSpace_;
00175 void defaultSettings(void);
00176 };
00177
00178 }
00179 }
00180
00181 #endif