ompl/base/spaces/DubinsStateSpace.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Mark Moll */ 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 00065 class DubinsStateSpace : public SE2StateSpace 00066 { 00067 public: 00068 00070 enum DubinsPathSegmentType { DUBINS_LEFT=0, DUBINS_STRAIGHT=1, DUBINS_RIGHT=2 }; 00072 static const DubinsPathSegmentType dubinsPathType[6][3]; 00074 class DubinsPath 00075 { 00076 public: 00077 DubinsPath(const DubinsPathSegmentType* type = dubinsPathType[0], 00078 double t=0., double p=std::numeric_limits<double>::max(), double q=0.) 00079 : type_(type), reverse_(false) 00080 { 00081 length_[0] = t; 00082 length_[1] = p; 00083 length_[2] = q; 00084 assert(t >= 0.); 00085 assert(p >= 0.); 00086 assert(q >= 0.); 00087 } 00088 double length() const 00089 { 00090 return length_[0] + length_[1] + length_[2]; 00091 } 00092 00094 const DubinsPathSegmentType* type_; 00096 double length_[3]; 00098 bool reverse_; 00099 }; 00100 00101 DubinsStateSpace(double turningRadius = 1.0, bool isSymmetric = false) 00102 : SE2StateSpace(), rho_(turningRadius), isSymmetric_(isSymmetric) 00103 { 00104 } 00105 00106 virtual bool isMetricSpace() const 00107 { 00108 return false; 00109 } 00110 00111 virtual double distance(const State *state1, const State *state2) const; 00112 00113 virtual void interpolate(const State *from, const State *to, const double t, 00114 State *state) const; 00115 virtual void interpolate(const State *from, const State *to, const double t, 00116 bool &firstTime, DubinsPath &path, State *state) const; 00117 00118 virtual bool hasSymmetricDistance() const 00119 { 00120 return isSymmetric_; 00121 } 00122 00123 virtual bool hasSymmetricInterpolate() const 00124 { 00125 return isSymmetric_; 00126 } 00127 00128 virtual void sanityChecks() const 00129 { 00130 double zero = std::numeric_limits<double>::epsilon(); 00131 double eps = std::numeric_limits<float>::epsilon(); 00132 int flags = ~(STATESPACE_INTERPOLATION | STATESPACE_TRIANGLE_INEQUALITY | STATESPACE_DISTANCE_BOUND); 00133 if (!isSymmetric_) 00134 flags &= ~STATESPACE_DISTANCE_SYMMETRIC; 00135 StateSpace::sanityChecks(zero, eps, flags); 00136 } 00137 00139 DubinsPath dubins(const State *state1, const State *state2) const; 00140 00141 protected: 00142 virtual void interpolate(const State *from, const DubinsPath &path, const double t, 00143 State *state) const; 00144 00146 double rho_; 00147 00155 bool isSymmetric_; 00156 }; 00157 00164 class DubinsMotionValidator : public MotionValidator 00165 { 00166 public: 00167 DubinsMotionValidator(SpaceInformation *si) : MotionValidator(si) 00168 { 00169 defaultSettings(); 00170 } 00171 DubinsMotionValidator(const SpaceInformationPtr &si) : MotionValidator(si) 00172 { 00173 defaultSettings(); 00174 } 00175 virtual ~DubinsMotionValidator() 00176 { 00177 } 00178 virtual bool checkMotion(const State *s1, const State *s2) const; 00179 virtual bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const; 00180 private: 00181 DubinsStateSpace *stateSpace_; 00182 void defaultSettings(); 00183 }; 00184 00185 } 00186 } 00187 00188 #endif