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