GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-universal.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 169 180 93.9%
Branches: 356 865 41.2%
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 93 JointMotionSubspaceUniversalTpl(const Eigen::MatrixBase<Matrix32Like> & subspace)
107 93 : m_S(subspace)
108 {
109 93 }
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 };
318 typedef _Scalar Scalar;
319 enum
320 {
321 Options = _Options
322 };
323 typedef JointDataUniversalTpl<Scalar, Options> JointDataDerived;
324 typedef JointModelUniversalTpl<Scalar, Options> JointModelDerived;
325 typedef JointMotionSubspaceUniversalTpl<Scalar, Options> Constraint_t;
326 typedef SE3Tpl<Scalar, Options> Transformation_t;
327 typedef MotionSphericalTpl<Scalar, Options> Motion_t;
328 typedef MotionSphericalTpl<Scalar, Options> Bias_t;
329
330 // [ABA]
331 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
332 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
333 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
334
335 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
336 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
337
338 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
339 };
340
341 template<typename _Scalar, int _Options>
342 struct traits<JointDataUniversalTpl<_Scalar, _Options>>
343 {
344 typedef JointUniversalTpl<_Scalar, _Options> JointDerived;
345 typedef _Scalar Scalar;
346 };
347
348 template<typename _Scalar, int _Options>
349 struct traits<JointModelUniversalTpl<_Scalar, _Options>>
350 {
351 typedef JointUniversalTpl<_Scalar, _Options> JointDerived;
352 typedef _Scalar Scalar;
353 };
354
355 template<typename _Scalar, int _Options>
356 struct JointDataUniversalTpl : public JointDataBase<JointDataUniversalTpl<_Scalar, _Options>>
357 {
358 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
359 typedef JointUniversalTpl<_Scalar, _Options> JointDerived;
360 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
361 910 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
362
363 ConfigVector_t joint_q;
364 TangentVector_t joint_v;
365
366 Transformation_t M;
367 Constraint_t S;
368 Motion_t v;
369 Bias_t c;
370
371 // [ABA] specific data
372 U_t U;
373 D_t Dinv;
374 UD_t UDinv;
375 D_t StU;
376
377 93 JointDataUniversalTpl()
378
1/2
✓ Branch 2 taken 91 times.
✗ Branch 3 not taken.
 93 : joint_q(ConfigVector_t::Zero())
379
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 93 , joint_v(TangentVector_t::Zero())
380
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 93 , M(Transformation_t::Identity())
381
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 93 , S(Constraint_t::Matrix32::Zero())
382
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 93 , v(Motion_t::Vector3::Zero())
383
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 93 , c(Bias_t::Vector3::Zero())
384
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 93 , U(U_t::Zero())
385
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 93 , Dinv(D_t::Zero())
386
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 93 , UDinv(UD_t::Zero())
387
3/5
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 86 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 186 , StU(D_t::Zero())
388 {
389 93 }
390
391 144 static std::string classname()
392 {
393
1/2
✓ Branch 2 taken 144 times.
✗ Branch 3 not taken.
 144 return std::string("JointDataUniversal");
394 }
395 3 std::string shortname() const
396 {
397 3 return classname();
398 }
399
400 }; // struct JointDataUniversalTpl
401
402 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelUniversalTpl);
403 template<typename _Scalar, int _Options>
404 struct JointModelUniversalTpl : public JointModelBase<JointModelUniversalTpl<_Scalar, _Options>>
405 {
406 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
407 typedef JointUniversalTpl<_Scalar, _Options> JointDerived;
408 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
409 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
410 typedef Eigen::Matrix<Scalar, 3, 3, _Options> Matrix3;
411
412 typedef JointModelBase<JointModelUniversalTpl> Base;
413 using Base::id;
414 using Base::idx_q;
415 using Base::idx_v;
416 using Base::setIndexes;
417
418 94 JointModelUniversalTpl()
419
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 94 {
420 94 }
421
422 ✗ JointModelUniversalTpl(
423 const Scalar & x1,
424 const Scalar & y1,
425 const Scalar & z1,
426 const Scalar & x2,
427 const Scalar & y2,
428 const Scalar & z2)
429 ✗ : axis1(x1, y1, z1)
430 ✗ , axis2(x2, y2, z2)
431 {
432 ✗ assert(isUnitary(axis1) && "First Rotation axis is not unitary");
433 ✗ assert(isUnitary(axis2) && "Second Rotation axis is not unitary");
434 ✗ assert(check_expression_if_real<Scalar>(axis1.dot(axis2) == 0) && "Axii are not orthogonal");
435 }
436
437 template<typename Vector3Like>
438 32 JointModelUniversalTpl(
439 const Eigen::MatrixBase<Vector3Like> & axis1_, const Eigen::MatrixBase<Vector3Like> & axis2_)
440 32 : axis1(axis1_)
441
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 64 , axis2(axis2_)
442 {
443 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
444
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");
445
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");
446
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(
447 check_expression_if_real<Scalar>(axis1.dot(axis2) == Scalar(0))
448 && "Axii are not orthogonal");
449 32 }
450
451 17 JointDataDerived createData() const
452 {
453 17 return JointDataDerived();
454 }
455
456 ✗ const std::vector<bool> hasConfigurationLimit() const
457 {
458 ✗ return {true, true};
459 }
460
461 ✗ const std::vector<bool> hasConfigurationLimitInTangent() const
462 {
463 ✗ return {true, true};
464 }
465
466 using Base::isEqual;
467 15 bool isEqual(const JointModelUniversalTpl & other) const
468 {
469
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 28 return Base::isEqual(other) && internal::comparison_eq(axis1, other.axis1)
470
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);
471 }
472
473 template<typename ConfigVector>
474 16 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
475 {
476
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.
 16 data.joint_q = qs.template segment<NQ>(idx_q());
477
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c0, s0;
478
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 16 SINCOS(data.joint_q(0), &s0, &c0);
479
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c1, s1;
480
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 16 SINCOS(data.joint_q(1), &s1, &c1);
481
482
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 16 Matrix3 rot1, rot2;
483
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 16 toRotationMatrix(axis1, c0, s0, rot1);
484
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 16 toRotationMatrix(axis2, c1, s1, rot2);
485
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.
 16 data.M.rotation() = rot1 * rot2;
486
487
4/10
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✗ Branch 4 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.
 20 data.S.angularSubspace() << rot2.coeffRef(0, 0) * axis1.x() + rot2.coeffRef(1, 0) * axis1.y()
488
8/23
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✗ Branch 3 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.
 22 + rot2.coeffRef(2, 0) * axis1.z(),
489
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.
 20 axis2.x(),
490
6/14
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✗ Branch 3 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.
 16 rot2.coeffRef(0, 1) * axis1.x() + rot2.coeffRef(1, 1) * axis1.y()
491
3/8
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 16 + rot2.coeffRef(2, 1) * axis1.z(),
492
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.
 20 axis2.y(),
493
6/14
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✗ Branch 3 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.
 16 rot2.coeffRef(0, 2) * axis1.x() + rot2.coeffRef(1, 2) * axis1.y()
494
3/8
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 30 + rot2.coeffRef(2, 2) * axis1.z(),
495
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 16 axis2.z();
496 16 }
497
498 template<typename TangentVector>
499 void
500 1 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
501 const
502 {
503
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 data.joint_v = vs.template segment<NV>(idx_v());
504
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;
505
506 // TODO: need to be done
507 // #define q_dot data.joint_v
508 // Scalar tmp;
509 // tmp = (-s1+axis2.x()*axis2.x()*s1)*axis1.x() +
510 // (axis2.x()*axis2.y()*s1+axis2.z()*c1)*axis1.y() +
511 // (axis2.x()*axis2.z()*s1-axis2.y()*c1)*axis1.z(); data.c()(0) = tmp *
512 // q_dot(1)*q_dot(0); tmp = (axis2.x()*axis2.y()*s1-axis2.z()*c1)*axis1.x() +
513 // (-s1+axis2.y()*axis2.y()*s1)*axis1.y() +
514 // (axis2.y()*axis2.z()*s1+axis2.x()*c1)*axis1.z(); data.c()(1) = tmp *
515 // q_dot(1)*q_dot(0); tmp = (axis2.z()*axis2.x()*s1+axis2.y()*c1)*axis1.x() +
516 // (axis2.y()*axis2.z()*s1-axis2.x()*c1)*axis1.y() +
517 // (-s1+axis2.z()*axis2.z()*s1)*axis1.z(); data.c()(2) = tmp * q_dot(1)*q_dot(0);
518 // #undef q_dot
519 1 }
520
521 template<typename ConfigVector, typename TangentVector>
522 20 void calc(
523 JointDataDerived & data,
524 const typename Eigen::MatrixBase<ConfigVector> & qs,
525 const typename Eigen::MatrixBase<TangentVector> & vs) const
526 {
527
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.
 20 data.joint_q = qs.template segment<NQ>(idx_q());
528
529
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c0, s0;
530
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 20 SINCOS(data.joint_q(0), &s0, &c0);
531
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c1, s1;
532
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 20 SINCOS(data.joint_q(1), &s1, &c1);
533
534
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 20 Matrix3 rot1, rot2;
535
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 20 toRotationMatrix(axis1, c0, s0, rot1);
536
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 20 toRotationMatrix(axis2, c1, s1, rot2);
537
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.
 20 data.M.rotation() = rot1 * rot2;
538
539
4/10
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✗ Branch 4 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.
 24 data.S.angularSubspace() << rot2.coeffRef(0, 0) * axis1.x() + rot2.coeffRef(1, 0) * axis1.y()
540
8/23
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✗ Branch 3 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.
 26 + rot2.coeffRef(2, 0) * axis1.z(),
541
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.
 24 axis2.x(),
542
6/14
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✗ Branch 3 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.
 20 rot2.coeffRef(0, 1) * axis1.x() + rot2.coeffRef(1, 1) * axis1.y()
543
3/8
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 20 + rot2.coeffRef(2, 1) * axis1.z(),
544
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.
 24 axis2.y(),
545
6/14
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✗ Branch 3 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.
 20 rot2.coeffRef(0, 2) * axis1.x() + rot2.coeffRef(1, 2) * axis1.y()
546
3/8
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 38 + rot2.coeffRef(2, 2) * axis1.z(),
547
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 20 axis2.z();
548
549
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.
 20 data.joint_v = vs.template segment<NV>(idx_v());
550
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.
 20 data.v().noalias() = data.S.angularSubspace() * data.joint_v;
551
552 #define q_dot data.joint_v
553
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Scalar tmp;
554
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.
 22 tmp = (-s1 + axis2.x() * axis2.x() * s1) * axis1.x()
555
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.
 20 + (axis2.x() * axis2.y() * s1 + axis2.z() * c1) * axis1.y()
556
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.
 20 + (axis2.x() * axis2.z() * s1 - axis2.y() * c1) * axis1.z();
557
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.
 20 data.c()(0) = tmp * q_dot(1) * q_dot(0);
558
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.
 22 tmp = (axis2.x() * axis2.y() * s1 - axis2.z() * c1) * axis1.x()
559
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.
 20 + (-s1 + axis2.y() * axis2.y() * s1) * 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.
 20 + (axis2.y() * axis2.z() * s1 + axis2.x() * 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.
 20 data.c()(1) = 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.
 22 tmp = (axis2.z() * axis2.x() * s1 + axis2.y() * c1) * axis1.x()
563
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.
 20 + (axis2.y() * axis2.z() * s1 - axis2.x() * c1) * axis1.y()
564
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.
 20 + (-s1 + axis2.z() * axis2.z() * s1) * 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.
 20 data.c()(2) = tmp * q_dot(1) * q_dot(0);
566 #undef q_dot
567 20 }
568
569 template<typename VectorLike, typename Matrix6Like>
570 5 void calc_aba(
571 JointDataDerived & data,
572 const Eigen::MatrixBase<VectorLike> & armature,
573 const Eigen::MatrixBase<Matrix6Like> & I,
574 const bool update_I) const
575 {
576
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();
577
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 data.StU.noalias() =
578
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);
579
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 data.StU.diagonal() += armature;
580 5 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
581
582
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;
583
584
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 if (update_I)
585 ✗ PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
586 5 }
587
588 166 static std::string classname()
589 {
590
1/2
✓ Branch 2 taken 166 times.
✗ Branch 3 not taken.
 166 return std::string("JointModelUniversal");
591 }
592 24 std::string shortname() const
593 {
594 24 return classname();
595 }
596
597 /// \returns An expression of *this with the Scalar type casted to NewScalar.
598 template<typename NewScalar>
599 5 JointModelUniversalTpl<NewScalar, Options> cast() const
600 {
601 typedef JointModelUniversalTpl<NewScalar, Options> ReturnType;
602
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>());
603
4/8
✓ 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.
 5 res.setIndexes(id(), idx_q(), idx_v());
604 5 return res;
605 }
606
607 /// \brief 3d main axii of the joint.
608 ///
609 Vector3 axis1;
610 Vector3 axis2;
611 }; // struct JointModelUniversalTpl
612
613 } // namespace pinocchio
614
615 #include <boost/type_traits.hpp>
616
617 namespace boost
618 {
619 template<typename Scalar, int Options>
620 struct has_nothrow_constructor<::pinocchio::JointModelUniversalTpl<Scalar, Options>>
621 : public integral_constant<bool, true>
622 {
623 };
624
625 template<typename Scalar, int Options>
626 struct has_nothrow_copy<::pinocchio::JointModelUniversalTpl<Scalar, Options>>
627 : public integral_constant<bool, true>
628 {
629 };
630
631 template<typename Scalar, int Options>
632 struct has_nothrow_constructor<::pinocchio::JointDataUniversalTpl<Scalar, Options>>
633 : public integral_constant<bool, true>
634 {
635 };
636
637 template<typename Scalar, int Options>
638 struct has_nothrow_copy<::pinocchio::JointDataUniversalTpl<Scalar, Options>>
639 : public integral_constant<bool, true>
640 {
641 };
642 } // namespace boost
643
644 #endif // ifndef __pinocchio_multibody_joint_universal_hpp__
645