37 #include "ompl/base/spaces/DubinsStateSpace.h"
38 #include "ompl/base/SpaceInformation.h"
39 #include "ompl/util/Exception.h"
41 #include <boost/math/constants/constants.hpp>
44 using namespace ompl::base;
48 const double twopi = 2. * boost::math::constants::pi<double>();
49 const double DUBINS_EPS = 1e-6;
50 const double DUBINS_ZERO = -1e-9;
52 inline double mod2pi(
double x)
54 if (x<0 && x>DUBINS_ZERO)
return 0;
55 return x - twopi * floor(x / twopi);
60 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
61 double tmp = 2. + d*d - 2.*(ca*cb +sa*sb - d*(sa - sb));
62 if (tmp >= DUBINS_ZERO)
64 double theta = atan2(cb - ca, d + sa - sb);
65 double t = mod2pi(-alpha + theta);
66 double p = sqrt(std::max(tmp, 0.));
67 double q = mod2pi(beta - theta);
68 assert(fabs(p*cos(alpha + t) - sa + sb - d) < DUBINS_EPS);
69 assert(fabs(p*sin(alpha + t) + ca - cb) < DUBINS_EPS);
70 assert(mod2pi(alpha + t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
78 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
79 double tmp = 2. + d*d - 2.*(ca*cb + sa*sb - d*(sb - sa));
80 if (tmp >= DUBINS_ZERO)
82 double theta = atan2(ca - cb, d - sa + sb);
83 double t = mod2pi(alpha - theta);
84 double p = sqrt(std::max(tmp, 0.));
85 double q = mod2pi(-beta + theta);
86 assert(fabs(p*cos(alpha - t) + sa - sb - d) < DUBINS_EPS);
87 assert(fabs(p*sin(alpha - t) - ca + cb) < DUBINS_EPS);
88 assert(mod2pi(alpha - t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
96 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
97 double tmp = d * d - 2. + 2. * (ca*cb + sa*sb - d * (sa + sb));
98 if (tmp >= DUBINS_ZERO)
100 double p = sqrt(std::max(tmp, 0.));
101 double theta = atan2(ca + cb, d - sa - sb) - atan2(2., p);
102 double t = mod2pi(alpha - theta);
103 double q = mod2pi(beta - theta);
104 assert(fabs(p*cos(alpha - t) - 2. * sin(alpha - t) + sa + sb - d) < DUBINS_EPS);
105 assert(fabs(p*sin(alpha - t) + 2. * cos(alpha - t) - ca - cb) < DUBINS_EPS);
106 assert(mod2pi(alpha - t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
114 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
115 double tmp = -2. + d * d + 2. * (ca*cb + sa*sb + d * (sa + sb));
116 if (tmp >= DUBINS_ZERO)
118 double p = sqrt(std::max(tmp, 0.));
119 double theta = atan2(-ca - cb, d + sa + sb) - atan2(-2., p);
120 double t = mod2pi(-alpha + theta);
121 double q = mod2pi(-beta + theta);
122 assert(fabs(p*cos(alpha + t) + 2. * sin(alpha + t) - sa - sb - d) < DUBINS_EPS);
123 assert(fabs(p*sin(alpha + t) - 2. * cos(alpha + t) + ca + cb) < DUBINS_EPS);
124 assert(mod2pi(alpha + t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
132 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
133 double tmp = .125 * (6. - d * d + 2. * (ca*cb + sa*sb + d * (sa - sb)));
136 double p = twopi - acos(tmp);
137 double theta = atan2(ca - cb, d - sa + sb);
138 double t = mod2pi(alpha - theta + .5 * p);
139 double q = mod2pi(alpha - beta - t + p);
140 assert(fabs( 2.*sin(alpha - t + p) - 2. * sin(alpha - t) - d + sa - sb) < DUBINS_EPS);
141 assert(fabs(-2.*cos(alpha - t + p) + 2. * cos(alpha - t) - ca + cb) < DUBINS_EPS);
142 assert(mod2pi(alpha - t + p - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
150 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
151 double tmp = .125 * (6. - d * d + 2. * (ca*cb + sa*sb - d * (sa - sb)));
154 double p = twopi - acos(tmp);
155 double theta = atan2(-ca + cb, d + sa - sb);
156 double t = mod2pi(-alpha + theta + .5 * p);
157 double q = mod2pi(beta - alpha - t + p);
158 assert(fabs(-2.*sin(alpha + t - p) + 2. * sin(alpha + t) - d - sa + sb) < DUBINS_EPS);
159 assert(fabs( 2.*cos(alpha + t - p) - 2. * cos(alpha + t) + ca - cb) < DUBINS_EPS);
160 assert(mod2pi(alpha + t - p + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
168 if (d<DUBINS_EPS && fabs(alpha-beta)<DUBINS_EPS)
172 double len, minLength = path.length();
174 if ((len = tmp.length()) < minLength)
179 tmp = dubinsRSL(d, alpha, beta);
180 if ((len = tmp.length()) < minLength)
185 tmp = dubinsLSR(d, alpha, beta);
186 if ((len = tmp.length()) < minLength)
191 tmp = dubinsRLR(d, alpha, beta);
192 if ((len = tmp.length()) < minLength)
197 tmp = dubinsLRL(d, alpha, beta);
198 if ((len = tmp.length()) < minLength)
206 { DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_LEFT },
207 { DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_RIGHT },
208 { DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_LEFT },
209 { DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_RIGHT },
210 { DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT },
211 { DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT }
217 return rho_ * std::min(dubins(state1, state2).length(), dubins(state2, state1).length());
219 return rho_ * dubins(state1, state2).length();
224 bool firstTime =
true;
226 interpolate(from, to, t, firstTime, path, state);
230 bool& firstTime, DubinsPath& path,
State *state)
const
237 copyState(state, to);
243 copyState(state, from);
247 path = dubins(from, to);
250 DubinsPath path2(dubins(to, from));
251 if (path2.length() < path.length())
253 path2.reverse_ =
true;
259 interpolate(from, path, t, state);
264 StateType *s = allocState()->as<StateType>();
265 double seg = t * path.length(), phi, v;
268 s->setYaw(from->
as<StateType>()->getYaw());
271 for (
unsigned int i=0; i<3 && seg>0; ++i)
273 v = std::min(seg, path.length_[i]);
276 switch(path.type_[i])
279 s->setXY(s->getX() + sin(phi+v) - sin(phi), s->getY() - cos(phi+v) + cos(phi));
283 s->setXY(s->getX() - sin(phi-v) + sin(phi), s->getY() + cos(phi-v) - cos(phi));
286 case DUBINS_STRAIGHT:
287 s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
294 for (
unsigned int i=0; i<3 && seg>0; ++i)
296 v = std::min(seg, path.length_[2-i]);
299 switch(path.type_[2-i])
302 s->setXY(s->getX() + sin(phi-v) - sin(phi), s->getY() - cos(phi-v) + cos(phi));
306 s->setXY(s->getX() - sin(phi+v) + sin(phi), s->getY() + cos(phi+v) - cos(phi));
309 case DUBINS_STRAIGHT:
310 s->setXY(s->getX() - v * cos(phi), s->getY() - v * sin(phi));
315 state->
as<StateType>()->setX(s->getX() * rho_ + from->
as<StateType>()->getX());
316 state->
as<StateType>()->setY(s->getY() * rho_ + from->
as<StateType>()->getY());
318 state->
as<StateType>()->setYaw(s->getYaw());
328 double dx = x2 - x1, dy = y2 - y1, d = sqrt(dx*dx + dy*dy) / rho_, th = atan2(dy, dx);
329 double alpha = mod2pi(th1 - th), beta = mod2pi(th2 - th);
330 return ::dubins(d, alpha, beta);
334 void ompl::base::DubinsMotionValidator::defaultSettings(
void)
338 throw Exception(
"No state space for motion validator");
345 bool result =
true, firstTime =
true;
347 int nd = stateSpace_->validSegmentCount(s1, s2);
352 State *test = si_->allocState();
354 for (
int j = 1 ; j < nd ; ++j)
356 stateSpace_->interpolate(s1, s2, (
double)j / (
double)nd, firstTime, path, test);
357 if (!si_->isValid(test))
359 lastValid.second = (double)(j - 1) / (double)nd;
361 stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
366 si_->freeState(test);
370 if (!si_->isValid(s2))
372 lastValid.second = (double)(nd - 1) / (double)nd;
374 stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
389 if (!si_->isValid(s2))
392 bool result =
true, firstTime =
true;
394 int nd = stateSpace_->validSegmentCount(s1, s2);
397 std::queue< std::pair<int, int> > pos;
400 pos.push(std::make_pair(1, nd - 1));
403 State *test = si_->allocState();
408 std::pair<int, int> x = pos.front();
410 int mid = (x.first + x.second) / 2;
411 stateSpace_->interpolate(s1, s2, (
double)mid / (
double)nd, firstTime, path, test);
413 if (!si_->isValid(test))
422 pos.push(std::make_pair(x.first, mid - 1));
424 pos.push(std::make_pair(mid + 1, x.second));
427 si_->freeState(test);