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