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() ? (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 }