All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
PathGeometric.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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     // a path with invalid endpoints cannot be fixed; planners should not return such paths anyway
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             // we now compute a state around which to sample
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             // and a radius of sampling around that state
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                 // we know nextValid will be initialised because n1 is certainly valid.
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     // free potentially allocated memory
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     // the remaining length of the path we need to add states along
00262     double remainingLength = length();
00263 
00264     // the new array of states this path will have
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         // the maximum number of states that can be added on the current motion (without its endpoints)
00276         // such that we can at least fit the remaining states
00277         int maxNStates = count + i - states.size();
00278 
00279         if (maxNStates > 0)
00280         {
00281             // compute an approximate number of states the following segment needs to contain; this includes endpoints
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             // if more than endpoints are needed
00286             if (ns > 2)
00287             {
00288                 ns -= 2; // subtract endpoints
00289 
00290                 // make sure we don't add too many states
00291                 if (ns > maxNStates)
00292                     ns = maxNStates;
00293 
00294                 // compute intermediate states
00295                 std::vector<base::State*> block;
00296                 unsigned int ans = si_->getMotionStates(s1, s2, block, ns, false, true);
00297                 // sanity checks
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             // update what remains to be done
00307             count -= (ns + 1);
00308             remainingLength -= segmentLength;
00309         }
00310         else
00311             count--;
00312     }
00313 
00314     // add the last state
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 }