Loading...
Searching...
No Matches
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
46namespace ompl
47{
48 namespace base
49 {
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
A space to allow the composition of state spaces.
Definition StateSpace.h:574
Definition of a compound state.
Definition State.h:87
The definition of a state (u,v) in the Mobius strip state space. The variable u is the position on th...
The Mobius strip is a 2-dimensional non-orientable surface.
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....
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...
Definition of an abstract state.
Definition State.h:50
Main namespace. Contains everything in this library.