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())
45  state->as<TimeStateSpace::StateType>()->position = rng_.uniformReal(
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 =
54  rng_.uniformReal(near->as<TimeStateSpace::StateType>()->position - distance,
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 =
62  rng_.gaussian(mean->as<TimeStateSpace::StateType>()->position, stdDev);
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, Eigen::Ref<Eigen::VectorXd> 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 != nullptr)
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 }
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)
Definition: RandomNumbers.h:73
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
A shared pointer wrapper for ompl::base::StateSampler.
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:107
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:104
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:71
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:87
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev).
void sampleUniform(State *state) override
Sample a state.
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
The definition of a time state.
double position
The position in time.
A state space representing time. The time can be unbounded, in which case enforceBounds() is a no-op,...
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
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 copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
bool isBounded() const
Check if the time is bounded or not.
void setBounds(double minTime, double maxTime)
Set the minimum and maximum time bounds. This will make the state space switch into bounded time mode...
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
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....
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
void freeState(State *state) const override
Free the memory of the allocated state.
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...
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
State * allocState() const override
Allocate a state that can store a point in the described space.
double getMinTimeBound() const
Get the minimum allowed value of position in a state. The function returns 0 if time is not bounded.
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
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 enforceBounds(State *state) const override
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
static const double PROJECTION_DIMENSION_SPLITS
When the cell sizes for a projection are automatically computed, this value defines the number of par...