00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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 }