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