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 }