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>
44 using namespace ompl::base;
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(
void)
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)
const T * as(void) const
Cast this instance to a desired type.
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.
The exception type for ompl.
An SE(2) state space where distance is measured by the length of Reeds-Shepp curves.