DART  6.10.1
ConfigurationSpace.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
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  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_MATH_CONFIGURATIONSPACE_HPP_
34 #define DART_MATH_CONFIGURATIONSPACE_HPP_
35 
36 #include <Eigen/Dense>
37 #include "dart/math/Geometry.hpp"
38 #include "dart/math/MathTypes.hpp"
39 
40 namespace dart {
41 namespace math {
42 
43 //==============================================================================
44 template <std::size_t Dimension>
46 {
47  static constexpr std::size_t NumDofs = Dimension;
48  static constexpr int NumDofsEigen = static_cast<int>(Dimension);
49 
51 
52  using Point = Eigen::Matrix<double, NumDofs, 1>;
53  using EuclideanPoint = Eigen::Matrix<double, NumDofs, 1>;
54  using Vector = Eigen::Matrix<double, NumDofs, 1>;
55  using Matrix = Eigen::Matrix<double, NumDofs, NumDofs>;
56  using JacobianMatrix = Eigen::Matrix<double, 6, NumDofs>;
57 };
58 
59 //==============================================================================
60 //
61 // These namespace-level definitions are required to enable ODR-use of static
62 // constexpr member variables.
63 //
64 // See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426
65 //
66 template <std::size_t Dimension>
67 constexpr std::size_t RealVectorSpace<Dimension>::NumDofs;
68 template <std::size_t Dimension>
70 
75 
76 //==============================================================================
77 struct SO3Space
78 {
79  static constexpr std::size_t NumDofs = 3u;
80  static constexpr int NumDofsEigen = 3;
81 
83 
84  using Point = Eigen::Matrix3d;
85  using EuclideanPoint = Eigen::Vector3d;
86  using Vector = Eigen::Vector3d;
87  using Matrix = Eigen::Matrix3d;
88  using JacobianMatrix = Eigen::Matrix<double, 6, NumDofs>;
89 };
90 
91 //==============================================================================
92 struct SE3Space
93 {
94  static constexpr std::size_t NumDofs = 6u;
95  static constexpr int NumDofsEigen = 6;
96 
98 
99  using Point = Eigen::Isometry3d;
104 };
105 
107 {
108 };
109 
110 //==============================================================================
111 template <typename SpaceT>
112 typename SpaceT::Matrix inverse(const typename SpaceT::Matrix& mat);
113 
114 //==============================================================================
115 template <typename SpaceT>
116 typename SpaceT::EuclideanPoint toEuclideanPoint(
117  const typename SpaceT::Point& point);
118 
119 //==============================================================================
120 template <typename SpaceT>
121 typename SpaceT::Point toManifoldPoint(
122  const typename SpaceT::EuclideanPoint& point);
123 
124 //==============================================================================
125 template <typename SpaceT>
126 typename SpaceT::Point integratePosition(
127  const typename SpaceT::Point& pos,
128  const typename SpaceT::Vector& vel,
129  double dt);
130 
131 //==============================================================================
132 template <typename SpaceT>
133 typename SpaceT::Vector integrateVelocity(
134  const typename SpaceT::Vector& vel,
135  const typename SpaceT::Vector& acc,
136  double dt);
137 
138 } // namespace math
139 } // namespace dart
140 
142 
143 #endif // DART_MATH_CONFIGURATIONSPACE_HPP_
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
Matrix< double, 6, 6 > Matrix6d
Definition: MathTypes.hpp:50
SpaceT::Matrix inverse(const typename SpaceT::Matrix &mat)
Definition: ConfigurationSpace.hpp:187
SpaceT::EuclideanPoint toEuclideanPoint(const typename SpaceT::Point &point)
Definition: ConfigurationSpace.hpp:199
SpaceT::Point toManifoldPoint(const typename SpaceT::EuclideanPoint &point)
Definition: ConfigurationSpace.hpp:207
SpaceT::Vector integrateVelocity(const typename SpaceT::Vector &vel, const typename SpaceT::Vector &acc, double dt)
Definition: ConfigurationSpace.hpp:225
SpaceT::Point integratePosition(const typename SpaceT::Point &pos, const typename SpaceT::Vector &vel, double dt)
Definition: ConfigurationSpace.hpp:215
Definition: BulletCollisionDetector.cpp:65
Definition: ConfigurationSpace.hpp:107
Definition: ConfigurationSpace.hpp:46
Eigen::Matrix< double, NumDofs, NumDofs > Matrix
Definition: ConfigurationSpace.hpp:55
Eigen::Matrix< double, NumDofs, 1 > Point
Definition: ConfigurationSpace.hpp:52
Eigen::Matrix< double, NumDofs, 1 > Vector
Definition: ConfigurationSpace.hpp:54
static constexpr int NumDofsEigen
Definition: ConfigurationSpace.hpp:48
Eigen::Matrix< double, NumDofs, 1 > EuclideanPoint
Definition: ConfigurationSpace.hpp:53
Eigen::Matrix< double, 6, NumDofs > JacobianMatrix
Definition: ConfigurationSpace.hpp:56
static constexpr std::size_t NumDofs
Definition: ConfigurationSpace.hpp:47
Definition: ConfigurationSpace.hpp:93
Eigen::Vector6d EuclideanPoint
Definition: ConfigurationSpace.hpp:100
static constexpr std::size_t NumDofs
Definition: ConfigurationSpace.hpp:94
Eigen::Isometry3d Point
Definition: ConfigurationSpace.hpp:99
static constexpr int NumDofsEigen
Definition: ConfigurationSpace.hpp:95
Eigen::Matrix6d JacobianMatrix
Definition: ConfigurationSpace.hpp:103
Eigen::Matrix6d Matrix
Definition: ConfigurationSpace.hpp:102
Eigen::Vector6d Vector
Definition: ConfigurationSpace.hpp:101
Definition: ConfigurationSpace.hpp:78
static constexpr std::size_t NumDofs
Definition: ConfigurationSpace.hpp:79
Eigen::Vector3d EuclideanPoint
Definition: ConfigurationSpace.hpp:85
Eigen::Vector3d Vector
Definition: ConfigurationSpace.hpp:86
static constexpr int NumDofsEigen
Definition: ConfigurationSpace.hpp:80
Eigen::Matrix< double, 6, NumDofs > JacobianMatrix
Definition: ConfigurationSpace.hpp:88
Eigen::Matrix3d Matrix
Definition: ConfigurationSpace.hpp:87
Eigen::Matrix3d Point
Definition: ConfigurationSpace.hpp:84