GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/se3-tpl.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 154 154 100.0%
Branches: 126 271 46.5%
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 416853 SE3Tpl()
65 416853 : rot()
66
1/2
✓ Branch 1 taken 1235 times.
✗ Branch 2 not taken.
 416853 , trans() {};
67
68 template<typename QuaternionLike, typename Vector3Like>
69 350 SE3Tpl(
70 const Eigen::QuaternionBase<QuaternionLike> & quat,
71 const Eigen::MatrixBase<Vector3Like> & trans)
72 350 : rot(quat.matrix())
73
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 350 , trans(trans)
74 {
75 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3)
76 350 }
77
78 template<typename Matrix3Like, typename Vector3Like>
79 2620335 SE3Tpl(const Eigen::MatrixBase<Matrix3Like> & R, const Eigen::MatrixBase<Vector3Like> & trans)
80 2620335 : rot(R)
81
1/2
✓ Branch 1 taken 1974 times.
✗ Branch 2 not taken.
 2620335 , trans(trans){EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3)
82 2620335 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, 3, 3)}
83
84 1006054 SE3Tpl(const SE3Tpl & other)
85
1/2
✓ Branch 2 taken 3472 times.
✗ Branch 3 not taken.
 1006054 {
86
1/2
✓ Branch 1 taken 3472 times.
✗ Branch 2 not taken.
 1006054 *this = other;
87 1006054 }
88
89 template<typename S2, int O2>
90 1 explicit SE3Tpl(const SE3Tpl<S2, O2> & other)
91 1 {
92
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 *this = other.template cast<Scalar>();
93 1 }
94
95 template<typename Matrix4Like>
96 5 explicit SE3Tpl(const Eigen::MatrixBase<Matrix4Like> & m)
97
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 : rot(m.template block<3, 3>(LINEAR, LINEAR))
98
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 5 , trans(m.template block<3, 1>(LINEAR, ANGULAR))
99 {
100 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like, 4, 4);
101 5 }
102
103 759169 explicit SE3Tpl(int)
104
1/2
✓ Branch 2 taken 758560 times.
✗ Branch 3 not taken.
 759169 : rot(AngularType::Identity())
105
3/5
✓ Branch 1 taken 579 times.
✓ Branch 2 taken 757981 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 579 times.
✗ Branch 5 not taken.
 759169 , trans(LinearType::Zero())
106 {
107 759169 }
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 2933733 SE3Tpl & operator=(const SE3Tpl & other)
130 {
131 2933733 rot = other.rotation();
132 2933733 trans = other.translation();
133 2933733 return *this;
134 }
135
136 759158 static SE3Tpl Identity()
137 {
138 759158 return SE3Tpl(1);
139 }
140
141 1114 SE3Tpl & setIdentity()
142 {
143 1114 rot.setIdentity();
144 1114 trans.setZero();
145 1114 return *this;
146 }
147
148 /// aXb = bXa.inverse()
149 694 SE3Tpl inverse() const
150 {
151
4/8
✓ Branch 2 taken 694 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 694 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 694 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 694 times.
✗ Branch 12 not taken.
 1388 return SE3Tpl(rot.transpose(), -rot.transpose() * trans);
152 }
153
154 329873 static SE3Tpl Random()
155 {
156
2/4
✓ Branch 2 taken 319872 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 319872 times.
✗ Branch 6 not taken.
 659746 return SE3Tpl().setRandom();
157 }
158
159 329955 SE3Tpl & setRandom()
160 {
161
1/2
✓ Branch 1 taken 319954 times.
✗ Branch 2 not taken.
 329955 Quaternion q;
162
1/2
✓ Branch 1 taken 319954 times.
✗ Branch 2 not taken.
 329955 quaternion::uniformRandom(q);
163
1/2
✓ Branch 1 taken 319954 times.
✗ Branch 2 not taken.
 329955 rot = q.matrix();
164
1/2
✓ Branch 1 taken 319954 times.
✗ Branch 2 not taken.
 329955 trans.setRandom();
165
166 329955 return *this;
167 }
168
169 74 HomogeneousMatrixType toHomogeneousMatrix_impl() const
170 {
171 74 HomogeneousMatrixType M;
172
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 74 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 74 M.template block<3, 3>(LINEAR, LINEAR) = rot;
173
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 74 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 74 M.template block<3, 1>(LINEAR, ANGULAR) = trans;
174
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 74 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 74 M.template block<1, 3>(ANGULAR, LINEAR).setZero();
175
0/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 74 M(3, 3) = 1;
176 74 return M;
177 }
178
179 /// Vb.toVector() = bXa.toMatrix() * Va.toVector()
180 template<typename Matrix6Like>
181 6140 void toActionMatrix_impl(const Eigen::MatrixBase<Matrix6Like> & action_matrix) const
182 {
183 typedef Eigen::Block<Matrix6Like, 3, 3> Block3;
184
185 6140 Matrix6Like & M = action_matrix.const_cast_derived();
186
4/8
✓ Branch 1 taken 6136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6136 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6136 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6136 times.
✗ Branch 11 not taken.
 6140 M.template block<3, 3>(ANGULAR, ANGULAR) = M.template block<3, 3>(LINEAR, LINEAR) = rot;
187
2/4
✓ Branch 1 taken 6136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6136 times.
✗ Branch 5 not taken.
 6140 M.template block<3, 3>(ANGULAR, LINEAR).setZero();
188
1/2
✓ Branch 1 taken 6136 times.
✗ Branch 2 not taken.
 6140 Block3 B = M.template block<3, 3>(LINEAR, ANGULAR);
189
190
4/8
✓ Branch 1 taken 6136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6136 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6136 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6136 times.
✗ Branch 11 not taken.
 6140 B.col(0) = trans.cross(rot.col(0));
191
4/8
✓ Branch 1 taken 6136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6136 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6136 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6136 times.
✗ Branch 11 not taken.
 6140 B.col(1) = trans.cross(rot.col(1));
192
4/8
✓ Branch 1 taken 6136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6136 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6136 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6136 times.
✗ Branch 11 not taken.
 6140 B.col(2) = trans.cross(rot.col(2));
193 6140 }
194
195 6140 ActionMatrixType toActionMatrix_impl() const
196 {
197 6140 ActionMatrixType res;
198
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6140 toActionMatrix_impl(res);
199 6140 return res;
200 }
201
202 template<typename Matrix6Like>
203 void
204 75 toActionMatrixInverse_impl(const Eigen::MatrixBase<Matrix6Like> & action_matrix_inverse) const
205 {
206 typedef Eigen::Block<Matrix6Like, 3, 3> Block3;
207
208 75 Matrix6Like & M = action_matrix_inverse.const_cast_derived();
209
4/8
✓ Branch 1 taken 75 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 75 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 75 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 75 times.
✗ Branch 11 not taken.
 75 M.template block<3, 3>(ANGULAR, ANGULAR) = M.template block<3, 3>(LINEAR, LINEAR) =
210
1/2
✓ Branch 1 taken 75 times.
✗ Branch 2 not taken.
 75 rot.transpose();
211
1/2
✓ Branch 1 taken 75 times.
✗ Branch 2 not taken.
 75 Block3 C = M.template block<3, 3>(ANGULAR, LINEAR); // used as temporary
212
1/2
✓ Branch 1 taken 75 times.
✗ Branch 2 not taken.
 75 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 75 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 75 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 75 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 75 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 75 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 75 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 75 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 75 times.
✗ Branch 23 not taken.
 75 PINOCCHIO_INTERNAL_COMPUTATION(0, trans, C.col(0), rot, B);
219
8/16
✓ Branch 1 taken 75 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 75 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 75 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 75 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 75 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 75 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 75 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 75 times.
✗ Branch 23 not taken.
 75 PINOCCHIO_INTERNAL_COMPUTATION(1, trans, C.col(0), rot, B);
220
8/16
✓ Branch 1 taken 75 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 75 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 75 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 75 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 75 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 75 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 75 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 75 times.
✗ Branch 23 not taken.
 75 PINOCCHIO_INTERNAL_COMPUTATION(2, trans, C.col(0), rot, B);
221
222 #undef PINOCCHIO_INTERNAL_COMPUTATION
223
224
1/2
✓ Branch 1 taken 75 times.
✗ Branch 2 not taken.
 75 C.setZero();
225 75 }
226
227 75 ActionMatrixType toActionMatrixInverse_impl() const
228 {
229 75 ActionMatrixType res;
230
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 75 toActionMatrixInverse_impl(res);
231 75 return res;
232 }
233
234 template<typename Matrix6Like>
235 268 void toDualActionMatrix_impl(const Eigen::MatrixBase<Matrix6Like> & dual_action_matrix) const
236 {
237 typedef Eigen::Block<Matrix6Like, 3, 3> Block3;
238
239 268 Matrix6Like & M = dual_action_matrix.const_cast_derived();
240
4/8
✓ Branch 1 taken 268 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 268 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 268 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 268 times.
✗ Branch 11 not taken.
 268 M.template block<3, 3>(ANGULAR, ANGULAR) = M.template block<3, 3>(LINEAR, LINEAR) = rot;
241
2/4
✓ Branch 1 taken 268 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 268 times.
✗ Branch 5 not taken.
 268 M.template block<3, 3>(LINEAR, ANGULAR).setZero();
242
1/2
✓ Branch 1 taken 268 times.
✗ Branch 2 not taken.
 268 Block3 B = M.template block<3, 3>(ANGULAR, LINEAR);
243
244
4/8
✓ Branch 1 taken 268 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 268 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 268 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 268 times.
✗ Branch 11 not taken.
 268 B.col(0) = trans.cross(rot.col(0));
245
4/8
✓ Branch 1 taken 268 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 268 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 268 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 268 times.
✗ Branch 11 not taken.
 268 B.col(1) = trans.cross(rot.col(1));
246
4/8
✓ Branch 1 taken 268 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 268 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 268 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 268 times.
✗ Branch 11 not taken.
 268 B.col(2) = trans.cross(rot.col(2));
247 268 }
248
249 268 ActionMatrixType toDualActionMatrix_impl() const
250 {
251 268 ActionMatrixType res;
252
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 268 toDualActionMatrix_impl(res);
253 268 return res;
254 }
255
256 100038 void disp_impl(std::ostream & os) const
257 {
258
2/4
✓ Branch 6 taken 100038 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 100038 times.
✗ Branch 10 not taken.
 100038 os << " R =\n" << rot << std::endl << " p = " << trans.transpose() << std::endl;
259 100038 }
260
261 /// --- GROUP ACTIONS ON M6, F6 and I6 ---
262
263 /// ay = aXb.act(by)
264 template<typename D>
265 694812 typename SE3GroupAction<D>::ReturnType act_impl(const D & d) const
266 {
267 694812 return d.se3Action(*this);
268 }
269
270 /// by = aXb.actInv(ay)
271 template<typename D>
272 336319 typename SE3GroupAction<D>::ReturnType actInv_impl(const D & d) const
273 {
274 336319 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 1680 Vector3 act_impl(const Vector3 & p) const
304 {
305
2/4
✓ Branch 4 taken 1680 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1680 times.
✗ Branch 8 not taken.
 1680 return Vector3(rotation() * p + translation());
306 }
307
308 6389 Vector3 actInv_impl(const Vector3 & p) const
309 {
310
4/8
✓ Branch 3 taken 6389 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6389 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6389 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6389 times.
✗ Branch 13 not taken.
 6389 return Vector3(rotation().transpose() * (p - translation()));
311 }
312
313 template<int O2>
314 1995131 SE3Tpl act_impl(const SE3Tpl<Scalar, O2> & m2) const
315 {
316
5/10
✓ Branch 4 taken 1994612 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1994612 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1994612 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1994612 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1994612 times.
✗ Branch 17 not taken.
 3990262 return SE3Tpl(rot * m2.rotation(), translation() + rotation() * m2.translation());
317 }
318
319 template<int O2>
320 18132 SE3Tpl actInv_impl(const SE3Tpl<Scalar, O2> & m2) const
321 {
322 return SE3Tpl(
323
6/12
✓ Branch 4 taken 18130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18130 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18130 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 18130 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 18130 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18130 times.
✗ Branch 20 not taken.
 36264 rot.transpose() * m2.rotation(), rot.transpose() * (m2.translation() - translation()));
324 }
325
326 template<int O2>
327 1995129 SE3Tpl __mult__(const SE3Tpl<Scalar, O2> & m2) const
328 {
329 1995129 return this->act_impl(m2);
330 }
331
332 template<int O2>
333 12707 bool isEqual(const SE3Tpl<Scalar, O2> & m2) const
334 {
335
3/4
✓ Branch 3 taken 8838 times.
✓ Branch 4 taken 4 times.
✓ Branch 8 taken 8838 times.
✗ Branch 9 not taken.
 12707 return (rotation() == m2.rotation() && translation() == m2.translation());
336 }
337
338 template<int O2>
339 65289 bool isApprox_impl(
340 const SE3Tpl<Scalar, O2> & m2,
341 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
342 {
343 65289 return rotation().isApprox(m2.rotation(), prec)
344
2/4
✓ Branch 0 taken 65285 times.
✗ Branch 1 not taken.
✓ Branch 5 taken 65285 times.
✗ Branch 6 not taken.
 65289 && translation().isApprox(m2.translation(), prec);
345 }
346
347 7 bool isIdentity(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
348 {
349
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 return rotation().isIdentity(prec) && translation().isZero(prec);
350 }
351
352 8308788 ConstAngularRef rotation_impl() const
353 {
354 8308788 return rot;
355 }
356 1145327 AngularRef rotation_impl()
357 {
358 1165956 return rot;
359 }
360 36199 void rotation_impl(const AngularType & R)
361 {
362 36218 rot = R;
363 36218 }
364 7680763 ConstLinearRef translation_impl() const
365 {
366 7685040 return trans;
367 }
368 449726 LinearRef translation_impl()
369 {
370 449850 return trans;
371 }
372 29872 void translation_impl(const LinearType & p)
373 {
374 29886 trans = p;
375 29886 }
376
377 /// \returns An expression of *this with the Scalar type casted to NewScalar.
378 template<typename NewScalar>
379 354595 SE3Tpl<NewScalar, Options> cast() const
380 {
381 typedef SE3Tpl<NewScalar, Options> ReturnType;
382
2/4
✓ Branch 2 taken 1265 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1265 times.
✗ Branch 6 not taken.
 354595 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 354595 internal::cast_call_normalize_method<SE3Tpl, NewScalar, Scalar>::run(res);
387 354595 return res;
388 }
389
390 3 bool isNormalized(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
391 {
392 3 return isUnitary(rot, prec);
393 }
394
395 344 void normalize()
396 {
397 344 rot = orthogonalProjection(rot);
398 344 }
399
400 1 PlainType normalized() const
401 {
402 1 PlainType res(*this);
403 1 res.normalize();
404 1 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 352394 static void run(T &)
435 {
436 352394 }
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 347 static void run(T & self)
444 {
445 347 if (
446
1/2
✓ Branch 2 taken 343 times.
✗ Branch 3 not taken.
 347 pinocchio::cast<NewScalar>(Eigen::NumTraits<Scalar>::epsilon())
447
2/2
✓ Branch 1 taken 338 times.
✓ Branch 2 taken 5 times.
 347 > Eigen::NumTraits<NewScalar>::epsilon())
448 340 self.normalize();
449 347 }
450 };
451
452 } // namespace internal
453
454 } // namespace pinocchio
455
456 #endif // ifndef __pinocchio_spatial_se3_tpl_hpp__
457