Loading...
Searching...
No Matches
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
43using namespace ompl::base;
44
45namespace
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
214double 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
221void 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
228void 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
261void 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
333void 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}
The exception type for ompl.
Definition Exception.h:47
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.
Complete description of a Dubins path.
An SE(2) state space where distance is measured by the length of Dubins curves.
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....
DubinsPathSegmentType
The Dubins path segment type.
DubinsPath dubins(const State *state1, const State *state2) const
Return a shortest Dubins path from SE(2) state state1 to SE(2) state state2.
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...
bool isSymmetric_
Whether the distance is "symmetrized".
static const DubinsPathSegmentType dubinsPathType[6][3]
Dubins path types.
A state in SE(2): (x, y, yaw)
double getX() const
Get the X component of the state.
The definition of a state in SO(2)
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
This namespace contains sampling based planning routines shared by both planning under geometric cons...