ompl/base/spaces/src/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()
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()
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() 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() ? (int)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() 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 double ompl::base::RealVectorStateSpace::getMeasure() const
00183 {
00184     double m = 1.0;
00185     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00186     {
00187         m *= bounds_.high[i] - bounds_.low[i];
00188     }
00189     return m;
00190 }
00191 
00192 void ompl::base::RealVectorStateSpace::enforceBounds(State *state) const
00193 {
00194     StateType *rstate = static_cast<StateType*>(state);
00195     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00196     {
00197         if (rstate->values[i] > bounds_.high[i])
00198             rstate->values[i] = bounds_.high[i];
00199         else
00200             if (rstate->values[i] < bounds_.low[i])
00201                 rstate->values[i] = bounds_.low[i];
00202     }
00203 }
00204 
00205 bool ompl::base::RealVectorStateSpace::satisfiesBounds(const State *state) const
00206 {
00207     const StateType *rstate = static_cast<const StateType*>(state);
00208     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00209         if (rstate->values[i] - std::numeric_limits<double>::epsilon() > bounds_.high[i] ||
00210             rstate->values[i] + std::numeric_limits<double>::epsilon() < bounds_.low[i])
00211             return false;
00212     return true;
00213 }
00214 
00215 void ompl::base::RealVectorStateSpace::copyState(State *destination, const State *source) const
00216 {
00217     memcpy(static_cast<StateType*>(destination)->values,
00218            static_cast<const StateType*>(source)->values, stateBytes_);
00219 }
00220 
00221 unsigned int ompl::base::RealVectorStateSpace::getSerializationLength() const
00222 {
00223     return stateBytes_;
00224 }
00225 
00226 void ompl::base::RealVectorStateSpace::serialize(void *serialization, const State *state) const
00227 {
00228     memcpy(serialization, state->as<StateType>()->values, stateBytes_);
00229 }
00230 
00231 void ompl::base::RealVectorStateSpace::deserialize(State *state, const void *serialization) const
00232 {
00233     memcpy(state->as<StateType>()->values, serialization, stateBytes_);
00234 }
00235 
00236 double ompl::base::RealVectorStateSpace::distance(const State *state1, const State *state2) const
00237 {
00238     double dist = 0.0;
00239     const double *s1 = static_cast<const StateType*>(state1)->values;
00240     const double *s2 = static_cast<const StateType*>(state2)->values;
00241 
00242     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00243     {
00244         double diff = (*s1++) - (*s2++);
00245         dist += diff * diff;
00246     }
00247     return sqrt(dist);
00248 }
00249 
00250 bool ompl::base::RealVectorStateSpace::equalStates(const State *state1, const State *state2) const
00251 {
00252     const double *s1 = static_cast<const StateType*>(state1)->values;
00253     const double *s2 = static_cast<const StateType*>(state2)->values;
00254     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00255     {
00256         double diff = (*s1++) - (*s2++);
00257         if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
00258             return false;
00259     }
00260     return true;
00261 }
00262 
00263 void ompl::base::RealVectorStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00264 {
00265     const StateType *rfrom = static_cast<const StateType*>(from);
00266     const StateType *rto = static_cast<const StateType*>(to);
00267     const StateType *rstate = static_cast<StateType*>(state);
00268     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00269         rstate->values[i] = rfrom->values[i] + (rto->values[i] - rfrom->values[i]) * t;
00270 }
00271 
00272 ompl::base::StateSamplerPtr ompl::base::RealVectorStateSpace::allocDefaultStateSampler() const
00273 {
00274     return StateSamplerPtr(new RealVectorStateSampler(this));
00275 }
00276 
00277 ompl::base::State* ompl::base::RealVectorStateSpace::allocState() const
00278 {
00279     StateType *rstate = new StateType();
00280     rstate->values = new double[dimension_];
00281     return rstate;
00282 }
00283 
00284 void ompl::base::RealVectorStateSpace::freeState(State *state) const
00285 {
00286     StateType *rstate = static_cast<StateType*>(state);
00287     delete[] rstate->values;
00288     delete rstate;
00289 }
00290 
00291 double* ompl::base::RealVectorStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00292 {
00293     return index < dimension_ ? static_cast<StateType*>(state)->values + index : NULL;
00294 }
00295 
00296 void ompl::base::RealVectorStateSpace::printState(const State *state, std::ostream &out) const
00297 {
00298     out << "RealVectorState [";
00299     if (state)
00300     {
00301         const StateType *rstate = static_cast<const StateType*>(state);
00302         for (unsigned int i = 0 ; i < dimension_ ; ++i)
00303         {
00304             out << rstate->values[i];
00305             if (i + 1 < dimension_)
00306                 out << ' ';
00307         }
00308     }
00309     else
00310         out << "NULL" << std::endl;
00311     out << ']' << std::endl;
00312 }
00313 
00314 void ompl::base::RealVectorStateSpace::printSettings(std::ostream &out) const
00315 {
00316     out << "Real vector state space '" << getName() << "' of dimension " << dimension_ << " with bounds: " << std::endl;
00317     out << "  - min: ";
00318     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00319         out << bounds_.low[i] << " ";
00320     out << std::endl;
00321     out << "  - max: ";
00322     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00323         out << bounds_.high[i] << " ";
00324     out << std::endl;
00325 
00326     bool printNames = false;
00327     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00328         if (!dimensionNames_[i].empty())
00329             printNames = true;
00330     if (printNames)
00331     {
00332         out << "  and dimension names: ";
00333         for (unsigned int i = 0 ; i < dimension_ ; ++i)
00334             out << "'" << dimensionNames_[i] << "' ";
00335         out << std::endl;
00336     }
00337 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines