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_DETAIL_CONFIGURATIONSPACE_H_
34 #define DART_MATH_DETAIL_CONFIGURATIONSPACE_H_
35 
37 
38 #include "dart/math/Geometry.hpp"
39 
40 namespace dart {
41 namespace math {
42 
43 namespace detail {
44 
45 template <bool>
46 struct Range;
47 
48 //==============================================================================
49 template <typename MatrixType, int Size, typename Enable = Range<true>>
51 {
52  static void run(const MatrixType& matrix, MatrixType& result)
53  {
54  result = matrix.ldlt().solve(MatrixType::Identity());
55  }
56 };
57 
58 //==============================================================================
59 template <typename MatrixType, int Size>
60 struct inverseImpl<MatrixType, Size, Range<(0 <= Size && Size <= 4)>>
61 {
62  static void run(const MatrixType& matrix, MatrixType& result)
63  {
64  result = matrix.inverse();
65  }
66 };
67 
68 //==============================================================================
69 template <typename SpaceT>
71 {
72  static typename SpaceT::EuclideanPoint run(
73  const typename SpaceT::Point& point)
74  {
75  return point;
76  }
77 };
78 
79 //==============================================================================
80 template <>
82 {
83  static typename SO3Space::EuclideanPoint run(
84  const typename SO3Space::Point& point)
85  {
86  return math::logMap(point);
87  }
88 };
89 
90 //==============================================================================
91 template <>
93 {
94  static typename SE3Space::EuclideanPoint run(
95  const typename SE3Space::Point& point)
96  {
98 
99  x.head<3>() = math::logMap(point.linear());
100  x.tail<3>() = point.translation();
101 
102  return x;
103  }
104 };
105 
106 //==============================================================================
107 template <typename SpaceT>
109 {
110  static typename SpaceT::Point run(
111  const typename SpaceT::EuclideanPoint& point)
112  {
113  return point;
114  }
115 };
116 
117 //==============================================================================
118 template <>
120 {
121  static typename SO3Space::Point run(
122  const typename SO3Space::EuclideanPoint& point)
123  {
124  return math::expMapRot(point);
125  }
126 };
127 
128 //==============================================================================
129 template <>
131 {
132  static typename SE3Space::Point run(
133  const typename SE3Space::EuclideanPoint& point)
134  {
135  Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
136 
137  tf.linear() = math::expMapRot(point.head<3>());
138  tf.translation() = point.tail<3>();
139 
140  return tf;
141  }
142 };
143 
144 //==============================================================================
145 template <typename SpaceT>
147 {
148  static typename SpaceT::Point run(
149  const typename SpaceT::Point& pos,
150  const typename SpaceT::Vector& vel,
151  double dt)
152  {
153  return pos + dt * vel;
154  }
155 };
156 
157 //==============================================================================
158 template <>
160 {
161  static typename SO3Space::Point run(
162  const typename SO3Space::Point& pos,
163  const typename SO3Space::Vector& vel,
164  double dt)
165  {
166  return pos * toManifoldPoint<SO3Space>(vel * dt);
167  }
168 };
169 
170 //==============================================================================
171 template <>
173 {
174  static typename SE3Space::Point run(
175  const typename SE3Space::Point& pos,
176  const typename SE3Space::Vector& vel,
177  double dt)
178  {
179  return pos * toManifoldPoint<SE3Space>(vel * dt);
180  }
181 };
182 
183 } // namespace detail
184 
185 //==============================================================================
186 template <typename SpaceT>
187 typename SpaceT::Matrix inverse(const typename SpaceT::Matrix& mat)
188 {
189  typename SpaceT::Matrix res;
190 
192  mat, res);
193 
194  return res;
195 }
196 
197 //==============================================================================
198 template <typename SpaceT>
199 typename SpaceT::EuclideanPoint toEuclideanPoint(
200  const typename SpaceT::Point& point)
201 {
203 }
204 
205 //==============================================================================
206 template <typename SpaceT>
207 typename SpaceT::Point toManifoldPoint(
208  const typename SpaceT::EuclideanPoint& point)
209 {
211 }
212 
213 //==============================================================================
214 template <typename SpaceT>
215 typename SpaceT::Point integratePosition(
216  const typename SpaceT::Point& pos,
217  const typename SpaceT::Vector& vel,
218  double dt)
219 {
220  return detail::integratePositionImpl<SpaceT>::run(pos, vel, dt);
221 }
222 
223 //==============================================================================
224 template <typename SpaceT>
225 typename SpaceT::Vector integrateVelocity(
226  const typename SpaceT::Vector& vel,
227  const typename SpaceT::Vector& acc,
228  double dt)
229 {
230  return vel + acc * dt;
231 }
232 
233 } // namespace math
234 } // namespace dart
235 
236 #endif // DART_MATH_DETAIL_CONFIGURATIONSPACE_H_
CollisionResult * result
Collision result of DART.
Definition: FCLCollisionDetector.cpp:160
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
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
Eigen::Vector3d logMap(const Eigen::Matrix3d &_R)
Log mapping.
Definition: Geometry.cpp:565
SpaceT::Point integratePosition(const typename SpaceT::Point &pos, const typename SpaceT::Vector &vel, double dt)
Definition: ConfigurationSpace.hpp:215
Eigen::Matrix3d expMapRot(const Eigen::Vector3d &_q)
Computes the Rotation matrix from a given expmap vector.
Definition: Geometry.cpp:471
Definition: BulletCollisionDetector.cpp:65
Definition: ConfigurationSpace.hpp:93
Eigen::Vector6d EuclideanPoint
Definition: ConfigurationSpace.hpp:100
Eigen::Isometry3d Point
Definition: ConfigurationSpace.hpp:99
Eigen::Vector6d Vector
Definition: ConfigurationSpace.hpp:101
Definition: ConfigurationSpace.hpp:78
Eigen::Vector3d EuclideanPoint
Definition: ConfigurationSpace.hpp:85
Eigen::Vector3d Vector
Definition: ConfigurationSpace.hpp:86
Eigen::Matrix3d Point
Definition: ConfigurationSpace.hpp:84
Definition: ConfigurationSpace.hpp:46
static SE3Space::Point run(const typename SE3Space::Point &pos, const typename SE3Space::Vector &vel, double dt)
Definition: ConfigurationSpace.hpp:174
static SO3Space::Point run(const typename SO3Space::Point &pos, const typename SO3Space::Vector &vel, double dt)
Definition: ConfigurationSpace.hpp:161
Definition: ConfigurationSpace.hpp:147
static SpaceT::Point run(const typename SpaceT::Point &pos, const typename SpaceT::Vector &vel, double dt)
Definition: ConfigurationSpace.hpp:148
static void run(const MatrixType &matrix, MatrixType &result)
Definition: ConfigurationSpace.hpp:62
Definition: ConfigurationSpace.hpp:51
static void run(const MatrixType &matrix, MatrixType &result)
Definition: ConfigurationSpace.hpp:52
static SE3Space::EuclideanPoint run(const typename SE3Space::Point &point)
Definition: ConfigurationSpace.hpp:94
static SO3Space::EuclideanPoint run(const typename SO3Space::Point &point)
Definition: ConfigurationSpace.hpp:83
Definition: ConfigurationSpace.hpp:71
static SpaceT::EuclideanPoint run(const typename SpaceT::Point &point)
Definition: ConfigurationSpace.hpp:72
static SE3Space::Point run(const typename SE3Space::EuclideanPoint &point)
Definition: ConfigurationSpace.hpp:132
static SO3Space::Point run(const typename SO3Space::EuclideanPoint &point)
Definition: ConfigurationSpace.hpp:121
Definition: ConfigurationSpace.hpp:109
static SpaceT::Point run(const typename SpaceT::EuclideanPoint &point)
Definition: ConfigurationSpace.hpp:110