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  const double RS_EPS = 1e-6;
52  const double ZERO = 10 * std::numeric_limits<double>::epsilon();
53 
54  inline double mod2pi(double x)
55  {
56  double v = fmod(x, twopi);
57  if (v < -pi)
58  v += twopi;
59  else if (v > pi)
60  v -= twopi;
61  return v;
62  }
63  inline void polar(double x, double y, double &r, double &theta)
64  {
65  r = sqrt(x * x + y * y);
66  theta = atan2(y, x);
67  }
68  inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
69  {
70  double delta = mod2pi(u - v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
71  double t1 = atan2(eta * A - xi * B, xi * A + eta * B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
72  tau = (t2 < 0) ? mod2pi(t1 + pi) : mod2pi(t1);
73  omega = mod2pi(tau - u + v - phi);
74  }
75 
76  // formula 8.1 in Reeds-Shepp paper
77  inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
78  {
79  polar(x - sin(phi), y - 1. + cos(phi), u, t);
80  if (t >= -ZERO)
81  {
82  v = mod2pi(phi - t);
83  if (v >= -ZERO)
84  {
85  assert(fabs(u * cos(t) + sin(phi) - x) < RS_EPS);
86  assert(fabs(u * sin(t) - cos(phi) + 1 - y) < RS_EPS);
87  assert(fabs(mod2pi(t + v - phi)) < RS_EPS);
88  return true;
89  }
90  }
91  return false;
92  }
93  // formula 8.2
94  inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
95  {
96  double t1, u1;
97  polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
98  u1 = u1 * u1;
99  if (u1 >= 4.)
100  {
101  double theta;
102  u = sqrt(u1 - 4.);
103  theta = atan2(2., u);
104  t = mod2pi(t1 + theta);
105  v = mod2pi(t - phi);
106  assert(fabs(2 * sin(t) + u * cos(t) - sin(phi) - x) < RS_EPS);
107  assert(fabs(-2 * cos(t) + u * sin(t) + cos(phi) + 1 - y) < RS_EPS);
108  assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
109  return t >= -ZERO && v >= -ZERO;
110  }
111  return false;
112  }
113  void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
114  {
115  double t, u, v, Lmin = path.length(), L;
116  if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
117  {
119  Lmin = L;
120  }
121  if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
122  {
124  Lmin = L;
125  }
126  if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
127  {
129  Lmin = L;
130  }
131  if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
132  {
134  Lmin = L;
135  }
136  if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
137  {
139  Lmin = L;
140  }
141  if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
142  {
144  Lmin = L;
145  }
146  if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
147  {
149  Lmin = L;
150  }
151  if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
153  }
154  // formula 8.3 / 8.4 *** TYPO IN PAPER ***
155  inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
156  {
157  double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
158  polar(xi, eta, u1, theta);
159  if (u1 <= 4.)
160  {
161  u = -2. * asin(.25 * u1);
162  t = mod2pi(theta + .5 * u + pi);
163  v = mod2pi(phi - t + u);
164  assert(fabs(2 * (sin(t) - sin(t - u)) + sin(phi) - x) < RS_EPS);
165  assert(fabs(2 * (-cos(t) + cos(t - u)) - cos(phi) + 1 - y) < RS_EPS);
166  assert(fabs(mod2pi(t - u + v - phi)) < RS_EPS);
167  return t >= -ZERO && u <= ZERO;
168  }
169  return false;
170  }
171  void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
172  {
173  double t, u, v, Lmin = path.length(), L;
174  if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
175  {
177  Lmin = L;
178  }
179  if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
180  {
182  Lmin = L;
183  }
184  if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
185  {
187  Lmin = L;
188  }
189  if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
190  {
192  Lmin = L;
193  }
194 
195  // backwards
196  double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
197  if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
198  {
200  Lmin = L;
201  }
202  if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
203  {
205  Lmin = L;
206  }
207  if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
208  {
210  Lmin = L;
211  }
212  if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
214  }
215  // formula 8.7
216  inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
217  {
218  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi * xi + eta * eta));
219  if (rho <= 1.)
220  {
221  u = acos(rho);
222  tauOmega(u, -u, xi, eta, phi, t, v);
223  assert(fabs(2 * (sin(t) - sin(t - u) + sin(t - 2 * u)) - sin(phi) - x) < RS_EPS);
224  assert(fabs(2 * (-cos(t) + cos(t - u) - cos(t - 2 * u)) + cos(phi) + 1 - y) < RS_EPS);
225  assert(fabs(mod2pi(t - 2 * u - v - phi)) < RS_EPS);
226  return t >= -ZERO && v <= ZERO;
227  }
228  return false;
229  }
230  // formula 8.8
231  inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
232  {
233  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi * xi - eta * eta) / 16.;
234  if (rho >= 0 && rho <= 1)
235  {
236  u = -acos(rho);
237  if (u >= -.5 * pi)
238  {
239  tauOmega(u, u, xi, eta, phi, t, v);
240  assert(fabs(4 * sin(t) - 2 * sin(t - u) - sin(phi) - x) < RS_EPS);
241  assert(fabs(-4 * cos(t) + 2 * cos(t - u) + cos(phi) + 1 - y) < RS_EPS);
242  assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
243  return t >= -ZERO && v >= -ZERO;
244  }
245  }
246  return false;
247  }
248  void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
249  {
250  double t, u, v, Lmin = path.length(), L;
251  if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
252  {
254  Lmin = L;
255  }
256  if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip
257  {
259  Lmin = L;
260  }
261  if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // reflect
262  {
264  Lmin = L;
265  }
266  if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip + reflect
267  {
269  Lmin = L;
270  }
271 
272  if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
273  {
275  Lmin = L;
276  }
277  if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip
278  {
280  Lmin = L;
281  }
282  if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // reflect
283  {
285  Lmin = L;
286  }
287  if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip + reflect
289  }
290  // formula 8.9
291  inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
292  {
293  double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
294  polar(xi, eta, rho, theta);
295  if (rho >= 2.)
296  {
297  double r = sqrt(rho * rho - 4.);
298  u = 2. - r;
299  t = mod2pi(theta + atan2(r, -2.));
300  v = mod2pi(phi - .5 * pi - t);
301  assert(fabs(2 * (sin(t) - cos(t)) - u * sin(t) + sin(phi) - x) < RS_EPS);
302  assert(fabs(-2 * (sin(t) + cos(t)) + u * cos(t) - cos(phi) + 1 - y) < RS_EPS);
303  assert(fabs(mod2pi(t + pi / 2 + v - phi)) < RS_EPS);
304  return t >= -ZERO && u <= ZERO && v <= ZERO;
305  }
306  return false;
307  }
308  // formula 8.10
309  inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
310  {
311  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
312  polar(-eta, xi, rho, theta);
313  if (rho >= 2.)
314  {
315  t = theta;
316  u = 2. - rho;
317  v = mod2pi(t + .5 * pi - phi);
318  assert(fabs(2 * sin(t) - cos(t - v) - u * sin(t) - x) < RS_EPS);
319  assert(fabs(-2 * cos(t) - sin(t - v) + u * cos(t) + 1 - y) < RS_EPS);
320  assert(fabs(mod2pi(t + pi / 2 - v - phi)) < RS_EPS);
321  return t >= -ZERO && u <= ZERO && v <= ZERO;
322  }
323  return false;
324  }
325  void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
326  {
327  double t, u, v, Lmin = path.length() - .5 * pi, L;
328  if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
329  {
331  Lmin = L;
332  }
333  if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
334  {
335  path =
337  Lmin = L;
338  }
339  if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
340  {
342  Lmin = L;
343  }
344  if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
345  {
346  path =
348  Lmin = L;
349  }
350 
351  if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
352  {
354  Lmin = L;
355  }
356  if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
357  {
358  path =
360  Lmin = L;
361  }
362  if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
363  {
365  Lmin = L;
366  }
367  if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
368  {
369  path =
371  Lmin = L;
372  }
373 
374  // backwards
375  double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
376  if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
377  {
379  Lmin = L;
380  }
381  if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
382  {
383  path =
385  Lmin = L;
386  }
387  if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
388  {
390  Lmin = L;
391  }
392  if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
393  {
394  path =
396  Lmin = L;
397  }
398 
399  if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
400  {
401  path =
403  Lmin = L;
404  }
405  if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
406  {
407  path =
409  Lmin = L;
410  }
411  if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
412  {
413  path =
415  Lmin = L;
416  }
417  if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
418  path =
420  }
421  // formula 8.11 *** TYPO IN PAPER ***
422  inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
423  {
424  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
425  polar(xi, eta, rho, theta);
426  if (rho >= 2.)
427  {
428  u = 4. - sqrt(rho * rho - 4.);
429  if (u <= ZERO)
430  {
431  t = mod2pi(atan2((4 - u) * xi - 2 * eta, -2 * xi + (u - 4) * eta));
432  v = mod2pi(t - phi);
433  assert(fabs(4 * sin(t) - 2 * cos(t) - u * sin(t) - sin(phi) - x) < RS_EPS);
434  assert(fabs(-4 * cos(t) - 2 * sin(t) + u * cos(t) + cos(phi) + 1 - y) < RS_EPS);
435  assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
436  return t >= -ZERO && v >= -ZERO;
437  }
438  }
439  return false;
440  }
441  void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
442  {
443  double t, u, v, Lmin = path.length() - pi, L;
444  if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
445  {
447  -.5 * pi, v);
448  Lmin = L;
449  }
450  if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
451  {
453  .5 * pi, -v);
454  Lmin = L;
455  }
456  if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
457  {
459  -.5 * pi, v);
460  Lmin = L;
461  }
462  if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
464  .5 * pi, -v);
465  }
466 
467  ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
468  {
470  CSC(x, y, phi, path);
471  CCC(x, y, phi, path);
472  CCCC(x, y, phi, path);
473  CCSC(x, y, phi, path);
474  CCSCC(x, y, phi, path);
475  return path;
476  }
477 }
478 
481  {RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 0
482  {RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP}, // 1
483  {RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 2
484  {RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP}, // 3
485  {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP}, // 4
486  {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP}, // 5
487  {RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP}, // 6
488  {RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 7
489  {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP}, // 8
490  {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP}, // 9
491  {RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP}, // 10
492  {RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 11
493  {RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP}, // 12
494  {RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 13
495  {RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 14
496  {RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP}, // 15
497  {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT}, // 16
498  {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT} // 17
499 };
500 
501 ompl::base::ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType *type, double t,
502  double u, double v, double w, double x)
503  : type_(type)
504 {
505  length_[0] = t;
506  length_[1] = u;
507  length_[2] = v;
508  length_[3] = w;
509  length_[4] = x;
510  totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
511 }
512 
513 double ompl::base::ReedsSheppStateSpace::distance(const State *state1, const State *state2) const
514 {
515  return rho_ * reedsShepp(state1, state2).length();
516 }
517 
518 void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t,
519  State *state) const
520 {
521  bool firstTime = true;
522  ReedsSheppPath path;
523  interpolate(from, to, t, firstTime, path, state);
524 }
525 
526 void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t, bool &firstTime,
527  ReedsSheppPath &path, State *state) const
528 {
529  if (firstTime)
530  {
531  if (t >= 1.)
532  {
533  if (to != state)
534  copyState(state, to);
535  return;
536  }
537  if (t <= 0.)
538  {
539  if (from != state)
540  copyState(state, from);
541  return;
542  }
543  path = reedsShepp(from, to);
544  firstTime = false;
545  }
546  interpolate(from, path, t, state);
547 }
548 
549 void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const ReedsSheppPath &path, double t,
550  State *state) const
551 {
552  StateType *s = allocState()->as<StateType>();
553  double seg = t * path.length(), phi, v;
554 
555  s->setXY(0., 0.);
556  s->setYaw(from->as<StateType>()->getYaw());
557  for (unsigned int i = 0; i < 5 && seg > 0; ++i)
558  {
559  if (path.length_[i] < 0)
560  {
561  v = std::max(-seg, path.length_[i]);
562  seg += v;
563  }
564  else
565  {
566  v = std::min(seg, path.length_[i]);
567  seg -= v;
568  }
569  phi = s->getYaw();
570  switch (path.type_[i])
571  {
572  case RS_LEFT:
573  s->setXY(s->getX() + sin(phi + v) - sin(phi), s->getY() - cos(phi + v) + cos(phi));
574  s->setYaw(phi + v);
575  break;
576  case RS_RIGHT:
577  s->setXY(s->getX() - sin(phi - v) + sin(phi), s->getY() + cos(phi - v) - cos(phi));
578  s->setYaw(phi - v);
579  break;
580  case RS_STRAIGHT:
581  s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
582  break;
583  case RS_NOP:
584  break;
585  }
586  }
587  state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
588  state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
589  getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
590  state->as<StateType>()->setYaw(s->getYaw());
591  freeState(s);
592 }
593 
595  const State *state2) const
596 {
597  const StateType *s1 = static_cast<const StateType *>(state1);
598  const StateType *s2 = static_cast<const StateType *>(state2);
599  double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
600  double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
601  double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1);
602  double x = c * dx + s * dy, y = -s * dx + c * dy, phi = th2 - th1;
603  return ::reedsShepp(x / rho_, y / rho_, phi);
604 }
605 
606 void ompl::base::ReedsSheppMotionValidator::defaultSettings()
607 {
608  stateSpace_ = dynamic_cast<ReedsSheppStateSpace *>(si_->getStateSpace().get());
609  if (!stateSpace_)
610  throw Exception("No state space for motion validator");
611 }
612 
614  std::pair<State *, double> &lastValid) const
615 {
616  /* assume motion starts in a valid configuration so s1 is valid */
617 
618  bool result = true, firstTime = true;
620  int nd = stateSpace_->validSegmentCount(s1, s2);
621 
622  if (nd > 1)
623  {
624  /* temporary storage for the checked state */
625  State *test = si_->allocState();
626 
627  for (int j = 1; j < nd; ++j)
628  {
629  stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
630  if (!si_->isValid(test))
631  {
632  lastValid.second = (double)(j - 1) / (double)nd;
633  if (lastValid.first)
634  stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
635  result = false;
636  break;
637  }
638  }
639  si_->freeState(test);
640  }
641 
642  if (result)
643  if (!si_->isValid(s2))
644  {
645  lastValid.second = (double)(nd - 1) / (double)nd;
646  if (lastValid.first)
647  stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
648  result = false;
649  }
650 
651  if (result)
652  valid_++;
653  else
654  invalid_++;
655 
656  return result;
657 }
658 
660 {
661  /* assume motion starts in a valid configuration so s1 is valid */
662  if (!si_->isValid(s2))
663  return false;
664 
665  bool result = true, firstTime = true;
667  int nd = stateSpace_->validSegmentCount(s1, s2);
668 
669  /* initialize the queue of test positions */
670  std::queue<std::pair<int, int>> pos;
671  if (nd >= 2)
672  {
673  pos.push(std::make_pair(1, nd - 1));
674 
675  /* temporary storage for the checked state */
676  State *test = si_->allocState();
677 
678  /* repeatedly subdivide the path segment in the middle (and check the middle) */
679  while (!pos.empty())
680  {
681  std::pair<int, int> x = pos.front();
682 
683  int mid = (x.first + x.second) / 2;
684  stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);
685 
686  if (!si_->isValid(test))
687  {
688  result = false;
689  break;
690  }
691 
692  pos.pop();
693 
694  if (x.first < mid)
695  pos.push(std::make_pair(x.first, mid - 1));
696  if (x.second > mid)
697  pos.push(std::make_pair(mid + 1, x.second));
698  }
699 
700  si_->freeState(test);
701  }
702 
703  if (result)
704  valid_++;
705  else
706  invalid_++;
707 
708  return result;
709 }
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...
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...
void interpolate(const State *from, const State *to, const 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...
Complete description of a ReedsShepp path.
void interpolate(const State *from, const State *to, const 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...
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
State * allocState() const override
Allocate a state that can store a point in the described space.
static const ReedsSheppPathSegmentType reedsSheppPathType[18][5]
Reeds-Shepp path types.
The definition of a state in SO(2)
Definition: SO2StateSpace.h:67
void freeState(State *state) const override
Free the memory of the allocated state.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ReedsSheppPathSegmentType
The Reeds-Shepp path segment types.
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
The exception type for ompl.
Definition: Exception.h:46
An SE(2) state space where distance is measured by the length of Reeds-Shepp curves.
ReedsSheppPath reedsShepp(const State *state1, const State *state2) const
Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2.