GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-helical-unaligned.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 204 238 85.7%
Branches: 213 589 36.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2022-2023 INRIA
3 //
4
5 #ifndef __pinocchio_multibody_joint_helical_unaligned_hpp__
6 #define __pinocchio_multibody_joint_helical_unaligned_hpp__
7
8 #include "pinocchio/fwd.hpp"
9 #include "pinocchio/multibody/joint/joint-base.hpp"
10 #include "pinocchio/multibody/joint-motion-subspace.hpp"
11 #include "pinocchio/spatial/inertia.hpp"
12
13 #include "pinocchio/math/matrix.hpp"
14 #include "pinocchio/math/rotation.hpp"
15
16 namespace pinocchio
17 {
18
19 template<typename Scalar, int Options>
20 struct MotionHelicalUnalignedTpl;
21
22 template<typename Scalar, int Options>
23 struct SE3GroupAction<MotionHelicalUnalignedTpl<Scalar, Options>>
24 {
25 typedef MotionTpl<Scalar, Options> ReturnType;
26 };
27
28 template<typename Scalar, int Options, typename MotionDerived>
29 struct MotionAlgebraAction<MotionHelicalUnalignedTpl<Scalar, Options>, MotionDerived>
30 {
31 typedef MotionTpl<Scalar, Options> ReturnType;
32 };
33
34 template<typename _Scalar, int _Options>
35 struct traits<MotionHelicalUnalignedTpl<_Scalar, _Options>>
36 {
37 typedef _Scalar Scalar;
38 enum
39 {
40 Options = _Options
41 };
42 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
43 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
44 typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
45 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
46 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
47 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
48 typedef Vector3 AngularType;
49 typedef Vector3 LinearType;
50 typedef const Vector3 ConstAngularType;
51 typedef const Vector3 ConstLinearType;
52 typedef Matrix6 ActionMatrixType;
53 typedef MotionTpl<Scalar, Options> MotionPlain;
54 typedef MotionPlain PlainReturnType;
55 typedef Matrix4 HomogeneousMatrixType;
56 enum
57 {
58 LINEAR = 0,
59 ANGULAR = 3
60 };
61 }; // traits MotionHelicalUnalignedTpl
62
63 template<typename _Scalar, int _Options>
64 struct MotionHelicalUnalignedTpl : MotionBase<MotionHelicalUnalignedTpl<_Scalar, _Options>>
65 {
66 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67
68 MOTION_TYPEDEF_TPL(MotionHelicalUnalignedTpl);
69
70 7 MotionHelicalUnalignedTpl()
71 7 {
72 7 }
73
74 template<typename Vector3Like, typename OtherScalar>
75 96 MotionHelicalUnalignedTpl(
76 const Eigen::MatrixBase<Vector3Like> & axis, const OtherScalar & w, const OtherScalar & v)
77 96 : m_axis(axis)
78
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 96 , m_w(w)
79
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 96 , m_v(v)
80 {
81 96 }
82
83 11 inline PlainReturnType plain() const
84 {
85
2/4
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 11 return PlainReturnType(m_axis * m_v, m_axis * m_w);
86 }
87
88 template<typename OtherScalar>
89 MotionHelicalUnalignedTpl __mult__(const OtherScalar & alpha) const
90 {
91 return MotionHelicalUnalignedTpl(m_axis, alpha * m_w, alpha * m_v);
92 }
93
94 template<typename MotionDerived>
95 3 void setTo(MotionDense<MotionDerived> & m) const
96 {
97
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 3 times.
 12 for (Eigen::DenseIndex k = 0; k < 3; ++k)
98 {
99
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 m.angular().noalias() = m_axis * m_w;
100
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 m.linear().noalias() = m_axis * m_v;
101 }
102 3 }
103
104 template<typename MotionDerived>
105 1 inline void addTo(MotionDense<MotionDerived> & v) const
106 {
107
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 v.angular() += m_axis * m_w;
108
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 v.linear() += m_axis * m_v;
109 1 }
110
111 template<typename S2, int O2, typename D2>
112 1 inline void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
113 {
114
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 v.angular().noalias() = m_w * m.rotation() * m_axis;
115
8/16
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
 1 v.linear().noalias() = m.translation().cross(v.angular()) + m_v * m.rotation() * m_axis;
116 1 }
117
118 template<typename S2, int O2>
119 1 MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const
120 {
121 1 MotionPlain res;
122
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 1 se3Action_impl(m, res);
123 1 return res;
124 }
125
126 template<typename S2, int O2, typename D2>
127 ✗ void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
128 {
129 // Linear
130 ✗ v.angular().noalias() = m_axis.cross(m.translation());
131 ✗ v.angular() *= m_w;
132 ✗ v.linear().noalias() =
133 ✗ m.rotation().transpose() * v.angular() + m_v * (m.rotation().transpose() * m_axis);
134
135 // Angular
136 ✗ v.angular().noalias() = m.rotation().transpose() * m_axis * m_w;
137 }
138
139 template<typename S2, int O2>
140 ✗ MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
141 {
142 ✗ MotionPlain res;
143 ✗ se3ActionInverse_impl(m, res);
144 ✗ return res;
145 }
146
147 template<typename M1, typename M2>
148 2 void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
149 {
150 // Linear
151
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
 2 mout.linear().noalias() = v.linear().cross(m_axis);
152
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 mout.linear() *= m_w;
153
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
 2 mout.angular().noalias() = v.angular().cross(m_axis);
154
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 mout.angular() *= m_v;
155
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 mout.linear() += mout.angular();
156
157 // Angular
158
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
 2 mout.angular().noalias() = v.angular().cross(m_axis);
159
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 mout.angular() *= m_w;
160 2 }
161
162 template<typename M1>
163 2 MotionPlain motionAction(const MotionDense<M1> & v) const
164 {
165 2 MotionPlain res;
166
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 2 motionAction(v, res);
167 2 return res;
168 }
169
170 45 Scalar & angularRate()
171 {
172 45 return m_w;
173 }
174 const Scalar & angularRate() const
175 {
176 return m_w;
177 }
178
179 45 Scalar & linearRate()
180 {
181 45 return m_v;
182 }
183 const Scalar & linearRate() const
184 {
185 return m_v;
186 }
187
188 45 Vector3 & axis()
189 {
190 45 return m_axis;
191 }
192 const Vector3 & axis() const
193 {
194 return m_axis;
195 }
196
197 17 bool isEqual_impl(const MotionHelicalUnalignedTpl & other) const
198 {
199 17 return internal::comparison_eq(m_axis, other.m_axis)
200
3/6
✓ Branch 0 taken 17 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 17 times.
✗ Branch 7 not taken.
 17 && internal::comparison_eq(m_w, other.m_w) && internal::comparison_eq(m_v, other.m_v);
201 }
202
203 protected:
204 Vector3 m_axis;
205 Scalar m_w, m_v;
206
207 }; // struct MotionHelicalUnalignedTpl
208
209 template<typename S1, int O1, typename MotionDerived>
210 typename MotionDerived::MotionPlain
211 ✗ operator+(const MotionHelicalUnalignedTpl<S1, O1> & m1, const MotionDense<MotionDerived> & m2)
212 {
213 ✗ typename MotionDerived::MotionPlain res(m2);
214 ✗ res += m1;
215 ✗ return res;
216 }
217
218 template<typename MotionDerived, typename S2, int O2>
219 EIGEN_STRONG_INLINE typename MotionDerived::MotionPlain
220 2 operator^(const MotionDense<MotionDerived> & m1, const MotionHelicalUnalignedTpl<S2, O2> & m2)
221 {
222 2 return m2.motionAction(m1);
223 }
224
225 template<typename Scalar, int Options>
226 struct JointMotionSubspaceHelicalUnalignedTpl;
227
228 template<typename Scalar, int Options>
229 struct SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>>
230 {
231 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
232 };
233
234 template<typename Scalar, int Options, typename MotionDerived>
235 struct MotionAlgebraAction<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>, MotionDerived>
236 {
237 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
238 };
239
240 template<typename Scalar, int Options, typename ForceDerived>
241 struct ConstraintForceOp<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>, ForceDerived>
242 {
243 typedef typename Eigen::Matrix<Scalar, 1, 1> ReturnType;
244 };
245
246 template<typename Scalar, int Options, typename ForceSet>
247 struct ConstraintForceSetOp<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>, ForceSet>
248 {
249 typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> ReturnType;
250 };
251
252 template<typename _Scalar, int _Options>
253 struct traits<JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options>>
254 {
255 typedef _Scalar Scalar;
256 enum
257 {
258 Options = _Options
259 };
260 enum
261 {
262 LINEAR = 0,
263 ANGULAR = 3
264 };
265
266 typedef MotionHelicalUnalignedTpl<Scalar, Options> JointMotion;
267 typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce;
268 typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase;
269 typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix;
270
271 typedef DenseBase MatrixReturnType;
272 typedef const DenseBase ConstMatrixReturnType;
273
274 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
275
276 typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType;
277 }; // traits JointMotionSubspaceHelicalUnalignedTpl
278
279 template<typename _Scalar, int _Options>
280 struct JointMotionSubspaceHelicalUnalignedTpl
281 : JointMotionSubspaceBase<JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options>>
282 {
283 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
284
285 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceHelicalUnalignedTpl)
286 enum
287 {
288 NV = 1
289 };
290
291 7 JointMotionSubspaceHelicalUnalignedTpl()
292 7 {
293 7 }
294
295 typedef typename traits<JointMotionSubspaceHelicalUnalignedTpl>::Vector3 Vector3;
296
297 template<typename Vector3Like>
298 92 JointMotionSubspaceHelicalUnalignedTpl(
299 const Eigen::MatrixBase<Vector3Like> & axis, const Scalar & h)
300 92 : m_axis(axis)
301
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 92 , m_pitch(h)
302 {
303 92 }
304
305 template<typename Vector1Like>
306 2 JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
307 {
308 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1);
309
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(v.size() == 1);
310
2/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 4 return JointMotion(m_axis, v[0], Scalar(v[0] * m_pitch));
311 }
312
313 template<typename S1, int O1>
314 typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType
315 7 se3Action(const SE3Tpl<S1, O1> & m) const
316 {
317 typedef
318 typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType ReturnType;
319 7 ReturnType res;
320
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 res.template segment<3>(ANGULAR).noalias() = m.rotation() * m_axis;
321
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 res.template segment<3>(LINEAR).noalias() =
322
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 m.translation().cross(res.template segment<3>(ANGULAR)) + m_pitch * (m.rotation() * m_axis);
323 7 return res;
324 }
325
326 template<typename S1, int O1>
327 typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType
328 3 se3ActionInverse(const SE3Tpl<S1, O1> & m) const
329 {
330 typedef
331 typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType ReturnType;
332
333 3 ReturnType res;
334
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 segment<3>(ANGULAR).noalias() = m.rotation().transpose() * m_axis;
335
7/14
✓ 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.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
 6 res.template segment<3>(LINEAR).noalias() =
336
4/8
✓ 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.
 3 -m.rotation().transpose() * m.translation().cross(m_axis)
337
1/7
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 3 + m.rotation().transpose() * m_axis * m_pitch;
338
339 3 return res;
340 }
341
342 18 int nv_impl() const
343 {
344 18 return NV;
345 }
346
347 struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceHelicalUnalignedTpl>
348 {
349 const JointMotionSubspaceHelicalUnalignedTpl & ref;
350 5 TransposeConst(const JointMotionSubspaceHelicalUnalignedTpl & ref)
351 5 : ref(ref)
352 {
353 5 }
354
355 template<typename ForceDerived>
356 typename ConstraintForceOp<JointMotionSubspaceHelicalUnalignedTpl, ForceDerived>::ReturnType
357 3 operator*(const ForceDense<ForceDerived> & f) const
358 {
359 typedef typename ConstraintForceOp<
360 JointMotionSubspaceHelicalUnalignedTpl, ForceDerived>::ReturnType ReturnType;
361 3 ReturnType res;
362
4/21
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
 3 res[0] = ref.axis().dot(f.angular()) + ref.axis().dot(f.linear()) * ref.m_pitch;
363 3 return res;
364 }
365
366 /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
367 template<typename Derived>
368 typename ConstraintForceSetOp<JointMotionSubspaceHelicalUnalignedTpl, Derived>::ReturnType
369 1 operator*(const Eigen::MatrixBase<Derived> & F) const
370 {
371
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(F.rows() == 6);
372 return (
373
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ref.axis().transpose() * F.template middleRows<3>(ANGULAR)
374
5/10
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 + (ref.axis().transpose() * F.template middleRows<3>(LINEAR) * ref.m_pitch));
375 }
376 }; // struct TransposeConst
377
378 5 TransposeConst transpose() const
379 {
380 5 return TransposeConst(*this);
381 }
382
383 /* CRBA joint operators
384 * - ForceSet::Block = ForceSet
385 * - ForceSet operator* (Inertia Y,Constraint S)
386 * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
387 * - SE3::act(ForceSet::Block)
388 */
389 9 DenseBase matrix_impl() const
390 {
391 9 DenseBase S;
392
2/8
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 9 S.template segment<3>(LINEAR) = m_axis * m_pitch;
393
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 9 S.template segment<3>(ANGULAR) = m_axis;
394 9 return S;
395 }
396
397 template<typename MotionDerived>
398 typename MotionAlgebraAction<JointMotionSubspaceHelicalUnalignedTpl, MotionDerived>::ReturnType
399 2 motionAction(const MotionDense<MotionDerived> & m) const
400 {
401
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const typename MotionDerived::ConstLinearType v = m.linear();
402
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const typename MotionDerived::ConstAngularType w = m.angular();
403
404
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 DenseBase res;
405
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 res.template segment<3>(LINEAR).noalias() = v.cross(m_axis);
406
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 2 res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis * m_pitch);
407
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 res.template segment<3>(LINEAR).noalias() += res.template segment<3>(ANGULAR);
408
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 res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis);
409
410 4 return res;
411 }
412
413 17 bool isEqual(const JointMotionSubspaceHelicalUnalignedTpl & other) const
414 {
415 17 return internal::comparison_eq(m_axis, other.m_axis)
416
2/4
✓ Branch 0 taken 17 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
 17 && internal::comparison_eq(m_pitch, other.m_pitch);
417 }
418
419 54 Vector3 & axis()
420 {
421 54 return m_axis;
422 }
423 24 const Vector3 & axis() const
424 {
425 24 return m_axis;
426 }
427
428 54 Scalar & h()
429 {
430 54 return m_pitch;
431 }
432 6 const Scalar & h() const
433 {
434 6 return m_pitch;
435 }
436
437 Vector3 m_axis;
438 Scalar m_pitch;
439
440 }; // struct JointMotionSubspaceHelicalUnalignedTpl
441
442 template<typename _Scalar, int _Options>
443 1 Eigen::Matrix<_Scalar, 1, 1, _Options> operator*(
444 const typename JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options>::TransposeConst &
445 S_transpose,
446 const JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options> & S)
447 {
448 1 Eigen::Matrix<_Scalar, 1, 1, _Options> res;
449
2/4
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 res(0) = (S_transpose.ref.axis() * S_transpose.ref.h()).dot(S.axis() * S.h())
450
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 + (S_transpose.ref.axis().dot(S.axis()));
451 1 return res;
452 }
453
454 template<typename S1, int O1, typename S2, int O2>
455 struct MultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalUnalignedTpl<S2, O2>>
456 {
457 typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType;
458 };
459
460 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
461 namespace impl
462 {
463 template<typename S1, int O1, typename S2, int O2>
464 struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalUnalignedTpl<S2, O2>>
465 {
466 typedef InertiaTpl<S1, O1> Inertia;
467 typedef JointMotionSubspaceHelicalUnalignedTpl<S2, O2> Constraint;
468 typedef typename JointMotionSubspaceHelicalUnalignedTpl<S2, O2>::Vector3 Vector3;
469 typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType;
470 2 static inline ReturnType run(const Inertia & Y, const Constraint & cru)
471 {
472
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 ReturnType res;
473 /* YS = [ m -mcx ; mcx I-mcxcx ] [ h ; w ] = [mh-mcxw ; mcxh+Iw-mcxcxw ] */
474
475 2 const S2 & m_pitch = cru.h();
476
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 2 const typename Inertia::Scalar & m = Y.mass();
477 2 const typename Inertia::Vector3 & c = Y.lever();
478 2 const typename Inertia::Symmetric3 & I = Y.inertia();
479
480
4/10
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 2 res.template segment<3>(Inertia::LINEAR) = -m * c.cross(cru.axis());
481
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
 2 res.template segment<3>(Inertia::ANGULAR).noalias() = I * cru.axis();
482
5/10
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
 2 res.template segment<3>(Inertia::ANGULAR) += c.cross(cru.axis()) * m * m_pitch;
483
3/6
✓ 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.
 2 res.template segment<3>(Inertia::ANGULAR) +=
484
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 c.cross(res.template segment<3>(Inertia::LINEAR));
485
3/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 2 res.template segment<3>(Inertia::LINEAR) += m * m_pitch * cru.axis();
486 4 return res;
487 }
488 };
489 } // namespace impl
490
491 template<typename M6Like, typename Scalar, int Options>
492 struct MultiplicationOp<
493 Eigen::MatrixBase<M6Like>,
494 JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>>
495 {
496 typedef const Eigen::Matrix<Scalar, 6, 1> ReturnType;
497 };
498
499 /* [ABA] operator* (Inertia Y,Constraint S) */
500 namespace impl
501 {
502 template<typename M6Like, typename Scalar, int Options>
503 struct LhsMultiplicationOp<
504 Eigen::MatrixBase<M6Like>,
505 JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>>
506 {
507 typedef JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options> Constraint;
508 typedef Eigen::Matrix<Scalar, 6, 1> ReturnType;
509 2 static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & cru)
510 {
511 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
512
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis()
513
4/8
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
 4 + Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis() * cru.h();
514 }
515 };
516 } // namespace impl
517
518 template<typename Scalar, int Options>
519 struct JointHelicalUnalignedTpl;
520
521 template<typename _Scalar, int _Options>
522 struct traits<JointHelicalUnalignedTpl<_Scalar, _Options>>
523 {
524 enum
525 {
526 NQ = 1,
527 NV = 1
528 };
529 typedef _Scalar Scalar;
530 enum
531 {
532 Options = _Options
533 };
534 typedef JointDataHelicalUnalignedTpl<Scalar, Options> JointDataDerived;
535 typedef JointModelHelicalUnalignedTpl<Scalar, Options> JointModelDerived;
536 typedef JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options> Constraint_t;
537 typedef SE3Tpl<Scalar, Options> Transformation_t;
538 typedef MotionHelicalUnalignedTpl<Scalar, Options> Motion_t;
539 typedef MotionZeroTpl<Scalar, Options> Bias_t;
540
541 // [ABA]
542 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
543 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
544 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
545
546 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
547 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
548
549 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
550 };
551
552 template<typename _Scalar, int _Options>
553 struct traits<JointDataHelicalUnalignedTpl<_Scalar, _Options>>
554 {
555 typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived;
556 typedef _Scalar Scalar;
557 };
558
559 template<typename _Scalar, int _Options>
560 struct traits<JointModelHelicalUnalignedTpl<_Scalar, _Options>>
561 {
562 typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived;
563 typedef _Scalar Scalar;
564 };
565
566 template<typename _Scalar, int _Options>
567 struct JointDataHelicalUnalignedTpl
568 : public JointDataBase<JointDataHelicalUnalignedTpl<_Scalar, _Options>>
569 {
570 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
571 typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived;
572 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
573 837 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
574
575 ConfigVector_t joint_q;
576 TangentVector_t joint_v;
577
578 Constraint_t S;
579 Transformation_t M;
580 Motion_t v;
581 Bias_t c;
582
583 // [ABA] specific data
584 U_t U;
585 D_t Dinv;
586 UD_t UDinv;
587 D_t StU;
588
589 92 JointDataHelicalUnalignedTpl()
590
1/2
✓ Branch 2 taken 90 times.
✗ Branch 3 not taken.
 92 : joint_q(ConfigVector_t::Zero())
591
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 85 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 92 , joint_v(TangentVector_t::Zero())
592
3/6
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 92 , S(Constraint_t::Vector3::Zero(), (Scalar)0)
593
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 92 , M(Transformation_t::Identity())
594
4/8
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 92 , v(Constraint_t::Vector3::Zero(), (Scalar)0, (Scalar)0)
595
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 85 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 92 , U(U_t::Zero())
596
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 85 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 92 , Dinv(D_t::Zero())
597
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 85 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 92 , UDinv(UD_t::Zero())
598
3/5
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 85 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 184 , StU(D_t::Zero())
599 {
600 92 }
601
602 template<typename Vector3Like>
603 ✗ JointDataHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
604 ✗ : joint_q(ConfigVector_t::Zero())
605 ✗ , joint_v(TangentVector_t::Zero())
606 ✗ , S(axis, (Scalar)0)
607 ✗ , M(Transformation_t::Identity())
608 ✗ , v(axis, (Scalar)0, (Scalar)0)
609 ✗ , U(U_t::Zero())
610 ✗ , Dinv(D_t::Zero())
611 ✗ , UDinv(UD_t::Zero())
612 ✗ , StU(D_t::Zero())
613 {
614 }
615
616 144 static std::string classname()
617 {
618
1/2
✓ Branch 2 taken 144 times.
✗ Branch 3 not taken.
 144 return std::string("JointDataHelicalUnaligned");
619 }
620 3 std::string shortname() const
621 {
622 3 return classname();
623 }
624
625 }; // struct JointDataHelicalUnalignedTpl
626
627 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelHelicalUnalignedTpl);
628 template<typename _Scalar, int _Options>
629 struct JointModelHelicalUnalignedTpl
630 : public JointModelBase<JointModelHelicalUnalignedTpl<_Scalar, _Options>>
631 {
632 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
633 typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived;
634 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
635 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
636
637 typedef JointModelBase<JointModelHelicalUnalignedTpl> Base;
638 using Base::id;
639 using Base::idx_q;
640 using Base::idx_v;
641 using Base::setIndexes;
642
643 94 JointModelHelicalUnalignedTpl()
644
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 94 {
645 94 }
646
647 template<typename Vector3Like>
648 13 JointModelHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
649 13 : axis(axis)
650 13 , m_pitch((Scalar)0)
651 {
652 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
653
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 13 assert(isUnitary(axis) && "Rotation axis is not unitary");
654 13 }
655
656 ✗ JointModelHelicalUnalignedTpl(
657 const Scalar & x, const Scalar & y, const Scalar & z, const Scalar & h)
658 ✗ : axis(x, y, z)
659 ✗ , m_pitch(h)
660 {
661 ✗ normalize(axis);
662 ✗ assert(isUnitary(axis) && "Rotation axis is not unitary");
663 }
664
665 template<typename Vector3Like>
666 23 JointModelHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis, const Scalar & h)
667 23 : axis(axis)
668
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 24 , m_pitch(h)
669 {
670 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
671
5/11
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 19 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.
 23 assert(isUnitary(axis) && "Rotation axis is not unitary");
672 23 }
673
674 ✗ const std::vector<bool> hasConfigurationLimit() const
675 {
676 ✗ return {true, true};
677 }
678
679 ✗ const std::vector<bool> hasConfigurationLimitInTangent() const
680 {
681 ✗ return {true, true};
682 }
683
684 16 JointDataDerived createData() const
685 {
686 16 return JointDataDerived();
687 }
688
689 using Base::isEqual;
690 15 bool isEqual(const JointModelHelicalUnalignedTpl & other) const
691 {
692
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 28 return Base::isEqual(other) && internal::comparison_eq(axis, other.axis)
693
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(m_pitch, other.m_pitch);
694 }
695
696 template<typename ConfigVector>
697 29 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
698 {
699
0/4
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 29 data.joint_q[0] = qs[idx_q()];
700
701 29 toRotationMatrix(axis, data.joint_q[0], data.M.rotation());
702
3/6
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
 29 data.M.translation() = axis * data.joint_q[0] * m_pitch;
703 29 data.S.h() = m_pitch;
704 29 data.S.axis() = axis;
705 29 }
706
707 template<typename TangentVector>
708 void
709 1 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
710 const
711 {
712 1 data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
713 1 data.v.axis() = axis;
714 1 data.v.linearRate() = static_cast<Scalar>(vs[idx_v()] * m_pitch);
715 1 }
716
717 template<typename ConfigVector, typename TangentVector>
718 16 void calc(
719 JointDataDerived & data,
720 const typename Eigen::MatrixBase<ConfigVector> & qs,
721 const typename Eigen::MatrixBase<TangentVector> & vs) const
722 {
723 16 calc(data, qs.derived());
724
1/3
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 16 data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
725 16 data.v.axis() = axis;
726
1/6
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 16 data.v.linearRate() = static_cast<Scalar>(vs[idx_v()] * m_pitch);
727 16 }
728
729 template<typename VectorLike, typename Matrix6Like>
730 5 void calc_aba(
731 JointDataDerived & data,
732 const Eigen::MatrixBase<VectorLike> & armature,
733 const Eigen::MatrixBase<Matrix6Like> & I,
734 const bool update_I) const
735 {
736
6/12
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
 5 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis
737
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 + m_pitch * I.template middleCols<3>(Motion::LINEAR) * axis;
738
1/14
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
 5 data.StU[0] = axis.dot(data.U.template segment<3>(Motion::ANGULAR))
739
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 5 + m_pitch * axis.dot(data.U.template segment<3>(Motion::LINEAR)) + armature[0];
740
0/6
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 5 data.Dinv[0] = Scalar(1) / data.StU[0];
741
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;
742
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 if (update_I)
743 ✗ PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
744 5 }
745
746 166 static std::string classname()
747 {
748
1/2
✓ Branch 2 taken 166 times.
✗ Branch 3 not taken.
 166 return std::string("JointModelHelicalUnaligned");
749 }
750 24 std::string shortname() const
751 {
752 24 return classname();
753 }
754
755 /// \returns An expression of *this with the Scalar type casted to NewScalar.
756 template<typename NewScalar>
757 5 JointModelHelicalUnalignedTpl<NewScalar, Options> cast() const
758 {
759 typedef JointModelHelicalUnalignedTpl<NewScalar, Options> ReturnType;
760
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 5 ReturnType res(axis.template cast<NewScalar>(), ScalarCast<NewScalar, Scalar>::cast(m_pitch));
761
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());
762 5 return res;
763 }
764
765 Vector3 axis;
766 Scalar m_pitch;
767
768 }; // struct JointModelHelicalUnalignedTpl
769
770 } // namespace pinocchio
771
772 #include <boost/type_traits.hpp>
773
774 namespace boost
775 {
776 template<typename Scalar, int Options>
777 struct has_nothrow_constructor<::pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
778 : public integral_constant<bool, true>
779 {
780 };
781
782 template<typename Scalar, int Options>
783 struct has_nothrow_copy<::pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
784 : public integral_constant<bool, true>
785 {
786 };
787
788 template<typename Scalar, int Options>
789 struct has_nothrow_constructor<::pinocchio::JointDataHelicalUnalignedTpl<Scalar, Options>>
790 : public integral_constant<bool, true>
791 {
792 };
793
794 template<typename Scalar, int Options>
795 struct has_nothrow_copy<::pinocchio::JointDataHelicalUnalignedTpl<Scalar, Options>>
796 : public integral_constant<bool, true>
797 {
798 };
799 } // namespace boost
800
801 #endif // ifndef __pinocchio_multibody_joint_helical_unaligned_hpp__
802