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