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> 47 const double twopi = 2. * boost::math::constants::pi<double>();
48 const double DUBINS_EPS = 1e-6;
49 const double DUBINS_ZERO = -1e-7;
51 inline double mod2pi(
double x)
53 if (x < 0 && x > DUBINS_ZERO)
55 double xm = x - twopi * floor(x / twopi);
56 if (twopi - xm < .5 * DUBINS_EPS) xm = 0.;
62 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
63 double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sa - sb));
64 if (tmp >= DUBINS_ZERO)
66 double theta = atan2(cb - ca, d + sa - sb);
67 double t = mod2pi(-alpha + theta);
68 double p = sqrt(std::max(tmp, 0.));
69 double q = mod2pi(beta - theta);
70 assert(fabs(p * cos(alpha + t) - sa + sb - d) < 2 * DUBINS_EPS);
71 assert(fabs(p * sin(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
72 assert(mod2pi(alpha + t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
80 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
81 double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sb - sa));
82 if (tmp >= DUBINS_ZERO)
84 double theta = atan2(ca - cb, d - sa + sb);
85 double t = mod2pi(alpha - theta);
86 double p = sqrt(std::max(tmp, 0.));
87 double q = mod2pi(-beta + theta);
88 assert(fabs(p * cos(alpha - t) + sa - sb - d) < 2* DUBINS_EPS);
89 assert(fabs(p * sin(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
90 assert(mod2pi(alpha - t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
98 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
99 double tmp = d * d - 2. + 2. * (ca * cb + sa * sb - d * (sa + sb));
100 if (tmp >= DUBINS_ZERO)
102 double p = sqrt(std::max(tmp, 0.));
103 double theta = atan2(ca + cb, d - sa - sb) - atan2(2., p);
104 double t = mod2pi(alpha - theta);
105 double q = mod2pi(beta - theta);
106 assert(fabs(p * cos(alpha - t) - 2. * sin(alpha - t) + sa + sb - d) < 2 * DUBINS_EPS);
107 assert(fabs(p * sin(alpha - t) + 2. * cos(alpha - t) - ca - cb) < 2 * DUBINS_EPS);
108 assert(mod2pi(alpha - t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
116 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
117 double tmp = -2. + d * d + 2. * (ca * cb + sa * sb + d * (sa + sb));
118 if (tmp >= DUBINS_ZERO)
120 double p = sqrt(std::max(tmp, 0.));
121 double theta = atan2(-ca - cb, d + sa + sb) - atan2(-2., p);
122 double t = mod2pi(-alpha + theta);
123 double q = mod2pi(-beta + theta);
124 assert(fabs(p * cos(alpha + t) + 2. * sin(alpha + t) - sa - sb - d) < 2 * DUBINS_EPS);
125 assert(fabs(p * sin(alpha + t) - 2. * cos(alpha + t) + ca + cb) < 2 * DUBINS_EPS);
126 assert(mod2pi(alpha + t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
134 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
135 double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb + d * (sa - sb)));
138 double p = twopi - acos(tmp);
139 double theta = atan2(ca - cb, d - sa + sb);
140 double t = mod2pi(alpha - theta + .5 * p);
141 double q = mod2pi(alpha - beta - t + p);
142 assert(fabs(2. * sin(alpha - t + p) - 2. * sin(alpha - t) - d + sa - sb) < 2 * DUBINS_EPS);
143 assert(fabs(-2. * cos(alpha - t + p) + 2. * cos(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
144 assert(mod2pi(alpha - t + p - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
152 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
153 double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb - d * (sa - sb)));
156 double p = twopi - acos(tmp);
157 double theta = atan2(-ca + cb, d + sa - sb);
158 double t = mod2pi(-alpha + theta + .5 * p);
159 double q = mod2pi(beta - alpha - t + p);
160 assert(fabs(-2. * sin(alpha + t - p) + 2. * sin(alpha + t) - d - sa + sb) < 2 * DUBINS_EPS);
161 assert(fabs(2. * cos(alpha + t - p) - 2. * cos(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
162 assert(mod2pi(alpha + t - p + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
170 if (d < DUBINS_EPS && fabs(alpha - beta) < DUBINS_EPS)
174 double len, minLength = path.length();
176 if ((len = tmp.length()) < minLength)
181 tmp = dubinsRSL(d, alpha, beta);
182 if ((len = tmp.length()) < minLength)
187 tmp = dubinsLSR(d, alpha, beta);
188 if ((len = tmp.length()) < minLength)
193 tmp = dubinsRLR(d, alpha, beta);
194 if ((len = tmp.length()) < minLength)
199 tmp = dubinsLRL(d, alpha, beta);
200 if ((len = tmp.length()) < minLength)
207 {DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_LEFT},
208 {DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_RIGHT},
209 {DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_LEFT},
210 {DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_RIGHT},
211 {DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT},
212 {DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT}};
217 return rho_ * std::min(dubins(state1, state2).length(), dubins(state2, state1).length());
218 return rho_ * dubins(state1, state2).length();
223 bool firstTime =
true;
225 interpolate(from, to, t, firstTime, path, state);
229 DubinsPath &path,
State *state)
const 236 copyState(state, to);
242 copyState(state, from);
246 path = dubins(from, to);
249 DubinsPath path2(dubins(to, from));
250 if (path2.length() < path.length())
252 path2.reverse_ =
true;
258 interpolate(from, path, t, state);
263 auto *s = allocState()->as<StateType>();
264 double seg = t * path.length(), phi, v;
267 s->setYaw(from->
as<StateType>()->getYaw());
270 for (
unsigned int i = 0; i < 3 && seg > 0; ++i)
272 v = std::min(seg, path.length_[i]);
275 switch (path.type_[i])
278 s->setXY(s->getX() + sin(phi + v) - sin(phi), s->getY() - cos(phi + v) + cos(phi));
282 s->setXY(s->getX() - sin(phi - v) + sin(phi), s->getY() + cos(phi - v) - cos(phi));
285 case DUBINS_STRAIGHT:
286 s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
293 for (
unsigned int i = 0; i < 3 && seg > 0; ++i)
295 v = std::min(seg, path.length_[2 - i]);
298 switch (path.type_[2 - i])
301 s->setXY(s->getX() + sin(phi - v) - sin(phi), s->getY() - cos(phi - v) + cos(phi));
305 s->setXY(s->getX() - sin(phi + v) + sin(phi), s->getY() + cos(phi + v) - cos(phi));
308 case DUBINS_STRAIGHT:
309 s->setXY(s->getX() - v * cos(phi), s->getY() - v * sin(phi));
314 state->
as<StateType>()->setX(s->getX() * rho_ + from->
as<StateType>()->getX());
315 state->
as<StateType>()->setY(s->getY() * rho_ + from->
as<StateType>()->getY());
317 state->
as<StateType>()->setYaw(s->getYaw());
322 const State *state2)
const 324 const auto *s1 =
static_cast<const StateType *
>(state1);
325 const auto *s2 =
static_cast<const StateType *
>(state2);
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);
333 void ompl::base::DubinsMotionValidator::defaultSettings()
336 if (stateSpace_ ==
nullptr)
337 throw Exception(
"No state space for motion validator");
341 std::pair<State *, double> &lastValid)
const 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;
360 if (lastValid.first !=
nullptr)
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;
373 if (lastValid.first !=
nullptr)
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);
bool checkMotion(const State *s1, const State *s2) const override
Check if the path between two states (from s1 to s2) is valid. This function assumes s1 is valid...
double distance(const State *state1, const State *state2) const override
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)
const T * as() const
Cast this instance to a desired type.
void interpolate(const State *from, const State *to, double t, State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
Definition of an abstract state.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The exception type for ompl.
DubinsPath dubins(const State *state1, const State *state2) const
Return the shortest Dubins path from SE(2) state state1 to SE(2) state state2.
static const DubinsPathSegmentType dubinsPathType[6][3]
Dubins path types.
Complete description of a Dubins path.