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() ? (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(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 unsigned int ompl::base::RealVectorStateSpace::getSerializationLength(void) const
00212 {
00213     return stateBytes_;
00214 }
00215 
00216 void ompl::base::RealVectorStateSpace::serialize(void *serialization, const State *state) const
00217 {
00218     memcpy(serialization, state->as<StateType>()->values, stateBytes_);
00219 }
00220 
00221 void ompl::base::RealVectorStateSpace::deserialize(State *state, const void *serialization) const
00222 {
00223     memcpy(state->as<StateType>()->values, serialization, stateBytes_);
00224 }
00225 
00226 double ompl::base::RealVectorStateSpace::distance(const State *state1, const State *state2) const
00227 {
00228     double dist = 0.0;
00229     const double *s1 = static_cast<const StateType*>(state1)->values;
00230     const double *s2 = static_cast<const StateType*>(state2)->values;
00231 
00232     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00233     {
00234         double diff = (*s1++) - (*s2++);
00235         dist += diff * diff;
00236     }
00237     return sqrt(dist);
00238 }
00239 
00240 bool ompl::base::RealVectorStateSpace::equalStates(const State *state1, const State *state2) const
00241 {
00242     const double *s1 = static_cast<const StateType*>(state1)->values;
00243     const double *s2 = static_cast<const StateType*>(state2)->values;
00244     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00245     {
00246         double diff = (*s1++) - (*s2++);
00247         if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
00248             return false;
00249     }
00250     return true;
00251 }
00252 
00253 void ompl::base::RealVectorStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00254 {
00255     const StateType *rfrom = static_cast<const StateType*>(from);
00256     const StateType *rto = static_cast<const StateType*>(to);
00257     const StateType *rstate = static_cast<StateType*>(state);
00258     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00259         rstate->values[i] = rfrom->values[i] + (rto->values[i] - rfrom->values[i]) * t;
00260 }
00261 
00262 ompl::base::StateSamplerPtr ompl::base::RealVectorStateSpace::allocDefaultStateSampler(void) const
00263 {
00264     return StateSamplerPtr(new RealVectorStateSampler(this));
00265 }
00266 
00267 ompl::base::State* ompl::base::RealVectorStateSpace::allocState(void) const
00268 {
00269     StateType *rstate = new StateType();
00270     rstate->values = new double[dimension_];
00271     return rstate;
00272 }
00273 
00274 void ompl::base::RealVectorStateSpace::freeState(State *state) const
00275 {
00276     StateType *rstate = static_cast<StateType*>(state);
00277     delete[] rstate->values;
00278     delete rstate;
00279 }
00280 
00281 double* ompl::base::RealVectorStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00282 {
00283     return index < dimension_ ? static_cast<StateType*>(state)->values + index : NULL;
00284 }
00285 
00286 void ompl::base::RealVectorStateSpace::printState(const State *state, std::ostream &out) const
00287 {
00288     out << "RealVectorState [";
00289     if (state)
00290     {
00291         const StateType *rstate = static_cast<const StateType*>(state);
00292         for (unsigned int i = 0 ; i < dimension_ ; ++i)
00293         {
00294             out << rstate->values[i];
00295             if (i + 1 < dimension_)
00296                 out << ' ';
00297         }
00298     }
00299     else
00300         out << "NULL" << std::endl;
00301     out << ']' << std::endl;
00302 }
00303 
00304 void ompl::base::RealVectorStateSpace::printSettings(std::ostream &out) const
00305 {
00306     out << "Real vector state space '" << getName() << "' of dimension " << dimension_ << " with bounds: " << std::endl;
00307     out << "  - min: ";
00308     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00309         out << bounds_.low[i] << " ";
00310     out << std::endl;
00311     out << "  - max: ";
00312     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00313         out << bounds_.high[i] << " ";
00314     out << std::endl;
00315 
00316     bool printNames = false;
00317     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00318         if (!dimensionNames_[i].empty())
00319             printNames = true;
00320     if (printNames)
00321     {
00322         out << "  and dimension names: ";
00323         for (unsigned int i = 0 ; i < dimension_ ; ++i)
00324             out << "'" << dimensionNames_[i] << "' ";
00325         out << std::endl;
00326     }
00327 }