GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-spherical.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 159 165 96.4%
Branches: 129 313 41.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5
6 #ifndef __pinocchio_multibody_joint_spherical_hpp__
7 #define __pinocchio_multibody_joint_spherical_hpp__
8
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/joint-motion-subspace.hpp"
12 #include "pinocchio/math/sincos.hpp"
13 #include "pinocchio/spatial/inertia.hpp"
14 #include "pinocchio/spatial/skew.hpp"
15
16 namespace pinocchio
17 {
18
19 template<typename Scalar, int Options = context::Options>
20 struct MotionSphericalTpl;
21 typedef MotionSphericalTpl<context::Scalar> MotionSpherical;
22
23 template<typename Scalar, int Options>
24 struct SE3GroupAction<MotionSphericalTpl<Scalar, Options>>
25 {
26 typedef MotionTpl<Scalar, Options> ReturnType;
27 };
28
29 template<typename Scalar, int Options, typename MotionDerived>
30 struct MotionAlgebraAction<MotionSphericalTpl<Scalar, Options>, MotionDerived>
31 {
32 typedef MotionTpl<Scalar, Options> ReturnType;
33 };
34
35 template<typename _Scalar, int _Options>
36 struct traits<MotionSphericalTpl<_Scalar, _Options>>
37 {
38 typedef _Scalar Scalar;
39 enum
40 {
41 Options = _Options
42 };
43 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
44 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
45 typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
46 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
47 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
48 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
49 typedef Vector3 AngularType;
50 typedef Vector3 LinearType;
51 typedef const Vector3 ConstAngularType;
52 typedef const Vector3 ConstLinearType;
53 typedef Matrix6 ActionMatrixType;
54 typedef Matrix4 HomogeneousMatrixType;
55 typedef MotionTpl<Scalar, Options> MotionPlain;
56 typedef MotionPlain PlainReturnType;
57 enum
58 {
59 LINEAR = 0,
60 ANGULAR = 3
61 };
62 }; // traits MotionSphericalTpl
63
64 template<typename _Scalar, int _Options>
65 struct MotionSphericalTpl : MotionBase<MotionSphericalTpl<_Scalar, _Options>>
66 {
67 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
68
69 MOTION_TYPEDEF_TPL(MotionSphericalTpl);
70
71 35 MotionSphericalTpl()
72 35 {
73 35 }
74
75 template<typename Vector3Like>
76 10002 MotionSphericalTpl(const Eigen::MatrixBase<Vector3Like> & w)
77 10002 : m_w(w)
78 {
79 10002 }
80
81 4246 Vector3 & operator()()
82 {
83 4246 return m_w;
84 }
85 const Vector3 & operator()() const
86 {
87 return m_w;
88 }
89
90 2086 inline PlainReturnType plain() const
91 {
92
1/2
✓ Branch 2 taken 2080 times.
✗ Branch 3 not taken.
 2086 return PlainReturnType(PlainReturnType::Vector3::Zero(), m_w);
93 }
94
95 template<typename MotionDerived>
96 9 void addTo(MotionDense<MotionDerived> & other) const
97 {
98
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 other.angular() += m_w;
99 9 }
100
101 template<typename Derived>
102 149 void setTo(MotionDense<Derived> & other) const
103 {
104
1/2
✓ Branch 2 taken 148 times.
✗ Branch 3 not taken.
 149 other.linear().setZero();
105
1/2
✓ Branch 2 taken 148 times.
✗ Branch 3 not taken.
 149 other.angular() = m_w;
106 149 }
107
108 ✗ MotionSphericalTpl __plus__(const MotionSphericalTpl & other) const
109 {
110 ✗ return MotionSphericalTpl(m_w + other.m_w);
111 }
112
113 85 bool isEqual_impl(const MotionSphericalTpl & other) const
114 {
115 85 return internal::comparison_eq(m_w, other.m_w);
116 }
117
118 template<typename MotionDerived>
119 2 bool isEqual_impl(const MotionDense<MotionDerived> & other) const
120 {
121
6/12
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
 2 return internal::comparison_eq(other.angular(), m_w) && other.linear().isZero(0);
122 }
123
124 template<typename S2, int O2, typename D2>
125 7 void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
126 {
127 // Angular
128
3/6
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
 7 v.angular().noalias() = m.rotation() * m_w;
129
130 // Linear
131
4/8
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 v.linear().noalias() = m.translation().cross(v.angular());
132 7 }
133
134 template<typename S2, int O2>
135 7 MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const
136 {
137 7 MotionPlain res;
138
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 7 se3Action_impl(m, res);
139 7 return res;
140 }
141
142 template<typename S2, int O2, typename D2>
143 2 void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
144 {
145 // Linear
146 // TODO: use v.angular() as temporary variable
147
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Vector3 v3_tmp;
148
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 v3_tmp.noalias() = m_w.cross(m.translation());
149
6/12
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
 2 v.linear().noalias() = m.rotation().transpose() * v3_tmp;
150
151 // Angular
152
6/12
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
 2 v.angular().noalias() = m.rotation().transpose() * m_w;
153 2 }
154
155 template<typename S2, int O2>
156 2 MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
157 {
158 2 MotionPlain res;
159
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 2 se3ActionInverse_impl(m, res);
160 2 return res;
161 }
162
163 template<typename M1, typename M2>
164 127 void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
165 {
166 // Linear
167
4/8
✓ Branch 2 taken 127 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 127 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 127 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 127 times.
✗ Branch 12 not taken.
 127 mout.linear().noalias() = v.linear().cross(m_w);
168
169 // Angular
170
4/8
✓ Branch 2 taken 127 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 127 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 127 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 127 times.
✗ Branch 12 not taken.
 127 mout.angular().noalias() = v.angular().cross(m_w);
171 127 }
172
173 template<typename M1>
174 127 MotionPlain motionAction(const MotionDense<M1> & v) const
175 {
176 127 MotionPlain res;
177
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 127 motionAction(v, res);
178 127 return res;
179 }
180
181 120 const Vector3 & angular() const
182 {
183 120 return m_w;
184 }
185 1308 Vector3 & angular()
186 {
187 1309 return m_w;
188 }
189
190 protected:
191 Vector3 m_w;
192 }; // struct MotionSphericalTpl
193
194 template<typename S1, int O1, typename MotionDerived>
195 inline typename MotionDerived::MotionPlain
196 120 operator+(const MotionSphericalTpl<S1, O1> & m1, const MotionDense<MotionDerived> & m2)
197 {
198
3/6
✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 120 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 120 times.
✗ Branch 10 not taken.
 240 return typename MotionDerived::MotionPlain(m2.linear(), m2.angular() + m1.angular());
199 }
200
201 template<typename Scalar, int Options>
202 struct JointMotionSubspaceSphericalTpl;
203
204 template<typename _Scalar, int _Options>
205 struct traits<JointMotionSubspaceSphericalTpl<_Scalar, _Options>>
206 {
207 typedef _Scalar Scalar;
208 enum
209 {
210 Options = _Options
211 };
212 enum
213 {
214 LINEAR = 0,
215 ANGULAR = 3
216 };
217 typedef MotionSphericalTpl<Scalar, Options> JointMotion;
218 typedef Eigen::Matrix<Scalar, 3, 1, Options> JointForce;
219 typedef Eigen::Matrix<Scalar, 6, 3, Options> DenseBase;
220 typedef Eigen::Matrix<Scalar, 3, 3, Options> ReducedSquaredMatrix;
221
222 typedef DenseBase MatrixReturnType;
223 typedef const DenseBase ConstMatrixReturnType;
224
225 typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType;
226 }; // struct traits struct JointMotionSubspaceSphericalTpl
227
228 template<typename _Scalar, int _Options>
229 struct JointMotionSubspaceSphericalTpl
230 : public JointMotionSubspaceBase<JointMotionSubspaceSphericalTpl<_Scalar, _Options>>
231 {
232 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
233
234 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceSphericalTpl)
235
236 3204 JointMotionSubspaceSphericalTpl()
237 {
238 3204 }
239
240 enum
241 {
242 NV = 3
243 };
244
245 24 int nv_impl() const
246 {
247 24 return NV;
248 }
249
250 template<typename Vector3Like>
251 117 JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & w) const
252 {
253 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
254 117 return JointMotion(w);
255 }
256
257 struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceSphericalTpl>
258 {
259 template<typename Derived>
260 6 typename ForceDense<Derived>::ConstAngularType operator*(const ForceDense<Derived> & phi)
261 {
262 6 return phi.angular();
263 }
264
265 /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
266 template<typename MatrixDerived>
267 const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
268 6 operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
269 {
270
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
 6 assert(F.rows() == 6);
271 6 return F.derived().template middleRows<3>(Inertia::ANGULAR);
272 }
273 };
274
275 13 TransposeConst transpose() const
276 {
277 13 return TransposeConst();
278 }
279
280 15 DenseBase matrix_impl() const
281 {
282 15 DenseBase S;
283
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 15 S.template block<3, 3>(LINEAR, 0).setZero();
284
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 15 S.template block<3, 3>(ANGULAR, 0).setIdentity();
285 15 return S;
286 }
287
288 template<typename S1, int O1>
289 59 Eigen::Matrix<S1, 6, 3, O1> se3Action(const SE3Tpl<S1, O1> & m) const
290 {
291 59 Eigen::Matrix<S1, 6, 3, O1> X_subspace;
292
3/11
✗ Branch 1 not taken.
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 59 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 59 cross(m.translation(), m.rotation(), X_subspace.template middleRows<3>(LINEAR));
293
1/7
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 59 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 59 X_subspace.template middleRows<3>(ANGULAR) = m.rotation();
294
295 59 return X_subspace;
296 }
297
298 template<typename S1, int O1>
299 3 Eigen::Matrix<S1, 6, 3, O1> se3ActionInverse(const SE3Tpl<S1, O1> & m) const
300 {
301 3 Eigen::Matrix<S1, 6, 3, O1> X_subspace;
302
3/11
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 3 XAxis::cross(m.translation(), X_subspace.template middleRows<3>(ANGULAR).col(0));
303
3/11
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 3 YAxis::cross(m.translation(), X_subspace.template middleRows<3>(ANGULAR).col(1));
304
3/11
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 3 ZAxis::cross(m.translation(), X_subspace.template middleRows<3>(ANGULAR).col(2));
305
306
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 X_subspace.template middleRows<3>(LINEAR).noalias() =
307
3/11
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 3 m.rotation().transpose() * X_subspace.template middleRows<3>(ANGULAR);
308
2/10
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 3 X_subspace.template middleRows<3>(ANGULAR) = m.rotation().transpose();
309
310 3 return X_subspace;
311 }
312
313 template<typename MotionDerived>
314 2 DenseBase motionAction(const MotionDense<MotionDerived> & m) const
315 {
316
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const typename MotionDerived::ConstLinearType v = m.linear();
317
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const typename MotionDerived::ConstAngularType w = m.angular();
318
319
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 DenseBase res;
320
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 skew(v, res.template middleRows<3>(LINEAR));
321
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 skew(w, res.template middleRows<3>(ANGULAR));
322
323 4 return res;
324 }
325
326 17 bool isEqual(const JointMotionSubspaceSphericalTpl &) const
327 {
328 17 return true;
329 }
330
331 }; // struct JointMotionSubspaceSphericalTpl
332
333 template<typename MotionDerived, typename S2, int O2>
334 inline typename MotionDerived::MotionPlain
335 125 operator^(const MotionDense<MotionDerived> & m1, const MotionSphericalTpl<S2, O2> & m2)
336 {
337 125 return m2.motionAction(m1);
338 }
339
340 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
341 template<typename S1, int O1, typename S2, int O2>
342 inline Eigen::Matrix<S2, 6, 3, O2>
343 7 operator*(const InertiaTpl<S1, O1> & Y, const JointMotionSubspaceSphericalTpl<S2, O2> &)
344 {
345 typedef InertiaTpl<S1, O1> Inertia;
346 typedef typename Inertia::Symmetric3 Symmetric3;
347 7 Eigen::Matrix<S2, 6, 3, O2> M;
348 // M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
349
2/12
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 7 M.template block<3, 3>(Inertia::LINEAR, 0) = alphaSkew(-Y.mass(), Y.lever());
350
4/8
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
 7 M.template block<3, 3>(Inertia::ANGULAR, 0) =
351
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 7 (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
352 7 return M;
353 }
354
355 /* [ABA] Y*S operator*/
356 template<typename M6Like, typename S2, int O2>
357 inline typename SizeDepType<3>::ColsReturn<M6Like>::ConstType
358 2 operator*(const Eigen::MatrixBase<M6Like> & Y, const JointMotionSubspaceSphericalTpl<S2, O2> &)
359 {
360 typedef JointMotionSubspaceSphericalTpl<S2, O2> Constraint;
361 2 return Y.derived().template middleCols<3>(Constraint::ANGULAR);
362 }
363
364 template<typename S1, int O1>
365 struct SE3GroupAction<JointMotionSubspaceSphericalTpl<S1, O1>>
366 {
367 typedef Eigen::Matrix<S1, 6, 3, O1> ReturnType;
368 };
369
370 template<typename S1, int O1, typename MotionDerived>
371 struct MotionAlgebraAction<JointMotionSubspaceSphericalTpl<S1, O1>, MotionDerived>
372 {
373 typedef Eigen::Matrix<S1, 6, 3, O1> ReturnType;
374 };
375
376 template<typename Scalar, int Options>
377 struct JointSphericalTpl;
378
379 template<typename _Scalar, int _Options>
380 struct traits<JointSphericalTpl<_Scalar, _Options>>
381 {
382 enum
383 {
384 NQ = 4,
385 NV = 3
386 };
387 typedef _Scalar Scalar;
388 enum
389 {
390 Options = _Options
391 };
392 typedef JointDataSphericalTpl<Scalar, Options> JointDataDerived;
393 typedef JointModelSphericalTpl<Scalar, Options> JointModelDerived;
394 typedef JointMotionSubspaceSphericalTpl<Scalar, Options> Constraint_t;
395 typedef SE3Tpl<Scalar, Options> Transformation_t;
396 typedef MotionSphericalTpl<Scalar, Options> Motion_t;
397 typedef MotionZeroTpl<Scalar, Options> Bias_t;
398
399 // [ABA]
400 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
401 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
402 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
403
404 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
405 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
406
407 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
408 };
409
410 template<typename _Scalar, int _Options>
411 struct traits<JointDataSphericalTpl<_Scalar, _Options>>
412 {
413 typedef JointSphericalTpl<_Scalar, _Options> JointDerived;
414 typedef _Scalar Scalar;
415 };
416
417 template<typename _Scalar, int _Options>
418 struct traits<JointModelSphericalTpl<_Scalar, _Options>>
419 {
420 typedef JointSphericalTpl<_Scalar, _Options> JointDerived;
421 typedef _Scalar Scalar;
422 };
423
424 template<typename _Scalar, int _Options>
425 struct JointDataSphericalTpl : public JointDataBase<JointDataSphericalTpl<_Scalar, _Options>>
426 {
427 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
428
429 typedef JointSphericalTpl<_Scalar, _Options> JointDerived;
430 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
431 2297 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
432
433 ConfigVector_t joint_q;
434 TangentVector_t joint_v;
435
436 Constraint_t S;
437 Transformation_t M;
438 Motion_t v;
439 Bias_t c;
440
441 // [ABA] specific data
442 U_t U;
443 D_t Dinv;
444 UD_t UDinv;
445 D_t StU;
446
447 3197 JointDataSphericalTpl()
448
5/9
✓ Branch 1 taken 3189 times.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
 3197 : joint_q(Scalar(0), Scalar(0), Scalar(0), Scalar(1))
449
3/5
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 3189 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 3197 , joint_v(TangentVector_t::Zero())
450
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 3197 , M(Transformation_t::Identity())
451
3/5
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 3189 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 3197 , v(Motion_t::Vector3::Zero())
452
3/5
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 3189 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 3197 , U(U_t::Zero())
453
3/5
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 3189 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 3197 , Dinv(D_t::Zero())
454
3/5
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 3189 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 3197 , UDinv(UD_t::Zero())
455
3/5
✓ Branch 3 taken 6 times.
✓ Branch 4 taken 3189 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
 9591 , StU(D_t::Zero())
456 {
457 3197 }
458
459 144 static std::string classname()
460 {
461
1/2
✓ Branch 2 taken 144 times.
✗ Branch 3 not taken.
 144 return std::string("JointDataSpherical");
462 }
463 3 std::string shortname() const
464 {
465 3 return classname();
466 }
467
468 }; // struct JointDataSphericalTpl
469
470 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalTpl);
471 template<typename _Scalar, int _Options>
472 struct JointModelSphericalTpl : public JointModelBase<JointModelSphericalTpl<_Scalar, _Options>>
473 {
474 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
475
476 typedef JointSphericalTpl<_Scalar, _Options> JointDerived;
477 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
478
479 typedef JointModelBase<JointModelSphericalTpl> Base;
480 using Base::id;
481 using Base::idx_q;
482 using Base::idx_v;
483 using Base::setIndexes;
484
485 3121 JointDataDerived createData() const
486 {
487 3121 return JointDataDerived();
488 }
489
490 ✗ const std::vector<bool> hasConfigurationLimit() const
491 {
492 ✗ return {false, false, false, false};
493 }
494
495 ✗ const std::vector<bool> hasConfigurationLimitInTangent() const
496 {
497 ✗ return {false, false, false};
498 }
499
500 template<typename ConfigVectorLike>
501 inline void forwardKinematics(
502 Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
503 {
504 typedef typename ConfigVectorLike::Scalar Scalar;
505 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t, ConfigVectorLike);
506 typedef
507 typename Eigen::Quaternion<Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options>
508 Quaternion;
509 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
510
511 ConstQuaternionMap quat(q_joint.derived().data());
512 // assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename
513 // V::Scalar>::epsilon())); TODO: check validity of the rhs precision
514 assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm() - 1)) <= 1e-4);
515
516 M.rotation(quat.matrix());
517 M.translation().setZero();
518 }
519
520 template<typename QuaternionDerived>
521 6337 void calc(
522 JointDataDerived & data, const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
523 {
524 6337 data.joint_q = quat.coeffs();
525
1/2
✓ Branch 2 taken 6329 times.
✗ Branch 3 not taken.
 6337 data.M.rotation(quat.matrix());
526 6337 }
527
528 template<typename ConfigVector>
529 EIGEN_DONT_INLINE void
530 6152 calc(JointDataDerived & data, const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
531 {
532 typedef typename Eigen::Quaternion<typename ConfigVector::Scalar, ConfigVector::Options>
533 Quaternion;
534 typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
535
536
3/6
✓ Branch 1 taken 6145 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6145 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 6145 times.
✗ Branch 9 not taken.
 6152 ConstQuaternionMap quat(qs.template segment<NQ>(idx_q()).data());
537
1/2
✓ Branch 1 taken 6145 times.
✗ Branch 2 not taken.
 6152 calc(data, quat);
538 6152 }
539
540 template<typename ConfigVector>
541 EIGEN_DONT_INLINE void
542 186 calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
543 {
544 typedef typename Eigen::Quaternion<Scalar, Options> Quaternion;
545
546
3/6
✓ Branch 1 taken 184 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 184 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 184 times.
✗ Branch 8 not taken.
 186 const Quaternion quat(qs.template segment<NQ>(idx_q()));
547
1/2
✓ Branch 1 taken 184 times.
✗ Branch 2 not taken.
 186 calc(data, quat);
548 186 }
549
550 template<typename TangentVector>
551 void
552 1 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
553 const
554 {
555
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 data.joint_v = vs.template segment<NV>(idx_v());
556 1 data.v.angular() = data.joint_v;
557 1 }
558
559 template<typename ConfigVector, typename TangentVector>
560 1169 void calc(
561 JointDataDerived & data,
562 const typename Eigen::MatrixBase<ConfigVector> & qs,
563 const typename Eigen::MatrixBase<TangentVector> & vs) const
564 {
565 1169 calc(data, qs.derived());
566
1/2
✓ Branch 3 taken 1165 times.
✗ Branch 4 not taken.
 1169 data.joint_v = vs.template segment<NV>(idx_v());
567 1169 data.v.angular() = data.joint_v;
568 1169 }
569
570 template<typename VectorLike, typename Matrix6Like>
571 8 void calc_aba(
572 JointDataDerived & data,
573 const Eigen::MatrixBase<VectorLike> & armature,
574 const Eigen::MatrixBase<Matrix6Like> & I,
575 const bool update_I) const
576 {
577
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 8 data.U = I.template block<6, 3>(0, Inertia::ANGULAR);
578
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 8 data.StU = data.U.template middleRows<3>(Inertia::ANGULAR);
579
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 8 data.StU.diagonal() += armature;
580
581 8 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
582
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 8 data.UDinv.noalias() = data.U * data.Dinv;
583
584
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 5 times.
 8 if (update_I)
585
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 3 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
586 8 }
587
588 16493 static std::string classname()
589 {
590
1/2
✓ Branch 2 taken 16493 times.
✗ Branch 3 not taken.
 16493 return std::string("JointModelSpherical");
591 }
592 16351 std::string shortname() const
593 {
594 16351 return classname();
595 }
596
597 /// \returns An expression of *this with the Scalar type casted to NewScalar.
598 template<typename NewScalar>
599 7 JointModelSphericalTpl<NewScalar, Options> cast() const
600 {
601 typedef JointModelSphericalTpl<NewScalar, Options> ReturnType;
602 7 ReturnType res;
603 7 res.setIndexes(id(), idx_q(), idx_v());
604 7 return res;
605 }
606
607 }; // struct JointModelSphericalTpl
608
609 } // namespace pinocchio
610
611 #include <boost/type_traits.hpp>
612
613 namespace boost
614 {
615 template<typename Scalar, int Options>
616 struct has_nothrow_constructor<::pinocchio::JointModelSphericalTpl<Scalar, Options>>
617 : public integral_constant<bool, true>
618 {
619 };
620
621 template<typename Scalar, int Options>
622 struct has_nothrow_copy<::pinocchio::JointModelSphericalTpl<Scalar, Options>>
623 : public integral_constant<bool, true>
624 {
625 };
626
627 template<typename Scalar, int Options>
628 struct has_nothrow_constructor<::pinocchio::JointDataSphericalTpl<Scalar, Options>>
629 : public integral_constant<bool, true>
630 {
631 };
632
633 template<typename Scalar, int Options>
634 struct has_nothrow_copy<::pinocchio::JointDataSphericalTpl<Scalar, Options>>
635 : public integral_constant<bool, true>
636 {
637 };
638 } // namespace boost
639
640 #endif // ifndef __pinocchio_multibody_joint_spherical_hpp__
641