MobiusStateSpace.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_MOBIUS_STATE_SPACE_
40 #define OMPL_BASE_SPACES_MOBIUS_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  {
52  class MobiusStateSpace : public CompoundStateSpace
53  {
54  public:
61  {
62  public:
63  StateType() = default;
64 
65  double getU() const
66  {
67  return as<SO2StateSpace::StateType>(0)->value;
68  }
69  double getV() const
70  {
71  return as<RealVectorStateSpace::StateType>(1)->values[0];
72  }
73 
74  void setU(double u)
75  {
76  as<SO2StateSpace::StateType>(0)->value = u;
77  }
78  void setV(double v)
79  {
80  as<RealVectorStateSpace::StateType>(1)->values[0] = v;
81  }
82  void setUV(double u, double v)
83  {
84  setU(u);
85  setV(v);
86  }
87  };
88 
89  MobiusStateSpace(double intervalMax = 1.0, double radius = 1.0);
90 
91  virtual ~MobiusStateSpace() override = default;
92 
93  virtual double distance(const State *state1, const State *state2) const override;
94 
95  virtual void interpolate(const State *from, const State *to, double t, State *state) const override;
96 
97  virtual State *allocState() const override;
98 
99  /* \brief Convert a state to a 3D vector to visualize the state
100  * space. */
101  Eigen::Vector3f toVector(const State *state) const;
102 
103  private:
104  double radius_{1.0}; // radius of inner circle
105  };
106  }
107 }
108 
109 #endif
virtual State * allocState() const override
Allocate a state that can store a point in the described space.
virtual 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....
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
virtual 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...
Main namespace. Contains everything in this library.