00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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
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)))
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)))
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)))
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)))
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)))
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)))
00161 path = ReedsSheppStateSpace::ReedsSheppPath(
00162 ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
00163 }
00164
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)))
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)))
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)))
00203 {
00204 path = ReedsSheppStateSpace::ReedsSheppPath(
00205 ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
00206 Lmin = L;
00207 }
00208
00209
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)))
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)))
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)))
00230 path = ReedsSheppStateSpace::ReedsSheppPath(
00231 ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
00232 }
00233
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
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)))
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)))
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)))
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)))
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)))
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)))
00313 path = ReedsSheppStateSpace::ReedsSheppPath(
00314 ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
00315 }
00316
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
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)))
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)))
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)))
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)))
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)))
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)))
00398 {
00399 path = ReedsSheppStateSpace::ReedsSheppPath(
00400 ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
00401 Lmin = L;
00402 }
00403
00404
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)))
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)))
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)))
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)))
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)))
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)))
00450 path = ReedsSheppStateSpace::ReedsSheppPath(
00451 ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
00452 }
00453
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)))
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)))
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)))
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 },
00514 { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP },
00515 { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP },
00516 { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP },
00517 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP },
00518 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP },
00519 { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },
00520 { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },
00521 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP },
00522 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP },
00523 { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },
00524 { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },
00525 { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },
00526 { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },
00527 { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },
00528 { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },
00529 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT },
00530 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT }
00531 };
00532
00533 ompl::base::ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
00534 double t, double u, double v, double w, double x)
00535 {
00536 memcpy(type_, type, 5*sizeof(ReedsSheppPathSegmentType));
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(void)
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
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
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
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
00696 std::queue< std::pair<int, int> > pos;
00697 if (nd >= 2)
00698 {
00699 pos.push(std::make_pair(1, nd - 1));
00700
00701
00702 State *test = si_->allocState();
00703
00704
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 }