TorusStateSpace.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2021,
5  * Max Planck Institute for Intelligent Systems (MPI-IS).
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the MPI-IS nor the names
19  * of its contributors may be used to endorse or promote products
20  * derived from this software without specific prior written
21  * permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *********************************************************************/
36 
37 /* Author: Andreas Orthey */
38 
39 #include <ompl/base/spaces/special/TorusStateSpace.h>
40 #include <ompl/tools/config/MagicConstants.h>
41 #include <cstring>
42 #include <cmath>
43 
44 #include <boost/math/constants/constants.hpp>
45 using namespace boost::math::double_constants; // pi
46 using namespace ompl::base;
47 
48 TorusStateSampler::TorusStateSampler(const StateSpace *space) : StateSampler(space)
49 {
50 }
51 
53 {
54  // https://stackoverflow.com/questions/26300510/generating-random-points-on-a-surface-of-an-n-dimensional-torus
55  // Based on publication "Random selection of points distributed on curved surfaces."
56  // Link: https://iopscience.iop.org/article/10.1088/0031-9155/32/10/009/pdf
57  const auto *T = static_cast<const TorusStateSpace *>(space_);
58 
59  bool acceptedSampleFound = false;
60  while (!acceptedSampleFound)
61  {
62  const double u = rng_.uniformReal(-pi, pi);
63  const double v = rng_.uniformReal(-pi, pi);
64 
65  const double &R = T->getMajorRadius();
66  const double &r = T->getMinorRadius();
67 
68  const double vprime = (R + r * std::cos(v)) / (R + r);
69 
70  const double mu = rng_.uniformReal(0, 1);
71  if (mu <= vprime)
72  {
73  auto *T = state->as<TorusStateSpace::StateType>();
74  T->setS1S2(u, v);
75  acceptedSampleFound = true;
76  }
77  }
78 }
79 
80 void TorusStateSampler::sampleUniformNear(State *state, const State *near, double distance)
81 {
82  auto *T = state->as<TorusStateSpace::StateType>();
83  const auto *Tnear = near->as<TorusStateSpace::StateType>();
84  T->setS1(rng_.uniformReal(Tnear->getS1() - distance, Tnear->getS1() + distance));
85  T->setS2(rng_.uniformReal(Tnear->getS2() - distance, Tnear->getS2() + distance));
86  space_->enforceBounds(state);
87 }
88 
89 void TorusStateSampler::sampleGaussian(State *state, const State *mean, double stdDev)
90 {
91  auto *T = state->as<TorusStateSpace::StateType>();
92  const auto *Tmean = mean->as<TorusStateSpace::StateType>();
93  T->setS1(rng_.gaussian(Tmean->getS1(), stdDev));
94  T->setS2(rng_.gaussian(Tmean->getS2(), stdDev));
95 
96  space_->enforceBounds(state);
97 }
98 
99 TorusStateSpace::TorusStateSpace(double majorRadius, double minorRadius)
100  : majorRadius_(majorRadius), minorRadius_(minorRadius)
101 {
102  setName("Torus" + getName());
103  type_ = STATE_SPACE_TORUS;
104  addSubspace(std::make_shared<SO2StateSpace>(), 1.0);
105  addSubspace(std::make_shared<SO2StateSpace>(), 1.0);
106  lock();
107 }
108 
110 {
111  return std::make_shared<TorusStateSampler>(this);
112 }
113 
114 double TorusStateSpace::distance(const State *state1, const State *state2) const
115 {
116  const auto *cstate1 = static_cast<const CompoundState *>(state1);
117  const auto *cstate2 = static_cast<const CompoundState *>(state2);
118  const double x = components_[0]->distance(cstate1->components[0], cstate2->components[0]);
119  const double y = components_[1]->distance(cstate1->components[1], cstate2->components[1]);
120  return std::sqrt(x * x + y * y);
121 }
122 
124 {
125  auto *state = new StateType();
126  allocStateComponents(state);
127  return state;
128 }
129 
130 double TorusStateSpace::getMajorRadius() const
131 {
132  return majorRadius_;
133 }
134 
135 double TorusStateSpace::getMinorRadius() const
136 {
137  return minorRadius_;
138 }
139 
140 Eigen::Vector3f TorusStateSpace::toVector(const State *state) const
141 {
142  Eigen::Vector3f v;
143 
144  const auto *s = state->as<TorusStateSpace::StateType>();
145  const float theta = s->getS1();
146  const float phi = s->getS2();
147 
148  const double &R = majorRadius_;
149  const double &r = minorRadius_;
150 
151  v[0] = (R + r * std::cos(phi)) * std::cos(theta);
152  v[1] = (R + r * std::cos(phi)) * std::sin(theta);
153  v[2] = r * std::sin(phi);
154 
155  return v;
156 }
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...
Definition of a compound state.
Definition: State.h:150
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev).
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
Definition of an abstract state.
Definition: State.h:113
This namespace contains sampling based planning routines shared by both planning under geometric cons...
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
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-...
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
@ STATE_SPACE_TORUS
ompl::base::TorusStateSpace
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:168
double gaussian(double mean, double stddev)
Generate a random real using a normal distribution with given mean and variance.
State * allocState() const override
Allocate a state that can store a point in the described space.
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces.
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
void sampleUniform(State *state) override
Sample a state.
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:171
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Abstract definition of a state space sampler.
Definition: StateSampler.h:128
A shared pointer wrapper for ompl::base::StateSampler.
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
Definition: StateSpace.h:799