ompl/base/spaces/src/DubinsStateSpace.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/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() 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 /* assume motion starts in a valid configuration so s1 is valid */ 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 /* temporary storage for the checked state */ 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 /* assume motion starts in a valid configuration so s1 is valid */ 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 /* initialize the queue of test positions */ 00397 std::queue< std::pair<int, int> > pos; 00398 if (nd >= 2) 00399 { 00400 pos.push(std::make_pair(1, nd - 1)); 00401 00402 /* temporary storage for the checked state */ 00403 State *test = si_->allocState(); 00404 00405 /* repeatedly subdivide the path segment in the middle (and check the middle) */ 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 }