Loading...
Searching...
No Matches
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
55void 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
67void 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
106void ompl::base::RealVectorStateSpace::addDimension(const std::string &name, double minBound, double maxBound)
107{
108 addDimension(minBound, maxBound);
109 setDimensionName(dimension_ - 1, name);
110}
111
112void 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
131{
132 RealVectorBounds bounds(dimension_);
133 bounds.setLow(low);
134 bounds.setHigh(high);
135 setBounds(bounds);
136}
137
139{
140 return dimension_;
141}
142
143const 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
151{
152 auto it = dimensionIndex_.find(name);
153 return it != dimensionIndex_.end() ? (int)it->second : -1;
154}
155
156void 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
210void 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
220void ompl::base::RealVectorStateSpace::serialize(void *serialization, const State *state) const
221{
222 memcpy(serialization, state->as<StateType>()->values, stateBytes_);
223}
224
225void ompl::base::RealVectorStateSpace::deserialize(State *state, const void *serialization) const
226{
227 memcpy(state->as<StateType>()->values, serialization, stateBytes_);
228}
229
230double 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
244bool 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
257void 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
286double *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
291void 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}
The exception type for ompl.
Definition Exception.h:47
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
The lower and upper bounds for an Rn space.
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 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,...
void sampleUniform(State *state) override
Sample a state.
double * values
The value of the actual vector in Rn
A state space representing Rn. The distance function is the L2 norm.
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-...
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
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...
void setDimensionName(unsigned int index, const std::string &name)
Set the name of a dimension.
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
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...
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...
State * allocState() const override
Allocate a state that can store a point in the described space.
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....
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
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.
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
void setBounds(const RealVectorBounds &bounds)
Set the bounds of this state space. This defines the range of the space in which sampling is performe...
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
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...
int getDimensionIndex(const std::string &name) const
Get the index of a specific dimension, by name. Return -1 if name is not found.
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
A shared pointer wrapper for ompl::base::StateSampler.
RNG rng_
An instance of a random number generator.
const StateSpace * space_
The state space this sampler samples.
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation....
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66