All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 
44 using namespace ompl::base;
45 
46 namespace
47 {
48  // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
49 
50  const double pi = boost::math::constants::pi<double>();
51  const double twopi = 2. * pi;
52  const double RS_EPS = 1e-6;
53  const double ZERO = 10*std::numeric_limits<double>::epsilon();
54 
55  inline double mod2pi(double x)
56  {
57  double v = fmod(x, twopi);
58  if (v < -pi)
59  v += twopi;
60  else
61  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  {
122  Lmin = L;
123  }
124  if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
125  {
128  Lmin = L;
129  }
130  if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
131  {
134  Lmin = L;
135  }
136  if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
137  {
140  Lmin = L;
141  }
142  if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
143  {
146  Lmin = L;
147  }
148  if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
149  {
152  Lmin = L;
153  }
154  if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
155  {
158  Lmin = L;
159  }
160  if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
163  }
164  // formula 8.3 / 8.4 *** TYPO IN PAPER ***
165  inline bool LpRmL(double x, double y, double phi, double& t, double& u, double& v)
166  {
167  double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
168  polar(xi, eta, u1, theta);
169  if (u1 <= 4.)
170  {
171  u = -2.*asin(.25 * u1);
172  t = mod2pi(theta + .5 * u + pi);
173  v = mod2pi(phi - t + u);
174  assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
175  assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
176  assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
177  return t>=-ZERO && u<=ZERO;
178  }
179  return false;
180  }
181  void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
182  {
183  double t, u, v, Lmin = path.length(), L;
184  if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
185  {
188  Lmin = L;
189  }
190  if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
191  {
194  Lmin = L;
195  }
196  if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
197  {
200  Lmin = L;
201  }
202  if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
203  {
206  Lmin = L;
207  }
208 
209  // backwards
210  double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
211  if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
212  {
215  Lmin = L;
216  }
217  if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
218  {
221  Lmin = L;
222  }
223  if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
224  {
227  Lmin = L;
228  }
229  if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
232  }
233  // formula 8.7
234  inline bool LpRupLumRm(double x, double y, double phi, double& t, double& u, double& v)
235  {
236  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
237  if (rho <= 1.)
238  {
239  u = acos(rho);
240  tauOmega(u, -u, xi, eta, phi, t, v);
241  assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
242  assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
243  assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
244  return t>=-ZERO && v<=ZERO;
245  }
246  return false;
247  }
248  // formula 8.8
249  inline bool LpRumLumRp(double x, double y, double phi, double& t, double& u, double& v)
250  {
251  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
252  if (rho>=0 && rho<=1)
253  {
254  u = -acos(rho);
255  if (u >= -.5 * pi)
256  {
257  tauOmega(u, u, xi, eta, phi, t, v);
258  assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
259  assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
260  assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
261  return t>=-ZERO && v>=-ZERO;
262  }
263  }
264  return false;
265  }
266  void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
267  {
268  double t, u, v, Lmin = path.length(), L;
269  if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
270  {
273  Lmin = L;
274  }
275  if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
276  {
278  ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
279  Lmin = L;
280  }
281  if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
282  {
285  Lmin = L;
286  }
287  if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
288  {
290  ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
291  Lmin = L;
292  }
293 
294  if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
295  {
298  Lmin = L;
299  }
300  if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
301  {
303  ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
304  Lmin = L;
305  }
306  if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
307  {
310  Lmin = L;
311  }
312  if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
314  ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
315  }
316  // formula 8.9
317  inline bool LpRmSmLm(double x, double y, double phi, double& t, double& u, double& v)
318  {
319  double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
320  polar(xi, eta, rho, theta);
321  if (rho >= 2.)
322  {
323  double r = sqrt(rho*rho - 4.);
324  u = 2. - r;
325  t = mod2pi(theta + atan2(r, -2.));
326  v = mod2pi(phi - .5*pi - t);
327  assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
328  assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
329  assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
330  return t>=-ZERO && u<=ZERO && v<=ZERO;
331  }
332  return false;
333  }
334  // formula 8.10
335  inline bool LpRmSmRm(double x, double y, double phi, double& t, double& u, double& v)
336  {
337  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
338  polar(-eta, xi, rho, theta);
339  if (rho >= 2.)
340  {
341  t = theta;
342  u = 2. - rho;
343  v = mod2pi(t + .5*pi - phi);
344  assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
345  assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
346  assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
347  return t>=-ZERO && u<=ZERO && v<=ZERO;
348  }
349  return false;
350  }
351  void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
352  {
353  double t, u, v, Lmin = path.length() - .5*pi, L;
354  if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
355  {
357  ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
358  Lmin = L;
359  }
360  if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
361  {
363  ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
364  Lmin = L;
365  }
366  if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
367  {
369  ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
370  Lmin = L;
371  }
372  if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
373  {
375  ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
376  Lmin = L;
377  }
378 
379  if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
380  {
382  ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
383  Lmin = L;
384  }
385  if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
386  {
388  ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
389  Lmin = L;
390  }
391  if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
392  {
394  ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
395  Lmin = L;
396  }
397  if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
398  {
400  ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
401  Lmin = L;
402  }
403 
404  // backwards
405  double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
406  if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
407  {
409  ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
410  Lmin = L;
411  }
412  if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
413  {
415  ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
416  Lmin = L;
417  }
418  if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
419  {
421  ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
422  Lmin = L;
423  }
424  if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
425  {
427  ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
428  Lmin = L;
429  }
430 
431  if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
432  {
434  ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
435  Lmin = L;
436  }
437  if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
438  {
440  ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
441  Lmin = L;
442  }
443  if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
444  {
446  ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
447  Lmin = L;
448  }
449  if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
451  ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
452  }
453  // formula 8.11 *** TYPO IN PAPER ***
454  inline bool LpRmSLmRp(double x, double y, double phi, double& t, double& u, double& v)
455  {
456  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
457  polar(xi, eta, rho, theta);
458  if (rho >= 2.)
459  {
460  u = 4. - sqrt(rho*rho - 4.);
461  if (u <= ZERO)
462  {
463  t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
464  v = mod2pi(t - phi);
465  assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
466  assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
467  assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
468  return t>=-ZERO && v>=-ZERO;
469  }
470  }
471  return false;
472  }
473  void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
474  {
475  double t, u, v, Lmin = path.length() - pi, L;
476  if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
477  {
479  ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
480  Lmin = L;
481  }
482  if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
483  {
485  ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
486  Lmin = L;
487  }
488  if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
489  {
491  ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
492  Lmin = L;
493  }
494  if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
496  ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5*pi, -u, .5*pi, -v);
497  }
498 
499  ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
500  {
502  CSC(x, y, phi, path);
503  CCC(x, y, phi, path);
504  CCCC(x, y, phi, path);
505  CCSC(x, y, phi, path);
506  CCSCC(x, y, phi, path);
507  return path;
508  }
509 }
510 
513  { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 0
514  { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP }, // 1
515  { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 2
516  { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP }, // 3
517  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 4
518  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 5
519  { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 6
520  { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 7
521  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 8
522  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 9
523  { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 10
524  { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 11
525  { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 12
526  { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 13
527  { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 14
528  { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 15
529  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT }, // 16
530  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT } // 17
531 };
532 
533 ompl::base::ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
534  double t, double u, double v, double w, double x)
535  : type_(type)
536 {
537  length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
538  totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
539 }
540 
541 
542 double ompl::base::ReedsSheppStateSpace::distance(const State *state1, const State *state2) const
543 {
544  return rho_ * reedsShepp(state1, state2).length();
545 }
546 
547 void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
548 {
549  bool firstTime = true;
550  ReedsSheppPath path;
551  interpolate(from, to, t, firstTime, path, state);
552 }
553 
554 void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t,
555  bool& firstTime, ReedsSheppPath& path, State *state) const
556 {
557  if (firstTime)
558  {
559  if (t>=1.)
560  {
561  if (to != state)
562  copyState(state, to);
563  return;
564  }
565  if (t<=0.)
566  {
567  if (from != state)
568  copyState(state, from);
569  return;
570  }
571  path = reedsShepp(from, to);
572  firstTime = false;
573  }
574  interpolate(from, path, t, state);
575 }
576 
577 void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const ReedsSheppPath& path, double t, State *state) const
578 {
579  StateType* s = allocState()->as<StateType>();
580  double seg = t * path.length(), phi, v;
581 
582  s->setXY(0., 0.);
583  s->setYaw(from->as<StateType>()->getYaw());
584  for (unsigned int i=0; i<5 && seg>0; ++i)
585  {
586  if (path.length_[i]<0)
587  {
588  v = std::max(-seg, path.length_[i]);
589  seg += v;
590  }
591  else
592  {
593  v = std::min(seg, path.length_[i]);
594  seg -= v;
595  }
596  phi = s->getYaw();
597  switch(path.type_[i])
598  {
599  case RS_LEFT:
600  s->setXY(s->getX() + sin(phi+v) - sin(phi), s->getY() - cos(phi+v) + cos(phi));
601  s->setYaw(phi+v);
602  break;
603  case RS_RIGHT:
604  s->setXY(s->getX() - sin(phi-v) + sin(phi), s->getY() + cos(phi-v) - cos(phi));
605  s->setYaw(phi-v);
606  break;
607  case RS_STRAIGHT:
608  s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
609  break;
610  case RS_NOP:
611  break;
612  }
613  }
614  state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
615  state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
616  getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
617  state->as<StateType>()->setYaw(s->getYaw());
618  freeState(s);
619 }
620 
622 {
623  const StateType *s1 = static_cast<const StateType*>(state1);
624  const StateType *s2 = static_cast<const StateType*>(state2);
625  double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
626  double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
627  double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1);
628  double x = c*dx + s*dy, y = -s*dx + c*dy, phi = th2 - th1;
629  return ::reedsShepp(x/rho_, y/rho_, phi);
630 }
631 
632 
633 void ompl::base::ReedsSheppMotionValidator::defaultSettings(void)
634 {
635  stateSpace_ = dynamic_cast<ReedsSheppStateSpace*>(si_->getStateSpace().get());
636  if (!stateSpace_)
637  throw Exception("No state space for motion validator");
638 }
639 
640 bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
641 {
642  /* assume motion starts in a valid configuration so s1 is valid */
643 
644  bool result = true, firstTime = true;
646  int nd = stateSpace_->validSegmentCount(s1, s2);
647 
648  if (nd > 1)
649  {
650  /* temporary storage for the checked state */
651  State *test = si_->allocState();
652 
653  for (int j = 1 ; j < nd ; ++j)
654  {
655  stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
656  if (!si_->isValid(test))
657  {
658  lastValid.second = (double)(j - 1) / (double)nd;
659  if (lastValid.first)
660  stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
661  result = false;
662  break;
663  }
664  }
665  si_->freeState(test);
666  }
667 
668  if (result)
669  if (!si_->isValid(s2))
670  {
671  lastValid.second = (double)(nd - 1) / (double)nd;
672  if (lastValid.first)
673  stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
674  result = false;
675  }
676 
677  if (result)
678  valid_++;
679  else
680  invalid_++;
681 
682  return result;
683 }
684 
686 {
687  /* assume motion starts in a valid configuration so s1 is valid */
688  if (!si_->isValid(s2))
689  return false;
690 
691  bool result = true, firstTime = true;
693  int nd = stateSpace_->validSegmentCount(s1, s2);
694 
695  /* initialize the queue of test positions */
696  std::queue< std::pair<int, int> > pos;
697  if (nd >= 2)
698  {
699  pos.push(std::make_pair(1, nd - 1));
700 
701  /* temporary storage for the checked state */
702  State *test = si_->allocState();
703 
704  /* repeatedly subdivide the path segment in the middle (and check the middle) */
705  while (!pos.empty())
706  {
707  std::pair<int, int> x = pos.front();
708 
709  int mid = (x.first + x.second) / 2;
710  stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);
711 
712  if (!si_->isValid(test))
713  {
714  result = false;
715  break;
716  }
717 
718  pos.pop();
719 
720  if (x.first < mid)
721  pos.push(std::make_pair(x.first, mid - 1));
722  if (x.second > mid)
723  pos.push(std::make_pair(mid + 1, x.second));
724  }
725 
726  si_->freeState(test);
727  }
728 
729  if (result)
730  valid_++;
731  else
732  invalid_++;
733 
734  return result;
735 }
Complete description of a ReedsShepp path.
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
static const ReedsSheppPathSegmentType reedsSheppPathType[18][5]
Reeds-Shepp path types.
The definition of a state in SO(2)
Definition: SO2StateSpace.h:70
const T * as(void) const
Cast this instance to a desired type.
Definition: State.h:74
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.
virtual void interpolate(const State *from, const State *to, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
virtual bool checkMotion(const State *s1, const State *s2) const
Check if the path between two states (from s1 to s2) is valid. This function assumes s1 is valid...
ReedsSheppPathSegmentType
The Reeds-Shepp path segment types.
Definition of an abstract state.
Definition: State.h:50
The exception type for ompl.
Definition: Exception.h:47
An SE(2) state space where distance is measured by the length of Reeds-Shepp curves.