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 
46 namespace ompl
47 {
48  namespace base
49  {
51  class KleinBottleStateSampler : public StateSampler
52  {
53  public:
54  KleinBottleStateSampler(const StateSpace *space);
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 
71  class KleinBottleStateSpace : public CompoundStateSpace
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
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 sampleUniform(State *state) override
Sample a state.
double getU() const
Access U, the height of the cylinder.
void setV(double v)
Set V, the angle of the cylinder.
void setU(double u)
Set U, the height of the cylinder.
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....
Definition of an abstract state.
Definition: State.h:113
double getV() const
Access V, the angle of the cylinder.
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev).
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
State * allocState() const override
Allocate a state that can store a point in the described space.
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
A shared pointer wrapper for ompl::base::StateSampler.
Main namespace. Contains everything in this library.