37 #include "ompl/base/spaces/ReedsSheppStateSpace.h"
38 #include "ompl/base/SpaceInformation.h"
39 #include "ompl/util/Exception.h"
41 #include <boost/math/constants/constants.hpp>
50 const double pi = boost::math::constants::pi<double>();
51 const double twopi = 2. * pi;
52 const double RS_EPS = 1e-6;
53 const double ZERO = 10*std::numeric_limits<double>::epsilon();
55 inline double mod2pi(
double x)
57 double v = fmod(x, twopi);
65 inline void polar(
double x,
double y,
double &r,
double &theta)
70 inline void tauOmega(
double u,
double v,
double xi,
double eta,
double phi,
double &tau,
double &omega)
72 double delta = mod2pi(u-v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
73 double t1 = atan2(eta*A - xi*B, xi*A + eta*B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
74 tau = (t2<0) ? mod2pi(t1+pi) : mod2pi(t1);
75 omega = mod2pi(tau - u + v - phi) ;
79 inline bool LpSpLp(
double x,
double y,
double phi,
double &t,
double &u,
double &v)
81 polar(x - sin(phi), y - 1. + cos(phi), u, t);
87 assert(fabs(u*cos(t) + sin(phi) - x) < RS_EPS);
88 assert(fabs(u*sin(t) - cos(phi) + 1 - y) < RS_EPS);
89 assert(fabs(mod2pi(t+v - phi)) < RS_EPS);
96 inline bool LpSpRp(
double x,
double y,
double phi,
double &t,
double &u,
double &v)
99 polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
105 theta = atan2(2., u);
106 t = mod2pi(t1 + theta);
108 assert(fabs(2*sin(t) + u*cos(t) - sin(phi) - x) < RS_EPS);
109 assert(fabs(-2*cos(t) + u*sin(t) + cos(phi) + 1 - y) < RS_EPS);
110 assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
111 return t>=-ZERO && v>=-ZERO;
117 double t, u, v, Lmin = path.length(), L;
118 if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
124 if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
130 if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
136 if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
142 if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
148 if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
154 if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
160 if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
165 inline bool LpRmL(
double x,
double y,
double phi,
double &t,
double &u,
double &v)
167 double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
168 polar(xi, eta, u1, theta);
171 u = -2.*asin(.25 * u1);
172 t = mod2pi(theta + .5 * u + pi);
173 v = mod2pi(phi - t + u);
174 assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
175 assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
176 assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
177 return t>=-ZERO && u<=ZERO;
183 double t, u, v, Lmin = path.length(), L;
184 if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
190 if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
196 if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
202 if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
210 double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
211 if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
217 if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
223 if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
229 if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
234 inline bool LpRupLumRm(
double x,
double y,
double phi,
double &t,
double &u,
double &v)
236 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
240 tauOmega(u, -u, xi, eta, phi, t, v);
241 assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
242 assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
243 assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
244 return t>=-ZERO && v<=ZERO;
249 inline bool LpRumLumRp(
double x,
double y,
double phi,
double &t,
double &u,
double &v)
251 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
252 if (rho>=0 && rho<=1)
257 tauOmega(u, u, xi, eta, phi, t, v);
258 assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
259 assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
260 assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
261 return t>=-ZERO && v>=-ZERO;
268 double t, u, v, Lmin = path.length(), L;
269 if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
275 if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
281 if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
287 if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
294 if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
300 if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
306 if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
312 if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
317 inline bool LpRmSmLm(
double x,
double y,
double phi,
double &t,
double &u,
double &v)
319 double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
320 polar(xi, eta, rho, theta);
323 double r = sqrt(rho*rho - 4.);
325 t = mod2pi(theta + atan2(r, -2.));
326 v = mod2pi(phi - .5*pi - t);
327 assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
328 assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
329 assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
330 return t>=-ZERO && u<=ZERO && v<=ZERO;
335 inline bool LpRmSmRm(
double x,
double y,
double phi,
double &t,
double &u,
double &v)
337 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
338 polar(-eta, xi, rho, theta);
343 v = mod2pi(t + .5*pi - phi);
344 assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
345 assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
346 assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
347 return t>=-ZERO && u<=ZERO && v<=ZERO;
353 double t, u, v, Lmin = path.length() - .5*pi, L;
354 if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
360 if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
366 if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
372 if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
379 if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
385 if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
391 if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
397 if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
405 double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
406 if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
412 if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
418 if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
424 if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
431 if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
437 if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
443 if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
449 if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
454 inline bool LpRmSLmRp(
double x,
double y,
double phi,
double &t,
double &u,
double &v)
456 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
457 polar(xi, eta, rho, theta);
460 u = 4. - sqrt(rho*rho - 4.);
463 t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
465 assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
466 assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
467 assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
468 return t>=-ZERO && v>=-ZERO;
475 double t, u, v, Lmin = path.length() - pi, L;
476 if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
482 if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
488 if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
494 if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
502 CSC(x, y, phi, path);
503 CCC(x, y, phi, path);
504 CCCC(x, y, phi, path);
505 CCSC(x, y, phi, path);
506 CCSCC(x, y, phi, path);
513 { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP },
514 { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP },
515 { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP },
516 { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP },
517 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP },
518 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP },
519 { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },
520 { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },
521 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP },
522 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP },
523 { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },
524 { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },
525 { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },
526 { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },
527 { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },
528 { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },
529 { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT },
530 { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT }
533 ompl::base::ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(
const ReedsSheppPathSegmentType* type,
534 double t,
double u,
double v,
double w,
double x)
537 length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
538 totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
544 return rho_ * reedsShepp(state1, state2).length();
549 bool firstTime =
true;
551 interpolate(from, to, t, firstTime, path, state);
555 bool &firstTime, ReedsSheppPath &path,
State *state)
const
562 copyState(state, to);
568 copyState(state, from);
571 path = reedsShepp(from, to);
574 interpolate(from, path, t, state);
579 StateType *s = allocState()->as<StateType>();
580 double seg = t * path.length(), phi, v;
583 s->setYaw(from->
as<StateType>()->getYaw());
584 for (
unsigned int i=0; i<5 && seg>0; ++i)
586 if (path.length_[i]<0)
588 v = std::max(-seg, path.length_[i]);
593 v = std::min(seg, path.length_[i]);
597 switch(path.type_[i])
600 s->setXY(s->getX() + sin(phi+v) - sin(phi), s->getY() - cos(phi+v) + cos(phi));
604 s->setXY(s->getX() - sin(phi-v) + sin(phi), s->getY() + cos(phi-v) - cos(phi));
608 s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
614 state->
as<StateType>()->setX(s->getX() * rho_ + from->
as<StateType>()->getX());
615 state->
as<StateType>()->setY(s->getY() * rho_ + from->
as<StateType>()->getY());
617 state->
as<StateType>()->setYaw(s->getYaw());
625 double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
626 double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
627 double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1);
628 double x = c*dx + s*dy, y = -s*dx + c*dy, phi = th2 - th1;
629 return ::reedsShepp(x/rho_, y/rho_, phi);
633 void ompl::base::ReedsSheppMotionValidator::defaultSettings()
637 throw Exception(
"No state space for motion validator");
644 bool result =
true, firstTime =
true;
646 int nd = stateSpace_->validSegmentCount(s1, s2);
651 State *test = si_->allocState();
653 for (
int j = 1 ; j < nd ; ++j)
655 stateSpace_->interpolate(s1, s2, (
double)j / (
double)nd, firstTime, path, test);
656 if (!si_->isValid(test))
658 lastValid.second = (double)(j - 1) / (double)nd;
660 stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
665 si_->freeState(test);
669 if (!si_->isValid(s2))
671 lastValid.second = (double)(nd - 1) / (double)nd;
673 stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
688 if (!si_->isValid(s2))
691 bool result =
true, firstTime =
true;
693 int nd = stateSpace_->validSegmentCount(s1, s2);
696 std::queue< std::pair<int, int> > pos;
699 pos.push(std::make_pair(1, nd - 1));
702 State *test = si_->allocState();
707 std::pair<int, int> x = pos.front();
709 int mid = (x.first + x.second) / 2;
710 stateSpace_->interpolate(s1, s2, (
double)mid / (
double)nd, firstTime, path, test);
712 if (!si_->isValid(test))
721 pos.push(std::make_pair(x.first, mid - 1));
723 pos.push(std::make_pair(mid + 1, x.second));
726 si_->freeState(test);
Complete description of a ReedsShepp path.
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...
static const ReedsSheppPathSegmentType reedsSheppPathType[18][5]
Reeds-Shepp path types.
The definition of a state in SO(2)
ReedsSheppPath reedsShepp(const State *state1, const State *state2) const
Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2.
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...
ReedsSheppPathSegmentType
The Reeds-Shepp path segment types.
Definition of an abstract state.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The exception type for ompl.
An SE(2) state space where distance is measured by the length of Reeds-Shepp curves.
const T * as() const
Cast this instance to a desired type.