All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
RealVectorStateSpace.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/base/spaces/RealVectorStateSpace.h"
38 #include "ompl/base/spaces/RealVectorStateProjections.h"
39 #include "ompl/util/Exception.h"
40 #include <boost/lexical_cast.hpp>
41 #include <algorithm>
42 #include <cstring>
43 #include <limits>
44 #include <cmath>
45 
47 {
48  const unsigned int dim = space_->getDimension();
49  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace*>(space_)->getBounds();
50 
52  for (unsigned int i = 0 ; i < dim ; ++i)
53  rstate->values[i] = rng_.uniformReal(bounds.low[i], bounds.high[i]);
54 }
55 
56 void ompl::base::RealVectorStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
57 {
58  const unsigned int dim = space_->getDimension();
59  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace*>(space_)->getBounds();
60 
62  const RealVectorStateSpace::StateType *rnear = static_cast<const RealVectorStateSpace::StateType*>(near);
63  for (unsigned int i = 0 ; i < dim ; ++i)
64  rstate->values[i] =
65  rng_.uniformReal(std::max(bounds.low[i], rnear->values[i] - distance),
66  std::min(bounds.high[i], rnear->values[i] + distance));
67 }
68 
69 void ompl::base::RealVectorStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
70 {
71  const unsigned int dim = space_->getDimension();
72  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace*>(space_)->getBounds();
73 
75  const RealVectorStateSpace::StateType *rmean = static_cast<const RealVectorStateSpace::StateType*>(mean);
76  for (unsigned int i = 0 ; i < dim ; ++i)
77  {
78  double v = rng_.gaussian(rmean->values[i], stdDev);
79  if (v < bounds.low[i])
80  v = bounds.low[i];
81  else
82  if (v > bounds.high[i])
83  v = bounds.high[i];
84  rstate->values[i] = v;
85  }
86 }
87 
89 {
90  // compute a default random projection
91  if (dimension_ > 0)
92  {
93  if (dimension_ > 2)
94  {
95  int p = std::max(2, (int)ceil(log((double)dimension_)));
96  registerDefaultProjection(ProjectionEvaluatorPtr(new RealVectorRandomLinearProjectionEvaluator(this, p)));
97  }
98  else
99  registerDefaultProjection(ProjectionEvaluatorPtr(new RealVectorIdentityProjectionEvaluator(this)));
100  }
101 }
102 
104 {
105  bounds_.check();
107 }
108 
109 void ompl::base::RealVectorStateSpace::addDimension(const std::string &name, double minBound, double maxBound)
110 {
111  addDimension(minBound, maxBound);
112  setDimensionName(dimension_ - 1, name);
113 }
114 
115 void ompl::base::RealVectorStateSpace::addDimension(double minBound, double maxBound)
116 {
117  dimension_++;
118  stateBytes_ = dimension_ * sizeof(double);
119  bounds_.low.push_back(minBound);
120  bounds_.high.push_back(maxBound);
121  dimensionNames_.resize(dimension_, "");
122 }
123 
125 {
126  bounds.check();
127  if (bounds.low.size() != dimension_)
128  throw Exception("Bounds do not match dimension of state space: expected dimension " +
129  boost::lexical_cast<std::string>(dimension_) + " but got dimension " +
130  boost::lexical_cast<std::string>(bounds.low.size()));
131  bounds_ = bounds;
132 }
133 
134 void ompl::base::RealVectorStateSpace::setBounds(double low, double high)
135 {
136  RealVectorBounds bounds(dimension_);
137  bounds.setLow(low);
138  bounds.setHigh(high);
139  setBounds(bounds);
140 }
141 
143 {
144  return dimension_;
145 }
146 
147 const std::string& ompl::base::RealVectorStateSpace::getDimensionName(unsigned int index) const
148 {
149  if (index < dimensionNames_.size())
150  return dimensionNames_[index];
151  throw Exception("Index out of bounds");
152 }
153 
154 int ompl::base::RealVectorStateSpace::getDimensionIndex(const std::string &name) const
155 {
156  std::map<std::string, unsigned int>::const_iterator it = dimensionIndex_.find(name);
157  return it != dimensionIndex_.end() ? (int)it->second : -1;
158 }
159 
160 void ompl::base::RealVectorStateSpace::setDimensionName(unsigned int index, const std::string &name)
161 {
162  if (index < dimensionNames_.size())
163  {
164  dimensionNames_[index] = name;
165  dimensionIndex_[name] = index;
166  }
167  else
168  throw Exception("Cannot set dimension name. Index out of bounds");
169 }
170 
172 {
173  double e = 0.0;
174  for (unsigned int i = 0 ; i < dimension_ ; ++i)
175  {
176  double d = bounds_.high[i] - bounds_.low[i];
177  e += d*d;
178  }
179  return sqrt(e);
180 }
181 
183 {
184  StateType *rstate = static_cast<StateType*>(state);
185  for (unsigned int i = 0 ; i < dimension_ ; ++i)
186  {
187  if (rstate->values[i] > bounds_.high[i])
188  rstate->values[i] = bounds_.high[i];
189  else
190  if (rstate->values[i] < bounds_.low[i])
191  rstate->values[i] = bounds_.low[i];
192  }
193 }
194 
196 {
197  const StateType *rstate = static_cast<const StateType*>(state);
198  for (unsigned int i = 0 ; i < dimension_ ; ++i)
199  if (rstate->values[i] - std::numeric_limits<double>::epsilon() > bounds_.high[i] ||
200  rstate->values[i] + std::numeric_limits<double>::epsilon() < bounds_.low[i])
201  return false;
202  return true;
203 }
204 
205 void ompl::base::RealVectorStateSpace::copyState(State *destination, const State *source) const
206 {
207  memcpy(static_cast<StateType*>(destination)->values,
208  static_cast<const StateType*>(source)->values, stateBytes_);
209 }
210 
212 {
213  return stateBytes_;
214 }
215 
216 void ompl::base::RealVectorStateSpace::serialize(void *serialization, const State *state) const
217 {
218  memcpy(serialization, state->as<StateType>()->values, stateBytes_);
219 }
220 
221 void ompl::base::RealVectorStateSpace::deserialize(State *state, const void *serialization) const
222 {
223  memcpy(state->as<StateType>()->values, serialization, stateBytes_);
224 }
225 
226 double ompl::base::RealVectorStateSpace::distance(const State *state1, const State *state2) const
227 {
228  double dist = 0.0;
229  const double *s1 = static_cast<const StateType*>(state1)->values;
230  const double *s2 = static_cast<const StateType*>(state2)->values;
231 
232  for (unsigned int i = 0 ; i < dimension_ ; ++i)
233  {
234  double diff = (*s1++) - (*s2++);
235  dist += diff * diff;
236  }
237  return sqrt(dist);
238 }
239 
240 bool ompl::base::RealVectorStateSpace::equalStates(const State *state1, const State *state2) const
241 {
242  const double *s1 = static_cast<const StateType*>(state1)->values;
243  const double *s2 = static_cast<const StateType*>(state2)->values;
244  for (unsigned int i = 0 ; i < dimension_ ; ++i)
245  {
246  double diff = (*s1++) - (*s2++);
247  if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
248  return false;
249  }
250  return true;
251 }
252 
253 void ompl::base::RealVectorStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
254 {
255  const StateType *rfrom = static_cast<const StateType*>(from);
256  const StateType *rto = static_cast<const StateType*>(to);
257  const StateType *rstate = static_cast<StateType*>(state);
258  for (unsigned int i = 0 ; i < dimension_ ; ++i)
259  rstate->values[i] = rfrom->values[i] + (rto->values[i] - rfrom->values[i]) * t;
260 }
261 
263 {
264  return StateSamplerPtr(new RealVectorStateSampler(this));
265 }
266 
268 {
269  StateType *rstate = new StateType();
270  rstate->values = new double[dimension_];
271  return rstate;
272 }
273 
275 {
276  StateType *rstate = static_cast<StateType*>(state);
277  delete[] rstate->values;
278  delete rstate;
279 }
280 
281 double* ompl::base::RealVectorStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
282 {
283  return index < dimension_ ? static_cast<StateType*>(state)->values + index : NULL;
284 }
285 
286 void ompl::base::RealVectorStateSpace::printState(const State *state, std::ostream &out) const
287 {
288  out << "RealVectorState [";
289  if (state)
290  {
291  const StateType *rstate = static_cast<const StateType*>(state);
292  for (unsigned int i = 0 ; i < dimension_ ; ++i)
293  {
294  out << rstate->values[i];
295  if (i + 1 < dimension_)
296  out << ' ';
297  }
298  }
299  else
300  out << "NULL" << std::endl;
301  out << ']' << std::endl;
302 }
303 
305 {
306  out << "Real vector state space '" << getName() << "' of dimension " << dimension_ << " with bounds: " << std::endl;
307  out << " - min: ";
308  for (unsigned int i = 0 ; i < dimension_ ; ++i)
309  out << bounds_.low[i] << " ";
310  out << std::endl;
311  out << " - max: ";
312  for (unsigned int i = 0 ; i < dimension_ ; ++i)
313  out << bounds_.high[i] << " ";
314  out << std::endl;
315 
316  bool printNames = false;
317  for (unsigned int i = 0 ; i < dimension_ ; ++i)
318  if (!dimensionNames_[i].empty())
319  printNames = true;
320  if (printNames)
321  {
322  out << " and dimension names: ";
323  for (unsigned int i = 0 ; i < dimension_ ; ++i)
324  out << "'" << dimensionNames_[i] << "' ";
325  out << std::endl;
326  }
327 }