ompl/base/spaces/src/DubinsStateSpace.cpp
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 #include "ompl/base/spaces/DubinsStateSpace.h"
00038 #include "ompl/base/SpaceInformation.h"
00039 #include "ompl/util/Exception.h"
00040 #include <queue>
00041 #include <boost/math/constants/constants.hpp>
00042 
00043 
00044 using namespace ompl::base;
00045 
00046 namespace
00047 {
00048     const double twopi = 2. * boost::math::constants::pi<double>();
00049     const double DUBINS_EPS = 1e-6;
00050     const double DUBINS_ZERO = -1e-9;
00051 
00052     inline double mod2pi(double x)
00053     {
00054         if (x<0 && x>DUBINS_ZERO) return 0;
00055         return x - twopi * floor(x / twopi);
00056     }
00057 
00058     DubinsStateSpace::DubinsPath dubinsLSL(double d, double alpha, double beta)
00059     {
00060         double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
00061         double tmp = 2. + d*d - 2.*(ca*cb +sa*sb - d*(sa - sb));
00062         if (tmp >= DUBINS_ZERO)
00063         {
00064             double theta = atan2(cb - ca, d + sa - sb);
00065             double t = mod2pi(-alpha + theta);
00066             double p = sqrt(std::max(tmp, 0.));
00067             double q = mod2pi(beta - theta);
00068             assert(fabs(p*cos(alpha + t) - sa + sb - d) < DUBINS_EPS);
00069             assert(fabs(p*sin(alpha + t) + ca - cb) < DUBINS_EPS);
00070             assert(mod2pi(alpha + t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
00071             return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[0], t, p, q);
00072         }
00073         return DubinsStateSpace::DubinsPath();
00074     }
00075 
00076     DubinsStateSpace::DubinsPath dubinsRSR(double d, double alpha, double beta)
00077     {
00078         double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
00079         double tmp = 2. + d*d - 2.*(ca*cb + sa*sb - d*(sb - sa));
00080         if (tmp >= DUBINS_ZERO)
00081         {
00082             double theta = atan2(ca - cb, d - sa + sb);
00083             double t = mod2pi(alpha - theta);
00084             double p = sqrt(std::max(tmp, 0.));
00085             double q = mod2pi(-beta + theta);
00086             assert(fabs(p*cos(alpha - t) + sa - sb - d) < DUBINS_EPS);
00087             assert(fabs(p*sin(alpha - t) - ca + cb) < DUBINS_EPS);
00088             assert(mod2pi(alpha - t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
00089             return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[1], t, p, q);
00090         }
00091         return DubinsStateSpace::DubinsPath();
00092     }
00093 
00094     DubinsStateSpace::DubinsPath dubinsRSL(double d, double alpha, double beta)
00095     {
00096         double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
00097         double tmp = d * d - 2. + 2. * (ca*cb + sa*sb - d * (sa + sb));
00098         if (tmp >= DUBINS_ZERO)
00099         {
00100             double p = sqrt(std::max(tmp, 0.));
00101             double theta = atan2(ca + cb, d - sa - sb) - atan2(2., p);
00102             double t = mod2pi(alpha - theta);
00103             double q = mod2pi(beta - theta);
00104             assert(fabs(p*cos(alpha - t) - 2. * sin(alpha - t) + sa + sb - d) < DUBINS_EPS);
00105             assert(fabs(p*sin(alpha - t) + 2. * cos(alpha - t) - ca - cb) < DUBINS_EPS);
00106             assert(mod2pi(alpha - t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
00107             return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[2], t, p, q);
00108         }
00109         return DubinsStateSpace::DubinsPath();
00110     }
00111 
00112     DubinsStateSpace::DubinsPath dubinsLSR(double d, double alpha, double beta)
00113     {
00114         double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
00115         double tmp = -2. + d * d + 2. * (ca*cb + sa*sb + d * (sa + sb));
00116         if (tmp >= DUBINS_ZERO)
00117         {
00118             double p = sqrt(std::max(tmp, 0.));
00119             double theta = atan2(-ca - cb, d + sa + sb) - atan2(-2., p);
00120             double t = mod2pi(-alpha + theta);
00121             double q = mod2pi(-beta + theta);
00122             assert(fabs(p*cos(alpha + t) + 2. * sin(alpha + t) - sa - sb - d) < DUBINS_EPS);
00123             assert(fabs(p*sin(alpha + t) - 2. * cos(alpha + t) + ca + cb) < DUBINS_EPS);
00124             assert(mod2pi(alpha + t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
00125             return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[3], t, p, q);
00126         }
00127         return DubinsStateSpace::DubinsPath();
00128     }
00129 
00130     DubinsStateSpace::DubinsPath dubinsRLR(double d, double alpha, double beta)
00131     {
00132         double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
00133         double tmp = .125 * (6. - d * d  + 2. * (ca*cb + sa*sb + d * (sa - sb)));
00134         if (fabs(tmp) < 1.)
00135         {
00136             double p = twopi - acos(tmp);
00137             double theta = atan2(ca - cb, d - sa + sb);
00138             double t = mod2pi(alpha - theta + .5 * p);
00139             double q = mod2pi(alpha - beta - t + p);
00140             assert(fabs( 2.*sin(alpha - t + p) - 2. * sin(alpha - t) - d + sa - sb) < DUBINS_EPS);
00141             assert(fabs(-2.*cos(alpha - t + p) + 2. * cos(alpha - t) - ca + cb) < DUBINS_EPS);
00142             assert(mod2pi(alpha - t + p - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
00143             return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[4], t, p, q);
00144         }
00145         return DubinsStateSpace::DubinsPath();
00146     }
00147 
00148     DubinsStateSpace::DubinsPath dubinsLRL(double d, double alpha, double beta)
00149     {
00150         double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
00151         double tmp = .125 * (6. - d * d  + 2. * (ca*cb + sa*sb - d * (sa - sb)));
00152         if (fabs(tmp) < 1.)
00153         {
00154             double p = twopi - acos(tmp);
00155             double theta = atan2(-ca + cb, d + sa - sb);
00156             double t = mod2pi(-alpha + theta + .5 * p);
00157             double q = mod2pi(beta - alpha - t + p);
00158             assert(fabs(-2.*sin(alpha + t - p) + 2. * sin(alpha + t) - d - sa + sb) < DUBINS_EPS);
00159             assert(fabs( 2.*cos(alpha + t - p) - 2. * cos(alpha + t) + ca - cb) < DUBINS_EPS);
00160             assert(mod2pi(alpha + t - p + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
00161             return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[5], t, p, q);
00162         }
00163         return DubinsStateSpace::DubinsPath();
00164     }
00165 
00166     DubinsStateSpace::DubinsPath dubins(double d, double alpha, double beta)
00167     {
00168         if (d<DUBINS_EPS && fabs(alpha-beta)<DUBINS_EPS)
00169             return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[0], 0, d, 0);
00170 
00171         DubinsStateSpace::DubinsPath path(dubinsLSL(d, alpha, beta)), tmp(dubinsRSR(d, alpha, beta));
00172         double len, minLength = path.length();
00173 
00174         if ((len = tmp.length()) < minLength)
00175         {
00176             minLength = len;
00177             path = tmp;
00178         }
00179         tmp = dubinsRSL(d, alpha, beta);
00180         if ((len = tmp.length()) < minLength)
00181         {
00182             minLength = len;
00183             path = tmp;
00184         }
00185         tmp = dubinsLSR(d, alpha, beta);
00186         if ((len = tmp.length()) < minLength)
00187         {
00188             minLength = len;
00189             path = tmp;
00190         }
00191         tmp = dubinsRLR(d, alpha, beta);
00192         if ((len = tmp.length()) < minLength)
00193         {
00194             minLength = len;
00195             path = tmp;
00196         }
00197         tmp = dubinsLRL(d, alpha, beta);
00198         if ((len = tmp.length()) < minLength)
00199             path = tmp;
00200         return path;
00201     }
00202 }
00203 
00204 const ompl::base::DubinsStateSpace::DubinsPathSegmentType
00205 ompl::base::DubinsStateSpace::dubinsPathType[6][3] = {
00206     { DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_LEFT },
00207     { DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_RIGHT },
00208     { DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_LEFT },
00209     { DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_RIGHT },
00210     { DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT },
00211     { DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT }
00212 };
00213 
00214 double ompl::base::DubinsStateSpace::distance(const State *state1, const State *state2) const
00215 {
00216     if (isSymmetric_)
00217         return rho_ * std::min(dubins(state1, state2).length(), dubins(state2, state1).length());
00218     else
00219         return rho_ * dubins(state1, state2).length();
00220 }
00221 
00222 void ompl::base::DubinsStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00223 {
00224     bool firstTime = true;
00225     DubinsPath path;
00226     interpolate(from, to, t, firstTime, path, state);
00227 }
00228 
00229 void ompl::base::DubinsStateSpace::interpolate(const State *from, const State *to, const double t,
00230     bool &firstTime, DubinsPath &path, State *state) const
00231 {
00232     if (firstTime)
00233     {
00234         if (t>=1.)
00235         {
00236             if (to != state)
00237                 copyState(state, to);
00238             return;
00239         }
00240         if (t<=0.)
00241         {
00242             if (from != state)
00243                 copyState(state, from);
00244             return;
00245         }
00246 
00247         path = dubins(from, to);
00248         if (isSymmetric_)
00249         {
00250             DubinsPath path2(dubins(to, from));
00251             if (path2.length() < path.length())
00252             {
00253                 path2.reverse_ = true;
00254                 path = path2;
00255             }
00256         }
00257         firstTime = false;
00258     }
00259     interpolate(from, path, t, state);
00260 }
00261 
00262 void ompl::base::DubinsStateSpace::interpolate(const State *from, const DubinsPath &path, double t, State *state) const
00263 {
00264     StateType *s  = allocState()->as<StateType>();
00265     double seg = t * path.length(), phi, v;
00266 
00267     s->setXY(0., 0.);
00268     s->setYaw(from->as<StateType>()->getYaw());
00269     if (!path.reverse_)
00270     {
00271         for (unsigned int i=0; i<3 && seg>0; ++i)
00272         {
00273             v = std::min(seg, path.length_[i]);
00274             phi = s->getYaw();
00275             seg -= v;
00276             switch(path.type_[i])
00277             {
00278                 case DUBINS_LEFT:
00279                     s->setXY(s->getX() + sin(phi+v) - sin(phi), s->getY() - cos(phi+v) + cos(phi));
00280                     s->setYaw(phi+v);
00281                     break;
00282                 case DUBINS_RIGHT:
00283                     s->setXY(s->getX() - sin(phi-v) + sin(phi), s->getY() + cos(phi-v) - cos(phi));
00284                     s->setYaw(phi-v);
00285                     break;
00286                 case DUBINS_STRAIGHT:
00287                     s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
00288                     break;
00289             }
00290         }
00291     }
00292     else
00293     {
00294         for (unsigned int i=0; i<3 && seg>0; ++i)
00295         {
00296             v = std::min(seg, path.length_[2-i]);
00297             phi = s->getYaw();
00298             seg -= v;
00299             switch(path.type_[2-i])
00300             {
00301                 case DUBINS_LEFT:
00302                     s->setXY(s->getX() + sin(phi-v) - sin(phi), s->getY() - cos(phi-v) + cos(phi));
00303                     s->setYaw(phi-v);
00304                     break;
00305                 case DUBINS_RIGHT:
00306                     s->setXY(s->getX() - sin(phi+v) + sin(phi), s->getY() + cos(phi+v) - cos(phi));
00307                     s->setYaw(phi+v);
00308                     break;
00309                 case DUBINS_STRAIGHT:
00310                     s->setXY(s->getX() - v * cos(phi), s->getY() - v * sin(phi));
00311                     break;
00312             }
00313         }
00314     }
00315     state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
00316     state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
00317     getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
00318     state->as<StateType>()->setYaw(s->getYaw());
00319     freeState(s);
00320 }
00321 
00322 ompl::base::DubinsStateSpace::DubinsPath ompl::base::DubinsStateSpace::dubins(const State *state1, const State *state2) const
00323 {
00324     const StateType *s1 = static_cast<const StateType*>(state1);
00325     const StateType *s2 = static_cast<const StateType*>(state2);
00326     double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
00327     double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
00328     double dx = x2 - x1, dy = y2 - y1, d = sqrt(dx*dx + dy*dy) / rho_, th = atan2(dy, dx);
00329     double alpha = mod2pi(th1 - th), beta = mod2pi(th2 - th);
00330     return ::dubins(d, alpha, beta);
00331 }
00332 
00333 
00334 void ompl::base::DubinsMotionValidator::defaultSettings()
00335 {
00336     stateSpace_ = dynamic_cast<DubinsStateSpace*>(si_->getStateSpace().get());
00337     if (!stateSpace_)
00338         throw Exception("No state space for motion validator");
00339 }
00340 
00341 bool ompl::base::DubinsMotionValidator::checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
00342 {
00343     /* assume motion starts in a valid configuration so s1 is valid */
00344 
00345     bool result = true, firstTime = true;
00346     DubinsStateSpace::DubinsPath path;
00347     int nd = stateSpace_->validSegmentCount(s1, s2);
00348 
00349     if (nd > 1)
00350     {
00351         /* temporary storage for the checked state */
00352         State *test = si_->allocState();
00353 
00354         for (int j = 1 ; j < nd ; ++j)
00355         {
00356             stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
00357             if (!si_->isValid(test))
00358             {
00359                 lastValid.second = (double)(j - 1) / (double)nd;
00360                 if (lastValid.first)
00361                     stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
00362                 result = false;
00363                 break;
00364             }
00365         }
00366         si_->freeState(test);
00367     }
00368 
00369     if (result)
00370         if (!si_->isValid(s2))
00371         {
00372             lastValid.second = (double)(nd - 1) / (double)nd;
00373             if (lastValid.first)
00374                 stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
00375             result = false;
00376         }
00377 
00378     if (result)
00379         valid_++;
00380     else
00381         invalid_++;
00382 
00383     return result;
00384 }
00385 
00386 bool ompl::base::DubinsMotionValidator::checkMotion(const State *s1, const State *s2) const
00387 {
00388     /* assume motion starts in a valid configuration so s1 is valid */
00389     if (!si_->isValid(s2))
00390         return false;
00391 
00392     bool result = true, firstTime = true;
00393     DubinsStateSpace::DubinsPath path;
00394     int nd = stateSpace_->validSegmentCount(s1, s2);
00395 
00396     /* initialize the queue of test positions */
00397     std::queue< std::pair<int, int> > pos;
00398     if (nd >= 2)
00399     {
00400         pos.push(std::make_pair(1, nd - 1));
00401 
00402         /* temporary storage for the checked state */
00403         State *test = si_->allocState();
00404 
00405         /* repeatedly subdivide the path segment in the middle (and check the middle) */
00406         while (!pos.empty())
00407         {
00408             std::pair<int, int> x = pos.front();
00409 
00410             int mid = (x.first + x.second) / 2;
00411             stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);
00412 
00413             if (!si_->isValid(test))
00414             {
00415                 result = false;
00416                 break;
00417             }
00418 
00419             pos.pop();
00420 
00421             if (x.first < mid)
00422                 pos.push(std::make_pair(x.first, mid - 1));
00423             if (x.second > mid)
00424                 pos.push(std::make_pair(mid + 1, x.second));
00425         }
00426 
00427         si_->freeState(test);
00428     }
00429 
00430     if (result)
00431         valid_++;
00432     else
00433         invalid_++;
00434 
00435     return result;
00436 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines