Loading...
Searching...
No Matches
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
43using namespace ompl::base;
44
45namespace
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::PathType &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::PathType(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::PathType(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::PathType(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::PathType(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::PathType(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::PathType(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::PathType(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::PathType(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::PathType &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::PathType(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::PathType(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::PathType(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::PathType(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::PathType(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::PathType(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::PathType(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::PathType(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::PathType &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::PathType(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::PathType(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::PathType(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::PathType(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::PathType(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::PathType(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::PathType(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::PathType(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::PathType &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::PathType(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 = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5 * pi, -u, -v);
338 Lmin = L;
339 }
340 if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
341 {
342 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5 * pi, u, v);
343 Lmin = L;
344 }
345 if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
346 {
347 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5 * pi, -u, -v);
348 Lmin = L;
349 }
350
351 if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
352 {
353 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5 * pi, u, v);
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 = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5 * pi, -u, -v);
359 Lmin = L;
360 }
361 if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
362 {
363 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5 * pi, u, v);
364 Lmin = L;
365 }
366 if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
367 {
368 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5 * pi, -u, -v);
369 Lmin = L;
370 }
371
372 // backwards
373 double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
374 if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
375 {
376 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5 * pi, t);
377 Lmin = L;
378 }
379 if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
380 {
381 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5 * pi, -t);
382 Lmin = L;
383 }
384 if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
385 {
386 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[7], 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))) // timeflip + reflect
390 {
391 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5 * pi, -t);
392 Lmin = L;
393 }
394
395 if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
396 {
397 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5 * pi, t);
398 Lmin = L;
399 }
400 if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
401 {
402 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5 * pi, -t);
403 Lmin = L;
404 }
405 if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
406 {
407 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5 * pi, t);
408 Lmin = L;
409 }
410 if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
411 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5 * pi, -t);
412 }
413 // formula 8.11 *** TYPO IN PAPER ***
414 inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
415 {
416 double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
417 polar(xi, eta, rho, theta);
418 if (rho >= 2.)
419 {
420 u = 4. - sqrt(rho * rho - 4.);
421 if (u <= ZERO)
422 {
423 t = mod2pi(atan2((4 - u) * xi - 2 * eta, -2 * xi + (u - 4) * eta));
424 v = mod2pi(t - phi);
425 assert(fabs(4 * sin(t) - 2 * cos(t) - u * sin(t) - sin(phi) - x) < RS_EPS);
426 assert(fabs(-4 * cos(t) - 2 * sin(t) + u * cos(t) + cos(phi) + 1 - y) < RS_EPS);
427 assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
428 return t >= -ZERO && v >= -ZERO;
429 }
430 }
431 return false;
432 }
433 void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::PathType &path)
434 {
435 double t, u, v, Lmin = path.length() - pi, L;
436 if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
437 {
438 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5 * pi, u,
439 -.5 * pi, v);
440 Lmin = L;
441 }
442 if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
443 {
444 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5 * pi, -u,
445 .5 * pi, -v);
446 Lmin = L;
447 }
448 if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
449 {
450 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5 * pi, u,
451 -.5 * pi, v);
452 Lmin = L;
453 }
454 if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
455 path = ReedsSheppStateSpace::PathType(ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5 * pi, -u,
456 .5 * pi, -v);
457 }
458
459 ReedsSheppStateSpace::PathType getPath(double x, double y, double phi)
460 {
461 ReedsSheppStateSpace::PathType path;
462 CSC(x, y, phi, path);
463 CCC(x, y, phi, path);
464 CCCC(x, y, phi, path);
465 CCSC(x, y, phi, path);
466 CCSCC(x, y, phi, path);
467 return path;
468 }
469} // namespace
470
471const ompl::base::ReedsSheppStateSpace::ReedsSheppPathSegmentType
472 ompl::base::ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
473 {RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 0
474 {RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP}, // 1
475 {RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 2
476 {RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP}, // 3
477 {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP}, // 4
478 {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP}, // 5
479 {RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP}, // 6
480 {RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 7
481 {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP}, // 8
482 {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP}, // 9
483 {RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP}, // 10
484 {RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 11
485 {RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP}, // 12
486 {RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 13
487 {RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 14
488 {RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP}, // 15
489 {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT}, // 16
490 {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT} // 17
491};
492
493ompl::base::ReedsSheppStateSpace::PathType::PathType(const ReedsSheppPathSegmentType *type, double t, double u,
494 double v, double w, double x)
495 : type_(type)
496{
497 length_[0] = t;
498 length_[1] = u;
499 length_[2] = v;
500 length_[3] = w;
501 length_[4] = x;
502 totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
503}
504
505double ompl::base::ReedsSheppStateSpace::distance(const State *state1, const State *state2) const
506{
507 return rho_ * getPath(state1, state2).length();
508}
509
510void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t,
511 State *state) const
512{
513 bool firstTime = true;
514 PathType path;
515 interpolate(from, to, t, firstTime, path, state);
516}
517
518void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t, bool &firstTime,
519 PathType &path, State *state) const
520{
521 if (firstTime)
522 {
523 if (t >= 1.)
524 {
525 if (to != state)
526 copyState(state, to);
527 return;
528 }
529 if (t <= 0.)
530 {
531 if (from != state)
532 copyState(state, from);
533 return;
534 }
535 path = getPath(from, to);
536 firstTime = false;
537 }
538 interpolate(from, path, t, state);
539}
540
541void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const PathType &path, double t,
542 State *state) const
543{
544 auto *s = allocState()->as<StateType>();
545 double seg = t * path.length(), phi, v;
546
547 s->setXY(0., 0.);
548 s->setYaw(from->as<StateType>()->getYaw());
549 for (unsigned int i = 0; i < 5 && seg > 0; ++i)
550 {
551 if (path.length_[i] < 0)
552 {
553 v = std::max(-seg, path.length_[i]);
554 seg += v;
555 }
556 else
557 {
558 v = std::min(seg, path.length_[i]);
559 seg -= v;
560 }
561 phi = s->getYaw();
562 switch (path.type_[i])
563 {
564 case RS_LEFT:
565 s->setXY(s->getX() + sin(phi + v) - sin(phi), s->getY() - cos(phi + v) + cos(phi));
566 s->setYaw(phi + v);
567 break;
568 case RS_RIGHT:
569 s->setXY(s->getX() - sin(phi - v) + sin(phi), s->getY() + cos(phi - v) - cos(phi));
570 s->setYaw(phi - v);
571 break;
572 case RS_STRAIGHT:
573 s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
574 break;
575 case RS_NOP:
576 break;
577 }
578 }
579 state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
580 state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
581 getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
582 state->as<StateType>()->setYaw(s->getYaw());
583 freeState(s);
584}
585
586ompl::base::ReedsSheppStateSpace::PathType ompl::base::ReedsSheppStateSpace::getPath(const State *state1,
587 const State *state2) const
588{
589 const auto *s1 = static_cast<const StateType *>(state1);
590 const auto *s2 = static_cast<const StateType *>(state2);
591 double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
592 double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
593 double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1);
594 double x = c * dx + s * dy, y = -s * dx + c * dy, phi = th2 - th1;
595 return ::getPath(x / rho_, y / rho_, phi);
596}
ompl::base::ReedsSheppStateSpace::PathType
Complete description of a ReedsShepp path.
Definition ReedsSheppStateSpace.h:79
ompl::base::ReedsSheppStateSpace::ReedsSheppPathSegmentType
ReedsSheppPathSegmentType
The Reeds-Shepp path segment types.
Definition ReedsSheppStateSpace.h:69
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:510
ompl::base::ReedsSheppStateSpace::getPath
PathType getPath(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:586
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:505
ompl::base::ReedsSheppStateSpace::reedsSheppPathType
static const ReedsSheppPathSegmentType reedsSheppPathType[18][5]
Reeds-Shepp path types.
Definition ReedsSheppStateSpace.h:472
ompl::base::SE2StateSpace::StateType
A state in SE(2): (x, y, yaw).
Definition SE2StateSpace.h:54
ompl::base::SE2StateSpace::StateType::getX
double getX() const
Get the X component of the state.
Definition SE2StateSpace.h:59
ompl::base::SO2StateSpace::StateType
The definition of a state in SO(2).
Definition SO2StateSpace.h:68
ompl::base::State
Definition of an abstract state.
Definition State.h:50
ompl::base::State::as
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
ompl::base
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition ConstrainedSpaceInformation.h:55