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 }
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) ...
virtual void freeState(State *state) const
Free the memory of the allocated state.
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:91
virtual double getMeasure() const
Get a measure of the space (this can be thought of as a generalization of volume) ...
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...
A boost shared pointer wrapper for ompl::base::StateSampler.
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:94
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
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...
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
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...
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 bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
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
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
virtual double getMaximumExtent() const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
Definition of an abstract state.
Definition: State.h:50
virtual State * allocState() const
Allocate a state that can store a point in the described space.
virtual void sampleUniformNear(State *state, const State *near, const double distance)
Sample a state near another, within specified distance.
The exception type for ompl.
Definition: Exception.h:47
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 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 void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
TimeStateSampler(const StateSpace *space)
Constructor.
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 StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
double getMinTimeBound() const
Get the minimum allowed value of position in a state. The function returns 0 if time is not bounded...
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 void printState(const State *state, std::ostream &out) const
Print a state to a stream.
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