Loading...
Searching...
No Matches
DubinsStateSpace.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/DubinsStateSpace.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 constexpr double twopi = 2. * boost::math::constants::pi<double>();
48 constexpr double onepi = boost::math::constants::pi<double>();
49 constexpr double halfpi = boost::math::constants::half_pi<double>();
50 const double DUBINS_EPS = 1e-6;
51 const double DUBINS_ZERO = -1e-7;
52
53 enum DubinsClass
54 {
55 A11 = 0,
56 A12 = 1,
57 A13 = 2,
58 A14 = 3,
59 A21 = 4,
60 A22 = 5,
61 A23 = 6,
62 A24 = 7,
63 A31 = 8,
64 A32 = 9,
65 A33 = 10,
66 A34 = 11,
67 A41 = 12,
68 A42 = 13,
69 A43 = 14,
70 A44 = 15
71 };
72
73 inline double mod2pi(double x)
74 {
75 if (x < 0 && x > DUBINS_ZERO)
76 return 0;
77 double xm = x - twopi * floor(x / twopi);
78 if (twopi - xm < .5 * DUBINS_EPS)
79 xm = 0.;
80 return xm;
81 }
82
83 inline double t_lsr(double d, double alpha, double beta)
84 {
85 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
86 const double tmp = -2.0 + d * d + 2.0 * (ca * cb + sa * sb + d * (sa + sb));
87 const double p = sqrtf(std::max(tmp, 0.0));
88 const double theta = atan2f(-ca - cb, d + sa + sb) - atan2f(-2.0, p);
89 return mod2pi(-alpha + theta); // t
90 }
91
92 inline double p_lsr(double d, double alpha, double beta)
93 {
94 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
95 const double tmp = -2.0 + d * d + 2.0 * (ca * cb + sa * sb + d * (sa + sb));
96 return sqrtf(std::max(tmp, 0.0)); // p
97 }
98
99 inline double q_lsr(double d, double alpha, double beta)
100 {
101 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
102 const double tmp = -2.0 + d * d + 2.0 * (ca * cb + sa * sb + d * (sa + sb));
103 const double p = sqrtf(std::max(tmp, 0.0));
104 const double theta = atan2f(-ca - cb, d + sa + sb) - atan2f(-2.0, p);
105 return mod2pi(-beta + theta); // q
106 }
107
108 inline double t_rsl(double d, double alpha, double beta)
109 {
110 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
111 const double tmp = d * d - 2.0 + 2.0 * (ca * cb + sa * sb - d * (sa + sb));
112 const double p = sqrtf(std::max(tmp, 0.0));
113 const double theta = atan2f(ca + cb, d - sa - sb) - atan2f(2.0, p);
114 return mod2pi(alpha - theta); // t
115 }
116
117 inline double p_rsl(double d, double alpha, double beta)
118 {
119 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
120 const double tmp = d * d - 2.0 + 2.0 * (ca * cb + sa * sb - d * (sa + sb));
121 return sqrtf(std::max(tmp, 0.0)); // p
122 }
123
124 inline double q_rsl(double d, double alpha, double beta)
125 {
126 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
127 const double tmp = d * d - 2.0 + 2.0 * (ca * cb + sa * sb - d * (sa + sb));
128 const double p = sqrtf(std::max(tmp, 0.));
129 const double theta = atan2f(ca + cb, d - sa - sb) - atan2f(2.0, p);
130 return mod2pi(beta - theta); // q
131 }
132
133 inline double t_rsr(double d, double alpha, double beta)
134 {
135 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
136 const double theta = atan2f(ca - cb, d - sa + sb);
137 return mod2pi(alpha - theta); // t
138 }
139
140 inline double p_rsr(double d, double alpha, double beta)
141 {
142 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
143 const double tmp = 2.0 + d * d - 2.0 * (ca * cb + sa * sb - d * (sb - sa));
144 return sqrtf(std::max(tmp, 0.0)); // p
145 }
146
147 inline double q_rsr(double d, double alpha, double beta)
148 {
149 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
150 const double theta = atan2f(ca - cb, d - sa + sb);
151 return mod2pi(-beta + theta); // q
152 }
153
154 inline double t_lsl(double d, double alpha, double beta)
155 {
156 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
157 const double theta = atan2f(cb - ca, d + sa - sb);
158 return mod2pi(-alpha + theta); // t
159 }
160
161 inline double p_lsl(double d, double alpha, double beta)
162 {
163 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
164 const double tmp = 2.0 + d * d - 2.0 * (ca * cb + sa * sb - d * (sa - sb));
165 return sqrtf(std::max(tmp, 0.0)); // p
166 }
167
168 inline double q_lsl(double d, double alpha, double beta)
169 {
170 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
171 const double theta = atan2f(cb - ca, d + sa - sb);
172 return mod2pi(beta - theta); // q
173 }
174
175 inline double s_12(double d, double alpha, double beta)
176 {
177 return p_rsr(d, alpha, beta) - p_rsl(d, alpha, beta) - 2.0 * (q_rsl(d, alpha, beta) - onepi);
178 }
179
180 inline double s_13(double d, double alpha, double beta)
181 { // t_rsr - pi
182 return t_rsr(d, alpha, beta) - onepi;
183 }
184
185 inline double s_14_1(double d, double alpha, double beta)
186 {
187 return t_rsr(d, alpha, beta) - onepi;
188 }
189
190 inline double s_21(double d, double alpha, double beta)
191 {
192 return p_lsl(d, alpha, beta) - p_rsl(d, alpha, beta) - 2.0 * (t_rsl(d, alpha, beta) - onepi);
193 }
194
195 inline double s_22_1(double d, double alpha, double beta)
196 {
197 return p_lsl(d, alpha, beta) - p_rsl(d, alpha, beta) - 2.0 * (t_rsl(d, alpha, beta) - onepi);
198 }
199
200 inline double s_22_2(double d, double alpha, double beta)
201 {
202 return p_rsr(d, alpha, beta) - p_rsl(d, alpha, beta) - 2.0 * (q_rsl(d, alpha, beta) - onepi);
203 }
204
205 inline double s_24(double d, double alpha, double beta)
206 {
207 return q_rsr(d, alpha, beta) - onepi;
208 }
209
210 inline double s_31(double d, double alpha, double beta)
211 {
212 return q_lsl(d, alpha, beta) - onepi;
213 }
214
215 inline double s_33_1(double d, double alpha, double beta)
216 {
217 return p_rsr(d, alpha, beta) - p_lsr(d, alpha, beta) - 2.0 * (t_lsr(d, alpha, beta) - onepi);
218 }
219
220 inline double s_33_2(double d, double alpha, double beta)
221 {
222 return p_lsl(d, alpha, beta) - p_lsr(d, alpha, beta) - 2.0 * (q_lsr(d, alpha, beta) - onepi);
223 }
224
225 inline double s_34(double d, double alpha, double beta)
226 {
227 return p_rsr(d, alpha, beta) - p_lsr(d, alpha, beta) - 2.0 * (t_lsr(d, alpha, beta) - onepi);
228 }
229
230 inline double s_41_1(double d, double alpha, double beta)
231 {
232 return t_lsl(d, alpha, beta) - onepi;
233 }
234
235 inline double s_41_2(double d, double alpha, double beta)
236 {
237 return q_lsl(d, alpha, beta) - onepi;
238 }
239
240 inline double s_42(double d, double alpha, double beta)
241 {
242 return t_lsl(d, alpha, beta) - onepi;
243 }
244
245 inline double s_43(double d, double alpha, double beta)
246 {
247 return p_lsl(d, alpha, beta) - p_lsr(d, alpha, beta) - 2.0 * (q_lsr(d, alpha, beta) - onepi);
248 }
249
250 DubinsStateSpace::PathType dubinsLSL(double d, double alpha, double beta)
251 {
252 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
253 double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sa - sb));
254 if (tmp >= DUBINS_ZERO)
255 {
256 double theta = atan2(cb - ca, d + sa - sb);
257 double t = mod2pi(-alpha + theta);
258 double p = sqrt(std::max(tmp, 0.));
259 double q = mod2pi(beta - theta);
260 assert(fabs(p * cos(alpha + t) - sa + sb - d) < (1 + p) * DUBINS_EPS);
261 assert(fabs(p * sin(alpha + t) + ca - cb) < (1 + p) * DUBINS_EPS);
262 assert(mod2pi(alpha + t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
263 return DubinsStateSpace::PathType(DubinsStateSpace::dubinsPathType()[0], t, p, q);
264 }
265 return {};
266 }
267
268 DubinsStateSpace::PathType dubinsRSR(double d, double alpha, double beta)
269 {
270 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
271 double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sb - sa));
272 if (tmp >= DUBINS_ZERO)
273 {
274 double theta = atan2(ca - cb, d - sa + sb);
275 double t = mod2pi(alpha - theta);
276 double p = sqrt(std::max(tmp, 0.));
277 double q = mod2pi(-beta + theta);
278 assert(fabs(p * cos(alpha - t) + sa - sb - d) < (1 + p) * DUBINS_EPS);
279 assert(fabs(p * sin(alpha - t) - ca + cb) < (1 + p) * DUBINS_EPS);
280 assert(mod2pi(alpha - t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
281 return DubinsStateSpace::PathType(DubinsStateSpace::dubinsPathType()[1], t, p, q);
282 }
283 return {};
284 }
285
286 DubinsStateSpace::PathType dubinsRSL(double d, double alpha, double beta)
287 {
288 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
289 double tmp = d * d - 2. + 2. * (ca * cb + sa * sb - d * (sa + sb));
290 if (tmp >= DUBINS_ZERO)
291 {
292 double p = sqrt(std::max(tmp, 0.));
293 double theta = atan2(ca + cb, d - sa - sb) - atan2(2., p);
294 double t = mod2pi(alpha - theta);
295 double q = mod2pi(beta - theta);
296 assert(fabs(p * cos(alpha - t) - 2. * sin(alpha - t) + sa + sb - d) < 2 * DUBINS_EPS);
297 assert(fabs(p * sin(alpha - t) + 2. * cos(alpha - t) - ca - cb) < 2 * DUBINS_EPS);
298 assert(mod2pi(alpha - t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
299 return DubinsStateSpace::PathType(DubinsStateSpace::dubinsPathType()[2], t, p, q);
300 }
301 return {};
302 }
303
304 DubinsStateSpace::PathType dubinsLSR(double d, double alpha, double beta)
305 {
306 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
307 double tmp = -2. + d * d + 2. * (ca * cb + sa * sb + d * (sa + sb));
308 if (tmp >= DUBINS_ZERO)
309 {
310 double p = sqrt(std::max(tmp, 0.));
311 double theta = atan2(-ca - cb, d + sa + sb) - atan2(-2., p);
312 double t = mod2pi(-alpha + theta);
313 double q = mod2pi(-beta + theta);
314 assert(fabs(p * cos(alpha + t) + 2. * sin(alpha + t) - sa - sb - d) < 2 * DUBINS_EPS);
315 assert(fabs(p * sin(alpha + t) - 2. * cos(alpha + t) + ca + cb) < 2 * DUBINS_EPS);
316 assert(mod2pi(alpha + t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
317 return DubinsStateSpace::PathType(DubinsStateSpace::dubinsPathType()[3], t, p, q);
318 }
319 return {};
320 }
321
322 DubinsStateSpace::PathType dubinsRLR(double d, double alpha, double beta)
323 {
324 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
325 double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb + d * (sa - sb)));
326 if (fabs(tmp) < 1.)
327 {
328 double p = twopi - acos(tmp);
329 double theta = atan2(ca - cb, d - sa + sb);
330 double t = mod2pi(alpha - theta + .5 * p);
331 double q = mod2pi(alpha - beta - t + p);
332 assert(fabs(2. * sin(alpha - t + p) - 2. * sin(alpha - t) - d + sa - sb) < 2 * DUBINS_EPS);
333 assert(fabs(-2. * cos(alpha - t + p) + 2. * cos(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
334 assert(mod2pi(alpha - t + p - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
335 return DubinsStateSpace::PathType(DubinsStateSpace::dubinsPathType()[4], t, p, q);
336 }
337 return {};
338 }
339
340 DubinsStateSpace::PathType dubinsLRL(double d, double alpha, double beta)
341 {
342 double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
343 double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb - d * (sa - sb)));
344 if (fabs(tmp) < 1.)
345 {
346 double p = twopi - acos(tmp);
347 double theta = atan2(-ca + cb, d + sa - sb);
348 double t = mod2pi(-alpha + theta + .5 * p);
349 double q = mod2pi(beta - alpha - t + p);
350 assert(fabs(-2. * sin(alpha + t - p) + 2. * sin(alpha + t) - d - sa + sb) < 2 * DUBINS_EPS);
351 assert(fabs(2. * cos(alpha + t - p) - 2. * cos(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
352 assert(mod2pi(alpha + t - p + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
353 return DubinsStateSpace::PathType(DubinsStateSpace::dubinsPathType()[5], t, p, q);
354 }
355 return {};
356 }
357
358 bool isLongPath(double d, double alpha, double beta)
359 {
360 return (std::abs(std::sin(alpha)) + std::abs(std::sin(beta)) +
361 std::sqrt(4 - std::pow(std::cos(alpha) + std::cos(beta), 2)) - d) < 0;
362 }
363
364 DubinsStateSpace::PathType dubinsExhaustive(const double d, const double alpha, const double beta)
365 {
366 if (d < DUBINS_EPS && fabs(alpha - beta) < DUBINS_EPS)
367 return {DubinsStateSpace::dubinsPathType()[0], 0, d, 0};
368
369 DubinsStateSpace::PathType path(dubinsLSL(d, alpha, beta)), tmp(dubinsRSR(d, alpha, beta));
370 double len, minLength = path.length();
371
372 if ((len = tmp.length()) < minLength)
373 {
374 minLength = len;
375 path = tmp;
376 }
377 tmp = dubinsRSL(d, alpha, beta);
378 if ((len = tmp.length()) < minLength)
379 {
380 minLength = len;
381 path = tmp;
382 }
383 tmp = dubinsLSR(d, alpha, beta);
384 if ((len = tmp.length()) < minLength)
385 {
386 minLength = len;
387 path = tmp;
388 }
389 tmp = dubinsRLR(d, alpha, beta);
390 if ((len = tmp.length()) < minLength)
391 {
392 minLength = len;
393 path = tmp;
394 }
395 tmp = dubinsLRL(d, alpha, beta);
396 if ((len = tmp.length()) < minLength)
397 path = tmp;
398 return path;
399 }
400
401 DubinsClass getDubinsClass(const double alpha, const double beta)
402 {
403 int row(0), column(0);
404 if (0 <= alpha && alpha <= halfpi)
405 {
406 row = 1;
407 }
408 else if (halfpi < alpha && alpha <= onepi)
409 {
410 row = 2;
411 }
412 else if (onepi < alpha && alpha <= 3 * halfpi)
413 {
414 row = 3;
415 }
416 else if (3 * halfpi < alpha && alpha <= twopi)
417 {
418 row = 4;
419 }
420
421 if (0 <= beta && beta <= halfpi)
422 {
423 column = 1;
424 }
425 else if (halfpi < beta && beta <= onepi)
426 {
427 column = 2;
428 }
429 else if (onepi < beta && beta <= 3 * halfpi)
430 {
431 column = 3;
432 }
433 else if (3 * halfpi < beta && beta <= 2.0 * onepi)
434 {
435 column = 4;
436 }
437
438 assert(row >= 1 && row <= 4 &&
439 "alpha is not in the range of [0,2pi] in classifyPath(double alpha, double beta).");
440 assert(column >= 1 && column <= 4 &&
441 "beta is not in the range of [0,2pi] in classifyPath(double alpha, double beta).");
442 assert((column - 1) + 4 * (row - 1) >= 0 && (column - 1) + 4 * (row - 1) <= 15 &&
443 "class is not in range [0,15].");
444 return (DubinsClass)((column - 1) + 4 * (row - 1));
445 }
446
447 DubinsStateSpace::PathType dubinsClassification(const double d, const double alpha, const double beta)
448 {
449 if (d < DUBINS_EPS && fabs(alpha - beta) < DUBINS_EPS)
450 return {DubinsStateSpace::dubinsPathType()[0], 0, d, 0};
451 // Dubins set classification scheme
452 // Shkel, Andrei M., and Vladimir Lumelsky. "Classification of the Dubins set."
453 // Robotics and Autonomous Systems 34.4 (2001): 179-202.
454 // Lim, Jaeyoung, et al. "Circling Back: Dubins set Classification Revisited."
455 // Workshop on Energy Efficient Aerial Robotic Systems, International Conference on Robotics and Automation
456 // 2023.
457 DubinsStateSpace::PathType path;
458 auto dubins_class = getDubinsClass(alpha, beta);
459 switch (dubins_class)
460 {
461 case DubinsClass::A11:
462 {
463 path = dubinsRSL(d, alpha, beta);
464 break;
465 }
466 case DubinsClass::A12:
467 {
468 if (s_13(d, alpha, beta) < 0.0)
469 {
470 path = (s_12(d, alpha, beta) < 0.0) ? dubinsRSR(d, alpha, beta) : dubinsRSL(d, alpha, beta);
471 }
472 else
473 {
474 path = dubinsLSR(d, alpha, beta);
475 DubinsStateSpace::PathType tmp = dubinsRSL(d, alpha, beta);
476 if (path.length() > tmp.length())
477 {
478 path = tmp;
479 }
480 }
481 break;
482 }
483 case DubinsClass::A13:
484 {
485 if (s_13(d, alpha, beta) < 0.0)
486 {
487 path = dubinsRSR(d, alpha, beta);
488 }
489 else
490 {
491 path = dubinsLSR(d, alpha, beta);
492 }
493 break;
494 }
495 case DubinsClass::A14:
496 {
497 if (s_14_1(d, alpha, beta) > 0.0)
498 {
499 path = dubinsLSR(d, alpha, beta);
500 }
501 else if (s_24(d, alpha, beta) > 0.0)
502 {
503 path = dubinsRSL(d, alpha, beta);
504 }
505 else
506 {
507 path = dubinsRSR(d, alpha, beta);
508 }
509 break;
510 }
511 case DubinsClass::A21:
512 {
513 if (s_31(d, alpha, beta) < 0.0)
514 {
515 if (s_21(d, alpha, beta) < 0.0)
516 {
517 path = dubinsLSL(d, alpha, beta);
518 }
519 else
520 {
521 path = dubinsRSL(d, alpha, beta);
522 }
523 }
524 else
525 {
526 path = dubinsLSR(d, alpha, beta);
527 DubinsStateSpace::PathType tmp = dubinsRSL(d, alpha, beta);
528 if (path.length() > tmp.length())
529 {
530 path = tmp;
531 }
532 }
533 break;
534 }
535 case DubinsClass::A22:
536 {
537 if (alpha > beta)
538 {
539 path = (s_22_1(d, alpha, beta) < 0.0) ? dubinsLSL(d, alpha, beta) : dubinsRSL(d, alpha, beta);
540 }
541 else
542 {
543 path = (s_22_2(d, alpha, beta) < 0.0) ? dubinsRSR(d, alpha, beta) : dubinsRSL(d, alpha, beta);
544 }
545 break;
546 }
547 case DubinsClass::A23:
548 {
549 path = dubinsRSR(d, alpha, beta);
550 break;
551 }
552 case DubinsClass::A24:
553 {
554 if (s_24(d, alpha, beta) < 0.0)
555 {
556 path = dubinsRSR(d, alpha, beta);
557 }
558 else
559 {
560 path = dubinsRSL(d, alpha, beta);
561 }
562 break;
563 }
564 case DubinsClass::A31:
565 {
566 if (s_31(d, alpha, beta) < 0.0)
567 {
568 path = dubinsLSL(d, alpha, beta);
569 }
570 else
571 {
572 path = dubinsLSR(d, alpha, beta);
573 }
574 break;
575 }
576 case DubinsClass::A32:
577 {
578 path = dubinsLSL(d, alpha, beta);
579 break;
580 }
581 case DubinsClass::A33:
582 {
583 if (alpha < beta)
584 {
585 if (s_33_1(d, alpha, beta) < 0.0)
586 {
587 path = dubinsRSR(d, alpha, beta);
588 }
589 else
590 {
591 path = dubinsLSR(d, alpha, beta);
592 }
593 }
594 else
595 {
596 if (s_33_2(d, alpha, beta) < 0.0)
597 {
598 path = dubinsLSL(d, alpha, beta);
599 }
600 else
601 {
602 path = dubinsLSR(d, alpha, beta);
603 }
604 }
605 break;
606 }
607 case DubinsClass::A34:
608 {
609 if (s_24(d, alpha, beta) < 0.0)
610 {
611 if (s_34(d, alpha, beta) < 0.0)
612 {
613 path = dubinsRSR(d, alpha, beta);
614 }
615 else
616 {
617 path = dubinsLSR(d, alpha, beta);
618 }
619 }
620 else
621 {
622 path = dubinsLSR(d, alpha, beta);
623 DubinsStateSpace::PathType tmp = dubinsRSL(d, alpha, beta);
624 if (path.length() > tmp.length())
625 {
626 path = tmp;
627 }
628 }
629 break;
630 }
631 case DubinsClass::A41:
632 {
633 if (s_41_1(d, alpha, beta) > 0.0)
634 {
635 path = dubinsRSL(d, alpha, beta);
636 }
637 else if (s_41_2(d, alpha, beta) > 0.0)
638 {
639 path = dubinsLSR(d, alpha, beta);
640 }
641 else
642 {
643 path = dubinsLSL(d, alpha, beta);
644 }
645 break;
646 }
647 case DubinsClass::A42:
648 {
649 if (s_42(d, alpha, beta) < 0.0)
650 {
651 path = dubinsLSL(d, alpha, beta);
652 }
653 else
654 {
655 path = dubinsRSL(d, alpha, beta);
656 }
657 break;
658 }
659 case DubinsClass::A43:
660 {
661 if (s_42(d, alpha, beta) < 0.0)
662 {
663 if (s_43(d, alpha, beta) < 0.0)
664 {
665 path = dubinsLSL(d, alpha, beta);
666 }
667 else
668 {
669 path = dubinsLSR(d, alpha, beta);
670 }
671 }
672 else
673 {
674 path = dubinsLSR(d, alpha, beta);
675 DubinsStateSpace::PathType tmp = dubinsRSL(d, alpha, beta);
676 if (path.length() > tmp.length())
677 {
678 path = tmp;
679 }
680 }
681 break;
682 }
683 case DubinsClass::A44:
684 {
685 path = dubinsLSR(d, alpha, beta);
686 break;
687 }
688 }
689 return path;
690 }
691
692} // namespace
693
694namespace ompl::base
695{
696 std::ostream &operator<<(std::ostream &os, const DubinsStateSpace::PathType &path)
697 {
698 os << "DubinsPath[ type=";
699 for (unsigned i = 0; i < 3; ++i)
700 if (path.type_->at(i) == DubinsStateSpace::DUBINS_LEFT)
701 os << "L";
702 else if (path.type_->at(i) == DubinsStateSpace::DUBINS_STRAIGHT)
703 os << "S";
704 else
705 os << "R";
706 os << ", length=" << path.length_[0] << '+' << path.length_[1] << '+' << path.length_[2] << '=' << path.length()
707 << ", reverse=" << path.reverse_ << " ]";
708 return os;
709 }
710} // namespace ompl::base
711
712DubinsStateSpace::PathType getPath(double d, double alpha, double beta)
713{
714 if (d < DUBINS_EPS && fabs(alpha - beta) < DUBINS_EPS)
715 return {DubinsStateSpace::dubinsPathType()[0], 0, d, 0};
716 alpha = mod2pi(alpha);
717 beta = mod2pi(beta);
718 return isLongPath(d, alpha, beta) ? ::dubinsClassification(d, alpha, beta) : ::dubinsExhaustive(d, alpha, beta);
719}
720
721const std::vector<std::vector<DubinsStateSpace::DubinsPathSegmentType>> &DubinsStateSpace::dubinsPathType()
722{
723 static std::vector<std::vector<DubinsStateSpace::DubinsPathSegmentType>> *pathType =
724 new std::vector<std::vector<DubinsStateSpace::DubinsPathSegmentType>>(
725 {{{{DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_LEFT}},
726 {{DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_RIGHT}},
727 {{DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_LEFT}},
728 {{DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_RIGHT}},
729 {{DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT}},
730 {{DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT}}}});
731 return *pathType;
732}
733
734double DubinsStateSpace::distance(const State *state1, const State *state2) const
735{
736 return isSymmetric_ ? symmetricDistance(state1, state2, rho_) : distance(state1, state2, rho_);
737}
738double DubinsStateSpace::distance(const State *state1, const State *state2, double radius)
739{
740 return radius * getPath(state1, state2, radius).length();
741}
742double DubinsStateSpace::symmetricDistance(const State *state1, const State *state2, double radius)
743{
744 return radius * std::min(getPath(state1, state2, radius).length(), getPath(state2, state1, radius).length());
745}
746
747void DubinsStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
748{
749 bool firstTime = true;
750 PathType path;
751 interpolate(from, to, t, firstTime, path, state);
752}
753
754void DubinsStateSpace::interpolate(const State *from, const State *to, const double t, bool &firstTime, PathType &path,
755 State *state) const
756{
757 if (firstTime)
758 {
759 if (t >= 1.)
760 {
761 if (to != state)
762 copyState(state, to);
763 return;
764 }
765 if (t <= 0.)
766 {
767 if (from != state)
768 copyState(state, from);
769 return;
770 }
771
772 path = getPath(from, to);
773 if (isSymmetric_)
774 {
775 PathType path2(getPath(to, from));
776 if (path2.length() < path.length())
777 {
778 path2.reverse_ = true;
779 path = path2;
780 }
781 }
782 firstTime = false;
783 }
784 interpolate(from, path, t, state, rho_);
785}
786
787void DubinsStateSpace::interpolate(const State *from, const PathType &path, double t, State *state, double radius) const
788{
789 auto *s = allocState()->as<StateType>();
790 double seg = t * path.length(), phi, v;
791
792 s->setXY(0., 0.);
793 s->setYaw(from->as<StateType>()->getYaw());
794 if (!path.reverse_)
795 {
796 for (unsigned int i = 0; i < 3 && seg > 0; ++i)
797 {
798 v = std::min(seg, path.length_[i]);
799 phi = s->getYaw();
800 seg -= v;
801 switch (path.type_->at(i))
802 {
803 case DUBINS_LEFT:
804 s->setXY(s->getX() + sin(phi + v) - sin(phi), s->getY() - cos(phi + v) + cos(phi));
805 s->setYaw(phi + v);
806 break;
807 case DUBINS_RIGHT:
808 s->setXY(s->getX() - sin(phi - v) + sin(phi), s->getY() + cos(phi - v) - cos(phi));
809 s->setYaw(phi - v);
810 break;
811 case DUBINS_STRAIGHT:
812 s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
813 break;
814 }
815 }
816 }
817 else
818 {
819 for (unsigned int i = 0; i < 3 && seg > 0; ++i)
820 {
821 v = std::min(seg, path.length_[2 - i]);
822 phi = s->getYaw();
823 seg -= v;
824 switch (path.type_->at(2 - i))
825 {
826 case DUBINS_LEFT:
827 s->setXY(s->getX() + sin(phi - v) - sin(phi), s->getY() - cos(phi - v) + cos(phi));
828 s->setYaw(phi - v);
829 break;
830 case DUBINS_RIGHT:
831 s->setXY(s->getX() - sin(phi + v) + sin(phi), s->getY() + cos(phi + v) - cos(phi));
832 s->setYaw(phi + v);
833 break;
834 case DUBINS_STRAIGHT:
835 s->setXY(s->getX() - v * cos(phi), s->getY() - v * sin(phi));
836 break;
837 }
838 }
839 }
840 state->as<StateType>()->setX(s->getX() * radius + from->as<StateType>()->getX());
841 state->as<StateType>()->setY(s->getY() * radius + from->as<StateType>()->getY());
842 getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
843 state->as<StateType>()->setYaw(s->getYaw());
844 freeState(s);
845}
846
847unsigned int DubinsStateSpace::validSegmentCount(const State *state1, const State *state2) const
848{
849 return StateSpace::validSegmentCount(state1, state2);
850}
851
852DubinsStateSpace::PathType DubinsStateSpace::getPath(const State *state1, const State *state2) const
853{
854 return getPath(state1, state2, rho_);
855}
856
857DubinsStateSpace::PathType DubinsStateSpace::getPath(const State *state1, const State *state2, double radius)
858{
859 const auto *s1 = static_cast<const DubinsStateSpace::StateType *>(state1);
860 const auto *s2 = static_cast<const DubinsStateSpace::StateType *>(state2);
861 double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
862 double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
863 double dx = x2 - x1, dy = y2 - y1, d = sqrt(dx * dx + dy * dy) / radius, th = atan2(dy, dx);
864 double alpha = mod2pi(th1 - th), beta = mod2pi(th2 - th);
865 return ::getPath(d, alpha, beta);
866}
ompl::base::CompoundStateSpace::getSubspace
const StateSpacePtr & getSubspace(unsigned int index) const
Get a specific subspace from the compound state space.
Definition StateSpace.cpp:915
ompl::base::CompoundStateSpace::copyState
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
Definition StateSpace.cpp:1036
ompl::base::DubinsStateSpace::PathType
Complete description of a Dubins path.
Definition DubinsStateSpace.h:78
ompl::base::DubinsStateSpace::dubinsPathType
static const std::vector< std::vector< DubinsPathSegmentType > > & dubinsPathType()
Dubins path types.
Definition DubinsStateSpace.cpp:721
ompl::base::DubinsStateSpace::distance
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...
Definition DubinsStateSpace.cpp:734
ompl::base::DubinsStateSpace::validSegmentCount
unsigned int validSegmentCount(const State *state1, const State *state2) const override
Count how many segments of the "longest valid length" fit on the motion from state1 to state2....
Definition DubinsStateSpace.cpp:847
ompl::base::DubinsStateSpace::getPath
PathType getPath(const State *state1, const State *state2) const
Return a shortest Dubins path from SE(2) state state1 to SE(2) state state2.
Definition DubinsStateSpace.cpp:852
ompl::base::DubinsStateSpace::interpolate
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 DubinsStateSpace.cpp:747
ompl::base::DubinsStateSpace::isSymmetric_
bool isSymmetric_
Whether the distance is "symmetrized".
Definition DubinsStateSpace.h:165
ompl::base::SE2StateSpace::StateType
A state in SE(2): (x, y, yaw).
Definition SE2StateSpace.h:54
ompl::base::SE2StateSpace::StateType::getYaw
double getYaw() const
Get the yaw component of the state. This is the rotation in plane, with respect to the Z axis.
Definition SE2StateSpace.h:73
ompl::base::SE2StateSpace::StateType::getY
double getY() const
Get the Y component of the state.
Definition SE2StateSpace.h:65
ompl::base::SE2StateSpace::StateType::getX
double getX() const
Get the X component of the state.
Definition SE2StateSpace.h:59
ompl::base::SE2StateSpace::allocState
State * allocState() const override
Allocate a state that can store a point in the described space.
Definition SE2StateSpace.cpp:41
ompl::base::SE2StateSpace::freeState
void freeState(State *state) const override
Free the memory of the allocated state.
Definition SE2StateSpace.cpp:48
ompl::base::StateSpace::StateType
ompl::base::State StateType
Define the type of state allocated by this space.
Definition StateSpace.h:78
ompl::base::StateSpace::validSegmentCount
virtual unsigned int validSegmentCount(const State *state1, const State *state2) const
Count how many segments of the "longest valid length" fit on the motion from state1 to state2.
Definition StateSpace.cpp:851
ompl::base::State
Definition of an abstract state.
Definition State.h:50
ompl::base::State::as
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
ompl::base
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition ConstrainedSpaceInformation.h:55
ompl::base::operator<<
std::ostream & operator<<(std::ostream &stream, Cost c)
Output operator for Cost.
Definition Cost.cpp:39