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 }