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>
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());
326 double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
327 double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->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()
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);
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
An SE(2) state space where distance is measured by the length of Dubins curves.
DubinsPathSegmentType
The Dubins path segment type.
The definition of a state in SO(2)
virtual void interpolate(const State *from, const State *to, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
virtual bool checkMotion(const State *s1, const State *s2) const
Check if the path between two states (from s1 to s2) is valid. This function assumes s1 is valid...
Definition of an abstract state.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The exception type for ompl.
static const DubinsPathSegmentType dubinsPathType[6][3]
Dubins path types.
const T * as() const
Cast this instance to a desired type.
DubinsPath dubins(const State *state1, const State *state2) const
Return the shortest Dubins path from SE(2) state state1 to SE(2) state state2.
Complete description of a Dubins path.