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