TimeStateSpace.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/TimeStateSpace.h"
38 #include "ompl/util/Exception.h"
39 #include "ompl/tools/config/MagicConstants.h"
40 #include <limits>
41 
43 {
44  if (space_->as<TimeStateSpace>()->isBounded())
46  space_->as<TimeStateSpace>()->getMaxTimeBound());
47  else
48  state->as<TimeStateSpace::StateType>()->position = 0.0;
49 }
50 
51 void ompl::base::TimeStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
52 {
53  state->as<TimeStateSpace::StateType>()->position =
55  near->as<TimeStateSpace::StateType>()->position + distance);
56  space_->enforceBounds(state);
57 }
58 
59 void ompl::base::TimeStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
60 {
61  state->as<TimeStateSpace::StateType>()->position =
63  space_->enforceBounds(state);
64 }
65 
67 {
68  return 1;
69 }
70 
71 void ompl::base::TimeStateSpace::setBounds(double minTime, double maxTime)
72 {
73  if (minTime > maxTime)
74  throw Exception("The maximum position in time cannot be before the minimum position in time");
75 
76  minTime_ = minTime;
77  maxTime_ = maxTime;
78  bounded_ = true;
79 }
80 
82 {
83  return bounded_ ? maxTime_ - minTime_ : 1.0;
84 }
85 
87 {
88  return getMaximumExtent();
89 }
90 
92 {
93  if (bounded_)
94  {
95  if (state->as<StateType>()->position > maxTime_)
96  state->as<StateType>()->position = maxTime_;
97  else
98  if (state->as<StateType>()->position < minTime_)
99  state->as<StateType>()->position = minTime_;
100  }
101 }
102 
104 {
105  return !bounded_ || (state->as<StateType>()->position >= minTime_ - std::numeric_limits<double>::epsilon() &&
106  state->as<StateType>()->position <= maxTime_ + std::numeric_limits<double>::epsilon());
107 }
108 
109 void ompl::base::TimeStateSpace::copyState(State *destination, const State *source) const
110 {
111  destination->as<StateType>()->position = source->as<StateType>()->position;
112 }
113 
115 {
116  return sizeof(double);
117 }
118 
119 void ompl::base::TimeStateSpace::serialize(void *serialization, const State *state) const
120 {
121  memcpy(serialization, &state->as<StateType>()->position, sizeof(double));
122 }
123 
124 void ompl::base::TimeStateSpace::deserialize(State *state, const void *serialization) const
125 {
126  memcpy(&state->as<StateType>()->position, serialization, sizeof(double));
127 }
128 
129 double ompl::base::TimeStateSpace::distance(const State *state1, const State *state2) const
130 {
131  return fabs(state1->as<StateType>()->position - state2->as<StateType>()->position);
132 }
133 
134 bool ompl::base::TimeStateSpace::equalStates(const State *state1, const State *state2) const
135 {
136  return fabs(state1->as<StateType>()->position - state2->as<StateType>()->position) < std::numeric_limits<double>::epsilon() * 2.0;
137 }
138 
139 void ompl::base::TimeStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
140 {
141  state->as<StateType>()->position = from->as<StateType>()->position +
142  (to->as<StateType>()->position - from->as<StateType>()->position) * t;
143 }
144 
146 {
147  return StateSamplerPtr(new TimeStateSampler(this));
148 }
149 
151 {
152  return new StateType();
153 }
154 
156 {
157  delete static_cast<StateType*>(state);
158 }
159 
161 {
162  class TimeDefaultProjection : public ProjectionEvaluator
163  {
164  public:
165 
166  TimeDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
167  {
168  }
169 
170  virtual unsigned int getDimension() const
171  {
172  return 1;
173  }
174 
175  virtual void defaultCellSizes()
176  {
177  cellSizes_.resize(1);
178  if (space_->as<TimeStateSpace>()->isBounded())
179  {
180  bounds_.resize(1);
181  bounds_.low[0] = space_->as<TimeStateSpace>()->getMinTimeBound();
182  bounds_.high[0] = space_->as<TimeStateSpace>()->getMaxTimeBound();
183  cellSizes_[0] = bounds_.getDifference()[0] / magic::PROJECTION_DIMENSION_SPLITS;
184  }
185  else
186  cellSizes_[0] = 1.0;
187  }
188 
189  virtual void project(const State *state, EuclideanProjection &projection) const
190  {
191  projection(0) = state->as<TimeStateSpace::StateType>()->position;
192  }
193  };
194 
195  registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new TimeDefaultProjection(this))));
196 }
197 
198 double* ompl::base::TimeStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
199 {
200  return index == 0 ? &(state->as<StateType>()->position) : NULL;
201 }
202 
203 void ompl::base::TimeStateSpace::printState(const State *state, std::ostream &out) const
204 {
205  out << "TimeState [";
206  if (state)
207  out << state->as<StateType>()->position;
208  else
209  out << "NULL";
210  out << ']' << std::endl;
211 }
212 
213 void ompl::base::TimeStateSpace::printSettings(std::ostream &out) const
214 {
215  out << "Time state space '" << getName() << "'" << std::endl;
216 }
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:91
virtual double getMaximumExtent() const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
virtual void freeState(State *state) const
Free the memory of the allocated state.
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
virtual State * allocState() const
Allocate a state that can store a point in the described space.
virtual void enforceBounds(State *state) const
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
virtual StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
A boost shared pointer wrapper for ompl::base::StateSampler.
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:94
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
virtual double getMeasure() const
Get a measure of the space (this can be thought of as a generalization of volume) ...
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:87
The definition of a time state.
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
virtual bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.
virtual unsigned int getDimension() const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
bool isBounded() const
Check if the time is bounded or not.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
static const double PROJECTION_DIMENSION_SPLITS
When the cell sizes for a projection are automatically computed, this value defines the number of par...
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition: RandomNumbers.h:68
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
virtual void enforceBounds(State *state) const =0
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
virtual double * getValueAddressAtIndex(State *state, const unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
Definition: State.h:50
double getMinTimeBound() const
Get the minimum allowed value of position in a state. The function returns 0 if time is not bounded...
virtual void sampleUniformNear(State *state, const State *near, const double distance)
Sample a state near another, within specified distance.
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
The exception type for ompl.
Definition: Exception.h:47
virtual void interpolate(const State *from, const State *to, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
virtual bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
TimeStateSampler(const StateSpace *space)
Constructor.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
void setBounds(double minTime, double maxTime)
Set the minimum and maximum time bounds. This will make the state space switch into bounded time mode...
virtual void sampleUniform(State *state)
Sample a state.
A state space representing time. The time can be unbounded, in which case enforceBounds() is a no-op...
virtual void sampleGaussian(State *state, const State *mean, const double stdDev)
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev) ...
double position
The position in time.
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
double gaussian(double mean, double stddev)
Generate a random real using a normal distribution with given mean and variance.
Definition: RandomNumbers.h:94