reeds_shepp_state_space.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Copyright (c) 2017 - for information on the respective copyright
3 * owner see the NOTICE file and/or the repository
4 *
5 * https://github.com/hbanzhaf/steering_functions.git
6 *
7 * Licensed under the Apache License, Version 2.0 (the "License");
8 * you may not use this file except in compliance with the License.
9 * You may obtain a copy of the License at
10 *
11 * http://www.apache.org/licenses/LICENSE-2.0
12 *
13 * Unless required by applicable law or agreed to in writing, software
14 * distributed under the License is distributed on an "AS IS" BASIS,
15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
16 * implied. See the License for the specific language governing
17 * permissions and limitations under the License.
18 
19 * This source code is derived from the Open Motion Planning Library
20 * (OMPL) V 1.3.1 (https://github.com/ompl/ompl).
21 * Copyright (c) 2010, Rice University, licensed under the BSD license,
22 * cf. 3rd-party-licenses.txt file in the root directory of this source
23 * tree.
24 **********************************************************************/
25 
26 /*********************************************************************
27 * Software License Agreement (BSD License)
28 *
29 * Copyright (c) 2010, Rice University
30 * All rights reserved.
31 *
32 * Redistribution and use in source and binary forms, with or without
33 * modification, are permitted provided that the following conditions
34 * are met:
35 *
36 * * Redistributions of source code must retain the above copyright
37 * notice, this list of conditions and the following disclaimer.
38 * * Redistributions in binary form must reproduce the above
39 * copyright notice, this list of conditions and the following
40 * disclaimer in the documentation and/or other materials provided
41 * with the distribution.
42 * * Neither the name of the Rice University nor the names of its
43 * contributors may be used to endorse or promote products derived
44 * from this software without specific prior written permission.
45 *
46 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
47 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
48 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
49 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
50 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
51 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
52 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
53 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
54 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
55 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
56 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
57 * POSSIBILITY OF SUCH DAMAGE.
58 *********************************************************************/
59 
60 #include <cmath>
61 
62 #include "steering_functions/reeds_shepp_state_space/reeds_shepp_state_space.hpp"
63 #include "steering_functions/utilities/utilities.hpp"
64 
65 using namespace std;
66 
67 namespace steering
68 {
69 
70 namespace /* helper */
71 {
72 // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
73 const double RS_EPS = 1e-6;
74 const double RS_ZERO = 10 * numeric_limits<double>::epsilon();
75 
76 inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
77 {
78  double delta = pify(u - v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
79  double t1 = atan2(eta * A - xi * B, xi * A + eta * B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
80  tau = (t2 < 0) ? pify(t1 + PI) : pify(t1);
81  omega = pify(tau - u + v - phi);
82 }
83 
84 // formula 8.1 in Reeds-Shepp paper
85 inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
86 {
87  polar(x - sin(phi), y - 1. + cos(phi), u, t);
88  if (t >= -RS_ZERO)
89  {
90  v = pify(phi - t);
91  if (v >= -RS_ZERO)
92  {
93  assert(fabs(u * cos(t) + sin(phi) - x) < RS_EPS);
94  assert(fabs(u * sin(t) - cos(phi) + 1 - y) < RS_EPS);
95  assert(fabs(pify(t + v - phi)) < RS_EPS);
96  return true;
97  }
98  }
99  return false;
100 }
101 // formula 8.2
102 inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
103 {
104  double t1, u1;
105  polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
106  u1 = u1 * u1;
107  if (u1 >= 4.)
108  {
109  double theta;
110  u = sqrt(u1 - 4.);
111  theta = atan2(2., u);
112  t = pify(t1 + theta);
113  v = pify(t - phi);
114  assert(fabs(2 * sin(t) + u * cos(t) - sin(phi) - x) < RS_EPS);
115  assert(fabs(-2 * cos(t) + u * sin(t) + cos(phi) + 1 - y) < RS_EPS);
116  assert(fabs(pify(t - v - phi)) < RS_EPS);
117  return t >= -RS_ZERO && v >= -RS_ZERO;
118  }
119  return false;
120 }
121 void CSC(double x, double y, double phi, Reeds_Shepp_State_Space::Reeds_Shepp_Path &path)
122 {
123  double t, u, v, Lmin = path.length(), L;
124  if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
125  {
126  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[14], t, u, v);
127  Lmin = L;
128  }
129  if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
130  {
131  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[14], -t, -u, -v);
132  Lmin = L;
133  }
134  if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
135  {
136  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[15], t, u, v);
137  Lmin = L;
138  }
139  if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
140  {
141  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[15], -t, -u, -v);
142  Lmin = L;
143  }
144  if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
145  {
146  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[12], t, u, v);
147  Lmin = L;
148  }
149  if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
150  {
151  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[12], -t, -u, -v);
152  Lmin = L;
153  }
154  if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
155  {
156  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[13], t, u, v);
157  Lmin = L;
158  }
159  if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
160  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[13], -t, -u, -v);
161 }
162 // formula 8.3 / 8.4 *** TYPO IN PAPER ***
163 inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
164 {
165  double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
166  polar(xi, eta, u1, theta);
167  if (u1 <= 4.)
168  {
169  u = -2. * asin(.25 * u1);
170  t = pify(theta + .5 * u + PI);
171  v = pify(phi - t + u);
172  assert(fabs(2 * (sin(t) - sin(t - u)) + sin(phi) - x) < RS_EPS);
173  assert(fabs(2 * (-cos(t) + cos(t - u)) - cos(phi) + 1 - y) < RS_EPS);
174  assert(fabs(pify(t - u + v - phi)) < RS_EPS);
175  return t >= -RS_ZERO && u <= RS_ZERO;
176  }
177  return false;
178 }
179 void CCC(double x, double y, double phi, Reeds_Shepp_State_Space::Reeds_Shepp_Path &path)
180 {
181  double t, u, v, Lmin = path.length(), L;
182  if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
183  {
184  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[0], t, u, v);
185  Lmin = L;
186  }
187  if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
188  {
189  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[0], -t, -u, -v);
190  Lmin = L;
191  }
192  if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
193  {
194  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[1], t, u, v);
195  Lmin = L;
196  }
197  if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
198  {
199  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[1], -t, -u, -v);
200  Lmin = L;
201  }
202 
203  // backwards
204  double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
205  if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
206  {
207  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[0], v, u, t);
208  Lmin = L;
209  }
210  if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
211  {
212  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[0], -v, -u, -t);
213  Lmin = L;
214  }
215  if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
216  {
217  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[1], v, u, t);
218  Lmin = L;
219  }
220  if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
221  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[1], -v, -u, -t);
222 }
223 // formula 8.7
224 inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
225 {
226  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi * xi + eta * eta));
227  if (rho <= 1.)
228  {
229  u = acos(rho);
230  tauOmega(u, -u, xi, eta, phi, t, v);
231  assert(fabs(2 * (sin(t) - sin(t - u) + sin(t - 2 * u)) - sin(phi) - x) < RS_EPS);
232  assert(fabs(2 * (-cos(t) + cos(t - u) - cos(t - 2 * u)) + cos(phi) + 1 - y) < RS_EPS);
233  assert(fabs(pify(t - 2 * u - v - phi)) < RS_EPS);
234  return t >= -RS_ZERO && v <= RS_ZERO;
235  }
236  return false;
237 }
238 // formula 8.8
239 inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
240 {
241  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi * xi - eta * eta) / 16.;
242  if (rho >= 0 && rho <= 1)
243  {
244  u = -acos(rho);
245  if (u >= -.5 * PI)
246  {
247  tauOmega(u, u, xi, eta, phi, t, v);
248  assert(fabs(4 * sin(t) - 2 * sin(t - u) - sin(phi) - x) < RS_EPS);
249  assert(fabs(-4 * cos(t) + 2 * cos(t - u) + cos(phi) + 1 - y) < RS_EPS);
250  assert(fabs(pify(t - v - phi)) < RS_EPS);
251  return t >= -RS_ZERO && v >= -RS_ZERO;
252  }
253  }
254  return false;
255 }
256 void CCCC(double x, double y, double phi, Reeds_Shepp_State_Space::Reeds_Shepp_Path &path)
257 {
258  double t, u, v, Lmin = path.length(), L;
259  if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
260  {
261  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[2], t, u, -u, v);
262  Lmin = L;
263  }
264  if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip
265  {
266  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[2], -t, -u, u, -v);
267  Lmin = L;
268  }
269  if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // reflect
270  {
271  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[3], t, u, -u, v);
272  Lmin = L;
273  }
274  if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip + reflect
275  {
276  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[3], -t, -u, u, -v);
277  Lmin = L;
278  }
279 
280  if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
281  {
282  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[2], t, u, u, v);
283  Lmin = L;
284  }
285  if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip
286  {
287  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[2], -t, -u, -u, -v);
288  Lmin = L;
289  }
290  if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // reflect
291  {
292  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[3], t, u, u, v);
293  Lmin = L;
294  }
295  if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip + reflect
296  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[3], -t, -u, -u, -v);
297 }
298 // formula 8.9
299 inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
300 {
301  double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
302  polar(xi, eta, rho, theta);
303  if (rho >= 2.)
304  {
305  double r = sqrt(rho * rho - 4.);
306  u = 2. - r;
307  t = pify(theta + atan2(r, -2.));
308  v = pify(phi - .5 * PI - t);
309  assert(fabs(2 * (sin(t) - cos(t)) - u * sin(t) + sin(phi) - x) < RS_EPS);
310  assert(fabs(-2 * (sin(t) + cos(t)) + u * cos(t) - cos(phi) + 1 - y) < RS_EPS);
311  assert(fabs(pify(t + PI / 2 + v - phi)) < RS_EPS);
312  return t >= -RS_ZERO && u <= RS_ZERO && v <= RS_ZERO;
313  }
314  return false;
315 }
316 // formula 8.10
317 inline bool LpRmSmRm(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(-eta, xi, rho, theta);
321  if (rho >= 2.)
322  {
323  t = theta;
324  u = 2. - rho;
325  v = pify(t + .5 * PI - phi);
326  assert(fabs(2 * sin(t) - cos(t - v) - u * sin(t) - x) < RS_EPS);
327  assert(fabs(-2 * cos(t) - sin(t - v) + u * cos(t) + 1 - y) < RS_EPS);
328  assert(fabs(pify(t + PI / 2 - v - phi)) < RS_EPS);
329  return t >= -RS_ZERO && u <= RS_ZERO && v <= RS_ZERO;
330  }
331  return false;
332 }
333 void CCSC(double x, double y, double phi, Reeds_Shepp_State_Space::Reeds_Shepp_Path &path)
334 {
335  double t, u, v, Lmin = path.length() - .5 * PI, L;
336  if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
337  {
338  path =
339  Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[4], t, -.5 * PI, u, v);
340  Lmin = L;
341  }
342  if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
343  {
344  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[4], -t, .5 * PI, -u,
345  -v);
346  Lmin = L;
347  }
348  if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
349  {
350  path =
351  Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[5], t, -.5 * PI, u, v);
352  Lmin = L;
353  }
354  if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
355  {
356  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[5], -t, .5 * PI, -u,
357  -v);
358  Lmin = L;
359  }
360 
361  if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
362  {
363  path =
364  Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[8], t, -.5 * PI, u, v);
365  Lmin = L;
366  }
367  if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
368  {
369  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[8], -t, .5 * PI, -u,
370  -v);
371  Lmin = L;
372  }
373  if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
374  {
375  path =
376  Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[9], t, -.5 * PI, u, v);
377  Lmin = L;
378  }
379  if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
380  {
381  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[9], -t, .5 * PI, -u,
382  -v);
383  Lmin = L;
384  }
385 
386  // backwards
387  double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
388  if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
389  {
390  path =
391  Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[6], 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
395  {
396  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[6], -v, -u, .5 * PI,
397  -t);
398  Lmin = L;
399  }
400  if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
401  {
402  path =
403  Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[7], v, u, -.5 * PI, t);
404  Lmin = L;
405  }
406  if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
407  {
408  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[7], -v, -u, .5 * PI,
409  -t);
410  Lmin = L;
411  }
412 
413  if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
414  {
415  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[10], v, u, -.5 * PI,
416  t);
417  Lmin = L;
418  }
419  if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
420  {
421  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[10], -v, -u,
422  .5 * PI, -t);
423  Lmin = L;
424  }
425  if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
426  {
427  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[11], v, u, -.5 * PI,
428  t);
429  Lmin = L;
430  }
431  if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
432  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[11], -v, -u,
433  .5 * PI, -t);
434 }
435 // formula 8.11 *** TYPO IN PAPER ***
436 inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
437 {
438  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
439  polar(xi, eta, rho, theta);
440  if (rho >= 2.)
441  {
442  u = 4. - sqrt(rho * rho - 4.);
443  if (u <= RS_ZERO)
444  {
445  t = pify(atan2((4 - u) * xi - 2 * eta, -2 * xi + (u - 4) * eta));
446  v = pify(t - phi);
447  assert(fabs(4 * sin(t) - 2 * cos(t) - u * sin(t) - sin(phi) - x) < RS_EPS);
448  assert(fabs(-4 * cos(t) - 2 * sin(t) + u * cos(t) + cos(phi) + 1 - y) < RS_EPS);
449  assert(fabs(pify(t - v - phi)) < RS_EPS);
450  return t >= -RS_ZERO && v >= -RS_ZERO;
451  }
452  }
453  return false;
454 }
455 void CCSCC(double x, double y, double phi, Reeds_Shepp_State_Space::Reeds_Shepp_Path &path)
456 {
457  double t, u, v, Lmin = path.length() - PI, L;
458  if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
459  {
460  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[16], 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
465  {
466  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[16], -t, .5 * PI,
467  -u, .5 * PI, -v);
468  Lmin = L;
469  }
470  if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
471  {
472  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[17], t, -.5 * PI, u,
473  -.5 * PI, v);
474  Lmin = L;
475  }
476  if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
477  path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[17], -t, .5 * PI,
478  -u, .5 * PI, -v);
479 }
480 
481 Reeds_Shepp_State_Space::Reeds_Shepp_Path reeds_shepp(double x, double y, double phi)
482 {
483  Reeds_Shepp_State_Space::Reeds_Shepp_Path path;
484  CSC(x, y, phi, path);
485  CCC(x, y, phi, path);
486  CCCC(x, y, phi, path);
487  CCSC(x, y, phi, path);
488  CCSCC(x, y, phi, path);
489  return path;
490 }
491 } // namespace helper
492 
493 const Reeds_Shepp_State_Space::Reeds_Shepp_Path_Segment_Type Reeds_Shepp_State_Space::reeds_shepp_path_type[18][5] = {
494  { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 0
495  { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP }, // 1
496  { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 2
497  { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP }, // 3
498  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 4
499  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 5
500  { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 6
501  { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 7
502  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 8
503  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 9
504  { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 10
505  { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 11
506  { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 12
507  { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 13
508  { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 14
509  { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 15
510  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT }, // 16
511  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT } // 17
512 };
513 
514 Reeds_Shepp_State_Space::Reeds_Shepp_Path::Reeds_Shepp_Path(const Reeds_Shepp_Path_Segment_Type *type, double t,
515  double u, double v, double w, double x)
516  : type_(type)
517 {
518  length_[0] = t;
519  length_[1] = u;
520  length_[2] = v;
521  length_[3] = w;
522  length_[4] = x;
523  total_length_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
524 }
525 
526 Reeds_Shepp_State_Space::Reeds_Shepp_Path Reeds_Shepp_State_Space::reeds_shepp(const State &state1,
527  const State &state2) const
528 {
529  double dx = state2.x - state1.x, dy = state2.y - state1.y, dth = state2.theta - state1.theta;
530  double c = cos(state1.theta), s = sin(state1.theta);
531  double x = c * dx + s * dy, y = -s * dx + c * dy;
532  return steering::reeds_shepp(x * kappa_, y * kappa_, dth);
533 }
534 
535 void Reeds_Shepp_State_Space::set_filter_parameters(const Motion_Noise &motion_noise,
536  const Measurement_Noise &measurement_noise,
537  const Controller &controller)
538 {
539  ekf_.set_parameters(motion_noise, measurement_noise, controller);
540 }
541 
542 double Reeds_Shepp_State_Space::get_distance(const State &state1, const State &state2) const
543 {
544  return kappa_inv_ * this->reeds_shepp(state1, state2).length();
545 }
546 
547 vector<Control> Reeds_Shepp_State_Space::get_controls(const State &state1, const State &state2) const
548 {
549  vector<Control> controls;
550  controls.reserve(5);
551  Reeds_Shepp_State_Space::Reeds_Shepp_Path path = this->reeds_shepp(state1, state2);
552  for (unsigned int i = 0; i < 5; ++i)
553  {
554  Control control;
555  switch (path.type_[i])
556  {
557  case RS_NOP:
558  return controls;
559  case RS_LEFT:
560  control.delta_s = kappa_inv_ * path.length_[i];
561  control.kappa = kappa_;
562  control.sigma = 0.0;
563  break;
564  case RS_RIGHT:
565  control.delta_s = kappa_inv_ * path.length_[i];
566  control.kappa = -kappa_;
567  control.sigma = 0.0;
568  break;
569  case RS_STRAIGHT:
570  control.delta_s = kappa_inv_ * path.length_[i];
571  control.kappa = 0.0;
572  control.sigma = 0.0;
573  break;
574  }
575  controls.push_back(control);
576  }
577  return controls;
578 }
579 
580 vector<State> Reeds_Shepp_State_Space::get_path(const State &state1, const State &state2) const
581 {
582  vector<Control> controls = get_controls(state1, state2);
583  return integrate(state1, controls);
584 }
585 
586 vector<State_With_Covariance> Reeds_Shepp_State_Space::get_path_with_covariance(const State_With_Covariance &state1,
587  const State &state2) const
588 {
589  vector<Control> controls = get_controls(state1.state, state2);
590  return integrate_with_covariance(state1, controls);
591 }
592 
593 vector<State> Reeds_Shepp_State_Space::integrate(const State &state, const vector<Control> &controls) const
594 {
595  vector<State> path;
596  State state_curr, state_next;
597  // reserve capacity of path
598  int n_states(0);
599  for (const auto &control : controls)
600  {
601  double abs_delta_s(fabs(control.delta_s));
602  n_states += ceil(abs_delta_s / discretization_);
603  }
604  path.reserve(n_states + 5);
605  // get first state
606  state_curr.x = state.x;
607  state_curr.y = state.y;
608  state_curr.theta = state.theta;
609 
610  for (const auto &control : controls)
611  {
612  double delta_s(control.delta_s);
613  double abs_delta_s(fabs(delta_s));
614  double s_seg(0.0);
615  double integration_step(0.0);
616  // push_back current state
617  state_curr.kappa = control.kappa;
618  state_curr.d = sgn(delta_s);
619  path.push_back(state_curr);
620 
621  for (int i = 0, n = ceil(abs_delta_s / discretization_); i < n; ++i)
622  {
623  // get integration step
624  s_seg += discretization_;
625  if (s_seg > abs_delta_s)
626  {
627  integration_step = discretization_ - (s_seg - abs_delta_s);
628  s_seg = abs_delta_s;
629  }
630  else
631  {
632  integration_step = discretization_;
633  }
634  state_next = integrate_ODE(state_curr, control, integration_step);
635  path.push_back(state_next);
636  state_curr = state_next;
637  }
638  }
639  return path;
640 }
641 
642 vector<State_With_Covariance> Reeds_Shepp_State_Space::integrate_with_covariance(const State_With_Covariance &state,
643  const vector<Control> &controls) const
644 {
645  vector<State_With_Covariance> path_with_covariance;
646  State_With_Covariance state_curr, state_pred, state_next;
647  // reserve capacity of path
648  int n_states(0);
649  for (const auto &control : controls)
650  {
651  double abs_delta_s(fabs(control.delta_s));
652  n_states += ceil(abs_delta_s / discretization_);
653  }
654  path_with_covariance.reserve(n_states + 5);
655  // get first state
656  state_curr.state.x = state.state.x;
657  state_curr.state.y = state.state.y;
658  state_curr.state.theta = state.state.theta;
659  for (int i = 0; i < 16; i++)
660  {
661  state_curr.Sigma[i] = state.Sigma[i];
662  state_curr.Lambda[i] = state.Lambda[i];
663  state_curr.covariance[i] = state.covariance[i];
664  }
665 
666  for (const auto &control : controls)
667  {
668  double delta_s(control.delta_s);
669  double abs_delta_s(fabs(delta_s));
670  double s_seg(0.0);
671  double integration_step(0.0);
672  // push_back current state
673  state_curr.state.kappa = control.kappa;
674  state_curr.state.d = sgn(delta_s);
675  path_with_covariance.push_back(state_curr);
676 
677  for (int i = 0, n = ceil(abs_delta_s / discretization_); i < n; ++i)
678  {
679  // get integration step
680  s_seg += discretization_;
681  if (s_seg > abs_delta_s)
682  {
683  integration_step = discretization_ - (s_seg - abs_delta_s);
684  s_seg = abs_delta_s;
685  }
686  else
687  {
688  integration_step = discretization_;
689  }
690  // predict
691  state_pred.state = integrate_ODE(state_curr.state, control, integration_step);
692  ekf_.predict(state_curr, control, integration_step, state_pred);
693  // update
694  state_next.state = state_pred.state;
695  ekf_.update(state_pred, state_next);
696 
697  path_with_covariance.push_back(state_next);
698  state_curr.state = state_next.state;
699  for (int i = 0; i < 16; i++)
700  {
701  state_curr.Sigma[i] = state_next.Sigma[i];
702  state_curr.Lambda[i] = state_next.Lambda[i];
703  state_curr.covariance[i] = state_next.covariance[i];
704  }
705  }
706  }
707  return path_with_covariance;
708 }
709 
710 State Reeds_Shepp_State_Space::interpolate(const State &state, const vector<Control> &controls, double t) const
711 {
712  State state_curr, state_next;
713  // get first state
714  state_curr.x = state.x;
715  state_curr.y = state.y;
716  state_curr.theta = state.theta;
717  // get arc length at t
718  double s_path(0.0);
719  double s_inter(0.0);
720  for (const auto &control : controls)
721  {
722  s_path += fabs(control.delta_s);
723  }
724  if (t <= 0.0)
725  return state_curr;
726  else if (t > 1.0)
727  s_inter = s_path;
728  else
729  s_inter = t * s_path;
730 
731  double s(0.0);
732  bool interpolated = false;
733  for (const auto &control : controls)
734  {
735  if (interpolated)
736  break;
737 
738  double delta_s(control.delta_s);
739  double abs_delta_s(fabs(delta_s));
740  double s_seg(0.0);
741  double integration_step(0.0);
742  // update current state
743  state_curr.kappa = control.kappa;
744  state_curr.d = sgn(delta_s);
745 
746  s += abs_delta_s;
747  if (s > s_inter)
748  {
749  abs_delta_s = abs_delta_s - (s - s_inter);
750  interpolated = true;
751  }
752 
753  for (int i = 0, n = ceil(abs_delta_s / discretization_); i < n; ++i)
754  {
755  // get integration step
756  s_seg += discretization_;
757  if (s_seg > abs_delta_s)
758  {
759  integration_step = discretization_ - (s_seg - abs_delta_s);
760  s_seg = abs_delta_s;
761  }
762  else
763  {
764  integration_step = discretization_;
765  }
766  state_next = integrate_ODE(state_curr, control, integration_step);
767  state_curr = state_next;
768  }
769  }
770  return state_curr;
771 }
772 
773 inline State Reeds_Shepp_State_Space::integrate_ODE(const State &state, const Control &control,
774  double integration_step) const
775 {
776  State state_next;
777  double d(sgn(control.delta_s));
778  if (fabs(state.kappa) > get_epsilon())
779  {
780  end_of_circular_arc(state.x, state.y, state.theta, state.kappa, d, integration_step, &state_next.x, &state_next.y,
781  &state_next.theta);
782  state_next.kappa = state.kappa;
783  state_next.d = d;
784  }
785  else
786  {
787  end_of_straight_line(state.x, state.y, state.theta, d, integration_step, &state_next.x, &state_next.y);
788  state_next.theta = state.theta;
789  state_next.kappa = state.kappa;
790  state_next.d = d;
791  }
792  return state_next;
793 }
794 
795 } // namespace steering
steering::Reeds_Shepp_State_Space::kappa_
double kappa_
Curvature.
Definition: reeds_shepp_state_space.hpp:280
steering::State_With_Covariance::state
State state
Expected state of the robot.
Definition: steering_functions.hpp:66
steering::State_With_Covariance::covariance
double covariance[16]
Covariance of the state given by Sigma + Lambda: (x_x x_y x_theta x_kappa y_x y_y y_theta y_kappa the...
Definition: steering_functions.hpp:78
steering::Reeds_Shepp_State_Space::kappa_inv_
double kappa_inv_
Inverse of curvature.
Definition: reeds_shepp_state_space.hpp:283
steering::Reeds_Shepp_State_Space::set_filter_parameters
void set_filter_parameters(const Motion_Noise &motion_noise, const Measurement_Noise &measurement_noise, const Controller &controller)
Sets the parameters required by the filter.
Definition: reeds_shepp_state_space.cpp:535
steering::State
Description of a kinematic car's state.
Definition: steering_functions.hpp:44
reeds_shepp_state_space.hpp
plot_states.path
path
Definition: plot_states.py:88
steering::Reeds_Shepp_State_Space::Reeds_Shepp_Path::length
double length() const
Definition: reeds_shepp_state_space.hpp:220
steering::Reeds_Shepp_State_Space::discretization_
double discretization_
Discretization of path.
Definition: reeds_shepp_state_space.hpp:286
steering::Control::sigma
double sigma
Sharpness (derivative of curvature with respect to arc length) of a segment.
Definition: steering_functions.hpp:91
s
XmlRpcServer s
steering::State::theta
double theta
Orientation of the robot.
Definition: steering_functions.hpp:70
steering::State_With_Covariance::Sigma
double Sigma[16]
Covariance of the state estimation due to motion and measurement noise.
Definition: steering_functions.hpp:69
steering::pify
double pify(double alpha)
Conversion of arbitrary angle given in [rad] to [-pi, pi[.
Definition: utilities.cpp:83
steering::Reeds_Shepp_State_Space::ekf_
EKF ekf_
Extended Kalman Filter for uncertainty propagation.
Definition: reeds_shepp_state_space.hpp:289
steering::Reeds_Shepp_State_Space::get_path_with_covariance
std::vector< State_With_Covariance > get_path_with_covariance(const State_With_Covariance &state1, const State &state2) const
Returns shortest path including covariances from state1 to state2 with curvature = kappa_.
Definition: reeds_shepp_state_space.cpp:586
steering::State::kappa
double kappa
Curvature at position (x,y)
Definition: steering_functions.hpp:73
steering::Reeds_Shepp_State_Space::RS_NOP
@ RS_NOP
Definition: reeds_shepp_state_space.hpp:257
steering::end_of_straight_line
void end_of_straight_line(double x_i, double y_i, double theta, double direction, double length, double *x_f, double *y_f)
Computation of the end point on a straight line x_i, y_i: initial configuration theta: angle of strai...
Definition: utilities.cpp:216
steering::Reeds_Shepp_State_Space::Reeds_Shepp_Path
Complete description of a ReedsShepp path.
Definition: reeds_shepp_state_space.hpp:212
steering::Reeds_Shepp_State_Space::RS_LEFT
@ RS_LEFT
Definition: reeds_shepp_state_space.hpp:258
steering::State::y
double y
Position in y of the robot.
Definition: steering_functions.hpp:67
steering::Reeds_Shepp_State_Space::Reeds_Shepp_Path_Segment_Type
Reeds_Shepp_Path_Segment_Type
The Reeds-Shepp path segment types.
Definition: reeds_shepp_state_space.hpp:200
steering::Control::kappa
double kappa
Curvature at the beginning of a segment.
Definition: steering_functions.hpp:88
steering::Control
Description of a path segment with its corresponding control inputs.
Definition: steering_functions.hpp:82
steering::EKF::update
void update(const State_With_Covariance &state_pred, State_With_Covariance &state_corr) const
Predicts the covariances.
Definition: ekf.cpp:251
steering::Reeds_Shepp_State_Space::get_path
std::vector< State > get_path(const State &state1, const State &state2) const
Returns shortest path from state1 to state2 with curvature = kappa_.
Definition: reeds_shepp_state_space.cpp:580
steering::EKF::set_parameters
void set_parameters(const Motion_Noise &motion_noise, const Measurement_Noise &measurement_noise, const Controller &_controller)
Sets the parameters required by the EKF.
Definition: ekf.cpp:46
steering::Reeds_Shepp_State_Space::interpolate
State interpolate(const State &state, const std::vector< Control > &controls, double t) const
Returns interpolated state at distance t in [0,1] (percent of total path length with curvature = kapp...
Definition: reeds_shepp_state_space.cpp:710
steering::Motion_Noise
Parameters of the motion noise model according to the book: Probabilistic Robotics,...
Definition: steering_functions.hpp:96
steering::end_of_circular_arc
void end_of_circular_arc(double x_i, double y_i, double theta_i, double kappa, double direction, double length, double *x_f, double *y_f, double *theta_f)
Computation of the end point on a circular arc x_i, y_i, theta_i: initial configuration kappa: curvat...
Definition: utilities.cpp:208
steering::State::x
double x
Position in x of the robot.
Definition: steering_functions.hpp:64
steering::EKF::predict
void predict(const State_With_Covariance &state, const Control &control, double integration_step, State_With_Covariance &state_pred) const
Predicts the covariances based on the paper: Rapidly-exploring random belief trees for motion plannin...
Definition: ekf.cpp:227
steering::epsilon
const double epsilon
Definition: utilities.hpp:41
steering::Reeds_Shepp_State_Space::Reeds_Shepp_Path::length_
double length_[5]
Definition: reeds_shepp_state_space.hpp:230
steering::Reeds_Shepp_State_Space::get_controls
std::vector< Control > get_controls(const State &state1, const State &state2) const
Returns controls of the shortest path from state1 to state2 with curvature = kappa_.
Definition: reeds_shepp_state_space.cpp:547
steering::Reeds_Shepp_State_Space::integrate_ODE
State integrate_ODE(const State &state, const Control &control, double integration_step) const
Returns integrated state given a start state, a control, and an integration step.
Definition: reeds_shepp_state_space.cpp:773
steering::Control::delta_s
double delta_s
Signed arc length of a segment.
Definition: steering_functions.hpp:85
steering::State_With_Covariance::Lambda
double Lambda[16]
Covariance of the state estimate due to the absence of measurements.
Definition: steering_functions.hpp:72
steering::get_epsilon
double get_epsilon()
Return value of epsilon.
Definition: utilities.cpp:57
steering::polar
void polar(double x, double y, double &r, double &theta)
Computation of a point's polar coordinates.
Definition: utilities.cpp:72
plot_states.d
d
Definition: plot_states.py:107
std
steering::Reeds_Shepp_State_Space::Reeds_Shepp_Path::total_length_
double total_length_
Definition: reeds_shepp_state_space.hpp:233
steering::State_With_Covariance
Description of a kinematic car's state with covariance.
Definition: steering_functions.hpp:63
steering::Controller
Parameters of the feedback controller.
Definition: steering_functions.hpp:121
steering
Definition: dubins_state_space.hpp:70
steering::Reeds_Shepp_State_Space::RS_STRAIGHT
@ RS_STRAIGHT
Definition: reeds_shepp_state_space.hpp:259
steering::sgn
double sgn(double x)
Return sign of a number.
Definition: utilities.cpp:62
PI
#define PI
Definition: utilities.hpp:31
steering::State::d
double d
Definition: steering_functions.hpp:76
steering::Reeds_Shepp_State_Space::RS_RIGHT
@ RS_RIGHT
Definition: reeds_shepp_state_space.hpp:260
steering::Reeds_Shepp_State_Space::integrate_with_covariance
std::vector< State_With_Covariance > integrate_with_covariance(const State_With_Covariance &state, const std::vector< Control > &controls) const
Returns integrated states including covariance given a start state and controls with curvature = kapp...
Definition: reeds_shepp_state_space.cpp:642
steering::Reeds_Shepp_State_Space::reeds_shepp
Reeds_Shepp_Path reeds_shepp(const State &state1, const State &state2) const
Returns type and length of segments of path from state1 to state2 with curvature = 1....
Definition: reeds_shepp_state_space.cpp:526
t
geometry_msgs::TransformStamped t
utilities.hpp
steering::Reeds_Shepp_State_Space::get_distance
double get_distance(const State &state1, const State &state2) const
Returns shortest path length from state1 to state2 with curvature = kappa_.
Definition: reeds_shepp_state_space.cpp:542
steering::Measurement_Noise
Parameters of the measurement noise.
Definition: steering_functions.hpp:108
steering::Reeds_Shepp_State_Space::integrate
std::vector< State > integrate(const State &state, const std::vector< Control > &controls) const
Returns integrated states given a start state and controls with curvature = kappa_.
Definition: reeds_shepp_state_space.cpp:593

steering_functions
Author(s): Holger Banzhaf
autogenerated on Mon Dec 11 2023 03:27:43