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>()->getMinTimeBound(), 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 if (state->as<StateType>()->position < minTime_)
98  state->as<StateType>()->position = minTime_;
99  }
100 }
101 
103 {
104  return !bounded_ || (state->as<StateType>()->position >= minTime_ - std::numeric_limits<double>::epsilon() &&
105  state->as<StateType>()->position <= maxTime_ + std::numeric_limits<double>::epsilon());
106 }
107 
108 void ompl::base::TimeStateSpace::copyState(State *destination, const State *source) const
109 {
110  destination->as<StateType>()->position = source->as<StateType>()->position;
111 }
112 
114 {
115  return sizeof(double);
116 }
117 
118 void ompl::base::TimeStateSpace::serialize(void *serialization, const State *state) const
119 {
120  memcpy(serialization, &state->as<StateType>()->position, sizeof(double));
121 }
122 
123 void ompl::base::TimeStateSpace::deserialize(State *state, const void *serialization) const
124 {
125  memcpy(&state->as<StateType>()->position, serialization, sizeof(double));
126 }
127 
128 double ompl::base::TimeStateSpace::distance(const State *state1, const State *state2) const
129 {
130  return fabs(state1->as<StateType>()->position - state2->as<StateType>()->position);
131 }
132 
133 bool ompl::base::TimeStateSpace::equalStates(const State *state1, const State *state2) const
134 {
135  return fabs(state1->as<StateType>()->position - state2->as<StateType>()->position) <
136  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 =
142  from->as<StateType>()->position + (to->as<StateType>()->position - from->as<StateType>()->position) * t;
143 }
144 
146 {
147  return std::make_shared<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  TimeDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
166  {
167  }
168 
169  unsigned int getDimension() const override
170  {
171  return 1;
172  }
173 
174  void defaultCellSizes() override
175  {
176  cellSizes_.resize(1);
177  if (space_->as<TimeStateSpace>()->isBounded())
178  {
179  bounds_.resize(1);
180  bounds_.low[0] = space_->as<TimeStateSpace>()->getMinTimeBound();
181  bounds_.high[0] = space_->as<TimeStateSpace>()->getMaxTimeBound();
182  cellSizes_[0] = bounds_.getDifference()[0] / magic::PROJECTION_DIMENSION_SPLITS;
183  }
184  else
185  cellSizes_[0] = 1.0;
186  }
187 
188  void project(const State *state, EuclideanProjection &projection) const override
189  {
190  projection(0) = state->as<TimeStateSpace::StateType>()->position;
191  }
192  };
193 
194  registerDefaultProjection(std::make_shared<TimeDefaultProjection>(this));
195 }
196 
197 double *ompl::base::TimeStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
198 {
199  return index == 0 ? &(state->as<StateType>()->position) : nullptr;
200 }
201 
202 void ompl::base::TimeStateSpace::printState(const State *state, std::ostream &out) const
203 {
204  out << "TimeState [";
205  if (state)
206  out << state->as<StateType>()->position;
207  else
208  out << "nullptr";
209  out << ']' << std::endl;
210 }
211 
212 void ompl::base::TimeStateSpace::printSettings(std::ostream &out) const
213 {
214  out << "Time state space '" << getName() << "'" << std::endl;
215 }
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:104
State * allocState() const override
Allocate a state that can store a point in the described space.
void sampleUniformNear(State *state, const State *near, const double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
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.
Definition: StateSampler.h:107
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...
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.
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume) ...
double * getValueAddressAtIndex(State *state, const unsigned int index) const override
Many states contain a number of double values. This function provides a means to get the memory addre...
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:87
The definition of a time state.
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
void sampleUniform(State *state) override
Sample a state.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
bool isBounded() const
Check if the time is bounded or not.
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
void interpolate(const State *from, const State *to, const 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 getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
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:74
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
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:49
The exception type for ompl.
Definition: Exception.h:46
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-...
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection...
void sampleGaussian(State *state, const State *mean, const double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev)...
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-...
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
void setBounds(double minTime, double maxTime)
Set the minimum and maximum time bounds. This will make the state space switch into bounded time mode...
A state space representing time. The time can be unbounded, in which case enforceBounds() is a no-op...
double getMinTimeBound() const
Get the minimum allowed value of position in a state. The function returns 0 if time is not bounded...
double position
The position in time.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
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.