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 using namespace boost::math::double_constants;
45 
47 {
48  state->as<SO2StateSpace::StateType>()->value =
49  rng_.uniformReal(-pi, pi);
50 }
51 
52 void ompl::base::SO2StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
53 {
54  state->as<SO2StateSpace::StateType>()->value = rng_.uniformReal(
55  near->as<SO2StateSpace::StateType>()->value - distance, near->as<SO2StateSpace::StateType>()->value + distance);
56  space_->enforceBounds(state);
57 }
58 
59 void ompl::base::SO2StateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
60 {
61  state->as<SO2StateSpace::StateType>()->value = rng_.gaussian(mean->as<SO2StateSpace::StateType>()->value, stdDev);
62  space_->enforceBounds(state);
63 }
64 
66 {
67  return 1;
68 }
69 
71 {
72  return pi;
73 }
74 
76 {
77  return 2.0 * pi;
78 }
79 
81 {
82  double v = fmod(state->as<StateType>()->value, 2.0 * pi);
83  if (v < -pi)
84  v += 2.0 * pi;
85  else if (v >= pi)
86  v -= 2.0 * pi;
87  state->as<StateType>()->value = v;
88 }
89 
91 {
92  return (state->as<StateType>()->value < pi) &&
93  (state->as<StateType>()->value >= -pi);
94 }
95 
96 void ompl::base::SO2StateSpace::copyState(State *destination, const State *source) const
97 {
98  destination->as<StateType>()->value = source->as<StateType>()->value;
99 }
100 
102 {
103  return sizeof(double);
104 }
105 
106 void ompl::base::SO2StateSpace::serialize(void *serialization, const State *state) const
107 {
108  memcpy(serialization, &state->as<StateType>()->value, sizeof(double));
109 }
110 
111 void ompl::base::SO2StateSpace::deserialize(State *state, const void *serialization) const
112 {
113  memcpy(&state->as<StateType>()->value, serialization, sizeof(double));
114 }
115 
116 double ompl::base::SO2StateSpace::distance(const State *state1, const State *state2) const
117 {
118  // assuming the states 1 & 2 are within bounds
119  double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value);
120  BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2), "The states passed to SO2StateSpace::distance "
121  "are not within bounds. Call "
122  "SO2StateSpace::enforceBounds() in, e.g., "
123  "ompl::control::ODESolver::"
124  "PostPropagationEvent, "
125  "ompl::control::StatePropagator, or "
126  "ompl::base::StateValidityChecker");
127  return (d > pi) ? 2.0 * pi - d : d;
128 }
129 
130 bool ompl::base::SO2StateSpace::equalStates(const State *state1, const State *state2) const
131 {
132  return fabs(state1->as<StateType>()->value - state2->as<StateType>()->value) <
133  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) <= pi)
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 * pi - diff;
146  else
147  diff = -2.0 * pi - diff;
148  v = from->as<StateType>()->value - diff * t;
149  // input states are within bounds, so the following check is sufficient
150  if (v > pi)
151  v -= 2.0 * pi;
152  else if (v < -pi)
153  v += 2.0 * pi;
154  }
155 }
156 
158 {
159  return std::make_shared<SO2StateSampler>(this);
160 }
161 
163 {
164  return new StateType();
165 }
166 
168 {
169  delete static_cast<StateType *>(state);
170 }
171 
173 {
174  class SO2DefaultProjection : public ProjectionEvaluator
175  {
176  public:
177  SO2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
178  {
179  }
180 
181  unsigned int getDimension() const override
182  {
183  return 1;
184  }
185 
186  void defaultCellSizes() override
187  {
188  cellSizes_.resize(1);
189  cellSizes_[0] = pi / magic::PROJECTION_DIMENSION_SPLITS;
190  bounds_.resize(1);
191  bounds_.low[0] = -pi;
192  bounds_.high[0] = pi;
193  }
194 
195  void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
196  {
197  projection(0) = state->as<SO2StateSpace::StateType>()->value;
198  }
199  };
200 
201  registerDefaultProjection(std::make_shared<SO2DefaultProjection>(this));
202 }
203 
204 double *ompl::base::SO2StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
205 {
206  return index == 0 ? &(state->as<StateType>()->value) : nullptr;
207 }
208 
209 void ompl::base::SO2StateSpace::printState(const State *state, std::ostream &out) const
210 {
211  out << "SO2State [";
212  if (state != nullptr)
213  out << state->as<StateType>()->value;
214  else
215  out << "nullptr";
216  out << ']' << std::endl;
217 }
218 
219 void ompl::base::SO2StateSpace::printSettings(std::ostream &out) const
220 {
221  out << "SO2 state space '" << getName() << "'" << std::endl;
222 }
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
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 sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
void sampleUniform(State *state) override
Sample a state.
The definition of a state in SO(2)
Definition: SO2StateSpace.h:68
double value
The value of the angle in the interval (-Pi, Pi].
Definition: SO2StateSpace.h:77
void enforceBounds(State *state) const override
Normalize the value of the state to the interval [-Pi, Pi)
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
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.
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...
void freeState(State *state) const override
Free the memory of the allocated state.
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
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.
State * allocState() const override
Allocate a state that can store a point in the described space.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
bool satisfiesBounds(const State *state) const override
Check if the value of the state is in the interval [-Pi, Pi)
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
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...
A shared pointer wrapper for ompl::base::StateSampler.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:71
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66