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 
43 using namespace ompl::base;
44 
45 namespace
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  {
120  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
121  Lmin = L;
122  }
123  if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
124  {
125  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
126  Lmin = L;
127  }
128  if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
129  {
130  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
131  Lmin = L;
132  }
133  if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
134  {
135  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
136  Lmin = L;
137  }
138  if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
139  {
140  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
141  Lmin = L;
142  }
143  if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
144  {
145  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
146  Lmin = L;
147  }
148  if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
149  {
150  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
151  Lmin = L;
152  }
153  if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
154  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
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  {
178  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
179  Lmin = L;
180  }
181  if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
182  {
183  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
184  Lmin = L;
185  }
186  if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
187  {
188  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
189  Lmin = L;
190  }
191  if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
192  {
193  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
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  {
201  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
202  Lmin = L;
203  }
204  if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
205  {
206  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
207  Lmin = L;
208  }
209  if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
210  {
211  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
212  Lmin = L;
213  }
214  if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
215  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
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  {
255  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
256  Lmin = L;
257  }
258  if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip
259  {
260  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
261  Lmin = L;
262  }
263  if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // reflect
264  {
265  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
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  {
270  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
271  Lmin = L;
272  }
273 
274  if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
275  {
276  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
277  Lmin = L;
278  }
279  if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip
280  {
281  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
282  Lmin = L;
283  }
284  if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // reflect
285  {
286  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
287  Lmin = L;
288  }
289  if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip + reflect
290  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
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  {
332  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5 * pi, u, v);
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 =
338  ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5 * pi, -u, -v);
339  Lmin = L;
340  }
341  if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
342  {
343  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5 * pi, u, v);
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 =
349  ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5 * pi, -u, -v);
350  Lmin = L;
351  }
352 
353  if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
354  {
355  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5 * pi, u, v);
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 =
361  ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5 * pi, -u, -v);
362  Lmin = L;
363  }
364  if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
365  {
366  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5 * pi, u, v);
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 =
372  ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5 * pi, -u, -v);
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  {
380  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5 * pi, t);
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 =
386  ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5 * pi, -t);
387  Lmin = L;
388  }
389  if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
390  {
391  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5 * pi, t);
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 =
397  ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5 * pi, -t);
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 =
404  ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5 * pi, t);
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 =
410  ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5 * pi, -t);
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 =
416  ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5 * pi, t);
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 =
421  ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5 * pi, -t);
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  {
448  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5 * pi, u,
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  {
454  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5 * pi, -u,
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  {
460  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5 * pi, u,
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
465  path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5 * pi, -u,
466  .5 * pi, -v);
467  }
468 
469  ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
470  {
471  ReedsSheppStateSpace::ReedsSheppPath path;
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 
481 const ompl::base::ReedsSheppStateSpace::ReedsSheppPathSegmentType
482  ompl::base::ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
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 
503 ompl::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 
515 double ompl::base::ReedsSheppStateSpace::distance(const State *state1, const State *state2) const
516 {
517  return rho_ * reedsShepp(state1, state2).length();
518 }
519 
520 void 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 
528 void 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 
551 void 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 
596 ompl::base::ReedsSheppStateSpace::ReedsSheppPath ompl::base::ReedsSheppStateSpace::reedsShepp(const State *state1,
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 
608 void 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 
615 bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2,
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;
621  ReedsSheppStateSpace::ReedsSheppPath path;
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 
661 bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2) const
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;
668  ReedsSheppStateSpace::ReedsSheppPath path;
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 }
ompl::base::ReedsSheppStateSpace
An SE(2) state space where distance is measured by the length of Reeds-Shepp curves.
Definition: ReedsSheppStateSpace.h:127
ompl::base::ReedsSheppStateSpace::reedsShepp
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.
Definition: ReedsSheppStateSpace.cpp:596
ompl::base::State
Definition of an abstract state.
Definition: State.h:113
ompl::base
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: ConstrainedSpaceInformation.h:86
ompl::base::ReedsSheppMotionValidator::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: ReedsSheppStateSpace.cpp:661
ompl::base::State::as
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
ompl::base::ReedsSheppStateSpace::reedsSheppPathType
static const ReedsSheppPathSegmentType reedsSheppPathType[18][5]
Reeds-Shepp path types.
Definition: ReedsSheppStateSpace.h:171
ompl::base::ReedsSheppStateSpace::ReedsSheppPathSegmentType
ReedsSheppPathSegmentType
The Reeds-Shepp path segment types.
Definition: ReedsSheppStateSpace.h:163
ompl::base::SO2StateSpace::StateType
The definition of a state in SO(2)
Definition: SO2StateSpace.h:131
ompl::base::ReedsSheppStateSpace::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: ReedsSheppStateSpace.cpp:520
ompl::Exception
The exception type for ompl.
Definition: Exception.h:78
ompl::base::ReedsSheppStateSpace::ReedsSheppPath
Complete description of a ReedsShepp path.
Definition: ReedsSheppStateSpace.h:173
ompl::base::ReedsSheppStateSpace::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: ReedsSheppStateSpace.cpp:515