SO2StateSpace.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/SO2StateSpace.h"
38 #include <algorithm>
39 #include <limits>
40 #include <cmath>
41 #include "ompl/tools/config/MagicConstants.h"
42 #include <boost/math/constants/constants.hpp>
43 
44 // Define for boost version < 1.47
45 #ifndef BOOST_ASSERT_MSG
46 #define BOOST_ASSERT_MSG(expr, msg) assert(expr)
47 #endif
48 
50 {
51  state->as<SO2StateSpace::StateType>()->value =
52  rng_.uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
53 }
54 
55 void ompl::base::SO2StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
56 {
57  state->as<SO2StateSpace::StateType>()->value = rng_.uniformReal(near->as<SO2StateSpace::StateType>()->value - distance,
58  near->as<SO2StateSpace::StateType>()->value + distance);
59  space_->enforceBounds(state);
60 }
61 
62 void ompl::base::SO2StateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
63 {
64  state->as<SO2StateSpace::StateType>()->value = rng_.gaussian(mean->as<SO2StateSpace::StateType>()->value, stdDev);
65  space_->enforceBounds(state);
66 }
67 
69 {
70  return 1;
71 }
72 
74 {
75  return boost::math::constants::pi<double>();
76 }
77 
79 {
80  return 2.0 * boost::math::constants::pi<double>();
81 }
82 
84 {
85  double v = fmod(state->as<StateType>()->value, 2.0 * boost::math::constants::pi<double>());
86  if (v <= -boost::math::constants::pi<double>())
87  v += 2.0 * boost::math::constants::pi<double>();
88  else
89  if (v > boost::math::constants::pi<double>())
90  v -= 2.0 * boost::math::constants::pi<double>();
91  state->as<StateType>()->value = v;
92 }
93 
95 {
96  return (state->as<StateType>()->value <= boost::math::constants::pi<double>()) &&
97  (state->as<StateType>()->value > -boost::math::constants::pi<double>());
98 }
99 
100 void ompl::base::SO2StateSpace::copyState(State *destination, const State *source) const
101 {
102  destination->as<StateType>()->value = source->as<StateType>()->value;
103 }
104 
106 {
107  return sizeof(double);
108 }
109 
110 void ompl::base::SO2StateSpace::serialize(void *serialization, const State *state) const
111 {
112  memcpy(serialization, &state->as<StateType>()->value, sizeof(double));
113 }
114 
115 void ompl::base::SO2StateSpace::deserialize(State *state, const void *serialization) const
116 {
117  memcpy(&state->as<StateType>()->value, serialization, sizeof(double));
118 }
119 
120 double ompl::base::SO2StateSpace::distance(const State *state1, const State *state2) const
121 {
122  // assuming the states 1 & 2 are within bounds
123  double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value);
124  BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2),
125  "The states passed to SO2StateSpace::distance are not within bounds. Call "
126  "SO2StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
127  "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
128  return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
129 }
130 
131 bool ompl::base::SO2StateSpace::equalStates(const State *state1, const State *state2) const
132 {
133  return fabs(state1->as<StateType>()->value - state2->as<StateType>()->value) < std::numeric_limits<double>::epsilon() * 2.0;
134 }
135 
136 void ompl::base::SO2StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
137 {
138  double diff = to->as<StateType>()->value - from->as<StateType>()->value;
139  if (fabs(diff) <= boost::math::constants::pi<double>())
140  state->as<StateType>()->value = from->as<StateType>()->value + diff * t;
141  else
142  {
143  double &v = state->as<StateType>()->value;
144  if (diff > 0.0)
145  diff = 2.0 * boost::math::constants::pi<double>() - diff;
146  else
147  diff = -2.0 * boost::math::constants::pi<double>() - diff;
148  v = from->as<StateType>()->value - diff * t;
149  // input states are within bounds, so the following check is sufficient
150  if (v > boost::math::constants::pi<double>())
151  v -= 2.0 * boost::math::constants::pi<double>();
152  else
153  if (v < -boost::math::constants::pi<double>())
154  v += 2.0 * boost::math::constants::pi<double>();
155  }
156 }
157 
159 {
160  return StateSamplerPtr(new SO2StateSampler(this));
161 }
162 
164 {
165  return new StateType();
166 }
167 
169 {
170  delete static_cast<StateType*>(state);
171 }
172 
174 {
175  class SO2DefaultProjection : public ProjectionEvaluator
176  {
177  public:
178 
179  SO2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
180  {
181  }
182 
183  virtual unsigned int getDimension() const
184  {
185  return 1;
186  }
187 
188  virtual void defaultCellSizes()
189  {
190  cellSizes_.resize(1);
191  cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
192  bounds_.resize(1);
193  bounds_.low[0] = -boost::math::constants::pi<double>();
194  bounds_.high[0] = boost::math::constants::pi<double>();
195  }
196 
197  virtual void project(const State *state, EuclideanProjection &projection) const
198  {
199  projection(0) = state->as<SO2StateSpace::StateType>()->value;
200  }
201  };
202 
203  registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO2DefaultProjection(this))));
204 }
205 
206 double* ompl::base::SO2StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
207 {
208  return index == 0 ? &(state->as<StateType>()->value) : NULL;
209 }
210 
211 void ompl::base::SO2StateSpace::printState(const State *state, std::ostream &out) const
212 {
213  out << "SO2State [";
214  if (state)
215  out << state->as<StateType>()->value;
216  else
217  out << "NULL";
218  out << ']' << std::endl;
219 }
220 
221 void ompl::base::SO2StateSpace::printSettings(std::ostream &out) const
222 {
223  out << "SO2 state space '" << getName() << "'" << std::endl;
224 }
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
virtual unsigned int getDimension() const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
virtual double getMaximumExtent() const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
double value
The value of the angle in the interval (-Pi, Pi].
Definition: SO2StateSpace.h:81
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...
A boost shared pointer wrapper for ompl::base::StateSampler.
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:94
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
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) ...
The definition of a state in SO(2)
Definition: SO2StateSpace.h:70
virtual StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
virtual void sampleUniform(State *state)
Sample a state.
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...
virtual void sampleUniformNear(State *state, const State *near, const double distance)
Sample a state near another, within specified distance.
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
State space sampler for SO(2)
Definition: SO2StateSpace.h:48
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
virtual bool satisfiesBounds(const State *state) const
Check if the value of the state is in the interval (-Pi, Pi].
virtual State * allocState() const
Allocate a state that can store a point in the described space.
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
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 printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
virtual void enforceBounds(State *state) const
Normalize the value of the state to the interval (-Pi, Pi].
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
virtual void freeState(State *state) const
Free the memory of the allocated state.
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 void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
virtual double getMeasure() const
Get a measure of the space (this can be thought of as a generalization of volume) ...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
virtual bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.