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::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             // view the path as a sequence of segments, and look at the triangles it forms:
00116             //          s1
00117             //          /\          s4
00118             //      a  /  \ b       |
00119             //        /    \        |
00120             //       /......\_______|
00121             //     s0    c   s2     s3
00122             //
00123             // use Pythagoras generalized theorem to find the cos of the angle between segments a and b
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                 // the smoothness is actually the outside angle of the one we compute
00131                 double angle = (boost::math::constants::pi<double>() - acos(acosValue));
00132 
00133                 // and we normalize by the length of the segments
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     // a path with invalid endpoints cannot be fixed; planners should not return such paths anyway
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             // we now compute a state around which to sample
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             // and a radius of sampling around that state
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                 // we know nextValid will be initialised because n1 is certainly valid.
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     // free potentially allocated memory
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     // the remaining length of the path we need to add states along
00285     double remainingLength = length();
00286 
00287     // the new array of states this path will have
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         // the maximum number of states that can be added on the current motion (without its endpoints)
00299         // such that we can at least fit the remaining states
00300         int maxNStates = count + i - states_.size();
00301 
00302         if (maxNStates > 0)
00303         {
00304             // compute an approximate number of states the following segment needs to contain; this includes endpoints
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             // if more than endpoints are needed
00309             if (ns > 2)
00310             {
00311                 ns -= 2; // subtract endpoints
00312 
00313                 // make sure we don't add too many states
00314                 if (ns > maxNStates)
00315                     ns = maxNStates;
00316 
00317                 // compute intermediate states
00318                 std::vector<base::State*> block;
00319                 unsigned int ans = si_->getMotionStates(s1, s2, block, ns, false, true);
00320                 // sanity checks
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             // update what remains to be done
00330             count -= (ns + 1);
00331             remainingLength -= segmentLength;
00332         }
00333         else
00334             count--;
00335     }
00336 
00337     // add the last state
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> &times, unsigned int maxSteps) const
00494 {
00495     //  This implementation greatly benefited from discussions with Kenneth Anderson (http://sites.google.com/site/kennethaanderson/
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     // compute the lengths of the segments along the path
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     // the time for the first state is 0
00523     times.resize(states_.size());
00524     times[0] = 0.0;
00525 
00526     // the velocity is maximum everywhere, except at endpoints
00527     std::vector<double> vel(states_.size(), maxVel);
00528     vel.front() = vel.back() = 0.0;
00529 
00530     // compute the cosine of the angle between consecutive segments
00531     // and scale the maximum desired velocity by that value
00532     // this has the effect of stopping at very sharp turns
00533     // and ignoring straight lines
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         // compute the time points for every state, using the currently considered velocity
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 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends