Loading...
Searching...
No Matches
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>
45using namespace boost::math::double_constants; // pi
46using namespace ompl::base;
47
48TorusStateSampler::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
80void 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
89void 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
99TorusStateSpace::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
114double 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();
127 return state;
128}
129
130double TorusStateSpace::getMajorRadius() const
131{
132 return majorRadius_;
133}
134
135double TorusStateSpace::getMinorRadius() const
136{
137 return minorRadius_;
138}
139
140Eigen::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 gaussian(double mean, double stddev)
Generate a random real using a normal distribution with given mean and variance.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces.
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
Definition StateSpace.h:735
Definition of a compound state.
Definition State.h:87
A shared pointer wrapper for ompl::base::StateSampler.
Abstract definition of a state space sampler.
RNG rng_
An instance of a random number generator.
const StateSpace * space_
The state space this sampler samples.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
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-...
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.
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...
State * allocState() const override
Allocate a state that can store a point in the described space.
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
@ STATE_SPACE_TORUS
ompl::base::TorusStateSpace