Directory: | ./ |
---|---|
File: | include/pinocchio/spatial/se3-tpl.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 122 | 149 | 81.9% |
Branches: | 105 | 234 | 44.9% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2021 CNRS INRIA | ||
3 | // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
4 | // | ||
5 | |||
6 | #ifndef __pinocchio_spatial_se3_tpl_hpp__ | ||
7 | #define __pinocchio_spatial_se3_tpl_hpp__ | ||
8 | |||
9 | #include "pinocchio/spatial/fwd.hpp" | ||
10 | #include "pinocchio/spatial/se3-base.hpp" | ||
11 | |||
12 | #include "pinocchio/math/quaternion.hpp" | ||
13 | #include "pinocchio/math/rotation.hpp" | ||
14 | #include "pinocchio/spatial/cartesian-axis.hpp" | ||
15 | |||
16 | #include <Eigen/Geometry> | ||
17 | |||
18 | namespace pinocchio | ||
19 | { | ||
20 | template<typename _Scalar, int _Options> | ||
21 | struct traits<SE3Tpl<_Scalar, _Options>> | ||
22 | { | ||
23 | enum | ||
24 | { | ||
25 | Options = _Options, | ||
26 | LINEAR = 0, | ||
27 | ANGULAR = 3 | ||
28 | }; | ||
29 | typedef _Scalar Scalar; | ||
30 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
31 | typedef Eigen::Matrix<Scalar, 4, 1, Options> Vector4; | ||
32 | typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6; | ||
33 | typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3; | ||
34 | typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4; | ||
35 | typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; | ||
36 | typedef Matrix3 AngularType; | ||
37 | typedef typename PINOCCHIO_EIGEN_REF_TYPE(Matrix3) AngularRef; | ||
38 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix3) ConstAngularRef; | ||
39 | typedef Vector3 LinearType; | ||
40 | typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector3) LinearRef; | ||
41 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector3) ConstLinearRef; | ||
42 | typedef Matrix6 ActionMatrixType; | ||
43 | typedef Matrix4 HomogeneousMatrixType; | ||
44 | typedef SE3Tpl<Scalar, Options> PlainType; | ||
45 | }; // traits SE3Tpl | ||
46 | |||
47 | template<typename _Scalar, int _Options> | ||
48 | struct SE3Tpl : public SE3Base<SE3Tpl<_Scalar, _Options>> | ||
49 | { | ||
50 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
51 | |||
52 | PINOCCHIO_SE3_TYPEDEF_TPL(SE3Tpl); | ||
53 | typedef SE3Base<SE3Tpl<_Scalar, _Options>> Base; | ||
54 | typedef Eigen::Quaternion<Scalar, Options> Quaternion; | ||
55 | typedef typename traits<SE3Tpl>::Vector3 Vector3; | ||
56 | typedef typename traits<SE3Tpl>::Matrix3 Matrix3; | ||
57 | typedef typename traits<SE3Tpl>::Matrix4 Matrix4; | ||
58 | typedef typename traits<SE3Tpl>::Vector4 Vector4; | ||
59 | typedef typename traits<SE3Tpl>::Matrix6 Matrix6; | ||
60 | |||
61 | using Base::rotation; | ||
62 | using Base::translation; | ||
63 | |||
64 | 3985 | SE3Tpl() | |
65 | 3985 | : rot() | |
66 | 3985 | , trans() {}; | |
67 | |||
68 | template<typename QuaternionLike, typename Vector3Like> | ||
69 | 6 | SE3Tpl( | |
70 | const Eigen::QuaternionBase<QuaternionLike> & quat, | ||
71 | const Eigen::MatrixBase<Vector3Like> & trans) | ||
72 | 6 | : rot(quat.matrix()) | |
73 | 6 | , trans(trans) | |
74 | { | ||
75 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3) | ||
76 | 6 | } | |
77 | |||
78 | template<typename Matrix3Like, typename Vector3Like> | ||
79 | 291053 | SE3Tpl(const Eigen::MatrixBase<Matrix3Like> & R, const Eigen::MatrixBase<Vector3Like> & trans) | |
80 | 291053 | : rot(R) | |
81 | 291053 | , trans(trans){EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3) | |
82 | 291053 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, 3, 3)} | |
83 | |||
84 | 70895 | SE3Tpl(const SE3Tpl & other) | |
85 | 70895 | { | |
86 | 70895 | *this = other; | |
87 | 70895 | } | |
88 | |||
89 | template<typename S2, int O2> | ||
90 | ✗ | explicit SE3Tpl(const SE3Tpl<S2, O2> & other) | |
91 | { | ||
92 | ✗ | *this = other.template cast<Scalar>(); | |
93 | } | ||
94 | |||
95 | template<typename Matrix4Like> | ||
96 | ✗ | explicit SE3Tpl(const Eigen::MatrixBase<Matrix4Like> & m) | |
97 | ✗ | : rot(m.template block<3, 3>(LINEAR, LINEAR)) | |
98 | ✗ | , trans(m.template block<3, 1>(LINEAR, ANGULAR)) | |
99 | { | ||
100 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like, 4, 4); | ||
101 | } | ||
102 | |||
103 | 75185 | explicit SE3Tpl(int) | |
104 |
1/2✓ Branch 2 taken 75185 times.
✗ Branch 3 not taken.
|
75185 | : rot(AngularType::Identity()) |
105 |
1/2✓ Branch 2 taken 75185 times.
✗ Branch 3 not taken.
|
75185 | , trans(LinearType::Zero()) |
106 | { | ||
107 | 75185 | } | |
108 | |||
109 | template<int O2> | ||
110 | SE3Tpl(const SE3Tpl<Scalar, O2> & clone) | ||
111 | : rot(clone.rotation()) | ||
112 | , trans(clone.translation()) | ||
113 | { | ||
114 | } | ||
115 | |||
116 | template<int O2> | ||
117 | SE3Tpl & operator=(const SE3Tpl<Scalar, O2> & other) | ||
118 | { | ||
119 | rot = other.rotation(); | ||
120 | trans = other.translation(); | ||
121 | return *this; | ||
122 | } | ||
123 | |||
124 | /// | ||
125 | /// \brief Copy assignment operator. | ||
126 | /// | ||
127 | /// \param[in] other SE3 to copy | ||
128 | /// | ||
129 | 205916 | SE3Tpl & operator=(const SE3Tpl & other) | |
130 | { | ||
131 | 205916 | rot = other.rotation(); | |
132 | 205916 | trans = other.translation(); | |
133 | 205916 | return *this; | |
134 | } | ||
135 | |||
136 | 75185 | static SE3Tpl Identity() | |
137 | { | ||
138 | 75185 | return SE3Tpl(1); | |
139 | } | ||
140 | |||
141 | 477 | SE3Tpl & setIdentity() | |
142 | { | ||
143 | 477 | rot.setIdentity(); | |
144 | 477 | trans.setZero(); | |
145 | 477 | return *this; | |
146 | } | ||
147 | |||
148 | /// aXb = bXa.inverse() | ||
149 | 250 | SE3Tpl inverse() const | |
150 | { | ||
151 |
4/8✓ Branch 2 taken 250 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 250 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 250 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 250 times.
✗ Branch 12 not taken.
|
500 | return SE3Tpl(rot.transpose(), -rot.transpose() * trans); |
152 | } | ||
153 | |||
154 | 3218 | static SE3Tpl Random() | |
155 | { | ||
156 |
2/4✓ Branch 2 taken 3218 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3218 times.
✗ Branch 6 not taken.
|
6436 | return SE3Tpl().setRandom(); |
157 | } | ||
158 | |||
159 | 2866 | SE3Tpl & setRandom() | |
160 | { | ||
161 |
1/2✓ Branch 1 taken 2866 times.
✗ Branch 2 not taken.
|
2866 | Quaternion q; |
162 |
1/2✓ Branch 1 taken 2866 times.
✗ Branch 2 not taken.
|
2866 | quaternion::uniformRandom(q); |
163 |
1/2✓ Branch 1 taken 2866 times.
✗ Branch 2 not taken.
|
2866 | rot = q.matrix(); |
164 |
1/2✓ Branch 1 taken 2866 times.
✗ Branch 2 not taken.
|
2866 | trans.setRandom(); |
165 | |||
166 | 2866 | return *this; | |
167 | } | ||
168 | |||
169 | ✗ | HomogeneousMatrixType toHomogeneousMatrix_impl() const | |
170 | { | ||
171 | ✗ | HomogeneousMatrixType M; | |
172 | ✗ | M.template block<3, 3>(LINEAR, LINEAR) = rot; | |
173 | ✗ | M.template block<3, 1>(LINEAR, ANGULAR) = trans; | |
174 | ✗ | M.template block<1, 3>(ANGULAR, LINEAR).setZero(); | |
175 | ✗ | M(3, 3) = 1; | |
176 | ✗ | return M; | |
177 | } | ||
178 | |||
179 | /// Vb.toVector() = bXa.toMatrix() * Va.toVector() | ||
180 | template<typename Matrix6Like> | ||
181 | 5468 | void toActionMatrix_impl(const Eigen::MatrixBase<Matrix6Like> & action_matrix) const | |
182 | { | ||
183 | typedef Eigen::Block<Matrix6Like, 3, 3> Block3; | ||
184 | |||
185 | 5468 | Matrix6Like & M = action_matrix.const_cast_derived(); | |
186 |
4/8✓ Branch 1 taken 5468 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5468 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5468 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5468 times.
✗ Branch 11 not taken.
|
5468 | M.template block<3, 3>(ANGULAR, ANGULAR) = M.template block<3, 3>(LINEAR, LINEAR) = rot; |
187 |
2/4✓ Branch 1 taken 5468 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5468 times.
✗ Branch 5 not taken.
|
5468 | M.template block<3, 3>(ANGULAR, LINEAR).setZero(); |
188 |
1/2✓ Branch 1 taken 5468 times.
✗ Branch 2 not taken.
|
5468 | Block3 B = M.template block<3, 3>(LINEAR, ANGULAR); |
189 | |||
190 |
4/8✓ Branch 1 taken 5468 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5468 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5468 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5468 times.
✗ Branch 11 not taken.
|
5468 | B.col(0) = trans.cross(rot.col(0)); |
191 |
4/8✓ Branch 1 taken 5468 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5468 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5468 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5468 times.
✗ Branch 11 not taken.
|
5468 | B.col(1) = trans.cross(rot.col(1)); |
192 |
4/8✓ Branch 1 taken 5468 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5468 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5468 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5468 times.
✗ Branch 11 not taken.
|
5468 | B.col(2) = trans.cross(rot.col(2)); |
193 | 5468 | } | |
194 | |||
195 | 5468 | ActionMatrixType toActionMatrix_impl() const | |
196 | { | ||
197 | 5468 | ActionMatrixType res; | |
198 | 5468 | toActionMatrix_impl(res); | |
199 | 5468 | return res; | |
200 | } | ||
201 | |||
202 | template<typename Matrix6Like> | ||
203 | void | ||
204 | 23 | toActionMatrixInverse_impl(const Eigen::MatrixBase<Matrix6Like> & action_matrix_inverse) const | |
205 | { | ||
206 | typedef Eigen::Block<Matrix6Like, 3, 3> Block3; | ||
207 | |||
208 | 23 | Matrix6Like & M = action_matrix_inverse.const_cast_derived(); | |
209 |
4/8✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
|
23 | M.template block<3, 3>(ANGULAR, ANGULAR) = M.template block<3, 3>(LINEAR, LINEAR) = |
210 |
1/2✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
|
23 | rot.transpose(); |
211 |
1/2✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
|
23 | Block3 C = M.template block<3, 3>(ANGULAR, LINEAR); // used as temporary |
212 |
1/2✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
|
23 | Block3 B = M.template block<3, 3>(LINEAR, ANGULAR); |
213 | |||
214 | #define PINOCCHIO_INTERNAL_COMPUTATION(axis_id, v3_in, v3_out, R, res) \ | ||
215 | CartesianAxis<axis_id>::cross(v3_in, v3_out); \ | ||
216 | res.col(axis_id).noalias() = R.transpose() * v3_out; | ||
217 | |||
218 |
8/16✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 23 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
|
23 | PINOCCHIO_INTERNAL_COMPUTATION(0, trans, C.col(0), rot, B); |
219 |
8/16✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 23 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
|
23 | PINOCCHIO_INTERNAL_COMPUTATION(1, trans, C.col(0), rot, B); |
220 |
8/16✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 23 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
|
23 | PINOCCHIO_INTERNAL_COMPUTATION(2, trans, C.col(0), rot, B); |
221 | |||
222 | #undef PINOCCHIO_INTERNAL_COMPUTATION | ||
223 | |||
224 |
1/2✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
|
23 | C.setZero(); |
225 | 23 | } | |
226 | |||
227 | 23 | ActionMatrixType toActionMatrixInverse_impl() const | |
228 | { | ||
229 | 23 | ActionMatrixType res; | |
230 | 23 | toActionMatrixInverse_impl(res); | |
231 | 23 | return res; | |
232 | } | ||
233 | |||
234 | template<typename Matrix6Like> | ||
235 | 71 | void toDualActionMatrix_impl(const Eigen::MatrixBase<Matrix6Like> & dual_action_matrix) const | |
236 | { | ||
237 | typedef Eigen::Block<Matrix6Like, 3, 3> Block3; | ||
238 | |||
239 | 71 | Matrix6Like & M = dual_action_matrix.const_cast_derived(); | |
240 |
4/8✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 71 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 71 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 71 times.
✗ Branch 11 not taken.
|
71 | M.template block<3, 3>(ANGULAR, ANGULAR) = M.template block<3, 3>(LINEAR, LINEAR) = rot; |
241 |
2/4✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 71 times.
✗ Branch 5 not taken.
|
71 | M.template block<3, 3>(LINEAR, ANGULAR).setZero(); |
242 |
1/2✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
|
71 | Block3 B = M.template block<3, 3>(ANGULAR, LINEAR); |
243 | |||
244 |
4/8✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 71 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 71 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 71 times.
✗ Branch 11 not taken.
|
71 | B.col(0) = trans.cross(rot.col(0)); |
245 |
4/8✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 71 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 71 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 71 times.
✗ Branch 11 not taken.
|
71 | B.col(1) = trans.cross(rot.col(1)); |
246 |
4/8✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 71 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 71 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 71 times.
✗ Branch 11 not taken.
|
71 | B.col(2) = trans.cross(rot.col(2)); |
247 | 71 | } | |
248 | |||
249 | 71 | ActionMatrixType toDualActionMatrix_impl() const | |
250 | { | ||
251 | 71 | ActionMatrixType res; | |
252 | 71 | toDualActionMatrix_impl(res); | |
253 | 71 | return res; | |
254 | } | ||
255 | |||
256 | 6 | void disp_impl(std::ostream & os) const | |
257 | { | ||
258 |
2/4✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
|
6 | os << " R =\n" << rot << std::endl << " p = " << trans.transpose() << std::endl; |
259 | 6 | } | |
260 | |||
261 | /// --- GROUP ACTIONS ON M6, F6 and I6 --- | ||
262 | |||
263 | /// ay = aXb.act(by) | ||
264 | template<typename D> | ||
265 | 466645 | typename SE3GroupAction<D>::ReturnType act_impl(const D & d) const | |
266 | { | ||
267 | 466645 | return d.se3Action(*this); | |
268 | } | ||
269 | |||
270 | /// by = aXb.actInv(ay) | ||
271 | template<typename D> | ||
272 | 235532 | typename SE3GroupAction<D>::ReturnType actInv_impl(const D & d) const | |
273 | { | ||
274 | 235532 | return d.se3ActionInverse(*this); | |
275 | } | ||
276 | |||
277 | template<typename EigenDerived> | ||
278 | typename EigenDerived::PlainObject | ||
279 | actOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) const | ||
280 | { | ||
281 | return (rotation() * p + translation()).eval(); | ||
282 | } | ||
283 | |||
284 | template<typename MapDerived> | ||
285 | Vector3 actOnEigenObject(const Eigen::MapBase<MapDerived> & p) const | ||
286 | { | ||
287 | return Vector3(rotation() * p + translation()); | ||
288 | } | ||
289 | |||
290 | template<typename EigenDerived> | ||
291 | typename EigenDerived::PlainObject | ||
292 | actInvOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) const | ||
293 | { | ||
294 | return (rotation().transpose() * (p - translation())).eval(); | ||
295 | } | ||
296 | |||
297 | template<typename MapDerived> | ||
298 | Vector3 actInvOnEigenObject(const Eigen::MapBase<MapDerived> & p) const | ||
299 | { | ||
300 | return Vector3(rotation().transpose() * (p - translation())); | ||
301 | } | ||
302 | |||
303 | 160 | Vector3 act_impl(const Vector3 & p) const | |
304 | { | ||
305 |
2/4✓ Branch 4 taken 160 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 160 times.
✗ Branch 8 not taken.
|
160 | return Vector3(rotation() * p + translation()); |
306 | } | ||
307 | |||
308 | 6074 | Vector3 actInv_impl(const Vector3 & p) const | |
309 | { | ||
310 |
4/8✓ Branch 3 taken 6074 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6074 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6074 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6074 times.
✗ Branch 13 not taken.
|
6074 | return Vector3(rotation().transpose() * (p - translation())); |
311 | } | ||
312 | |||
313 | template<int O2> | ||
314 | 180082 | SE3Tpl act_impl(const SE3Tpl<Scalar, O2> & m2) const | |
315 | { | ||
316 |
5/10✓ Branch 4 taken 180082 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 180082 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 180082 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 180082 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 180082 times.
✗ Branch 17 not taken.
|
360164 | return SE3Tpl(rot * m2.rotation(), translation() + rotation() * m2.translation()); |
317 | } | ||
318 | |||
319 | template<int O2> | ||
320 | 3382 | SE3Tpl actInv_impl(const SE3Tpl<Scalar, O2> & m2) const | |
321 | { | ||
322 | return SE3Tpl( | ||
323 |
6/12✓ Branch 4 taken 3382 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3382 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3382 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3382 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3382 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3382 times.
✗ Branch 20 not taken.
|
6764 | rot.transpose() * m2.rotation(), rot.transpose() * (m2.translation() - translation())); |
324 | } | ||
325 | |||
326 | template<int O2> | ||
327 | 180081 | SE3Tpl __mult__(const SE3Tpl<Scalar, O2> & m2) const | |
328 | { | ||
329 | 180081 | return this->act_impl(m2); | |
330 | } | ||
331 | |||
332 | template<int O2> | ||
333 | 98 | bool isEqual(const SE3Tpl<Scalar, O2> & m2) const | |
334 | { | ||
335 |
2/4✓ Branch 3 taken 98 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 98 times.
✗ Branch 9 not taken.
|
98 | return (rotation() == m2.rotation() && translation() == m2.translation()); |
336 | } | ||
337 | |||
338 | template<int O2> | ||
339 | 336 | bool isApprox_impl( | |
340 | const SE3Tpl<Scalar, O2> & m2, | ||
341 | const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | ||
342 | { | ||
343 | 336 | return rotation().isApprox(m2.rotation(), prec) | |
344 |
2/4✓ Branch 0 taken 336 times.
✗ Branch 1 not taken.
✓ Branch 5 taken 336 times.
✗ Branch 6 not taken.
|
336 | && translation().isApprox(m2.translation(), prec); |
345 | } | ||
346 | |||
347 | ✗ | bool isIdentity(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | |
348 | { | ||
349 | ✗ | return rotation().isIdentity(prec) && translation().isZero(prec); | |
350 | } | ||
351 | |||
352 | 941040 | ConstAngularRef rotation_impl() const | |
353 | { | ||
354 | 941040 | return rot; | |
355 | } | ||
356 | 80039 | AngularRef rotation_impl() | |
357 | { | ||
358 | 80039 | return rot; | |
359 | } | ||
360 | 21351 | void rotation_impl(const AngularType & R) | |
361 | { | ||
362 | 21351 | rot = R; | |
363 | 21351 | } | |
364 | 743558 | ConstLinearRef translation_impl() const | |
365 | { | ||
366 | 743558 | return trans; | |
367 | } | ||
368 | 13888 | LinearRef translation_impl() | |
369 | { | ||
370 | 13888 | return trans; | |
371 | } | ||
372 | 21241 | void translation_impl(const LinearType & p) | |
373 | { | ||
374 | 21241 | trans = p; | |
375 | 21241 | } | |
376 | |||
377 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
378 | template<typename NewScalar> | ||
379 | 1777 | SE3Tpl<NewScalar, Options> cast() const | |
380 | { | ||
381 | typedef SE3Tpl<NewScalar, Options> ReturnType; | ||
382 |
0/4✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
1777 | ReturnType res(rot.template cast<NewScalar>(), trans.template cast<NewScalar>()); |
383 | |||
384 | // During the cast, it may appear that the matrix is not normalized correctly. | ||
385 | // Force the normalization of the rotation part of the matrix. | ||
386 | 1777 | internal::cast_call_normalize_method<SE3Tpl, NewScalar, Scalar>::run(res); | |
387 | 1777 | return res; | |
388 | } | ||
389 | |||
390 | ✗ | bool isNormalized(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | |
391 | { | ||
392 | ✗ | return isUnitary(rot, prec); | |
393 | } | ||
394 | |||
395 | ✗ | void normalize() | |
396 | { | ||
397 | ✗ | rot = orthogonalProjection(rot); | |
398 | } | ||
399 | |||
400 | ✗ | PlainType normalized() const | |
401 | { | ||
402 | ✗ | PlainType res(*this); | |
403 | ✗ | res.normalize(); | |
404 | ✗ | return res; | |
405 | } | ||
406 | |||
407 | /// | ||
408 | /// \brief Linear interpolation on the SE3 manifold. | ||
409 | /// | ||
410 | /// \param[in] A Initial transformation. | ||
411 | /// \param[in] B Target transformation. | ||
412 | /// \param[in] alpha Interpolation factor in [0 ... 1]. | ||
413 | /// | ||
414 | /// \returns An interpolated transformation between A and B. | ||
415 | /// | ||
416 | /// \note This is similar to the SLERP operation which acts initially for rotation but applied | ||
417 | /// here to rigid transformation. | ||
418 | /// | ||
419 | template<typename OtherScalar> | ||
420 | static SE3Tpl Interpolate(const SE3Tpl & A, const SE3Tpl & B, const OtherScalar & alpha); | ||
421 | |||
422 | protected: | ||
423 | AngularType rot; | ||
424 | LinearType trans; | ||
425 | |||
426 | }; // class SE3Tpl | ||
427 | |||
428 | namespace internal | ||
429 | { | ||
430 | template<typename Scalar, int Options> | ||
431 | struct cast_call_normalize_method<SE3Tpl<Scalar, Options>, Scalar, Scalar> | ||
432 | { | ||
433 | template<typename T> | ||
434 | 1777 | static void run(T &) | |
435 | { | ||
436 | 1777 | } | |
437 | }; | ||
438 | |||
439 | template<typename Scalar, int Options, typename NewScalar> | ||
440 | struct cast_call_normalize_method<SE3Tpl<Scalar, Options>, NewScalar, Scalar> | ||
441 | { | ||
442 | template<typename T> | ||
443 | ✗ | static void run(T & self) | |
444 | { | ||
445 | ✗ | if ( | |
446 | ✗ | pinocchio::cast<NewScalar>(Eigen::NumTraits<Scalar>::epsilon()) | |
447 | ✗ | > Eigen::NumTraits<NewScalar>::epsilon()) | |
448 | ✗ | self.normalize(); | |
449 | } | ||
450 | }; | ||
451 | |||
452 | } // namespace internal | ||
453 | |||
454 | } // namespace pinocchio | ||
455 | |||
456 | #endif // ifndef __pinocchio_spatial_se3_tpl_hpp__ | ||
457 |