All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
SpaceInformation.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Rice University
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 Rice University 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/control/SpaceInformation.h"
00038 #include "ompl/util/Exception.h"
00039 #include <cassert>
00040 #include <utility>
00041 #include <limits>
00042 
00043 void ompl::control::SpaceInformation::setup(void)
00044 {
00045     base::SpaceInformation::setup();
00046     if (minSteps_ > maxSteps_)
00047         throw Exception("The minimum number of steps cannot be larger than the maximum number of steps");
00048     if (minSteps_ == 0 && maxSteps_ == 0)
00049     {
00050         minSteps_ = 1;
00051         maxSteps_ = 10;
00052         msg_.warn("Assuming propagation will always have between %d and %d steps", minSteps_, maxSteps_);
00053     }
00054     if (minSteps_ < 1)
00055         throw Exception("The minimum number of steps must be at least 1");
00056 
00057     if (stepSize_ < std::numeric_limits<double>::epsilon())
00058     {
00059         stepSize_ = getStateValidityCheckingResolution() * getMaximumExtent();
00060         if (stepSize_ < std::numeric_limits<double>::epsilon())
00061             throw Exception("The propagation step size must be larger than 0");
00062         msg_.warn("The propagation step size is assumed to be %f", stepSize_);
00063     }
00064 
00065     controlManifold_->setup();
00066     if (controlManifold_->getDimension() <= 0)
00067         throw Exception("The dimension of the control manifold we plan in must be > 0");
00068 }
00069 
00070 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control* control, unsigned int steps, base::State *result) const
00071 {
00072     if (steps == 0)
00073     {
00074         if (result != state)
00075             copyState(result, state);
00076     }
00077     else
00078     {
00079         controlManifold_->propagate(state, control, stepSize_, result);
00080         for (unsigned int i = 1 ; i < steps ; ++i)
00081             controlManifold_->propagate(result, control, stepSize_, result);
00082     }
00083 }
00084 
00085 unsigned int ompl::control::SpaceInformation::propagateWhileValid(const base::State *state, const Control* control, unsigned int steps, base::State *result) const
00086 {
00087     if (steps == 0)
00088     {
00089         if (result != state)
00090             copyState(result, state);
00091         return 0;
00092     }
00093 
00094     // perform the first step of propagation
00095     controlManifold_->propagate(state, control, stepSize_, result);
00096 
00097     // if we found a valid state after one step, we can go on
00098     if (isValid(result))
00099     {
00100         base::State *temp1 = result;
00101         base::State *temp2 = allocState();
00102         base::State *toDelete = temp2;
00103         unsigned int r = steps;
00104 
00105         // for the remaining number of steps
00106         for (unsigned int i = 1 ; i < steps ; ++i)
00107         {
00108             controlManifold_->propagate(temp1, control, stepSize_, temp2);
00109             if (isValid(temp2))
00110                 std::swap(temp1, temp2);
00111             else
00112             {
00113                 // the last valid state is temp1;
00114                 r = i;
00115                 break;
00116             }
00117         }
00118 
00119         // if we finished the for-loop without finding an invalid state, the last valid state is temp1
00120         // make sure result contains that information
00121         if (result != temp1)
00122             copyState(result, temp1);
00123 
00124         // free the temporary memory
00125         freeState(toDelete);
00126 
00127         return r;
00128     }
00129     // if the first propagation step produced an invalid step, return 0 steps
00130     // the last valid state is the starting one (assumed to be valid)
00131     else
00132     {
00133         if (result != state)
00134             copyState(result, state);
00135         return 0;
00136     }
00137 }
00138 
00139 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control* control, unsigned int steps, std::vector<base::State*> &result, bool alloc) const
00140 {
00141     if (alloc)
00142     {
00143         result.resize(steps);
00144         for (unsigned int i = 0 ; i < result.size() ; ++i)
00145             result[i] = allocState();
00146     }
00147     else
00148     {
00149         if (result.empty())
00150             return;
00151         steps = std::min(steps, (unsigned int)result.size());
00152     }
00153 
00154     unsigned int st = 0;
00155 
00156     if (st < steps)
00157     {
00158         controlManifold_->propagate(state, control, stepSize_, result[st]);
00159         ++st;
00160 
00161         while (st < steps)
00162         {
00163             controlManifold_->propagate(result[st-1], control, stepSize_, result[st]);
00164             ++st;
00165         }
00166     }
00167 }
00168 
00169 unsigned int ompl::control::SpaceInformation::propagateWhileValid(const base::State *state, const Control* control, unsigned int steps, std::vector<base::State*> &result, bool alloc) const
00170 {
00171     if (alloc)
00172         result.resize(steps);
00173     else
00174     {
00175         if (result.empty())
00176             return 0;
00177         steps = std::min(steps, (unsigned int)result.size());
00178     }
00179 
00180     unsigned int st = 0;
00181 
00182     if (st < steps)
00183     {
00184         if (alloc)
00185             result[st] = allocState();
00186         controlManifold_->propagate(state, control, stepSize_, result[st]);
00187 
00188         if (isValid(result[st]))
00189         {
00190                  ++st;
00191             while (st < steps)
00192             {
00193                 if (alloc)
00194                     result[st] = allocState();
00195                 controlManifold_->propagate(result[st-1], control, stepSize_, result[st]);
00196 
00197                 if (!isValid(result[st]))
00198                 {
00199                     if (alloc)
00200                     {
00201                         freeState(result[st]);
00202                         result.resize(st);
00203                     }
00204                     break;
00205                 }
00206                 else
00207                     ++st;
00208             }
00209         }
00210         else
00211         {
00212             if (alloc)
00213             {
00214                 freeState(result[st]);
00215                 result.resize(st);
00216             }
00217         }
00218     }
00219 
00220     return st;
00221 }
00222 
00223 void ompl::control::SpaceInformation::printSettings(std::ostream &out) const
00224 {
00225     base::SpaceInformation::printSettings(out);
00226     out << "  - control manifold:" << std::endl;
00227     controlManifold_->printSettings(out);
00228     out << "  - propagation step size: " << stepSize_ << std::endl;
00229     out << "  - propagation duration: [" << minSteps_ << ", " << maxSteps_ << "]" << std::endl;
00230 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator