GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp
Date: 2025-04-30 16:14:33
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 3264 JointMotionSubspaceSphericalZYXTpl(const Eigen::MatrixBase<Matrix3Like> & subspace)
69 3264 : m_S(subspace)
70 {
71 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, 3, 3);
72 3264 }
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 154 DenseBase matrix_impl() const
132 {
133 154 DenseBase S;
134
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 153 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 154 S.template middleRows<3>(LINEAR).setZero();
135
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 153 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 154 S.template middleRows<3>(ANGULAR) = m_S;
136 154 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 6413 Matrix3 & angularSubspace()
195 {
196 6413 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 NVExtended = 3
284 };
285 typedef _Scalar Scalar;
286 enum
287 {
288 Options = _Options
289 };
290 typedef JointDataSphericalZYXTpl<Scalar, Options> JointDataDerived;
291 typedef JointModelSphericalZYXTpl<Scalar, Options> JointModelDerived;
292 typedef JointMotionSubspaceSphericalZYXTpl<Scalar, Options> Constraint_t;
293 typedef SE3Tpl<Scalar, Options> Transformation_t;
294 typedef MotionSphericalTpl<Scalar, Options> Motion_t;
295 typedef MotionSphericalTpl<Scalar, Options> Bias_t;
296
297 // [ABA]
298 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
299 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
300 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
301
302 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
303 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
304
305 typedef boost::mpl::false_ is_mimicable_t;
306
307 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
308 };
309
310 template<typename _Scalar, int _Options>
311 struct traits<JointDataSphericalZYXTpl<_Scalar, _Options>>
312 {
313 typedef JointSphericalZYXTpl<_Scalar, _Options> JointDerived;
314 typedef _Scalar Scalar;
315 };
316
317 template<typename _Scalar, int _Options>
318 struct traits<JointModelSphericalZYXTpl<_Scalar, _Options>>
319 {
320 typedef JointSphericalZYXTpl<_Scalar, _Options> JointDerived;
321 typedef _Scalar Scalar;
322 };
323
324 template<typename _Scalar, int _Options>
325 struct JointDataSphericalZYXTpl
326 : public JointDataBase<JointDataSphericalZYXTpl<_Scalar, _Options>>
327 {
328 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
329 typedef JointSphericalZYXTpl<_Scalar, _Options> JointDerived;
330 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
331 1615 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
332
333 ConfigVector_t joint_q;
334 TangentVector_t joint_v;
335
336 Constraint_t S;
337 Transformation_t M;
338 Motion_t v;
339 Bias_t c;
340
341 // [ABA] specific data
342 U_t U;
343 D_t Dinv;
344 UD_t UDinv;
345 D_t StU;
346
347 3264 JointDataSphericalZYXTpl()
348
1/2
✓ Branch 2 taken 3262 times.
✗ Branch 3 not taken.
 3264 : joint_q(ConfigVector_t::Zero())
349
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3257 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3264 , joint_v(TangentVector_t::Zero())
350
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3257 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3264 , S(Constraint_t::Matrix3::Zero())
351
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 3264 , M(Transformation_t::Identity())
352
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3257 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3264 , v(Motion_t::Vector3::Zero())
353
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3257 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3264 , c(Bias_t::Vector3::Zero())
354
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3257 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3264 , U(U_t::Zero())
355
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3257 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3264 , Dinv(D_t::Zero())
356
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3257 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3264 , UDinv(UD_t::Zero())
357
3/5
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 3257 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 6528 , StU(D_t::Zero())
358 {
359 3264 }
360
361 150 static std::string classname()
362 {
363
1/2
✓ Branch 2 taken 150 times.
✗ Branch 3 not taken.
 150 return std::string("JointDataSphericalZYX");
364 }
365 3 std::string shortname() const
366 {
367 3 return classname();
368 }
369
370 }; // strcut JointDataSphericalZYXTpl
371
372 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalZYXTpl);
373 template<typename _Scalar, int _Options>
374 struct JointModelSphericalZYXTpl
375 : public JointModelBase<JointModelSphericalZYXTpl<_Scalar, _Options>>
376 {
377 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
378 typedef JointSphericalZYXTpl<_Scalar, _Options> JointDerived;
379 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
380
381 typedef JointModelBase<JointModelSphericalZYXTpl> Base;
382 using Base::id;
383 using Base::idx_q;
384 using Base::idx_v;
385 using Base::idx_vExtended;
386 using Base::setIndexes;
387
388 3185 JointDataDerived createData() const
389 {
390 3185 return JointDataDerived();
391 }
392
393 ✗ const std::vector<bool> hasConfigurationLimit() const
394 {
395 ✗ return {true, true, true};
396 }
397
398 ✗ const std::vector<bool> hasConfigurationLimitInTangent() const
399 {
400 ✗ return {true, true, true};
401 }
402
403 template<typename ConfigVector>
404 4264 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
405 {
406
3/6
✓ Branch 1 taken 4176 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4176 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4176 times.
✗ Branch 8 not taken.
 4264 data.joint_q = qs.template segment<NQ>(idx_q());
407
408
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c0, s0;
409
2/4
✓ Branch 1 taken 4176 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4264 SINCOS(data.joint_q(0), &s0, &c0);
410
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c1, s1;
411
2/4
✓ Branch 1 taken 4176 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4264 SINCOS(data.joint_q(1), &s1, &c1);
412
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c2, s2;
413
2/4
✓ Branch 1 taken 4176 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4264 SINCOS(data.joint_q(2), &s2, &c2);
414
415
20/40
✓ Branch 1 taken 4176 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4176 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4176 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4176 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4176 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.
 4266 data.M.rotation() << c0 * c1, c0 * s1 * s2 - s0 * c2, c0 * s1 * c2 + s0 * s2, s0 * c1,
416
11/22
✓ Branch 1 taken 4176 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4176 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4176 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4176 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4176 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.
 4264 s0 * s1 * s2 + c0 * c2, s0 * s1 * c2 - c0 * s2, -s1, c1 * s2, c1 * c2;
417
418
16/32
✓ Branch 2 taken 4176 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4176 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4176 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4176 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 4176 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 4176 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 4176 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 4176 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.
 4268 data.S.angularSubspace() << -s1, Scalar(0), Scalar(1), c1 * s2, c2, Scalar(0), c1 * c2, -s2,
419
1/2
✓ Branch 1 taken 4176 times.
✗ Branch 2 not taken.
 4266 Scalar(0);
420 4264 }
421
422 template<typename TangentVector>
423 void
424 1 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
425 const
426 {
427
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 data.joint_v = vs.template segment<NV>(idx_v());
428
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;
429
430 // TODO(jcarpent): to be done
431 // #define q_dot data.joint_v
432 // data.c()(0) = -c1 * q_dot(0) * q_dot(1);
433 // data.c()(1) = -s1 * s2 * q_dot(0) * q_dot(1) + c1 * c2 * q_dot(0) * q_dot(2) - s2 *
434 // q_dot(1) * q_dot(2); data.c()(2) = -s1 * c2 * q_dot(0) * q_dot(1) - c1 * s2 *
435 // q_dot(0) * q_dot(2) - c2 * q_dot(1) * q_dot(2);
436 // #undef q_dot
437 1 }
438
439 template<typename ConfigVector, typename TangentVector>
440 1154 void calc(
441 JointDataDerived & data,
442 const typename Eigen::MatrixBase<ConfigVector> & qs,
443 const typename Eigen::MatrixBase<TangentVector> & vs) const
444 {
445
3/6
✓ Branch 1 taken 1093 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1093 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1093 times.
✗ Branch 8 not taken.
 1154 data.joint_q = qs.template segment<NQ>(idx_q());
446
447
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c0, s0;
448
2/4
✓ Branch 1 taken 1093 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1154 SINCOS(data.joint_q(0), &s0, &c0);
449
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c1, s1;
450
2/4
✓ Branch 1 taken 1093 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1154 SINCOS(data.joint_q(1), &s1, &c1);
451
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar c2, s2;
452
2/4
✓ Branch 1 taken 1093 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1154 SINCOS(data.joint_q(2), &s2, &c2);
453
454
20/40
✓ Branch 1 taken 1093 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1093 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1093 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1093 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1093 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.
 1156 data.M.rotation() << c0 * c1, c0 * s1 * s2 - s0 * c2, c0 * s1 * c2 + s0 * s2, s0 * c1,
455
11/22
✓ Branch 1 taken 1093 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1093 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1093 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1093 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1093 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.
 1154 s0 * s1 * s2 + c0 * c2, s0 * s1 * c2 - c0 * s2, -s1, c1 * s2, c1 * c2;
456
457
16/32
✓ Branch 2 taken 1093 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1093 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1093 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1093 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1093 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1093 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1093 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1093 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.
 1158 data.S.angularSubspace() << -s1, Scalar(0), Scalar(1), c1 * s2, c2, Scalar(0), c1 * c2, -s2,
458
1/2
✓ Branch 1 taken 1093 times.
✗ Branch 2 not taken.
 1156 Scalar(0);
459
460
3/6
✓ Branch 1 taken 1093 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1093 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1093 times.
✗ Branch 8 not taken.
 1154 data.joint_v = vs.template segment<NV>(idx_v());
461
3/6
✓ Branch 2 taken 1093 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1093 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1093 times.
✗ Branch 10 not taken.
 1154 data.v().noalias() = data.S.angularSubspace() * data.joint_v;
462
463 #define q_dot data.joint_v
464
8/15
✓ Branch 1 taken 1093 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1093 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✓ Branch 8 taken 1092 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.
 1154 data.c()(0) = -c1 * q_dot(0) * q_dot(1);
465
10/20
✓ Branch 1 taken 1093 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.
 1158 data.c()(1) =
466
9/18
✓ Branch 1 taken 1093 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1093 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1093 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1093 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1093 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1093 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.
 1158 -s1 * s2 * q_dot(0) * q_dot(1) + c1 * c2 * q_dot(0) * q_dot(2) - s2 * q_dot(1) * q_dot(2);
467
10/20
✓ Branch 1 taken 1093 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.
 1158 data.c()(2) =
468
9/18
✓ Branch 1 taken 1093 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1093 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1093 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1093 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1093 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1093 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.
 1158 -s1 * c2 * q_dot(0) * q_dot(1) - c1 * s2 * q_dot(0) * q_dot(2) - c2 * q_dot(1) * q_dot(2);
469 #undef q_dot
470 1154 }
471
472 template<typename VectorLike, typename Matrix6Like>
473 9 void calc_aba(
474 JointDataDerived & data,
475 const Eigen::MatrixBase<VectorLike> & armature,
476 const Eigen::MatrixBase<Matrix6Like> & I,
477 const bool update_I) const
478 {
479
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();
480
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 9 data.StU.noalias() =
481
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);
482
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 9 data.StU.diagonal() += armature;
483
484 9 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
485
486
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;
487
488
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 5 times.
 9 if (update_I)
489
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();
490 9 }
491
492 17519 static std::string classname()
493 {
494
1/2
✓ Branch 2 taken 17519 times.
✗ Branch 3 not taken.
 17519 return std::string("JointModelSphericalZYX");
495 }
496 17371 std::string shortname() const
497 {
498 17371 return classname();
499 }
500
501 /// \returns An expression of *this with the Scalar type casted to NewScalar.
502 template<typename NewScalar>
503 5 JointModelSphericalZYXTpl<NewScalar, Options> cast() const
504 {
505 typedef JointModelSphericalZYXTpl<NewScalar, Options> ReturnType;
506 5 ReturnType res;
507 5 res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
508 5 return res;
509 }
510
511 }; // struct JointModelSphericalZYXTpl
512
513 } // namespace pinocchio
514
515 #include <boost/type_traits.hpp>
516
517 namespace boost
518 {
519 template<typename Scalar, int Options>
520 struct has_nothrow_constructor<::pinocchio::JointModelSphericalZYXTpl<Scalar, Options>>
521 : public integral_constant<bool, true>
522 {
523 };
524
525 template<typename Scalar, int Options>
526 struct has_nothrow_copy<::pinocchio::JointModelSphericalZYXTpl<Scalar, Options>>
527 : public integral_constant<bool, true>
528 {
529 };
530
531 template<typename Scalar, int Options>
532 struct has_nothrow_constructor<::pinocchio::JointDataSphericalZYXTpl<Scalar, Options>>
533 : public integral_constant<bool, true>
534 {
535 };
536
537 template<typename Scalar, int Options>
538 struct has_nothrow_copy<::pinocchio::JointDataSphericalZYXTpl<Scalar, Options>>
539 : public integral_constant<bool, true>
540 {
541 };
542 } // namespace boost
543
544 #endif // ifndef __pinocchio_multibody_joint_spherical_ZYX_hpp__
545