ompl/geometric/src/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/OptimizationObjective.h"
00040 #include "ompl/base/ScopedState.h"
00041 #include <algorithm>
00042 #include <cmath>
00043 #include <limits>
00044 #include <boost/math/constants/constants.hpp>
00045 
00046 ompl::geometric::PathGeometric::PathGeometric(const PathGeometric &path) : base::Path(path.si_)
00047 {
00048     copyFrom(path);
00049 }
00050 
00051 ompl::geometric::PathGeometric::PathGeometric(const base::SpaceInformationPtr &si, const base::State *state) : base::Path(si)
00052 {
00053     states_.resize(1);
00054     states_[0] = si_->cloneState(state);
00055 }
00056 
00057 ompl::geometric::PathGeometric::PathGeometric(const base::SpaceInformationPtr &si, const base::State *state1, const base::State *state2) : base::Path(si)
00058 {
00059     states_.resize(2);
00060     states_[0] = si_->cloneState(state1);
00061     states_[1] = si_->cloneState(state2);
00062 }
00063 
00064 ompl::geometric::PathGeometric& ompl::geometric::PathGeometric::operator=(const PathGeometric &other)
00065 {
00066     if (this != &other)
00067     {
00068         freeMemory();
00069         si_ = other.si_;
00070         copyFrom(other);
00071     }
00072     return *this;
00073 }
00074 
00075 void ompl::geometric::PathGeometric::copyFrom(const PathGeometric &other)
00076 {
00077     states_.resize(other.states_.size());
00078     for (unsigned int i = 0 ; i < states_.size() ; ++i)
00079         states_[i] = si_->cloneState(other.states_[i]);
00080 }
00081 
00082 void ompl::geometric::PathGeometric::freeMemory()
00083 {
00084     for (unsigned int i = 0 ; i < states_.size() ; ++i)
00085         si_->freeState(states_[i]);
00086 }
00087 
00088 ompl::base::Cost ompl::geometric::PathGeometric::cost(const base::OptimizationObjectivePtr &opt) const
00089 {
00090     if (states_.empty()) return opt->identityCost();
00091     // Compute path cost by accumulating the cost along the path
00092     base::Cost cost(opt->initialCost(states_.front()));
00093     for (std::size_t i = 1; i < states_.size(); ++i)
00094         cost = opt->combineCosts(cost, opt->motionCost(states_[i - 1], states_[i]));
00095     cost = opt->combineCosts(cost, opt->terminalCost(states_.back()));
00096     return cost;
00097 }
00098 
00099 double ompl::geometric::PathGeometric::length() const
00100 {
00101     double L = 0.0;
00102     for (unsigned int i = 1 ; i < states_.size() ; ++i)
00103         L += si_->distance(states_[i-1], states_[i]);
00104     return L;
00105 }
00106 
00107 double ompl::geometric::PathGeometric::clearance() const
00108 {
00109     double c = 0.0;
00110     for (unsigned int i = 0 ; i < states_.size() ; ++i)
00111         c += si_->getStateValidityChecker()->clearance(states_[i]);
00112     if (states_.empty())
00113         c = std::numeric_limits<double>::infinity();
00114     else
00115         c /= (double)states_.size();
00116     return c;
00117 }
00118 
00119 double ompl::geometric::PathGeometric::smoothness() const
00120 {
00121     double s = 0.0;
00122     if (states_.size() > 2)
00123     {
00124         double a = si_->distance(states_[0], states_[1]);
00125         for (unsigned int i = 2 ; i < states_.size() ; ++i)
00126         {
00127             // view the path as a sequence of segments, and look at the triangles it forms:
00128             //          s1
00129             //          /\          s4
00130             //      a  /  \ b       |
00131             //        /    \        |
00132             //       /......\_______|
00133             //     s0    c   s2     s3
00134             //
00135             // use Pythagoras generalized theorem to find the cos of the angle between segments a and b
00136             double b = si_->distance(states_[i-1], states_[i]);
00137             double c = si_->distance(states_[i-2], states_[i]);
00138             double acosValue = (a*a + b*b - c*c) / (2.0*a*b);
00139 
00140             if (acosValue > -1.0 && acosValue < 1.0)
00141             {
00142                 // the smoothness is actually the outside angle of the one we compute
00143                 double angle = (boost::math::constants::pi<double>() - acos(acosValue));
00144 
00145                 // and we normalize by the length of the segments
00146                 double k = 2.0 * angle / (a + b);
00147                 s += k * k;
00148             }
00149             a = b;
00150         }
00151     }
00152     return s;
00153 }
00154 
00155 bool ompl::geometric::PathGeometric::check() const
00156 {
00157     bool result = true;
00158     if (states_.size() > 0)
00159     {
00160         if (si_->isValid(states_[0]))
00161         {
00162             int last = states_.size() - 1;
00163             for (int j = 0 ; result && j < last ; ++j)
00164                 if (!si_->checkMotion(states_[j], states_[j + 1]))
00165                     result = false;
00166         }
00167         else
00168             result = false;
00169     }
00170 
00171     return result;
00172 }
00173 
00174 void ompl::geometric::PathGeometric::print(std::ostream &out) const
00175 {
00176     out << "Geometric path with " << states_.size() << " states" << std::endl;
00177     for (unsigned int i = 0 ; i < states_.size() ; ++i)
00178         si_->printState(states_[i], out);
00179     out << std::endl;
00180 }
00181 void ompl::geometric::PathGeometric::printAsMatrix(std::ostream &out) const
00182 {
00183     const base::StateSpace* space(si_->getStateSpace().get());
00184     std::vector<double> reals;
00185     for (unsigned int i = 0 ; i < states_.size() ; ++i)
00186     {
00187         space->copyToReals(reals, states_[i]);
00188         std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " "));
00189         out << std::endl;
00190     }
00191     out << std::endl;
00192 }
00193 
00194 std::pair<bool, bool> ompl::geometric::PathGeometric::checkAndRepair(unsigned int attempts)
00195 {
00196     if (states_.empty())
00197         return std::make_pair(true, true);
00198     if (states_.size() == 1)
00199     {
00200         bool result = si_->isValid(states_[0]);
00201         return std::make_pair(result, result);
00202     }
00203 
00204     // a path with invalid endpoints cannot be fixed; planners should not return such paths anyway
00205     const int n1 = states_.size() - 1;
00206     if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1]))
00207         return std::make_pair(false, false);
00208 
00209     base::State *temp = NULL;
00210     base::UniformValidStateSampler *uvss = NULL;
00211     bool result = true;
00212 
00213     for (int i = 1 ; i < n1 ; ++i)
00214         if (!si_->checkMotion(states_[i-1], states_[i]))
00215         {
00216             // we now compute a state around which to sample
00217             if (!temp)
00218                 temp = si_->allocState();
00219             if (!uvss)
00220             {
00221                 uvss = new base::UniformValidStateSampler(si_.get());
00222                 uvss->setNrAttempts(attempts);
00223             }
00224 
00225             // and a radius of sampling around that state
00226             double radius = 0.0;
00227 
00228             if (si_->isValid(states_[i]))
00229             {
00230                 si_->copyState(temp, states_[i]);
00231                 radius = si_->distance(states_[i-1], states_[i]);
00232             }
00233             else
00234             {
00235                 unsigned int nextValid = n1;
00236                 for (int j = i + 1 ; j < n1 ; ++j)
00237                     if (si_->isValid(states_[j]))
00238                     {
00239                         nextValid = j;
00240                         break;
00241                     }
00242                 // we know nextValid will be initialised because n1 is certainly valid.
00243                 si_->getStateSpace()->interpolate(states_[i - 1], states_[nextValid], 0.5, temp);
00244                 radius = std::max(si_->distance(states_[i-1], temp), si_->distance(states_[i-1], states_[i]));
00245             }
00246 
00247             bool success = false;
00248 
00249             for (unsigned int a = 0 ; a < attempts ; ++a)
00250                 if (uvss->sampleNear(states_[i], temp, radius))
00251                 {
00252                     if (si_->checkMotion(states_[i-1], states_[i]))
00253                     {
00254                         success = true;
00255                         break;
00256                     }
00257                 }
00258                 else
00259                     break;
00260             if (!success)
00261             {
00262                 result = false;
00263                 break;
00264             }
00265         }
00266 
00267     // free potentially allocated memory
00268     if (temp)
00269         si_->freeState(temp);
00270     bool originalValid = uvss == NULL;
00271     if (uvss)
00272         delete uvss;
00273 
00274     return std::make_pair(originalValid, result);
00275 }
00276 
00277 void ompl::geometric::PathGeometric::subdivide()
00278 {
00279     if (states_.size() < 2)
00280         return;
00281     std::vector<base::State*> newStates(1, states_[0]);
00282     for (unsigned int i = 1 ; i < states_.size() ; ++i)
00283     {
00284         base::State *temp = si_->allocState();
00285         si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp);
00286         newStates.push_back(temp);
00287         newStates.push_back(states_[i]);
00288     }
00289     states_.swap(newStates);
00290 }
00291 
00292 void ompl::geometric::PathGeometric::interpolate()
00293 {
00294     unsigned int n = 0;
00295     const int n1 = states_.size() - 1;
00296     for (int i = 0 ; i < n1 ; ++i)
00297         n += si_->getStateSpace()->validSegmentCount(states_[i], states_[i + 1]);
00298     interpolate(n);
00299 }
00300 
00301 void ompl::geometric::PathGeometric::interpolate(unsigned int requestCount)
00302 {
00303     if (requestCount < states_.size() || states_.size() < 2)
00304         return;
00305 
00306     unsigned int count = requestCount;
00307 
00308     // the remaining length of the path we need to add states along
00309     double remainingLength = length();
00310 
00311     // the new array of states this path will have
00312     std::vector<base::State*> newStates;
00313     const int n1 = states_.size() - 1;
00314 
00315     for (int i = 0 ; i < n1 ; ++i)
00316     {
00317         base::State *s1 = states_[i];
00318         base::State *s2 = states_[i + 1];
00319 
00320         newStates.push_back(s1);
00321 
00322         // the maximum number of states that can be added on the current motion (without its endpoints)
00323         // such that we can at least fit the remaining states
00324         int maxNStates = count + i - states_.size();
00325 
00326         if (maxNStates > 0)
00327         {
00328             // compute an approximate number of states the following segment needs to contain; this includes endpoints
00329             double segmentLength = si_->distance(s1, s2);
00330             int ns = i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (double)count * segmentLength / remainingLength) + 1;
00331 
00332             // if more than endpoints are needed
00333             if (ns > 2)
00334             {
00335                 ns -= 2; // subtract endpoints
00336 
00337                 // make sure we don't add too many states
00338                 if (ns > maxNStates)
00339                     ns = maxNStates;
00340 
00341                 // compute intermediate states
00342                 std::vector<base::State*> block;
00343                 unsigned int ans = si_->getMotionStates(s1, s2, block, ns, false, true);
00344                 // sanity checks
00345                 if ((int)ans != ns || block.size() != ans)
00346                     throw Exception("Internal error in path interpolation. Incorrect number of intermediate states. Please contact the developers.");
00347 
00348                 newStates.insert(newStates.end(), block.begin(), block.end());
00349             }
00350             else
00351                 ns = 0;
00352 
00353             // update what remains to be done
00354             count -= (ns + 1);
00355             remainingLength -= segmentLength;
00356         }
00357         else
00358             count--;
00359     }
00360 
00361     // add the last state
00362     newStates.push_back(states_[n1]);
00363     states_.swap(newStates);
00364     if (requestCount != states_.size())
00365         throw Exception("Internal error in path interpolation. This should never happen. Please contact the developers.");
00366 }
00367 
00368 void ompl::geometric::PathGeometric::reverse()
00369 {
00370     std::reverse(states_.begin(), states_.end());
00371 }
00372 
00373 void ompl::geometric::PathGeometric::random()
00374 {
00375     freeMemory();
00376     states_.resize(2);
00377     states_[0] = si_->allocState();
00378     states_[1] = si_->allocState();
00379     base::StateSamplerPtr ss = si_->allocStateSampler();
00380     ss->sampleUniform(states_[0]);
00381     ss->sampleUniform(states_[1]);
00382 }
00383 
00384 bool ompl::geometric::PathGeometric::randomValid(unsigned int attempts)
00385 {
00386     freeMemory();
00387     states_.resize(2);
00388     states_[0] = si_->allocState();
00389     states_[1] = si_->allocState();
00390     base::UniformValidStateSampler *uvss = new base::UniformValidStateSampler(si_.get());
00391     uvss->setNrAttempts(attempts);
00392     bool ok = false;
00393     for (unsigned int i = 0 ; i < attempts ; ++i)
00394     {
00395         if (uvss->sample(states_[0]) && uvss->sample(states_[1]))
00396             if (si_->checkMotion(states_[0], states_[1]))
00397             {
00398                 ok = true;
00399                 break;
00400             }
00401     }
00402     delete uvss;
00403     if (!ok)
00404     {
00405         freeMemory();
00406         states_.clear();
00407     }
00408     return ok;
00409 }
00410 
00411 void ompl::geometric::PathGeometric::overlay(const PathGeometric &over, unsigned int startIndex)
00412 {
00413     if (startIndex > states_.size())
00414         throw Exception("Index on path is out of bounds");
00415     const base::StateSpacePtr &sm = over.si_->getStateSpace();
00416     const base::StateSpacePtr &dm = si_->getStateSpace();
00417     bool copy = !states_.empty();
00418     for (unsigned int i = 0, j = startIndex ; i < over.states_.size() ; ++i, ++j)
00419     {
00420         if (j == states_.size())
00421         {
00422             base::State *s = si_->allocState();
00423             if (copy)
00424                 si_->copyState(s, states_.back());
00425             states_.push_back(s);
00426         }
00427 
00428         copyStateData(dm, states_[j], sm, over.states_[i]);
00429     }
00430 }
00431 
00432 void ompl::geometric::PathGeometric::append(const base::State *state)
00433 {
00434     states_.push_back(si_->cloneState(state));
00435 }
00436 
00437 void ompl::geometric::PathGeometric::append(const PathGeometric &path)
00438 {
00439     if (path.si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
00440     {
00441         PathGeometric copy(path);
00442         states_.insert(states_.end(), copy.states_.begin(), copy.states_.end());
00443         copy.states_.clear();
00444     }
00445     else
00446         overlay(path, states_.size());
00447 }
00448 
00449 void ompl::geometric::PathGeometric::prepend(const base::State *state)
00450 {
00451     states_.insert(states_.begin(), si_->cloneState(state));
00452 }
00453 
00454 void ompl::geometric::PathGeometric::keepAfter(const base::State *state)
00455 {
00456     int index = getClosestIndex(state);
00457     if (index > 0)
00458     {
00459         if ((std::size_t)(index + 1) < states_.size())
00460         {
00461             double b = si_->distance(state, states_[index-1]);
00462             double a = si_->distance(state, states_[index+1]);
00463             if (b > a)
00464                 ++index;
00465         }
00466         for (int i = 0 ; i < index ; ++i)
00467             si_->freeState(states_[i]);
00468         states_.erase(states_.begin(), states_.begin() + index);
00469     }
00470 }
00471 
00472 void ompl::geometric::PathGeometric::keepBefore(const base::State *state)
00473 {
00474     int index = getClosestIndex(state);
00475     if (index >= 0)
00476     {
00477         if (index > 0 && (std::size_t)(index + 1) < states_.size())
00478         {
00479             double b = si_->distance(state, states_[index-1]);
00480             double a = si_->distance(state, states_[index+1]);
00481             if (b < a)
00482                 --index;
00483         }
00484         if ((std::size_t)(index + 1) < states_.size())
00485         {
00486             for (std::size_t i = index + 1 ; i < states_.size() ; ++i)
00487                 si_->freeState(states_[i]);
00488             states_.resize(index + 1);
00489         }
00490     }
00491 }
00492 
00493 int ompl::geometric::PathGeometric::getClosestIndex(const base::State *state) const
00494 {
00495     if (states_.empty())
00496         return -1;
00497 
00498     int index = 0;
00499     double min_d = si_->distance(states_[0], state);
00500     for (std::size_t i = 1 ; i < states_.size() ; ++i)
00501     {
00502         double d = si_->distance(states_[i], state);
00503         if (d < min_d)
00504         {
00505             min_d = d;
00506             index = i;
00507         }
00508     }
00509     return index;
00510 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines