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