All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 
44 using namespace ompl::base;
45 
46 namespace
47 {
48  const double twopi = 2. * boost::math::constants::pi<double>();
49  const double DUBINS_EPS = 1e-6;
50  const double DUBINS_ZERO = -1e-9;
51 
52  inline double mod2pi(double x)
53  {
54  if (x<0 && x>DUBINS_ZERO) 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 
206  { DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_LEFT },
207  { DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_RIGHT },
208  { DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_LEFT },
209  { DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_RIGHT },
210  { DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT },
211  { DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT }
212 };
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  else
219  return rho_ * dubins(state1, state2).length();
220 }
221 
222 void ompl::base::DubinsStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
223 {
224  bool firstTime = true;
225  DubinsPath path;
226  interpolate(from, to, t, firstTime, path, state);
227 }
228 
229 void ompl::base::DubinsStateSpace::interpolate(const State *from, const State *to, const double t,
230  bool& firstTime, DubinsPath& path, State *state) const
231 {
232  if (firstTime)
233  {
234  if (t>=1.)
235  {
236  if (to != state)
237  copyState(state, to);
238  return;
239  }
240  if (t<=0.)
241  {
242  if (from != state)
243  copyState(state, from);
244  return;
245  }
246 
247  path = dubins(from, to);
248  if (isSymmetric_)
249  {
250  DubinsPath path2(dubins(to, from));
251  if (path2.length() < path.length())
252  {
253  path2.reverse_ = true;
254  path = path2;
255  }
256  }
257  firstTime = false;
258  }
259  interpolate(from, path, t, state);
260 }
261 
262 void ompl::base::DubinsStateSpace::interpolate(const State *from, const DubinsPath& path, double t, State *state) const
263 {
264  StateType *s = allocState()->as<StateType>();
265  double seg = t * path.length(), phi, v;
266 
267  s->setXY(0., 0.);
268  s->setYaw(from->as<StateType>()->getYaw());
269  if (!path.reverse_)
270  {
271  for (unsigned int i=0; i<3 && seg>0; ++i)
272  {
273  v = std::min(seg, path.length_[i]);
274  phi = s->getYaw();
275  seg -= v;
276  switch(path.type_[i])
277  {
278  case DUBINS_LEFT:
279  s->setXY(s->getX() + sin(phi+v) - sin(phi), s->getY() - cos(phi+v) + cos(phi));
280  s->setYaw(phi+v);
281  break;
282  case DUBINS_RIGHT:
283  s->setXY(s->getX() - sin(phi-v) + sin(phi), s->getY() + cos(phi-v) - cos(phi));
284  s->setYaw(phi-v);
285  break;
286  case DUBINS_STRAIGHT:
287  s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
288  break;
289  }
290  }
291  }
292  else
293  {
294  for (unsigned int i=0; i<3 && seg>0; ++i)
295  {
296  v = std::min(seg, path.length_[2-i]);
297  phi = s->getYaw();
298  seg -= v;
299  switch(path.type_[2-i])
300  {
301  case DUBINS_LEFT:
302  s->setXY(s->getX() + sin(phi-v) - sin(phi), s->getY() - cos(phi-v) + cos(phi));
303  s->setYaw(phi-v);
304  break;
305  case DUBINS_RIGHT:
306  s->setXY(s->getX() - sin(phi+v) + sin(phi), s->getY() + cos(phi+v) - cos(phi));
307  s->setYaw(phi+v);
308  break;
309  case DUBINS_STRAIGHT:
310  s->setXY(s->getX() - v * cos(phi), s->getY() - v * sin(phi));
311  break;
312  }
313  }
314  }
315  state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
316  state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
317  getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
318  state->as<StateType>()->setYaw(s->getYaw());
319  freeState(s);
320 }
321 
323 {
324  const StateType *s1 = static_cast<const StateType*>(state1);
325  const StateType *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 
334 void ompl::base::DubinsMotionValidator::defaultSettings(void)
335 {
336  stateSpace_ = dynamic_cast<DubinsStateSpace*>(si_->getStateSpace().get());
337  if (!stateSpace_)
338  throw Exception("No state space for motion validator");
339 }
340 
341 bool ompl::base::DubinsMotionValidator::checkMotion(const State *s1, const State *s2, 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)
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)
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.push(std::make_pair(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.push(std::make_pair(x.first, mid - 1));
423  if (x.second > mid)
424  pos.push(std::make_pair(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 }