All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
RealVectorStateSpace.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/base/spaces/RealVectorStateSpace.h"
00038 #include "ompl/base/spaces/RealVectorStateProjections.h"
00039 #include "ompl/util/Exception.h"
00040 #include <boost/lexical_cast.hpp>
00041 #include <algorithm>
00042 #include <cstring>
00043 #include <limits>
00044 #include <cmath>
00045 
00046 void ompl::base::RealVectorStateSampler::sampleUniform(State *state)
00047 {
00048     const unsigned int dim = space_->getDimension();
00049     const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace*>(space_)->getBounds();
00050 
00051     RealVectorStateSpace::StateType *rstate = static_cast<RealVectorStateSpace::StateType*>(state);
00052     for (unsigned int i = 0 ; i < dim ; ++i)
00053         rstate->values[i] = rng_.uniformReal(bounds.low[i], bounds.high[i]);
00054 }
00055 
00056 void ompl::base::RealVectorStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
00057 {
00058     const unsigned int dim = space_->getDimension();
00059     const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace*>(space_)->getBounds();
00060 
00061     RealVectorStateSpace::StateType *rstate = static_cast<RealVectorStateSpace::StateType*>(state);
00062     const RealVectorStateSpace::StateType *rnear = static_cast<const RealVectorStateSpace::StateType*>(near);
00063     for (unsigned int i = 0 ; i < dim ; ++i)
00064         rstate->values[i] =
00065             rng_.uniformReal(std::max(bounds.low[i], rnear->values[i] - distance),
00066                              std::min(bounds.high[i], rnear->values[i] + distance));
00067 }
00068 
00069 void ompl::base::RealVectorStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
00070 {
00071     const unsigned int dim = space_->getDimension();
00072     const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace*>(space_)->getBounds();
00073 
00074     RealVectorStateSpace::StateType *rstate = static_cast<RealVectorStateSpace::StateType*>(state);
00075     const RealVectorStateSpace::StateType *rmean = static_cast<const RealVectorStateSpace::StateType*>(mean);
00076     for (unsigned int i = 0 ; i < dim ; ++i)
00077     {
00078         double v = rng_.gaussian(rmean->values[i], stdDev);
00079         if (v < bounds.low[i])
00080             v = bounds.low[i];
00081         else
00082             if (v > bounds.high[i])
00083                 v = bounds.high[i];
00084         rstate->values[i] = v;
00085     }
00086 }
00087 
00088 void ompl::base::RealVectorStateSpace::registerProjections(void)
00089 {
00090     // compute a default random projection
00091     if (dimension_ > 0)
00092     {
00093         if (dimension_ > 2)
00094         {
00095             int p = std::max(2, (int)ceil(log((double)dimension_)));
00096             registerDefaultProjection(ProjectionEvaluatorPtr(new RealVectorRandomLinearProjectionEvaluator(this, p)));
00097         }
00098         else
00099             registerDefaultProjection(ProjectionEvaluatorPtr(new RealVectorIdentityProjectionEvaluator(this)));
00100     }
00101 }
00102 
00103 void ompl::base::RealVectorStateSpace::setup(void)
00104 {
00105     bounds_.check();
00106     StateSpace::setup();
00107 }
00108 
00109 void ompl::base::RealVectorStateSpace::addDimension(const std::string &name, double minBound, double maxBound)
00110 {
00111     addDimension(minBound, maxBound);
00112     setDimensionName(dimension_ - 1, name);
00113 }
00114 
00115 void ompl::base::RealVectorStateSpace::addDimension(double minBound, double maxBound)
00116 {
00117     dimension_++;
00118     stateBytes_ = dimension_ * sizeof(double);
00119     bounds_.low.push_back(minBound);
00120     bounds_.high.push_back(maxBound);
00121     dimensionNames_.resize(dimension_, "");
00122 }
00123 
00124 void ompl::base::RealVectorStateSpace::setBounds(const RealVectorBounds &bounds)
00125 {
00126     bounds.check();
00127     if (bounds.low.size() != dimension_)
00128         throw Exception("Bounds do not match dimension of state space: expected dimension " +
00129                         boost::lexical_cast<std::string>(dimension_) + " but got dimension " +
00130                         boost::lexical_cast<std::string>(bounds.low.size()));
00131     bounds_ = bounds;
00132 }
00133 
00134 void ompl::base::RealVectorStateSpace::setBounds(double low, double high)
00135 {
00136     RealVectorBounds bounds(dimension_);
00137     bounds.setLow(low);
00138     bounds.setHigh(high);
00139     setBounds(bounds);
00140 }
00141 
00142 unsigned int ompl::base::RealVectorStateSpace::getDimension(void) const
00143 {
00144     return dimension_;
00145 }
00146 
00147 const std::string& ompl::base::RealVectorStateSpace::getDimensionName(unsigned int index) const
00148 {
00149     if (index < dimensionNames_.size())
00150         return dimensionNames_[index];
00151     throw Exception("Index out of bounds");
00152 }
00153 
00154 int ompl::base::RealVectorStateSpace::getDimensionIndex(const std::string &name) const
00155 {
00156     std::map<std::string, unsigned int>::const_iterator it = dimensionIndex_.find(name);
00157     return it != dimensionIndex_.end() ? it->second : -1;
00158 }
00159 
00160 void ompl::base::RealVectorStateSpace::setDimensionName(unsigned int index, const std::string &name)
00161 {
00162     if (index < dimensionNames_.size())
00163     {
00164         dimensionNames_[index] = name;
00165         dimensionIndex_[name] = index;
00166     }
00167     else
00168         throw Exception("Cannot set dimension name. Index out of bounds");
00169 }
00170 
00171 double ompl::base::RealVectorStateSpace::getMaximumExtent(void) const
00172 {
00173     double e = 0.0;
00174     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00175     {
00176         double d = bounds_.high[i] - bounds_.low[i];
00177         e += d*d;
00178     }
00179     return sqrt(e);
00180 }
00181 
00182 void ompl::base::RealVectorStateSpace::enforceBounds(State *state) const
00183 {
00184     StateType *rstate = static_cast<StateType*>(state);
00185     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00186     {
00187         if (rstate->values[i] > bounds_.high[i])
00188             rstate->values[i] = bounds_.high[i];
00189         else
00190             if (rstate->values[i] < bounds_.low[i])
00191                 rstate->values[i] = bounds_.low[i];
00192     }
00193 }
00194 
00195 bool ompl::base::RealVectorStateSpace::satisfiesBounds(const State *state) const
00196 {
00197     const StateType *rstate = static_cast<const StateType*>(state);
00198     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00199         if (rstate->values[i] - std::numeric_limits<double>::epsilon() > bounds_.high[i] ||
00200             rstate->values[i] + std::numeric_limits<double>::epsilon() < bounds_.low[i])
00201             return false;
00202     return true;
00203 }
00204 
00205 void ompl::base::RealVectorStateSpace::copyState(State *destination, const State *source) const
00206 {
00207     memcpy(static_cast<StateType*>(destination)->values,
00208            static_cast<const StateType*>(source)->values, stateBytes_);
00209 }
00210 
00211 double ompl::base::RealVectorStateSpace::distance(const State *state1, const State *state2) const
00212 {
00213     double dist = 0.0;
00214     const double *s1 = static_cast<const StateType*>(state1)->values;
00215     const double *s2 = static_cast<const StateType*>(state2)->values;
00216 
00217     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00218     {
00219         double diff = (*s1++) - (*s2++);
00220         dist += diff * diff;
00221     }
00222     return sqrt(dist);
00223 }
00224 
00225 bool ompl::base::RealVectorStateSpace::equalStates(const State *state1, const State *state2) const
00226 {
00227     const double *s1 = static_cast<const StateType*>(state1)->values;
00228     const double *s2 = static_cast<const StateType*>(state2)->values;
00229     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00230     {
00231         double diff = (*s1++) - (*s2++);
00232         if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
00233             return false;
00234     }
00235     return true;
00236 }
00237 
00238 void ompl::base::RealVectorStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00239 {
00240     const StateType *rfrom = static_cast<const StateType*>(from);
00241     const StateType *rto = static_cast<const StateType*>(to);
00242     const StateType *rstate = static_cast<StateType*>(state);
00243     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00244         rstate->values[i] = rfrom->values[i] + (rto->values[i] - rfrom->values[i]) * t;
00245 }
00246 
00247 ompl::base::StateSamplerPtr ompl::base::RealVectorStateSpace::allocStateSampler(void) const
00248 {
00249     return StateSamplerPtr(new RealVectorStateSampler(this));
00250 }
00251 
00252 ompl::base::State* ompl::base::RealVectorStateSpace::allocState(void) const
00253 {
00254     StateType *rstate = new StateType();
00255     rstate->values = new double[dimension_];
00256     return rstate;
00257 }
00258 
00259 void ompl::base::RealVectorStateSpace::freeState(State *state) const
00260 {
00261     StateType *rstate = static_cast<StateType*>(state);
00262     delete[] rstate->values;
00263     delete rstate;
00264 }
00265 
00266 double* ompl::base::RealVectorStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00267 {
00268     return index < dimension_ ? static_cast<StateType*>(state)->values + index : NULL;
00269 }
00270 
00271 void ompl::base::RealVectorStateSpace::printState(const State *state, std::ostream &out) const
00272 {
00273     out << "RealVectorState [";
00274     if (state)
00275     {
00276         const StateType *rstate = static_cast<const StateType*>(state);
00277         for (unsigned int i = 0 ; i < dimension_ ; ++i)
00278         {
00279             out << rstate->values[i];
00280             if (i + 1 < dimension_)
00281                 out << ' ';
00282         }
00283     }
00284     else
00285         out << "NULL" << std::endl;
00286     out << ']' << std::endl;
00287 }
00288 
00289 void ompl::base::RealVectorStateSpace::printSettings(std::ostream &out) const
00290 {
00291     out << "Real vector state space '" << getName() << "' of dimension " << dimension_ << " with bounds: " << std::endl;
00292     out << "  - min: ";
00293     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00294         out << bounds_.low[i] << " ";
00295     out << std::endl;
00296     out << "  - max: ";
00297     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00298         out << bounds_.high[i] << " ";
00299     out << std::endl;
00300 
00301     bool printNames = false;
00302     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00303         if (!dimensionNames_[i].empty())
00304             printNames = true;
00305     if (printNames)
00306     {
00307         out << "  and dimension names: ";
00308         for (unsigned int i = 0 ; i < dimension_ ; ++i)
00309             out << "'" << dimensionNames_[i] << "' ";
00310         out << std::endl;
00311     }
00312 }