All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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 
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