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 
43 using namespace ompl::base;
44 
45 namespace
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::DubinsPath 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::DubinsPath(&DubinsStateSpace::dubinsPathType[0], t, p, q);
264  }
265  return {};
266  }
267 
268  DubinsStateSpace::DubinsPath 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::DubinsPath(&DubinsStateSpace::dubinsPathType[1], t, p, q);
282  }
283  return {};
284  }
285 
286  DubinsStateSpace::DubinsPath 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::DubinsPath(&DubinsStateSpace::dubinsPathType[2], t, p, q);
300  }
301  return {};
302  }
303 
304  DubinsStateSpace::DubinsPath 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::DubinsPath(&DubinsStateSpace::dubinsPathType[3], t, p, q);
318  }
319  return {};
320  }
321 
322  DubinsStateSpace::DubinsPath 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::DubinsPath(&DubinsStateSpace::dubinsPathType[4], t, p, q);
336  }
337  return {};
338  }
339 
340  DubinsStateSpace::DubinsPath 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::DubinsPath(&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::DubinsPath 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::DubinsPath 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::DubinsPath 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::DubinsPath 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::DubinsPath 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::DubinsPath 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::DubinsPath 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::DubinsPath 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 
694 namespace ompl::base
695 {
696  std::ostream &operator<<(std::ostream &os, const DubinsStateSpace::DubinsPath &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 
712 DubinsStateSpace::DubinsPath dubins(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 
721 // const DubinsStateSpace::DubinsPathSegmentType DubinsStateSpace::dubinsPathType[6][3] = {
722 const std::vector<std::vector<DubinsStateSpace::DubinsPathSegmentType> > DubinsStateSpace::dubinsPathType {{
723  {{DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_LEFT}},
724  {{DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_RIGHT}},
725  {{DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_LEFT}},
726  {{DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_RIGHT}},
727  {{DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT}},
728  {{DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT}}
729  }};
730 
731 double DubinsStateSpace::distance(const State *state1, const State *state2) const
732 {
733  return isSymmetric_ ? symmetricDistance(state1, state2, rho_) : distance(state1, state2, rho_);
734 }
735 double DubinsStateSpace::distance(const State *state1, const State *state2, double radius)
736 {
737  return radius * dubins(state1, state2, radius).length();
738 }
739 double DubinsStateSpace::symmetricDistance(const State *state1, const State *state2, double radius)
740 {
741  return radius * std::min(dubins(state1, state2, radius).length(), dubins(state2, state1, radius).length());
742 }
743 
744 void DubinsStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
745 {
746  bool firstTime = true;
747  DubinsPath path;
748  interpolate(from, to, t, firstTime, path, state);
749 }
750 
751 void DubinsStateSpace::interpolate(const State *from, const State *to, const double t, bool &firstTime,
752  DubinsPath &path, State *state) const
753 {
754  if (firstTime)
755  {
756  if (t >= 1.)
757  {
758  if (to != state)
759  copyState(state, to);
760  return;
761  }
762  if (t <= 0.)
763  {
764  if (from != state)
765  copyState(state, from);
766  return;
767  }
768 
769  path = dubins(from, to);
770  if (isSymmetric_)
771  {
772  DubinsPath path2(dubins(to, from));
773  if (path2.length() < path.length())
774  {
775  path2.reverse_ = true;
776  path = path2;
777  }
778  }
779  firstTime = false;
780  }
781  interpolate(from, path, t, state, rho_);
782 }
783 
784 void DubinsStateSpace::interpolate(const State *from, const DubinsPath &path, double t, State *state,
785  double radius) const
786 {
787  auto *s = allocState()->as<StateType>();
788  double seg = t * path.length(), phi, v;
789 
790  s->setXY(0., 0.);
791  s->setYaw(from->as<StateType>()->getYaw());
792  if (!path.reverse_)
793  {
794  for (unsigned int i = 0; i < 3 && seg > 0; ++i)
795  {
796  v = std::min(seg, path.length_[i]);
797  phi = s->getYaw();
798  seg -= v;
799  switch (path.type_->at(i))
800  {
801  case DUBINS_LEFT:
802  s->setXY(s->getX() + sin(phi + v) - sin(phi), s->getY() - cos(phi + v) + cos(phi));
803  s->setYaw(phi + v);
804  break;
805  case DUBINS_RIGHT:
806  s->setXY(s->getX() - sin(phi - v) + sin(phi), s->getY() + cos(phi - v) - cos(phi));
807  s->setYaw(phi - v);
808  break;
809  case DUBINS_STRAIGHT:
810  s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
811  break;
812  }
813  }
814  }
815  else
816  {
817  for (unsigned int i = 0; i < 3 && seg > 0; ++i)
818  {
819  v = std::min(seg, path.length_[2 - i]);
820  phi = s->getYaw();
821  seg -= v;
822  switch (path.type_->at(2 - i))
823  {
824  case DUBINS_LEFT:
825  s->setXY(s->getX() + sin(phi - v) - sin(phi), s->getY() - cos(phi - v) + cos(phi));
826  s->setYaw(phi - v);
827  break;
828  case DUBINS_RIGHT:
829  s->setXY(s->getX() - sin(phi + v) + sin(phi), s->getY() + cos(phi + v) - cos(phi));
830  s->setYaw(phi + v);
831  break;
832  case DUBINS_STRAIGHT:
833  s->setXY(s->getX() - v * cos(phi), s->getY() - v * sin(phi));
834  break;
835  }
836  }
837  }
838  state->as<StateType>()->setX(s->getX() * radius + from->as<StateType>()->getX());
839  state->as<StateType>()->setY(s->getY() * radius + from->as<StateType>()->getY());
840  getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
841  state->as<StateType>()->setYaw(s->getYaw());
842  freeState(s);
843 }
844 
845 unsigned int DubinsStateSpace::validSegmentCount(const State *state1, const State *state2) const
846 {
847  return StateSpace::validSegmentCount(state1, state2);
848 }
849 
850 DubinsStateSpace::DubinsPath DubinsStateSpace::dubins(const State *state1, const State *state2) const
851 {
852  return dubins(state1, state2, rho_);
853 }
854 
855 DubinsStateSpace::DubinsPath DubinsStateSpace::dubins(const State *state1, const State *state2, double radius)
856 {
857  const auto *s1 = static_cast<const DubinsStateSpace::StateType *>(state1);
858  const auto *s2 = static_cast<const DubinsStateSpace::StateType *>(state2);
859  double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
860  double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
861  double dx = x2 - x1, dy = y2 - y1, d = sqrt(dx * dx + dy * dy) / radius, th = atan2(dy, dx);
862  double alpha = mod2pi(th1 - th), beta = mod2pi(th2 - th);
863  return ::dubins(d, alpha, beta);
864 }
865 
866 void DubinsMotionValidator::defaultSettings()
867 {
868  stateSpace_ = dynamic_cast<DubinsStateSpace *>(si_->getStateSpace().get());
869  if (stateSpace_ == nullptr)
870  throw Exception("No state space for motion validator");
871 }
872 
873 bool DubinsMotionValidator::checkMotion(const State *s1, const State *s2, std::pair<State *, double> &lastValid) const
874 {
875  /* assume motion starts in a valid configuration so s1 is valid */
876 
877  bool result = true, firstTime = true;
878  DubinsStateSpace::DubinsPath path;
879  int nd = stateSpace_->validSegmentCount(s1, s2);
880 
881  if (nd > 1)
882  {
883  /* temporary storage for the checked state */
884  State *test = si_->allocState();
885 
886  for (int j = 1; j < nd; ++j)
887  {
888  stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
889  if (!si_->isValid(test))
890  {
891  lastValid.second = (double)(j - 1) / (double)nd;
892  if (lastValid.first != nullptr)
893  stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
894  result = false;
895  break;
896  }
897  }
898  si_->freeState(test);
899  }
900 
901  if (result)
902  if (!si_->isValid(s2))
903  {
904  lastValid.second = (double)(nd - 1) / (double)nd;
905  if (lastValid.first != nullptr)
906  stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
907  result = false;
908  }
909 
910  if (result)
911  valid_++;
912  else
913  invalid_++;
914 
915  return result;
916 }
917 
918 bool DubinsMotionValidator::checkMotion(const State *s1, const State *s2) const
919 {
920  /* assume motion starts in a valid configuration so s1 is valid */
921  if (!si_->isValid(s2))
922  return false;
923 
924  bool result = true, firstTime = true;
925  DubinsStateSpace::DubinsPath path;
926  int nd = stateSpace_->validSegmentCount(s1, s2);
927 
928  /* initialize the queue of test positions */
929  std::queue<std::pair<int, int>> pos;
930  if (nd >= 2)
931  {
932  pos.emplace(1, nd - 1);
933 
934  /* temporary storage for the checked state */
935  State *test = si_->allocState();
936 
937  /* repeatedly subdivide the path segment in the middle (and check the middle) */
938  while (!pos.empty())
939  {
940  std::pair<int, int> x = pos.front();
941 
942  int mid = (x.first + x.second) / 2;
943  stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);
944 
945  if (!si_->isValid(test))
946  {
947  result = false;
948  break;
949  }
950 
951  pos.pop();
952 
953  if (x.first < mid)
954  pos.emplace(x.first, mid - 1);
955  if (x.second > mid)
956  pos.emplace(mid + 1, x.second);
957  }
958 
959  si_->freeState(test);
960  }
961 
962  if (result)
963  valid_++;
964  else
965  invalid_++;
966 
967  return result;
968 }
ompl::base::SE2StateSpace::StateType::getX
double getX() const
Get the X component of the state.
Definition: SE2StateSpace.h:187
ompl::base::operator<<
std::ostream & operator<<(std::ostream &stream, Cost c)
Output operator for Cost.
Definition: Cost.cpp:39
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::DubinsStateSpace::dubinsPathType
static const std::vector< std::vector< DubinsPathSegmentType > > dubinsPathType
Dubins path types.
Definition: DubinsStateSpace.h:170
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:744
ompl::base::State
Definition of an abstract state.
Definition: State.h:113
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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: ConstrainedSpaceInformation.h:86
ompl::base::SpaceInformation::freeState
void freeState(State *state) const
Free the memory of a state.
Definition: SpaceInformation.h:302
ompl::base::State::as
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
ompl::base::DubinsMotionValidator::checkMotion
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.
Definition: DubinsStateSpace.cpp:918
ompl::base::DubinsStateSpace::rho_
double rho_
Turning radius.
Definition: DubinsStateSpace.h:250
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
An SE(2) state space where distance is measured by the length of Dubins curves.
Definition: DubinsStateSpace.h:126
ompl::base::SpaceInformation::allocState
State * allocState() const
Allocate memory for a state.
Definition: SpaceInformation.h:289
ompl::base::SO2StateSpace::StateType
The definition of a state in SO(2)
Definition: SO2StateSpace.h:131
ompl::base::MotionValidator::invalid_
unsigned int invalid_
Number of invalid segments.
Definition: MotionValidator.h:203
ompl::base::CompoundStateSpace::StateType
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
ompl::base::MotionValidator::si_
SpaceInformation * si_
The instance of space information this state validity checker operates on.
Definition: MotionValidator.h:197
ompl::base::DubinsStateSpace::DubinsPath
Complete description of a Dubins path.
Definition: DubinsStateSpace.h:172
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:731
ompl::base::DubinsStateSpace::dubins
DubinsPath dubins(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:850
ompl::base::SE2StateSpace::StateType
A state in SE(2): (x, y, yaw)
Definition: SE2StateSpace.h:149
ompl::base::SpaceInformation::getStateSpace
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
Definition: SpaceInformation.h:164
ompl::base::SE2StateSpace::freeState
void freeState(State *state) const override
Free the memory of the allocated state.
Definition: SE2StateSpace.cpp:48
ompl::base::DubinsStateSpace::isSymmetric_
bool isSymmetric_
Whether the distance is "symmetrized".
Definition: DubinsStateSpace.h:259
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:845
ompl::Exception
The exception type for ompl.
Definition: Exception.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::MotionValidator::valid_
unsigned int valid_
Number of valid segments.
Definition: MotionValidator.h:200
ompl::base::SpaceInformation::isValid
bool isValid(const State *state) const
Check if a given state is valid or not.
Definition: SpaceInformation.h:158