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