GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 135 139 97.1%
Branches: 271 573 47.3%
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_ZYX_hpp__
7 #define __pinocchio_multibody_joint_spherical_ZYX_hpp__
8
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/joint/joint-spherical.hpp"
12 #include "pinocchio/multibody/joint-motion-subspace.hpp"
13 #include "pinocchio/math/sincos.hpp"
14 #include "pinocchio/math/matrix.hpp"
15 #include "pinocchio/spatial/inertia.hpp"
16 #include "pinocchio/spatial/skew.hpp"
17
18 namespace pinocchio
19 {
20 template<typename Scalar, int Options>
21 struct JointMotionSubspaceSphericalZYXTpl;
22
23 template<typename _Scalar, int _Options>
24 struct traits<JointMotionSubspaceSphericalZYXTpl<_Scalar, _Options>>
25 {
26 typedef _Scalar Scalar;
27 enum
28 {
29 Options = _Options
30 };
31
32 enum
33 {
34 LINEAR = 0,
35 ANGULAR = 3
36 };
37
38 typedef MotionSphericalTpl<Scalar, Options> JointMotion;
39 typedef Eigen::Matrix<Scalar, 3, 1, Options> JointForce;
40 typedef Eigen::Matrix<Scalar, 6, 3, Options> DenseBase;
41 typedef Eigen::Matrix<Scalar, 3, 3, Options> ReducedSquaredMatrix;
42
43 typedef DenseBase MatrixReturnType;
44 typedef const DenseBase ConstMatrixReturnType;
45
46 typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType;
47 }; // struct traits struct ConstraintRotationalSubspace
48
49 template<typename _Scalar, int _Options>
50 struct JointMotionSubspaceSphericalZYXTpl
51 : public JointMotionSubspaceBase<JointMotionSubspaceSphericalZYXTpl<_Scalar, _Options>>
52 {
53 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
54
55 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceSphericalZYXTpl)
56
57 enum
58 {
59 NV = 3
60 };
61 typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
62
63 7 JointMotionSubspaceSphericalZYXTpl()
64 7 {
65 7 }
66
67 template<typename Matrix3Like>
68 3179 JointMotionSubspaceSphericalZYXTpl(const Eigen::MatrixBase<Matrix3Like> & subspace)
69 3179 : m_S(subspace)
70 {
71 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, 3, 3);
72 3179 }
73
74 template<typename Vector3Like>
75 4 JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const
76 {
77 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
78
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 return JointMotion(m_S * v);
79 }
80
81 Matrix3 & operator()()
82 {
83 return m_S;
84 }
85 const Matrix3 & operator()() const
86 {
87 return m_S;
88 }
89
90 24 int nv_impl() const
91 {
92 24 return NV;
93 }
94
95 struct ConstraintTranspose
96 : JointMotionSubspaceTransposeBase<JointMotionSubspaceSphericalZYXTpl>
97 {
98 const JointMotionSubspaceSphericalZYXTpl & ref;
99 7 ConstraintTranspose(const JointMotionSubspaceSphericalZYXTpl & ref)
100 7 : ref(ref)
101 {
102 7 }
103
104 template<typename Derived>
105 const typename MatrixMatrixProduct<
106 Eigen::Transpose<const Matrix3>,
107 Eigen::Block<const typename ForceDense<Derived>::Vector6, 3, 1>>::type
108 5 operator*(const ForceDense<Derived> & phi) const
109 {
110
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 return ref.m_S.transpose() * phi.angular();
111 }
112
113 /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
114 template<typename D>
115 const typename MatrixMatrixProduct<
116 typename Eigen::Transpose<const Matrix3>,
117 typename Eigen::MatrixBase<const D>::template NRowsBlockXpr<3>::Type>::type
118 1 operator*(const Eigen::MatrixBase<D> & F) const
119 {
120 EIGEN_STATIC_ASSERT(
121 D::RowsAtCompileTime == 6, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
122
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);
123 }
124 }; // struct ConstraintTranspose
125
126 7 ConstraintTranspose transpose() const
127 {
128 7 return ConstraintTranspose(*this);
129 }
130
131 19 DenseBase matrix_impl() const
132 {
133 19 DenseBase S;
134
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 19 S.template middleRows<3>(LINEAR).setZero();
135
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 19 S.template middleRows<3>(ANGULAR) = m_S;
136 19 return S;
137 }
138
139 // const typename Eigen::ProductReturnType<
140 // const ConstraintDense,
141 // const Matrix3
142 // >::Type
143 template<typename S1, int O1>
144 7 Eigen::Matrix<Scalar, 6, 3, Options> se3Action(const SE3Tpl<S1, O1> & m) const
145 {
146 // Eigen::Matrix <Scalar,6,3,Options> X_subspace;
147 // X_subspace.template block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) *
148 // m.rotation (); X_subspace.template block <3,3> (Motion::ANGULAR, 0) = m.rotation ();
149 //
150 // return (X_subspace * m_S).eval();
151
152 7 Eigen::Matrix<Scalar, 6, 3, Options> result;
153
154 // ANGULAR
155
3/13
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 7 result.template middleRows<3>(ANGULAR).noalias() = m.rotation() * m_S;
156
157 // LINEAR
158 7 cross(
159
3/6
✓ 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.
 7 m.translation(), result.template middleRows<3>(Motion::ANGULAR),
160
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 7 result.template middleRows<3>(LINEAR));
161
162 7 return result;
163 }
164
165 template<typename S1, int O1>
166 3 Eigen::Matrix<Scalar, 6, 3, Options> se3ActionInverse(const SE3Tpl<S1, O1> & m) const
167 {
168 3 Eigen::Matrix<Scalar, 6, 3, Options> result;
169
170 // LINEAR
171
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, result.template middleRows<3>(ANGULAR));
172
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 result.template middleRows<3>(LINEAR).noalias() =
173
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() * result.template middleRows<3>(ANGULAR);
174
175 // ANGULAR
176
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 result.template middleRows<3>(ANGULAR).noalias() = m.rotation().transpose() * m_S;
177
178 3 return result;
179 }
180
181 template<typename MotionDerived>
182 2 DenseBase motionAction(const MotionDense<MotionDerived> & m) const
183 {
184
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const typename MotionDerived::ConstLinearType v = m.linear();
185
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const typename MotionDerived::ConstAngularType w = m.angular();
186
187
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 DenseBase res;
188
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));
189
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));
190
191 4 return res;
192 }
193
194 6224 Matrix3 & angularSubspace()
195 {
196 6224 return m_S;
197 }
198 4 const Matrix3 & angularSubspace() const
199 {
200 4 return m_S;
201 }
202
203 17 bool isEqual(const JointMotionSubspaceSphericalZYXTpl & other) const
204 {
205 17 return internal::comparison_eq(m_S, other.m_S);
206 }
207
208 protected:
209 Matrix3 m_S;
210
211 }; // struct JointMotionSubspaceSphericalZYXTpl
212
213 namespace details
214 {
215 template<typename Scalar, int Options>
216 struct StDiagonalMatrixSOperation<JointMotionSubspaceSphericalZYXTpl<Scalar, Options>>
217 {
218 typedef JointMotionSubspaceSphericalZYXTpl<Scalar, Options> Constraint;
219 typedef typename traits<Constraint>::StDiagonalMatrixSOperationReturnType ReturnType;
220
221 1 static ReturnType run(const JointMotionSubspaceBase<Constraint> & constraint)
222 {
223
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();
224 }
225 };
226 } // namespace details
227
228 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
229 template<typename S1, int O1, typename S2, int O2>
230 Eigen::Matrix<S1, 6, 3, O1>
231 2 operator*(const InertiaTpl<S1, O1> & Y, const JointMotionSubspaceSphericalZYXTpl<S2, O2> & S)
232 {
233 typedef typename InertiaTpl<S1, O1>::Symmetric3 Symmetric3;
234 typedef JointMotionSubspaceSphericalZYXTpl<S2, O2> Constraint;
235
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Eigen::Matrix<S1, 6, 3, O1> M;
236
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));
237
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) =
238
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 2 (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
239
240
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 4 return (M * S.angularSubspace()).eval();
241 }
242
243 /* [ABA] Y*S operator (Inertia Y,Constraint S) */
244 // inline Eigen::Matrix<context::Scalar,6,3>
245 template<typename Matrix6Like, typename S2, int O2>
246 const typename MatrixMatrixProduct<
247 typename Eigen::internal::remove_const<
248 typename SizeDepType<3>::ColsReturn<Matrix6Like>::ConstType>::type,
249 typename JointMotionSubspaceSphericalZYXTpl<S2, O2>::Matrix3>::type
250 2 operator*(
251 const Eigen::MatrixBase<Matrix6Like> & Y, const JointMotionSubspaceSphericalZYXTpl<S2, O2> & S)
252 {
253 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, 6, 6);
254
1/2
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 return Y.derived().template middleCols<3>(Inertia::ANGULAR) * S.angularSubspace();
255 }
256
257 template<typename S1, int O1>
258 struct SE3GroupAction<JointMotionSubspaceSphericalZYXTpl<S1, O1>>
259 {
260 // typedef const typename Eigen::ProductReturnType<
261 // Eigen::Matrix <context::Scalar,6,3,0>,
262 // Eigen::Matrix <context::Scalar,3,3,0>
263 // >::Type Type;
264 typedef Eigen::Matrix<S1, 6, 3, O1> ReturnType;
265 };
266
267 template<typename S1, int O1, typename MotionDerived>
268 struct MotionAlgebraAction<JointMotionSubspaceSphericalZYXTpl<S1, O1>, MotionDerived>
269 {
270 typedef Eigen::Matrix<S1, 6, 3, O1> ReturnType;
271 };
272
273 template<typename Scalar, int Options>
274 struct JointSphericalZYXTpl;
275
276 template<typename _Scalar, int _Options>
277 struct traits<JointSphericalZYXTpl<_Scalar, _Options>>
278 {
279 enum
280 {
281 NQ = 3,
282 NV = 3
283 };
284 typedef _Scalar Scalar;
285 enum
286 {
287 Options = _Options
288 };
289 typedef JointDataSphericalZYXTpl<Scalar, Options> JointDataDerived;
290 typedef JointModelSphericalZYXTpl<Scalar, Options> JointModelDerived;
291 typedef JointMotionSubspaceSphericalZYXTpl<Scalar, Options> Constraint_t;
292 typedef SE3Tpl<Scalar, Options> Transformation_t;
293 typedef MotionSphericalTpl<Scalar, Options> Motion_t;
294 typedef MotionSphericalTpl<Scalar, Options> Bias_t;
295
296 // [ABA]
297 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
298 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
299 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
300
301 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
302 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
303
304 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
305 };
306
307 template<typename _Scalar, int _Options>
308 struct traits<JointDataSphericalZYXTpl<_Scalar, _Options>>
309 {
310 typedef JointSphericalZYXTpl<_Scalar, _Options> JointDerived;
311 typedef _Scalar Scalar;
312 };
313
314 template<typename _Scalar, int _Options>
315 struct traits<JointModelSphericalZYXTpl<_Scalar, _Options>>
316 {
317 typedef JointSphericalZYXTpl<_Scalar, _Options> JointDerived;
318 typedef _Scalar Scalar;
319 };
320
321 template<typename _Scalar, int _Options>
322 struct JointDataSphericalZYXTpl
323 : public JointDataBase<JointDataSphericalZYXTpl<_Scalar, _Options>>
324 {
325 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
326 typedef JointSphericalZYXTpl<_Scalar, _Options> JointDerived;
327 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
328 859 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
329
330 ConfigVector_t joint_q;
331 TangentVector_t joint_v;
332
333 Constraint_t S;
334 Transformation_t M;
335 Motion_t v;
336 Bias_t c;
337
338 // [ABA] specific data
339 U_t U;
340 D_t Dinv;
341 UD_t UDinv;
342 D_t StU;
343
344 3179 JointDataSphericalZYXTpl()
345
1/2
✓ Branch 2 taken 3177 times.
✗ Branch 3 not taken.
 3179 : joint_q(ConfigVector_t::Zero())
346
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3172 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3179 , joint_v(TangentVector_t::Zero())
347
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3172 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3179 , S(Constraint_t::Matrix3::Zero())
348
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 3179 , M(Transformation_t::Identity())
349
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3172 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3179 , v(Motion_t::Vector3::Zero())
350
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3172 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3179 , c(Bias_t::Vector3::Zero())
351
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3172 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3179 , U(U_t::Zero())
352
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3172 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3179 , Dinv(D_t::Zero())
353
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3172 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3179 , UDinv(UD_t::Zero())
354
3/5
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 3172 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 6358 , StU(D_t::Zero())
355 {
356 3179 }
357
358 144 static std::string classname()
359 {
360
1/2
✓ Branch 2 taken 144 times.
✗ Branch 3 not taken.
 144 return std::string("JointDataSphericalZYX");
361 }
362 3 std::string shortname() const
363 {
364 3 return classname();
365 }
366
367 }; // strcut JointDataSphericalZYXTpl
368
369 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalZYXTpl);
370 template<typename _Scalar, int _Options>
371 struct JointModelSphericalZYXTpl
372 : public JointModelBase<JointModelSphericalZYXTpl<_Scalar, _Options>>
373 {
374 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
375 typedef JointSphericalZYXTpl<_Scalar, _Options> JointDerived;
376 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
377
378 typedef JointModelBase<JointModelSphericalZYXTpl> Base;
379 using Base::id;
380 using Base::idx_q;
381 using Base::idx_v;
382 using Base::setIndexes;
383
384 3103 JointDataDerived createData() const
385 {
386 3103 return JointDataDerived();
387 }
388
389 ✗ const std::vector<bool> hasConfigurationLimit() const
390 {
391 ✗ return {true, true, true};
392 }
393
394 ✗ const std::vector<bool> hasConfigurationLimitInTangent() const
395 {
396 ✗ return {true, true, true};
397 }
398
399 template<typename ConfigVector>
400 4102 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
401 {
402
3/6
✓ Branch 1 taken 4095 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4095 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4095 times.
✗ Branch 8 not taken.
 4102 data.joint_q = qs.template segment<NQ>(idx_q());
403
404
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c0, s0;
405
2/4
✓ Branch 1 taken 4095 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4102 SINCOS(data.joint_q(0), &s0, &c0);
406
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c1, s1;
407
2/4
✓ Branch 1 taken 4095 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4102 SINCOS(data.joint_q(1), &s1, &c1);
408
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c2, s2;
409
2/4
✓ Branch 1 taken 4095 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4102 SINCOS(data.joint_q(2), &s2, &c2);
410
411
20/40
✓ Branch 1 taken 4095 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4095 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4095 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4095 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4095 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.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
 4104 data.M.rotation() << c0 * c1, c0 * s1 * s2 - s0 * c2, c0 * s1 * c2 + s0 * s2, s0 * c1,
412
11/22
✓ Branch 1 taken 4095 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4095 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4095 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4095 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4095 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.
 4102 s0 * s1 * s2 + c0 * c2, s0 * s1 * c2 - c0 * s2, -s1, c1 * s2, c1 * c2;
413
414
16/32
✓ Branch 2 taken 4095 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4095 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4095 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4095 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 4095 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 4095 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 4095 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 4095 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 1 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 1 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 1 times.
✗ Branch 48 not taken.
 4106 data.S.angularSubspace() << -s1, Scalar(0), Scalar(1), c1 * s2, c2, Scalar(0), c1 * c2, -s2,
415
1/2
✓ Branch 1 taken 4095 times.
✗ Branch 2 not taken.
 4104 Scalar(0);
416 4102 }
417
418 template<typename TangentVector>
419 void
420 1 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
421 const
422 {
423
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 data.joint_v = vs.template segment<NV>(idx_v());
424
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;
425
426 // TODO(jcarpent): to be done
427 // #define q_dot data.joint_v
428 // data.c()(0) = -c1 * q_dot(0) * q_dot(1);
429 // data.c()(1) = -s1 * s2 * q_dot(0) * q_dot(1) + c1 * c2 * q_dot(0) * q_dot(2) - s2 *
430 // q_dot(1) * q_dot(2); data.c()(2) = -s1 * c2 * q_dot(0) * q_dot(1) - c1 * s2 *
431 // q_dot(0) * q_dot(2) - c2 * q_dot(1) * q_dot(2);
432 // #undef q_dot
433 1 }
434
435 template<typename ConfigVector, typename TangentVector>
436 1041 void calc(
437 JointDataDerived & data,
438 const typename Eigen::MatrixBase<ConfigVector> & qs,
439 const typename Eigen::MatrixBase<TangentVector> & vs) const
440 {
441
3/6
✓ Branch 1 taken 1039 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1039 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1039 times.
✗ Branch 8 not taken.
 1041 data.joint_q = qs.template segment<NQ>(idx_q());
442
443
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c0, s0;
444
2/4
✓ Branch 1 taken 1039 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1041 SINCOS(data.joint_q(0), &s0, &c0);
445
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c1, s1;
446
2/4
✓ Branch 1 taken 1039 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1041 SINCOS(data.joint_q(1), &s1, &c1);
447
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c2, s2;
448
2/4
✓ Branch 1 taken 1039 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1041 SINCOS(data.joint_q(2), &s2, &c2);
449
450
20/40
✓ Branch 1 taken 1039 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1039 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1039 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1039 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1039 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.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
 1043 data.M.rotation() << c0 * c1, c0 * s1 * s2 - s0 * c2, c0 * s1 * c2 + s0 * s2, s0 * c1,
451
11/22
✓ Branch 1 taken 1039 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1039 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1039 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1039 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1039 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.
 1041 s0 * s1 * s2 + c0 * c2, s0 * s1 * c2 - c0 * s2, -s1, c1 * s2, c1 * c2;
452
453
16/32
✓ Branch 2 taken 1039 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1039 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1039 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1039 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1039 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1039 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1039 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1039 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 1 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 1 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 1 times.
✗ Branch 48 not taken.
 1045 data.S.angularSubspace() << -s1, Scalar(0), Scalar(1), c1 * s2, c2, Scalar(0), c1 * c2, -s2,
454
1/2
✓ Branch 1 taken 1039 times.
✗ Branch 2 not taken.
 1043 Scalar(0);
455
456
3/6
✓ Branch 1 taken 1039 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1039 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1039 times.
✗ Branch 8 not taken.
 1041 data.joint_v = vs.template segment<NV>(idx_v());
457
3/6
✓ Branch 2 taken 1039 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1039 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1039 times.
✗ Branch 10 not taken.
 1041 data.v().noalias() = data.S.angularSubspace() * data.joint_v;
458
459 #define q_dot data.joint_v
460
8/15
✓ Branch 1 taken 1039 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1039 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✓ Branch 8 taken 1038 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
 1041 data.c()(0) = -c1 * q_dot(0) * q_dot(1);
461
10/20
✓ Branch 1 taken 1039 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.
✓ 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 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
 1045 data.c()(1) =
462
9/18
✓ Branch 1 taken 1039 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1039 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1039 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1039 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1039 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1039 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.
 1045 -s1 * s2 * q_dot(0) * q_dot(1) + c1 * c2 * q_dot(0) * q_dot(2) - s2 * q_dot(1) * q_dot(2);
463
10/20
✓ Branch 1 taken 1039 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.
✓ 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 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
 1045 data.c()(2) =
464
9/18
✓ Branch 1 taken 1039 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1039 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1039 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1039 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1039 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1039 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.
 1045 -s1 * c2 * q_dot(0) * q_dot(1) - c1 * s2 * q_dot(0) * q_dot(2) - c2 * q_dot(1) * q_dot(2);
465 #undef q_dot
466 1041 }
467
468 template<typename VectorLike, typename Matrix6Like>
469 9 void calc_aba(
470 JointDataDerived & data,
471 const Eigen::MatrixBase<VectorLike> & armature,
472 const Eigen::MatrixBase<Matrix6Like> & I,
473 const bool update_I) const
474 {
475
3/6
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8 times.
✗ Branch 10 not taken.
 9 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.angularSubspace();
476
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 9 data.StU.noalias() =
477
2/4
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
 9 data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR);
478
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 9 data.StU.diagonal() += armature;
479
480 9 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
481
482
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 9 data.UDinv.noalias() = data.U * data.Dinv;
483
484
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 5 times.
 9 if (update_I)
485
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
 4 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
486 9 }
487
488 17513 static std::string classname()
489 {
490
1/2
✓ Branch 2 taken 17513 times.
✗ Branch 3 not taken.
 17513 return std::string("JointModelSphericalZYX");
491 }
492 17371 std::string shortname() const
493 {
494 17371 return classname();
495 }
496
497 /// \returns An expression of *this with the Scalar type casted to NewScalar.
498 template<typename NewScalar>
499 5 JointModelSphericalZYXTpl<NewScalar, Options> cast() const
500 {
501 typedef JointModelSphericalZYXTpl<NewScalar, Options> ReturnType;
502 5 ReturnType res;
503 5 res.setIndexes(id(), idx_q(), idx_v());
504 5 return res;
505 }
506
507 }; // struct JointModelSphericalZYXTpl
508
509 } // namespace pinocchio
510
511 #include <boost/type_traits.hpp>
512
513 namespace boost
514 {
515 template<typename Scalar, int Options>
516 struct has_nothrow_constructor<::pinocchio::JointModelSphericalZYXTpl<Scalar, Options>>
517 : public integral_constant<bool, true>
518 {
519 };
520
521 template<typename Scalar, int Options>
522 struct has_nothrow_copy<::pinocchio::JointModelSphericalZYXTpl<Scalar, Options>>
523 : public integral_constant<bool, true>
524 {
525 };
526
527 template<typename Scalar, int Options>
528 struct has_nothrow_constructor<::pinocchio::JointDataSphericalZYXTpl<Scalar, Options>>
529 : public integral_constant<bool, true>
530 {
531 };
532
533 template<typename Scalar, int Options>
534 struct has_nothrow_copy<::pinocchio::JointDataSphericalZYXTpl<Scalar, Options>>
535 : public integral_constant<bool, true>
536 {
537 };
538 } // namespace boost
539
540 #endif // ifndef __pinocchio_multibody_joint_spherical_ZYX_hpp__
541