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);
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);
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);
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);
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);
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);
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 
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 
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 
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;
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 
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;
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 }
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:113
This namespace contains sampling based planning routines shared by both planning under geometric cons...
DubinsPath dubins(const State *state1, const State *state2) const
Return a shortest Dubins path from SE(2) state state1 to SE(2) state state2.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
static const DubinsPathSegmentType dubinsPathType[6][3]
Dubins path types.
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.
An SE(2) state space where distance is measured by the length of Dubins curves.
The definition of a state in SO(2)
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...
Complete description of a Dubins path.
The exception type for ompl.
Definition: Exception.h:78
DubinsPathSegmentType
The Dubins path segment type.