Loading...
Searching...
No Matches
KleinBottleStateSpace.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/KleinBottleStateSpace.h>
40#include <ompl/tools/config/MagicConstants.h>
41#include <cstring>
42#include <cmath>
43#include <boost/math/constants/constants.hpp>
44
45using namespace boost::math::double_constants; // pi
46using namespace ompl::base;
47
48KleinBottleStateSampler::KleinBottleStateSampler(const StateSpace *space) : StateSampler(space)
49{
50}
51
53{
54 bool acceptedSampleFound = false;
55 while (!acceptedSampleFound)
56 {
57 const double u = rng_.uniformReal(0, pi);
58 const double v = rng_.uniformReal(-pi, pi);
59
60 // NOTE: The idea here is that to compute the norm of the gradient at each
61 // point of the surface (i.e. the gradient of the coordinate mapping from
62 //(u,v) to (x,y,z)). To get the norm, we divide by the maximum norm of the
63 // gradient over the whole surface. This gives a number between [0,1]. We
64 // then do rejection sampling, by choosing a random number in [0,1] and
65 // accept if the norm is larger than this random number. Surface elements
66 // with a high curvature will have a small norm and will therefore be
67 // penalized under this method (i.e. rejected more often).
68 // See also: https://mathematica.stackexchange.com/questions/148693/generating-random-points-on-a-kleins-bottle
69
70 // NOTE: Automatic differential via sympy script
71 const double cu = std::cos(u);
72 const double cv = std::cos(v);
73 const double su = std::sin(u);
74 const double sv = std::sin(v);
75 const double cu3 = std::pow(cu, 3);
76 const double cu5 = std::pow(cu, 5);
77 const double cu6 = std::pow(cu, 6);
78 const double cu7 = std::pow(cu, 7);
79 const double cu8 = std::pow(cu, 8);
80
81 const double su2 = std::pow(su, 2);
82 const double su3 = std::pow(su, 3);
83 const double su4 = std::pow(su, 4);
84 const double su5 = std::pow(su, 5);
85 const double su6 = std::pow(su, 6);
86 const double su7 = std::pow(su, 7);
87 const double su8 = std::pow(su, 8);
88
89 const double aprime = (64.0 * su8 - 128.0 * su6 + 60.0 * su4 + 0.4 * su * cv - (1.0 / 6.0) * cu * cv -
90 0.5 * std::cos(3 * u) * cv);
91
92 const double a = (-aprime * cv + (2.0 / 3.0) * sv * sv * cu * std::cos(2.0 * u));
93
94 const double bprime =
95 ((26 + 2.0 / 3.0) * su7 * cv - 55.0 * su5 * cv - (37 + 1.0 / 3.0) * su3 * cu6 * cv + 28.0 * su3 * cv +
96 (10 + 2.0 / 3.0) * su * cu8 * cv - (10 + 2.0 / 3.0) * su * cu6 * cv - 4.0 * std::sin(2.0 * u) +
97 22.4 * cu7 * cv - 35.2 * cu5 * cv + 12.2 * cu3 * cv + 0.6 * cu * cv);
98
99 const double cprime = ((5 + 1.0 / 3.0) * su5 * cu + 3.2 * su4 - (10 + 2.0 / 3.0) * su3 * cu - 6.4 * su2 +
100 2.5 * std::sin(2 * u) + 3.0);
101
102 const double b = (((1.0 / 3.0) * std::sin(2.0 * u) + 0.4) * bprime * cu - cprime * aprime * su3);
103
104 const double c = ((5.0 / 6.0) * std::sin(2.0 * u) + 1);
105
106 const double d = (-((1.0 / 3.0) * std::sin(2.0 * u) + 0.4) * bprime * cv +
107 (2.0 / 3.0) * cprime * su3 * sv * sv * std::cos(2.0 * u));
108
109 double s = std::sqrt(a * a * (0.16 * c * c) + b * b * sv * sv + d * d);
110
111 if (s > gMax_)
112 {
113 OMPL_ERROR("Norm of gradient (%.10f) larger than maximum norm (%.10f).", s, gMax_);
114 throw "Wrong norm error.";
115 }
116 s = s / gMax_;
117
118 const double mu = rng_.uniformReal(0, 1);
119 if (mu <= s)
120 {
121 auto *K = state->as<KleinBottleStateSpace::StateType>();
122 K->setUV(u, v);
123 acceptedSampleFound = true;
124 }
125 }
126}
127
128void KleinBottleStateSampler::sampleUniformNear(State *state, const State *near, double distance)
129{
130 auto *K = state->as<KleinBottleStateSpace::StateType>();
131 const auto *Knear = near->as<KleinBottleStateSpace::StateType>();
132 K->setU(rng_.uniformReal(Knear->getU() - distance, Knear->getU() + distance));
133 K->setV(rng_.uniformReal(Knear->getV() - distance, Knear->getV() + distance));
134 space_->enforceBounds(state);
135}
136
137void KleinBottleStateSampler::sampleGaussian(State *state, const State *mean, double stdDev)
138{
139 auto *K = state->as<KleinBottleStateSpace::StateType>();
140 const auto *Kmean = mean->as<KleinBottleStateSpace::StateType>();
141 K->setU(rng_.gaussian(Kmean->getU(), stdDev));
142 K->setV(rng_.gaussian(Kmean->getV(), stdDev));
143
144 space_->enforceBounds(state);
145}
146
147KleinBottleStateSpace::KleinBottleStateSpace()
148{
149 setName("KleinBottle" + getName());
151
152 // We model the Klein bottle as a regular cylinder, but where both ends are
153 // glued inversely together. For more information, check out the
154 // wikipedia article: https://en.wikipedia.org/wiki/Klein_bottle.
155 // Both interpolation and distance computation have to take
156 // the gluing into account when crossing over the boundary.
157 // ------<-------
158 // | |
159 // | |
160 // v v u-dimension (0 to pi)
161 // | |
162 // | |
163 // ------>-------
164 // v-dimension (0 to 2*pi)
165 //
166 // Gluing:
167 // u=pi+0.001: 0 ----------- -pi pi ---------- 0
168 // u=0 : -pi ----------- 0 0 ---------- pi
169
170 StateSpacePtr R1(std::make_shared<RealVectorStateSpace>(1));
171 R1->as<RealVectorStateSpace>()->setBounds(0, pi);
172
173 StateSpacePtr SO2(std::make_shared<SO2StateSpace>());
174
175 addSubspace(R1, 1.0);
176 addSubspace(SO2, 1.0);
177
178 lock();
179}
180
182{
183 return std::make_shared<KleinBottleStateSampler>(this);
184}
185
186double KleinBottleStateSpace::distance(const State *state1, const State *state2) const
187{
188 const double u1 = state1->as<KleinBottleStateSpace::StateType>()->getU();
189 const double u2 = state2->as<KleinBottleStateSpace::StateType>()->getU();
190
191 const double diffU = u2 - u1;
192
193 if (std::abs(diffU) <= 0.5 * pi)
194 {
195 return CompoundStateSpace::distance(state1, state2);
196 }
197 else
198 {
199 const double d_u = pi - std::abs(diffU);
200
201 const double v1 = state1->as<KleinBottleStateSpace::StateType>()->getV();
202 double v2 = state2->as<KleinBottleStateSpace::StateType>()->getV();
203
204 // reverse v2 (valid for both directions)
205 v2 = (v2 > 0.0 ? pi - v2 : -pi - v2);
206
207 double d_v = std::abs(v2 - v1);
208 d_v = (d_v > pi) ? 2.0 * pi - d_v : d_v;
209
210 double dist = d_u + d_v;
211
212 return dist;
213 }
214}
215
216void KleinBottleStateSpace::interpolate(const State *from, const State *to, double t, State *state) const
217{
218 const double u1 = from->as<KleinBottleStateSpace::StateType>()->getU();
219 const double u2 = to->as<KleinBottleStateSpace::StateType>()->getU();
220
221 double diffU = u2 - u1;
222
223 if (std::abs(diffU) <= 0.5 * pi)
224 {
225 // interpolate as if it would be a cylinder
226 CompoundStateSpace::interpolate(from, to, t, state);
227 }
228 else
229 {
230 // Interpolate along u-dimension
231 if (diffU > 0.0)
232 {
233 diffU = pi - diffU;
234 }
235 else
236 {
237 diffU = -pi - diffU;
238 }
239
240 double u = u1 - diffU * t;
241
242 bool crossed = false;
243 if (u > pi)
244 {
245 u -= pi;
246 crossed = true;
247 }
248 else if (u < 0.0)
249 {
250 u += pi;
251 crossed = true;
252 }
253
254 state->as<KleinBottleStateSpace::StateType>()->setU(u);
255
256 double v1 = from->as<KleinBottleStateSpace::StateType>()->getV();
257 double v2 = to->as<KleinBottleStateSpace::StateType>()->getV();
258
259 // If we crossed the gluing, we need to invert the "from"-state, otherwise
260 // we need to invert the "to"-state (similar to default SO2 interpolation)
261 if (crossed)
262 {
263 v1 = (v1 > 0.0 ? pi - v1 : -pi - v1);
264 }
265 else
266 {
267 v2 = (v2 > 0.0 ? pi - v2 : -pi - v2);
268 }
269
270 double diffV = v2 - v1;
271 double v = 0;
272
273 if (std::abs(diffV) <= pi)
274 {
275 v = v1 + diffV * t;
276 }
277 else
278 {
279 if (diffV > 0.0)
280 diffV = 2.0 * pi - diffV;
281 else
282 diffV = -2.0 * pi - diffV;
283 v = v1 - diffV * t;
284
285 if (v > pi)
286 v -= 2.0 * pi;
287 else if (v < -pi)
288 v += 2.0 * pi;
289 }
290
291 state->as<KleinBottleStateSpace::StateType>()->setV(v);
292 }
293}
294
296{
297 auto *state = new StateType();
299 return state;
300}
301
302Eigen::Vector3f KleinBottleStateSpace::toVector(const State *state) const
303{
304 // Formula from https://en.wikipedia.org/wiki/Klein_bottle#Bottle_shape
305 const auto *s = state->as<KleinBottleStateSpace::StateType>();
306 const float u = s->getU();
307 const float v = s->getV() + pi; // NOTE: SO2 state space has bounds [-pi, +pi]
308
309 assert(u >= 0.0);
310 assert(u <= pi);
311 assert(v >= 0.0);
312 assert(v <= 2 * pi);
313
314 double cu = std::cos(u);
315 double cv = std::cos(v);
316 double su = std::sin(u);
317 double sv = std::sin(v);
318 double cu2 = std::pow(cu, 2);
319 double cu3 = std::pow(cu, 3);
320 double cu4 = std::pow(cu, 4);
321 double cu5 = std::pow(cu, 5);
322 double cu6 = std::pow(cu, 6);
323 double cu7 = std::pow(cu, 7);
324
325 double a = 3 * cv - 30 * su + 90 * cu4 * su - 60 * cu6 * su + 5 * cu * cv * su;
326
327 Eigen::Vector3f q;
328 q[0] = -2.0 / 15.0 * cu * a;
329
330 double b = 3 * cv - 3 * cu2 * cv - 48 * cu4 * cv + 48 * cu6 * cv - 60 * su + 5 * cu * cv * su - 5 * cu3 * cv * su -
331 80 * cu5 * cv * su + 80 * cu7 * cv * su;
332
333 q[1] = -1.0 / 15.0 * su * b;
334
335 q[2] = 2.0 / 15.0 * (3 + 5 * cu * su) * sv;
336
337 return q;
338}
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.
void addSubspace(const StateSpacePtr &component, double weight)
Adds a new state space as part of the compound state space. For computing distances within the compou...
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 lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
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....
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
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.
The definition of a state (u,v) in the Klein bottle state space. A state is represented as a cylinder...
double getU() const
Access U, the height of the cylinder.
void setU(double u)
Set U, the height of the cylinder.
double getV() const
Access V, the angle of the cylinder.
State * allocState() const override
Allocate a state that can store a point in the described space.
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....
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...
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
A state space representing Rn. The distance function is the L2 norm.
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.
A shared pointer wrapper for ompl::base::StateSpace.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
int type_
A type assigned for this state space.
Definition StateSpace.h:531
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-...
void setName(const std::string &name)
Set the name of the state space.
const std::string & getName() const
Get the name of the state space.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
This namespace contains sampling based planning routines shared by both planning under geometric cons...
@ STATE_SPACE_KLEIN_BOTTLE
ompl::base::KleinBottleStateSpace