GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-helical-unaligned.hpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 204 238 85.7%
Branches: 214 591 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 99 MotionHelicalUnalignedTpl(
76 const Eigen::MatrixBase<Vector3Like> & axis, const OtherScalar & w, const OtherScalar & v)
77 99 : m_axis(axis)
78
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 99 , m_w(w)
79
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 99 , m_v(v)
80 {
81 99 }
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 95 JointMotionSubspaceHelicalUnalignedTpl(
299 const Eigen::MatrixBase<Vector3Like> & axis, const Scalar & h)
300 95 : m_axis(axis)
301
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 95 , m_pitch(h)
302 {
303 95 }
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 NVExtended = 1
529 };
530 typedef _Scalar Scalar;
531 enum
532 {
533 Options = _Options
534 };
535 typedef JointDataHelicalUnalignedTpl<Scalar, Options> JointDataDerived;
536 typedef JointModelHelicalUnalignedTpl<Scalar, Options> JointModelDerived;
537 typedef JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options> Constraint_t;
538 typedef SE3Tpl<Scalar, Options> Transformation_t;
539 typedef MotionHelicalUnalignedTpl<Scalar, Options> Motion_t;
540 typedef MotionZeroTpl<Scalar, Options> Bias_t;
541
542 // [ABA]
543 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
544 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
545 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
546
547 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
548 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
549
550 typedef boost::mpl::true_ is_mimicable_t;
551
552 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
553 };
554
555 template<typename _Scalar, int _Options>
556 struct traits<JointDataHelicalUnalignedTpl<_Scalar, _Options>>
557 {
558 typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived;
559 typedef _Scalar Scalar;
560 };
561
562 template<typename _Scalar, int _Options>
563 struct traits<JointModelHelicalUnalignedTpl<_Scalar, _Options>>
564 {
565 typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived;
566 typedef _Scalar Scalar;
567 };
568
569 template<typename _Scalar, int _Options>
570 struct JointDataHelicalUnalignedTpl
571 : public JointDataBase<JointDataHelicalUnalignedTpl<_Scalar, _Options>>
572 {
573 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
574 typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived;
575 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
576 837 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
577
578 ConfigVector_t joint_q;
579 TangentVector_t joint_v;
580
581 Constraint_t S;
582 Transformation_t M;
583 Motion_t v;
584 Bias_t c;
585
586 // [ABA] specific data
587 U_t U;
588 D_t Dinv;
589 UD_t UDinv;
590 D_t StU;
591
592 95 JointDataHelicalUnalignedTpl()
593
1/2
✓ Branch 2 taken 93 times.
✗ Branch 3 not taken.
 95 : joint_q(ConfigVector_t::Zero())
594
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 88 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 95 , joint_v(TangentVector_t::Zero())
595
3/6
✓ Branch 1 taken 93 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 93 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 95 , S(Constraint_t::Vector3::Zero(), (Scalar)0)
596
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 95 , M(Transformation_t::Identity())
597
4/8
✓ Branch 1 taken 93 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 93 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 95 , v(Constraint_t::Vector3::Zero(), (Scalar)0, (Scalar)0)
598
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 88 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 95 , U(U_t::Zero())
599
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 88 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 95 , Dinv(D_t::Zero())
600
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 88 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 95 , UDinv(UD_t::Zero())
601
3/5
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 88 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 190 , StU(D_t::Zero())
602 {
603 95 }
604
605 template<typename Vector3Like>
606 ✗ JointDataHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
607 ✗ : joint_q(ConfigVector_t::Zero())
608 ✗ , joint_v(TangentVector_t::Zero())
609 ✗ , S(axis, (Scalar)0)
610 ✗ , M(Transformation_t::Identity())
611 ✗ , v(axis, (Scalar)0, (Scalar)0)
612 ✗ , U(U_t::Zero())
613 ✗ , Dinv(D_t::Zero())
614 ✗ , UDinv(UD_t::Zero())
615 ✗ , StU(D_t::Zero())
616 {
617 }
618
619 150 static std::string classname()
620 {
621
1/2
✓ Branch 2 taken 150 times.
✗ Branch 3 not taken.
 150 return std::string("JointDataHelicalUnaligned");
622 }
623 3 std::string shortname() const
624 {
625 3 return classname();
626 }
627
628 }; // struct JointDataHelicalUnalignedTpl
629
630 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelHelicalUnalignedTpl);
631 template<typename _Scalar, int _Options>
632 struct JointModelHelicalUnalignedTpl
633 : public JointModelBase<JointModelHelicalUnalignedTpl<_Scalar, _Options>>
634 {
635 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
636 typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived;
637 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
638 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
639
640 typedef JointModelBase<JointModelHelicalUnalignedTpl> Base;
641 using Base::id;
642 using Base::idx_q;
643 using Base::idx_v;
644 using Base::idx_vExtended;
645 using Base::setIndexes;
646
647 97 JointModelHelicalUnalignedTpl()
648
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 97 {
649 97 }
650
651 template<typename Vector3Like>
652 13 JointModelHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
653 13 : axis(axis)
654 13 , m_pitch((Scalar)0)
655 {
656 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
657
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");
658 13 }
659
660 ✗ JointModelHelicalUnalignedTpl(
661 const Scalar & x, const Scalar & y, const Scalar & z, const Scalar & h)
662 ✗ : axis(x, y, z)
663 ✗ , m_pitch(h)
664 {
665 ✗ normalize(axis);
666 ✗ assert(isUnitary(axis) && "Rotation axis is not unitary");
667 }
668
669 template<typename Vector3Like>
670 23 JointModelHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis, const Scalar & h)
671 23 : axis(axis)
672
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 24 , m_pitch(h)
673 {
674 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
675
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");
676 23 }
677
678 ✗ const std::vector<bool> hasConfigurationLimit() const
679 {
680 ✗ return {true, true};
681 }
682
683 ✗ const std::vector<bool> hasConfigurationLimitInTangent() const
684 {
685 ✗ return {true, true};
686 }
687
688 16 JointDataDerived createData() const
689 {
690 16 return JointDataDerived();
691 }
692
693 using Base::isEqual;
694 15 bool isEqual(const JointModelHelicalUnalignedTpl & other) const
695 {
696
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 28 return Base::isEqual(other) && internal::comparison_eq(axis, other.axis)
697
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);
698 }
699
700 template<typename ConfigVector>
701 33 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
702 {
703
0/4
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 33 data.joint_q[0] = qs[idx_q()];
704
705 33 toRotationMatrix(axis, data.joint_q[0], data.M.rotation());
706
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.
 33 data.M.translation() = axis * data.joint_q[0] * m_pitch;
707 33 data.S.h() = m_pitch;
708 33 data.S.axis() = axis;
709 33 }
710
711 template<typename TangentVector>
712 void
713 1 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
714 const
715 {
716 1 data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
717 1 data.v.axis() = axis;
718 1 data.v.linearRate() = static_cast<Scalar>(vs[idx_v()] * m_pitch);
719 1 }
720
721 template<typename ConfigVector, typename TangentVector>
722 19 void calc(
723 JointDataDerived & data,
724 const typename Eigen::MatrixBase<ConfigVector> & qs,
725 const typename Eigen::MatrixBase<TangentVector> & vs) const
726 {
727 19 calc(data, qs.derived());
728
1/3
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 19 data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
729 19 data.v.axis() = axis;
730
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.
 19 data.v.linearRate() = static_cast<Scalar>(vs[idx_v()] * m_pitch);
731 19 }
732
733 template<typename VectorLike, typename Matrix6Like>
734 5 void calc_aba(
735 JointDataDerived & data,
736 const Eigen::MatrixBase<VectorLike> & armature,
737 const Eigen::MatrixBase<Matrix6Like> & I,
738 const bool update_I) const
739 {
740
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
741
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 + m_pitch * I.template middleCols<3>(Motion::LINEAR) * axis;
742
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))
743
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];
744
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];
745
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;
746
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 if (update_I)
747 ✗ PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
748 5 }
749
750 172 static std::string classname()
751 {
752
1/2
✓ Branch 2 taken 172 times.
✗ Branch 3 not taken.
 172 return std::string("JointModelHelicalUnaligned");
753 }
754 24 std::string shortname() const
755 {
756 24 return classname();
757 }
758
759 /// \returns An expression of *this with the Scalar type casted to NewScalar.
760 template<typename NewScalar>
761 5 JointModelHelicalUnalignedTpl<NewScalar, Options> cast() const
762 {
763 typedef JointModelHelicalUnalignedTpl<NewScalar, Options> ReturnType;
764
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));
765
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 5 res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
766 5 return res;
767 }
768
769 Vector3 axis;
770 Scalar m_pitch;
771
772 }; // struct JointModelHelicalUnalignedTpl
773
774 } // namespace pinocchio
775
776 #include <boost/type_traits.hpp>
777
778 namespace boost
779 {
780 template<typename Scalar, int Options>
781 struct has_nothrow_constructor<::pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
782 : public integral_constant<bool, true>
783 {
784 };
785
786 template<typename Scalar, int Options>
787 struct has_nothrow_copy<::pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
788 : public integral_constant<bool, true>
789 {
790 };
791
792 template<typename Scalar, int Options>
793 struct has_nothrow_constructor<::pinocchio::JointDataHelicalUnalignedTpl<Scalar, Options>>
794 : public integral_constant<bool, true>
795 {
796 };
797
798 template<typename Scalar, int Options>
799 struct has_nothrow_copy<::pinocchio::JointDataHelicalUnalignedTpl<Scalar, Options>>
800 : public integral_constant<bool, true>
801 {
802 };
803 } // namespace boost
804
805 #endif // ifndef __pinocchio_multibody_joint_helical_unaligned_hpp__
806