DubinsStateSpace.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/DubinsStateSpace.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  const double twopi = 2. * boost::math::constants::pi<double>();
48  const double DUBINS_EPS = 1e-6;
49  const double DUBINS_ZERO = -1e-7;
50 
51  inline double mod2pi(double x)
52  {
53  if (x < 0 && x > DUBINS_ZERO)
54  return 0;
55  double xm = x - twopi * floor(x / twopi);
56  if (twopi - xm < .5 * DUBINS_EPS) xm = 0.;
57  return xm;
58  }
59 
60  DubinsStateSpace::DubinsPath dubinsLSL(double d, double alpha, double beta)
61  {
62  double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
63  double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sa - sb));
64  if (tmp >= DUBINS_ZERO)
65  {
66  double theta = atan2(cb - ca, d + sa - sb);
67  double t = mod2pi(-alpha + theta);
68  double p = sqrt(std::max(tmp, 0.));
69  double q = mod2pi(beta - theta);
70  assert(fabs(p * cos(alpha + t) - sa + sb - d) < 2 * DUBINS_EPS);
71  assert(fabs(p * sin(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
72  assert(mod2pi(alpha + t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
73  return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[0], t, p, q);
74  }
75  return {};
76  }
77 
78  DubinsStateSpace::DubinsPath dubinsRSR(double d, double alpha, double beta)
79  {
80  double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
81  double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sb - sa));
82  if (tmp >= DUBINS_ZERO)
83  {
84  double theta = atan2(ca - cb, d - sa + sb);
85  double t = mod2pi(alpha - theta);
86  double p = sqrt(std::max(tmp, 0.));
87  double q = mod2pi(-beta + theta);
88  assert(fabs(p * cos(alpha - t) + sa - sb - d) < 2* DUBINS_EPS);
89  assert(fabs(p * sin(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
90  assert(mod2pi(alpha - t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
91  return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[1], t, p, q);
92  }
93  return {};
94  }
95 
96  DubinsStateSpace::DubinsPath dubinsRSL(double d, double alpha, double beta)
97  {
98  double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
99  double tmp = d * d - 2. + 2. * (ca * cb + sa * sb - d * (sa + sb));
100  if (tmp >= DUBINS_ZERO)
101  {
102  double p = sqrt(std::max(tmp, 0.));
103  double theta = atan2(ca + cb, d - sa - sb) - atan2(2., p);
104  double t = mod2pi(alpha - theta);
105  double q = mod2pi(beta - theta);
106  assert(fabs(p * cos(alpha - t) - 2. * sin(alpha - t) + sa + sb - d) < 2 * DUBINS_EPS);
107  assert(fabs(p * sin(alpha - t) + 2. * cos(alpha - t) - ca - cb) < 2 * DUBINS_EPS);
108  assert(mod2pi(alpha - t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
109  return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[2], t, p, q);
110  }
111  return {};
112  }
113 
114  DubinsStateSpace::DubinsPath dubinsLSR(double d, double alpha, double beta)
115  {
116  double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
117  double tmp = -2. + d * d + 2. * (ca * cb + sa * sb + d * (sa + sb));
118  if (tmp >= DUBINS_ZERO)
119  {
120  double p = sqrt(std::max(tmp, 0.));
121  double theta = atan2(-ca - cb, d + sa + sb) - atan2(-2., p);
122  double t = mod2pi(-alpha + theta);
123  double q = mod2pi(-beta + theta);
124  assert(fabs(p * cos(alpha + t) + 2. * sin(alpha + t) - sa - sb - d) < 2 * DUBINS_EPS);
125  assert(fabs(p * sin(alpha + t) - 2. * cos(alpha + t) + ca + cb) < 2 * DUBINS_EPS);
126  assert(mod2pi(alpha + t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
127  return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[3], t, p, q);
128  }
129  return {};
130  }
131 
132  DubinsStateSpace::DubinsPath dubinsRLR(double d, double alpha, double beta)
133  {
134  double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
135  double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb + d * (sa - sb)));
136  if (fabs(tmp) < 1.)
137  {
138  double p = twopi - acos(tmp);
139  double theta = atan2(ca - cb, d - sa + sb);
140  double t = mod2pi(alpha - theta + .5 * p);
141  double q = mod2pi(alpha - beta - t + p);
142  assert(fabs(2. * sin(alpha - t + p) - 2. * sin(alpha - t) - d + sa - sb) < 2 * DUBINS_EPS);
143  assert(fabs(-2. * cos(alpha - t + p) + 2. * cos(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
144  assert(mod2pi(alpha - t + p - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
145  return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[4], t, p, q);
146  }
147  return {};
148  }
149 
150  DubinsStateSpace::DubinsPath dubinsLRL(double d, double alpha, double beta)
151  {
152  double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
153  double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb - d * (sa - sb)));
154  if (fabs(tmp) < 1.)
155  {
156  double p = twopi - acos(tmp);
157  double theta = atan2(-ca + cb, d + sa - sb);
158  double t = mod2pi(-alpha + theta + .5 * p);
159  double q = mod2pi(beta - alpha - t + p);
160  assert(fabs(-2. * sin(alpha + t - p) + 2. * sin(alpha + t) - d - sa + sb) < 2 * DUBINS_EPS);
161  assert(fabs(2. * cos(alpha + t - p) - 2. * cos(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
162  assert(mod2pi(alpha + t - p + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
163  return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[5], t, p, q);
164  }
165  return {};
166  }
167 
168  DubinsStateSpace::DubinsPath dubins(double d, double alpha, double beta)
169  {
170  if (d < DUBINS_EPS && fabs(alpha - beta) < DUBINS_EPS)
171  return {DubinsStateSpace::dubinsPathType[0], 0, d, 0};
172 
173  DubinsStateSpace::DubinsPath path(dubinsLSL(d, alpha, beta)), tmp(dubinsRSR(d, alpha, beta));
174  double len, minLength = path.length();
175 
176  if ((len = tmp.length()) < minLength)
177  {
178  minLength = len;
179  path = tmp;
180  }
181  tmp = dubinsRSL(d, alpha, beta);
182  if ((len = tmp.length()) < minLength)
183  {
184  minLength = len;
185  path = tmp;
186  }
187  tmp = dubinsLSR(d, alpha, beta);
188  if ((len = tmp.length()) < minLength)
189  {
190  minLength = len;
191  path = tmp;
192  }
193  tmp = dubinsRLR(d, alpha, beta);
194  if ((len = tmp.length()) < minLength)
195  {
196  minLength = len;
197  path = tmp;
198  }
199  tmp = dubinsLRL(d, alpha, beta);
200  if ((len = tmp.length()) < minLength)
201  path = tmp;
202  return path;
203  }
204 }
205 
206 const ompl::base::DubinsStateSpace::DubinsPathSegmentType ompl::base::DubinsStateSpace::dubinsPathType[6][3] = {
207  {DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_LEFT},
208  {DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_RIGHT},
209  {DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_LEFT},
210  {DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_RIGHT},
211  {DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT},
212  {DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT}};
213 
214 double ompl::base::DubinsStateSpace::distance(const State *state1, const State *state2) const
215 {
216  if (isSymmetric_)
217  return rho_ * std::min(dubins(state1, state2).length(), dubins(state2, state1).length());
218  return rho_ * dubins(state1, state2).length();
219 }
220 
221 void ompl::base::DubinsStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
222 {
223  bool firstTime = true;
224  DubinsPath path;
225  interpolate(from, to, t, firstTime, path, state);
226 }
227 
228 void ompl::base::DubinsStateSpace::interpolate(const State *from, const State *to, const double t, bool &firstTime,
229  DubinsPath &path, State *state) const
230 {
231  if (firstTime)
232  {
233  if (t >= 1.)
234  {
235  if (to != state)
236  copyState(state, to);
237  return;
238  }
239  if (t <= 0.)
240  {
241  if (from != state)
242  copyState(state, from);
243  return;
244  }
245 
246  path = dubins(from, to);
247  if (isSymmetric_)
248  {
249  DubinsPath path2(dubins(to, from));
250  if (path2.length() < path.length())
251  {
252  path2.reverse_ = true;
253  path = path2;
254  }
255  }
256  firstTime = false;
257  }
258  interpolate(from, path, t, state);
259 }
260 
261 void ompl::base::DubinsStateSpace::interpolate(const State *from, const DubinsPath &path, double t, State *state) const
262 {
263  auto *s = allocState()->as<StateType>();
264  double seg = t * path.length(), phi, v;
265 
266  s->setXY(0., 0.);
267  s->setYaw(from->as<StateType>()->getYaw());
268  if (!path.reverse_)
269  {
270  for (unsigned int i = 0; i < 3 && seg > 0; ++i)
271  {
272  v = std::min(seg, path.length_[i]);
273  phi = s->getYaw();
274  seg -= v;
275  switch (path.type_[i])
276  {
277  case DUBINS_LEFT:
278  s->setXY(s->getX() + sin(phi + v) - sin(phi), s->getY() - cos(phi + v) + cos(phi));
279  s->setYaw(phi + v);
280  break;
281  case DUBINS_RIGHT:
282  s->setXY(s->getX() - sin(phi - v) + sin(phi), s->getY() + cos(phi - v) - cos(phi));
283  s->setYaw(phi - v);
284  break;
285  case DUBINS_STRAIGHT:
286  s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
287  break;
288  }
289  }
290  }
291  else
292  {
293  for (unsigned int i = 0; i < 3 && seg > 0; ++i)
294  {
295  v = std::min(seg, path.length_[2 - i]);
296  phi = s->getYaw();
297  seg -= v;
298  switch (path.type_[2 - i])
299  {
300  case DUBINS_LEFT:
301  s->setXY(s->getX() + sin(phi - v) - sin(phi), s->getY() - cos(phi - v) + cos(phi));
302  s->setYaw(phi - v);
303  break;
304  case DUBINS_RIGHT:
305  s->setXY(s->getX() - sin(phi + v) + sin(phi), s->getY() + cos(phi + v) - cos(phi));
306  s->setYaw(phi + v);
307  break;
308  case DUBINS_STRAIGHT:
309  s->setXY(s->getX() - v * cos(phi), s->getY() - v * sin(phi));
310  break;
311  }
312  }
313  }
314  state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
315  state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
316  getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
317  state->as<StateType>()->setYaw(s->getYaw());
318  freeState(s);
319 }
320 
321 ompl::base::DubinsStateSpace::DubinsPath ompl::base::DubinsStateSpace::dubins(const State *state1,
322  const State *state2) const
323 {
324  const auto *s1 = static_cast<const StateType *>(state1);
325  const auto *s2 = static_cast<const StateType *>(state2);
326  double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
327  double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
328  double dx = x2 - x1, dy = y2 - y1, d = sqrt(dx * dx + dy * dy) / rho_, th = atan2(dy, dx);
329  double alpha = mod2pi(th1 - th), beta = mod2pi(th2 - th);
330  return ::dubins(d, alpha, beta);
331 }
332 
333 void ompl::base::DubinsMotionValidator::defaultSettings()
334 {
335  stateSpace_ = dynamic_cast<DubinsStateSpace *>(si_->getStateSpace().get());
336  if (stateSpace_ == nullptr)
337  throw Exception("No state space for motion validator");
338 }
339 
340 bool ompl::base::DubinsMotionValidator::checkMotion(const State *s1, const State *s2,
341  std::pair<State *, double> &lastValid) const
342 {
343  /* assume motion starts in a valid configuration so s1 is valid */
344 
345  bool result = true, firstTime = true;
346  DubinsStateSpace::DubinsPath path;
347  int nd = stateSpace_->validSegmentCount(s1, s2);
348 
349  if (nd > 1)
350  {
351  /* temporary storage for the checked state */
352  State *test = si_->allocState();
353 
354  for (int j = 1; j < nd; ++j)
355  {
356  stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
357  if (!si_->isValid(test))
358  {
359  lastValid.second = (double)(j - 1) / (double)nd;
360  if (lastValid.first != nullptr)
361  stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
362  result = false;
363  break;
364  }
365  }
366  si_->freeState(test);
367  }
368 
369  if (result)
370  if (!si_->isValid(s2))
371  {
372  lastValid.second = (double)(nd - 1) / (double)nd;
373  if (lastValid.first != nullptr)
374  stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
375  result = false;
376  }
377 
378  if (result)
379  valid_++;
380  else
381  invalid_++;
382 
383  return result;
384 }
385 
386 bool ompl::base::DubinsMotionValidator::checkMotion(const State *s1, const State *s2) const
387 {
388  /* assume motion starts in a valid configuration so s1 is valid */
389  if (!si_->isValid(s2))
390  return false;
391 
392  bool result = true, firstTime = true;
393  DubinsStateSpace::DubinsPath path;
394  int nd = stateSpace_->validSegmentCount(s1, s2);
395 
396  /* initialize the queue of test positions */
397  std::queue<std::pair<int, int>> pos;
398  if (nd >= 2)
399  {
400  pos.emplace(1, nd - 1);
401 
402  /* temporary storage for the checked state */
403  State *test = si_->allocState();
404 
405  /* repeatedly subdivide the path segment in the middle (and check the middle) */
406  while (!pos.empty())
407  {
408  std::pair<int, int> x = pos.front();
409 
410  int mid = (x.first + x.second) / 2;
411  stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);
412 
413  if (!si_->isValid(test))
414  {
415  result = false;
416  break;
417  }
418 
419  pos.pop();
420 
421  if (x.first < mid)
422  pos.emplace(x.first, mid - 1);
423  if (x.second > mid)
424  pos.emplace(mid + 1, x.second);
425  }
426 
427  si_->freeState(test);
428  }
429 
430  if (result)
431  valid_++;
432  else
433  invalid_++;
434 
435  return result;
436 }
ompl::base::DubinsStateSpace::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: DubinsStateSpace.cpp:221
ompl::base::State
Definition of an abstract state.
Definition: State.h:113
ompl::base
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: ConstrainedSpaceInformation.h:86
ompl::base::DubinsStateSpace::dubins
DubinsPath dubins(const State *state1, const State *state2) const
Return a shortest Dubins path from SE(2) state state1 to SE(2) state state2.
Definition: DubinsStateSpace.cpp:321
ompl::base::State::as
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
ompl::base::DubinsStateSpace::dubinsPathType
static const DubinsPathSegmentType dubinsPathType[6][3]
Dubins path types.
Definition: DubinsStateSpace.h:171
ompl::base::DubinsMotionValidator::checkMotion
bool checkMotion(const State *s1, const State *s2) const override
Check if the path between two states (from s1 to s2) is valid. This function assumes s1 is valid.
Definition: DubinsStateSpace.cpp:386
ompl::base::DubinsStateSpace
An SE(2) state space where distance is measured by the length of Dubins curves.
Definition: DubinsStateSpace.h:128
ompl::base::SO2StateSpace::StateType
The definition of a state in SO(2)
Definition: SO2StateSpace.h:131
ompl::base::DubinsStateSpace::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: DubinsStateSpace.cpp:214
ompl::base::DubinsStateSpace::DubinsPath
Complete description of a Dubins path.
Definition: DubinsStateSpace.h:173
ompl::Exception
The exception type for ompl.
Definition: Exception.h:78
ompl::base::DubinsStateSpace::DubinsPathSegmentType
DubinsPathSegmentType
The Dubins path segment type.
Definition: DubinsStateSpace.h:164