Loading...
Searching...
No Matches
ReedsSheppStateSpace.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2010, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Mark Moll */
36
37#include "ompl/base/spaces/ReedsSheppStateSpace.h"
38#include "ompl/base/SpaceInformation.h"
39#include "ompl/util/Exception.h"
40#include <queue>
41#include <boost/math/constants/constants.hpp>
42
43using namespace ompl::base;
44
45namespace
46{
47 // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
48
49 const double pi = boost::math::constants::pi<double>();
50 const double twopi = 2. * pi;
51#ifndef NDEBUG
52 const double RS_EPS = 1e-6;
53#endif
54 const double ZERO = 10 * std::numeric_limits<double>::epsilon();
55
56 inline double mod2pi(double x)
57 {
58 double v = fmod(x, twopi);
59 if (v < -pi)
60 v += twopi;
61 else if (v > pi)
62 v -= twopi;
63 return v;
64 }
65 inline void polar(double x, double y, double &r, double &theta)
66 {
67 r = sqrt(x * x + y * y);
68 theta = atan2(y, x);
69 }
70 inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
71 {
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);
76 }
77
78 // formula 8.1 in Reeds-Shepp paper
79 inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
80 {
81 polar(x - sin(phi), y - 1. + cos(phi), u, t);
82 if (t >= -ZERO)
83 {
84 v = mod2pi(phi - t);
85 if (v >= -ZERO)
86 {
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);
90 return true;
91 }
92 }
93 return false;
94 }
95 // formula 8.2
96 inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
97 {
98 double t1, u1;
99 polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
100 u1 = u1 * u1;
101 if (u1 >= 4.)
102 {
103 double theta;
104 u = sqrt(u1 - 4.);
105 theta = atan2(2., u);
106 t = mod2pi(t1 + theta);
107 v = mod2pi(t - phi);
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;
112 }
113 return false;
114 }
115 void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
116 {
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)))
119 {
121 Lmin = L;
122 }
123 if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
124 {
126 Lmin = L;
127 }
128 if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
129 {
131 Lmin = L;
132 }
133 if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
134 {
136 Lmin = L;
137 }
138 if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
139 {
141 Lmin = L;
142 }
143 if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
144 {
146 Lmin = L;
147 }
148 if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
149 {
151 Lmin = L;
152 }
153 if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
155 }
156 // formula 8.3 / 8.4 *** TYPO IN PAPER ***
157 inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
158 {
159 double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
160 polar(xi, eta, u1, theta);
161 if (u1 <= 4.)
162 {
163 u = -2. * asin(.25 * u1);
164 t = mod2pi(theta + .5 * u + pi);
165 v = mod2pi(phi - t + u);
166 assert(fabs(2 * (sin(t) - sin(t - u)) + sin(phi) - x) < RS_EPS);
167 assert(fabs(2 * (-cos(t) + cos(t - u)) - cos(phi) + 1 - y) < RS_EPS);
168 assert(fabs(mod2pi(t - u + v - phi)) < RS_EPS);
169 return t >= -ZERO && u <= ZERO;
170 }
171 return false;
172 }
173 void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
174 {
175 double t, u, v, Lmin = path.length(), L;
176 if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
177 {
179 Lmin = L;
180 }
181 if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
182 {
184 Lmin = L;
185 }
186 if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
187 {
189 Lmin = L;
190 }
191 if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
192 {
194 Lmin = L;
195 }
196
197 // backwards
198 double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
199 if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
200 {
202 Lmin = L;
203 }
204 if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
205 {
207 Lmin = L;
208 }
209 if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
210 {
212 Lmin = L;
213 }
214 if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
216 }
217 // formula 8.7
218 inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
219 {
220 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi * xi + eta * eta));
221 if (rho <= 1.)
222 {
223 u = acos(rho);
224 tauOmega(u, -u, xi, eta, phi, t, v);
225 assert(fabs(2 * (sin(t) - sin(t - u) + sin(t - 2 * u)) - sin(phi) - x) < RS_EPS);
226 assert(fabs(2 * (-cos(t) + cos(t - u) - cos(t - 2 * u)) + cos(phi) + 1 - y) < RS_EPS);
227 assert(fabs(mod2pi(t - 2 * u - v - phi)) < RS_EPS);
228 return t >= -ZERO && v <= ZERO;
229 }
230 return false;
231 }
232 // formula 8.8
233 inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
234 {
235 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi * xi - eta * eta) / 16.;
236 if (rho >= 0 && rho <= 1)
237 {
238 u = -acos(rho);
239 if (u >= -.5 * pi)
240 {
241 tauOmega(u, u, xi, eta, phi, t, v);
242 assert(fabs(4 * sin(t) - 2 * sin(t - u) - sin(phi) - x) < RS_EPS);
243 assert(fabs(-4 * cos(t) + 2 * cos(t - u) + cos(phi) + 1 - y) < RS_EPS);
244 assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
245 return t >= -ZERO && v >= -ZERO;
246 }
247 }
248 return false;
249 }
250 void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
251 {
252 double t, u, v, Lmin = path.length(), L;
253 if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
254 {
256 Lmin = L;
257 }
258 if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip
259 {
261 Lmin = L;
262 }
263 if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // reflect
264 {
266 Lmin = L;
267 }
268 if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip + reflect
269 {
271 Lmin = L;
272 }
273
274 if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
275 {
277 Lmin = L;
278 }
279 if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip
280 {
282 Lmin = L;
283 }
284 if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // reflect
285 {
287 Lmin = L;
288 }
289 if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip + reflect
291 }
292 // formula 8.9
293 inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
294 {
295 double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
296 polar(xi, eta, rho, theta);
297 if (rho >= 2.)
298 {
299 double r = sqrt(rho * rho - 4.);
300 u = 2. - r;
301 t = mod2pi(theta + atan2(r, -2.));
302 v = mod2pi(phi - .5 * pi - t);
303 assert(fabs(2 * (sin(t) - cos(t)) - u * sin(t) + sin(phi) - x) < RS_EPS);
304 assert(fabs(-2 * (sin(t) + cos(t)) + u * cos(t) - cos(phi) + 1 - y) < RS_EPS);
305 assert(fabs(mod2pi(t + pi / 2 + v - phi)) < RS_EPS);
306 return t >= -ZERO && u <= ZERO && v <= ZERO;
307 }
308 return false;
309 }
310 // formula 8.10
311 inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
312 {
313 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
314 polar(-eta, xi, rho, theta);
315 if (rho >= 2.)
316 {
317 t = theta;
318 u = 2. - rho;
319 v = mod2pi(t + .5 * pi - phi);
320 assert(fabs(2 * sin(t) - cos(t - v) - u * sin(t) - x) < RS_EPS);
321 assert(fabs(-2 * cos(t) - sin(t - v) + u * cos(t) + 1 - y) < RS_EPS);
322 assert(fabs(mod2pi(t + pi / 2 - v - phi)) < RS_EPS);
323 return t >= -ZERO && u <= ZERO && v <= ZERO;
324 }
325 return false;
326 }
327 void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
328 {
329 double t, u, v, Lmin = path.length() - .5 * pi, L;
330 if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
331 {
333 Lmin = L;
334 }
335 if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
336 {
337 path =
339 Lmin = L;
340 }
341 if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
342 {
344 Lmin = L;
345 }
346 if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
347 {
348 path =
350 Lmin = L;
351 }
352
353 if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
354 {
356 Lmin = L;
357 }
358 if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
359 {
360 path =
362 Lmin = L;
363 }
364 if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
365 {
367 Lmin = L;
368 }
369 if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
370 {
371 path =
373 Lmin = L;
374 }
375
376 // backwards
377 double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
378 if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
379 {
381 Lmin = L;
382 }
383 if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
384 {
385 path =
387 Lmin = L;
388 }
389 if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
390 {
392 Lmin = L;
393 }
394 if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
395 {
396 path =
398 Lmin = L;
399 }
400
401 if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
402 {
403 path =
405 Lmin = L;
406 }
407 if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
408 {
409 path =
411 Lmin = L;
412 }
413 if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
414 {
415 path =
417 Lmin = L;
418 }
419 if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
420 path =
422 }
423 // formula 8.11 *** TYPO IN PAPER ***
424 inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
425 {
426 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
427 polar(xi, eta, rho, theta);
428 if (rho >= 2.)
429 {
430 u = 4. - sqrt(rho * rho - 4.);
431 if (u <= ZERO)
432 {
433 t = mod2pi(atan2((4 - u) * xi - 2 * eta, -2 * xi + (u - 4) * eta));
434 v = mod2pi(t - phi);
435 assert(fabs(4 * sin(t) - 2 * cos(t) - u * sin(t) - sin(phi) - x) < RS_EPS);
436 assert(fabs(-4 * cos(t) - 2 * sin(t) + u * cos(t) + cos(phi) + 1 - y) < RS_EPS);
437 assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
438 return t >= -ZERO && v >= -ZERO;
439 }
440 }
441 return false;
442 }
443 void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
444 {
445 double t, u, v, Lmin = path.length() - pi, L;
446 if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
447 {
449 -.5 * pi, v);
450 Lmin = L;
451 }
452 if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
453 {
455 .5 * pi, -v);
456 Lmin = L;
457 }
458 if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
459 {
461 -.5 * pi, v);
462 Lmin = L;
463 }
464 if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
466 .5 * pi, -v);
467 }
468
469 ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
470 {
472 CSC(x, y, phi, path);
473 CCC(x, y, phi, path);
474 CCCC(x, y, phi, path);
475 CCSC(x, y, phi, path);
476 CCSCC(x, y, phi, path);
477 return path;
478 }
479}
480
483 {RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 0
484 {RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP}, // 1
485 {RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 2
486 {RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP}, // 3
487 {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP}, // 4
488 {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP}, // 5
489 {RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP}, // 6
490 {RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 7
491 {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP}, // 8
492 {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP}, // 9
493 {RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP}, // 10
494 {RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 11
495 {RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP}, // 12
496 {RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 13
497 {RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 14
498 {RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP}, // 15
499 {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT}, // 16
500 {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT} // 17
501};
502
503ompl::base::ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType *type, double t,
504 double u, double v, double w, double x)
505 : type_(type)
506{
507 length_[0] = t;
508 length_[1] = u;
509 length_[2] = v;
510 length_[3] = w;
511 length_[4] = x;
512 totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
513}
514
515double ompl::base::ReedsSheppStateSpace::distance(const State *state1, const State *state2) const
516{
517 return rho_ * reedsShepp(state1, state2).length();
518}
519
520void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t,
521 State *state) const
522{
523 bool firstTime = true;
524 ReedsSheppPath path;
525 interpolate(from, to, t, firstTime, path, state);
526}
527
528void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t, bool &firstTime,
529 ReedsSheppPath &path, State *state) const
530{
531 if (firstTime)
532 {
533 if (t >= 1.)
534 {
535 if (to != state)
536 copyState(state, to);
537 return;
538 }
539 if (t <= 0.)
540 {
541 if (from != state)
542 copyState(state, from);
543 return;
544 }
545 path = reedsShepp(from, to);
546 firstTime = false;
547 }
548 interpolate(from, path, t, state);
549}
550
551void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const ReedsSheppPath &path, double t,
552 State *state) const
553{
554 auto *s = allocState()->as<StateType>();
555 double seg = t * path.length(), phi, v;
556
557 s->setXY(0., 0.);
558 s->setYaw(from->as<StateType>()->getYaw());
559 for (unsigned int i = 0; i < 5 && seg > 0; ++i)
560 {
561 if (path.length_[i] < 0)
562 {
563 v = std::max(-seg, path.length_[i]);
564 seg += v;
565 }
566 else
567 {
568 v = std::min(seg, path.length_[i]);
569 seg -= v;
570 }
571 phi = s->getYaw();
572 switch (path.type_[i])
573 {
574 case RS_LEFT:
575 s->setXY(s->getX() + sin(phi + v) - sin(phi), s->getY() - cos(phi + v) + cos(phi));
576 s->setYaw(phi + v);
577 break;
578 case RS_RIGHT:
579 s->setXY(s->getX() - sin(phi - v) + sin(phi), s->getY() + cos(phi - v) - cos(phi));
580 s->setYaw(phi - v);
581 break;
582 case RS_STRAIGHT:
583 s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
584 break;
585 case RS_NOP:
586 break;
587 }
588 }
589 state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
590 state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
591 getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
592 state->as<StateType>()->setYaw(s->getYaw());
593 freeState(s);
594}
595
597 const State *state2) const
598{
599 const auto *s1 = static_cast<const StateType *>(state1);
600 const auto *s2 = static_cast<const StateType *>(state2);
601 double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
602 double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
603 double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1);
604 double x = c * dx + s * dy, y = -s * dx + c * dy, phi = th2 - th1;
605 return ::reedsShepp(x / rho_, y / rho_, phi);
606}
607
608void ompl::base::ReedsSheppMotionValidator::defaultSettings()
609{
610 stateSpace_ = dynamic_cast<ReedsSheppStateSpace *>(si_->getStateSpace().get());
611 if (stateSpace_ == nullptr)
612 throw Exception("No state space for motion validator");
613}
614
616 std::pair<State *, double> &lastValid) const
617{
618 /* assume motion starts in a valid configuration so s1 is valid */
619
620 bool result = true, firstTime = true;
622 int nd = stateSpace_->validSegmentCount(s1, s2);
623
624 if (nd > 1)
625 {
626 /* temporary storage for the checked state */
627 State *test = si_->allocState();
628
629 for (int j = 1; j < nd; ++j)
630 {
631 stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
632 if (!si_->isValid(test))
633 {
634 lastValid.second = (double)(j - 1) / (double)nd;
635 if (lastValid.first != nullptr)
636 stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
637 result = false;
638 break;
639 }
640 }
641 si_->freeState(test);
642 }
643
644 if (result)
645 if (!si_->isValid(s2))
646 {
647 lastValid.second = (double)(nd - 1) / (double)nd;
648 if (lastValid.first != nullptr)
649 stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
650 result = false;
651 }
652
653 if (result)
654 valid_++;
655 else
656 invalid_++;
657
658 return result;
659}
660
662{
663 /* assume motion starts in a valid configuration so s1 is valid */
664 if (!si_->isValid(s2))
665 return false;
666
667 bool result = true, firstTime = true;
669 int nd = stateSpace_->validSegmentCount(s1, s2);
670
671 /* initialize the queue of test positions */
672 std::queue<std::pair<int, int>> pos;
673 if (nd >= 2)
674 {
675 pos.emplace(1, nd - 1);
676
677 /* temporary storage for the checked state */
678 State *test = si_->allocState();
679
680 /* repeatedly subdivide the path segment in the middle (and check the middle) */
681 while (!pos.empty())
682 {
683 std::pair<int, int> x = pos.front();
684
685 int mid = (x.first + x.second) / 2;
686 stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);
687
688 if (!si_->isValid(test))
689 {
690 result = false;
691 break;
692 }
693
694 pos.pop();
695
696 if (x.first < mid)
697 pos.emplace(x.first, mid - 1);
698 if (x.second > mid)
699 pos.emplace(mid + 1, x.second);
700 }
701
702 si_->freeState(test);
703 }
704
705 if (result)
706 valid_++;
707 else
708 invalid_++;
709
710 return result;
711}
The exception type for ompl.
Definition Exception.h:47
SpaceInformation * si_
The instance of space information this state validity checker operates on.
unsigned int valid_
Number of valid segments.
unsigned int invalid_
Number of invalid segments.
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.
Complete description of a ReedsShepp path.
An SE(2) state space where distance is measured by the length of Reeds-Shepp curves.
ReedsSheppPathSegmentType
The Reeds-Shepp path segment types.
ReedsSheppPath reedsShepp(const State *state1, const State *state2) const
Return a shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2.
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....
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...
static const ReedsSheppPathSegmentType reedsSheppPathType[18][5]
Reeds-Shepp path types.
A state in SE(2): (x, y, yaw)
double getX() const
Get the X component of the state.
The definition of a state in SO(2)
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
This namespace contains sampling based planning routines shared by both planning under geometric cons...