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