ompl/base/spaces/src/ReedsSheppStateSpace.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/ReedsSheppStateSpace.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     // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
00049 
00050     const double pi = boost::math::constants::pi<double>();
00051     const double twopi = 2. * pi;
00052     const double RS_EPS = 1e-6;
00053     const double ZERO = 10*std::numeric_limits<double>::epsilon();
00054 
00055     inline double mod2pi(double x)
00056     {
00057         double v = fmod(x, twopi);
00058         if (v < -pi)
00059             v += twopi;
00060         else
00061             if (v > pi)
00062                 v -= twopi;
00063         return v;
00064     }
00065     inline void polar(double x, double y, double &r, double &theta)
00066     {
00067         r = sqrt(x*x + y*y);
00068         theta = atan2(y, x);
00069     }
00070     inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
00071     {
00072         double delta = mod2pi(u-v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
00073         double t1 = atan2(eta*A - xi*B, xi*A + eta*B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
00074         tau = (t2<0) ? mod2pi(t1+pi) : mod2pi(t1);
00075         omega = mod2pi(tau - u + v - phi) ;
00076     }
00077 
00078     // formula 8.1 in Reeds-Shepp paper
00079     inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
00080     {
00081         polar(x - sin(phi), y - 1. + cos(phi), u, t);
00082         if (t >= -ZERO)
00083         {
00084             v = mod2pi(phi - t);
00085             if (v >= -ZERO)
00086             {
00087                 assert(fabs(u*cos(t) + sin(phi) - x) < RS_EPS);
00088                 assert(fabs(u*sin(t) - cos(phi) + 1 - y) < RS_EPS);
00089                 assert(fabs(mod2pi(t+v - phi)) < RS_EPS);
00090                 return true;
00091             }
00092         }
00093         return false;
00094     }
00095     // formula 8.2
00096     inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
00097     {
00098         double t1, u1;
00099         polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
00100         u1 = u1*u1;
00101         if (u1 >= 4.)
00102         {
00103             double theta;
00104             u = sqrt(u1 - 4.);
00105             theta = atan2(2., u);
00106             t = mod2pi(t1 + theta);
00107             v = mod2pi(t - phi);
00108             assert(fabs(2*sin(t) + u*cos(t) - sin(phi) - x) < RS_EPS);
00109             assert(fabs(-2*cos(t) + u*sin(t) + cos(phi) + 1 - y) < RS_EPS);
00110             assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
00111             return t>=-ZERO && v>=-ZERO;
00112         }
00113         return false;
00114     }
00115     void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
00116     {
00117         double t, u, v, Lmin = path.length(), L;
00118         if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
00119         {
00120             path = ReedsSheppStateSpace::ReedsSheppPath(
00121                 ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
00122             Lmin = L;
00123         }
00124         if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
00125         {
00126             path = ReedsSheppStateSpace::ReedsSheppPath(
00127                 ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
00128             Lmin = L;
00129         }
00130         if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
00131         {
00132             path = ReedsSheppStateSpace::ReedsSheppPath(
00133                 ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
00134             Lmin = L;
00135         }
00136         if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
00137         {
00138             path = ReedsSheppStateSpace::ReedsSheppPath(
00139                 ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
00140             Lmin = L;
00141         }
00142         if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
00143         {
00144             path = ReedsSheppStateSpace::ReedsSheppPath(
00145                 ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
00146             Lmin = L;
00147         }
00148         if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
00149         {
00150             path = ReedsSheppStateSpace::ReedsSheppPath(
00151                 ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
00152             Lmin = L;
00153         }
00154         if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
00155         {
00156             path = ReedsSheppStateSpace::ReedsSheppPath(
00157                 ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
00158             Lmin = L;
00159         }
00160         if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
00161             path = ReedsSheppStateSpace::ReedsSheppPath(
00162                 ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
00163     }
00164     // formula 8.3 / 8.4  *** TYPO IN PAPER ***
00165     inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
00166     {
00167         double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
00168         polar(xi, eta, u1, theta);
00169         if (u1 <= 4.)
00170         {
00171             u = -2.*asin(.25 * u1);
00172             t = mod2pi(theta + .5 * u + pi);
00173             v = mod2pi(phi - t + u);
00174             assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
00175             assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
00176             assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
00177             return t>=-ZERO && u<=ZERO;
00178         }
00179         return false;
00180     }
00181     void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
00182     {
00183         double t, u, v, Lmin = path.length(), L;
00184         if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
00185         {
00186             path = ReedsSheppStateSpace::ReedsSheppPath(
00187                 ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
00188             Lmin = L;
00189         }
00190         if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
00191         {
00192             path = ReedsSheppStateSpace::ReedsSheppPath(
00193                 ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
00194             Lmin = L;
00195         }
00196         if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
00197         {
00198             path = ReedsSheppStateSpace::ReedsSheppPath(
00199                 ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
00200             Lmin = L;
00201         }
00202         if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
00203         {
00204             path = ReedsSheppStateSpace::ReedsSheppPath(
00205                 ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
00206             Lmin = L;
00207         }
00208 
00209         // backwards
00210         double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
00211         if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
00212         {
00213             path = ReedsSheppStateSpace::ReedsSheppPath(
00214                 ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
00215             Lmin = L;
00216         }
00217         if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
00218         {
00219             path = ReedsSheppStateSpace::ReedsSheppPath(
00220                 ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
00221             Lmin = L;
00222         }
00223         if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
00224         {
00225             path = ReedsSheppStateSpace::ReedsSheppPath(
00226                 ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
00227             Lmin = L;
00228         }
00229         if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
00230             path = ReedsSheppStateSpace::ReedsSheppPath(
00231                 ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
00232     }
00233     // formula 8.7
00234     inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
00235     {
00236         double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
00237         if (rho <= 1.)
00238         {
00239             u = acos(rho);
00240             tauOmega(u, -u, xi, eta, phi, t, v);
00241             assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
00242             assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
00243             assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
00244             return t>=-ZERO && v<=ZERO;
00245         }
00246         return false;
00247     }
00248     // formula 8.8
00249     inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
00250     {
00251         double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
00252         if (rho>=0 && rho<=1)
00253         {
00254             u = -acos(rho);
00255             if (u >= -.5 * pi)
00256             {
00257                 tauOmega(u, u, xi, eta, phi, t, v);
00258                 assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
00259                 assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
00260                 assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
00261                 return t>=-ZERO && v>=-ZERO;
00262             }
00263         }
00264         return false;
00265     }
00266     void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
00267     {
00268         double t, u, v, Lmin = path.length(), L;
00269         if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
00270         {
00271             path = ReedsSheppStateSpace::ReedsSheppPath(
00272                 ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
00273             Lmin = L;
00274         }
00275         if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
00276         {
00277             path = ReedsSheppStateSpace::ReedsSheppPath(
00278                 ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
00279             Lmin = L;
00280         }
00281         if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
00282         {
00283             path = ReedsSheppStateSpace::ReedsSheppPath(
00284                 ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
00285             Lmin = L;
00286         }
00287         if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
00288         {
00289             path = ReedsSheppStateSpace::ReedsSheppPath(
00290                 ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
00291             Lmin = L;
00292         }
00293 
00294         if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
00295         {
00296             path = ReedsSheppStateSpace::ReedsSheppPath(
00297                 ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
00298             Lmin = L;
00299         }
00300         if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
00301         {
00302             path = ReedsSheppStateSpace::ReedsSheppPath(
00303                 ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
00304             Lmin = L;
00305         }
00306         if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
00307         {
00308             path = ReedsSheppStateSpace::ReedsSheppPath(
00309                 ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
00310             Lmin = L;
00311         }
00312         if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
00313             path = ReedsSheppStateSpace::ReedsSheppPath(
00314                 ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
00315     }
00316     // formula 8.9
00317     inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
00318     {
00319         double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
00320         polar(xi, eta, rho, theta);
00321         if (rho >= 2.)
00322         {
00323             double r = sqrt(rho*rho - 4.);
00324             u = 2. - r;
00325             t = mod2pi(theta + atan2(r, -2.));
00326             v = mod2pi(phi - .5*pi - t);
00327             assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
00328             assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
00329             assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
00330             return t>=-ZERO && u<=ZERO && v<=ZERO;
00331         }
00332         return false;
00333     }
00334     // formula 8.10
00335     inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
00336     {
00337         double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
00338         polar(-eta, xi, rho, theta);
00339         if (rho >= 2.)
00340         {
00341             t = theta;
00342             u = 2. - rho;
00343             v = mod2pi(t + .5*pi - phi);
00344             assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
00345             assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
00346             assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
00347             return t>=-ZERO && u<=ZERO && v<=ZERO;
00348         }
00349         return false;
00350     }
00351     void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
00352     {
00353         double t, u, v, Lmin = path.length() - .5*pi, L;
00354         if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
00355         {
00356             path = ReedsSheppStateSpace::ReedsSheppPath(
00357                 ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
00358             Lmin = L;
00359         }
00360         if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
00361         {
00362             path = ReedsSheppStateSpace::ReedsSheppPath(
00363                 ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
00364             Lmin = L;
00365         }
00366         if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
00367         {
00368             path = ReedsSheppStateSpace::ReedsSheppPath(
00369                 ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
00370             Lmin = L;
00371         }
00372         if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
00373         {
00374             path = ReedsSheppStateSpace::ReedsSheppPath(
00375                 ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
00376             Lmin = L;
00377         }
00378 
00379         if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
00380         {
00381             path = ReedsSheppStateSpace::ReedsSheppPath(
00382                 ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
00383             Lmin = L;
00384         }
00385         if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
00386         {
00387             path = ReedsSheppStateSpace::ReedsSheppPath(
00388                 ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
00389             Lmin = L;
00390         }
00391         if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
00392         {
00393             path = ReedsSheppStateSpace::ReedsSheppPath(
00394                 ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
00395             Lmin = L;
00396         }
00397         if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
00398         {
00399             path = ReedsSheppStateSpace::ReedsSheppPath(
00400                 ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
00401             Lmin = L;
00402         }
00403 
00404         // backwards
00405         double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
00406         if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
00407         {
00408             path = ReedsSheppStateSpace::ReedsSheppPath(
00409                 ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
00410             Lmin = L;
00411         }
00412         if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
00413         {
00414             path = ReedsSheppStateSpace::ReedsSheppPath(
00415                 ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
00416             Lmin = L;
00417         }
00418         if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
00419         {
00420             path = ReedsSheppStateSpace::ReedsSheppPath(
00421                 ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
00422             Lmin = L;
00423         }
00424         if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
00425         {
00426             path = ReedsSheppStateSpace::ReedsSheppPath(
00427                 ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
00428             Lmin = L;
00429         }
00430 
00431         if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
00432         {
00433             path = ReedsSheppStateSpace::ReedsSheppPath(
00434                 ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
00435             Lmin = L;
00436         }
00437         if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
00438         {
00439             path = ReedsSheppStateSpace::ReedsSheppPath(
00440                 ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
00441             Lmin = L;
00442         }
00443         if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
00444         {
00445             path = ReedsSheppStateSpace::ReedsSheppPath(
00446                 ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
00447             Lmin = L;
00448         }
00449         if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
00450             path = ReedsSheppStateSpace::ReedsSheppPath(
00451                 ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
00452     }
00453     // formula 8.11 *** TYPO IN PAPER ***
00454     inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
00455     {
00456         double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
00457         polar(xi, eta, rho, theta);
00458         if (rho >= 2.)
00459         {
00460             u = 4. - sqrt(rho*rho - 4.);
00461             if (u <= ZERO)
00462             {
00463                 t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
00464                 v = mod2pi(t - phi);
00465                 assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
00466                 assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
00467                 assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
00468                 return t>=-ZERO && v>=-ZERO;
00469             }
00470         }
00471         return false;
00472     }
00473     void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
00474     {
00475         double t, u, v, Lmin = path.length() - pi, L;
00476         if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
00477         {
00478             path = ReedsSheppStateSpace::ReedsSheppPath(
00479                 ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
00480             Lmin = L;
00481         }
00482         if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
00483         {
00484             path = ReedsSheppStateSpace::ReedsSheppPath(
00485                 ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
00486             Lmin = L;
00487         }
00488         if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
00489         {
00490             path = ReedsSheppStateSpace::ReedsSheppPath(
00491                 ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
00492             Lmin = L;
00493         }
00494         if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
00495             path = ReedsSheppStateSpace::ReedsSheppPath(
00496                 ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5*pi, -u, .5*pi, -v);
00497     }
00498 
00499     ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
00500     {
00501         ReedsSheppStateSpace::ReedsSheppPath path;
00502         CSC(x, y, phi, path);
00503         CCC(x, y, phi, path);
00504         CCCC(x, y, phi, path);
00505         CCSC(x, y, phi, path);
00506         CCSCC(x, y, phi, path);
00507         return path;
00508     }
00509 }
00510 
00511 const ompl::base::ReedsSheppStateSpace::ReedsSheppPathSegmentType
00512 ompl::base::ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
00513     { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP },             // 0
00514     { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP },            // 1
00515     { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP },           // 2
00516     { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP },           // 3
00517     { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP },        // 4
00518     { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP },       // 5
00519     { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },        // 6
00520     { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },       // 7
00521     { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP },       // 8
00522     { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP },        // 9
00523     { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },       // 10
00524     { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },        // 11
00525     { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },         // 12
00526     { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },         // 13
00527     { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },          // 14
00528     { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },        // 15
00529     { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT },      // 16
00530     { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT }       // 17
00531 };
00532 
00533 ompl::base::ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
00534     double t, double u, double v, double w, double x)
00535     : type_(type)
00536 {
00537     length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
00538     totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
00539 }
00540 
00541 
00542 double ompl::base::ReedsSheppStateSpace::distance(const State *state1, const State *state2) const
00543 {
00544     return rho_ * reedsShepp(state1, state2).length();
00545 }
00546 
00547 void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00548 {
00549     bool firstTime = true;
00550     ReedsSheppPath path;
00551     interpolate(from, to, t, firstTime, path, state);
00552 }
00553 
00554 void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t,
00555     bool &firstTime, ReedsSheppPath &path, State *state) const
00556 {
00557     if (firstTime)
00558     {
00559         if (t>=1.)
00560         {
00561             if (to != state)
00562                 copyState(state, to);
00563             return;
00564         }
00565         if (t<=0.)
00566         {
00567             if (from != state)
00568                 copyState(state, from);
00569             return;
00570         }
00571         path = reedsShepp(from, to);
00572         firstTime = false;
00573     }
00574     interpolate(from, path, t, state);
00575 }
00576 
00577 void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const ReedsSheppPath &path, double t, State *state) const
00578 {
00579     StateType *s = allocState()->as<StateType>();
00580     double seg = t * path.length(), phi, v;
00581 
00582     s->setXY(0., 0.);
00583     s->setYaw(from->as<StateType>()->getYaw());
00584     for (unsigned int i=0; i<5 && seg>0; ++i)
00585     {
00586         if (path.length_[i]<0)
00587         {
00588             v = std::max(-seg, path.length_[i]);
00589             seg += v;
00590         }
00591         else
00592         {
00593             v = std::min(seg, path.length_[i]);
00594             seg -= v;
00595         }
00596         phi = s->getYaw();
00597         switch(path.type_[i])
00598         {
00599             case RS_LEFT:
00600                 s->setXY(s->getX() + sin(phi+v) - sin(phi), s->getY() - cos(phi+v) + cos(phi));
00601                 s->setYaw(phi+v);
00602                 break;
00603             case RS_RIGHT:
00604                 s->setXY(s->getX() - sin(phi-v) + sin(phi), s->getY() + cos(phi-v) - cos(phi));
00605                 s->setYaw(phi-v);
00606                 break;
00607             case RS_STRAIGHT:
00608                 s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
00609                 break;
00610             case RS_NOP:
00611                 break;
00612         }
00613     }
00614     state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
00615     state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
00616     getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
00617     state->as<StateType>()->setYaw(s->getYaw());
00618     freeState(s);
00619 }
00620 
00621 ompl::base::ReedsSheppStateSpace::ReedsSheppPath ompl::base::ReedsSheppStateSpace::reedsShepp(const State *state1, const State *state2) const
00622 {
00623     const StateType *s1 = static_cast<const StateType*>(state1);
00624     const StateType *s2 = static_cast<const StateType*>(state2);
00625     double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
00626     double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
00627     double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1);
00628     double x = c*dx + s*dy, y = -s*dx + c*dy, phi = th2 - th1;
00629     return ::reedsShepp(x/rho_, y/rho_, phi);
00630 }
00631 
00632 
00633 void ompl::base::ReedsSheppMotionValidator::defaultSettings()
00634 {
00635     stateSpace_ = dynamic_cast<ReedsSheppStateSpace*>(si_->getStateSpace().get());
00636     if (!stateSpace_)
00637         throw Exception("No state space for motion validator");
00638 }
00639 
00640 bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
00641 {
00642     /* assume motion starts in a valid configuration so s1 is valid */
00643 
00644     bool result = true, firstTime = true;
00645     ReedsSheppStateSpace::ReedsSheppPath path;
00646     int nd = stateSpace_->validSegmentCount(s1, s2);
00647 
00648     if (nd > 1)
00649     {
00650         /* temporary storage for the checked state */
00651         State *test = si_->allocState();
00652 
00653         for (int j = 1 ; j < nd ; ++j)
00654         {
00655             stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
00656             if (!si_->isValid(test))
00657             {
00658                 lastValid.second = (double)(j - 1) / (double)nd;
00659                 if (lastValid.first)
00660                     stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
00661                 result = false;
00662                 break;
00663             }
00664         }
00665         si_->freeState(test);
00666     }
00667 
00668     if (result)
00669         if (!si_->isValid(s2))
00670         {
00671             lastValid.second = (double)(nd - 1) / (double)nd;
00672             if (lastValid.first)
00673                 stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
00674             result = false;
00675         }
00676 
00677     if (result)
00678         valid_++;
00679     else
00680         invalid_++;
00681 
00682     return result;
00683 }
00684 
00685 bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2) const
00686 {
00687     /* assume motion starts in a valid configuration so s1 is valid */
00688     if (!si_->isValid(s2))
00689         return false;
00690 
00691     bool result = true, firstTime = true;
00692     ReedsSheppStateSpace::ReedsSheppPath path;
00693     int nd = stateSpace_->validSegmentCount(s1, s2);
00694 
00695     /* initialize the queue of test positions */
00696     std::queue< std::pair<int, int> > pos;
00697     if (nd >= 2)
00698     {
00699         pos.push(std::make_pair(1, nd - 1));
00700 
00701         /* temporary storage for the checked state */
00702         State *test = si_->allocState();
00703 
00704         /* repeatedly subdivide the path segment in the middle (and check the middle) */
00705         while (!pos.empty())
00706         {
00707             std::pair<int, int> x = pos.front();
00708 
00709             int mid = (x.first + x.second) / 2;
00710             stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);
00711 
00712             if (!si_->isValid(test))
00713             {
00714                 result = false;
00715                 break;
00716             }
00717 
00718             pos.pop();
00719 
00720             if (x.first < mid)
00721                 pos.push(std::make_pair(x.first, mid - 1));
00722             if (x.second > mid)
00723                 pos.push(std::make_pair(mid + 1, x.second));
00724         }
00725 
00726         si_->freeState(test);
00727     }
00728 
00729     if (result)
00730         valid_++;
00731     else
00732         invalid_++;
00733 
00734     return result;
00735 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines