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 <algorithm>
41 #include <cstring>
42 #include <limits>
43 #include <cmath>
44 
46 {
47  const unsigned int dim = space_->getDimension();
48  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace *>(space_)->getBounds();
49 
50  auto *rstate = static_cast<RealVectorStateSpace::StateType *>(state);
51  for (unsigned int i = 0; i < dim; ++i)
52  rstate->values[i] = rng_.uniformReal(bounds.low[i], bounds.high[i]);
53 }
54 
55 void ompl::base::RealVectorStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
56 {
57  const unsigned int dim = space_->getDimension();
58  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace *>(space_)->getBounds();
59 
60  auto *rstate = static_cast<RealVectorStateSpace::StateType *>(state);
61  const auto *rnear = static_cast<const RealVectorStateSpace::StateType *>(near);
62  for (unsigned int i = 0; i < dim; ++i)
63  rstate->values[i] = rng_.uniformReal(std::max(bounds.low[i], rnear->values[i] - distance),
64  std::min(bounds.high[i], rnear->values[i] + distance));
65 }
66 
67 void ompl::base::RealVectorStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
68 {
69  const unsigned int dim = space_->getDimension();
70  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace *>(space_)->getBounds();
71 
72  auto *rstate = static_cast<RealVectorStateSpace::StateType *>(state);
73  const auto *rmean = static_cast<const RealVectorStateSpace::StateType *>(mean);
74  for (unsigned int i = 0; i < dim; ++i)
75  {
76  double v = rng_.gaussian(rmean->values[i], stdDev);
77  if (v < bounds.low[i])
78  v = bounds.low[i];
79  else if (v > bounds.high[i])
80  v = bounds.high[i];
81  rstate->values[i] = v;
82  }
83 }
84 
86 {
87  // compute a default random projection
88  if (dimension_ > 0)
89  {
90  if (dimension_ > 2)
91  {
92  int p = std::max(2, (int)ceil(log((double)dimension_)));
93  registerDefaultProjection(std::make_shared<RealVectorRandomLinearProjectionEvaluator>(this, p));
94  }
95  else
96  registerDefaultProjection(std::make_shared<RealVectorIdentityProjectionEvaluator>(this));
97  }
98 }
99 
101 {
102  bounds_.check();
104 }
105 
106 void ompl::base::RealVectorStateSpace::addDimension(const std::string &name, double minBound, double maxBound)
107 {
108  addDimension(minBound, maxBound);
109  setDimensionName(dimension_ - 1, name);
110 }
111 
112 void ompl::base::RealVectorStateSpace::addDimension(double minBound, double maxBound)
113 {
114  dimension_++;
115  stateBytes_ = dimension_ * sizeof(double);
116  bounds_.low.push_back(minBound);
117  bounds_.high.push_back(maxBound);
118  dimensionNames_.resize(dimension_, "");
119 }
120 
122 {
123  bounds.check();
124  if (bounds.low.size() != dimension_)
125  throw Exception("Bounds do not match dimension of state space: expected dimension " +
126  std::to_string(dimension_) + " but got dimension " + std::to_string(bounds.low.size()));
127  bounds_ = bounds;
128 }
129 
130 void ompl::base::RealVectorStateSpace::setBounds(double low, double high)
131 {
132  RealVectorBounds bounds(dimension_);
133  bounds.setLow(low);
134  bounds.setHigh(high);
135  setBounds(bounds);
136 }
137 
139 {
140  return dimension_;
141 }
142 
143 const std::string &ompl::base::RealVectorStateSpace::getDimensionName(unsigned int index) const
144 {
145  if (index < dimensionNames_.size())
146  return dimensionNames_[index];
147  throw Exception("Index out of bounds");
148 }
149 
150 int ompl::base::RealVectorStateSpace::getDimensionIndex(const std::string &name) const
151 {
152  auto it = dimensionIndex_.find(name);
153  return it != dimensionIndex_.end() ? (int)it->second : -1;
154 }
155 
156 void ompl::base::RealVectorStateSpace::setDimensionName(unsigned int index, const std::string &name)
157 {
158  if (index < dimensionNames_.size())
159  {
160  dimensionNames_[index] = name;
161  dimensionIndex_[name] = index;
162  }
163  else
164  throw Exception("Cannot set dimension name. Index out of bounds");
165 }
166 
168 {
169  double e = 0.0;
170  for (unsigned int i = 0; i < dimension_; ++i)
171  {
172  double d = bounds_.high[i] - bounds_.low[i];
173  e += d * d;
174  }
175  return sqrt(e);
176 }
177 
179 {
180  double m = 1.0;
181  for (unsigned int i = 0; i < dimension_; ++i)
182  {
183  m *= bounds_.high[i] - bounds_.low[i];
184  }
185  return m;
186 }
187 
189 {
190  auto *rstate = static_cast<StateType *>(state);
191  for (unsigned int i = 0; i < dimension_; ++i)
192  {
193  if (rstate->values[i] > bounds_.high[i])
194  rstate->values[i] = bounds_.high[i];
195  else if (rstate->values[i] < bounds_.low[i])
196  rstate->values[i] = bounds_.low[i];
197  }
198 }
199 
201 {
202  const auto *rstate = static_cast<const StateType *>(state);
203  for (unsigned int i = 0; i < dimension_; ++i)
204  if (rstate->values[i] - std::numeric_limits<double>::epsilon() > bounds_.high[i] ||
205  rstate->values[i] + std::numeric_limits<double>::epsilon() < bounds_.low[i])
206  return false;
207  return true;
208 }
209 
210 void ompl::base::RealVectorStateSpace::copyState(State *destination, const State *source) const
211 {
212  memcpy(static_cast<StateType *>(destination)->values, static_cast<const StateType *>(source)->values, stateBytes_);
213 }
214 
216 {
217  return stateBytes_;
218 }
219 
220 void ompl::base::RealVectorStateSpace::serialize(void *serialization, const State *state) const
221 {
222  memcpy(serialization, state->as<StateType>()->values, stateBytes_);
223 }
224 
225 void ompl::base::RealVectorStateSpace::deserialize(State *state, const void *serialization) const
226 {
227  memcpy(state->as<StateType>()->values, serialization, stateBytes_);
228 }
229 
230 double ompl::base::RealVectorStateSpace::distance(const State *state1, const State *state2) const
231 {
232  double dist = 0.0;
233  const double *s1 = static_cast<const StateType *>(state1)->values;
234  const double *s2 = static_cast<const StateType *>(state2)->values;
235 
236  for (unsigned int i = 0; i < dimension_; ++i)
237  {
238  double diff = (*s1++) - (*s2++);
239  dist += diff * diff;
240  }
241  return sqrt(dist);
242 }
243 
244 bool ompl::base::RealVectorStateSpace::equalStates(const State *state1, const State *state2) const
245 {
246  const double *s1 = static_cast<const StateType *>(state1)->values;
247  const double *s2 = static_cast<const StateType *>(state2)->values;
248  for (unsigned int i = 0; i < dimension_; ++i)
249  {
250  double diff = (*s1++) - (*s2++);
251  if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
252  return false;
253  }
254  return true;
255 }
256 
257 void ompl::base::RealVectorStateSpace::interpolate(const State *from, const State *to, const double t,
258  State *state) const
259 {
260  const auto *rfrom = static_cast<const StateType *>(from);
261  const auto *rto = static_cast<const StateType *>(to);
262  const StateType *rstate = static_cast<StateType *>(state);
263  for (unsigned int i = 0; i < dimension_; ++i)
264  rstate->values[i] = rfrom->values[i] + (rto->values[i] - rfrom->values[i]) * t;
265 }
266 
268 {
269  return std::make_shared<RealVectorStateSampler>(this);
270 }
271 
273 {
274  auto *rstate = new StateType();
275  rstate->values = new double[dimension_];
276  return rstate;
277 }
278 
280 {
281  auto *rstate = static_cast<StateType *>(state);
282  delete[] rstate->values;
283  delete rstate;
284 }
285 
286 double *ompl::base::RealVectorStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
287 {
288  return index < dimension_ ? static_cast<StateType *>(state)->values + index : nullptr;
289 }
290 
291 void ompl::base::RealVectorStateSpace::printState(const State *state, std::ostream &out) const
292 {
293  out << "RealVectorState [";
294  if (state != nullptr)
295  {
296  const auto *rstate = static_cast<const StateType *>(state);
297  for (unsigned int i = 0; i < dimension_; ++i)
298  {
299  out << rstate->values[i];
300  if (i + 1 < dimension_)
301  out << ' ';
302  }
303  }
304  else
305  out << "nullptr" << std::endl;
306  out << ']' << std::endl;
307 }
308 
310 {
311  out << "Real vector state space '" << getName() << "' of dimension " << dimension_ << " with bounds: " << std::endl;
312  out << " - min: ";
313  for (unsigned int i = 0; i < dimension_; ++i)
314  out << bounds_.low[i] << " ";
315  out << std::endl;
316  out << " - max: ";
317  for (unsigned int i = 0; i < dimension_; ++i)
318  out << bounds_.high[i] << " ";
319  out << std::endl;
320 
321  bool printNames = false;
322  for (unsigned int i = 0; i < dimension_; ++i)
323  if (!dimensionNames_[i].empty())
324  printNames = true;
325  if (printNames)
326  {
327  out << " and dimension names: ";
328  for (unsigned int i = 0; i < dimension_; ++i)
329  out << "'" << dimensionNames_[i] << "' ";
330  out << std::endl;
331  }
332 }
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state such that each component state[i] has a Gaussian distribution with mean mean[i] and st...
void enforceBounds(State *state) const override
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
Definition of an abstract state.
Definition: State.h:50
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
double * values
The value of the actual vector in Rn
void addDimension(double minBound=0.0, double maxBound=0.0)
Increase the dimensionality of the state space by 1. Optionally, bounds can be specified for this add...
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
State * allocState() const override
Allocate a state that can store a point in the described space.
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
double * getValueAddressAtIndex(State *state, unsigned int index) const override
Many states contain a number of double values. This function provides a means to get the memory addre...
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
A state space representing Rn. The distance function is the L2 norm.
void sampleUniform(State *state) override
Sample a state.
int getDimensionIndex(const std::string &name) const
Get the index of a specific dimension, by name. Return -1 if name is not found.
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:104
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
void freeState(State *state) const override
Free the memory of the allocated state.
void interpolate(const State *from, const State *to, double t, State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
bool satisfiesBounds(const State *state) const override
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
void setBounds(const RealVectorBounds &bounds)
Set the bounds of this state space. This defines the range of the space in which sampling is performe...
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:107
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition: RandomNumbers.h:73
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state such that each component state[i] is uniformly sampled from [near[i]-distance,...
A shared pointer wrapper for ompl::base::StateSampler.
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation....
Definition: StateSpace.cpp:237
const std::string & getDimensionName(unsigned int index) const
Each dimension can optionally have a name associated to it. If it does, this function returns that na...
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
void setDimensionName(unsigned int index, const std::string &name)
Set the name of a dimension.
The exception type for ompl.
Definition: Exception.h:47
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
The lower and upper bounds for an Rn space.