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 (!statePropagator_)
00047         throw Exception("State propagator not defined");
00048     if (minSteps_ > maxSteps_)
00049         throw Exception("The minimum number of steps cannot be larger than the maximum number of steps");
00050     if (minSteps_ == 0 && maxSteps_ == 0)
00051     {
00052         minSteps_ = 1;
00053         maxSteps_ = 10;
00054         msg_.warn("Assuming propagation will always have between %d and %d steps", minSteps_, maxSteps_);
00055     }
00056     if (minSteps_ < 1)
00057         throw Exception("The minimum number of steps must be at least 1");
00058 
00059     if (stepSize_ < std::numeric_limits<double>::epsilon())
00060     {
00061         stepSize_ = getStateValidityCheckingResolution() * getMaximumExtent();
00062         if (stepSize_ < std::numeric_limits<double>::epsilon())
00063             throw Exception("The propagation step size must be larger than 0");
00064         msg_.warn("The propagation step size is assumed to be %f", stepSize_);
00065     }
00066 
00067     controlSpace_->setup();
00068     if (controlSpace_->getDimension() <= 0)
00069         throw Exception("The dimension of the control space we plan in must be > 0");
00070 }
00071 
00072 void ompl::control::SpaceInformation::setStatePropagator(const StatePropagatorFn &fn)
00073 {
00074     class BoostFnStatePropagator : public StatePropagator
00075     {
00076     public:
00077 
00078         BoostFnStatePropagator(SpaceInformation *si, const StatePropagatorFn &fn) : StatePropagator(si), fn_(fn)
00079         {
00080         }
00081 
00082         virtual void propagate(const base::State *state, const Control* control, const double duration, base::State *result) const
00083         {
00084             fn_(state, control, duration, result);
00085         }
00086 
00087     protected:
00088 
00089         StatePropagatorFn fn_;
00090 
00091     };
00092 
00093     setStatePropagator(StatePropagatorPtr(dynamic_cast<StatePropagator*>(new BoostFnStatePropagator(this, fn))));
00094 }
00095 
00096 void ompl::control::SpaceInformation::setStatePropagator(const StatePropagatorPtr &sp)
00097 {
00098     statePropagator_ = sp;
00099 }
00100 
00101 bool ompl::control::SpaceInformation::canPropagateBackward(void) const
00102 {
00103     return statePropagator_->canPropagateBackward();
00104 }
00105 
00106 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control* control, int steps, base::State *result) const
00107 {
00108     if (steps == 0)
00109     {
00110         if (result != state)
00111             copyState(result, state);
00112     }
00113     else
00114     {
00115         double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
00116         steps = abs(steps);
00117 
00118         statePropagator_->propagate(state, control, signedStepSize, result);
00119         for (int i = 1 ; i < steps ; ++i)
00120             statePropagator_->propagate(result, control, signedStepSize, result);
00121     }
00122 }
00123 
00124 unsigned int ompl::control::SpaceInformation::propagateWhileValid(const base::State *state, const Control* control, int steps, base::State *result) const
00125 {
00126     if (steps == 0)
00127     {
00128         if (result != state)
00129             copyState(result, state);
00130         return 0;
00131     }
00132 
00133     double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
00134     steps = abs(steps);
00135 
00136     // perform the first step of propagation
00137     statePropagator_->propagate(state, control, signedStepSize, result);
00138 
00139     // if we found a valid state after one step, we can go on
00140     if (isValid(result))
00141     {
00142         base::State *temp1 = result;
00143         base::State *temp2 = allocState();
00144         base::State *toDelete = temp2;
00145         unsigned int r = steps;
00146 
00147         // for the remaining number of steps
00148         for (int i = 1 ; i < steps ; ++i)
00149         {
00150             statePropagator_->propagate(temp1, control, signedStepSize, temp2);
00151             if (isValid(temp2))
00152                 std::swap(temp1, temp2);
00153             else
00154             {
00155                 // the last valid state is temp1;
00156                 r = i;
00157                 break;
00158             }
00159         }
00160 
00161         // if we finished the for-loop without finding an invalid state, the last valid state is temp1
00162         // make sure result contains that information
00163         if (result != temp1)
00164             copyState(result, temp1);
00165 
00166         // free the temporary memory
00167         freeState(toDelete);
00168 
00169         return r;
00170     }
00171     // if the first propagation step produced an invalid step, return 0 steps
00172     // the last valid state is the starting one (assumed to be valid)
00173     else
00174     {
00175         if (result != state)
00176             copyState(result, state);
00177         return 0;
00178     }
00179 }
00180 
00181 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control* control, int steps, std::vector<base::State*> &result, bool alloc) const
00182 {
00183     double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
00184     steps = abs(steps);
00185 
00186     if (alloc)
00187     {
00188         result.resize(steps);
00189         for (unsigned int i = 0 ; i < result.size() ; ++i)
00190             result[i] = allocState();
00191     }
00192     else
00193     {
00194         if (result.empty())
00195             return;
00196         steps = std::min(steps, (int)result.size());
00197     }
00198 
00199     int st = 0;
00200 
00201     if (st < steps)
00202     {
00203         statePropagator_->propagate(state, control, signedStepSize, result[st]);
00204         ++st;
00205 
00206         while (st < steps)
00207         {
00208             statePropagator_->propagate(result[st-1], control, signedStepSize, result[st]);
00209             ++st;
00210         }
00211     }
00212 }
00213 
00214 unsigned int ompl::control::SpaceInformation::propagateWhileValid(const base::State *state, const Control* control, int steps, std::vector<base::State*> &result, bool alloc) const
00215 {
00216     double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
00217     steps = abs(steps);
00218 
00219     if (alloc)
00220         result.resize(steps);
00221     else
00222     {
00223         if (result.empty())
00224             return 0;
00225         steps = std::min(steps, (int)result.size());
00226     }
00227 
00228     int st = 0;
00229 
00230     if (st < steps)
00231     {
00232         if (alloc)
00233             result[st] = allocState();
00234         statePropagator_->propagate(state, control, signedStepSize, result[st]);
00235 
00236         if (isValid(result[st]))
00237         {
00238             ++st;
00239             while (st < steps)
00240             {
00241                 if (alloc)
00242                     result[st] = allocState();
00243                 statePropagator_->propagate(result[st-1], control, signedStepSize, result[st]);
00244 
00245                 if (!isValid(result[st]))
00246                 {
00247                     if (alloc)
00248                     {
00249                         freeState(result[st]);
00250                         result.resize(st);
00251                     }
00252                     break;
00253                 }
00254                 else
00255                     ++st;
00256             }
00257         }
00258         else
00259         {
00260             if (alloc)
00261             {
00262                 freeState(result[st]);
00263                 result.resize(st);
00264             }
00265         }
00266     }
00267 
00268     return st;
00269 }
00270 
00271 void ompl::control::SpaceInformation::printSettings(std::ostream &out) const
00272 {
00273     base::SpaceInformation::printSettings(out);
00274     out << "  - control space:" << std::endl;
00275     controlSpace_->printSettings(out);
00276     out << "  - can propagate backward: " << (canPropagateBackward() ? "yes" : "no") << std::endl;
00277     out << "  - propagation step size: " << stepSize_ << std::endl;
00278     out << "  - propagation duration: [" << minSteps_ << ", " << maxSteps_ << "]" << std::endl;
00279 }