GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-helical.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 245 258 95.0%
Branches: 217 706 30.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2022-2023 INRIA
3 //
4
5 #ifndef __pinocchio_multibody_joint_helical_hpp__
6 #define __pinocchio_multibody_joint_helical_hpp__
7
8 #include "pinocchio/math/sincos.hpp"
9 #include "pinocchio/spatial/inertia.hpp"
10 #include "pinocchio/multibody/joint-motion-subspace.hpp"
11 #include "pinocchio/multibody/joint/joint-base.hpp"
12 #include "pinocchio/spatial/spatial-axis.hpp"
13 #include "pinocchio/utils/axis-label.hpp"
14
15 namespace pinocchio
16 {
17
18 template<typename Scalar, int Options, int axis>
19 struct MotionHelicalTpl;
20
21 template<typename Scalar, int Options, int axis>
22 struct SE3GroupAction<MotionHelicalTpl<Scalar, Options, axis>>
23 {
24 typedef MotionTpl<Scalar, Options> ReturnType;
25 };
26
27 template<typename Scalar, int Options, int axis, typename MotionDerived>
28 struct MotionAlgebraAction<MotionHelicalTpl<Scalar, Options, axis>, MotionDerived>
29 {
30 typedef MotionTpl<Scalar, Options> ReturnType;
31 };
32
33 template<typename _Scalar, int _Options, int axis>
34 struct traits<MotionHelicalTpl<_Scalar, _Options, axis>>
35 {
36 typedef _Scalar Scalar;
37 enum
38 {
39 Options = _Options
40 };
41 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
42 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
43 typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
44 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
45 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
46 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
47 typedef Vector3 AngularType;
48 typedef Vector3 LinearType;
49 typedef const Vector3 ConstAngularType;
50 typedef const Vector3 ConstLinearType;
51 typedef Matrix6 ActionMatrixType;
52 typedef MotionTpl<Scalar, Options> MotionPlain;
53 typedef MotionPlain PlainReturnType;
54 typedef Matrix4 HomogeneousMatrixType;
55 enum
56 {
57 LINEAR = 0,
58 ANGULAR = 3
59 };
60 }; // traits MotionHelicalTpl
61
62 template<typename Scalar, int Options, int axis>
63 struct TransformHelicalTpl;
64
65 template<typename _Scalar, int _Options, int _axis>
66 struct traits<TransformHelicalTpl<_Scalar, _Options, _axis>>
67 {
68 enum
69 {
70 axis = _axis,
71 Options = _Options,
72 LINEAR = 0,
73 ANGULAR = 3
74 };
75 typedef _Scalar Scalar;
76 typedef SE3Tpl<Scalar, Options> PlainType;
77 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
78 typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
79 typedef Matrix3 AngularType;
80 typedef Matrix3 AngularRef;
81 typedef Matrix3 ConstAngularRef;
82 typedef Vector3 LinearType;
83 typedef typename Vector3::ConstantReturnType LinearRef;
84 typedef const typename Vector3::ConstantReturnType ConstLinearRef;
85 typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
86 typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
87 }; // traits TransformHelicalTpl
88
89 template<typename Scalar, int Options, int axis>
90 struct SE3GroupAction<TransformHelicalTpl<Scalar, Options, axis>>
91 {
92 typedef typename traits<TransformHelicalTpl<Scalar, Options, axis>>::PlainType ReturnType;
93 };
94
95 template<typename _Scalar, int _Options, int axis>
96 struct TransformHelicalTpl : SE3Base<TransformHelicalTpl<_Scalar, _Options, axis>>
97 {
98 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
99 PINOCCHIO_SE3_TYPEDEF_TPL(TransformHelicalTpl);
100
101 typedef SpatialAxis<axis + LINEAR> AxisLinear;
102 typedef typename AxisLinear::CartesianAxis3 CartesianAxis3Linear;
103
104 42 TransformHelicalTpl()
105 {
106 42 }
107 514 TransformHelicalTpl(const Scalar & sin, const Scalar & cos, const Scalar & displacement)
108 544 : m_sin(sin)
109
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 544 , m_cos(cos)
110
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 544 , m_displacement(displacement)
111 {
112 544 }
113
114 88 PlainType plain() const
115 {
116 88 PlainType res(PlainType::Identity());
117
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 88 _setRotation(res.rotation());
118
3/6
✓ 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.
 88 res.translation()[axis] = m_displacement;
119 88 return res;
120 }
121
122 88 operator PlainType() const
123 {
124 88 return plain();
125 }
126
127 template<typename S2, int O2>
128 typename SE3GroupAction<TransformHelicalTpl>::ReturnType
129 se3action(const SE3Tpl<S2, O2> & m) const
130 {
131 typedef typename SE3GroupAction<TransformHelicalTpl>::ReturnType ReturnType;
132 ReturnType res;
133 switch (axis)
134 {
135 case 0: {
136 res.rotation().col(0) = m.rotation().col(0);
137 res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2);
138 res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1));
139 break;
140 }
141 case 1: {
142 res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
143 res.rotation().col(1) = m.rotation().col(1);
144 res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
145 break;
146 }
147 case 2: {
148 res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1);
149 res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0));
150 res.rotation().col(2) = m.rotation().col(2);
151 break;
152 }
153 default: {
154 assert(false && "must never happen");
155 break;
156 }
157 }
158 res.translation() = m.translation();
159 res.translation()[axis] += m_displacement;
160 return res;
161 }
162
163 const Scalar & sin() const
164 {
165 return m_sin;
166 }
167 168 Scalar & sin()
168 {
169 168 return m_sin;
170 }
171
172 const Scalar & cos() const
173 {
174 return m_cos;
175 }
176 168 Scalar & cos()
177 {
178 168 return m_cos;
179 }
180
181 6 const Scalar & displacement() const
182 {
183 6 return m_displacement;
184 }
185 168 Scalar & displacement()
186 {
187 168 return m_displacement;
188 }
189
190 template<typename Scalar1, typename Scalar2, typename Scalar3>
191 124 void setValues(const Scalar1 & sin, const Scalar2 & cos, const Scalar3 & displacement)
192 {
193 124 m_sin = sin;
194 124 m_cos = cos;
195 124 m_displacement = displacement;
196 124 }
197
198 6 LinearType translation() const
199 {
200
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 12 return CartesianAxis3Linear() * displacement();
201 }
202 3 AngularType rotation() const
203 {
204
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 AngularType m(AngularType::Identity());
205 6 _setRotation(m);
206 6 return m;
207 }
208
209 102 bool isEqual(const TransformHelicalTpl & other) const
210 {
211 102 return internal::comparison_eq(m_cos, other.m_cos)
212
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 && internal::comparison_eq(m_sin, other.m_sin)
213
2/4
✓ Branch 0 taken 51 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 51 times.
✗ Branch 4 not taken.
 204 && internal::comparison_eq(m_displacement, other.m_displacement);
214 }
215
216 protected:
217 Scalar m_sin, m_cos, m_displacement;
218 94 inline void _setRotation(typename PlainType::AngularRef & rot) const
219 {
220 switch (axis)
221 {
222 case 0: {
223 50 rot.coeffRef(1, 1) = m_cos;
224
2/5
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 50 rot.coeffRef(1, 2) = -m_sin;
225 50 rot.coeffRef(2, 1) = m_sin;
226 50 rot.coeffRef(2, 2) = m_cos;
227 50 break;
228 }
229 case 1: {
230 22 rot.coeffRef(0, 0) = m_cos;
231 22 rot.coeffRef(0, 2) = m_sin;
232
2/5
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 22 rot.coeffRef(2, 0) = -m_sin;
233 22 rot.coeffRef(2, 2) = m_cos;
234 22 break;
235 }
236 case 2: {
237 22 rot.coeffRef(0, 0) = m_cos;
238
2/5
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 22 rot.coeffRef(0, 1) = -m_sin;
239 22 rot.coeffRef(1, 0) = m_sin;
240 22 rot.coeffRef(1, 1) = m_cos;
241 22 break;
242 }
243 default: {
244 assert(false && "must never happen");
245 break;
246 }
247 }
248 94 }
249 }; // struct TransformHelicalTpl
250
251 template<typename _Scalar, int _Options, int axis>
252 struct MotionHelicalTpl : MotionBase<MotionHelicalTpl<_Scalar, _Options, axis>>
253 {
254 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
255
256 MOTION_TYPEDEF_TPL(MotionHelicalTpl);
257 typedef SpatialAxis<axis + ANGULAR> AxisAngular;
258 typedef typename AxisAngular::CartesianAxis3 CartesianAxis3Angular;
259 typedef SpatialAxis<axis + LINEAR> AxisLinear;
260 typedef typename AxisLinear::CartesianAxis3 CartesianAxis3Linear;
261
262 42 MotionHelicalTpl()
263 {
264 42 }
265
266 556 MotionHelicalTpl(const Scalar & w, const Scalar & v)
267 556 : m_w(w)
268
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 556 , m_v(v)
269 {
270 556 }
271
272 60 inline PlainReturnType plain() const
273 {
274
3/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
 60 return PlainReturnType(CartesianAxis3Linear() * m_v, CartesianAxis3Angular() * m_w);
275 }
276
277 template<typename OtherScalar>
278 MotionHelicalTpl __mult__(const OtherScalar & alpha) const
279 {
280 return MotionHelicalTpl(alpha * m_w, alpha * m_v);
281 }
282
283 template<typename MotionDerived>
284 14 void setTo(MotionDense<MotionDerived> & m) const
285 {
286
2/2
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 7 times.
 56 for (Eigen::DenseIndex k = 0; k < 3; ++k)
287 {
288
3/9
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 14 times.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 42 m.angular()[k] = k == axis ? m_w : (Scalar)0;
289
3/9
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 14 times.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 42 m.linear()[k] = k == axis ? m_v : (Scalar)0;
290 }
291 }
292
293 template<typename MotionDerived>
294 6 inline void addTo(MotionDense<MotionDerived> & v) const
295 {
296 typedef typename MotionDense<MotionDerived>::Scalar OtherScalar;
297
1/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 6 v.angular()[axis] += (OtherScalar)m_w;
298
1/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 6 v.linear()[axis] += (OtherScalar)m_v;
299 }
300
301 template<typename S2, int O2, typename D2>
302 10 inline void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
303 {
304
4/8
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
 10 v.angular().noalias() = m.rotation().col(axis) * m_w;
305
8/16
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 5 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 5 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 5 times.
✗ Branch 25 not taken.
 10 v.linear().noalias() = m.translation().cross(v.angular()) + m_v * (m.rotation().col(axis));
306 10 }
307
308 template<typename S2, int O2>
309 10 MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const
310 {
311
312 10 MotionPlain res;
313
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 10 se3Action_impl(m, res);
314 10 return res;
315 }
316
317 template<typename S2, int O2, typename D2>
318 6 void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
319 {
320 // Linear
321
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 6 CartesianAxis3Linear::alphaCross(m_w, m.translation(), v.angular());
322
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.
 6 v.linear().noalias() =
323
6/12
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
 6 m.rotation().transpose() * v.angular() + m_v * (m.rotation().transpose().col(axis));
324
325 // Angular
326
5/10
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
 6 v.angular().noalias() = m.rotation().transpose().col(axis) * m_w;
327 6 }
328
329 template<typename S2, int O2>
330 6 MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
331 {
332 6 MotionPlain res;
333
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 6 se3ActionInverse_impl(m, res);
334 6 return res;
335 }
336
337 template<typename M1, typename M2>
338 16 EIGEN_STRONG_INLINE void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
339 {
340 // Linear
341
2/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 16 CartesianAxis3Linear::alphaCross(-m_w, v.linear(), mout.linear());
342
2/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 16 CartesianAxis3Linear::alphaCross(-m_v, v.angular(), mout.angular());
343
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 16 mout.linear() += mout.angular();
344 // Angular
345
2/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 16 CartesianAxis3Angular::alphaCross(-m_w, v.angular(), mout.angular());
346 16 }
347
348 template<typename M1>
349 16 MotionPlain motionAction(const MotionDense<M1> & v) const
350 {
351 16 MotionPlain res;
352
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 16 motionAction(v, res);
353 16 return res;
354 }
355
356 252 Scalar & angularRate()
357 {
358 252 return m_w;
359 }
360 const Scalar & angularRate() const
361 {
362 return m_w;
363 }
364
365 252 Scalar & linearRate()
366 {
367 252 return m_v;
368 }
369 const Scalar & linearRate() const
370 {
371 return m_v;
372 }
373
374 102 bool isEqual_impl(const MotionHelicalTpl & other) const
375 {
376
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 102 return internal::comparison_eq(m_w, other.m_w) && internal::comparison_eq(m_v, other.m_v);
377 }
378
379 protected:
380 Scalar m_w, m_v;
381 }; // struct MotionHelicalTpl
382 template<typename S1, int O1, int axis, typename MotionDerived>
383 typename MotionDerived::MotionPlain
384 ✗ operator+(const MotionHelicalTpl<S1, O1, axis> & m1, const MotionDense<MotionDerived> & m2)
385 {
386 ✗ typename MotionDerived::MotionPlain res(m2);
387 ✗ res += m1;
388 ✗ return res;
389 }
390
391 template<typename MotionDerived, typename S2, int O2, int axis>
392 EIGEN_STRONG_INLINE typename MotionDerived::MotionPlain
393 10 operator^(const MotionDense<MotionDerived> & m1, const MotionHelicalTpl<S2, O2, axis> & m2)
394 {
395 10 return m2.motionAction(m1);
396 }
397
398 template<typename Scalar, int Options, int axis>
399 struct JointMotionSubspaceHelicalTpl;
400
401 template<typename Scalar, int Options, int axis>
402 struct SE3GroupAction<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>>
403 {
404 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
405 };
406
407 template<typename Scalar, int Options, int axis, typename MotionDerived>
408 struct MotionAlgebraAction<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>, MotionDerived>
409 {
410 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
411 };
412
413 template<typename Scalar, int Options, int axis, typename ForceDerived>
414 struct ConstraintForceOp<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>, ForceDerived>
415 {
416 typedef typename Eigen::Matrix<Scalar, 1, 1> ReturnType;
417 };
418
419 template<typename Scalar, int Options, int axis, typename ForceSet>
420 struct ConstraintForceSetOp<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>, ForceSet>
421 {
422 typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> ReturnType;
423 };
424
425 template<typename _Scalar, int _Options, int axis>
426 struct traits<JointMotionSubspaceHelicalTpl<_Scalar, _Options, axis>>
427 {
428 typedef _Scalar Scalar;
429 enum
430 {
431 Options = _Options
432 };
433 enum
434 {
435 LINEAR = 0,
436 ANGULAR = 3
437 };
438
439 typedef MotionHelicalTpl<Scalar, Options, axis> JointMotion;
440 typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce;
441 typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase;
442 typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix;
443
444 typedef DenseBase MatrixReturnType;
445 typedef const DenseBase ConstMatrixReturnType;
446
447 typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType;
448 }; // traits JointMotionSubspaceHelicalTpl
449
450 template<class ConstraintDerived>
451 struct TransposeConstraintActionConstraint
452 {
453 typedef
454 typename Eigen::Matrix<typename ConstraintDerived::Scalar, 1, 1, ConstraintDerived::Options>
455 ReturnType;
456 };
457
458 template<typename _Scalar, int _Options, int axis>
459 struct JointMotionSubspaceHelicalTpl
460 : JointMotionSubspaceBase<JointMotionSubspaceHelicalTpl<_Scalar, _Options, axis>>
461 {
462 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
463
464 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceHelicalTpl)
465 enum
466 {
467 NV = 1
468 };
469
470 typedef SpatialAxis<ANGULAR + axis> AxisAngular;
471 typedef SpatialAxis<ANGULAR + axis> AxisLinear;
472
473 typedef typename AxisAngular::CartesianAxis3 CartesianAxis3Angular;
474 typedef typename AxisLinear::CartesianAxis3 CartesianAxis3Linear;
475
476 42 JointMotionSubspaceHelicalTpl()
477 {
478 42 }
479
480 538 JointMotionSubspaceHelicalTpl(const Scalar & h)
481 538 : m_pitch(h)
482 {
483 538 }
484
485 template<typename Vector1Like>
486 12 JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
487 {
488 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1);
489
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
 12 assert(v.size() == 1);
490
1/8
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 12 return JointMotion(v[0], v[0] * m_pitch);
491 }
492
493 template<typename S1, int O1>
494 typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType
495 34 se3Action(const SE3Tpl<S1, O1> & m) const
496 {
497 typedef typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType ReturnType;
498 34 ReturnType res;
499
4/8
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
 34 res.template segment<3>(LINEAR) =
500
4/16
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 17 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 17 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 17 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 34 m.translation().cross(m.rotation().col(axis)) + m_pitch * (m.rotation().col(axis));
501
2/10
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 17 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 34 res.template segment<3>(ANGULAR) = m.rotation().col(axis);
502 34 return res;
503 }
504
505 template<typename S1, int O1>
506 typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType
507 18 se3ActionInverse(const SE3Tpl<S1, O1> & m) const
508 {
509 typedef typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType ReturnType;
510 typedef typename AxisAngular::CartesianAxis3 CartesianAxis3;
511 18 ReturnType res;
512
5/10
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
 36 res.template segment<3>(LINEAR).noalias() =
513
5/10
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
 18 m.rotation().transpose() * CartesianAxis3::cross(m.translation())
514
1/7
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 18 + m.rotation().transpose().col(axis) * m_pitch;
515
3/13
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 18 res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis);
516 18 return res;
517 }
518
519 108 int nv_impl() const
520 {
521 108 return NV;
522 }
523
524 // For force T
525 struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceHelicalTpl>
526 {
527 const JointMotionSubspaceHelicalTpl & ref;
528 30 TransposeConst(const JointMotionSubspaceHelicalTpl & ref)
529 30 : ref(ref)
530 {
531 30 }
532
533 template<typename ForceDerived>
534 typename ConstraintForceOp<JointMotionSubspaceHelicalTpl, ForceDerived>::ReturnType
535 16 operator*(const ForceDense<ForceDerived> & f) const
536 {
537
4/12
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 32 return Eigen::Matrix<Scalar, 1, 1>(f.angular()(axis) + f.linear()(axis) * ref.m_pitch);
538 }
539
540 /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
541 template<typename Derived>
542 typename ConstraintForceSetOp<JointMotionSubspaceHelicalTpl, Derived>::ReturnType
543 8 operator*(const Eigen::MatrixBase<Derived> & F) const
544 {
545
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
 8 assert(F.rows() == 6);
546
4/8
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
 8 return F.row(ANGULAR + axis) + F.row(LINEAR + axis) * ref.m_pitch;
547 }
548 }; // struct TransposeConst
549
550 30 TransposeConst transpose() const
551 {
552 30 return TransposeConst(*this);
553 }
554
555 /* CRBA joint operators
556 * - ForceSet::Block = ForceSet
557 * - ForceSet operator* (Inertia Y,Constraint S)
558 * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
559 * - SE3::act(ForceSet::Block)
560 */
561 48 DenseBase matrix_impl() const
562 {
563
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 DenseBase S;
564
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 MotionRef<DenseBase> v(S);
565
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 v << AxisAngular();
566
1/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 48 S(LINEAR + axis) = m_pitch;
567 96 return S;
568 }
569
570 template<typename MotionDerived>
571 typename MotionAlgebraAction<JointMotionSubspaceHelicalTpl, MotionDerived>::ReturnType
572 12 motionAction(const MotionDense<MotionDerived> & m) const
573 {
574 typedef typename MotionAlgebraAction<JointMotionSubspaceHelicalTpl, MotionDerived>::ReturnType
575 ReturnType;
576 12 ReturnType res;
577 // Linear
578
3/11
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 12 CartesianAxis3Linear::cross(-m.linear(), res.template segment<3>(LINEAR));
579
2/10
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 12 CartesianAxis3Linear::alphaCross(-m_pitch, m.angular(), res.template segment<3>(ANGULAR));
580
2/8
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 12 res.template segment<3>(LINEAR) += res.template segment<3>(ANGULAR);
581
582 // Angular
583
3/11
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 12 CartesianAxis3Angular::cross(-m.angular(), res.template segment<3>(ANGULAR));
584 12 return res;
585 }
586
587 102 bool isEqual(const JointMotionSubspaceHelicalTpl &) const
588 {
589 102 return true;
590 }
591
592 292 Scalar & h()
593 {
594 292 return m_pitch;
595 }
596 19 const Scalar & h() const
597 {
598 38 return m_pitch;
599 }
600
601 protected:
602 Scalar m_pitch;
603 }; // struct JointMotionSubspaceHelicalTpl
604
605 template<typename _Scalar, int _Options, int _axis>
606 6 Eigen::Matrix<_Scalar, 1, 1, _Options> operator*(
607 const typename JointMotionSubspaceHelicalTpl<_Scalar, _Options, _axis>::TransposeConst &
608 S_transpose,
609 const JointMotionSubspaceHelicalTpl<_Scalar, _Options, _axis> & S)
610 {
611 6 Eigen::Matrix<_Scalar, 1, 1, _Options> res;
612 6 res(0) = 1.0 + S_transpose.ref.h() * S.h();
613 6 return res;
614 }
615
616 template<typename _Scalar, int _Options, int _axis>
617 struct JointHelicalTpl
618 {
619 typedef _Scalar Scalar;
620
621 enum
622 {
623 Options = _Options,
624 axis = _axis
625 };
626 };
627
628 template<typename S1, int O1, typename S2, int O2, int axis>
629 struct MultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalTpl<S2, O2, axis>>
630 {
631 typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType;
632 };
633
634 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
635 namespace impl
636 {
637 template<typename S1, int O1, typename S2, int O2>
638 struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalTpl<S2, O2, 0>>
639 {
640 typedef InertiaTpl<S1, O1> Inertia;
641 typedef JointMotionSubspaceHelicalTpl<S2, O2, 0> Constraint;
642 typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType;
643 3 static inline ReturnType run(const Inertia & Y, const Constraint & constraint)
644 {
645
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 ReturnType res;
646 3 const S2 & m_pitch = constraint.h();
647
648 /* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */
649 /* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
650
3/14
✗ Branch 1 not taken.
✗ 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 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 3 const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2];
651 3 const typename Inertia::Symmetric3 & I = Y.inertia();
652
653
5/41
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 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 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
✗ Branch 58 not taken.
✗ Branch 59 not taken.
 3 res << m * m_pitch, -m * z, m * y, I(0, 0) + m * (y * y + z * z),
654
4/21
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
 3 I(0, 1) - m * x * y + m * z * m_pitch, I(0, 2) - m * x * z - m * y * m_pitch;
655
656 6 return res;
657 }
658 };
659
660 template<typename S1, int O1, typename S2, int O2>
661 struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalTpl<S2, O2, 1>>
662 {
663 typedef InertiaTpl<S1, O1> Inertia;
664 typedef JointMotionSubspaceHelicalTpl<S2, O2, 1> Constraint;
665 typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType;
666 2 static inline ReturnType run(const Inertia & Y, const Constraint & constraint)
667 {
668
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 ReturnType res;
669 2 const S2 & m_pitch = constraint.h();
670
671 /* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */
672 /* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
673
3/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 2 const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2];
674 2 const typename Inertia::Symmetric3 & I = Y.inertia();
675
676
5/41
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 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.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
✗ Branch 58 not taken.
✗ Branch 59 not taken.
 2 res << m * z, m * m_pitch, -m * x, I(1, 0) - m * x * y - m * z * m_pitch,
677
4/21
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
 2 I(1, 1) + m * (x * x + z * z), I(1, 2) - m * y * z + m * x * m_pitch;
678
679 4 return res;
680 }
681 };
682
683 template<typename S1, int O1, typename S2, int O2>
684 struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalTpl<S2, O2, 2>>
685 {
686 typedef InertiaTpl<S1, O1> Inertia;
687 typedef JointMotionSubspaceHelicalTpl<S2, O2, 2> Constraint;
688 typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType;
689 2 static inline ReturnType run(const Inertia & Y, const Constraint & constraint)
690 {
691
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 ReturnType res;
692 2 const S2 & m_pitch = constraint.h();
693
694 /* Y(:,5) = (-y, x, 0, I20-xz , I21-yz , I22+xx+yy) */
695 /* Y(:,2) = ( 0,0, 1, y , -x , 0) */
696
3/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 2 const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2];
697 2 const typename Inertia::Symmetric3 & I = Y.inertia();
698
699
5/43
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 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.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
✗ Branch 58 not taken.
✗ Branch 59 not taken.
✗ Branch 61 not taken.
✗ Branch 62 not taken.
 2 res << -m * y, m * x, m * m_pitch, I(2, 0) - m * x * z + m * y * m_pitch,
700
4/19
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 2 I(2, 1) - m * y * z - m * x * m_pitch, I(2, 2) + m * (x * x + y * y);
701
702 4 return res;
703 }
704 };
705 } // namespace impl
706
707 template<typename M6Like, typename S2, int O2, int axis>
708 struct MultiplicationOp<Eigen::MatrixBase<M6Like>, JointMotionSubspaceHelicalTpl<S2, O2, axis>>
709 {
710 typedef Eigen::Matrix<S2, 6, 1> ReturnType;
711 };
712
713 /* [ABA] operator* (Inertia Y,Constraint S) */
714 namespace impl
715 {
716 template<typename M6Like, typename Scalar, int Options, int axis>
717 struct LhsMultiplicationOp<
718 Eigen::MatrixBase<M6Like>,
719 JointMotionSubspaceHelicalTpl<Scalar, Options, axis>>
720 {
721 typedef JointMotionSubspaceHelicalTpl<Scalar, Options, axis> Constraint;
722 typedef Eigen::Matrix<Scalar, 6, 1> ReturnType;
723 static inline ReturnType
724 12 run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & constraint)
725 {
726 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
727
4/8
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6 times.
✗ Branch 13 not taken.
 12 return (Y.col(Inertia::ANGULAR + axis) + Y.col(Inertia::LINEAR + axis) * constraint.h());
728 }
729 };
730 } // namespace impl
731
732 template<typename _Scalar, int _Options, int axis>
733 struct traits<JointHelicalTpl<_Scalar, _Options, axis>>
734 {
735 enum
736 {
737 NQ = 1,
738 NV = 1
739 };
740 typedef _Scalar Scalar;
741 enum
742 {
743 Options = _Options
744 };
745 typedef JointDataHelicalTpl<Scalar, Options, axis> JointDataDerived;
746 typedef JointModelHelicalTpl<Scalar, Options, axis> JointModelDerived;
747 typedef JointMotionSubspaceHelicalTpl<Scalar, Options, axis> Constraint_t;
748 typedef TransformHelicalTpl<Scalar, Options, axis> Transformation_t;
749 typedef MotionHelicalTpl<Scalar, Options, axis> Motion_t;
750 typedef MotionZeroTpl<Scalar, Options> Bias_t;
751
752 // [ABA]
753 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
754 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
755 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
756
757 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
758 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
759
760 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
761 };
762
763 template<typename _Scalar, int _Options, int axis>
764 struct traits<JointDataHelicalTpl<_Scalar, _Options, axis>>
765 {
766 typedef JointHelicalTpl<_Scalar, _Options, axis> JointDerived;
767 typedef _Scalar Scalar;
768 };
769
770 template<typename _Scalar, int _Options, int axis>
771 struct traits<JointModelHelicalTpl<_Scalar, _Options, axis>>
772 {
773 typedef JointHelicalTpl<_Scalar, _Options, axis> JointDerived;
774 typedef _Scalar Scalar;
775 };
776
777 template<typename _Scalar, int _Options, int axis>
778 struct JointDataHelicalTpl : public JointDataBase<JointDataHelicalTpl<_Scalar, _Options, axis>>
779 {
780 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
781 typedef JointHelicalTpl<_Scalar, _Options, axis> JointDerived;
782 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
783 2468 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
784
785 ConfigVector_t joint_q;
786 TangentVector_t joint_v;
787
788 Constraint_t S;
789 Transformation_t M;
790 Motion_t v;
791 Bias_t c;
792
793 // [ABA] specific data
794 U_t U;
795 D_t Dinv;
796 UD_t UDinv;
797 D_t StU;
798
799 538 JointDataHelicalTpl()
800
1/2
✓ Branch 2 taken 269 times.
✗ Branch 3 not taken.
 538 : joint_q(ConfigVector_t::Zero())
801
3/5
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 254 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 538 , joint_v(TangentVector_t::Zero())
802
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 538 , S((Scalar)0)
803
4/8
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 538 , M((Scalar)0, (Scalar)1, (Scalar)0)
804
3/6
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
 538 , v((Scalar)0, (Scalar)0)
805
3/5
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 254 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 538 , U(U_t::Zero())
806
3/5
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 254 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 538 , Dinv(D_t::Zero())
807
3/5
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 254 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 538 , UDinv(UD_t::Zero())
808
3/5
✓ Branch 2 taken 15 times.
✓ Branch 3 taken 254 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 1076 , StU(D_t::Zero())
809 {
810 538 }
811
812 864 static std::string classname()
813 {
814
2/4
✓ Branch 3 taken 432 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 432 times.
✗ Branch 7 not taken.
 1728 return std::string("JointDataH") + axisLabel<axis>();
815 }
816 9 std::string shortname() const
817 {
818 18 return classname();
819 }
820
821 }; // struct JointDataHelicalTpl
822
823 template<typename NewScalar, typename Scalar, int Options, int axis>
824 struct CastType<NewScalar, JointModelHelicalTpl<Scalar, Options, axis>>
825 {
826 typedef JointModelHelicalTpl<NewScalar, Options, axis> type;
827 };
828
829 template<typename _Scalar, int _Options, int axis>
830 struct JointModelHelicalTpl : public JointModelBase<JointModelHelicalTpl<_Scalar, _Options, axis>>
831 {
832 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
833 typedef JointHelicalTpl<_Scalar, _Options, axis> JointDerived;
834 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
835
836 typedef JointModelBase<JointModelHelicalTpl> Base;
837 using Base::id;
838 using Base::idx_q;
839 using Base::idx_v;
840 using Base::setIndexes;
841
842 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
843
844 82 JointDataDerived createData() const
845 {
846 82 return JointDataDerived();
847 }
848
849 564 JointModelHelicalTpl()
850 564 {
851 564 }
852
853 123 explicit JointModelHelicalTpl(const Scalar & h)
854 123 : m_pitch(h)
855 {
856 123 }
857
858 ✗ const std::vector<bool> hasConfigurationLimit() const
859 {
860 ✗ return {true, true};
861 }
862
863 ✗ const std::vector<bool> hasConfigurationLimitInTangent() const
864 {
865 ✗ return {true, true};
866 }
867
868 template<typename ConfigVector>
869 EIGEN_DONT_INLINE void
870 124 calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
871 {
872
4/8
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 62 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 124 data.joint_q[0] = qs[idx_q()];
873
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 Scalar ca, sa;
874
2/4
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 124 SINCOS(data.joint_q[0], &sa, &ca);
875
3/6
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 124 data.M.setValues(sa, ca, data.joint_q[0] * m_pitch);
876
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 124 data.S.h() = m_pitch;
877 124 }
878
879 template<typename TangentVector>
880 EIGEN_DONT_INLINE void
881 6 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
882 const
883 {
884 6 data.joint_v[0] = vs[idx_v()];
885 6 data.v.angularRate() = data.joint_v[0];
886 6 data.v.linearRate() = data.joint_v[0] * m_pitch;
887 6 }
888
889 template<typename ConfigVector, typename TangentVector>
890 78 EIGEN_DONT_INLINE void calc(
891 JointDataDerived & data,
892 const typename Eigen::MatrixBase<ConfigVector> & qs,
893 const typename Eigen::MatrixBase<TangentVector> & vs) const
894 {
895 78 calc(data, qs.derived());
896
897
0/4
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 78 data.joint_v[0] = vs[idx_v()];
898 78 data.v.angularRate() = data.joint_v[0];
899
1/2
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 78 data.v.linearRate() = data.joint_v[0] * m_pitch;
900 78 }
901
902 template<typename VectorLike, typename Matrix6Like>
903 32 void calc_aba(
904 JointDataDerived & data,
905 const Eigen::MatrixBase<VectorLike> & armature,
906 const Eigen::MatrixBase<Matrix6Like> & I,
907 const bool update_I) const
908 {
909
4/8
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 16 times.
✗ Branch 12 not taken.
 32 data.U = I.col(Inertia::ANGULAR + axis) + m_pitch * I.col(Inertia::LINEAR + axis);
910
0/6
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 64 data.StU[0] =
911
0/8
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 32 data.U(Inertia::ANGULAR + axis) + m_pitch * data.U(Inertia::LINEAR + axis) + armature[0];
912
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.
 32 data.Dinv[0] = Scalar(1) / data.StU[0];
913
2/4
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
 32 data.UDinv.noalias() = data.U * data.Dinv;
914
915
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 32 if (update_I)
916 ✗ PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
917 32 }
918
919 993 static std::string classname()
920 {
921
2/4
✓ Branch 3 taken 495 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 495 times.
✗ Branch 7 not taken.
 1986 return std::string("JointModelH") + axisLabel<axis>();
922 }
923 18 std::string shortname() const
924 {
925 90 return classname();
926 }
927
928 ✗ Vector3 getMotionAxis() const
929 {
930 switch (axis)
931 {
932 case 0:
933 ✗ return Vector3::UnitX();
934 case 1:
935 ✗ return Vector3::UnitY();
936 case 2:
937 ✗ return Vector3::UnitZ();
938 default:
939 assert(false && "must never happen");
940 break;
941 }
942 }
943
944 /// \returns An expression of *this with the Scalar type casted to NewScalar.
945 template<typename NewScalar>
946 18 JointModelHelicalTpl<NewScalar, Options, axis> cast() const
947 {
948 typedef JointModelHelicalTpl<NewScalar, Options, axis> ReturnType;
949
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 18 ReturnType res(ScalarCast<NewScalar, Scalar>::cast(m_pitch));
950
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.
 18 res.setIndexes(id(), idx_q(), idx_v());
951 18 return res;
952 }
953
954 Scalar m_pitch;
955
956 }; // struct JointModelHelicalTpl
957
958 typedef JointHelicalTpl<context::Scalar, context::Options, 0> JointHX;
959 typedef JointDataHelicalTpl<context::Scalar, context::Options, 0> JointDataHX;
960 typedef JointModelHelicalTpl<context::Scalar, context::Options, 0> JointModelHX;
961
962 typedef JointHelicalTpl<context::Scalar, context::Options, 1> JointHY;
963 typedef JointDataHelicalTpl<context::Scalar, context::Options, 1> JointDataHY;
964 typedef JointModelHelicalTpl<context::Scalar, context::Options, 1> JointModelHY;
965
966 typedef JointHelicalTpl<context::Scalar, context::Options, 2> JointHZ;
967 typedef JointDataHelicalTpl<context::Scalar, context::Options, 2> JointDataHZ;
968 typedef JointModelHelicalTpl<context::Scalar, context::Options, 2> JointModelHZ;
969
970 } // namespace pinocchio
971
972 #include <boost/type_traits.hpp>
973
974 namespace boost
975 {
976 template<typename Scalar, int Options, int axis>
977 struct has_nothrow_constructor<::pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
978 : public integral_constant<bool, true>
979 {
980 };
981
982 template<typename Scalar, int Options, int axis>
983 struct has_nothrow_copy<::pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
984 : public integral_constant<bool, true>
985 {
986 };
987
988 template<typename Scalar, int Options, int axis>
989 struct has_nothrow_constructor<::pinocchio::JointDataHelicalTpl<Scalar, Options, axis>>
990 : public integral_constant<bool, true>
991 {
992 };
993
994 template<typename Scalar, int Options, int axis>
995 struct has_nothrow_copy<::pinocchio::JointDataHelicalTpl<Scalar, Options, axis>>
996 : public integral_constant<bool, true>
997 {
998 };
999 } // namespace boost
1000
1001 #endif // ifndef __pinocchio_multibody_joint_helical_hpp__
1002