00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/geometric/PathGeometric.h"
00038 #include "ompl/base/samplers/UniformValidStateSampler.h"
00039 #include "ompl/base/ScopedState.h"
00040 #include <algorithm>
00041 #include <cmath>
00042 #include <limits>
00043 #include <boost/math/constants/constants.hpp>
00044
00045 ompl::geometric::PathGeometric::PathGeometric(const PathGeometric &path) : base::Path(path.si_)
00046 {
00047 copyFrom(path);
00048 }
00049
00050 ompl::geometric::PathGeometric::PathGeometric(const base::SpaceInformationPtr &si, const base::State *state) : base::Path(si)
00051 {
00052 states_.resize(1);
00053 states_[0] = si_->cloneState(state);
00054 }
00055
00056 ompl::geometric::PathGeometric::PathGeometric(const base::SpaceInformationPtr &si, const base::State *state1, const base::State *state2) : base::Path(si)
00057 {
00058 states_.resize(2);
00059 states_[0] = si_->cloneState(state1);
00060 states_[1] = si_->cloneState(state2);
00061 }
00062
00063 ompl::geometric::PathGeometric& ompl::geometric::PathGeometric::operator=(const PathGeometric &other)
00064 {
00065 if (this != &other)
00066 {
00067 freeMemory();
00068 si_ = other.si_;
00069 copyFrom(other);
00070 }
00071 return *this;
00072 }
00073
00074 void ompl::geometric::PathGeometric::copyFrom(const PathGeometric &other)
00075 {
00076 states_.resize(other.states_.size());
00077 for (unsigned int i = 0 ; i < states_.size() ; ++i)
00078 states_[i] = si_->cloneState(other.states_[i]);
00079 }
00080
00081 void ompl::geometric::PathGeometric::freeMemory(void)
00082 {
00083 for (unsigned int i = 0 ; i < states_.size() ; ++i)
00084 si_->freeState(states_[i]);
00085 }
00086
00087 double ompl::geometric::PathGeometric::length(void) const
00088 {
00089 double L = 0.0;
00090 for (unsigned int i = 1 ; i < states_.size() ; ++i)
00091 L += si_->distance(states_[i-1], states_[i]);
00092 return L;
00093 }
00094
00095 double ompl::geometric::PathGeometric::clearance(void) const
00096 {
00097 double c = 0.0;
00098 for (unsigned int i = 0 ; i < states_.size() ; ++i)
00099 c += si_->getStateValidityChecker()->clearance(states_[i]);
00100 if (states_.empty())
00101 c = std::numeric_limits<double>::infinity();
00102 else
00103 c /= (double)states_.size();
00104 return c;
00105 }
00106
00107 double ompl::geometric::PathGeometric::smoothness(void) const
00108 {
00109 double s = 0.0;
00110 if (states_.size() > 2)
00111 {
00112 double a = si_->distance(states_[0], states_[1]);
00113 for (unsigned int i = 2 ; i < states_.size() ; ++i)
00114 {
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124 double b = si_->distance(states_[i-1], states_[i]);
00125 double c = si_->distance(states_[i-2], states_[i]);
00126 double acosValue = (a*a + b*b - c*c) / (2.0*a*b);
00127
00128 if (acosValue > -1.0 && acosValue < 1.0)
00129 {
00130
00131 double angle = (boost::math::constants::pi<double>() - acos(acosValue));
00132
00133
00134 double k = 2.0 * angle / (a + b);
00135 s += k * k;
00136 }
00137 a = b;
00138 }
00139 }
00140 return s;
00141 }
00142
00143 bool ompl::geometric::PathGeometric::check(void) const
00144 {
00145 bool result = true;
00146 if (states_.size() > 0)
00147 {
00148 if (si_->isValid(states_[0]))
00149 {
00150 int last = states_.size() - 1;
00151 for (int j = 0 ; result && j < last ; ++j)
00152 if (!si_->checkMotion(states_[j], states_[j + 1]))
00153 result = false;
00154 }
00155 else
00156 result = false;
00157 }
00158
00159 return result;
00160 }
00161
00162 void ompl::geometric::PathGeometric::print(std::ostream &out) const
00163 {
00164 out << "Geometric path with " << states_.size() << " states" << std::endl;
00165 for (unsigned int i = 0 ; i < states_.size() ; ++i)
00166 si_->printState(states_[i], out);
00167 out << std::endl;
00168 }
00169
00170 std::pair<bool, bool> ompl::geometric::PathGeometric::checkAndRepair(unsigned int attempts)
00171 {
00172 if (states_.empty())
00173 return std::make_pair(true, true);
00174 if (states_.size() == 1)
00175 {
00176 bool result = si_->isValid(states_[0]);
00177 return std::make_pair(result, result);
00178 }
00179
00180
00181 const int n1 = states_.size() - 1;
00182 if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1]))
00183 return std::make_pair(false, false);
00184
00185 base::State *temp = NULL;
00186 base::UniformValidStateSampler *uvss = NULL;
00187 bool result = true;
00188
00189 for (int i = 1 ; i < n1 ; ++i)
00190 if (!si_->checkMotion(states_[i-1], states_[i]))
00191 {
00192
00193 if (!temp)
00194 temp = si_->allocState();
00195 if (!uvss)
00196 {
00197 uvss = new base::UniformValidStateSampler(si_.get());
00198 uvss->setNrAttempts(attempts);
00199 }
00200
00201
00202 double radius = 0.0;
00203
00204 if (si_->isValid(states_[i]))
00205 {
00206 si_->copyState(temp, states_[i]);
00207 radius = si_->distance(states_[i-1], states_[i]);
00208 }
00209 else
00210 {
00211 unsigned int nextValid = n1;
00212 for (int j = i + 1 ; j < n1 ; ++j)
00213 if (si_->isValid(states_[j]))
00214 {
00215 nextValid = j;
00216 break;
00217 }
00218
00219 si_->getStateSpace()->interpolate(states_[i - 1], states_[nextValid], 0.5, temp);
00220 radius = std::max(si_->distance(states_[i-1], temp), si_->distance(states_[i-1], states_[i]));
00221 }
00222
00223 bool success = false;
00224
00225 for (unsigned int a = 0 ; a < attempts ; ++a)
00226 if (uvss->sampleNear(states_[i], temp, radius))
00227 {
00228 if (si_->checkMotion(states_[i-1], states_[i]))
00229 {
00230 success = true;
00231 break;
00232 }
00233 }
00234 else
00235 break;
00236 if (!success)
00237 {
00238 result = false;
00239 break;
00240 }
00241 }
00242
00243
00244 if (temp)
00245 si_->freeState(temp);
00246 bool originalValid = uvss == NULL;
00247 if (uvss)
00248 delete uvss;
00249
00250 return std::make_pair(originalValid, result);
00251 }
00252
00253 void ompl::geometric::PathGeometric::subdivide(void)
00254 {
00255 if (states_.size() < 2)
00256 return;
00257 std::vector<base::State*> newStates(1, states_[0]);
00258 for (unsigned int i = 1 ; i < states_.size() ; ++i)
00259 {
00260 base::State *temp = si_->allocState();
00261 si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp);
00262 newStates.push_back(temp);
00263 newStates.push_back(states_[i]);
00264 }
00265 states_.swap(newStates);
00266 }
00267
00268 void ompl::geometric::PathGeometric::interpolate(void)
00269 {
00270 unsigned int n = 0;
00271 const int n1 = states_.size() - 1;
00272 for (int i = 0 ; i < n1 ; ++i)
00273 n += si_->getStateSpace()->validSegmentCount(states_[i], states_[i + 1]);
00274 interpolate(n);
00275 }
00276
00277 void ompl::geometric::PathGeometric::interpolate(unsigned int requestCount)
00278 {
00279 if (requestCount < states_.size() || states_.size() < 2)
00280 return;
00281
00282 unsigned int count = requestCount;
00283
00284
00285 double remainingLength = length();
00286
00287
00288 std::vector<base::State*> newStates;
00289 const int n1 = states_.size() - 1;
00290
00291 for (int i = 0 ; i < n1 ; ++i)
00292 {
00293 base::State *s1 = states_[i];
00294 base::State *s2 = states_[i + 1];
00295
00296 newStates.push_back(s1);
00297
00298
00299
00300 int maxNStates = count + i - states_.size();
00301
00302 if (maxNStates > 0)
00303 {
00304
00305 double segmentLength = si_->distance(s1, s2);
00306 int ns = i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (double)count * segmentLength / remainingLength) + 1;
00307
00308
00309 if (ns > 2)
00310 {
00311 ns -= 2;
00312
00313
00314 if (ns > maxNStates)
00315 ns = maxNStates;
00316
00317
00318 std::vector<base::State*> block;
00319 unsigned int ans = si_->getMotionStates(s1, s2, block, ns, false, true);
00320
00321 if ((int)ans != ns || block.size() != ans)
00322 throw Exception("Internal error in path interpolation. Incorrect number of intermediate states. Please contact the developers.");
00323
00324 newStates.insert(newStates.end(), block.begin(), block.end());
00325 }
00326 else
00327 ns = 0;
00328
00329
00330 count -= (ns + 1);
00331 remainingLength -= segmentLength;
00332 }
00333 else
00334 count--;
00335 }
00336
00337
00338 newStates.push_back(states_[n1]);
00339 states_.swap(newStates);
00340 if (requestCount != states_.size())
00341 throw Exception("Internal error in path interpolation. This should never happen. Please contact the developers.");
00342 }
00343
00344 void ompl::geometric::PathGeometric::reverse(void)
00345 {
00346 std::reverse(states_.begin(), states_.end());
00347 }
00348
00349 void ompl::geometric::PathGeometric::random(void)
00350 {
00351 freeMemory();
00352 states_.resize(2);
00353 states_[0] = si_->allocState();
00354 states_[1] = si_->allocState();
00355 base::StateSamplerPtr ss = si_->allocStateSampler();
00356 ss->sampleUniform(states_[0]);
00357 ss->sampleUniform(states_[1]);
00358 }
00359
00360 bool ompl::geometric::PathGeometric::randomValid(unsigned int attempts)
00361 {
00362 freeMemory();
00363 states_.resize(2);
00364 states_[0] = si_->allocState();
00365 states_[1] = si_->allocState();
00366 base::UniformValidStateSampler *uvss = new base::UniformValidStateSampler(si_.get());
00367 uvss->setNrAttempts(attempts);
00368 bool ok = false;
00369 for (unsigned int i = 0 ; i < attempts ; ++i)
00370 {
00371 if (uvss->sample(states_[0]) && uvss->sample(states_[1]))
00372 if (si_->checkMotion(states_[0], states_[1]))
00373 {
00374 ok = true;
00375 break;
00376 }
00377 }
00378 delete uvss;
00379 if (!ok)
00380 {
00381 freeMemory();
00382 states_.clear();
00383 }
00384 return ok;
00385 }
00386
00387 void ompl::geometric::PathGeometric::overlay(const PathGeometric &over, unsigned int startIndex)
00388 {
00389 if (startIndex > states_.size())
00390 throw Exception("Index on path is out of bounds");
00391 const base::StateSpacePtr &sm = over.si_->getStateSpace();
00392 const base::StateSpacePtr &dm = si_->getStateSpace();
00393 bool copy = !states_.empty();
00394 for (unsigned int i = 0, j = startIndex ; i < over.states_.size() ; ++i, ++j)
00395 {
00396 if (j == states_.size())
00397 {
00398 base::State *s = si_->allocState();
00399 if (copy)
00400 si_->copyState(s, states_.back());
00401 states_.push_back(s);
00402 }
00403
00404 copyStateData(dm, states_[j], sm, over.states_[i]);
00405 }
00406 }
00407
00408 void ompl::geometric::PathGeometric::append(const base::State *state)
00409 {
00410 states_.push_back(si_->cloneState(state));
00411 }
00412
00413 void ompl::geometric::PathGeometric::append(const PathGeometric &path)
00414 {
00415 if (path.si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
00416 {
00417 PathGeometric copy(path);
00418 states_.insert(states_.end(), copy.states_.begin(), copy.states_.end());
00419 copy.states_.clear();
00420 }
00421 else
00422 overlay(path, states_.size());
00423 }
00424
00425 void ompl::geometric::PathGeometric::keepAfter(const base::State *state)
00426 {
00427 int index = getClosestIndex(state);
00428 if (index > 0)
00429 {
00430 if ((std::size_t)(index + 1) < states_.size())
00431 {
00432 double b = si_->distance(state, states_[index-1]);
00433 double a = si_->distance(state, states_[index+1]);
00434 if (b > a)
00435 ++index;
00436 }
00437 for (int i = 0 ; i < index ; ++i)
00438 si_->freeState(states_[i]);
00439 states_.erase(states_.begin(), states_.begin() + index);
00440 }
00441 }
00442
00443 void ompl::geometric::PathGeometric::keepBefore(const base::State *state)
00444 {
00445 int index = getClosestIndex(state);
00446 if (index >= 0)
00447 {
00448 if (index > 0 && (std::size_t)(index + 1) < states_.size())
00449 {
00450 double b = si_->distance(state, states_[index-1]);
00451 double a = si_->distance(state, states_[index+1]);
00452 if (b < a)
00453 --index;
00454 }
00455 if ((std::size_t)(index + 1) < states_.size())
00456 {
00457 for (std::size_t i = index + 1 ; i < states_.size() ; ++i)
00458 si_->freeState(states_[i]);
00459 states_.resize(index + 1);
00460 }
00461 }
00462 }
00463
00464 int ompl::geometric::PathGeometric::getClosestIndex(const base::State *state) const
00465 {
00466 if (states_.empty())
00467 return -1;
00468
00469 int index = 0;
00470 double min_d = si_->distance(states_[0], state);
00471 for (std::size_t i = 1 ; i < states_.size() ; ++i)
00472 {
00473 double d = si_->distance(states_[i], state);
00474 if (d < min_d)
00475 {
00476 min_d = d;
00477 index = i;
00478 }
00479 }
00480 return index;
00481 }
00482
00483 namespace ompl
00484 {
00485 namespace magic
00486 {
00488 static const double TIME_PARAMETRIZATION_VELOCITY_DECREASE_FACTOR = 0.95;
00489 }
00490 }
00491
00492
00493 void ompl::geometric::PathGeometric::computeFastTimeParametrization(double maxVel, double maxAcc, std::vector<double> ×, unsigned int maxSteps) const
00494 {
00495
00496
00497 if (states_.empty())
00498 {
00499 times.clear();
00500 return;
00501 }
00502 if (states_.size() == 1)
00503 {
00504 times.resize(1);
00505 times[0] = 0.0;
00506 return;
00507 }
00508 if (states_.size() == 2)
00509 {
00510 double d = si_->distance(states_[0], states_[1]);
00511 times.resize(2);
00512 times[0] = 0.0;
00513 times[1] = std::max(2.0 * d / maxVel, sqrt(2.0 * d / maxAcc));
00514 return;
00515 }
00516
00517
00518 std::vector<double> L(states_.size() - 1);
00519 for (std::size_t i = 0 ; i < L.size() ; ++i)
00520 L[i] = si_->distance(states_[i], states_[i + 1]);
00521
00522
00523 times.resize(states_.size());
00524 times[0] = 0.0;
00525
00526
00527 std::vector<double> vel(states_.size(), maxVel);
00528 vel.front() = vel.back() = 0.0;
00529
00530
00531
00532
00533
00534 for (std::size_t i = 1 ; i < L.size() ; ++i)
00535 {
00536 double a = L[i-1];
00537 double b = L[i];
00538 double c = si_->distance(states_[i-1], states_[i+1]);
00539 double acosValue = (a*a + b*b - c*c) / (2.0*a*b);
00540 vel[i] *= std::min(1.0, fabs(acosValue));
00541 }
00542
00543 bool change = true;
00544 unsigned int steps = 0;
00545 while (change && steps <= maxSteps)
00546 {
00547 ++steps;
00548 change = false;
00549
00550
00551 for (std::size_t i = 1 ; i < times.size() ; ++i)
00552 times[i] = times[i - 1] + (2.0 * L[i-1]) / (vel[i-1] + vel[i]);
00553
00554 for (std::size_t i = 0 ; i < L.size() ; ++i)
00555 {
00556 double acc = (vel[i + 1] - vel[i]) / (times[i + 1] - times[i]);
00557 if (acc > maxAcc)
00558 {
00559 vel[i + 1] *= magic::TIME_PARAMETRIZATION_VELOCITY_DECREASE_FACTOR;
00560 change = true;
00561 }
00562 else
00563 if (acc < -maxAcc)
00564 {
00565 vel[i] *= magic::TIME_PARAMETRIZATION_VELOCITY_DECREASE_FACTOR;
00566 change = true;
00567 }
00568 }
00569
00570 if (change)
00571 for (int i = L.size() - 1 ; i >= 0 ; --i)
00572 {
00573 double acc = (vel[i + 1] - vel[i]) / (times[i + 1] - times[i]);
00574 if (acc > maxAcc)
00575 {
00576 vel[i + 1] *= magic::TIME_PARAMETRIZATION_VELOCITY_DECREASE_FACTOR;
00577 change = true;
00578 }
00579 else
00580 if (acc < -maxAcc)
00581 {
00582 vel[i] *= magic::TIME_PARAMETRIZATION_VELOCITY_DECREASE_FACTOR;
00583 change = true;
00584 }
00585 }
00586 }
00587
00588 }