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  else
217  return rho_ * dubins(state1, state2).length();
218 }
219 
220 void ompl::base::DubinsStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
221 {
222  bool firstTime = true;
223  DubinsPath path;
224  interpolate(from, to, t, firstTime, path, state);
225 }
226 
227 void ompl::base::DubinsStateSpace::interpolate(const State *from, const State *to, const double t, bool &firstTime,
228  DubinsPath &path, State *state) const
229 {
230  if (firstTime)
231  {
232  if (t >= 1.)
233  {
234  if (to != state)
235  copyState(state, to);
236  return;
237  }
238  if (t <= 0.)
239  {
240  if (from != state)
241  copyState(state, from);
242  return;
243  }
244 
245  path = dubins(from, to);
246  if (isSymmetric_)
247  {
248  DubinsPath path2(dubins(to, from));
249  if (path2.length() < path.length())
250  {
251  path2.reverse_ = true;
252  path = path2;
253  }
254  }
255  firstTime = false;
256  }
257  interpolate(from, path, t, state);
258 }
259 
260 void ompl::base::DubinsStateSpace::interpolate(const State *from, const DubinsPath &path, double t, State *state) const
261 {
262  StateType *s = allocState()->as<StateType>();
263  double seg = t * path.length(), phi, v;
264 
265  s->setXY(0., 0.);
266  s->setYaw(from->as<StateType>()->getYaw());
267  if (!path.reverse_)
268  {
269  for (unsigned int i = 0; i < 3 && seg > 0; ++i)
270  {
271  v = std::min(seg, path.length_[i]);
272  phi = s->getYaw();
273  seg -= v;
274  switch (path.type_[i])
275  {
276  case DUBINS_LEFT:
277  s->setXY(s->getX() + sin(phi + v) - sin(phi), s->getY() - cos(phi + v) + cos(phi));
278  s->setYaw(phi + v);
279  break;
280  case DUBINS_RIGHT:
281  s->setXY(s->getX() - sin(phi - v) + sin(phi), s->getY() + cos(phi - v) - cos(phi));
282  s->setYaw(phi - v);
283  break;
284  case DUBINS_STRAIGHT:
285  s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
286  break;
287  }
288  }
289  }
290  else
291  {
292  for (unsigned int i = 0; i < 3 && seg > 0; ++i)
293  {
294  v = std::min(seg, path.length_[2 - i]);
295  phi = s->getYaw();
296  seg -= v;
297  switch (path.type_[2 - i])
298  {
299  case DUBINS_LEFT:
300  s->setXY(s->getX() + sin(phi - v) - sin(phi), s->getY() - cos(phi - v) + cos(phi));
301  s->setYaw(phi - v);
302  break;
303  case DUBINS_RIGHT:
304  s->setXY(s->getX() - sin(phi + v) + sin(phi), s->getY() + cos(phi + v) - cos(phi));
305  s->setYaw(phi + v);
306  break;
307  case DUBINS_STRAIGHT:
308  s->setXY(s->getX() - v * cos(phi), s->getY() - v * sin(phi));
309  break;
310  }
311  }
312  }
313  state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
314  state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
315  getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
316  state->as<StateType>()->setYaw(s->getYaw());
317  freeState(s);
318 }
319 
321  const State *state2) const
322 {
323  const StateType *s1 = static_cast<const StateType *>(state1);
324  const StateType *s2 = static_cast<const StateType *>(state2);
325  double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
326  double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
327  double dx = x2 - x1, dy = y2 - y1, d = sqrt(dx * dx + dy * dy) / rho_, th = atan2(dy, dx);
328  double alpha = mod2pi(th1 - th), beta = mod2pi(th2 - th);
329  return ::dubins(d, alpha, beta);
330 }
331 
332 void ompl::base::DubinsMotionValidator::defaultSettings()
333 {
334  stateSpace_ = dynamic_cast<DubinsStateSpace *>(si_->getStateSpace().get());
335  if (!stateSpace_)
336  throw Exception("No state space for motion validator");
337 }
338 
340  std::pair<State *, double> &lastValid) const
341 {
342  /* assume motion starts in a valid configuration so s1 is valid */
343 
344  bool result = true, firstTime = true;
346  int nd = stateSpace_->validSegmentCount(s1, s2);
347 
348  if (nd > 1)
349  {
350  /* temporary storage for the checked state */
351  State *test = si_->allocState();
352 
353  for (int j = 1; j < nd; ++j)
354  {
355  stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
356  if (!si_->isValid(test))
357  {
358  lastValid.second = (double)(j - 1) / (double)nd;
359  if (lastValid.first)
360  stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
361  result = false;
362  break;
363  }
364  }
365  si_->freeState(test);
366  }
367 
368  if (result)
369  if (!si_->isValid(s2))
370  {
371  lastValid.second = (double)(nd - 1) / (double)nd;
372  if (lastValid.first)
373  stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
374  result = false;
375  }
376 
377  if (result)
378  valid_++;
379  else
380  invalid_++;
381 
382  return result;
383 }
384 
386 {
387  /* assume motion starts in a valid configuration so s1 is valid */
388  if (!si_->isValid(s2))
389  return false;
390 
391  bool result = true, firstTime = true;
393  int nd = stateSpace_->validSegmentCount(s1, s2);
394 
395  /* initialize the queue of test positions */
396  std::queue<std::pair<int, int>> pos;
397  if (nd >= 2)
398  {
399  pos.push(std::make_pair(1, nd - 1));
400 
401  /* temporary storage for the checked state */
402  State *test = si_->allocState();
403 
404  /* repeatedly subdivide the path segment in the middle (and check the middle) */
405  while (!pos.empty())
406  {
407  std::pair<int, int> x = pos.front();
408 
409  int mid = (x.first + x.second) / 2;
410  stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);
411 
412  if (!si_->isValid(test))
413  {
414  result = false;
415  break;
416  }
417 
418  pos.pop();
419 
420  if (x.first < mid)
421  pos.push(std::make_pair(x.first, mid - 1));
422  if (x.second > mid)
423  pos.push(std::make_pair(mid + 1, x.second));
424  }
425 
426  si_->freeState(test);
427  }
428 
429  if (result)
430  valid_++;
431  else
432  invalid_++;
433 
434  return result;
435 }
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, const 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.