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& ompl::geometric::PathGeometric::operator=(const PathGeometric &other)
00057 {
00058 freeMemory();
00059 si_ = other.si_;
00060 copyFrom(other);
00061 return *this;
00062 }
00063
00064 void ompl::geometric::PathGeometric::copyFrom(const PathGeometric &other)
00065 {
00066 states.resize(other.states.size());
00067 for (unsigned int i = 0 ; i < states.size() ; ++i)
00068 states[i] = si_->cloneState(other.states[i]);
00069 }
00070
00071 void ompl::geometric::PathGeometric::freeMemory(void)
00072 {
00073 for (unsigned int i = 0 ; i < states.size() ; ++i)
00074 si_->freeState(states[i]);
00075 }
00076
00077 double ompl::geometric::PathGeometric::length(void) const
00078 {
00079 double L = 0.0;
00080 for (unsigned int i = 1 ; i < states.size() ; ++i)
00081 L += si_->distance(states[i-1], states[i]);
00082 return L;
00083 }
00084
00085 double ompl::geometric::PathGeometric::clearance(void) const
00086 {
00087 double c = 0.0;
00088 for (unsigned int i = 0 ; i < states.size() ; ++i)
00089 c += si_->getStateValidityChecker()->clearance(states[i]);
00090 if (states.empty())
00091 c = std::numeric_limits<double>::infinity();
00092 else
00093 c /= (double)states.size();
00094 return c;
00095 }
00096
00097 double ompl::geometric::PathGeometric::smoothness(void) const
00098 {
00099 double s = 0.0;
00100 if (states.size() > 2)
00101 {
00102 double a = si_->distance(states[0], states[1]);
00103 for (unsigned int i = 2 ; i < states.size() ; ++i)
00104 {
00105 double b = si_->distance(states[i-1], states[i]);
00106 double c = si_->distance(states[i-2], states[i]);
00107 double acosValue = (a*a + b*b - c*c) / (2.0*a*b);
00108 if (acosValue > -1.0 && acosValue < 1.0)
00109 {
00110 double angle = (boost::math::constants::pi<double>() - acos(acosValue));
00111 double k = 2.0 * angle / (a + b);
00112 s += k * k;
00113 }
00114 a = b;
00115 }
00116 }
00117 return s;
00118 }
00119
00120 bool ompl::geometric::PathGeometric::check(void) const
00121 {
00122 bool result = true;
00123 if (states.size() > 0)
00124 {
00125 if (si_->isValid(states[0]))
00126 {
00127 int last = states.size() - 1;
00128 for (int j = 0 ; result && j < last ; ++j)
00129 if (!si_->checkMotion(states[j], states[j + 1]))
00130 result = false;
00131 }
00132 else
00133 result = false;
00134 }
00135
00136 return result;
00137 }
00138
00139 void ompl::geometric::PathGeometric::print(std::ostream &out) const
00140 {
00141 out << "Geometric path with " << states.size() << " states" << std::endl;
00142 for (unsigned int i = 0 ; i < states.size() ; ++i)
00143 si_->printState(states[i], out);
00144 out << std::endl;
00145 }
00146
00147 std::pair<bool, bool> ompl::geometric::PathGeometric::checkAndRepair(unsigned int attempts)
00148 {
00149 if (states.empty())
00150 return std::make_pair(true, true);
00151 if (states.size() == 1)
00152 {
00153 bool result = si_->isValid(states[0]);
00154 return std::make_pair(result, result);
00155 }
00156
00157
00158 const int n1 = states.size() - 1;
00159 if (!si_->isValid(states[0]) || !si_->isValid(states[n1]))
00160 return std::make_pair(false, false);
00161
00162 base::State *temp = NULL;
00163 base::UniformValidStateSampler *uvss = NULL;
00164 bool result = true;
00165
00166 for (int i = 1 ; i < n1 ; ++i)
00167 if (!si_->checkMotion(states[i-1], states[i]))
00168 {
00169
00170 if (!temp)
00171 temp = si_->allocState();
00172 if (!uvss)
00173 {
00174 uvss = new base::UniformValidStateSampler(si_.get());
00175 uvss->setNrAttempts(attempts);
00176 }
00177
00178
00179 double radius = 0.0;
00180
00181 if (si_->isValid(states[i]))
00182 {
00183 si_->copyState(temp, states[i]);
00184 radius = si_->distance(states[i-1], states[i]);
00185 }
00186 else
00187 {
00188 unsigned int nextValid = n1;
00189 for (int j = i + 1 ; j < n1 ; ++j)
00190 if (si_->isValid(states[j]))
00191 {
00192 nextValid = j;
00193 break;
00194 }
00195
00196 si_->getStateSpace()->interpolate(states[i - 1], states[nextValid], 0.5, temp);
00197 radius = std::max(si_->distance(states[i-1], temp), si_->distance(states[i-1], states[i]));
00198 }
00199
00200 bool success = false;
00201
00202 for (unsigned int a = 0 ; a < attempts ; ++a)
00203 if (uvss->sampleNear(states[i], temp, radius))
00204 {
00205 if (si_->checkMotion(states[i-1], states[i]))
00206 {
00207 success = true;
00208 break;
00209 }
00210 }
00211 else
00212 break;
00213 if (!success)
00214 {
00215 result = false;
00216 break;
00217 }
00218 }
00219
00220
00221 if (temp)
00222 si_->freeState(temp);
00223 bool originalValid = uvss == NULL;
00224 if (uvss)
00225 delete uvss;
00226
00227 return std::make_pair(originalValid, result);
00228 }
00229
00230 void ompl::geometric::PathGeometric::subdivide(void)
00231 {
00232 if (states.size() < 2)
00233 return;
00234 std::vector<base::State*> newStates(1, states[0]);
00235 for (unsigned int i = 1 ; i < states.size() ; ++i)
00236 {
00237 base::State *temp = si_->allocState();
00238 si_->getStateSpace()->interpolate(newStates.back(), states[i], 0.5, temp);
00239 newStates.push_back(temp);
00240 newStates.push_back(states[i]);
00241 }
00242 states.swap(newStates);
00243 }
00244
00245 void ompl::geometric::PathGeometric::interpolate(void)
00246 {
00247 unsigned int n = 0;
00248 const int n1 = states.size() - 1;
00249 for (int i = 0 ; i < n1 ; ++i)
00250 n += si_->getStateSpace()->validSegmentCount(states[i], states[i + 1]);
00251 interpolate(n);
00252 }
00253
00254 void ompl::geometric::PathGeometric::interpolate(unsigned int requestCount)
00255 {
00256 if (requestCount < states.size() || states.size() < 2)
00257 return;
00258
00259 unsigned int count = requestCount;
00260
00261
00262 double remainingLength = length();
00263
00264
00265 std::vector<base::State*> newStates;
00266 const int n1 = states.size() - 1;
00267
00268 for (int i = 0 ; i < n1 ; ++i)
00269 {
00270 base::State *s1 = states[i];
00271 base::State *s2 = states[i + 1];
00272
00273 newStates.push_back(s1);
00274
00275
00276
00277 int maxNStates = count + i - states.size();
00278
00279 if (maxNStates > 0)
00280 {
00281
00282 double segmentLength = si_->distance(s1, s2);
00283 int ns = i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (double)count * segmentLength / remainingLength) + 1;
00284
00285
00286 if (ns > 2)
00287 {
00288 ns -= 2;
00289
00290
00291 if (ns > maxNStates)
00292 ns = maxNStates;
00293
00294
00295 std::vector<base::State*> block;
00296 unsigned int ans = si_->getMotionStates(s1, s2, block, ns, false, true);
00297
00298 if ((int)ans != ns || block.size() != ans)
00299 throw Exception("Internal error in path interpolation. Incorrect number of intermediate states. Please contact the developers.");
00300
00301 newStates.insert(newStates.end(), block.begin(), block.end());
00302 }
00303 else
00304 ns = 0;
00305
00306
00307 count -= (ns + 1);
00308 remainingLength -= segmentLength;
00309 }
00310 else
00311 count--;
00312 }
00313
00314
00315 newStates.push_back(states[n1]);
00316 states.swap(newStates);
00317 if (requestCount != states.size())
00318 throw Exception("Internal error in path interpolation. This should never happen. Please contact the developers.");
00319 }
00320
00321 void ompl::geometric::PathGeometric::reverse(void)
00322 {
00323 std::reverse(states.begin(), states.end());
00324 }
00325
00326 void ompl::geometric::PathGeometric::random(void)
00327 {
00328 freeMemory();
00329 states.resize(2);
00330 states[0] = si_->allocState();
00331 states[1] = si_->allocState();
00332 base::StateSamplerPtr ss = si_->allocStateSampler();
00333 ss->sampleUniform(states[0]);
00334 ss->sampleUniform(states[1]);
00335 }
00336
00337 bool ompl::geometric::PathGeometric::randomValid(unsigned int attempts)
00338 {
00339 freeMemory();
00340 states.resize(2);
00341 states[0] = si_->allocState();
00342 states[1] = si_->allocState();
00343 base::UniformValidStateSampler *uvss = new base::UniformValidStateSampler(si_.get());
00344 uvss->setNrAttempts(attempts);
00345 bool ok = false;
00346 for (unsigned int i = 0 ; i < attempts ; ++i)
00347 {
00348 if (uvss->sample(states[0]) && uvss->sample(states[1]))
00349 if (si_->checkMotion(states[0], states[1]))
00350 {
00351 ok = true;
00352 break;
00353 }
00354 }
00355 delete uvss;
00356 if (!ok)
00357 {
00358 freeMemory();
00359 states.clear();
00360 }
00361 return ok;
00362 }
00363
00364 void ompl::geometric::PathGeometric::overlay(const PathGeometric &over, unsigned int startIndex)
00365 {
00366 if (startIndex > states.size())
00367 throw Exception("Index on path is out of bounds");
00368 const base::StateSpacePtr &sm = over.si_->getStateSpace();
00369 const base::StateSpacePtr &dm = si_->getStateSpace();
00370 bool copy = !states.empty();
00371 for (unsigned int i = 0, j = startIndex ; i < over.states.size() ; ++i, ++j)
00372 {
00373 if (j == states.size())
00374 {
00375 base::State *s = si_->allocState();
00376 if (copy)
00377 si_->copyState(s, states.back());
00378 states.push_back(s);
00379 }
00380
00381 copyStateData(dm, states[j], sm, over.states[i]);
00382 }
00383 }
00384
00385 void ompl::geometric::PathGeometric::append(const PathGeometric &path)
00386 {
00387 if (path.si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
00388 {
00389 PathGeometric copy(path);
00390 states.insert(states.end(), copy.states.begin(), copy.states.end());
00391 copy.states.clear();
00392 }
00393 else
00394 overlay(path, states.size());
00395 }