Loading...
Searching...
No Matches
KleinBottleStateSpace.h
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#ifndef OMPL_BASE_SPACES_KLEINBOTTLE_STATE_SPACE_
40#define OMPL_BASE_SPACES_KLEINBOTTLE_STATE_SPACE_
41
42#include <ompl/base/StateSpace.h>
43#include <ompl/base/spaces/SO2StateSpace.h>
44#include <ompl/base/spaces/RealVectorStateSpace.h>
45
46namespace ompl
47{
48 namespace base
49 {
52 {
53 public:
55
56 void sampleUniform(State *state) override;
57
58 void sampleUniformNear(State *state, const State *near, double distance) override;
59
60 void sampleGaussian(State *state, const State *mean, double stdDev) override;
61
62 private:
65 const double gMax_{4.1455};
66 };
67
72 {
73 public:
83 {
84 public:
85 StateType() = default;
86
88 double getU() const
89 {
90 return as<RealVectorStateSpace::StateType>(0)->values[0];
91 }
93 double getV() const
94 {
95 return as<SO2StateSpace::StateType>(1)->value;
96 }
97
99 void setU(double u)
100 {
101 as<RealVectorStateSpace::StateType>(0)->values[0] = u;
102 }
104 void setV(double v)
105 {
106 as<SO2StateSpace::StateType>(1)->value = v;
107 }
108 void setUV(double u, double v)
109 {
110 setU(u);
111 setV(v);
112 }
113 };
114
115 KleinBottleStateSpace();
116
117 ~KleinBottleStateSpace() override = default;
118
120
121 double distance(const State *state1, const State *state2) const override;
122
123 void interpolate(const State *from, const State *to, double t, State *state) const override;
124
125 State *allocState() const override;
126
127 /* \brief Convert a state to a 3D vector to visualize the state
128 * space. */
129 Eigen::Vector3f toVector(const State *state) const;
130 };
131 }
132}
133
134#endif
A space to allow the composition of state spaces.
Definition StateSpace.h:574
Definition of a compound state.
Definition State.h:87
State sampler for the Klein bottle state space.
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.
void setV(double v)
Set V, the angle of the cylinder.
The Klein bottle is a 2-dimensional non-orientable surface. In this class, we implement a 3-dimension...
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 shared pointer wrapper for ompl::base::StateSampler.
Abstract definition of a state space sampler.
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
Main namespace. Contains everything in this library.