GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-universal.hpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 169 180 93.9%
Branches: 357 851 42.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2023 INRIA
3 //
4
5 #ifndef __pinocchio_multibody_joint_universal_hpp__
6 #define __pinocchio_multibody_joint_universal_hpp__
7
8 #include "pinocchio/fwd.hpp"
9 #include "pinocchio/multibody/joint/joint-base.hpp"
10 #include "pinocchio/multibody/joint/joint-spherical.hpp"
11 #include "pinocchio/multibody/joint-motion-subspace.hpp"
12 #include "pinocchio/spatial/inertia.hpp"
13 #include "pinocchio/utils/check.hpp"
14
15 #include "pinocchio/math/matrix.hpp"
16 #include "pinocchio/math/rotation.hpp"
17
18 namespace pinocchio
19 {
20 template<typename Scalar, int Options>
21 struct JointMotionSubspaceUniversalTpl;
22
23 template<typename _Scalar, int _Options>
24 struct traits<JointMotionSubspaceUniversalTpl<_Scalar, _Options>>
25 {
26 typedef _Scalar Scalar;
27 enum
28 {
29 Options = _Options
30 };
31 enum
32 {
33 LINEAR = 0,
34 ANGULAR = 3
35 };
36
37 typedef MotionSphericalTpl<Scalar, Options> JointMotion;
38 typedef Eigen::Matrix<Scalar, 2, 1, Options> JointForce;
39 typedef Eigen::Matrix<Scalar, 6, 2, Options> DenseBase;
40 typedef Eigen::Matrix<Scalar, 3, 3, Options> ReducedSquaredMatrix;
41
42 typedef DenseBase MatrixReturnType;
43 typedef const DenseBase ConstMatrixReturnType;
44
45 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
46 typedef Eigen::Matrix<Scalar, 3, 2, Options> Matrix32;
47 typedef Eigen::Matrix<Scalar, 2, 2, Options> Matrix2;
48
49 typedef Matrix2 StDiagonalMatrixSOperationReturnType;
50 }; // traits JointMotionSubspaceUniversalTpl
51
52 template<typename Scalar, int Options>
53 struct SE3GroupAction<JointMotionSubspaceUniversalTpl<Scalar, Options>>
54 {
55 typedef Eigen::Matrix<Scalar, 6, 2, Options> ReturnType;
56 };
57
58 template<typename Scalar, int Options, typename MotionDerived>
59 struct MotionAlgebraAction<JointMotionSubspaceUniversalTpl<Scalar, Options>, MotionDerived>
60 {
61 typedef Eigen::Matrix<Scalar, 6, 2, Options> ReturnType;
62 };
63
64 template<typename Scalar, int Options, typename ForceDerived>
65 struct ConstraintForceOp<JointMotionSubspaceUniversalTpl<Scalar, Options>, ForceDerived>
66 {
67 typedef typename traits<JointMotionSubspaceUniversalTpl<Scalar, Options>>::Vector3 Vector3;
68 typedef Eigen::Matrix<
69 typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(
70 Vector3, typename ForceDense<ForceDerived>::ConstAngularType),
71 2,
72 1,
73 Options>
74 ReturnType;
75 };
76
77 template<typename Scalar, int Options, typename ForceSet>
78 struct ConstraintForceSetOp<JointMotionSubspaceUniversalTpl<Scalar, Options>, ForceSet>
79 {
80 typedef typename traits<JointMotionSubspaceUniversalTpl<Scalar, Options>>::Matrix32 Matrix32;
81 typedef typename MatrixMatrixProduct<
82 Eigen::Transpose<const Matrix32>,
83 typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type>::type ReturnType;
84 };
85
86 template<typename _Scalar, int _Options>
87 struct JointMotionSubspaceUniversalTpl
88 : JointMotionSubspaceBase<JointMotionSubspaceUniversalTpl<_Scalar, _Options>>
89 {
90 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
91 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceUniversalTpl)
92
93 enum
94 {
95 NV = 2
96 };
97
98 typedef typename traits<JointMotionSubspaceUniversalTpl>::Vector3 Vector3;
99 typedef typename traits<JointMotionSubspaceUniversalTpl>::Matrix32 Matrix32;
100
101 7 JointMotionSubspaceUniversalTpl()
102 7 {
103 7 }
104
105 template<typename Matrix32Like>
106 96 JointMotionSubspaceUniversalTpl(const Eigen::MatrixBase<Matrix32Like> & subspace)
107 96 : m_S(subspace)
108 {
109 96 }
110
111 template<typename Vector3Like>
112 3 JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const
113 {
114 // EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
115
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 return JointMotion(m_S * v);
116 }
117
118 template<typename S1, int O1>
119 typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType
120 11 se3Action(const SE3Tpl<S1, O1> & m) const
121 {
122 typedef typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType ReturnType;
123
124 11 ReturnType res;
125
3/13
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 11 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 11 res.template middleRows<3>(ANGULAR).noalias() = m.rotation() * m_S;
126 11 cross(
127
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
 11 m.translation(), res.template middleRows<3>(Motion::ANGULAR),
128
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 11 res.template middleRows<3>(LINEAR));
129 11 return res;
130 }
131
132 template<typename S1, int O1>
133 typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType
134 3 se3ActionInverse(const SE3Tpl<S1, O1> & m) const
135 {
136 typedef typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType ReturnType;
137
138 3 ReturnType res;
139
2/8
✗ 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 not taken.
 3 cross(m.translation(), m_S, res.template middleRows<3>(ANGULAR));
140
5/10
✓ 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.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
 3 res.template middleRows<3>(LINEAR).noalias() =
141
2/8
✗ 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 not taken.
 3 -m.rotation().transpose() * res.template middleRows<3>(ANGULAR);
142
143 // ANGULAR
144
4/16
✗ 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 9 taken 3 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 3 res.template middleRows<3>(ANGULAR).noalias() = m.rotation().transpose() * m_S;
145 3 return res;
146 }
147
148 21 int nv_impl() const
149 {
150 21 return NV;
151 }
152
153 struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceUniversalTpl>
154 {
155 const JointMotionSubspaceUniversalTpl & ref;
156 7 TransposeConst(const JointMotionSubspaceUniversalTpl & ref)
157 7 : ref(ref)
158 {
159 7 }
160
161 template<typename ForceDerived>
162 typename ConstraintForceOp<JointMotionSubspaceUniversalTpl, ForceDerived>::ReturnType
163 5 operator*(const ForceDense<ForceDerived> & f) const
164 {
165
3/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
 10 return ref.m_S.transpose() * f.angular();
166 }
167
168 /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
169 template<typename ForceSet>
170 typename ConstraintForceSetOp<JointMotionSubspaceUniversalTpl, ForceSet>::ReturnType
171 1 operator*(const Eigen::MatrixBase<ForceSet> & F)
172 {
173 EIGEN_STATIC_ASSERT(
174 ForceSet::RowsAtCompileTime == 6, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
175 /* Return ax.T * F[3:end,:] */
176
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 return ref.m_S.transpose() * F.template middleRows<3>(ANGULAR);
177 }
178 };
179
180 7 TransposeConst transpose() const
181 {
182 7 return TransposeConst(*this);
183 }
184
185 /* CRBA joint operators
186 * - ForceSet::Block = ForceSet
187 * - ForceSet operator* (Inertia Y,Constraint S)
188 * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
189 * - SE3::act(ForceSet::Block)
190 */
191 11 DenseBase matrix_impl() const
192 {
193 11 DenseBase S;
194
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 11 S.template middleRows<3>(LINEAR).setZero();
195
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 11 S.template middleRows<3>(ANGULAR) = m_S;
196 11 return S;
197 }
198
199 template<typename MotionDerived>
200 typename MotionAlgebraAction<JointMotionSubspaceUniversalTpl, MotionDerived>::ReturnType
201 2 motionAction(const MotionDense<MotionDerived> & m) const
202 {
203
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const typename MotionDerived::ConstLinearType v = m.linear();
204
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const typename MotionDerived::ConstAngularType w = m.angular();
205
206
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 DenseBase res;
207
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 cross(v, m_S, res.template middleRows<3>(LINEAR));
208
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 cross(w, m_S, res.template middleRows<3>(ANGULAR));
209 4 return res;
210 }
211
212 4 const Matrix32 & angularSubspace() const
213 {
214 4 return m_S;
215 }
216 94 Matrix32 & angularSubspace()
217 {
218 94 return m_S;
219 }
220
221 17 bool isEqual(const JointMotionSubspaceUniversalTpl & other) const
222 {
223 17 return internal::comparison_eq(m_S, other.m_S);
224 }
225
226 protected:
227 Matrix32 m_S;
228
229 }; // struct JointMotionSubspaceUniversalTpl
230
231 namespace details
232 {
233 template<typename Scalar, int Options>
234 struct StDiagonalMatrixSOperation<JointMotionSubspaceUniversalTpl<Scalar, Options>>
235 {
236 typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint;
237 typedef typename traits<Constraint>::StDiagonalMatrixSOperationReturnType ReturnType;
238
239 1 static ReturnType run(const JointMotionSubspaceBase<Constraint> & constraint)
240 {
241
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 return constraint.matrix().transpose() * constraint.matrix();
242 }
243 };
244 } // namespace details
245
246 template<typename S1, int O1, typename S2, int O2>
247 struct MultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceUniversalTpl<S2, O2>>
248 {
249 typedef Eigen::Matrix<S2, 6, 2, O2> ReturnType;
250 };
251
252 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
253 namespace impl
254 {
255 template<typename S1, int O1, typename S2, int O2>
256 struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceUniversalTpl<S2, O2>>
257 {
258 typedef InertiaTpl<S1, O1> Inertia;
259 typedef JointMotionSubspaceUniversalTpl<S2, O2> Constraint;
260 typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType;
261 2 static inline ReturnType run(const Inertia & Y, const Constraint & cru)
262 {
263 typedef typename InertiaTpl<S1, O1>::Symmetric3 Symmetric3;
264
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Eigen::Matrix<S1, 6, 3, O1> M;
265
2/9
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 2 alphaSkew(-Y.mass(), Y.lever(), M.template middleRows<3>(Constraint::LINEAR));
266
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 M.template middleRows<3>(Constraint::ANGULAR) =
267
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 2 (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
268
269
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 4 return (M * cru.angularSubspace()).eval();
270 }
271 };
272 } // namespace impl
273
274 template<typename M6Like, typename Scalar, int Options>
275 struct MultiplicationOp<
276 Eigen::MatrixBase<M6Like>,
277 JointMotionSubspaceUniversalTpl<Scalar, Options>>
278 {
279 typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
280 typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
281
282 typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint;
283 typedef typename Constraint::Matrix32 Matrix32;
284 typedef const typename MatrixMatrixProduct<M6LikeColsNonConst, Matrix32>::type ReturnType;
285 };
286
287 /* [ABA] operator* (Inertia Y,Constraint S) */
288 namespace impl
289 {
290 template<typename M6Like, typename Scalar, int Options>
291 struct LhsMultiplicationOp<
292 Eigen::MatrixBase<M6Like>,
293 JointMotionSubspaceUniversalTpl<Scalar, Options>>
294 {
295 typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint;
296 typedef
297 typename MultiplicationOp<Eigen::MatrixBase<M6Like>, Constraint>::ReturnType ReturnType;
298
299 2 static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & cru)
300 {
301 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
302
1/2
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.angularSubspace();
303 }
304 };
305 } // namespace impl
306
307 template<typename Scalar, int Options>
308 struct JointUniversalTpl;
309
310 template<typename _Scalar, int _Options>
311 struct traits<JointUniversalTpl<_Scalar, _Options>>
312 {
313 enum
314 {
315 NQ = 2,
316 NV = 2,
317 NVExtended = 2
318 };
319 typedef _Scalar Scalar;
320 enum
321 {
322 Options = _Options
323 };
324 typedef JointDataUniversalTpl<Scalar, Options> JointDataDerived;
325 typedef JointModelUniversalTpl<Scalar, Options> JointModelDerived;
326 typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint_t;
327 typedef SE3Tpl<Scalar, Options> Transformation_t;
328 typedef MotionSphericalTpl<Scalar, Options> Motion_t;
329 typedef MotionSphericalTpl<Scalar, Options> Bias_t;
330
331 // [ABA]
332 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
333 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
334 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
335
336 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
337 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
338
339 typedef boost::mpl::false_ is_mimicable_t;
340
341 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
342 };
343
344 template<typename _Scalar, int _Options>
345 struct traits<JointDataUniversalTpl<_Scalar, _Options>>
346 {
347 typedef JointUniversalTpl<_Scalar, _Options> JointDerived;
348 typedef _Scalar Scalar;
349 };
350
351 template<typename _Scalar, int _Options>
352 struct traits<JointModelUniversalTpl<_Scalar, _Options>>
353 {
354 typedef JointUniversalTpl<_Scalar, _Options> JointDerived;
355 typedef _Scalar Scalar;
356 };
357
358 template<typename _Scalar, int _Options>
359 struct JointDataUniversalTpl : public JointDataBase<JointDataUniversalTpl<_Scalar, _Options>>
360 {
361 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
362 typedef JointUniversalTpl<_Scalar, _Options> JointDerived;
363 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
364 910 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
365
366 ConfigVector_t joint_q;
367 TangentVector_t joint_v;
368
369 Transformation_t M;
370 Constraint_t S;
371 Motion_t v;
372 Bias_t c;
373
374 // [ABA] specific data
375 U_t U;
376 D_t Dinv;
377 UD_t UDinv;
378 D_t StU;
379
380 96 JointDataUniversalTpl()
381
1/2
✓ Branch 2 taken 94 times.
✗ Branch 3 not taken.
 96 : joint_q(ConfigVector_t::Zero())
382
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 96 , joint_v(TangentVector_t::Zero())
383
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 96 , M(Transformation_t::Identity())
384
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 96 , S(Constraint_t::Matrix32::Zero())
385
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 96 , v(Motion_t::Vector3::Zero())
386
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 96 , c(Bias_t::Vector3::Zero())
387
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 96 , U(U_t::Zero())
388
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 96 , Dinv(D_t::Zero())
389
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 96 , UDinv(UD_t::Zero())
390
3/5
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 89 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 192 , StU(D_t::Zero())
391 {
392 96 }
393
394 150 static std::string classname()
395 {
396
1/2
✓ Branch 2 taken 150 times.
✗ Branch 3 not taken.
 150 return std::string("JointDataUniversal");
397 }
398 3 std::string shortname() const
399 {
400 3 return classname();
401 }
402
403 }; // struct JointDataUniversalTpl
404
405 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelUniversalTpl);
406 template<typename _Scalar, int _Options>
407 struct JointModelUniversalTpl : public JointModelBase<JointModelUniversalTpl<_Scalar, _Options>>
408 {
409 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
410 typedef JointUniversalTpl<_Scalar, _Options> JointDerived;
411 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
412 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
413 typedef Eigen::Matrix<Scalar, 3, 3, _Options> Matrix3;
414
415 typedef JointModelBase<JointModelUniversalTpl> Base;
416 using Base::id;
417 using Base::idx_q;
418 using Base::idx_v;
419 using Base::idx_vExtended;
420 using Base::setIndexes;
421
422 97 JointModelUniversalTpl()
423
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 97 {
424 97 }
425
426 ✗ JointModelUniversalTpl(
427 const Scalar & x1,
428 const Scalar & y1,
429 const Scalar & z1,
430 const Scalar & x2,
431 const Scalar & y2,
432 const Scalar & z2)
433 ✗ : axis1(x1, y1, z1)
434 ✗ , axis2(x2, y2, z2)
435 {
436 ✗ assert(isUnitary(axis1) && "First Rotation axis is not unitary");
437 ✗ assert(isUnitary(axis2) && "Second Rotation axis is not unitary");
438 ✗ assert(check_expression_if_real<Scalar>(axis1.dot(axis2) == 0) && "Axii are not orthogonal");
439 }
440
441 template<typename Vector3Like>
442 32 JointModelUniversalTpl(
443 const Eigen::MatrixBase<Vector3Like> & axis1_, const Eigen::MatrixBase<Vector3Like> & axis2_)
444 32 : axis1(axis1_)
445
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 64 , axis2(axis2_)
446 {
447 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
448
5/11
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 32 assert(isUnitary(axis1) && "First Rotation axis is not unitary");
449
5/11
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 32 assert(isUnitary(axis2) && "Second Rotation axis is not unitary");
450
9/29
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 19 times.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
 32 assert(
451 check_expression_if_real<Scalar>(axis1.dot(axis2) == Scalar(0))
452 && "Axii are not orthogonal");
453 32 }
454
455 17 JointDataDerived createData() const
456 {
457 17 return JointDataDerived();
458 }
459
460 ✗ const std::vector<bool> hasConfigurationLimit() const
461 {
462 ✗ return {true, true};
463 }
464
465 ✗ const std::vector<bool> hasConfigurationLimitInTangent() const
466 {
467 ✗ return {true, true};
468 }
469
470 using Base::isEqual;
471 15 bool isEqual(const JointModelUniversalTpl & other) const
472 {
473
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 28 return Base::isEqual(other) && internal::comparison_eq(axis1, other.axis1)
474
3/4
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 2 times.
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 28 && internal::comparison_eq(axis2, other.axis2);
475 }
476
477 template<typename ConfigVector>
478 22 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
479 {
480
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 22 data.joint_q = qs.template segment<NQ>(idx_q());
481
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c0, s0;
482
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 22 SINCOS(data.joint_q(0), &s0, &c0);
483
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c1, s1;
484
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 22 SINCOS(data.joint_q(1), &s1, &c1);
485
486
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 22 Matrix3 rot1, rot2;
487
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 22 toRotationMatrix(axis1, c0, s0, rot1);
488
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 22 toRotationMatrix(axis2, c1, s1, rot2);
489
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 22 data.M.rotation() = rot1 * rot2;
490
491
4/9
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
 26 data.S.angularSubspace() << rot2.coeffRef(0, 0) * axis1.x() + rot2.coeffRef(1, 0) * axis1.y()
492
8/22
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
 28 + rot2.coeffRef(2, 0) * axis1.z(),
493
5/16
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
 26 axis2.x(),
494
6/13
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 22 rot2.coeffRef(0, 1) * axis1.x() + rot2.coeffRef(1, 1) * axis1.y()
495
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 22 + rot2.coeffRef(2, 1) * axis1.z(),
496
5/16
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
 26 axis2.y(),
497
6/13
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 22 rot2.coeffRef(0, 2) * axis1.x() + rot2.coeffRef(1, 2) * axis1.y()
498
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 42 + rot2.coeffRef(2, 2) * axis1.z(),
499
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 22 axis2.z();
500 22 }
501
502 template<typename TangentVector>
503 void
504 1 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
505 const
506 {
507
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 data.joint_v = vs.template segment<NV>(idx_v());
508
2/4
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 data.v().noalias() = data.S.angularSubspace() * data.joint_v;
509
510 // TODO: need to be done
511 // #define q_dot data.joint_v
512 // Scalar tmp;
513 // tmp = (-s1+axis2.x()*axis2.x()*s1)*axis1.x() +
514 // (axis2.x()*axis2.y()*s1+axis2.z()*c1)*axis1.y() +
515 // (axis2.x()*axis2.z()*s1-axis2.y()*c1)*axis1.z(); data.c()(0) = tmp *
516 // q_dot(1)*q_dot(0); tmp = (axis2.x()*axis2.y()*s1-axis2.z()*c1)*axis1.x() +
517 // (-s1+axis2.y()*axis2.y()*s1)*axis1.y() +
518 // (axis2.y()*axis2.z()*s1+axis2.x()*c1)*axis1.z(); data.c()(1) = tmp *
519 // q_dot(1)*q_dot(0); tmp = (axis2.z()*axis2.x()*s1+axis2.y()*c1)*axis1.x() +
520 // (axis2.y()*axis2.z()*s1-axis2.x()*c1)*axis1.y() +
521 // (-s1+axis2.z()*axis2.z()*s1)*axis1.z(); data.c()(2) = tmp * q_dot(1)*q_dot(0);
522 // #undef q_dot
523 1 }
524
525 template<typename ConfigVector, typename TangentVector>
526 26 void calc(
527 JointDataDerived & data,
528 const typename Eigen::MatrixBase<ConfigVector> & qs,
529 const typename Eigen::MatrixBase<TangentVector> & vs) const
530 {
531
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 26 data.joint_q = qs.template segment<NQ>(idx_q());
532
533
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c0, s0;
534
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 26 SINCOS(data.joint_q(0), &s0, &c0);
535
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c1, s1;
536
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 26 SINCOS(data.joint_q(1), &s1, &c1);
537
538
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 26 Matrix3 rot1, rot2;
539
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 26 toRotationMatrix(axis1, c0, s0, rot1);
540
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 26 toRotationMatrix(axis2, c1, s1, rot2);
541
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 26 data.M.rotation() = rot1 * rot2;
542
543
4/9
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
 30 data.S.angularSubspace() << rot2.coeffRef(0, 0) * axis1.x() + rot2.coeffRef(1, 0) * axis1.y()
544
8/22
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
 32 + rot2.coeffRef(2, 0) * axis1.z(),
545
5/16
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
 30 axis2.x(),
546
6/13
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 26 rot2.coeffRef(0, 1) * axis1.x() + rot2.coeffRef(1, 1) * axis1.y()
547
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 26 + rot2.coeffRef(2, 1) * axis1.z(),
548
5/16
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
 30 axis2.y(),
549
6/13
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 26 rot2.coeffRef(0, 2) * axis1.x() + rot2.coeffRef(1, 2) * axis1.y()
550
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 50 + rot2.coeffRef(2, 2) * axis1.z(),
551
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 26 axis2.z();
552
553
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 26 data.joint_v = vs.template segment<NV>(idx_v());
554
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
 26 data.v().noalias() = data.S.angularSubspace() * data.joint_v;
555
556 #define q_dot data.joint_v
557
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Scalar tmp;
558
17/34
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 1 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 1 times.
✗ Branch 50 not taken.
 28 tmp = (-s1 + axis2.x() * axis2.x() * s1) * axis1.x()
559
6/12
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 26 + (axis2.x() * axis2.y() * s1 + axis2.z() * c1) * axis1.y()
560
6/12
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 26 + (axis2.x() * axis2.z() * s1 - axis2.y() * c1) * axis1.z();
561
7/13
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✓ Branch 8 taken 17 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 26 data.c()(0) = tmp * q_dot(1) * q_dot(0);
562
19/38
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 1 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 1 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
 28 tmp = (axis2.x() * axis2.y() * s1 - axis2.z() * c1) * axis1.x()
563
4/8
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 26 + (-s1 + axis2.y() * axis2.y() * s1) * axis1.y()
564
6/12
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 26 + (axis2.y() * axis2.z() * s1 + axis2.x() * c1) * axis1.z();
565
7/13
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✓ Branch 8 taken 17 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 26 data.c()(1) = tmp * q_dot(1) * q_dot(0);
566
19/38
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 1 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 1 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
 28 tmp = (axis2.z() * axis2.x() * s1 + axis2.y() * c1) * axis1.x()
567
6/12
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 26 + (axis2.y() * axis2.z() * s1 - axis2.x() * c1) * axis1.y()
568
4/8
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 26 + (-s1 + axis2.z() * axis2.z() * s1) * axis1.z();
569
7/13
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✓ Branch 8 taken 17 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 26 data.c()(2) = tmp * q_dot(1) * q_dot(0);
570 #undef q_dot
571 26 }
572
573 template<typename VectorLike, typename Matrix6Like>
574 5 void calc_aba(
575 JointDataDerived & data,
576 const Eigen::MatrixBase<VectorLike> & armature,
577 const Eigen::MatrixBase<Matrix6Like> & I,
578 const bool update_I) const
579 {
580
3/6
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
 5 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.angularSubspace();
581
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 data.StU.noalias() =
582
2/4
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
 5 data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR);
583
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 data.StU.diagonal() += armature;
584 5 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
585
586
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 data.UDinv.noalias() = data.U * data.Dinv;
587
588
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 if (update_I)
589 ✗ PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
590 5 }
591
592 172 static std::string classname()
593 {
594
1/2
✓ Branch 2 taken 172 times.
✗ Branch 3 not taken.
 172 return std::string("JointModelUniversal");
595 }
596 24 std::string shortname() const
597 {
598 24 return classname();
599 }
600
601 /// \returns An expression of *this with the Scalar type casted to NewScalar.
602 template<typename NewScalar>
603 5 JointModelUniversalTpl<NewScalar, Options> cast() const
604 {
605 typedef JointModelUniversalTpl<NewScalar, Options> ReturnType;
606
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 5 ReturnType res(axis1.template cast<NewScalar>(), axis2.template cast<NewScalar>());
607
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 5 res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
608 5 return res;
609 }
610
611 /// \brief 3d main axii of the joint.
612 ///
613 Vector3 axis1;
614 Vector3 axis2;
615 }; // struct JointModelUniversalTpl
616
617 } // namespace pinocchio
618
619 #include <boost/type_traits.hpp>
620
621 namespace boost
622 {
623 template<typename Scalar, int Options>
624 struct has_nothrow_constructor<::pinocchio::JointModelUniversalTpl<Scalar, Options>>
625 : public integral_constant<bool, true>
626 {
627 };
628
629 template<typename Scalar, int Options>
630 struct has_nothrow_copy<::pinocchio::JointModelUniversalTpl<Scalar, Options>>
631 : public integral_constant<bool, true>
632 {
633 };
634
635 template<typename Scalar, int Options>
636 struct has_nothrow_constructor<::pinocchio::JointDataUniversalTpl<Scalar, Options>>
637 : public integral_constant<bool, true>
638 {
639 };
640
641 template<typename Scalar, int Options>
642 struct has_nothrow_copy<::pinocchio::JointDataUniversalTpl<Scalar, Options>>
643 : public integral_constant<bool, true>
644 {
645 };
646 } // namespace boost
647
648 #endif // ifndef __pinocchio_multibody_joint_universal_hpp__
649